From c875b76cf67ae188ff065949acc25ed3c03ace4d Mon Sep 17 00:00:00 2001 From: Jiwen Cai Date: Thu, 27 Aug 2026 23:50:39 +0000 Subject: [PATCH 1/2] viz: promote the robot twin into the Televiz library The MuJoCo-backed digital twin was buried in examples/mujoco_xr, reachable only by installing it. It is a Televiz capability, not an example. The backend moves to src/viz/robot_twin and its Python surface to isaacteleop.viz.robot, both shipping in the wheel; the app left behind is renamed examples/robot_viz. deps/third_party builds MuJoCo under a private name, reached through dlopen/dlsym so the extension carries no undefined mj* for a foreign libmujoco to answer. Users may pip install mujoco at any version, or none. The clutch preview comes with it: LeRobot's isaac_teleop_to_so101 needs the same preview, and copying it would duplicate the phase machine, the ghost calibration and the asset fetch. app.py drops from 1121 to 329 lines, and the fetch script becomes assets.ensure_so101_scene(), which assembles into a cache directory. The three MJCF wrappers it reads ship as viz.robot package data. OperatorFrame measures the XR-to-robot yaw the rebase used to assume. A wrong one reads as an intuitive-but-wrong hand-to-EE mapping, not an error: stand 90 deg off the arm and pushing the controller away moves the jaw 90 deg off what you meant. The wheel now redistributes MuJoCo binaries, and every BUILD_VIZ build compiles it with no opt-out (~40 s, needs libegl-dev). Tests are #999. BREAKING: isaacteleop_examples.mujoco_xr is gone. Signed-off-by: Jiwen Cai --- .github/workflows/build-ubuntu.yml | 2 +- .gitignore | 12 +- CMakeLists.txt | 3 - cmake/CheckBuildDeps.cmake | 11 + deps/third_party/CMakeLists.txt | 35 + deps/third_party/Mujoco.cmake | 62 + .../build_from_source/index.rst | 2 +- docs/source/references/cloudxr.rst | 2 +- docs/source/references/retargeting/so101.rst | 2 +- examples/mujoco_xr/CMakeLists.txt | 195 --- examples/mujoco_xr/cpp/CMakeLists.txt | 81 - examples/mujoco_xr/cpp/gl_functions.inc | 55 - examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp | 249 --- examples/mujoco_xr/pyproject.toml | 91 -- .../mujoco_xr/__init__.py | 35 - .../isaacteleop_examples/mujoco_xr/app.py | 1359 ----------------- .../mujoco_xr/follower.py | 730 --------- .../isaacteleop_examples/mujoco_xr/harness.py | 254 --- examples/mujoco_xr/scripts/fetch-so-arm.sh | 88 -- examples/{mujoco_xr => robot_viz}/README.md | 370 ++--- examples/robot_viz/pyproject.toml | 52 + .../robot_viz/__init__.py | 9 + .../robot_viz}/__main__.py | 2 +- .../isaacteleop_examples/robot_viz/app.py | 329 ++++ rigs/{mujoco_xr.yaml => robot_viz.yaml} | 21 +- src/core/python/pyproject.toml.in | 4 + src/python/CMakeLists.txt | 7 +- .../retargeters/SO101/clutch_retargeter.py | 2 +- .../isaacteleop/retargeters/__init__.py | 7 + .../retargeters/controller_pose.py | 135 ++ .../teleop_session_manager/config.py | 59 + .../teleop_session_manager/teleop_session.py | 107 ++ .../teleop_session_manager/twin_runner.py | 267 ++++ src/python/isaacteleop/viz/robot/__init__.py | 142 ++ src/python/isaacteleop/viz/robot/anchor.py | 106 ++ src/python/isaacteleop/viz/robot/assets.py | 211 +++ .../viz/robot/assets}/follower_arm.xml | 16 +- .../viz/robot/assets}/leader_gripper.xml | 8 +- .../isaacteleop/viz/robot}/assets/scene.xml | 15 +- .../isaacteleop/viz/robot/clutch_phase.py | 85 ++ .../isaacteleop/viz/robot/clutch_preview.py | 667 ++++++++ .../isaacteleop/viz/robot/engage_gate.py | 320 ++++ .../isaacteleop/viz/robot/frame_info.py | 91 ++ src/python/isaacteleop/viz/robot/frames.py | 87 ++ src/python/isaacteleop/viz/robot/harness.py | 128 ++ src/python/isaacteleop/viz/robot/joint_map.py | 110 ++ .../isaacteleop/viz/robot/operator_frame.py | 137 ++ .../isaacteleop/viz/robot/preview_arm.py | 392 +++++ .../isaacteleop/viz/robot/quaternion.py | 106 ++ src/python/isaacteleop/viz/robot/scene.py | 421 +++++ src/python/isaacteleop/viz/robot/session.py | 220 +++ .../isaacteleop/viz/robot/so101_ghost.py | 174 +++ src/python/isaacteleop/viz/robot/twin.py | 92 ++ src/viz/CMakeLists.txt | 11 + src/viz/robot_twin/cpp/CMakeLists.txt | 61 + .../viz/robot_twin}/cpp/frames.hpp | 10 +- .../viz/robot_twin}/cpp/gl.cpp | 26 +- .../viz/robot_twin}/cpp/gl.hpp | 10 +- src/viz/robot_twin/cpp/gl_context.cpp | 220 +++ src/viz/robot_twin/cpp/gl_context.hpp | 51 + src/viz/robot_twin/cpp/gl_functions.inc | 54 + .../viz/robot_twin}/cpp/gl_readback.cpp | 24 +- .../viz/robot_twin}/cpp/gl_readback.hpp | 4 +- .../viz/robot_twin}/cpp/glcamera.hpp | 8 +- src/viz/robot_twin/cpp/mj_api.cpp | 102 ++ src/viz/robot_twin/cpp/mj_api.hpp | 35 + src/viz/robot_twin/cpp/mj_functions.inc | 40 + src/viz/robot_twin/cpp/mj_guard.cpp | 63 + src/viz/robot_twin/cpp/mj_guard.hpp | 28 + .../robot_twin/cpp/robot_twin_bindings.cpp | 348 +++++ src/viz/robot_twin/cpp/scene.cpp | 66 + src/viz/robot_twin/cpp/scene.hpp | 58 + .../viz/robot_twin}/cpp/scene_renderer.cpp | 94 +- .../viz/robot_twin}/cpp/scene_renderer.hpp | 26 +- tests/AGENTS.md | 2 +- tests/python/examples/CMakeLists.txt | 1 - .../python/examples/mujoco_xr/CMakeLists.txt | 43 - tests/python/examples/mujoco_xr/conftest.py | 21 - .../python/examples/mujoco_xr/pyproject.toml | 30 - .../examples/mujoco_xr/test_app_helpers.py | 94 -- .../python/examples/mujoco_xr/test_frames.py | 92 -- tests/python/examples/mujoco_xr/test_ghost.py | 457 ------ .../examples/mujoco_xr/test_projection.py | 81 - .../examples/mujoco_xr/test_readback.py | 181 --- 84 files changed, 6022 insertions(+), 4486 deletions(-) create mode 100644 deps/third_party/Mujoco.cmake delete mode 100644 examples/mujoco_xr/CMakeLists.txt delete mode 100644 examples/mujoco_xr/cpp/CMakeLists.txt delete mode 100644 examples/mujoco_xr/cpp/gl_functions.inc delete mode 100644 examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp delete mode 100644 examples/mujoco_xr/pyproject.toml delete mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py delete mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py delete mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/follower.py delete mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/harness.py delete mode 100755 examples/mujoco_xr/scripts/fetch-so-arm.sh rename examples/{mujoco_xr => robot_viz}/README.md (71%) create mode 100644 examples/robot_viz/pyproject.toml create mode 100644 examples/robot_viz/python/isaacteleop_examples/robot_viz/__init__.py rename examples/{mujoco_xr/python/isaacteleop_examples/mujoco_xr => robot_viz/python/isaacteleop_examples/robot_viz}/__main__.py (78%) create mode 100644 examples/robot_viz/python/isaacteleop_examples/robot_viz/app.py rename rigs/{mujoco_xr.yaml => robot_viz.yaml} (54%) create mode 100644 src/python/isaacteleop/retargeters/controller_pose.py create mode 100644 src/python/isaacteleop/teleop_session_manager/twin_runner.py create mode 100644 src/python/isaacteleop/viz/robot/__init__.py create mode 100644 src/python/isaacteleop/viz/robot/anchor.py create mode 100644 src/python/isaacteleop/viz/robot/assets.py rename {examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower => src/python/isaacteleop/viz/robot/assets}/follower_arm.xml (69%) rename {examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader => src/python/isaacteleop/viz/robot/assets}/leader_gripper.xml (94%) rename {examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr => src/python/isaacteleop/viz/robot}/assets/scene.xml (70%) create mode 100644 src/python/isaacteleop/viz/robot/clutch_phase.py create mode 100644 src/python/isaacteleop/viz/robot/clutch_preview.py create mode 100644 src/python/isaacteleop/viz/robot/engage_gate.py create mode 100644 src/python/isaacteleop/viz/robot/frame_info.py create mode 100644 src/python/isaacteleop/viz/robot/frames.py create mode 100644 src/python/isaacteleop/viz/robot/harness.py create mode 100644 src/python/isaacteleop/viz/robot/joint_map.py create mode 100644 src/python/isaacteleop/viz/robot/operator_frame.py create mode 100644 src/python/isaacteleop/viz/robot/preview_arm.py create mode 100644 src/python/isaacteleop/viz/robot/quaternion.py create mode 100644 src/python/isaacteleop/viz/robot/scene.py create mode 100644 src/python/isaacteleop/viz/robot/session.py create mode 100644 src/python/isaacteleop/viz/robot/so101_ghost.py create mode 100644 src/python/isaacteleop/viz/robot/twin.py create mode 100644 src/viz/robot_twin/cpp/CMakeLists.txt rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/frames.hpp (92%) rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/gl.cpp (78%) rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/gl.hpp (83%) create mode 100644 src/viz/robot_twin/cpp/gl_context.cpp create mode 100644 src/viz/robot_twin/cpp/gl_context.hpp create mode 100644 src/viz/robot_twin/cpp/gl_functions.inc rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/gl_readback.cpp (92%) rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/gl_readback.hpp (98%) rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/glcamera.hpp (92%) create mode 100644 src/viz/robot_twin/cpp/mj_api.cpp create mode 100644 src/viz/robot_twin/cpp/mj_api.hpp create mode 100644 src/viz/robot_twin/cpp/mj_functions.inc create mode 100644 src/viz/robot_twin/cpp/mj_guard.cpp create mode 100644 src/viz/robot_twin/cpp/mj_guard.hpp create mode 100644 src/viz/robot_twin/cpp/robot_twin_bindings.cpp create mode 100644 src/viz/robot_twin/cpp/scene.cpp create mode 100644 src/viz/robot_twin/cpp/scene.hpp rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/scene_renderer.cpp (65%) rename {examples/mujoco_xr => src/viz/robot_twin}/cpp/scene_renderer.hpp (77%) delete mode 100644 tests/python/examples/mujoco_xr/CMakeLists.txt delete mode 100644 tests/python/examples/mujoco_xr/conftest.py delete mode 100644 tests/python/examples/mujoco_xr/pyproject.toml delete mode 100644 tests/python/examples/mujoco_xr/test_app_helpers.py delete mode 100644 tests/python/examples/mujoco_xr/test_frames.py delete mode 100644 tests/python/examples/mujoco_xr/test_ghost.py delete mode 100644 tests/python/examples/mujoco_xr/test_projection.py delete mode 100644 tests/python/examples/mujoco_xr/test_readback.py diff --git a/.github/workflows/build-ubuntu.yml b/.github/workflows/build-ubuntu.yml index 337889ad4c..1bcc29af68 100644 --- a/.github/workflows/build-ubuntu.yml +++ b/.github/workflows/build-ubuntu.yml @@ -18,7 +18,7 @@ concurrency: env: # Shared apt build dependencies. Each job installs these plus its own extras # (ccache, clang-format, the OAK camera autotools chain). - CORE_APT_DEPS: build-essential cmake glslang-tools libvulkan-dev libwayland-dev libx11-dev libxcursor-dev libxext-dev libxi-dev libxinerama-dev libxkbcommon-dev libxrandr-dev patchelf pkg-config wayland-protocols + CORE_APT_DEPS: build-essential cmake glslang-tools libegl-dev libvulkan-dev libwayland-dev libx11-dev libxcursor-dev libxext-dev libxi-dev libxinerama-dev libxkbcommon-dev libxrandr-dev patchelf pkg-config wayland-protocols # Pin vcpkg to an immutable commit for reproducible, supply-chain-stable CI # instead of tracking its moving default branch. Port versions are fixed by the # consuming manifest's builtin-baseline (e.g. DepthAI's vcpkg.json), so this diff --git a/.gitignore b/.gitignore index 4ee8aff6e2..0af74bcbc7 100644 --- a/.gitignore +++ b/.gitignore @@ -65,11 +65,7 @@ MUJOCO_LOG.TXT # SO-101 leader-gripper and follower-arm assets, fetched by # scripts/fetch-so-arm.sh into the package's assets directory, so only the # authored wrapper XML is tracked. -# -# Keep this rule HERE, not in examples/mujoco_xr/.gitignore: scikit-build-core -# resolves .gitignore against the project root, so a rule there would strip the -# meshes out of the wheel too. -/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/* -!/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml -/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower/* -!/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower/follower_arm.xml +/examples/robot_viz/python/isaacteleop_examples/robot_viz/assets/leader/* +!/examples/robot_viz/python/isaacteleop_examples/robot_viz/assets/leader/leader_gripper.xml +/examples/robot_viz/python/isaacteleop_examples/robot_viz/assets/follower/* +!/examples/robot_viz/python/isaacteleop_examples/robot_viz/assets/follower/follower_arm.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index e8b04d0b67..fbf706b563 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -171,9 +171,6 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/mcap_record_replay) add_subdirectory(examples/deviceio_live_view) add_subdirectory(examples/haptic_feedback) - if(BUILD_VIZ) - add_subdirectory(examples/mujoco_xr) - endif() elseif(BUILD_EXAMPLE_TELEOP_ROS2) add_subdirectory(examples/teleop_ros2) endif() diff --git a/cmake/CheckBuildDeps.cmake b/cmake/CheckBuildDeps.cmake index 019a74d2e0..956d091c3a 100644 --- a/cmake/CheckBuildDeps.cmake +++ b/cmake/CheckBuildDeps.cmake @@ -65,6 +65,17 @@ function(isaac_teleop_check_build_deps) endif() endif() + # EGL's headers, for the robot twin's headless OpenGL context. This function has + # already returned on anything but Linux, so BUILD_VIZ is the whole gate. Only the + # headers: gl_context.cpp dlopens libEGL, so the wheel carries no NEEDED entry. + if(BUILD_VIZ) + find_path(EGL_INCLUDE_DIR "EGL/eglext.h") + if(NOT EGL_INCLUDE_DIR) + list(APPEND _missing_tools "EGL/eglext.h (BUILD_VIZ=ON -- the robot twin's headless OpenGL context)") + list(APPEND _missing_pkgs "libegl-dev") + endif() + endif() + # patchelf strips the spurious libssl.so.3 NEEDED entry from libcloudxr.so # (src/core/cloudxr/python/CMakeLists.txt). Checked unconditionally: the SDK # tarball that triggers it is downloaded *during* configure, so whether it diff --git a/deps/third_party/CMakeLists.txt b/deps/third_party/CMakeLists.txt index 7a07656669..74f04c807c 100644 --- a/deps/third_party/CMakeLists.txt +++ b/deps/third_party/CMakeLists.txt @@ -202,3 +202,38 @@ if(BUILD_VIZ) FetchContent_MakeAvailable(glfw) message(STATUS "GLFW 3.4 fetched") endif() + +# ============================================================================== +# MuJoCo (robot twin scene + renderer) +# ============================================================================== +# Built from upstream sources unmodified and shipped under a private name, so the robot +# twin's MuJoCo is an implementation detail nothing else in the process can see. +# Mujoco.cmake beside this file is that contract and the function consumers reach it +# through; src/viz/robot_twin is the only consumer. +# +# Upstream builds one library -- engine, renderer, MJCF parser -- with no renderer-only +# target, so this is the whole of it even though the twin calls no dynamics. +if(BUILD_VIZ) + message(STATUS "Fetching MuJoCo from GitHub...") + FetchContent_Declare( + mujoco + GIT_REPOSITORY https://github.com/google-deepmind/mujoco.git + GIT_TAG 3.11.0 + GIT_SHALLOW TRUE + ) + + # Before MakeAvailable, which is what runs MuJoCo's own CMakeLists -- so these cannot + # move into Mujoco.cmake below. Plain variables, not the `CACHE ... FORCE` the entries + # above use: MuJoCo sets CMAKE_POLICY_DEFAULT_CMP0077 NEW before its own project(), so + # a normal variable wins over its option() and leaves no knob in the cache. Turning the + # tests off is what keeps abseil, googletest and benchmark out of _deps. + set(MUJOCO_BUILD_EXAMPLES OFF) + set(MUJOCO_BUILD_SIMULATE OFF) + set(MUJOCO_BUILD_TESTS OFF) + set(MUJOCO_TEST_PYTHON_UTIL OFF) + + FetchContent_MakeAvailable(mujoco) + message(STATUS "MuJoCo 3.11.0 fetched") + + include("${CMAKE_CURRENT_LIST_DIR}/Mujoco.cmake") +endif() diff --git a/deps/third_party/Mujoco.cmake b/deps/third_party/Mujoco.cmake new file mode 100644 index 0000000000..a4c8ded422 --- /dev/null +++ b/deps/third_party/Mujoco.cmake @@ -0,0 +1,62 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Makes the MuJoCo fetched beside this file private, so a user may `pip install mujoco` at +# any version, or none, and never collide with ours. Three things hold that up, none +# optional: +# +# OUTPUT_NAME a private SONAME. The mujoco wheel's extensions carry a DT_NEEDED on +# libmujoco.so.3.x, and the loader satisfies it from whatever is already +# loaded under that SONAME -- so an unrenamed copy of ours, loaded first, +# answers the user's own `import mujoco`. +# -Bsymbolic libmujoco's cross-references bind to itself rather than to whatever copy +# is in the global scope. Not -Bsymbolic-functions: mju_user_error and +# mju_user_warning are data, and libmujoco reads them. +# dlopen/dlsym src/viz/robot_twin/cpp/mj_api.cpp resolves MuJoCo at import instead of +# linking it, so the extension has no undefined mj* for a foreign +# libmujoco to answer. +# +# Do not replace the dlopen with a plain link: an undefined mj* resolves through the +# global scope, which is searched first, and the wrong libmujoco answering is silent -- +# no error, no version warning, just mjModel laid out one way and read another. + +# Upstream's own install rules come with the subdirectory and stay; suppressing them would +# mean patching. They cannot reach the wheel -- pyproject.toml's install.components names +# only isaacteleop_wheel and isaacteleop_binaries. + +set_target_properties(mujoco PROPERTIES OUTPUT_NAME isaacteleop_mujoco) +# Upstream's VERSION would make libisaacteleop_mujoco.so a symlink to ...so.3.11.0, and +# wheels do not carry symlinks. Unset with no value, which REMOVES the property; "" leaves +# it set and names the library `libisaacteleop_mujoco.so.`. +set_property(TARGET mujoco PROPERTY VERSION) +set_property(TARGET mujoco PROPERTY SOVERSION) +set_property(TARGET mujoco APPEND PROPERTY LINK_OPTIONS "-Wl,-Bsymbolic") + +# Set up an extension that reaches MuJoCo through mj_api.cpp: headers to compile against, +# libisaacteleop_mujoco.so staged beside the module for the dlopen to find, and a dynamic +# symbol table holding nothing but the entry point. `module_name` is the importable name, +# whose PyInit_ symbol is the one export. +function(isaacteleop_link_mujoco target module_name) + # Headers only. Nothing links MuJoCo, so add_dependencies supplies the build order a + # link line would otherwise have implied. + target_include_directories(${target} PRIVATE + $) + add_dependencies(${target} mujoco) + + # mj_api.cpp opens it by this module's own directory, so the copy has to be there -- + # in the build tree for ctest, and in the staged python_package that install(DIRECTORY) + # turns into the wheel. + add_custom_command(TARGET ${target} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$" "$" + COMMENT "Staging $ beside $") + + # A whitelist, so it covers what pybind11's -fvisibility=hidden misses: symbols from + # static archives (cudart_static) and the typeinfo pybind11 emits for the mjt* enums. + # Measured on robot_twin_py: 1 export, against 11 for -Wl,--exclude-libs,ALL alone. + set(_version_script "${CMAKE_CURRENT_BINARY_DIR}/${target}_exports.map") + file(GENERATE OUTPUT "${_version_script}" + CONTENT "{ global: PyInit_${module_name}; local: *; };\n") + target_link_options(${target} PRIVATE "-Wl,--version-script,${_version_script}") + set_property(TARGET ${target} APPEND PROPERTY LINK_DEPENDS "${_version_script}") +endfunction() diff --git a/docs/source/getting_started/build_from_source/index.rst b/docs/source/getting_started/build_from_source/index.rst index bfb992807d..e989e1e039 100644 --- a/docs/source/getting_started/build_from_source/index.rst +++ b/docs/source/getting_started/build_from_source/index.rst @@ -58,7 +58,7 @@ the list of dependencies. On **Ubuntu**, install build tools and clang-format: .. code-block:: bash sudo apt-get update - sudo apt-get install -y build-essential cmake libx11-dev libwayland-dev clang-format-14 ccache patchelf pkg-config glslang-tools + sudo apt-get install -y build-essential cmake libegl-dev libx11-dev libwayland-dev clang-format-14 ccache patchelf pkg-config glslang-tools Runtime-only dependencies (needed to actually run teleop, not to build): diff --git a/docs/source/references/cloudxr.rst b/docs/source/references/cloudxr.rst index 4fa342684f..6a9acf24ef 100644 --- a/docs/source/references/cloudxr.rst +++ b/docs/source/references/cloudxr.rst @@ -200,7 +200,7 @@ accept ``--no-launch-cloudxr-runtime``. That returns a :class:`~isaacteleop.clou the process does not start or attach to CloudXR and leaves ``XR_RUNTIME_JSON`` and related environment variables unchanged. Use this when another runtime is already configured (for example Monado) or when a host singleton must not be duplicated -(see ``examples/mujoco_xr/README.md`` and ``--no-launch-cloudxr-runtime`` there). +(see ``examples/robot_viz/README.md`` and ``--no-launch-cloudxr-runtime`` there). .. code-block:: bash diff --git a/docs/source/references/retargeting/so101.rst b/docs/source/references/retargeting/so101.rst index cfa69b3d43..af0dbbad85 100644 --- a/docs/source/references/retargeting/so101.rst +++ b/docs/source/references/retargeting/so101.rst @@ -152,7 +152,7 @@ fires against *that* frame's controller pose, not the one it was first squeezed ``ValueInput``-leaf rule as ``MEASURED_BASE_T_EE_INPUT`` applies: a producer that wires it must send the key on every step. -The motivating consumer is ``examples/mujoco_xr``, whose owner shows the operator an SO-101 +The motivating consumer is ``examples/robot_viz``, whose owner shows the operator an SO-101 follower before engaging and withholds permission until the operator's wrist is turned the way that arm is -- see that example's README. diff --git a/examples/mujoco_xr/CMakeLists.txt b/examples/mujoco_xr/CMakeLists.txt deleted file mode 100644 index 51a272a012..0000000000 --- a/examples/mujoco_xr/CMakeLists.txt +++ /dev/null @@ -1,195 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# Orchestrator for the MuJoCo XR example. Defines no target of its own. -# -# Configured two ways, and the branches below turn on which: -# -# IN-TREE add_subdirectory'd from the root. Builds the extension in place -# for ctest and a bare `pytest`, and installs nothing. -# STANDALONE top-level, as scikit-build-core drives it for -# `uv pip install ./examples/mujoco_xr`. Sets up for itself what -# root scope provided: Python3_EXECUTABLE, pybind11, output path. -# -# SKBUILD is not the discriminator: a plain `cmake -B build examples/mujoco_xr` -# has none of the root scope either and must fail for the same reasons. - -cmake_minimum_required(VERSION 3.20) - -# CMAKE_SOURCE_DIR is set before any project() call, so this is legal here. -# PROJECT_IS_TOP_LEVEL is not -- it needs the project() this branch is deciding -# whether to make. -if(CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) - set(_mujoco_xr_standalone TRUE) -else() - set(_mujoco_xr_standalone FALSE) -endif() - -if(_mujoco_xr_standalone) - project(mujoco_xr LANGUAGES CXX) - - # Inherited from the root build in the in-tree case; restated because cpp/ - # compiles as C++20 either way. - set(CMAKE_CXX_STANDARD 20) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_POSITION_INDEPENDENT_CODE ON) - - # Development.Module, not the bare `Development` that - # cmake/SetupPython.cmake:83 uses under SKBUILD: `Development` also demands - # libpython, which the manylinux and uv-managed interpreters that install - # this wheel frequently do not ship. - find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) - - # From the PEP-517 build environment, not FetchContent: an isolated wheel - # build should not clone from GitHub, and nothing pybind11-typed crosses - # this module's boundary. scikit-build-core puts the build env's - # site-packages on CMAKE_PREFIX_PATH, which is what finds it. - find_package(pybind11 CONFIG REQUIRED) -endif() - -# There is deliberately no `option(BUILD_EXAMPLE_MUJOCO_XR ...)`: the probe -# below is the whole gate, and an option on top would report ON while the -# example was skipped. - -# ============================================================================== -# MuJoCo, discovered through the build interpreter's wheel -# ============================================================================== -# No find_package(mujoco): the wheel is the only supported source and is what -# the Python side loads at runtime, so one libmujoco serves both languages. - -# In-tree only in practice: standalone, find_package(Python3 REQUIRED) above has -# already failed the configure. -if(NOT DEFINED Python3_EXECUTABLE) - message(STATUS "mujoco_xr: skipped (Python3_EXECUTABLE is not defined)") - return() -endif() - -# Not hardcoded: the pyproject.toml files are read and cross-checked against the -# installed version, so drift fails the configure rather than surfacing as an -# ImportError on the headset. MATCHALL because pyproject.toml carries the pin -# twice and those must agree -- and because it would match a version written in -# prose too, which is why those comments never restate the number. -set(_mujoco_pin_paths "${CMAKE_CURRENT_SOURCE_DIR}/pyproject.toml") -# The ctest pin lives in the repository's tests/ tree, which the standalone -# wheel configure cannot see -- and has no ctest to keep in sync either. -if(NOT _mujoco_xr_standalone) - list(APPEND _mujoco_pin_paths - "${CMAKE_SOURCE_DIR}/tests/python/examples/mujoco_xr/pyproject.toml") -endif() -set(_mujoco_pins "") -set(_mujoco_pin_labels "") -foreach(_pin_path IN LISTS _mujoco_pin_paths) - if(_pin_path STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/pyproject.toml") - set(_pin_label "pyproject.toml") - else() - set(_pin_label "tests/python/examples/mujoco_xr/pyproject.toml") - endif() - file(READ "${_pin_path}" _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_label} 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_label}") - endforeach() -endforeach() -list(GET _mujoco_pins 0 _mujoco_declared_pin) - -# Before the probe, because the cross-check below only runs on a machine that -# has mujoco. Deduplicated into a COPY: _mujoco_pins must keep one entry per -# pin, or the ZIP_LISTS cross-check silently stops checking the later ones. -set(_mujoco_distinct_pins "${_mujoco_pins}") -list(REMOVE_DUPLICATES _mujoco_distinct_pins) -list(LENGTH _mujoco_distinct_pins _mujoco_distinct_pin_count) -if(NOT _mujoco_distinct_pin_count EQUAL 1) - message(FATAL_ERROR "mujoco_xr: the mujoco pins disagree -- pyproject.toml (both " - "build-system.requires and project.dependencies) and " - "tests/python/examples/mujoco_xr/pyproject.toml must all name the SAME mujoco version " - "(exactly one libmujoco may be loaded in the process). " - "Found: ${_mujoco_pins} in ${_mujoco_pin_labels}") -endif() - -execute_process( - COMMAND "${Python3_EXECUTABLE}" -c - "import mujoco, os; print(mujoco.__version__); print(os.path.dirname(mujoco.__file__))" - OUTPUT_VARIABLE _mujoco_probe - ERROR_VARIABLE _mujoco_probe_err - RESULT_VARIABLE _mujoco_probe_rc - OUTPUT_STRIP_TRAILING_WHITESPACE -) -if(NOT _mujoco_probe_rc EQUAL 0) - # Fatal standalone, where the build IS the example: returning would emit a - # valid wheel with no _mujoco_xr*.so in it, and an unexplained ImportError. - if(_mujoco_xr_standalone) - message(FATAL_ERROR "mujoco_xr: '${Python3_EXECUTABLE}' cannot import mujoco, so the " - "extension cannot be compiled and this wheel would contain no " - "_mujoco_xr*.so at all. pyproject.toml's build-system.requires " - "declares it -- either allow build isolation to install it, or " - "pre-install it into this interpreter: " - "uv pip install --python ${Python3_EXECUTABLE} " - "\"mujoco==${_mujoco_declared_pin}\"") - endif() - # In-tree, the one message between a green build and a silently uncompiled - # example, so it names the exact command. Configure CREATES that - # interpreter, hence the re-configure step in the README. - message(STATUS "mujoco_xr: skipped -- '${Python3_EXECUTABLE}' cannot import mujoco. " - "Install it and re-run cmake --preset with: " - "uv pip install --python ${Python3_EXECUTABLE} \"mujoco==${_mujoco_declared_pin}\"") - return() -endif() - -string(REPLACE "\n" ";" _mujoco_probe_lines "${_mujoco_probe}") -list(GET _mujoco_probe_lines 0 _mujoco_version) -list(GET _mujoco_probe_lines 1 _mujoco_dir) -string(STRIP "${_mujoco_version}" _mujoco_version) -string(STRIP "${_mujoco_dir}" _mujoco_dir) - -# After the probe, so a machine with no mujoco gets the skip message rather than -# a pin complaint. Every pin matters: this module links the installed version's -# SONAME while the pyproject pins decide what the wheel build, the app and ctest -# each resolve -- and exactly one libmujoco may be loaded. -foreach(_pin_file _pin IN ZIP_LISTS _mujoco_pin_labels _mujoco_pins) - # STREQUAL, not VERSION_EQUAL: the regex admits PEP-440 suffixes and - # VERSION_EQUAL discards them, reporting EQUAL for exactly the strings that - # need distinguishing. A pin is an exact string. - if(NOT _pin STREQUAL "${_mujoco_version}") - message(FATAL_ERROR - "mujoco_xr: ${_pin_file} pins mujoco==${_pin}, but '${Python3_EXECUTABLE}' has " - "${_mujoco_version}. The C++ module and the Python app must load ONE libmujoco. Either " - "install the declared pin (uv pip install --python ${Python3_EXECUTABLE} " - "\"mujoco==${_pin}\") or update EVERY pin -- pyproject.toml carries it twice " - "(build-system.requires and project.dependencies) and tests/python/examples/mujoco_xr/pyproject.toml once -- " - "to ${_mujoco_version}.") - endif() -endforeach() - -file(GLOB _mujoco_libs "${_mujoco_dir}/libmujoco.so.*") -list(LENGTH _mujoco_libs _mujoco_lib_count) -if(NOT _mujoco_lib_count EQUAL 1) - message(FATAL_ERROR "mujoco_xr: expected exactly one libmujoco.so.* in ${_mujoco_dir}, " - "found ${_mujoco_lib_count}: ${_mujoco_libs}") -endif() -# Lowercase on purpose: hand-set variables read through inherited scope, not the -# find_package output MUJOCO_LIBRARY / MUJOCO_INCLUDE_DIR would imply. -list(GET _mujoco_libs 0 _mujoco_library) -set(_mujoco_include_dir "${_mujoco_dir}/include") -if(NOT EXISTS "${_mujoco_include_dir}/mujoco/mujoco.h") - message(FATAL_ERROR "mujoco_xr: ${_mujoco_include_dir}/mujoco/mujoco.h is missing " - "(is this a source checkout rather than a wheel?)") -endif() - -# The line to grep for: a green build does not imply this example compiled. -message(STATUS "mujoco_xr: ON (mujoco=${_mujoco_version} lib=${_mujoco_library})") - -# Handed down explicitly: ${CMAKE_SOURCE_DIR} means different things in the two -# configures, and "../" is not allowed in CMake paths here. -set(_mujoco_xr_root "${CMAKE_CURRENT_SOURCE_DIR}") - -add_subdirectory(cpp) - -# No install() rules here, deliberately: the wheel is the only run path, and -# standalone it is cpp/CMakeLists.txt's install(TARGETS) that fills it. diff --git a/examples/mujoco_xr/cpp/CMakeLists.txt b/examples/mujoco_xr/cpp/CMakeLists.txt deleted file mode 100644 index 962cebe891..0000000000 --- a/examples/mujoco_xr/cpp/CMakeLists.txt +++ /dev/null @@ -1,81 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# The pybind11 module `_mujoco_xr` -- MuJoCo's own OpenGL renderer, read back -# into CUDA-visible buffers for viz::ProjectionLayer. -# -# Protocol-only linkage: no viz:: target, because __cuda_array_interface__ is -# the whole interface, and for the same reason this pybind11 need not be the one -# the root build FetchContents. Do not pin them together. -# -# No OpenGL on the link line either -- gl.hpp resolves through the platform -# GetProcAddress, and libGL here would be a second dispatch path for one -# context. Its HEADERS are already implied by CUDA::cudart. -# -# 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(CUDAToolkit REQUIRED) - -pybind11_add_module(mujoco_xr_py - mujoco_xr_bindings.cpp - gl.cpp - gl_readback.cpp - scene_renderer.cpp - frames.hpp - gl.hpp - gl_functions.inc - glcamera.hpp - gl_readback.hpp - scene_renderer.hpp -) - -target_include_directories(mujoco_xr_py - PRIVATE - "${_mujoco_include_dir}" -) - -target_link_libraries(mujoco_xr_py - PRIVATE - # Matches viz_core: no runtime libcudart.so on a driver-only machine. - CUDA::cudart_static - # gl.cpp's loader needs dlopen/dlsym. - ${CMAKE_DL_LIBS} - "${_mujoco_library}" -) - -target_compile_options(mujoco_xr_py PRIVATE -Wall -Wextra) - -set_target_properties(mujoco_xr_py PROPERTIES - OUTPUT_NAME "_mujoco_xr" - # No RPATH, deliberately -- do not "fix" this. __init__.py imports `mujoco` - # first, so the already-loaded library satisfies our NEEDED entry. An RPATH - # would silently load a second libmujoco and hand mjModel* across two - # copies; without one, a mismatch is a clean ImportError. - BUILD_WITH_INSTALL_RPATH ON - INSTALL_RPATH "" -) - -# ============================================================================== -# Where the .so goes, and it is a different place in each configure -# ============================================================================== -if(_mujoco_xr_standalone) - # CMAKE_INSTALL_PREFIX is scikit-build-core's platlib staging root, so the - # DESTINATION must spell the namespace too -- drop `isaacteleop_examples/` - # and the .so lands outside the package. `sdist.exclude` in - # ../pyproject.toml is what stops a stale in-place .so shipping alongside - # it, and that breakage is intermittent: only a cross-ABI build ships two. - install(TARGETS mujoco_xr_py - LIBRARY DESTINATION isaacteleop_examples/mujoco_xr - ) -else() - # In-tree: drop the .so beside __init__.py, which is what tests/conftest.py - # reaches by prepending python/ to sys.path. ${_mujoco_xr_root} rather than - # ${CMAKE_SOURCE_DIR}, which is this directory in the standalone configure. - # This copies and never removes, so a renamed module leaves a stale .so. - set_target_properties(mujoco_xr_py PROPERTIES - LIBRARY_OUTPUT_DIRECTORY "${_mujoco_xr_root}/python/isaacteleop_examples/mujoco_xr" - ) -endif() diff --git a/examples/mujoco_xr/cpp/gl_functions.inc b/examples/mujoco_xr/cpp/gl_functions.inc deleted file mode 100644 index 83bb29164c..0000000000 --- a/examples/mujoco_xr/cpp/gl_functions.inc +++ /dev/null @@ -1,55 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 -// -// Every OpenGL entry point this module calls: the name we call it by, then the -// spelling its glcorearb.h PFNGL...PROC typedef uses. -// -// No include guard, deliberately: gl.hpp and gl.cpp include this three times -// over with MUJOCO_XR_GL defined differently -- declare, define, load -- so no -// two lists can drift and leave an entry point null at the first call. - -MUJOCO_XR_GL(Enable, ENABLE) -MUJOCO_XR_GL(Disable, DISABLE) -MUJOCO_XR_GL(GetError, GETERROR) -MUJOCO_XR_GL(Viewport, VIEWPORT) -MUJOCO_XR_GL(PixelStorei, PIXELSTOREI) -MUJOCO_XR_GL(GetIntegerv, GETINTEGERV) -MUJOCO_XR_GL(ReadPixels, READPIXELS) -MUJOCO_XR_GL(DrawArrays, DRAWARRAYS) -MUJOCO_XR_GL(GenTextures, GENTEXTURES) -MUJOCO_XR_GL(DeleteTextures, DELETETEXTURES) -MUJOCO_XR_GL(BindTexture, BINDTEXTURE) -MUJOCO_XR_GL(TexImage2D, TEXIMAGE2D) -MUJOCO_XR_GL(TexParameteri, TEXPARAMETERI) -MUJOCO_XR_GL(ActiveTexture, ACTIVETEXTURE) -MUJOCO_XR_GL(GenFramebuffers, GENFRAMEBUFFERS) -MUJOCO_XR_GL(DeleteFramebuffers, DELETEFRAMEBUFFERS) -MUJOCO_XR_GL(BindFramebuffer, BINDFRAMEBUFFER) -MUJOCO_XR_GL(FramebufferTexture2D, FRAMEBUFFERTEXTURE2D) -MUJOCO_XR_GL(CheckFramebufferStatus, CHECKFRAMEBUFFERSTATUS) -MUJOCO_XR_GL(GetFramebufferAttachmentParameteriv, GETFRAMEBUFFERATTACHMENTPARAMETERIV) -MUJOCO_XR_GL(BlitFramebuffer, BLITFRAMEBUFFER) -MUJOCO_XR_GL(DrawBuffers, DRAWBUFFERS) -MUJOCO_XR_GL(ReadBuffer, READBUFFER) -MUJOCO_XR_GL(GenBuffers, GENBUFFERS) -MUJOCO_XR_GL(DeleteBuffers, DELETEBUFFERS) -MUJOCO_XR_GL(BindBuffer, BINDBUFFER) -MUJOCO_XR_GL(BufferData, BUFFERDATA) -MUJOCO_XR_GL(GenVertexArrays, GENVERTEXARRAYS) -MUJOCO_XR_GL(DeleteVertexArrays, DELETEVERTEXARRAYS) -MUJOCO_XR_GL(BindVertexArray, BINDVERTEXARRAY) -MUJOCO_XR_GL(CreateShader, CREATESHADER) -MUJOCO_XR_GL(ShaderSource, SHADERSOURCE) -MUJOCO_XR_GL(CompileShader, COMPILESHADER) -MUJOCO_XR_GL(GetShaderiv, GETSHADERIV) -MUJOCO_XR_GL(GetShaderInfoLog, GETSHADERINFOLOG) -MUJOCO_XR_GL(DeleteShader, DELETESHADER) -MUJOCO_XR_GL(CreateProgram, CREATEPROGRAM) -MUJOCO_XR_GL(AttachShader, ATTACHSHADER) -MUJOCO_XR_GL(LinkProgram, LINKPROGRAM) -MUJOCO_XR_GL(GetProgramiv, GETPROGRAMIV) -MUJOCO_XR_GL(GetProgramInfoLog, GETPROGRAMINFOLOG) -MUJOCO_XR_GL(DeleteProgram, DELETEPROGRAM) -MUJOCO_XR_GL(UseProgram, USEPROGRAM) -MUJOCO_XR_GL(GetUniformLocation, GETUNIFORMLOCATION) -MUJOCO_XR_GL(Uniform1i, UNIFORM1I) diff --git a/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp b/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp deleted file mode 100644 index 3f9ec66266..0000000000 --- a/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp +++ /dev/null @@ -1,249 +0,0 @@ -// 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 typed crosses this boundary in either direction. viz::Pose3D / Fov -// are registered in `_viz` and not castable here (this module links no viz -// target), so poses and fovs cross as flat float arrays; mjModel / mjData cross -// as integer addresses, Python owning them and C++ owning mjvScene / mjrContext. - -#include "frames.hpp" -#include "glcamera.hpp" -#include "scene_renderer.hpp" - -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace mujoco_xr -{ -namespace -{ - -namespace py = pybind11; - -// A non-owning view of one CUDA-mapped pack buffer, shaped for viz's -// `cuda_array_to_viz_buffer`: kRGBA8 -> "|u1" (H, W, 4), kD32F -> "(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_; -}; - -CudaImageView image_view(SceneRenderer& r, int view, bool is_depth) -{ - if (view < 0 || static_cast(view) >= r.view_count()) - { - throw std::out_of_range("mujoco_xr: view index out of range"); - } - const Readback& rb = r.readback(); - const auto index = static_cast(view); - void* ptr = is_depth ? rb.depth_ptr(index) : rb.color_ptr(index); - return CudaImageView{ reinterpret_cast(ptr), rb.width(), rb.height(), is_depth }; -} - -} // namespace -} // namespace mujoco_xr - -PYBIND11_MODULE(_mujoco_xr, m) -{ - namespace py = pybind11; - using namespace pybind11::literals; - - m.doc() = "MuJoCo's OpenGL renderer, read back into CUDA for Isaac Teleop's Televiz ProjectionLayer."; - - m.def( - "mujoco_version", []() { return std::string(mj_versionString()); }, - "The libmujoco this extension is linked against, as reported at runtime. Compare with " - "mujoco.mj_versionString() -- they MUST be equal, and they are only equal because there is exactly one " - "libmujoco loaded in the process."); - - // ── Frames ──────────────────────────────────────────────────────────── - // Exposed rather than reimplemented, so frames.hpp stays the one definition. - - m.def( - "mj_from_xr_pos", [](std::array p_xr) { return mujoco_xr::mj_from_xr_pos(p_xr); }, "p_xr"_a, - "XR reference-space point (metres, Y-up) -> MuJoCo world point (Z-up). Applies both the handedness " - "rotation and the workspace translation."); - - 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."); - - // SCREAMING_CASE attributes, not getters: a snake_case getter would put - // `quat_mj_from_xr` beside `mj_from_xr_quat`, with only word order telling a - // constant from a transform. - m.attr("QUAT_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kQuatMjFromXr)); - m.attr("TRANS_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kTransMjFromXr)); - - // ── Projection ──────────────────────────────────────────────────────── - - m.def( - "frustum_from_fov", - [](std::array fov_lrud, float near_z, float far_z) - { - const mujoco_xr::Frustum f = mujoco_xr::frustum_from_fov(fov_lrud, near_z, far_z); - return std::vector{ f.center, f.half_width, f.bottom, f.top, f.near_z, f.far_z }; - }, - "fov_lrud"_a, "near_z"_a, "far_z"_a, - "The mjvGLCamera frustum fields for one asymmetric fov (angle_left, angle_right, angle_up, angle_down) " - "in radians, as (center, half_width, bottom, top, near, far). Same code path the renderer uses; exposed " - "so the convention is testable without a GPU. Raises ValueError on a degenerate fov or a bad near/far."); - - m.def( - "submitted_depth", - [](float distance, float near_z, float far_z) { return mujoco_xr::submitted_depth(distance, near_z, far_z); }, - "distance"_a, "near_z"_a, "far_z"_a, - "What a view-space distance ahead of the eye becomes in the depth buffer handed to " - "ProjectionLayer.submit(): standard Z, near -> 0, far -> 1. MuJoCo's renderer writes the reverse; " - "shaders/readback inverts it."); - - // ── Renderer ────────────────────────────────────────────────────────── - - py::class_(m, "CudaImageView", - R"doc( -Non-owning CUDA view of one of the renderer's pixel-pack buffers. - -Exposes ``__cuda_array_interface__``, which is all -``isaacteleop.viz.ProjectionLayer.submit()`` needs. Do NOT hold one past the -frame it came from, and never past ``Renderer.close()``: the memory belongs to -the renderer and is unmapped on the next ``render()``. -)doc") - .def_property_readonly("__cuda_array_interface__", &mujoco_xr::CudaImageView::cuda_array_interface); - - py::class_(m, "Renderer", - R"doc( -MuJoCo's OpenGL renderer, read back into CUDA-visible colour + depth buffers. - -An OpenGL context must be current on this thread BEFORE construction, on the -same GPU viz chose (``mujoco.GLContext``; set ``MUJOCO_EGL_DEVICE_ID`` if the -machine has more than one card). The constructor checks this and raises rather -than render into another card's memory. - -Per frame, in this order and on ONE thread:: - - 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() -)doc") - .def(py::init(), "width"_a, "height"_a, "view_count"_a, - "near_z"_a, "far_z"_a, "model_address"_a, - "`model_address` is mujoco.MjModel._address. No Vulkan handles: this renderer reaches viz through " - "CUDA alone, and finds viz's GPU as the process's current CUDA device.") - .def( - "update_scene", - [](mujoco_xr::PyRenderer& self, uintptr_t model_address, uintptr_t data_address) - { - 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) - { - // No gil_scoped_release: this all runs on the GL context bound - // to THIS thread, and releasing the GIL would let another - // thread issue GL on a context it does not hold. - self.get().render(poses_xyz_qwxyz, fovs_lrud); - }, - "poses_xyz_qwxyz"_a, "fovs_lrud"_a, - "Render every view. `poses_xyz_qwxyz` is view_count*7 floats (x, y, z, qw, qx, qy, qz) and " - "`fovs_lrud` is view_count*4 (angle_left, angle_right, angle_up, angle_down) -- flatten them from " - "FrameInfo.views.") - .def( - "frustum", [](mujoco_xr::PyRenderer& self, int view) { return self.get().frustum(view); }, "view"_a, - "The mjvGLCamera frustum used for `view` on the last render(), as (center, half_width, bottom, top, " - "near, far), so the caller can assert the convention per frame.") - .def( - "color", - [](mujoco_xr::PyRenderer& self, int view) - { return mujoco_xr::image_view(self.get(), view, /*is_depth=*/false); }, - // keep_alive<0, 1>: the view is a bare device pointer into the - // Renderer's buffers, so a caller who keeps `buf = renderer.color(0)` - // and drops `renderer` would use-after-free at submit time. - py::keep_alive<0, 1>(), "view"_a, - "RGBA8 colour for `view` as a CudaImageView. Valid until the next render().") - .def( - "depth", - [](mujoco_xr::PyRenderer& self, int view) - { return mujoco_xr::image_view(self.get(), view, /*is_depth=*/true); }, - py::keep_alive<0, 1>(), "view"_a, // see color() above - "float32 depth for `view` as a CudaImageView, standard Z: near -> 0.0, far -> 1.0. Valid until the " - "next render().") - .def_property_readonly("view_count", [](mujoco_xr::PyRenderer& self) { return self.get().view_count(); }) - .def_property_readonly("ngeom", [](mujoco_xr::PyRenderer& self) { return self.get().ngeom(); }) - .def_property_readonly("maxgeom", [](mujoco_xr::PyRenderer& self) { return self.get().maxgeom(); }) - .def("close", &mujoco_xr::PyRenderer::close, - "Release the OpenGL and CUDA resources. Must happen while the GL context is still current, so " - "BEFORE mujoco.GLContext.free()."); -} diff --git a/examples/mujoco_xr/pyproject.toml b/examples/mujoco_xr/pyproject.toml deleted file mode 100644 index 57526f94df..0000000000 --- a/examples/mujoco_xr/pyproject.toml +++ /dev/null @@ -1,91 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# This example is its own wheel, and the wheel is the only way to run it: -# -# uv pip install ./examples/mujoco_xr -# python -m isaacteleop_examples.mujoco_xr # needs a headset + CloudXR -# -# Its own wheel because _mujoco_xr links libmujoco: folded into `isaacteleop`, -# that wheel's contents would depend on whether the build host had mujoco. -# `install_python_example()` cannot ship a compiled, ABI-tagged extension. - -[build-system] -requires = [ - "scikit-build-core>=1.0", - # Not pinned to the root build's pybind11: no pybind11-registered type - # crosses this boundary, so the two share no ABI. - "pybind11>=2.12", - # Not optional: the extension compiles against this wheel's headers, and - # without it an isolated build emits a wheel with no extension. Always equal - # to `dependencies` below -- and never restate the number in prose here, - # since CMakeLists.txt matches every `mujoco==` in this file. - "mujoco==3.11.0", -] -build-backend = "scikit_build_core.build" - -[project] -# The dist name mirrors the import path rather than the directory name, so an -# installed example does not claim a bare top-level `mujoco_xr` in -# site-packages, right next to the real `mujoco`. -name = "isaacteleop-examples-mujoco-xr" -version = "0.0.0" # Internal example - not versioned -description = "MuJoCo scene rendered into an Isaac Teleop Televiz XR session" - -# A range, not a pin: scikit-build-core tags the wheel with the installing -# interpreter's ABI. Bounds match ISAAC_TELEOP_PYTHON_VERSION_MIN / -# _MAX_EXCLUSIVE in the root CMakeLists.txt. -requires-python = ">=3.11,<3.14" - -dependencies = [ - # Run-time mujoco, equal to the build-time pin above. Exactly one libmujoco - # may be loaded: mjModel* / mjData* addresses cross the pybind boundary. - "mujoco==3.11.0", - # Unversioned, and a live hazard: a published isaacteleop exists on PyPI, so - # this resolves happily against a release that is not this checkout's viz. - # Install the locally built wheel first, into the same environment: - # uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ - "isaacteleop", - # app.py imports numpy directly. mujoco would pull it in anyway, but a - # transitive dependency imported by name breaks on an upstream change. - "numpy", -] - -[tool.scikit-build] -# Floor matches CMakeLists.txt's cmake_minimum_required; the <4 cap mirrors the -# root pyproject.toml. -cmake.version = ">=3.20,<4" -cmake.build-type = "Release" - -# Persistent, so `uv pip install --reinstall-package ...` stays incremental -# instead of reconfiguring from scratch. `{cache_tag}` keeps per-interpreter -# caches apart. `**/build/` is gitignored. -build-dir = "build/wheel-{cache_tag}" - -# `wheel.packages` owns every authored file under the package; CMake's -# install(TARGETS) owns the one build-produced file. Their intersection must be -# empty, and without this line it is not, because the in-tree build drops its -# own .so there for ctest. Measured: omit it and a cp313 wheel ships the root -# build's stale cp312 .so as well. -# -# `sdist.exclude`, not `wheel.exclude`: the latter is applied twice, so `*.so` -# there would delete the freshly compiled extension too. -[tool.scikit-build.sdist] -exclude = ["python/isaacteleop_examples/mujoco_xr/*.so"] - -# Key is the path inside the wheel, value the source directory. -# -# `isaacteleop_examples` is deliberately not listed: it is a PEP 420 namespace -# with no __init__.py and no owner, and scikit-build-core creates the -# intermediate directory from this key. Adding an __init__.py there (or listing -# the directory as a package) makes it a regular package owned by this wheel, -# and a second example distribution then collides or is shadowed. -[tool.scikit-build.wheel] -packages = { "isaacteleop_examples/mujoco_xr" = "python/isaacteleop_examples/mujoco_xr" } - -# The absent [tool.scikit-build.editable] block is deliberate. `pip install -e` -# is not supported: an editable install redirects the package back to the source -# tree, which is where the in-tree CMake build drops its own _mujoco_xr*.so, so -# you would silently import that one instead. `mode = "redirect"` is already the -# default, so adding the block would not help. Use -# `uv pip install --reinstall-package isaacteleop-examples-mujoco-xr .` instead. diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py deleted file mode 100644 index 8ce4f1cdee..0000000000 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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.""" - -import os as _os - -# Must precede `import mujoco`, which reads MUJOCO_GL at import time. EGL rather -# than the GLFW default because this renders offscreen, usually with no display, -# and only the EGL path honours MUJOCO_EGL_DEVICE_ID. setdefault, so an explicit -# MUJOCO_GL still wins. -_os.environ.setdefault("MUJOCO_GL", "egl") - -# Load order is load-bearing -- do not let an import sorter move this. `import -# mujoco` pulls the wheel's libmujoco in first, and `_mujoco_xr` has a NEEDED -# entry for that same SONAME with no RPATH, so it binds to the already-loaded -# copy. That is what guarantees one libmujoco, and so one mjModel* layout. -import mujoco as _mujoco - -from . import _mujoco_xr - -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/app.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py deleted file mode 100644 index a1a8d8b057..0000000000 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py +++ /dev/null @@ -1,1359 +0,0 @@ -# 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 MuJoCo's own renderer and reaches -ProjectionLayer.submit() by CUDA pointer, never through host memory. - - VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession - │ │ - │ recommended resolution │ EePoseRateLimiter output - ▼ ▼ │ - _mujoco_xr.Renderer ──__cuda_array_interface__──▶ ProjectionLayer │ - ▲ │ - └──────────────── mjData.mocap_pos/_quat ◀─────────────────────┘ - -The renderer needs an OpenGL context current on this thread; viz and the renderer -meet through CUDA alone, on VizSession's GPU. C++ owns -mjvScene/mjvOption/mjvCamera/mjrContext, Python owns mjModel/mjData. - -Nothing is integrated -- mj_step is never called -- which makes one invariant -load-bearing: **one mj_forward after every `qpos`, `body_pos` or `mocap_*` write, -before every `xpos` / `xquat` / `geom_xpos` read, including the read inside -mjv_updateScene.** `mocap_*` is an INPUT to forward kinematics; the renderer draws -`geom_xpos`. See README.md for the rest of the design. -""" - -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 ( - ExecutionEvents, - ExecutionState, - OutputCombiner, - TensorGroup, - ValueInput, -) -from isaacteleop.retargeting_engine.interface.tensor_group_type import ( - OptionalType, - TensorGroupType, -) -from isaacteleop.retargeting_engine.tensor_types import BoolType, ControllerInputIndex -from isaacteleop.retargeters.rate_limiter import ( - EE_POSE_KEY, - EePoseRateLimiter, - RateLimiterConfig, -) -from isaacteleop.retargeters.SO101.clutch_retargeter import SO101ClutchRetargeter -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, follower -from .harness import ControllerPoseSource, HandPose, HarnessBand, InterventionMonitor - -LOG = logging.getLogger("mujoco_xr") - -# The app's only clip planes. VizSessionConfig, the projection and the submitted depth -# must all agree, or world-locked geometry swims under head motion, and only a headset -# shows it. There is no near/far literal in cpp/, by construction. -NEAR_Z = 0.05 -FAR_Z = 50.0 - -# Wall-clock ceiling for one advance of the app's own clock. See _clamp_dt. -MAX_DT_S = 0.1 - -# The only mode: this needs a headset and a CloudXR runtime. A headless fallback -# should arrive with the CI job that runs it (NVIDIA/IsaacTeleop#880). -_DISPLAY_MODE = viz.DisplayMode.kXr - -# layer.submit() in _loop is spelled out per eye and cannot read this name, so -# changing it means editing that call too. -_VIEW_COUNT = 2 - -_CLOCK_SOURCE = ( - "FrameInfo.predicted_display_time; frames with no prediction are skipped, " - "not sampled as 0" -) - -# Package data, so it resolves the same from the wheel and the source tree. Keep it -# ABSOLUTE: on mujoco 3.11.0 a relative model path composes scene.xml's nested -# path onto itself and opens `//so101_new_calib.xml`. -DEFAULT_SCENE = Path(__file__).parent / "assets" / "scene.xml" - -# Checked by name before MuJoCo sees the scene: its failure for a missing -# target is a bare "Error opening file .stl", naming a file -# nobody asked for. -FETCH_SCRIPT = "examples/mujoco_xr/scripts/fetch-so-arm.sh" -_ASSETS = Path(__file__).parent / "assets" -_LEADER_ASSETS = _ASSETS / "leader" -_FETCHED = ( - "leader/Wrist_Roll_SO101.stl", - "leader/Trigger_SO101.stl", - "leader/Handle_SO101.stl", - "leader/STS3215_03a.stl", - "follower/so101_new_calib.xml", - "follower/base_motor_holder_so101_v1.stl", - "follower/base_so101_v2.stl", - "follower/motor_holder_so101_base_v1.stl", - "follower/motor_holder_so101_wrist_v1.stl", - "follower/moving_jaw_so101_v1.stl", - "follower/rotation_pitch_so101_v1.stl", - "follower/sts3215_03a_no_horn_v1.stl", - "follower/sts3215_03a_v1.stl", - "follower/under_arm_so101_v1.stl", - "follower/upper_arm_so101_v1.stl", - "follower/waveshare_mounting_plate_so101_v2.stl", - "follower/wrist_roll_follower_so101_v1.stl", - "follower/wrist_roll_pitch_so101_v2.stl", -) - - -def _missing_assets() -> list[str]: - """The fetched files that are not on disk. Empty when fetched. - - Both tools, because one scene includes both. Hand-kept in step with the ``ASSETS`` - list in ``scripts/fetch-so-arm.sh``: a destination renamed in one and not the other - makes this demand a file nothing fetches, or miss one the scene needs. - """ - return [n for n in _FETCHED if not (_ASSETS / n).is_file()] - - -# One hand and no flag: the ghost is a right-handed gripper. -GHOST_HAND = ControllersSource.RIGHT - -# The two mocap bodies leader_gripper.xml declares. -GHOST_BODY = "leader_ghost" -GHOST_JAW_BODY = "leader_ghost_jaw" - -# The four ghost geoms, hidden as a set whenever the follower is the tool on -# show. Named here rather than discovered, so a renamed geom is an error. -GHOST_GEOMS = ( - "leader_ghost_wrist_roll", - "leader_ghost_motor", - "leader_ghost_handle", - "leader_ghost_trigger", -) - -# Three pose channels, three jobs, and none substitutes for another. HAND_POSE_KEY is -# Optional and is the app's ONLY tracking-loss oracle; it is what drives the follower -# and the gate's rotation operand. COMMANDED_POSE_KEY is what the limiter was handed, -# the reference its band is measured against; it is required, so it can never signal -# loss. EE_POSE_KEY is the limiter's output and the only channel anything draws. -HAND_POSE_KEY = "hand_pose" -COMMANDED_POSE_KEY = "commanded_ee_pose" - -# The one hand frame, reaching the follower's drive, the gate's operand, the clutch's -# home and the ghost. Aim because only aim's -Z is a pointing ray; grip's runs little -# finger to thumb, whose azimuth turns 1:1 with the hand but has an arbitrary zero. The -# cost is aim's device-specific ray origin, which gives the arm's position a lever arm -# as the wrist turns. Everything relative to this frame must be re-derived when it -# changes. See README.md. -HAND_POSE = HandPose.AIM - -# The B button. ControllerInput carries no field of that name: the OpenXR bindings put -# `/user/hand/right/input/b/click` on SECONDARY_CLICK -# (live_controller_tracker_impl.cpp:292). GHOST_HAND is the right controller. -_RESET_OFFSET_BUTTON = ControllerInputIndex.SECONDARY_CLICK - -# The A button, held to put the right thumbstick on the yaw trim instead of the grip -# offset. Modal rather than a second stick because only one controller is wired. -_YAW_TRIM_BUTTON = ControllerInputIndex.PRIMARY_CLICK - -# The external graph leaf carrying the engage gate's verdict. TeleopSession validates -# every external leaf name is present in external_inputs on EVERY step, independently -# of OptionalType, so this key is sent unconditionally. -ENGAGE_PERMISSION_LEAF = "engage_permission" - -# The clutch's own degenerate-quaternion threshold (clutch_retargeter.py:109), -# restated so `after_step`'s usable-pose test covers its whole disarm set. Reused -# for the head pose, which has no such consumer but is just as unusable at zero. -_MIN_QUAT_NORM = 1e-6 - -# ── Where the ghost sits on the hand ─────────────────────────────────── -# Euler degrees, intrinsic XYZ, i.e. MuJoCo's `euler=`. Solve this rotation from Q_HOME; -# do not port a grip-measured value, which demands a wrist pitch nobody chose. It fixes -# the posture the gate asks for, and this value makes that posture level and unrolled. -# Re-solve when Q_HOME moves: it is the gripper's xquat at Q_HOME and base yaw 0, carried -# into XR by _xr_from_mj_quat, as intrinsic-XYZ Euler. -_EULER_HAND_FROM_GHOST_DEG = (270, 0, 90) -# Measured on a headset: a claim about a hand holding a CONTROLLER, so do not re-derive -# it from the mesh. Relative to HAND_POSE; `_log_hand_frames` prints the replacement -# when that changes. -_POS_HAND_FROM_GHOST = np.array((0, 0, 0)) - -# ── 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 leader's trigger sits in the 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: 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. Do not extend to the joint's lower limit -# (-10 deg): that end swings the lever 0.4 mm into the servo. -_TRIGGER_RELEASED_RAD = math.radians(100.0) # closedness 0, jaw wide open -_TRIGGER_SQUEEZED_RAD = 0.0 # closedness 1, tucked to the authored pose - - -def _quat_from_euler_deg(angles_deg) -> np.ndarray: - """Intrinsic X-then-Y-then-Z degrees -> a wxyz quaternion, MuJoCo's `euler=`. - - Right-multiplication is what makes it intrinsic. Spelled out rather than calling - mju_euler2Quat so the sequence is visible where it is used. - """ - quat = np.array((1.0, 0.0, 0.0, 0.0)) - for axis, angle in zip(np.eye(3), angles_deg): - step = np.empty(4) - mujoco.mju_axisAngle2Quat(step, axis, math.radians(angle)) - composed = np.empty(4) - mujoco.mju_mulQuat(composed, quat, step) - quat = composed - return quat - - -# ── Derived below; nothing from here on is authored ──────────────────────── -_QUAT_HAND_FROM_GHOST = _quat_from_euler_deg(_EULER_HAND_FROM_GHOST_DEG) - - -def _clamp_dt(dt: float) -> float: - """NaN-safe clamp into [0, MAX_DT_S]. - - Comparisons, not min/max: max(nan, 0) is nan, so the obvious form passes NaN - through both limits and into whatever accumulates it. - """ - if dt > 0: - return MAX_DT_S if dt > MAX_DT_S else dt - return 0.0 - - -# ── What the harness lets through ────────────────────────────────────────── -# Chosen for this demo, not measured against a follower: ordinary reaching passes -# through and a deliberate flick trips the clamp and then the reject band. An -# SO-101's own envelope is lower -- RateLimiterConfig defaults to 0.25 m/s. -_HARNESS = RateLimiterConfig( - max_linear_velocity=0.5, # m/s - max_angular_velocity=2.5, # rad/s, ~143 deg/s - reject_linear_velocity=2.0, # m/s - reject_angular_velocity=10.0, # rad/s -) - - -_PERMISSION_TYPE = TensorGroupType( - SO101ClutchRetargeter.ENGAGE_PERMITTED_INPUT, [BoolType("permitted")] -) - - -def _permission(permitted: bool) -> TensorGroup: - """One frame of the permission leaf's payload. BoolType wants a real Python bool.""" - group = TensorGroup(_PERMISSION_TYPE) - group[SO101ClutchRetargeter.PERMITTED_INDEX] = bool(permitted) - return group - - -def _build_pipeline( # noqa: N803 - home_base_T_ee: np.ndarray, -) -> tuple[OutputCombiner, SO101ClutchRetargeter]: - """Controllers, the SO-101 jaw and clutch retargeters, and the pose harness. - - ControllerPoseSource is a parallel branch rather than a link in the clutch's chain: its - Optional output is the app's only tracking-validity oracle. The jaw is ungoverned. - Returns the clutch too, because the app reads `is_engaged` off it. - """ - controllers = ControllersSource(name="controllers") - jaw = SO101GripperRetargeter(name="ghost_jaw", input_device=GHOST_HAND).connect( - {GHOST_HAND: controllers.output(GHOST_HAND)} - ) - hand = ControllerPoseSource( - name="hand_pose", pose=HAND_POSE, input_device=GHOST_HAND - ).connect({GHOST_HAND: controllers.output(GHOST_HAND)}) - permission = ValueInput(ENGAGE_PERMISSION_LEAF, OptionalType(_PERMISSION_TYPE)) - clutch = SO101ClutchRetargeter( - name="ee_pose", - home_base_T_ee=home_base_T_ee, - input_device=GHOST_HAND, - # The same frame the rest of the app drives from. Its orientation delta is - # invariant to the choice, so this is here for the translation pivot alone. - controller_pose=HAND_POSE.value, - ) - # MEASURED_BASE_T_EE_INPUT is left unwired on purpose: it is position-only - # (its own docstring carries the measurement), so it cannot put the leader on - # the follower's orientation. - commanded = clutch.connect( - { - GHOST_HAND: controllers.output(GHOST_HAND), - SO101ClutchRetargeter.ENGAGE_PERMITTED_INPUT: permission.output( - ValueInput.VALUE - ), - } - ) - governed = EePoseRateLimiter(name="ghost_harness", config=_HARNESS).connect( - {EE_POSE_KEY: commanded.output(EE_POSE_KEY)} - ) - return ( - OutputCombiner( - { - ControllersSource.LEFT: controllers.output(ControllersSource.LEFT), - ControllersSource.RIGHT: controllers.output(ControllersSource.RIGHT), - GRIPPER_COMMAND_KEY: jaw.output(GRIPPER_COMMAND_KEY), - HAND_POSE_KEY: hand.output(EE_POSE_KEY), - COMMANDED_POSE_KEY: commanded.output(EE_POSE_KEY), - EE_POSE_KEY: governed.output(EE_POSE_KEY), - } - ), - clutch, - ) - - -def _head_pose(info) -> np.ndarray | None: - """FrameInfo.views[0] as a 7-D XR pose (position, xyzw), or None if unusable. - - The left eye rather than a head centre, which the runtime does not report; the - ~32 mm between them is well inside what HOME_GRIP_FROM_HEAD_XR is authored to. - """ - if len(info.views) == 0: - return None - view = info.views[0] - px, py, pz = view.pose.position - qw, qx, qy, qz = view.pose.orientation - pose = np.array([px, py, pz, qx, qy, qz, qw], dtype=float) - if ( - not np.all(np.isfinite(pose)) - or float(np.linalg.norm(pose[3:7])) < _MIN_QUAT_NORM - ): - return None - return pose - - -def _flatten_xr_views(info) -> tuple[list[float], list[float]]: - """FrameInfo.views -> the flat float arrays the renderer takes. - - Field by field, never sliced: viz.Pose3D.orientation is (w,x,y,z) while a - controller's GRIP_ORIENTATION is (x,y,z,w). - """ - poses: list[float] = [] - fovs: list[float] = [] - for view in info.views: - px, py, pz = view.pose.position - qw, qx, qy, qz = view.pose.orientation - poses.extend((px, py, pz, qw, qx, qy, qz)) - fovs.extend( - ( - view.fov.angle_left, - view.fov.angle_right, - view.fov.angle_up, - view.fov.angle_down, - ) - ) - return poses, fovs - - -def _assert_frustum(f: list[float], fov, near: float, far: float) -> None: - """The frustum handed to mjvGLCamera, checked against the fov it came from. - - `f` is (center, half_width, bottom, top, near, far). The projection's shape - is MuJoCo's business; which numbers reach it is this app's. - """ - center, half_width, bottom, top, f_near, f_far = f - - # At zero half_width mjr_render derives the horizontal extent from the - # viewport aspect, rendering something plausible from a fov carrying nothing. - assert half_width > 0.0 and top > bottom, ( - f"degenerate frustum {f}: a zeroed Fov reached the camera" - ) - # float32 tolerances throughout: the frustum crosses as C floats, so an - # exact comparison against a Python float fails on rounding alone. - for name, got, want in ( - ("left", center - half_width, near * math.tan(fov.angle_left)), - ("right", center + half_width, near * math.tan(fov.angle_right)), - ("bottom", bottom, near * math.tan(fov.angle_down)), - ("top", top, near * math.tan(fov.angle_up)), - ): - assert abs(got - want) <= 1e-6 * max(1.0, abs(want)), ( - f"frustum {name}={got}, expected {want}" - ) - - # viz's XrCompositionLayerDepthInfoKHR pair must be the encoding pair, or - # the runtime reprojects against the wrong range. - assert abs(f_near - near) <= 1e-6 * near and abs(f_far - far) <= 1e-6 * far, ( - f"clip planes drifted: camera has ({f_near}, {f_far}), viz was told ({near}, {far})" - ) - - -def _log_startup(resolution, gl_backend: str) -> None: - """One block naming every assumption that is invisible at runtime.""" - try: - version = importlib.metadata.version("isaacteleop") - except importlib.metadata.PackageNotFoundError: - version = "" - trans = _mujoco_xr.TRANS_MJ_FROM_XR - - LOG.info("scene: %s", DEFAULT_SCENE) - # Several examples ship their own .venv, and picking up the wrong - # isaacteleop is invisible without this line. - LOG.info( - "isaacteleop: %s (version %s)", Path(viz.__file__).resolve().parent, version - ) - LOG.info( - "mujoco: %s (extension links %s)", - mujoco.mj_versionString(), - _mujoco_xr.mujoco_version(), - ) - LOG.info( - "views: %d (stereo) view resolution: %sx%s", - _VIEW_COUNT, - resolution.width, - resolution.height, - ) - LOG.info( - "renderer: MuJoCo's own (mjr_render), OpenGL backend %s, offsamples=0; " - "blitted, y-flipped, depth-inverted, read back through a PBO CUDA imports", - gl_backend, - ) - 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 this session's reference space does not establish (cpp/frames.hpp)", - trans[0], - trans[1], - trans[2], - ) - LOG.info("clock: %s", _CLOCK_SOURCE) - LOG.info( - "depth: D32F requested. Whether the runtime ACCEPTED it is not queryable, so " - "the absence of errors is not confirmation." - ) - - -class _GhostChannels(NamedTuple): - """The ghost's two mocap rows, resolved once at startup. - - Mocap indices, not body ids: mocap_pos/mocap_quat index by body_mocapid, - and a body id there writes into another body's row. - """ - - body: int - jaw: int - - -def _resolve_ghost(model) -> _GhostChannels: - """Both ghost mocap rows. The shipped scene always declares them.""" - body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_BODY) - jaw = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_JAW_BODY) - if body < 0 or jaw < 0: - raise RuntimeError( - f"mujoco_xr: {DEFAULT_SCENE} declares no `{GHOST_BODY}` / " - f"`{GHOST_JAW_BODY}` pair; it must assets/leader/leader_gripper.xml." - ) - return _GhostChannels(int(model.body_mocapid[body]), int(model.body_mocapid[jaw])) - - -def _pose(result, key: str) -> np.ndarray | None: - """One of the pipeline's three 7-D pose channels, or None when it carries nothing. - - Carrying nothing has two spellings and `is_none` is only one: for the two REQUIRED - channels it is hardcoded False, and the limiter keeps a path that returns without - writing (rate_limiter.py:424-427). Reading an unset tensor raises, and there is no - "has it been set" predicate, so that raise is the other spelling. - """ - pose = result[key] - if pose.is_none: - return None - try: - tensor = pose[0] - except ValueError: - return None - return np.asarray(np.from_dlpack(tensor), dtype=float) - - -def _xr_from_mj_pos(p_mj: np.ndarray) -> np.ndarray: - """MuJoCo world point -> XR reference-space point. - - The inverse of `_mujoco_xr.mj_from_xr_pos`, derived from the same two exported - constants so there is still only one definition of the frame. - """ - out = np.empty(3) - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, np.array(_mujoco_xr.QUAT_MJ_FROM_XR, dtype=float)) - mujoco.mju_rotVecQuat( - out, - np.asarray(p_mj, dtype=float) - np.array(_mujoco_xr.TRANS_MJ_FROM_XR), - inverse, - ) - return out - - -def _xr_from_mj_quat(q_wxyz: np.ndarray) -> np.ndarray: - """MuJoCo world orientation (wxyz) -> XR orientation as xyzw.""" - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, np.array(_mujoco_xr.QUAT_MJ_FROM_XR, dtype=float)) - q_xr = np.empty(4) - mujoco.mju_mulQuat(q_xr, inverse, np.asarray(q_wxyz, dtype=float)) - return np.array([q_xr[1], q_xr[2], q_xr[3], q_xr[0]]) - - -def ghost_body_from_pose(pose: np.ndarray) -> tuple[np.ndarray, np.ndarray]: - """A 7-D XR hand pose -> where the leader ghost BODY goes in MuJoCo world. - - The grip calibration lives on this side of the boundary, so follower.py never owns - it. _QUAT_HAND_FROM_GHOST right-multiplies because it is fixed in the gripper's - frame; left-multiplying swings the ghost around the room as the operator turns. - """ - p_xr = [float(pose[0]), float(pose[1]), float(pose[2])] - q_xyzw = [float(pose[3]), float(pose[4]), float(pose[5]), float(pose[6])] - - 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_HAND_FROM_GHOST) - p_offset = np.empty(3) - mujoco.mju_rotVecQuat(p_offset, _POS_HAND_FROM_GHOST, q_grip) - return p_grip + p_offset, q_body - - -def _grip_quat_mj(q_body: np.ndarray) -> np.ndarray: - """The MuJoCo grip orientation (wxyz) whose ghost body lands at ``q_body``.""" - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, _QUAT_HAND_FROM_GHOST) - q_grip = np.empty(4) - mujoco.mju_mulQuat(q_grip, np.asarray(q_body, dtype=float), inverse) - return q_grip - - -def grip_quat_from_ghost_body(q_body: np.ndarray) -> np.ndarray: - """The XR hand orientation (xyzw) that would put the ghost body at ``q_body``. - - The engage gate's second operand: it is what the clutch will latch, so the gate - compares the hand against this rather than the tool's own orientation. Both - operands are xyzw in XR, which is what makes a geodesic angle meaningful. - """ - return _xr_from_mj_quat(_grip_quat_mj(q_body)) - - -def pose_from_ghost_body(p_body: np.ndarray, q_body: np.ndarray) -> np.ndarray: - """The exact inverse of :func:`ghost_body_from_pose`, as a 4x4 in the XR frame. - - 4x4 because its one consumer is ``SO101ClutchRetargeter.set_home_base_T_ee``, and - XR because that is the frame the clutch's controller stream is already in -- the - app does no rebase, so "base" is the XR anchor. - """ - q_grip = _grip_quat_mj(q_body) - p_offset = np.empty(3) - mujoco.mju_rotVecQuat(p_offset, _POS_HAND_FROM_GHOST, q_grip) - - transform = np.eye(4) - transform[:3, 3] = _xr_from_mj_pos(np.asarray(p_body, dtype=float) - p_offset) - rot = np.empty(9) - q_xyzw = _xr_from_mj_quat(q_grip) - mujoco.mju_quat2Mat(rot, np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]])) - transform[:3, :3] = rot.reshape(3, 3) - return transform - - -def _update_ghost( - data, ghost: _GhostChannels, pose: np.ndarray, closedness: float -) -> None: - """Lock the leader gripper to the governed pose; swing its trigger. - - `pose` is the harness output, not the controller. Both arguments must be held - frozen by the caller on an untracked frame -- (0, 0, 0) is the scene origin, and a - jaw articulating on a frozen body reads as an actuated gripper. Writes `mocap_*` - only; the caller owns the mj_forward. - """ - p_body, q_body = ghost_body_from_pose(pose) - - data.mocap_pos[ghost.body] = p_body - data.mocap_quat[ghost.body] = q_body - - # Rotated ABOUT the hinge, not placed at it: the jaw's XML rest pose equals the - # ghost's, so the pivot lives in exactly one place. - angle = _TRIGGER_RELEASED_RAD + closedness * ( - _TRIGGER_SQUEEZED_RAD - _TRIGGER_RELEASED_RAD - ) - q_hinge = np.empty(4) - mujoco.mju_axisAngle2Quat(q_hinge, _TRIGGER_HINGE_AXIS, angle) - q_jaw = np.empty(4) - mujoco.mju_mulQuat(q_jaw, q_body, q_hinge) - - # Rotating the ghost frame about the hinge maps 0 to (pivot - R_hinge.pivot). - swung = np.empty(3) - mujoco.mju_rotVecQuat(swung, _TRIGGER_HINGE_POS, q_hinge) - offset = np.empty(3) - mujoco.mju_rotVecQuat(offset, _TRIGGER_HINGE_POS - swung, q_body) - - data.mocap_pos[ghost.jaw] = p_body + offset - data.mocap_quat[ghost.jaw] = q_jaw - - -# ── The wrist posture the gate will demand ───────────────────────────────── -# _QUAT_HAND_FROM_GHOST cancels EXACTLY through the engage handoff, so no geometric -# test can discriminate it; its one surviving effect is the wrist posture the operator -# must adopt, which is what this reports. Only the THUMB direction carries it -- the -# tool direction reads 15.2 deg for every calibration swept, so it is a guard on -# Q_HOME. Both are in the OPERATOR'S frame. See README.md. -_OPERATOR_FORWARD = np.array((0.0, 0.0, -1.0)) -# Handle centroid to wrist-roll centroid in the ghost body frame, measured on the -# fetched meshes: (-56.9, -0.5, -63.2) mm -> (-4.3, -1.4, -13.2) mm. Rotated below by -# the FOLLOWER `gripper` quaternion, because the handoff puts the ghost body on its -# orientation exactly even though the position comes from the hand. -_GHOST_POINTING_AXIS = np.array((0.7228, -0.0124, 0.6910)) -# The -Z of whatever HAND_POSE names, reported so the demanded posture is legible. What -# it MEANS depends on that constant: on GRIP it is the thumb axis, up through the fist; -# on AIM it is the pointing ray. The angle is only "comfortable to hold" on the reading -# that matches, so read the log's wording and not just the number. -_HAND_REPORT_AXIS = np.array((0.0, 0.0, -1.0)) -# Warned on, never asserted: test-bounding the posture angle would pin it and -# make _EULER_HAND_FROM_GHOST_DEG untunable on a headset, which is the one place it -# can be tuned. -_POSTURE_LIMIT_DEG = 45.0 - -# Which axis of HAND_POSE the yaw drive reads: aim's -Z, the pointing ray. Every axis -# tracks a world-vertical turn 1:1 and is blind to rotation about itself, so the choice -# only decides how much wrist roll and pitch leak into the arm's yaw. README.md -# tabulates the leak per candidate, measured on grip; aim's is unmeasured here because -# the grip-to-aim transform is per-device. Re-measure on a headset before trusting it. -_HAND_FORWARD_AXIS = np.array((0.0, 0.0, -1.0)) - -# Whatever azimuth is left between where the operator means to point and what the app -# reads. On AIM this SHOULD be zero -- that is the entire reason for reading aim -- so a -# session that has to dial in a large value is evidence the switch did not do its job, -# not a knob to lean on. Kept because it is the only thing that can absorb a runtime -# whose aim convention differs from the operator's expectation. Tuned on a headset: hold -# A and push the right thumbstick, then paste back what the app prints. Degrees, positive -# turning the arm the way a positive XR yaw does, applied as a constant on top of the -# reading so it cannot introduce leakage of its own. -_YAW_TRIM_DEG = 0.0 -_YAW_TRIM_RATE_DEG_S = 20.0 - - -def hand_facing_xr(q_hand_xyzw: np.ndarray) -> np.ndarray: - """The XR yaw (wxyz) the operator's hand is facing, for the follower's base.""" - return follower.yaw_of_axis(q_hand_xyzw, _HAND_FORWARD_AXIS) - - -def _log_hand_frames(result) -> bool: - """Measure the device's grip-to-aim transform and print the calibration it implies. - - The grip-to-aim transform is per-device, so no constant can carry - :data:`_POS_HAND_FROM_GHOST` across a :data:`HAND_POSE` change -- but the runtime - publishes both poses on one controller, so one frame with both valid yields the - replacement. Position only: porting the rotation would hand back the wrist pitch that - solving it from Q_HOME removes. Returns whether it got a reading. Never raises. - """ - try: - controller = result[GHOST_HAND] - if controller.is_none or not ( - bool(controller[ControllerInputIndex.GRIP_IS_VALID]) - and bool(controller[ControllerInputIndex.AIM_IS_VALID]) - ): - return False - grip = np.asarray( - controller[ControllerInputIndex.GRIP_ORIENTATION], dtype=float - ) - aim = np.asarray(controller[ControllerInputIndex.AIM_ORIENTATION], dtype=float) - grip_pos = np.asarray( - controller[ControllerInputIndex.GRIP_POSITION], dtype=float - ) - aim_pos = np.asarray(controller[ControllerInputIndex.AIM_POSITION], dtype=float) - if min(np.linalg.norm(aim), np.linalg.norm(grip)) < _MIN_QUAT_NORM: - return False - except (ValueError, IndexError, TypeError): - return False - - # aim^-1 . grip, wxyz: what carries a direction from the GRIP frame into AIM's. - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, aim[[3, 0, 1, 2]]) - aim_from_grip = np.empty(4) - mujoco.mju_mulQuat(aim_from_grip, inverse, grip[[3, 0, 1, 2]]) - # The ghost offset is applied in the hand's frame, so carrying it across costs both - # terms: the origins' separation pulled back into the new frame, and the old offset - # turned by the same rotation the orientation above is. Dropping the second leaves - # the ghost centimetres out while its orientation looks perfect. - separation = np.empty(3) - mujoco.mju_rotVecQuat(separation, grip_pos - aim_pos, inverse) - turned = np.empty(3) - mujoco.mju_rotVecQuat( - turned, np.asarray(_POS_HAND_FROM_GHOST, dtype=float), aim_from_grip - ) - offset = separation + turned - - LOG.info( - "hand frames: this device's aim pose sits %.0f deg and %.0f mm off its grip " - "pose. HAND_POSE is %s, so for the ghost to sit where it did on GRIP, its " - "position wants:", - math.degrees(2.0 * math.acos(min(1.0, abs(float(aim_from_grip[0]))))), - 1000.0 * float(np.linalg.norm(grip_pos - aim_pos)), - HAND_POSE.value.upper(), - ) - LOG.info( - "hand frames: _POS_HAND_FROM_GHOST = np.array((%.3f, %.3f, %.3f))", *offset - ) - return True - - -def base_yaw_bias(arm) -> np.ndarray: - """How far the base yaw must LEAD the hand for the JAW to face it (wxyz). - - Measured off the arm at startup rather than authored: how far the jaw sits off its - base yaw follows from Q_HOME and upstream's chain, J5 above all. Both operands are - yaws about +Y, so the order is free. - """ - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, arm.jaw_yaw_xr) - bias = np.empty(4) - mujoco.mju_mulQuat(bias, arm.base_yaw_xr, inverse) - return bias - - -def _log_grip_posture(arm) -> tuple[float, float]: - """Invert the chain at Q_HOME and report the posture the gate will ask for. - - Both angles are un-yawed into the operator's own frame, so they read the same - whichever way they face -- which is also what lets this run before the anchor. Warns - rather than raises: this app is the only place the calibration can be judged. - """ - p_body, q_body = arm.gripper_pose_mj() - ghost_axis = np.empty(3) - mujoco.mju_rotVecQuat(ghost_axis, _GHOST_POINTING_AXIS, q_body) - # The OPERATOR's frame is the HAND's yaw, which the base leads by base_yaw_bias. - # Un-yawing by the base instead reports the demand a whole bias out, which is - # invisible while that bias is small and wrong by 93 degrees once it is not. - inverse_bias, hand_yaw, unyaw = np.empty(4), np.empty(4), np.empty(4) - mujoco.mju_negQuat(inverse_bias, base_yaw_bias(arm)) - mujoco.mju_mulQuat(hand_yaw, arm.base_yaw_xr, inverse_bias) - mujoco.mju_negQuat(unyaw, hand_yaw) - - def in_operator_frame(direction_xr): - out = np.empty(3) - mujoco.mju_rotVecQuat(out, np.asarray(direction_xr, dtype=float), unyaw) - return out - - tool = in_operator_frame( - _xr_from_mj_pos(p_body + ghost_axis) - _xr_from_mj_pos(p_body) - ) - hand_axis = in_operator_frame( - pose_from_ghost_body(p_body, q_body)[:3, :3] @ _HAND_REPORT_AXIS - ) - - def ahead(direction): - return math.degrees( - math.acos(min(1.0, max(-1.0, float(direction @ _OPERATOR_FORWARD)))) - ) - - LOG.info( - "grip calib: at Q_HOME the tool points (%+.2f, %+.2f, %+.2f), %.0f deg off the " - "operator's forward, and the gate will demand a hand whose %s axis is " - "(%+.2f, %+.2f, %+.2f), %.0f deg off. XR axes, in the operator's frame. Only the " - "SECOND depends on _EULER_HAND_FROM_GHOST_DEG.", - *tool, - ahead(tool), - "pointing" if HAND_POSE is HandPose.AIM else "thumb", - *hand_axis, - ahead(hand_axis), - ) - # The hand axis only: the tool angle does not depend on the calibration at all, so a - # warning on it would report a Q_HOME or mesh change under a misleading name. - if ahead(hand_axis) > _POSTURE_LIMIT_DEG: - LOG.warning( - "grip calib: the gate will demand a hand held %.0f deg off neutral, past the " - "%.0f deg that reads as a comfortable hold. Check _EULER_HAND_FROM_GHOST_DEG " - "-- it is the only constant this angle depends on.", - ahead(hand_axis), - _POSTURE_LIMIT_DEG, - ) - return ahead(tool), ahead(hand_axis) - - -def _frame_clock(info) -> float | None: - """The app's clock, or None if this frame carries no time. - - viz zeroes predicted_display_time with should_render on every frame before - kRunning. The caller must skip the sample rather than record a zero, or the next - real frame computes dt from 0 and _clamp_dt reports a full MAX_DT_S stall. - """ - 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) - # Before the Renderer, which uploads geometry once from the model address: the - # follower repoints geom materials and poses its joints here. Not placed yet -- - # that waits for the first head pose, in _Preview.before_step. - arm = follower.Follower(model, data) - - # Order is load-bearing: VizSession calls xrCreateInstance, so an extension - # discovered after it cannot be added, and a controller tracker missing - # XR_NVX1_action_context is silently dead rather than an error. The clutch's home - # is pushed every non-ENGAGED frame, so this constructor value only has to be - # well-formed -- nothing can latch before the anchor exists. - pipeline, clutch = _build_pipeline(pose_from_ghost_body(*arm.gripper_pose_mj())) - required_extensions = get_required_oxr_extensions_from_pipeline(pipeline) - - config = viz.VizSessionConfig() - config.mode = _DISPLAY_MODE - config.app_name = "MuJoCoXR" - config.xr_near_z = NEAR_Z - config.xr_far_z = FAR_Z - config.required_extensions = required_extensions - # Alpha 0 = "show passthrough here", honoured at the runtime's discretion: - # viz sets the source-alpha blend bit only for a non-opaque environment, so - # a VR headset composites black instead, which is legible rather than broken. - config.clear_color = (0.0, 0.0, 0.0, 0.0) - - viz_session = viz.VizSession.create(config) - renderer = None - gl_context = 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) - - # After VizSession.create, which cudaSetDevice's the GPU behind its - # Vulkan device; the renderer checks this context landed on that one. - gl_context = mujoco.GLContext(resolution.width, resolution.height) - gl_context.make_current() - - # MuJoCo resolves multisample renderbuffers only inside mjr_readPixels, - # which this path never calls, and a multisample source cannot be - # blitted with a y flip in one step. - model.vis.quality.offsamples = 0 - - renderer = _mujoco_xr.Renderer( - 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, type(gl_context).__module__) - - # 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), - ) - - arm.log_placement() - # Before the anchor, and correct there: both angles are reported in the - # operator's own frame, which the anchor's yaw is exactly what defines. - _log_grip_posture(arm) - - monitor = InterventionMonitor(model) - LOG.info( - "harness: the ghost renders the EePoseRateLimiter output, clamped at " - "%.2f m/s / %.0f deg/s and rejecting above %.2f m/s / %.0f deg/s. Amber " - "while clamping, red while rejecting, authored blue passing through.", - _HARNESS.max_linear_velocity, - math.degrees(_HARNESS.max_angular_velocity), - _HARNESS.reject_linear_velocity, - math.degrees(_HARNESS.reject_angular_velocity), - ) - - 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 graph, - # and passing them again duplicates the set. - oxr_handles=OpenXRSessionHandles(*oxr), - ) - with TeleopSession(teleop_config) as teleop_session: - try: - _loop( - viz_session, - layer, - renderer, - model, - data, - teleop_session, - ghost, - monitor, - arm, - clutch, - ) - finally: - LOG.info(monitor.summary()) - finally: - # Innermost first: the renderer's GL objects need a current context. - if renderer is not None: - renderer.close() - if gl_context is not None: - gl_context.free() - viz_session.destroy() - return 0 - - -# ── The per-frame protocol ───────────────────────────────────────────────── -# It spans four files, so it is stated once, here. Each frame, in order: -# -# 1. before_step() -- anchor the arm to the head if it is not anchored yet, -# push the hand's position at the follower's rotation as -# the clutch home (every non-ENGAGED frame), and emit the -# permission leaf. -# 2. step() -- the retargeting graph runs: jaw, grip source, clutch, -# limiter. The clutch reads permission on THIS frame. -# 3. after_step() -- advance the phase, drive the arm, write the ghost's -# mocap rows, mj_forward, re-evaluate the gate. -# 4. render -- mjv_updateScene reads geom_xpos, then submit. -# -# Permission is one frame stale, deliberately: step N's leaf carries the verdict -# after_step produced on frame N-1, because the gate needs a limiter band step N has -# not computed yet. Squeezing inside that 14 ms costs nothing -- a denied latch stays -# OWED and fires on the first permitted frame. -class _Preview: - """The follower/leader handoff: one call before ``step()``, one after. - - Everything in ``_loop`` that is not rendering. Split out so the whole engage - sequence can be driven headlessly at frame rate, which is the only way to exercise - the gate's pass-through conjunct -- a quasi-static drive passes it by luck. - """ - - def __init__(self, model, data, ghost, monitor, arm, clutch) -> None: - """Bind to one scene, one pair of tools and one clutch. - - Measures the arm's yaw bias here, while the base still stands on the yaw - ``Follower.__init__`` left it on -- ``anchor`` has not run, so the reading is the - arm's own and not a head's. - """ - self._model = model - self._data = data - self._ghost = ghost - self._monitor = monitor - self._arm = arm - self._clutch = clutch - self._ghost_geoms = np.array( - [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, n) for n in GHOST_GEOMS] - ) - self.phases = follower.PhaseMachine() - self._gate = follower.EngageGate() - # Its own key, not the un-anchored one: they share a frame in practice, and - # merging them would suppress the first verdict of the session. - self.verdict = follower.GateResult((("startup", "starting up"),)) - # Neither tool is drawn until the arm is anchored, so start both hidden rather - # than relying on the first after_step to arrive. - follower.set_geoms_visible(model, self._ghost_geoms, False) - # SO101GripperRetargeter's own released end, held until the first frame with a - # usable hand pose refreshes it. - self._closedness = 0.0 - # B is edge-triggered, so a held button resets the offset once. - self._reset_held = False - # The ghost body of the last usable hand pose, latched by after_step because - # before_step runs a frame ahead of it. None until one arrives, and no latch can - # be permitted before then: the gate reports `controller not tracked`. - self._hand_body_mj: np.ndarray | None = None - self._yaw_bias = base_yaw_bias(arm) - self._yaw_trim_deg = _YAW_TRIM_DEG - self._trimming = False - # One reading is all it takes, and it needs a tracked controller, so it cannot - # happen at construction. - self._frames_logged = False - LOG.info( - "follower: the base leads the hand by %+.2f deg of yaw, measured off " - "Q_HOME so the JAW faces where the controller does.", - math.degrees(2.0 * math.atan2(self._yaw_bias[2], self._yaw_bias[0])), - ) - - def before_step(self, head: np.ndarray | None) -> tuple[dict, ExecutionEvents]: - """Anchor the arm if it is not anchored, then build this frame's step() kwargs. - - Two rules on the home push. Key it off the app's phase, never - ``clutch.is_engaged``, which drops on four paths of which one is a real disengage; - and take the position from the hand and the rotation from the gripper, so the - leader appears in the operator's hand at the rotation they aimed. The gripper's - own position would carry the preview's offset into the clutch's delta. - """ - if head is not None and not self._arm.anchored: - self._arm.anchor(head) - if ( - self.phases.phase is not follower.ClutchPhase.ENGAGED - and self._hand_body_mj is not None - ): - self._clutch.set_home_base_T_ee( - pose_from_ghost_body(self._hand_body_mj, self._arm.gripper_pose_mj()[1]) - ) - # The reset pulse re-seeds the limiter's baseline onto the first frame after a - # disengage, where the commanded pose jumps from the leader back to the - # follower. Without it the limiter rejects for ~30 frames (0.92 s). - # execution_state is spelled out because ExecutionEvents defaults to UNKNOWN, - # which would make the clutch silently never engage. - reset = self.phases.reset_requested - self.phases.reset_requested = False - permitted = self.phases.permits_engagement or self.verdict.ok - return ( - {ENGAGE_PERMISSION_LEAF: {ValueInput.VALUE: _permission(permitted)}}, - ExecutionEvents(reset=reset, execution_state=ExecutionState.RUNNING), - ) - - def after_step(self, result, dt: float) -> follower.ClutchPhase: - """Advance the phase, drive both tools, and re-evaluate the gate.""" - if not self._arm.anchored: - # Nowhere to put the arm yet. Draw neither tool and hold the gate shut: - # with `permits_engagement` false too, `before_step` cannot permit a latch. - # Returns before the phase machine, so a frame with no placement is a frame - # that did not happen. - self._show(follower_visible=False, ghost_visible=False) - self._blocked(follower.GateResult((("unanchored", "no head pose yet"),))) - return self.phases.phase - - # HAND_POSE_KEY is the only channel that can report tracking loss. - hand = _pose(result, HAND_POSE_KEY) - commanded = _pose(result, COMMANDED_POSE_KEY) - governed = _pose(result, EE_POSE_KEY) - - # ControllerPoseSource drops on the pose's IS_VALID; the clutch ALSO disarms on a - # non-finite pose and on a finite but degenerate quaternion. Both folded in, so - # `hand` covers the clutch's whole disarm set -- otherwise a bad frame leaves - # hand_present True while is_engaged has just gone False, and the phase machine - # reads that as a real disengage. - if hand is not None and ( - not np.all(np.isfinite(hand)) - or float(np.linalg.norm(hand[3:7])) < _MIN_QUAT_NORM - ): - hand = None - - # Latched, and refreshed only on a usable frame. SO101GripperRetargeter tests - # `inp.is_none` alone, so it keeps articulating the trigger while GRIP_IS_VALID - # is false, and a jaw swinging on a frozen body reads as "the gripper actuated". - if hand is not None: - self._closedness = float(result[GRIPPER_COMMAND_KEY][0]) - # Latched for the same reason and one of its own: `before_step` pushes the - # clutch's home a frame before this one exists. Taken from the hand rather - # than read back off the arm, so the grip offset -- tuned or not -- cannot - # leak into an engagement the clutch composes as a delta. - self._hand_body_mj = ghost_body_from_pose(hand)[0] - - if not self._frames_logged: - self._frames_logged = _log_hand_frames(result) - - # Before the phase advance, so a press takes effect on this frame rather - # than the next. - self._reset_offset(result) - - phase = self.phases.advance( - is_engaged=self._clutch.is_engaged, hand_present=hand is not None, dt=dt - ) - - # Nothing moves the arm while ENGAGED -- it is hidden and frozen where it stood - # on the engage frame. The disengage edge lands DISENGAGED, so the drag resumes - # on that very frame with no ramp in between. - if phase is follower.ClutchPhase.DISENGAGED and hand is not None: - # The hand, not the governed pose: the limiter governs only what the leader - # renders. Raw XR, not the ghost body -- follower.py is free of the grip - # calibration and must stay that way. - stick_x, stick_y = self._stick(result) - if self._trim_yaw(result, stick_x, dt): - # A owns the stick while held, so a trim cannot also walk the offset. - stick_x = stick_y = 0.0 - facing, base_yaw = self._yaws(hand[3:7]) - self._arm.drive(hand[:3], facing, base_yaw, stick_x, stick_y, dt) - - engaged = phase is follower.ClutchPhase.ENGAGED - self._show(follower_visible=not engaged, ghost_visible=engaged) - - limiter_passing = False - if commanded is not None and governed is not None: - # The body needs no tracking-loss gate: the clutch emits its held pose on - # every disarm path, so the pipeline freezes it. The JAW does, hence the - # latch above. - _update_ghost(self._data, self._ghost, governed, self._closedness) - # Classified on every governed frame, painted only while the ghost is the - # tool on show: the band needs an unbroken baseline to tell a refused frame - # from a clamped one. - band = self._monitor.update(self._model, commanded, governed, paint=engaged) - limiter_passing = band is HarnessBand.PASS_THROUGH - - # The module docstring's invariant, at the one place that writes mocap_*. - # Unconditional, and here rather than in _loop so the tests drive it: without - # it the gate's xpos read below and mjv_updateScene both see the ghost's XML - # rest pose, and the leader appears in the right place and never moves again. - mujoco.mj_forward(self._model, self._data) - - self._blocked( - self._gate.evaluate( - phase=phase, - hand_quat_xyzw=None if hand is None else hand[3:7], - home_quat_xyzw=grip_quat_from_ghost_body( - self._arm.gripper_pose_mj()[1] - ), - limiter_passing=limiter_passing, - dt=dt, - ) - ) - return phase - - def _yaws(self, q_hand_xyzw: np.ndarray) -> tuple[np.ndarray, np.ndarray]: - """``(where the controller points, what to turn the base onto)``, both wxyz. - - The app's half of the yaw drive, and the whole of it: which axis of a pose is its - facing is a fact about the calibration, so follower.py is handed the answer. The - second leads the first by the measured bias and the operator's trim, all yaws - about +Y, so their order is free. The follower needs both -- the base takes the - second, the grip offset is carried on the first. - """ - facing = hand_facing_xr(q_hand_xyzw) - trim = np.empty(4) - mujoco.mju_axisAngle2Quat( - trim, np.array([0.0, 1.0, 0.0]), math.radians(self._yaw_trim_deg) - ) - biased = np.empty(4) - mujoco.mju_mulQuat(biased, facing, self._yaw_bias) - base_yaw = np.empty(4) - mujoco.mju_mulQuat(base_yaw, biased, trim) - return facing, base_yaw - - def _trim_yaw(self, result, stick_x: float, dt: float) -> bool: - """A + the right thumbstick: walk the yaw trim. True while it owns the stick. - - A rate, like the grip offset, so the trim holds where the stick left it. Logs - once on the release in the constant's own form, with what the AIM pose would - have said beside it. - """ - controller = result[GHOST_HAND] - held = not controller.is_none and bool(controller[_YAW_TRIM_BUTTON]) - if not held: - if self._trimming: - self._trimming = False - LOG.info( - "follower: yaw trim -> _YAW_TRIM_DEG = %.1f", self._yaw_trim_deg - ) - return False - step = follower.deflection(stick_x) * _YAW_TRIM_RATE_DEG_S * float(dt) - self._yaw_trim_deg += step - # Only a stick that actually moved arms the log, so holding A to keep the trim - # off the grip offset does not print a line every time it is released. - self._trimming = self._trimming or step != 0.0 - return True - - def _stick(self, result) -> tuple[float, float]: - """The right thumbstick's two raw axes, or a stick at rest. - - ``Follower.drive`` owns which way each one points: the horizontal drive has one - definition and it is not here. - """ - controller = result[GHOST_HAND] - if controller.is_none: - # Absent before the tracker has a controller, and reading the group there - # raises rather than reporting a stick at rest -- see `_reset_offset`. - return 0.0, 0.0 - return ( - float(controller[ControllerInputIndex.THUMBSTICK_X]), - float(controller[ControllerInputIndex.THUMBSTICK_Y]), - ) - - def _reset_offset(self, result) -> None: - """B, on its rising edge: put the grip offset back to its authored value. - - The operator's escape hatch, so deliberately phase-free. Needs no pose: the - offset is a constant in the anchor's frame, not a point in the world. - """ - # The controller group is Optional and absent before the tracker has one; - # reading it there raises rather than returning a falsy button, which took the - # whole session down at startup. Absent is "not pressed", so a press spanning a - # dropout re-arms and the first tracked frame is a fresh rising edge. - controller = result[GHOST_HAND] - pressed = not controller.is_none and bool(controller[_RESET_OFFSET_BUTTON]) - rising = pressed and not self._reset_held - self._reset_held = pressed - if not rising: - return - self._arm.reset_offset() - LOG.info( - "follower: grip offset reset to XR (%.2f, %.2f, %.2f).", - *self._arm.grip_from_controller_xr, - ) - - def _show(self, *, follower_visible: bool, ghost_visible: bool) -> None: - """The only place either tool's visibility is set. At most one is drawn.""" - self._arm.set_visible(follower_visible) - follower.set_geoms_visible(self._model, self._ghost_geoms, ghost_visible) - - def _blocked(self, verdict: follower.GateResult) -> None: - """Publish the gate's verdict: the arm's colour, and a log on transitions. - - Keyed on `verdict.keys`, never on the text it renders. The verdict is stored - even on frames nothing is logged, so the first frame after a release is - compared against the last engaged one and the release is always heard. - """ - previous_keys = self.verdict.keys - self.verdict = verdict - self._arm.set_engageable(verdict.ok) - if verdict.keys == previous_keys or follower.GATE_KEY_ENGAGED in verdict.keys: - return - LOG.info( - "clutch: %s", - "engageable" - if verdict.ok - else "blocked (" + "; ".join(verdict.blocked) + ")", - ) - - -def _loop( - viz_session, - layer, - renderer, - model, - data, - teleop_session, - ghost, - monitor, - arm, - clutch, -) -> None: - view_count = renderer.view_count - checked_frustum = False - preview = _Preview(model, data, ghost, monitor, arm, clutch) - previous_clock: float | None = None - - while not viz_session.should_close(): - info = viz_session.begin_frame() - try: - # Nothing integrates, so there is no fixed-step accumulator and the whole - # frame hangs off should_render. That is also what keeps - # teleop_session.step() from calling xrSyncActions on the unthrottled - # pre-kRunning burst. The cost is that retargeter compute follows render - # cadence; a resumed session sees one large dt, bounded by max_dt. - if not info.should_render: - continue - - now = _frame_clock(info) - dt = ( - 0.0 - if now is None or previous_clock is None - else _clamp_dt(now - previous_clock) - ) - previous_clock = previous_clock if now is None else now - - # Input first, so it precedes everything it feeds. The head pose comes - # from this frame's views, which viz only fills past should_render. - external_inputs, events = preview.before_step(_head_pose(info)) - result = teleop_session.step( - external_inputs=external_inputs, execution_events=events - ) - preview.after_step(result, dt) - - renderer.update_scene(model._address, data._address) - # mjv_updateScene truncates on overflow and returns normally, with - # only a stderr warning nobody reads in a frame loop. - if renderer.ngeom >= renderer.maxgeom: - raise RuntimeError( - f"mjvScene is full: ngeom={renderer.ngeom} maxgeom={renderer.maxgeom}. " - "Geometry is being dropped -- raise kMaxGeom in " - "cpp/scene_renderer.cpp." - ) - - # No view-count check here: render() sees the flattened lengths and - # rejects a mismatch in those terms. - poses, fovs = _flatten_xr_views(info) - renderer.render(poses, fovs) - - # First rendered frame only: the fov changes per frame, the - # convention does not. - if not checked_frustum: - for view in range(view_count): - _assert_frustum( - renderer.frustum(view), info.views[view].fov, NEAR_Z, FAR_Z - ) - LOG.info( - "frustum verified on the first rendered frame (matches FrameInfo fov, clip planes agree " - "with VizSessionConfig)" - ) - checked_frustum = True - - layer.submit( - renderer.color(0), - renderer.depth(0), - renderer.color(1), - renderer.depth(1), - ) - finally: - # Follows EVERY begin_frame(), including the should_render == False - # path and any exception above. Skipping it wedges the frame loop. - viz_session.end_frame() - - -def main(argv: list[str]) -> int: - parser = argparse.ArgumentParser( - description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter - ) - parser.add_argument("--verbose", action="store_true", help="Debug-level logging.") - CloudXRLauncher.add_launcher_arguments(parser) - args = parser.parse_args(argv[1:]) - - logging.basicConfig( - level=logging.DEBUG if args.verbose else logging.INFO, - format="[mujoco_xr] %(message)s", - ) - - # Before launch_context starts the runtime, so an unfetched checkout says so - # plainly rather than buried in the runtime's startup logging. - missing = _missing_assets() - if missing: - raise SystemExit( - f"mujoco_xr: the SO-101 assets 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.owns_runtime: - 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/follower.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/follower.py deleted file mode 100644 index 4508ae1fd3..0000000000 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/follower.py +++ /dev/null @@ -1,730 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""The SO-101 follower preview: the arm, the phase it is in, and whether the clutch may latch. - -The joints are locked: ``qpos`` is written once, to :data:`Q_HOME`, and the arm is moved -as a rigid body. ``Follower._place`` is the one writer of ``body_quat`` and -``Follower._move_base`` the one writer of ``body_pos``. This module must not learn the -leader ghost's grip calibration, which is a claim about a hand holding a CONTROLLER; -app.py converts between the two. -""" - -from __future__ import annotations - -import dataclasses -import enum -import logging -import math - -import mujoco -import numpy as np -from isaacteleop.retargeters.rate_limiter import _quat_geodesic_angle - -from . import _mujoco_xr - -LOG = logging.getLogger("mujoco_xr") - -# Declared by assets/follower/follower_arm.xml and repointed onto every follower geom -# at startup, so the arm recolours in one write. -FOLLOWER_MATERIAL = "follower_arm" - -BASE_BODY = "base" -GRIPPER_BODY = "gripper" - -# Upstream's own tool frame, declared on the `gripper` body 98.4 mm out from its origin. -# The arm is placed BY this point, so it is also the axis the yaw turns about: it sits -# 3.8 mm off the closed jaw surface, where a grasped object would be. Placing by the -# gripper body instead pins a point 98.4 mm short of the jaw, which then swings on a -# 15.8 mm arc across +-90 degrees of yaw. -GRIPPER_SITE = "gripperframe" - -# Upstream's joint order, which is also the qpos order Q_HOME is written in. Asserted -# by name at startup: a reordered upstream file would put each of Q_HOME's angles on a -# different joint and still look like an arm. -ARM_JOINTS = ( - "shoulder_pan", - "shoulder_lift", - "elbow_flex", - "wrist_flex", - "wrist_roll", -) -GRIPPER_JOINT = "gripper" - -# The configuration the arm holds for the whole session, written to qpos once at -# construction. This pose IS the wrist posture the engage gate demands, so turning -# these re-aims the operator's hand, so re-solve _EULER_HAND_FROM_GHOST_DEG with them. -# J2+J3+J4 set the gripper's elevation and J4 stops at +-95 degrees; J5 rolls the jaw -# about the tool axis, which is a bearing shift in the posture the gate demands. -Q_HOME_DEG = ( - 0.00, # J1 shoulder_pan -- base yaw - -45.00, # J2 shoulder_lift -- first segment elevation - 45.00, # J3 elbow_flex -- second segment elevation - 90.00, # J4 wrist_flex -- wrist up/down - -90.00, # J5 wrist_roll -- spin about the tool axis - 00.00, # J6 gripper -- jaw opening, 0 is the authored pose -) -Q_HOME = np.radians(Q_HOME_DEG) - -# Where the home gripper sits relative to the OPERATOR'S HEAD, in XR axes: 0.30 m -# below eye level and 0.60 m ahead on the head's yaw-projected facing (anchor_from_head). -# Measured from the head, not the reference-space origin: the app does not get to choose -# that origin, and a stage-origin space puts anything authored against it a standing -# height out. Whether it lands inside the gaze cone is a headset judgement. A starting -# pose only: the position holds until the first frame carrying a controller, and the yaw -# only turns the arm to face the operator meanwhile. The controller owns both after it. -HOME_GRIP_FROM_HEAD_XR = np.array([0.0, -0.30, -0.60]) - -# Where the gripper's JAW sits relative to the CONTROLLER, in metres, XR axes: level -# with the hand laterally, 0.25 m ahead and 0.10 m below it (XR is y-up and -z-forward, -# cpp/frames.hpp). Only the starting value for the horizontal pair; the live one is -# Follower.grip_from_controller_xr, which the thumbstick walks. The vertical term is -# fixed. Carried on the controller's own facing, so stick forward sends the arm along -# the pointing ray and yawing the controller carries it around at a fixed offset. -GRIP_FROM_CONTROLLER_XR = np.array([0.0, -0.10, -0.25]) - -# What the thumbstick does to the two horizontal terms above. Deflection is a RATE, so -# the offset holds where the stick left it. Metres per second at full deflection, -# scaled by the frame dt -- not per frame, or its feel would track the frame rate. -_TUNE_RATE_M_S = 0.20 -# Sticks drift and the offset is latched, so a resting controller would walk the arm -# away over a session. -_STICK_DEADZONE = 0.15 -# Each tuned term, absolutely: a stuck stick must not push the arm out of sight. The -# vertical term is not tuned and so not bounded here. -_TUNE_LIMIT_M = 0.60 - -# mjv_defaultOption enables geom groups 0-2 and disables 3-5, so this pair is "drawn" -# and "not drawn". A hidden geom never becomes an mjvGeom, so it never writes depth. -DRAWN_GROUP = 2 -HIDDEN_GROUP = 3 - -# Engageable. The blocked colour is authored in follower_arm.xml: neutral grey, and -# darker at 0.45 against this one's 0.68 luminance. There is no HUD to fall back on, so -# brightness carries the signal as well as hue -- and a translucent arm dilutes both -# against whatever is behind it, so check the pair on a headset before trusting either. -_ENGAGEABLE_RGB = (0.20, 0.85, 0.35) - -# ENGAGED is held while the hand channel is absent, so a one-frame tracking blip does -# not cost a teleport back onto the hand. This bounds the hold, so a genuinely lost -# controller cannot strand the app engaged. -_DROPOUT_TIMEOUT_S = 0.5 - -# The engage gate's one metric conjunct, with hysteresis. Only the RELATION is pinned -# -- enter tighter than exit -- because no value here is defensible without a headset. -_ROTATION_ENTER_RAD = math.radians(20.0) -_ROTATION_EXIT_RAD = math.radians(30.0) -# Time the gate must stay inside the enter band before it goes green. -_DWELL_S = 0.1 - - -def yaw_of_direction(forward_xr: np.ndarray, fallback_xr: np.ndarray) -> np.ndarray: - """The horizontal bearing of an XR direction, as a wxyz quaternion about +Y. - - ``fallback_xr`` covers a direction within a hair of vertical, which has no bearing to - report. Every yaw reading goes through here, so all of them track a world-vertical - turn 1:1; what differs between callers is only what leaks in from the other two - degrees of freedom, which is a property of the axis they pick. - """ - forward = np.asarray(forward_xr, dtype=float) - if abs(forward[0]) < 1e-6 and abs(forward[2]) < 1e-6: - # Straight up or down -- a headset face-down on a desk, a controller held - # muzzle-up. The fallback then points along the horizon: forwards when the - # direction points down, backwards when up. - forward = -math.copysign(1.0, forward[1]) * np.asarray(fallback_xr, dtype=float) - - q_yaw = np.empty(4) - mujoco.mju_axisAngle2Quat( - q_yaw, np.array([0.0, 1.0, 0.0]), math.atan2(-forward[0], -forward[2]) - ) - return q_yaw - - -def yaw_of_axis(q_xyzw: np.ndarray, forward_local: np.ndarray) -> np.ndarray: - """The horizontal facing of an XR orientation, as a wxyz quaternion about +Y. - - ``forward_local`` names which axis of the pose is its facing, in the pose's own - frame. No default: each axis is blind to rotation about itself and sensitive to the - rest, so it must be chosen against the motions the reading has to ignore. See - app.py's _HAND_FORWARD_AXIS. - """ - q_wxyz = np.asarray(q_xyzw, dtype=float)[[3, 0, 1, 2]] - forward = np.empty(3) - mujoco.mju_rotVecQuat(forward, np.asarray(forward_local, dtype=float), q_wxyz) - up = np.empty(3) - mujoco.mju_rotVecQuat(up, np.array([0.0, 1.0, 0.0]), q_wxyz) - return yaw_of_direction(forward, up) - - -def yaw_of(q_xyzw: np.ndarray) -> np.ndarray: - """The horizontal facing of a HEAD pose, reading its -Z as the view direction.""" - return yaw_of_axis(q_xyzw, np.array([0.0, 0.0, -1.0])) - - -def anchor_from_head(head_pose_xr: np.ndarray) -> tuple[np.ndarray, np.ndarray]: - """Where the home grip goes and which way the arm faces, from a 7-D head pose. - - Takes ``(position, xyzw)`` in XR; returns the XR home-grip position and the head's - YAW as a wxyz quaternion. The same yaw does both jobs: it carries - :data:`HOME_GRIP_FROM_HEAD_XR` onto the head's facing, and :meth:`Follower.anchor` - turns the arm by it. - """ - pose = np.asarray(head_pose_xr, dtype=float) - q_yaw = yaw_of(pose[3:7]) - offset = np.empty(3) - mujoco.mju_rotVecQuat(offset, HOME_GRIP_FROM_HEAD_XR, q_yaw) - return pose[:3] + offset, q_yaw - - -def mj_from_xr_rotation(q_xr_wxyz: np.ndarray) -> np.ndarray: - """An XR-frame ROTATION expressed in MuJoCo: ``Q q Q^-1``, wxyz throughout. - - Not ``_mujoco_xr.mj_from_xr_quat``, which maps a body's ORIENTATION across the - frames and is a single left-multiply. Conjugating keeps the axis map with its one - definition in cpp/frames.hpp: XR +Y is MuJoCo +z, so an XR yaw of theta comes out - as a MuJoCo rotation of theta about +z. - """ - q_frame = np.array(_mujoco_xr.QUAT_MJ_FROM_XR, dtype=float) - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, q_frame) - rotated = np.empty(4) - mujoco.mju_mulQuat(rotated, q_frame, np.asarray(q_xr_wxyz, dtype=float)) - out = np.empty(4) - mujoco.mju_mulQuat(out, rotated, inverse) - return out - - -def set_geoms_visible(model, geoms: np.ndarray, visible: bool) -> None: - """Add or remove ``geoms`` from what ``mjv_updateScene`` emits.""" - model.geom_group[geoms] = DRAWN_GROUP if visible else HIDDEN_GROUP - - -class ClutchPhase(enum.Enum): - """Where the app is in the engage cycle, and so which tool it draws. - - Never the authority on "is the clutch latched?" -- that is - ``SO101ClutchRetargeter.is_engaged``, which this is derived from. - """ - - #: The follower is drawn and dragged by the hand; the leader is hidden. - DISENGAGED = "disengaged" - #: The leader is drawn and follows the hand; the follower is hidden and frozen. - ENGAGED = "engaged" - - -class PhaseMachine: - """``DISENGAGED <-> ENGAGED``, one call per frame. - - Takes ``is_engaged`` as an input on every call and never copies it into a field, - so the two cannot drift. - """ - - def __init__(self) -> None: - """Start disengaged, with the arm already at Q_HOME.""" - self.phase = ClutchPhase.DISENGAGED - #: Set on the disengage edge; the app clears it once it has pulsed the limiter. - #: Without that pulse the limiter rejects the next ~30 frames -- its per-frame - #: reject threshold at 72 Hz is only 27.8 mm. - self.reset_requested = False - self._dropout_s = 0.0 - - def advance( - self, *, is_engaged: bool, hand_present: bool, dt: float - ) -> ClutchPhase: - """Fold one frame in and return the new phase. - - ``is_engaged`` is read, never re-derived from the squeeze: the latch can be - deferred by frames the app cannot observe. ``hand_present`` is what makes the - disengage edge trustworthy -- ``is_engaged`` drops on four paths and only one - of them is a real disengage. - """ - if self.phase is ClutchPhase.DISENGAGED: - if is_engaged: - self.phase = ClutchPhase.ENGAGED - self._dropout_s = 0.0 - elif not hand_present: - # Hold ENGAGED through the gap. The clutch re-arms itself and re-latches at - # _last_commanded_*, where the leader already is, so the resumed frame is - # jump-free. Past the timeout the arm simply stays where it froze. - self._dropout_s += dt - if self._dropout_s > _DROPOUT_TIMEOUT_S: - self._disengage() - else: - self._dropout_s = 0.0 - if not is_engaged: - self._disengage() - return self.phase - - def _disengage(self) -> None: - self.phase = ClutchPhase.DISENGAGED - self.reset_requested = True - self._dropout_s = 0.0 - - @property - def permits_engagement(self) -> bool: - """One disjunct of what the app feeds the clutch's latch gate. - - The other is the engage gate's verdict; the app sends ``permits_engagement or - verdict.ok``. Reads the phase rather than ``is_engaged``: during a tracking - dropout ``is_engaged`` is False on exactly the frames this exists to cover. - """ - return self.phase is ClutchPhase.ENGAGED - - -#: The ``keys`` entry for the latched-clutch conjunct. While the clutch is latched -#: there is nothing to engage, so app.py logs no verdict carrying it. -GATE_KEY_ENGAGED = ClutchPhase.ENGAGED.value - - -@dataclasses.dataclass(frozen=True) -class GateResult: - """Whether the clutch may latch, and -- when it may not -- why not. - - ``failed`` names **every** failing conjunct, not the first: an operator who fixes - their wrist angle and immediately hits an unreported second failure has been told - half the truth twice. Each entry is ``(key, phrase)``, kept as a pair so the - value-free identity and the text it identifies cannot fall out of step. - """ - - #: ``key`` is app.py's transition key -- value-free, or a rounded angle in it - #: would move the key every frame. ``phrase`` carries the measurement, for display. - failed: tuple[tuple[str, str], ...] = () - - @property - def ok(self) -> bool: - return not self.failed - - @property - def keys(self) -> tuple[str, ...]: - return tuple(key for key, _ in self.failed) - - @property - def blocked(self) -> tuple[str, ...]: - return tuple(phrase for _, phrase in self.failed) - - -class EngageGate: - """The three conjuncts of requirement 3, plus hysteresis and a dwell. - - Pure with respect to the app: it returns the failing conjuncts and app.py decides - what to log. - """ - - def __init__(self) -> None: - """Start closed, with no dwell credit.""" - self._ok = False - self._dwell_s = 0.0 - - def evaluate( - self, - *, - phase: ClutchPhase, - hand_quat_xyzw: np.ndarray | None, - home_quat_xyzw: np.ndarray, - limiter_passing: bool, - dt: float, - ) -> GateResult: - """Fold one frame in. - - ``hand_quat_xyzw`` and ``home_quat_xyzw`` must share a layout -- the geodesic - angle is layout-agnostic only under that condition, and both are xyzw in XR here. - ``home_quat_xyzw`` carries the hand's own yaw, put there by :meth:`Follower.drive`, - so the two cancel and what is left is the wrist's pitch and roll against a session - constant. - """ - failed: list[tuple[str, str]] = [] - # The whole post-release debounce: `ok` is held False for the entire - # engagement, so the dwell below is zeroed on every engaged frame and a release - # cannot re-latch for at least _DWELL_S. It is also why the app must feed the - # clutch `permits_engagement or verdict.ok`. - if phase is ClutchPhase.ENGAGED: - failed.append((GATE_KEY_ENGAGED, phase.value)) - # There is no reach conjunct and no reach envelope. The gripper is placed - # exactly at its offset from the hand every frame, so a position residual is - # zero by construction. Do not add one back believing it bounds a workspace: - # this preview has none. - # - # The rotation conjunct is judged inside the tracked branch: with no hand there - # is no angle, and reporting one would be a second failure derived from the first. - if hand_quat_xyzw is None: - failed.append(("untracked", "controller not tracked")) - else: - theta = _quat_geodesic_angle( - np.asarray(hand_quat_xyzw, dtype=float), - np.asarray(home_quat_xyzw, dtype=float), - ) - rotation_tol = _ROTATION_EXIT_RAD if self._ok else _ROTATION_ENTER_RAD - if not theta < rotation_tol: - failed.append( - ( - "rotation", - f"rotation {math.degrees(theta):.0f} deg " - f"> {math.degrees(rotation_tol):.0f}", - ) - ) - # The leader renders the LIMITER's output, so a gate that opens while it is - # still clamping reveals a tool tens of degrees and hundreds of milliseconds - # behind the hand. 4 deg/cm x 0.5 m/s is 3.49 rad/s against a 2.5 rad/s clamp, - # so this trips at about 0.36 m/s of hand speed -- ordinary dragging. - if not limiter_passing: - failed.append(("limiter", "still catching up")) - - if failed: - self._dwell_s = 0.0 - self._ok = False - return GateResult(tuple(failed)) - - self._dwell_s += dt - if self._dwell_s < _DWELL_S: - return GateResult((("settling", "settling"),)) - self._ok = True - return GateResult() - - -class Follower: - """The follower arm in one scene: posed once, drawn, and driven rigidly by the hand. - - :meth:`drive` moves it two independent ways: position from the controller plus a - thumbstick-trimmed offset, yaw from the wrist. Placed by :data:`GRIPPER_SITE`, - upstream's tool frame at the jaw, which is therefore also the axis the yaw turns - about; the gripper body carries the orientation and sits 98.4 mm short of it. - """ - - def __init__(self, model, data) -> None: - """Resolve the arm, check its qpos layout and pose it. NOT yet placed.""" - self._model = model - self._data = data - - self._base = _body(model, BASE_BODY) - self._gripper = _body(model, GRIPPER_BODY) - self._jaw = _site(model, GRIPPER_SITE) - # Kept because the anchor composes its yaw onto it rather than replacing it, - # so a scene that authors a base tilt keeps it. - self._authored_base_quat = np.array(model.body_quat[self._base], dtype=float) - _check_qpos_layout(model) - - self._material = mujoco.mj_name2id( - model, mujoco.mjtObj.mjOBJ_MATERIAL, FOLLOWER_MATERIAL - ) - if self._material < 0: - raise RuntimeError( - f"mujoco_xr: the scene declares no `{FOLLOWER_MATERIAL}` material; it must " - " assets/follower/follower_arm.xml rather than upstream's MJCF directly." - ) - self._geoms = _subtree_geoms(model, self._base) - self._visual_geoms = self._geoms[model.geom_group[self._geoms] == DRAWN_GROUP] - if self._visual_geoms.size == 0: - # Upstream numbers its visual geoms 2 and its collision geoms 3. A - # renumbering has to be an error, not a silently invisible arm. - raise RuntimeError( - f"mujoco_xr: no follower geom is in group {DRAWN_GROUP}, so the arm would " - "never be drawn; upstream's geom groups changed." - ) - # One material for thirteen upstream ones, on every follower geom rather than - # just the drawn ones, so the rule has no exception to remember. - model.geom_matid[self._geoms] = self._material - self._blocked_rgba = np.array(model.mat_rgba[self._material], dtype=np.float64) - - # The one and only qpos write. Everything after this moves the base. - self._data.qpos[: len(Q_HOME)] = Q_HOME - self._jaw_from_base = self._measure_jaw_from_base() - - # The live grip offset. This class is its only definition; app.py passes two raw - # stick axes and never learns which way either points. - self._grip_from_controller_xr = GRIP_FROM_CONTROLLER_XR.copy() - # Whether the stick has moved the offset since it was last at rest, so the tuned - # value is logged once on the release rather than at 72 Hz. - self._tuning = False - - # The frame the offset is carried on. None until anchor() takes it off the head, - # which is also what `anchored` reports. - self._anchored = False - # The yaw the base is currently turned by -- the wrist's, past the first driven - # frame, and so not the operator's above. - self._base_yaw_xr = np.array([1.0, 0.0, 0.0, 0.0]) - self.set_visible(False) - - # ---------------------------------------------------------------- geometry - - @property - def anchored(self) -> bool: - """Whether a head pose has placed the arm. False until then; never back.""" - return self._anchored - - @property - def base_yaw_xr(self) -> np.ndarray: - """The XR yaw (wxyz) the base is currently turned by; identity before any. - - Past the first driven frame this is the wrist's yaw, not the operator's. Kept as - the value that was used rather than read back off ``body_quat``. - """ - return self._base_yaw_xr.copy() - - def anchor(self, head_pose_xr: np.ndarray) -> np.ndarray: - """Take the offset's frame off the first head pose, and park the arm. - - Returns the XR home grip. Where the arm waits until a controller arrives, and the - head's yaw is only what turns it to face the operator meanwhile -- from the first - driven frame the controller owns both position and yaw. - """ - home_xr, q_yaw_xr = anchor_from_head(head_pose_xr) - self._anchored = True - self._place(home_xr, q_yaw_xr) - LOG.info( - "follower: anchored to a head at XR (%.2f, %.2f, %.2f) facing %.0f deg; " - "home grip at XR (%.2f, %.2f, %.2f), base at MuJoCo (%.3f, %.3f, %.3f). " - "The controller owns both from the first driven frame.", - *np.asarray(head_pose_xr, dtype=float)[:3], - math.degrees(2.0 * math.atan2(q_yaw_xr[2], q_yaw_xr[0])), - *home_xr, - *self._model.body_pos[self._base], - ) - return home_xr - - def reset_offset(self) -> None: - """Put the grip offset back to :data:`GRIP_FROM_CONTROLLER_XR`. Any phase. - - The operator's escape hatch for an offset walked out to its clamp, or a drifting - stick that got there on its own. Nothing else to reset: the arm is already on - the hand and already on its yaw. - """ - self._grip_from_controller_xr = GRIP_FROM_CONTROLLER_XR.copy() - self._tuning = False - - def _place(self, grip_xr: np.ndarray, q_yaw_xr: np.ndarray) -> None: - """Turn the base onto a yaw and put the gripper on an XR point. Does both, always. - - The order is load-bearing: turning the base swings the gripper around it, so - ``_jaw_from_base`` is re-measured between the ``body_quat`` and ``body_pos`` - writes. That second ``mj_forward`` costs 202 us a frame on a Jetson AGX Orin, - 1.5% of a 72 Hz frame -- cheap enough to keep the offset measured, not derived. - """ - # Yaw on the LEFT: it turns the arm in the WORLD, where upstream's quat orients - # it in its own frame. Upstream authors identity, so no shipped scene can tell - # the two orders apart -- this comment is the only guard. - turned = np.empty(4) - mujoco.mju_mulQuat( - turned, mj_from_xr_rotation(q_yaw_xr), self._authored_base_quat - ) - self._model.body_quat[self._base] = turned - self._base_yaw_xr = np.asarray(q_yaw_xr, dtype=float).copy() - self._jaw_from_base = self._measure_jaw_from_base() - self._move_base( - np.array(_mujoco_xr.mj_from_xr_pos(list(grip_xr)), dtype=float) - - self._jaw_from_base - ) - - def _measure_jaw_from_base(self) -> np.ndarray: - """Base origin -> the jaw tool frame in MuJoCo world. Measured, never derived. - - With the base at the MuJoCo origin the site's world position is the offset. Only - the base's yaw can change it, so only :meth:`_place` calls this -- and it must - write ``body_pos`` afterwards, because this leaves the base at the origin. Goes - through ``_move_base``, the one writer of ``body_pos``. - """ - self._move_base(np.zeros(3)) - return np.array(self._data.site_xpos[self._jaw], dtype=float) - - def _move_base(self, base_pos_mj: np.ndarray) -> None: - """Slide the whole arm. The one place ``body_pos`` is written. - - ``base`` is a fixed child of world, so every link translates with it and no - joint moves. Never touches ``body_quat``: :meth:`_place` is that one writer. - """ - self._model.body_pos[self._base] = base_pos_mj - mujoco.mj_forward(self._model, self._data) - - @property - def jaw_yaw_xr(self) -> np.ndarray: - """The XR yaw (wxyz) the jaw faces along: :data:`GRIPPER_SITE`'s +Z. - - Which way the gripper is turned, and what app.py aims at the controller. Not the - links' reach: J5 rolls the jaw about the tool axis without moving them, so the - two part company by exactly that roll. The site's +X is the tool axis and points - down at Q_HOME, which is why a roll there reads as a bearing change here. - """ - facing = np.array(self._data.site_xmat[self._jaw], dtype=float).reshape(3, 3)[ - :, 2 - ] - inverse = np.empty(4) - mujoco.mju_negQuat(inverse, np.array(_mujoco_xr.QUAT_MJ_FROM_XR, dtype=float)) - facing_xr = np.empty(3) - mujoco.mju_rotVecQuat(facing_xr, facing, inverse) - return yaw_of_direction(facing_xr, np.array([0.0, 0.0, -1.0])) - - def gripper_pose_mj(self) -> tuple[np.ndarray, np.ndarray]: - """The gripper body's ``(pos, quat_wxyz)`` in MuJoCo world coordinates. - - Callers want the orientation: it is what the gate demands of the wrist and what - the clutch latches. The position is the body's, 98.4 mm short of the jaw the arm - is placed by, so do not read it as the tool point. - """ - return ( - np.array(self._data.xpos[self._gripper], dtype=float), - np.array(self._data.xquat[self._gripper], dtype=float), - ) - - # ------------------------------------------------------------------ drives - - def drive( - self, - hand_pos_xr: np.ndarray, - q_facing_xr: np.ndarray, - q_base_yaw_xr: np.ndarray, - stick_x: float, - stick_y: float, - dt: float, - ) -> None: - """One disengaged frame: the jaw at the live grip offset off ``hand_pos_xr``, the - base on ``q_base_yaw_xr``. - - Both land on :data:`GRIPPER_SITE`, so the yaw turns the arm about the jaw. Both - yaws arrive already computed: deriving them needs the grip calibration, which this - module does not get to learn. ``q_facing_xr`` is where the controller points and - ``q_base_yaw_xr`` what to turn the base onto; they differ by app.py's measured - bias. Only legal once :attr:`anchored` and while DISENGAGED -- an offset moving - while the arm is frozen applies its excursion on the release frame. - """ - self._walk(stick_x, stick_y, dt) - # A direction, so it crosses onto the yaw by rotation alone. The CONTROLLER'S - # facing, so the offset is what the operator sees: stick forward sends the arm - # away along the pointing ray, and yawing the controller carries the arm around - # with it at a fixed relative position. Not the BASE yaw, which leads the facing - # by the bias and would send "forward" off by that much. A yaw leaves the vertical - # term untouched, so this one rotate is correct for all three. - offset = np.empty(3) - mujoco.mju_rotVecQuat(offset, self._grip_from_controller_xr, q_facing_xr) - self._place(np.asarray(hand_pos_xr, dtype=float) + offset, q_base_yaw_xr) - - @property - def grip_from_controller_xr(self) -> np.ndarray: - """The live gripper-from-controller offset in XR axes, tuning included.""" - return self._grip_from_controller_xr.copy() - - def _walk(self, stick_x: float, stick_y: float, dt: float) -> None: - """Walk the offset's two horizontal terms at the thumbstick's deflection. - - The caller passes raw stick axes and this decides where they point: OpenXR's - stick is +x right and +y forward while XR is +x right and -z forward, so x follows - the stick and z opposes it. Both are read in the CONTROLLER's frame by - :meth:`drive`, so forward is further along the pointing ray. The vertical term is - never touched. - """ - step = _TUNE_RATE_M_S * float(dt) - delta = np.array([deflection(stick_x) * step, 0.0, -deflection(stick_y) * step]) - if not delta.any(): - if self._tuning: - self._tuning = False - # In the constant's own form, so a headset session ends in a value that - # can be pasted back into this file. - LOG.info( - "follower: offset tuned to GRIP_FROM_CONTROLLER_XR = " - "np.array([%.2f, %.2f, %.2f])", - *self._grip_from_controller_xr, - ) - return - tuned = self._grip_from_controller_xr + delta - # Indexed rather than whole-vector: the vertical term is not tuned, so it must - # not be bounded by a limit chosen for the horizontal ones. - tuned[[0, 2]] = np.clip(tuned[[0, 2]], -_TUNE_LIMIT_M, _TUNE_LIMIT_M) - self._grip_from_controller_xr = tuned - self._tuning = True - - # -------------------------------------------------------------- appearance - - def set_visible(self, visible: bool) -> None: - """Draw the arm, or not. Its collision geoms are never drawn either way. - - An un-anchored arm cannot be shown at all: drawing it against the - reference-space origin is the bug the anchor exists to fix. Enforced here - rather than only at the call site. - """ - set_geoms_visible(self._model, self._visual_geoms, visible and self.anchored) - - def set_engageable(self, engageable: bool) -> None: - """Green when the clutch would latch on a squeeze, the authored colour otherwise.""" - rgba = self._blocked_rgba.copy() - if engageable: - rgba[:3] = _ENGAGEABLE_RGB - self._model.mat_rgba[self._material] = rgba - - def log_placement(self) -> None: - """One line naming the placement rule, before any head pose exists.""" - LOG.info( - "follower: SO-101 home grip %.2f m below and %.2f m in front of the HEAD, " - "turned onto its facing, on the first frame carrying one. Hidden until then. " - "After it: the JAW dragged rigidly by the controller at (%.2f, %.2f, %.2f) " - "off it, turning about itself on the wrist's own yaw, with the right " - "thumbstick trimming the horizontal pair to +-%.2f m.", - -HOME_GRIP_FROM_HEAD_XR[1], - -HOME_GRIP_FROM_HEAD_XR[2], - *GRIP_FROM_CONTROLLER_XR, - _TUNE_LIMIT_M, - ) - - -def deflection(axis: float) -> float: - """One stick axis past the deadzone, or zero inside it. NaN reads as inside. - - Spelled ``not >=`` rather than ``<`` so a non-finite axis falls inside: everything - the stick drives is latched, so one bad frame would poison it for the whole session. - Public because app.py's yaw trim integrates the same axis under the same deadzone. - """ - value = float(axis) - return 0.0 if not abs(value) >= _STICK_DEADZONE else value - - -def _site(model, name: str) -> int: - site = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) - if site < 0: - raise RuntimeError( - f"mujoco_xr: the scene declares no `{name}` site; upstream's MJCF stopped " - "publishing its tool frame, and the arm has no point to be placed by." - ) - return site - - -def _body(model, name: str) -> int: - body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if body < 0: - raise RuntimeError( - f"mujoco_xr: the scene declares no `{name}` body; it must " - "assets/follower/follower_arm.xml." - ) - return body - - -def _subtree_geoms(model, root: int) -> np.ndarray: - """Every geom on ``root`` and its descendants, by geom id. - - ``body_rootid`` is the top of the kinematic tree a body belongs to, so this holds - exactly while ``root`` is a direct child of world -- which the scene guarantees. - """ - return np.where(model.body_rootid[model.geom_bodyid] == root)[0].astype(np.int32) - - -def _check_qpos_layout(model) -> None: - """What licenses writing ``Q_HOME`` straight into ``qpos[:6]``. - - The follower must be the scene's only jointed body, in upstream's order, so a - scene that gains a second one fails here rather than landing Q_HOME's angles on - somebody else's joints. - """ - names = tuple(ARM_JOINTS) + (GRIPPER_JOINT,) - if not (model.nq == model.njnt == len(names)): - raise RuntimeError( - f"mujoco_xr: expected {len(names)} hinge DOFs and nothing else, got " - f"nq={model.nq} njnt={model.njnt}." - ) - for index, expected in enumerate(names): - actual = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, index) - if actual != expected: - raise RuntimeError( - f"mujoco_xr: joint {index} is `{actual}`, expected `{expected}`; upstream's " - "joint order changed and Q_HOME would pose the wrong joints." - ) - # Q_HOME is six angles in radians; a slide joint would read them as metres. - # Hinges take one qpos slot each, so this is also what makes the addresses - # 0..5 and lets Q_HOME be written as a slice. - if model.jnt_type[index] != mujoco.mjtJoint.mjJNT_HINGE: - raise RuntimeError(f"mujoco_xr: joint `{actual}` is not a hinge.") diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/harness.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/harness.py deleted file mode 100644 index 5587427349..0000000000 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/harness.py +++ /dev/null @@ -1,254 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""The safety harness the ghost renders, and the signal that it intervened. - -``EePoseRateLimiter`` is a three-band governor -- pass-through, clamped, refused -- and -the ghost renders its *output*, so an intervention already shows as the tool lagging -the hand. A lag alone does not say which band, so :class:`InterventionMonitor` recovers -it by comparing what the limiter was given against what it emitted, and recolours the -ghost. Colour goes on the shared ``leader_ghost`` material rather than per geom. -""" - -from __future__ import annotations - -import enum - -import mujoco -import numpy as np -from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource -from isaacteleop.retargeting_engine.interface import BaseRetargeter, RetargeterIOType -from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO -from isaacteleop.retargeting_engine.interface.tensor_group_type import ( - OptionalType, - TensorGroupType, -) -from isaacteleop.retargeting_engine.tensor_types import ( - ControllerInput, - ControllerInputIndex, - DLDataType, - NDArrayType, -) -from isaacteleop.retargeters.rate_limiter import EE_POSE_KEY - -# The material every ghost geom in assets/leader/leader_gripper.xml carries. -GHOST_MATERIAL = "leader_ghost" - - -class HandPose(enum.Enum): - """Which standard OpenXR controller pose the app drives from. - - Different frames for different jobs, per the OpenXR spec. Grip is the palm centroid, - for rendering a held object; its -Z runs little finger to thumb, through the fist, - and is not a pointing direction. Aim's -Z is the pointing ray. A facing read off grip - therefore turns 1:1 with the hand but has an arbitrary zero. - """ - - GRIP = "grip" - AIM = "aim" - - @property - def indices(self) -> tuple[int, int, int]: - """``(position, orientation, is_valid)`` in ``ControllerInput`` for this pose.""" - if self is HandPose.GRIP: - return ( - ControllerInputIndex.GRIP_POSITION, - ControllerInputIndex.GRIP_ORIENTATION, - ControllerInputIndex.GRIP_IS_VALID, - ) - return ( - ControllerInputIndex.AIM_POSITION, - ControllerInputIndex.AIM_ORIENTATION, - ControllerInputIndex.AIM_IS_VALID, - ) - - -def _pose_type() -> TensorGroupType: - """The 7-D ``[x, y, z, qx, qy, qz, qw]`` contract EePoseRateLimiter governs.""" - return TensorGroupType( - EE_POSE_KEY, - [NDArrayType("pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32)], - ) - - -class ControllerPoseSource(BaseRetargeter): - """One controller pose, repacked as the 7-D ``ee_pose`` the limiter takes. - - Emits in the XR reference frame; ``mj_from_xr`` is rigid, so limiting here and - transforming afterwards bounds the same metres and radians. Goes absent on an invalid - pose rather than holding the last one, which is the limiter's job. Everything - downstream consumes this output, so :class:`HandPose` switches the whole app at once. - """ - - def __init__( - self, - name: str, - pose: HandPose = HandPose.GRIP, - input_device: str = ControllersSource.RIGHT, - ) -> None: - """Initialize the controller-pose adapter. - - Args: - name: Name identifier for this retargeter node. - pose: Which OpenXR controller pose to read. - input_device: Controller source key to read the pose from. - """ - self._input_device = input_device - self._pose = pose - super().__init__(name=name) - - @property - def pose(self) -> HandPose: - """Which OpenXR controller pose this emits.""" - return self._pose - - def input_spec(self) -> RetargeterIOType: - """Requires the configured controller (Optional).""" - return {self._input_device: OptionalType(ControllerInput())} - - def output_spec(self) -> RetargeterIOType: - """Outputs an Optional absolute 7-D ``ee_pose``.""" - return {EE_POSE_KEY: OptionalType(_pose_type())} - - def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: - """Repacks the pose; goes absent when the controller is untracked.""" - out = outputs[EE_POSE_KEY] - inp = inputs[self._input_device] - position_index, orientation_index, valid_index = self._pose.indices - if inp.is_none or not bool(inp[valid_index]): - out.set_none() - return - - position = inp[position_index] - orientation = inp[orientation_index] - # Both orientations are already (x, y, z, w), the limiter's convention. - out[0] = np.array( - [ - float(position[0]), - float(position[1]), - float(position[2]), - float(orientation[0]), - float(orientation[1]), - float(orientation[2]), - float(orientation[3]), - ], - dtype=np.float32, - ) - - -class HarnessBand(enum.Enum): - """Which of the limiter's three bands produced the frame the ghost renders.""" - - PASS_THROUGH = "pass-through" - CLAMPED = "clamped" - REJECTED = "rejected" - - -# Above the float32 round-trip and quaternion-recomposition floor (~1e-7), far below -# anything an operator could see. A band decided by numerical noise would strobe the -# ghost every frame. -_POS_EPS_M = 1e-4 -_ANG_EPS_RAD = 1e-3 - - -def _moved(a: np.ndarray, b: np.ndarray) -> bool: - """True when two 7-D poses differ by more than the noise floor. - - Double-cover aware on the quaternion: the two signs are the same rotation. - """ - dot = min(1.0, abs(float(np.dot(a[3:7], b[3:7])))) - return ( - float(np.linalg.norm(a[:3] - b[:3])) > _POS_EPS_M - or 2.0 * float(np.arccos(dot)) > _ANG_EPS_RAD - ) - - -def classify( - given: np.ndarray, emitted: np.ndarray, previous: np.ndarray | None -) -> HarnessBand: - """The band, from the pose the limiter was given and the one it emitted. - - Reading it off the poses keeps the limiter unmodified and works for any governor - with the same contract. - - Args: - given: The 7-D pose handed to the limiter this frame. - emitted: The 7-D pose it produced. - previous: The pose it produced last frame, or None on the first. - """ - if not _moved(given, emitted): - return HarnessBand.PASS_THROUGH - # Emitted nothing new while the input moved away: refused, not approached. A clamp - # always closes some of the gap, so it cannot land here. - if previous is not None and not _moved(emitted, previous): - return HarnessBand.REJECTED - return HarnessBand.CLAMPED - - -# rgb only: the authored alpha is kept, because the ghost is opaque by design (see -# assets/leader/leader_gripper.xml). -_BAND_RGB = { - HarnessBand.CLAMPED: (1.00, 0.72, 0.20), - HarnessBand.REJECTED: (1.00, 0.25, 0.20), -} - - -class InterventionMonitor: - """Classifies each governed frame and recolours the ghost to match. - - Holds the previous emitted pose, which is what separates a refused frame from a - clamped one, and counts the bands so a session can be summarised afterwards. - """ - - def __init__(self, model) -> None: - """Latch the authored ghost colour as the pass-through colour. - - Args: - model: The compiled ``mjModel``; must declare :data:`GHOST_MATERIAL`. - """ - self._mat = mujoco.mj_name2id( - model, mujoco.mjtObj.mjOBJ_MATERIAL, GHOST_MATERIAL - ) - if self._mat < 0: - raise RuntimeError( - f"mujoco_xr: the scene declares no `{GHOST_MATERIAL}` material; " - "the ghost cannot report harness interventions." - ) - self._rgba = np.array(model.mat_rgba[self._mat], dtype=np.float64) - self._previous: np.ndarray | None = None - self.counts = dict.fromkeys(HarnessBand, 0) - - @property - def pass_through_rgba(self) -> np.ndarray: - """The authored ghost colour, restored whenever the harness is not acting.""" - return self._rgba.copy() - - def update( - self, model, given: np.ndarray, emitted: np.ndarray, *, paint: bool = True - ) -> HarnessBand: - """Classify this frame, advance the baseline, and (unless told not to) paint. - - Classification runs on every governed frame even while the ghost is hidden: a - gap in the baseline would misclassify the frame the ghost reappears on. - """ - band = classify(given, emitted, self._previous) - self._previous = np.array(emitted, dtype=np.float64) - self.counts[band] += 1 - - if paint: - rgba = self._rgba.copy() - if band in _BAND_RGB: - rgba[:3] = _BAND_RGB[band] - model.mat_rgba[self._mat] = rgba - return band - - def summary(self) -> str: - """One line: how much of the session the harness spent intervening.""" - total = sum(self.counts.values()) - if total == 0: - return "harness: no governed frames" - return "harness: {} frames -- {} clamped, {} rejected".format( - total, - self.counts[HarnessBand.CLAMPED], - self.counts[HarnessBand.REJECTED], - ) diff --git a/examples/mujoco_xr/scripts/fetch-so-arm.sh b/examples/mujoco_xr/scripts/fetch-so-arm.sh deleted file mode 100755 index 38708c1537..0000000000 --- a/examples/mujoco_xr/scripts/fetch-so-arm.sh +++ /dev/null @@ -1,88 +0,0 @@ -#!/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 assets this example draws -- the leader gripper the ghost -# is made of, and the follower arm -- rather than vendoring 18 MB of binary STL -# that Git LFS made every clone pay for. -# -# Nothing calls this at build time: an isolated PEP-517 wheel build must not -# reach the network, so it is an explicit step and the app names it at startup. -# The files are package data, so REINSTALL afterwards -- skip that and the ghost -# works from the source tree and fails from the wheel. -set -euo pipefail - -# The pin. Bump it and the checksums together or the download is refused. -COMMIT="fda892cba81032c46c40976a48c9ceadbf40a9ca" -REPO="TheRobotStudio/SO-ARM100" - -DEST="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)/python/isaacteleop_examples/mujoco_xr/assets" - -# upstream path destination relative to assets/ sha256 -# -# The destination is per entry, not per script: the two tools are separate MJCF -# fragments in separate directories. Both land FLAT beside their fragment -- -# MuJoCo drops an included file's own `meshdir`, so meshes under a further -# assets/ subdirectory fail to open. -# -# The URDF is where app.py's trigger hinge comes from, and having it on disk is -# what lets test_ghost.py check those constants against their source. -# -# sts3215_03a_v1.stl is fetched TWICE, once per tool, because each fragment -# resolves its meshes against its own directory. A symlink or a `../leader/` -# path would tie the follower fragment's layout to the leader's. -# -# joints_properties.xml is deliberately absent: upstream inlines its `` -# block into so101_new_calib.xml rather than ing it, so the file is -# never read. -ASSETS=( - "STL/SO101/Individual/Wrist_Roll_SO101.stl leader/Wrist_Roll_SO101.stl de3a65044dd4ae8bcb9659d8ca2b49598e3f5571edf89f45ad975e9776a7ffee" - "STL/SO101/Individual/Trigger_SO101.stl leader/Trigger_SO101.stl 48ecec3a3710cffdc0ae96d28547e49ddf4cbc93ccd915be7549f78e00ad2850" - "STL/SO101/Individual/Handle_SO101.stl leader/Handle_SO101.stl fb8757bdff009c04c207481dd664813ccdac2ad989acea6057df780b52327281" - "Simulation/SO101/assets/sts3215_03a_v1.stl leader/STS3215_03a.stl a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0" - "Simulation/SO101/so101_new_calib.urdf leader/so101_new_calib.urdf 3a65d2d35e68a8d2f0c2cc176d19b884506543c93ba72980145b80abe276022c" - "LICENSE leader/LICENSE c71d239df91726fc519c6eb72d318ec65820627232b2f796219e87dcf35d0ab4" - - "Simulation/SO101/so101_new_calib.xml follower/so101_new_calib.xml d75253eb568e8a7214db9c631ab7bed4217f608a26f7276ebe9a7636cac82580" - "Simulation/SO101/assets/base_motor_holder_so101_v1.stl follower/base_motor_holder_so101_v1.stl 8cd2f241037ea377af1191fffe0dd9d9006beea6dcc48543660ed41647072424" - "Simulation/SO101/assets/base_so101_v2.stl follower/base_so101_v2.stl bb12b7026575e1f70ccc7240051f9d943553bf34e5128537de6cd86fae33924d" - "Simulation/SO101/assets/motor_holder_so101_base_v1.stl follower/motor_holder_so101_base_v1.stl 31242ae6fb59d8b15c66617b88ad8e9bded62d57c35d11c0c43a70d2f4caa95b" - "Simulation/SO101/assets/motor_holder_so101_wrist_v1.stl follower/motor_holder_so101_wrist_v1.stl 887f92e6013cb64ea3a1ab8675e92da1e0beacfd5e001f972523540545e08011" - "Simulation/SO101/assets/moving_jaw_so101_v1.stl follower/moving_jaw_so101_v1.stl 785a9dded2f474bc1d869e0d3dae398a3dcd9c0c345640040472210d2861fa9d" - "Simulation/SO101/assets/rotation_pitch_so101_v1.stl follower/rotation_pitch_so101_v1.stl 9be900cc2a2bf718102841ef82ef8d2873842427648092c8ed2ca1e2ef4ffa34" - "Simulation/SO101/assets/sts3215_03a_no_horn_v1.stl follower/sts3215_03a_no_horn_v1.stl 75ef3781b752e4065891aea855e34dc161a38a549549cd0970cedd07eae6f887" - "Simulation/SO101/assets/sts3215_03a_v1.stl follower/sts3215_03a_v1.stl a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0" - "Simulation/SO101/assets/under_arm_so101_v1.stl follower/under_arm_so101_v1.stl d01d1f2de365651dcad9d6669e94ff87ff7652b5bb2d10752a66a456a86dbc71" - "Simulation/SO101/assets/upper_arm_so101_v1.stl follower/upper_arm_so101_v1.stl 475056e03a17e71919b82fd88ab9a0b898ab50164f2a7943652a6b2941bb2d4f" - "Simulation/SO101/assets/waveshare_mounting_plate_so101_v2.stl follower/waveshare_mounting_plate_so101_v2.stl e197e24005a07d01bbc06a8c42311664eaeda415bf859f68fa247884d0f1a6e9" - "Simulation/SO101/assets/wrist_roll_follower_so101_v1.stl follower/wrist_roll_follower_so101_v1.stl 4b17b410a12d64ec39554abc3e8054d8a97384b2dc4a8d95a5ecb2a93670f5f4" - "Simulation/SO101/assets/wrist_roll_pitch_so101_v2.stl follower/wrist_roll_pitch_so101_v2.stl 6c7ec5525b4d8b9e397a30ab4bb0037156a5d5f38a4adf2c7d943d6c56eda5ae" -) - -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}" - mkdir -p "$(dirname "$target")" - 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/README.md b/examples/robot_viz/README.md similarity index 71% rename from examples/mujoco_xr/README.md rename to examples/robot_viz/README.md index c3733c6bd3..168a126922 100644 --- a/examples/mujoco_xr/README.md +++ b/examples/robot_viz/README.md @@ -3,31 +3,39 @@ SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All SPDX-License-Identifier: Apache-2.0 --> -# MuJoCo XR +# Robot Viz -A MuJoCo scene rendered stereoscopically into an Isaac Teleop Televiz XR +A robot twin rendered stereoscopically into an Isaac Teleop Televiz XR session: an SO-101 **follower arm** the operator drags around by hand, and an SO-101 **leader gripper** that replaces it once the clutch engages. -Single process, single thread, **one** OpenXR session: +Single process, **one** OpenXR session, two threads: ``` -VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession - │ │ - │ recommended resolution │ EePoseRateLimiter output - ▼ ▼ +TeleopSession ──┬── DeviceIOSession ── trackers ── the retargeting graph + │ │ + └── render thread ── SceneTwin ◀── publish(…) + │ mjr_render ─blit─▶ flip + depth-invert ─glReadPixels─▶ PBO ═CUDA═▶ submit() ``` -That is the thesis: `VizSession` (rendering) and `TeleopSession` (input) share -one OpenXR session via `get_oxr_handles()`, and **MuJoCo's own renderer** -reaches `ProjectionLayer.submit()` by CUDA pointer with no copy through host -memory. Nothing else in this repository does that. - -**`cpp/` is a readback, not a renderer.** `mjr_render` draws into MuJoCo's -offscreen framebuffer; `cpp/gl_readback.cpp` blits that into a sampleable pair, +That is the thesis: `TeleopSession` owns both halves — it creates the OpenXR +session the trackers and the compositor share, and runs the twin's frame loop on +a thread of its own — and **MuJoCo's own renderer** reaches +`ProjectionLayer.submit()` by CUDA pointer with no copy through host memory. +Nothing else in this repository does that. + +`isaacteleop.viz.robot` holds no `mjModel` and no `mjData`. It addresses the scene by name and +publishes what moved; `twin.py` applies the lot on the render thread, runs +forward kinematics and draws. Four things move — a body's pose, the joint array, a +group's visibility, a material's colour — and one thing is measured, a frame's +offset from another, **once, at load**. That bound is what lets the backend be +linked against a MuJoCo the user's environment knows nothing about. + +**The backend is a readback, not a renderer.** `mjr_render` draws into MuJoCo's +offscreen framebuffer; `src/viz/robot_twin/cpp/gl_readback.cpp` blits that into a sampleable pair, runs one fullscreen pass, and reads the result into a pixel-pack buffer that -CUDA imports. Every step stays in video memory. The GL half of `cpp/` — `gl.*`, +CUDA imports. Every step stays in video memory. The GL half of that module — `gl.*`, `gl_readback.*`, `gl_functions.inc` — is ~700 lines of it, and owns no shading, no meshes and no camera maths beyond six frustum numbers. @@ -56,29 +64,34 @@ buffer → mailbox array → swapchain. **What it buys:** every geom type, the s XML's materials, lights, shadows and reflections, and MuJoCo's own mesh handling. -`_mujoco_xr` links `libmujoco`, so this example ships as its own wheel rather -than inside `isaacteleop` — otherwise that wheel's contents would depend on -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. +**MuJoCo is an implementation detail of `isaacteleop`, not a dependency of it.** +The backend lives in `src/viz/robot_twin/`, ships with Televiz on Linux, and +ships **its own MuJoCo under a private name** — `libisaacteleop_mujoco.so`, +loaded by `dlopen`/`dlsym`, so the extension exports exactly one symbol, +`PyInit__robot_twin`, and has no undefined `mj*` for another `libmujoco` to +answer. So whatever `mujoco` the environment has, at whatever version or +none at all, is unrelated to the twin's. + +The name-only API above is what makes that safe. Were an `mjModel*` allocated by +a `mujoco` wheel and dereferenced by field offset in our extension, two +libmujocos would make that version skew *undetectable* rather than merely +unsafe. Nothing outside `scene.py` holds a scene handle, so there is no second +copy's layout to agree with. + +This example is therefore **pure Python** — no compiled extension, no ABI tag, +no `mujoco` pin. ## Status — read this before anything else | | | |---|---| -| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the frustum, the clock, the ghost overlay and its jaw channel, the safety harness the ghost renders, the follower's head anchoring and rigid drag, and the whole clutch handoff driven at frame rate through the real pipeline, all pure CPU; **plus `test_readback.py`, which drives the real GPU path** (mjr_render → blit → flip/invert → PBO → CUDA). That one needs CUDA-OpenGL interop, so it wants a discrete NVIDIA GPU; it skips loudly elsewhere. **Measured on Jetson/Tegra it skips**, because `cudaGLGetDevices` reports the EGL context on no CUDA device — so a green `ctest` there does *not* mean the GPU path ran. | | **Never executed anywhere** | **The XR half** — everything downstream of the readback. See [Not verified anywhere](#not-verified-anywhere-in-ci-or-on-a-developer-desktop). | -| **Wrong by construction until calibrated** | Nothing, now. The follower is placed against the **measured head pose** rather than the reference-space origin, and its offset is authored in the **XR** frame and pushed through `mj_from_xr`, so `kTransMjFromXr` appears once with each sign and cancels — see [Frames](#frames-cppframeshpp). | +| **Wrong by construction until calibrated** | Nothing, now. The follower is placed against the **measured head pose** rather than the reference-space origin, and its offset is authored in the **XR** frame and pushed through `mj_from_xr`, so `kTransMjFromXr` appears once with each sign and cancels — see [Frames](#frames-robot_twincppframeshpp). | -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 +Renderer + MuJoCo + rig, and one scene: `viz/robot/assets/scene.xml` — an **SO-101 follower arm** and an **SO-101 leader gripper ghost**, exactly one of which is drawn at a time. No table, no blocks, no ground plane: this is an AR scene and passthrough is the background. @@ -86,7 +99,7 @@ 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 +one. It cannot show a wrong `src/viz/robot_twin/cpp/frames.hpp`: those constants place it, and the eye pose reaches MuJoCo world through the same ones, so they cancel and the ghost lands in the hand whatever they say. Only static content shows them, and the shipped scene has none. @@ -103,149 +116,97 @@ SO-101 that will read the same output arrives with the scene catalogue. harness governs is the **clutch's** output; see [The clutch and the follower preview](#the-clutch-and-the-follower-preview). -Two calibrations, and they are different in kind. `cpp/frames.hpp` is a +Two calibrations, and they are different in kind. `src/viz/robot_twin/cpp/frames.hpp` is a *convention* fixed by two specs and cannot be wrong at runtime. -`_QUAT_HAND_FROM_GHOST` / `_POS_HAND_FROM_GHOST` in `app.py` are a *measurement* +`_QUAT_HAND_FROM_GHOST` / `POS_HAND_FROM_GHOST` in `viz.robot.so101_ghost` +are a *measurement* of how a hand holds a tool, taken on a headset and checkable nowhere else. See -[Frames](#frames-cppframeshpp). +[Frames](#frames-robot_twincppframeshpp). ## Build -**This example is its own wheel, and the wheel is the only way to run it.** +**Nothing here is compiled.** What has to be built is `isaacteleop` itself; this +example is pure Python that imports 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 +cmake -B build -DBUILD_VIZ=ON +cmake --build build --parallel ``` -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.20, a C++ compiler, CUDA, and the OpenGL headers -(`libgl-dev` on Debian/Ubuntu — `cuda_gl_interop.h` includes `` -unconditionally, so this is CUDA's requirement as much as ours). No Vulkan and -no `glslangValidator`: the readback shader is a string the driver compiles at -runtime. Nothing is *linked* against OpenGL either — `cpp/gl.hpp` takes the -enums and the `PFNGL...PROC` typedefs from ``, which declares no -symbol, and resolves the 45 entry points in `cpp/gl_functions.inc` through the -platform `GetProcAddress` against the context `mujoco.GLContext` created. -Running the app additionally needs a GPU with EGL + CUDA and a headset. **Build -isolation does not cover the non-Python half of that list**: on a host missing -CUDA or the GL headers the install fails *inside* the isolated PEP-517 build, -with the CMake or compiler error wrapped in backend output. - -**On a multi-GPU host, set `MUJOCO_EGL_DEVICE_ID`.** The OpenGL context has to -land on the same card viz picked, and nothing makes that happen by default — -`MUJOCO_EGL_DEVICE_ID` indexes EGL devices, which need not agree with CUDA's -ordering. The renderer checks at construction and names both device numbers -rather than render into the wrong card's memory. +There is no flag for the twin: a Linux `BUILD_VIZ` build has it. The first +configure fetches MuJoCo and its six vendored dependencies from GitHub and +compiles them (~40 s on 12 cores). A Windows Televiz build omits the twin — its +headless OpenGL context is EGL. -**`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. +To just *run* the thing, `uv pip install .` drives the same CMake and skips the +`build/` tree entirely: ```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 +uv pip install . # from the repo root +uv pip install -e ./examples/robot_viz # same environment +python -m isaacteleop_examples.robot_viz ``` -A green build does **not** mean this example compiled. The reliable check: +Both must land in **one** environment. Beware `uv pip install isaacteleop` without a +path or `--find-links`: a published `isaacteleop` exists on PyPI and will resolve, with +no robot twin in it. -```bash -cmake --preset py3.12 -DBUILD_VIZ=ON 2>&1 | grep '^-- mujoco_xr:' -``` +`-e` is the way to iterate: the example is pure Python, so there is no extension to go +stale behind an editable install. -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. +Running additionally needs a GPU with EGL + CUDA, and a headset. -**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. +**On a multi-GPU host, pass the GPU.** The OpenGL context has to land on the same +card viz picked, and nothing makes that happen by default: `SceneTwin` takes a +`gl_device_index`, which indexes EGL devices and need not agree with CUDA's +ordering. The renderer checks at construction and names both device numbers +rather than render into the wrong card's memory. ## Run ```bash -python -m isaacteleop_examples.mujoco_xr --help # includes CloudXRLauncher's flags +python -m isaacteleop_examples.robot_viz --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 -m isaacteleop.rig rigs/robot_viz.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: +`{python}` in the rig expands to the interpreter you launch it with, so the +`isaacteleop` wheel has to be installed *there* — not in the build venv. 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__)" +python -c "import sys, isaacteleop; from isaacteleop.viz.robot import SceneTwin; print(sys.executable, isaacteleop.__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 +That import is the whole check: it fails on an `isaacteleop` built without the +twin, and says so. 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 +python -m isaacteleop_examples.robot_viz --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]` +an OpenXR error before any of this example's code runs — **no `[robot_viz]` lines at all** is the tell. -There is one scene and no flag to change it: `assets/scene.xml` is package data +There is one scene and no flag to change it: `viz/robot/assets/scene.xml` is package data beside the module, and editing it is how you load something else. There is no -desktop or headless display mode; without a headset the verification path is -[`ctest -L mujoco_xr`](#tests). +desktop or headless display mode. ## The harness the ghost renders -The ghost's pose comes from an `EePoseRateLimiter` (`harness.py`, +The ghost's pose comes from an `EePoseRateLimiter` (`viz.robot.harness`, `_build_pipeline()`), so what the operator sees is **the command a follower would execute**, not where their hand is. That is the point: [#738](https://github.com/NVIDIA/IsaacTeleop/issues/738) reports operators @@ -268,7 +229,7 @@ Colour is written to the shared `leader_ghost` **material**, so one write recolours the whole tool. Do not switch it to `geom_rgba`: that silently wins over the material, and the four geoms would then have to be kept in step by hand. Alpha stays 1.0 in every band — -`assets/leader/leader_gripper.xml` explains what opacity buys, and a translucent +`viz/robot/assets/leader_gripper.xml` explains what opacity buys, and a translucent "intervening" state would quietly take those risks back. `InterventionMonitor` recovers the band by comparing what the limiter was handed @@ -303,7 +264,7 @@ package now lives. Disengaged, the operator drives the **follower's** gripper, and two things move it independently: -- **Position** is the controller's, all three axes — `follower.py` slides the +- **Position** is the controller's, all three axes — `viz.robot.preview_arm` slides the whole arm so its **jaw** tracks the hand every frame, the grip offset off it *exactly*. - **Yaw** is the controller's own yaw, every frame, with no button held. The base @@ -345,7 +306,7 @@ in the constant's own form: follower: offset tuned to GRIP_FROM_CONTROLLER_XR = np.array([-0.31, -0.10, -0.22]) ``` -**Paste that back into `follower.py` as the new default.** It is a headset +**Paste that back into `viz.robot.preview_arm` as the new default.** It is a headset judgement and no headless test can make it, so this is how a session becomes a constant rather than a note-to-self. B (`SECONDARY_CLICK`) puts it back to the authored value on its rising edge, for an offset walked out to its clamp or a @@ -398,7 +359,7 @@ translation pivot only. **The cost is real and is the thing to watch in a headset.** `aim`'s *origin* is a device-specific ray origin rather than the palm centroid, so the arm's position gains a lever arm that swings as the wrist turns. Flip `HAND_POSE` back to -`HandPose.GRIP` to compare — but re-tune `_EULER_HAND_FROM_GHOST_DEG` when you do, +`HandPose.GRIP` to compare — but re-tune `EULER_HAND_FROM_GHOST_DEG` when you do, because the ghost calibration is relative to whichever frame that constant names. **Which axis you read was a leakage budget on `grip`; on `aim` there is nothing to @@ -422,8 +383,8 @@ axis sits from horizontal at the posture held; the 27.88° there is a near-verti singularity reached only because that stand-in axis starts 43.7° up, and a real aim ray held level does not go near it. **That is an expectation, not a measurement** — the grip-to-aim transform is per-device and no headless test here can supply it, so -re-measure the leak on a headset. `follower.yaw_of_axis` takes the axis as a -**required** argument for exactly this reason; `follower.yaw_of` keeps `-Z` and is +re-measure the leak on a headset. `viz.robot.yaw_of_axis` takes the axis as a +**required** argument for exactly this reason; `viz.robot.yaw_of` keeps `-Z` and is for the **head** alone, whose `-Z` genuinely is its view direction. **`_YAW_TRIM_DEG` should now be zero, and a session that needs a large one is @@ -444,7 +405,7 @@ leakage of its own. **The ghost calibration's two halves are now sourced differently, and that is the point.** -Its **rotation**, `_EULER_HAND_FROM_GHOST_DEG`, is **solved, not measured**. Nothing +Its **rotation**, `EULER_HAND_FROM_GHOST_DEG`, is **solved, not measured**. Nothing in it is a free choice once you decide what posture the engage gate should ask for, and *that* is the thing worth choosing. On `aim` a pose's `-Z` is the pointing ray, so demanding "level and unrolled" means "hold the controller the way you would @@ -461,13 +422,13 @@ ceiling. Bearing is deliberately unpinned, hence the rounding to whole degrees: base tracks the hand's yaw, so the gate's yaw cancels and `base_yaw_bias` absorbs the 2.8° that is left. -Its **translation**, `_POS_HAND_FROM_GHOST`, stays a headset measurement — no posture +Its **translation**, `POS_HAND_FROM_GHOST`, stays a headset measurement — no posture pins it — and the shipped value was measured on `grip`. That port *is* per-device, so `_log_hand_frames` computes it from one frame with both poses valid and prints it: ``` hand frames: this device's aim pose sits 50 deg and 40 mm off its grip pose. HAND_POSE is AIM, so for the ghost to sit where it did on GRIP, its POSITION wants: -hand frames: _POS_HAND_FROM_GHOST = np.array((-0.000, 0.020, 0.015)) +hand frames: POS_HAND_FROM_GHOST = np.array((-0.000, 0.020, 0.015)) ``` Both terms of that port are needed — the origins' separation *and* the old offset @@ -487,7 +448,7 @@ roll: J5 turns the gripper about its tool axis without moving the links, so aimi one leaves the other off by that much. Aiming the jaw is the choice here, and the cost is the arm's body sitting **92.79°** to the side of where you point. The two are coupled through the calibration — moving the bias moves the demanded posture -with it — so `_EULER_HAND_FROM_GHOST_DEG` must be re-solved whenever the aimed axis +with it — so `EULER_HAND_FROM_GHOST_DEG` must be re-solved whenever the aimed axis or `Q_HOME` changes. **The arm's yaw cancels the hand's, and that is why no button locks one.** The @@ -537,20 +498,37 @@ clutch latched?". `SO101ClutchRetargeter.is_engaged` is the sole authority for that; the phase takes it as an input every frame and never copies it into a field. -**Green means all three of these hold**, and `follower.py`'s gate returns every -one that does not, which `app.py` logs on each transition: +**Green means all three of these hold**, and +`isaacteleop.viz.robot.EngageGate` — a plain object, driven once a frame from +`after_step` — returns every one that does not, which `app.py` logs on each +transition: - the hand's rotation is within the enter band of the rotation the clutch would latch, - the rate limiter is passing through, not clamping, - and all of that has held for a dwell. +The first and third are the gate's; the second is this app's, passed as `app_ok` +and named `("limiter", "still catching up")` so the operator's log says what +actually blocked. + +It is deliberately **not** a graph node. Three of its four operands — the +reference pose, the app's conjunct, and the phase machine's +`permits_engagement` — are things `_Preview` already holds, so routing them +through the DAG only meant marshalling bools into tensors and back. The one +thing node-hood bought was same-frame controller data; driving the gate from +`after_step` instead gets that back, and now the reference no longer lags the +controller by a frame either. What it costs is that the permission reaches the +clutch on the following step — 14 ms at 72 Hz, against the gate's own 100 ms +dwell — and squeezing inside that costs nothing, because a denied latch stays +**owed**. + **There is no reach conjunct, and no reach envelope.** The rigid drag puts the gripper exactly at its offset from the hand every frame, so a position residual is identically zero and a limit on it would forbid nothing — the arm goes -wherever the hand goes, including places no articulated SO-101 could reach. -`EngageGate.evaluate` says so where the conjunct used to be; do not add one back -believing it keeps the operator inside a workspace this preview does not have. +wherever the hand goes, including places no articulated SO-101 could reach. The +node says so where the conjunct used to be; do not add one back believing it +keeps the operator inside a workspace this preview does not have. The rotation conjunct is the whole point. The leader is rebased onto the follower's rotation at engage, so if the operator's wrist is 40° from where the @@ -590,7 +568,7 @@ and **warns** past 45° on the thumb. It does not refuse to start: this app is t only place the calibration can be judged, so aborting would prevent the inspection the log exists for. -Read the **hand-axis** direction when judging `_EULER_HAND_FROM_GHOST_DEG` — the log names it `pointing` on `aim` and `thumb` on `grip`. The tool +Read the **hand-axis** direction when judging `EULER_HAND_FROM_GHOST_DEG` — the log names it `pointing` on `aim` and `thumb` on `grip`. The tool direction does not contain the calibration at all — it is a guard on `Q_HOME` and a mesh refresh, and it reads the same 15° for a calibration that is right and one that is 118° wrong. Both are reported in the **arm's own frame** — XR axes @@ -601,37 +579,42 @@ Against the reference space's `-Z` they would just report where the operator happened to stand. **Nothing integrates.** `mj_step` is never called: the follower is slid by its -base and read back through `mj_forward`, and the ghost is two mocap bodies. +base and every frame on it derived from two constants measured at `Q_HOME`, and +the ghost is two mocap bodies. Upstream's six `position` actuators are therefore inert — with `ctrl = 0` and `mj_step` they would drag the arm back to `qpos0` at about 1 rad per 0.4 s. There are deliberately no `gravity="0 0 0"` or `` attributes in `scene.xml`: flags that suppress dynamics nobody runs tell the next reader that dynamics run. The invariant that replaces them is simply *`qpos` is written -once, by `follower.py`, and `body_pos` only by `Follower._move_base`*. +once, by `viz.robot.preview_arm`, and `body_pos` only by `Follower._move_base`*. With no `mj_step` there is also nothing left to refresh derived state, so the second invariant is the pass `mj_step` used to supply for free: **one -`mj_forward` after every `qpos`, `body_pos` *or* `mocap_*` write, before every +forward-kinematics pass after every joint, body-pose *or* mocap write, before every `xpos` / `xquat` / `geom_xpos` read — including the read inside `mjv_updateScene`.** `mocap_pos`/`mocap_quat` are *inputs to* forward kinematics and the renderer draws `geom_xpos`, so a correct mocap row is not a drawn pose. -`follower.py` owns that call for the arm and `_Preview.after_step` for the ghost; -nothing while `ENGAGED` moves the arm, which is why the ghost cannot borrow the -drag's. +`SceneTwin.render` owns that call and is the only place it appears, which is what +the publish-and-apply split buys: a caller cannot forget it, because a caller +cannot write to the scene at all. `SO101ClutchRetargeter` gains one input for this, `ENGAGE_PERMITTED_INPUT`: an `OptionalType` boolean checked **only** where a latch is owed, so it gates the latch and never the engagement, and absent or unwired means permitted. It is an -enable precondition, not a safety-rated stop. +enable precondition, not a safety-rated stop. `EngageGate` is what fills it here, +through the one external leaf this app feeds; the alignment math lives in +`isaacteleop.viz.robot` rather than in this example, because its reference +operand is measured off the twin and the affordance is one the operator has to +see. ## Conventions you can break -### Frames (`cpp/frames.hpp`) +### Frames (`robot_twin/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 +translation. 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). @@ -658,10 +641,10 @@ applied to one of the two conversions and not the other would move the gripper and leave the scene put, which is precisely the symptom this example exists to disambiguate. -### Where the ghost sits on the hand (`app.py`) +### Where the ghost sits on the hand (`viz.robot.so101_ghost`) -A *second* calibration, and a different kind: `_EULER_HAND_FROM_GHOST_DEG` and -`_POS_HAND_FROM_GHOST` place the leader gripper on the operator's hand. Without +A *second* calibration, and a different kind: `EULER_HAND_FROM_GHOST_DEG` and +`POS_HAND_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. @@ -685,11 +668,9 @@ side of it, and the jaws run **60.7°** off the loop's long axis. The OpenXR 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_HAND_FROM_GHOST` slides it along the hand-pose axes if the angle +convention as a MuJoCo `euler=` attribute. Change one angle in +`src/python/isaacteleop/viz/robot/so101_ghost.py` and relaunch: `Rz` spins the gripper about its own long axis, `Rx` / `Ry` tilt it in +the hand, and `POS_HAND_FROM_GHOST` slides it along the hand-pose 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 @@ -757,28 +738,27 @@ correctly. The 17 STLs are **fetched, not vendored** — 18 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. - -Each entry names its own destination: the two tools are separate MJCF fragments -in separate directories, and each resolves meshes against its own, so -`sts3215_03a_v1.stl` is fetched **twice** rather than aliased across. Both sets -land **flat** beside their fragment — MuJoCo drops an included file's own -`meshdir`, so upstream's `meshdir="assets"` is inert once included. +every clone pay for them. `viz.robot.assets.ensure_so101_scene()` fetches them on +the first run into `~/.cache/isaacteleop/so101-assets/`; `ISAACTELEOP_SO101_ASSETS` +overrides that, which is how a host with no route to GitHub gets a pre-populated +directory. Nothing fetches at *build* time: an isolated PEP-517 wheel build must +not reach the network. + +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 three MJCF wrappers are tracked package data and are +re-copied into the cache on every call, so editing one takes effect on the next +launch with no cache to clear and no reinstall. + +Each entry names its own destination, and everything lands **flat in one +directory** — MuJoCo drops an included file's own `meshdir`, so upstream's +`meshdir="assets"` is inert once included and one directory resolves the scene, +both fragments and every mesh under any resolution rule. `sts3215_03a_v1.stl` is +fetched **twice** rather than aliased across, because the leader fragment names +its copy `STS3215_03a.stl`. The follower's MJCF is upstream's own `so101_new_calib.xml`, fetched verbatim and -never edited; `assets/follower/follower_arm.xml` is a tracked wrapper — one +never edited; `viz/robot/assets/follower_arm.xml` is a tracked wrapper — one material and an ``, under a provenance comment. (`joints_properties.xml` is deliberately not fetched: upstream inlines its `` block rather than ``ing it, so the file is never read.) @@ -796,7 +776,7 @@ trigger cannot be a hinged child of the gripper: its angle would live in `qpos`, which nothing here writes — this app never calls `mj_step` and drives the ghost through `mocap_*` alone — so the jaw would never swing. -The ghost is **opaque**, and `test_ghost.py` asserts it. That removes the +The ghost is **opaque**. That removes the draw-order constraint (at alpha 1.0 the depth test decides everything) and the ghost-writes-depth-into-the-reprojection-buffer concern. This scene **does** now put a robot under the ghost — the follower — so opacity is the only thing still @@ -817,36 +797,18 @@ not (`mjv_defaultOption`), so hiding a tool is one write to a slice of draw loop and never writes depth — which is why this is a group switch and not an alpha. No C++ renderer change is needed or wanted for it. -## 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 mjvGLCamera frustum (that it is the fov projected onto the near plane, and that the half-width is set so mjr_render's aspect fallback stays off) and the standard-Z depth contract | -| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, and that the first-frame frustum assertion passes on the real thing and fires on each way it can go wrong | -| `test_readback.py` | **the GPU path**: that something is drawn at all, that row 0 is the top of the operator's view and the image is not mirrored, that the depth handed to `submit()` is standard Z with the background at exactly 1.0, and that the two eyes carry parallax of the right sign. Skips with a reason when there is no GPU | -| `test_ghost.py` | the overlay: that the ghost is opaque, collision-free and carries no mass, that both its bodies are kinematic mocap bodies with no joint anywhere, that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, that the print STLs are scaled from millimetres and the servo is not, 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 | - -All but `test_readback.py` run on a CPU with no GPU, no headset, no CloudXR -runtime and no window system; keep it that way, because a permanently-skipping -test reports green while covering nothing. `test_readback.py` is the deliberate -exception: what it covers is otherwise invisible until someone is wearing a -headset, and it needs no headset itself. - ## Not verified anywhere in CI or on a developer desktop **Everything downstream of the readback.** `ProjectionLayer.submit()`, the -frame loop that sequences it, OpenXR session sharing via `oxr_handles`, whether -the runtime accepts the depth layer, and **controllers on a shared session** — -none of it is executed by any test or on any machine here. `test_readback.py` -covers the render and the CUDA hand-off and stops at `submit()`. How the ghost -*looks* is unverified too, and so is the grip-to-gripper calibration, by -construction — `tests/test_ghost.py` pins the *machinery* and leaves the shipped -constants free to be tuned. +frame loop that sequences it, the OpenXR session the compositor and the trackers +share, 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. Since the +render loop moved onto its own thread there is one more: **`xrSyncActions` now +runs concurrently with `xrWaitFrame` / `xrBeginFrame` / `xrEndFrame`.** That is +legal per the OpenXR spec and unverified against CloudXR; the measurement harness +and its pass criteria are in +`/code/work/20260820-robot-twin-library/s2-xrsync-threading/`. How the ghost +*looks* is unverified too, and so is the grip-to-gripper calibration. Controllers on a shared session have no precedent elsewhere in this repository: `xrAttachSessionActionSets` is legal once per `XrSession`, Teleop sidesteps it diff --git a/examples/robot_viz/pyproject.toml b/examples/robot_viz/pyproject.toml new file mode 100644 index 0000000000..b2b76dd5cf --- /dev/null +++ b/examples/robot_viz/pyproject.toml @@ -0,0 +1,52 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Pure Python -- nothing here is compiled. The scene backend lives in +# isaacteleop.viz.robot, which ships with Televiz on Linux. +# +# uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ +# uv pip install -e ./examples/robot_viz +# python -m isaacteleop_examples.robot_viz # needs a headset; fetches its +# # scene on the first run +# +# setuptools rather than no build backend at all: the neighbouring examples are `uv run` +# manifests and are not installable, but this one is a package, and a pyproject that +# installs to an EMPTY wheel is worse than one that refuses. `-e` is supported and is the +# way to iterate -- there is no extension to go stale. + +[build-system] +requires = ["setuptools>=64"] +build-backend = "setuptools.build_meta" + +[project] +name = "isaacteleop-examples-robot-viz" +version = "0.0.0" # Internal example - not versioned +description = "A robot's digital twin rendered into an Isaac Teleop Televiz XR session" +# Bounds match ISAAC_TELEOP_PYTHON_VERSION_MIN / _MAX_EXCLUSIVE in the root CMakeLists. +requires-python = ">=3.11,<3.14" + +dependencies = [ + # Unversioned, and a live hazard: a published isaacteleop exists on PyPI, so this + # resolves happily against a release that is not this checkout's viz -- and against + # one with no robot twin at all. Install the locally built wheel first. + "isaacteleop", + # app.py imports numpy directly. + "numpy", +] + +[project.optional-dependencies] +dev = ["pytest", "numpy"] + +# `namespaces = true` is load-bearing: isaacteleop_examples is a PEP 420 namespace with +# no __init__.py, shared with every other example. Do not add one to make a discovery +# problem go away -- it would stop a second example sharing the namespace. +[tool.setuptools.packages.find] +where = ["python"] +include = ["isaacteleop_examples*"] +namespaces = true + +# No package-data: the scene and its meshes belong to isaacteleop.viz.robot, which fetches +# them into a cache on first use. This example ships nothing but Python. + +[tool.pytest.ini_options] +pythonpath = ["python"] diff --git a/examples/robot_viz/python/isaacteleop_examples/robot_viz/__init__.py b/examples/robot_viz/python/isaacteleop_examples/robot_viz/__init__.py new file mode 100644 index 0000000000..38f172176d --- /dev/null +++ b/examples/robot_viz/python/isaacteleop_examples/robot_viz/__init__.py @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""A robot's digital twin rendered into an Isaac Teleop Televiz XR session. + +Pure Python. The scene backend lives in `isaacteleop.viz.robot` and carries a MuJoCo of +its own, so this example neither compiles anything nor cares what `mujoco` the +environment happens to have. +""" diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py b/examples/robot_viz/python/isaacteleop_examples/robot_viz/__main__.py similarity index 78% rename from examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py rename to examples/robot_viz/python/isaacteleop_examples/robot_viz/__main__.py index 0b4cb8ca92..a90bbf0d19 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py +++ b/examples/robot_viz/python/isaacteleop_examples/robot_viz/__main__.py @@ -1,7 +1,7 @@ # 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``.""" +"""Entry point: ``python -m isaacteleop_examples.robot_viz``.""" import sys diff --git a/examples/robot_viz/python/isaacteleop_examples/robot_viz/app.py b/examples/robot_viz/python/isaacteleop_examples/robot_viz/app.py new file mode 100644 index 0000000000..74ca0cff1b --- /dev/null +++ b/examples/robot_viz/python/isaacteleop_examples/robot_viz/app.py @@ -0,0 +1,329 @@ +# 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. + +TeleopSession owns both halves: it creates the OpenXR session the trackers and the +compositor share, and runs the twin's frame loop on its own thread. + + TeleopSession ──┬── DeviceIOSession ── trackers ── the retargeting graph + │ │ + └── render thread ── SceneTwin ◀── publish(...) + │ + ProjectionLayer.submit() + +This module holds no mjModel and no mjData. It addresses the scene by name and +publishes what moved -- the base's pose, the ghost's two mocap bodies, which group +is drawn, which material is which colour -- and the render thread applies the lot, +runs one forward-kinematics pass and draws. See twin.py for that contract and README.md for the +rest of the design. + +Two cadences, deliberately: the control loop below is paced off the wall clock and +the render thread off the display. The one thing that crosses back is the head +pose, which anchors the arm once and is read as "where the operator was", never as +a per-frame signal. +""" + +from __future__ import annotations + +import argparse +import importlib.metadata +import logging +import math +import sys +import time +from pathlib import Path + +import numpy as np + +from isaacteleop import viz +from isaacteleop.cloudxr import CloudXRLauncher +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import OutputCombiner, ValueInput +from isaacteleop.retargeters.controller_pose import ControllerPoseSource +from isaacteleop.retargeters.rate_limiter import ( + EE_POSE_KEY, + EePoseRateLimiter, + RateLimiterConfig, +) +from isaacteleop.retargeters.SO101.clutch_retargeter import SO101ClutchRetargeter +from isaacteleop.retargeters.SO101.gripper_retargeter import ( + GRIPPER_COMMAND_KEY, + SO101GripperRetargeter, +) +from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig +from isaacteleop.teleop_session_manager.config import TwinRenderConfig +from isaacteleop.viz.robot import ( + VIEW_COUNT, + ClutchPreview, + EngageGate, + InterventionMonitor, + PreviewArm, + SceneTwin, + assets, + frames, +) +from isaacteleop.viz.robot.clutch_preview import ( + COMMANDED_POSE_KEY, + ENGAGE_PERMITTED_LEAF, + GHOST_HAND, + HAND_POSE, + HAND_POSE_KEY, + PERMITTED_TYPE, + log_grip_posture, +) +from isaacteleop.viz.robot.so101_ghost import ( + GHOST_BODY, + GHOST_JAW_BODY, + TRIGGER_RELEASED_RAD, + TRIGGER_SQUEEZED_RAD, + pose_from_ghost_body, +) + +LOG = logging.getLogger("robot_viz") + +# The app's only clip planes. TwinRenderConfig hands the same pair to the compositor +# and to the twin's projection, or world-locked geometry swims under head motion -- +# and only a headset shows it. There is no near/far literal in the backend, by construction. +NEAR_Z = 0.05 +FAR_Z = 50.0 + +_CLOCK_SOURCE = ( + "FrameInfo.predicted_display_time; frames with no prediction are skipped, " + "not sampled as 0" +) + +# ── What the harness lets through ────────────────────────────────────────── +# Chosen for this demo, not measured against a follower: ordinary reaching passes +# through and a deliberate flick trips the clamp and then the reject band. An +# SO-101's own envelope is lower -- RateLimiterConfig defaults to 0.25 m/s. +_HARNESS = RateLimiterConfig( + max_linear_velocity=0.5, # m/s + max_angular_velocity=2.5, # rad/s, ~143 deg/s + reject_linear_velocity=2.0, # m/s + reject_angular_velocity=10.0, # rad/s +) + + +def _build_pipeline( # noqa: N803 + home_base_T_ee: np.ndarray, +) -> tuple[OutputCombiner, SO101ClutchRetargeter]: + """Controllers, the SO-101 jaw and clutch retargeters, the engage gate and the harness. + + ControllerPoseSource is a parallel branch rather than a link in the clutch's chain: its + Optional output is the app's only tracking-validity oracle. The jaw is ungoverned. + Returns the clutch and the gate too, because the app reads `is_engaged` off the one + and `verdict` off the other. + """ + controllers = ControllersSource(name="controllers") + jaw = SO101GripperRetargeter(name="ghost_jaw", input_device=GHOST_HAND).connect( + {GHOST_HAND: controllers.output(GHOST_HAND)} + ) + hand = ControllerPoseSource( + name="hand_pose", pose=HAND_POSE, input_device=GHOST_HAND + ).connect({GHOST_HAND: controllers.output(GHOST_HAND)}) + + clutch = SO101ClutchRetargeter( + name="ee_pose", + home_base_T_ee=home_base_T_ee, + input_device=GHOST_HAND, + # The same frame the rest of the app drives from. Its orientation delta is + # invariant to the choice, so this is here for the translation pivot alone. + controller_pose=HAND_POSE.value, + ) + # MEASURED_BASE_T_EE_INPUT is left unwired on purpose: it is position-only + # (its own docstring carries the measurement), so it cannot put the leader on + # the follower's orientation. + commanded = clutch.connect( + { + GHOST_HAND: controllers.output(GHOST_HAND), + SO101ClutchRetargeter.ENGAGE_PERMITTED_INPUT: ValueInput( + ENGAGE_PERMITTED_LEAF, PERMITTED_TYPE + ).output(ValueInput.VALUE), + } + ) + governed = EePoseRateLimiter(name="ghost_harness", config=_HARNESS).connect( + {EE_POSE_KEY: commanded.output(EE_POSE_KEY)} + ) + return ( + OutputCombiner( + { + ControllersSource.LEFT: controllers.output(ControllersSource.LEFT), + ControllersSource.RIGHT: controllers.output(ControllersSource.RIGHT), + GRIPPER_COMMAND_KEY: jaw.output(GRIPPER_COMMAND_KEY), + HAND_POSE_KEY: hand.output(EE_POSE_KEY), + COMMANDED_POSE_KEY: commanded.output(EE_POSE_KEY), + EE_POSE_KEY: governed.output(EE_POSE_KEY), + } + ), + clutch, + ) + + +def _log_startup(scene_path, resolution, backend: str, gl_device: int) -> None: + """One block naming every assumption that is invisible at runtime.""" + try: + version = importlib.metadata.version("isaacteleop") + except importlib.metadata.PackageNotFoundError: + version = "" + trans = frames.TRANS_MJ_FROM_XR + + LOG.info("scene: %s", scene_path) + # Several examples ship their own .venv, and picking up the wrong + # isaacteleop is invisible without this line. + LOG.info( + "isaacteleop: %s (version %s)", Path(viz.__file__).resolve().parent, version + ) + # The version the twin is BUILT with, not one the environment supplies: a scene + # authored against a newer MuJoCo fails to compile with an upstream parser error + # that names neither, so this line is where the reader learns which one rejected it. + LOG.info("scene backend: MuJoCo %s (private to the twin)", backend) + LOG.info( + "views: %d (stereo) view resolution: %sx%s", + VIEW_COUNT, + resolution.width, + resolution.height, + ) + LOG.info( + "renderer: MuJoCo's own (mjr_render), headless EGL on device %d, " + "offsamples=0; blitted, y-flipped, depth-inverted, read back through a PBO " + "CUDA imports", + gl_device, + ) + LOG.info( + "clip: near=%.4f far=%.2f (one pair -> XrTwinSession, 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 this session's reference space does not establish (viz.robot.frames)", + trans[0], + trans[1], + trans[2], + ) + LOG.info("clock: %s", _CLOCK_SOURCE) + LOG.info( + "depth: D32F requested. Whether the runtime ACCEPTED it is not queryable, so " + "the absence of errors is not confirmation." + ) + + +def run() -> int: + scene_path = assets.ensure_so101_scene() + twin = SceneTwin(scene_path) + # Before the Renderer, which uploads geometry once: the follower repoints geom + # materials and poses its joints here. Not placed yet -- that waits for the first + # head pose, in _Preview.before_step. + arm = PreviewArm(twin) + monitor = InterventionMonitor(twin) + + # The clutch's home is pushed every non-ENGAGED frame, so this constructor value + # only has to be well-formed -- nothing can latch before the anchor exists. + pipeline, clutch = _build_pipeline(pose_from_ghost_body(*arm.gripper_pose_mj())) + gate = EngageGate(app_conjunct=("limiter", "still catching up")) + preview = ClutchPreview(twin, monitor, arm, clutch, gate) + + teleop_config = TeleopSessionConfig( + app_name="RobotViz", + pipeline=pipeline, + # Never pass trackers=: TeleopSession discovers them from the graph, and + # passing them again duplicates the set. It also aggregates their OpenXR + # extensions and hands that list to the twin's xrCreateInstance, which is why + # nothing here calls get_required_oxr_extensions_from_pipeline. + joint_publisher=twin, + twin_render=TwinRenderConfig(near_z=NEAR_Z, far_z=FAR_Z), + ) + with TeleopSession(teleop_config) as teleop_session: + _log_startup( + scene_path, + teleop_session.twin_resolution, + twin.backend_version, + twin.gl_device_index, + ) + LOG.info( + "leader ghost: %s / %s driven as mocap bodies; trigger driven by " + "SO101GripperRetargeter, %.0f deg released to %.0f deg squeezed", + GHOST_BODY, + GHOST_JAW_BODY, + math.degrees(TRIGGER_RELEASED_RAD), + math.degrees(TRIGGER_SQUEEZED_RAD), + ) + arm.log_placement() + # Before the anchor, and correct there: both angles are reported in the + # operator's own frame, which the anchor's yaw is exactly what defines. + log_grip_posture(arm) + LOG.info( + "harness: the ghost renders the EePoseRateLimiter output, clamped at " + "%.2f m/s / %.0f deg/s and rejecting above %.2f m/s / %.0f deg/s. Amber " + "while clamping, red while rejecting, authored blue passing through.", + _HARNESS.max_linear_velocity, + math.degrees(_HARNESS.max_angular_velocity), + _HARNESS.reject_linear_velocity, + math.degrees(_HARNESS.reject_angular_velocity), + ) + try: + _loop(teleop_session, preview) + finally: + LOG.info(monitor.summary()) + if teleop_session.twin_teardown_clean is False: + # The render thread is still inside the runtime, so CloudXRLauncher must not + # stop a runtime it owns on the way out -- see TeleopSession.twin_teardown_clean. + LOG.error( + "the twin's render thread did not exit; leaving the OpenXR session and " + "the runtime alive. This process will not exit until it completes." + ) + return 1 + return 0 + + +# The control loop's cadence. Wall-clock paced and independent of the display: the +# render thread runs at whatever the runtime hands it, and the graph has no reason to +# match. 30 Hz is the rate a real SO-101 loop runs at, so the preview behaves like one. +CONTROL_HZ = 30.0 +_CONTROL_DT = 1.0 / CONTROL_HZ + + +def _loop(teleop_session, preview: ClutchPreview) -> None: + """Step the graph at :data:`CONTROL_HZ` for as long as the twin is being drawn. + + Nothing here renders. The head pose comes off the session, which publishes whatever + the render thread last saw -- the arm is anchored to where the operator stood, once, + so a pose one frame old is the same pose. + """ + deadline = time.perf_counter() + while teleop_session.twin_rendering: + external_inputs, events = preview.before_step(teleop_session.twin_head_pose) + result = teleop_session.step( + external_inputs=external_inputs, execution_events=events + ) + preview.after_step(result, _CONTROL_DT) + deadline += _CONTROL_DT + time.sleep(max(0.0, deadline - time.perf_counter())) + + +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="[robot_viz] %(message)s", + ) + + with CloudXRLauncher.launch_context(args) as launcher: + if launcher.owns_runtime: + 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/rigs/mujoco_xr.yaml b/rigs/robot_viz.yaml similarity index 54% rename from rigs/mujoco_xr.yaml rename to rigs/robot_viz.yaml index 22c806e009..0d289ce3ee 100644 --- a/rigs/mujoco_xr.yaml +++ b/rigs/robot_viz.yaml @@ -1,27 +1,32 @@ # 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 +# Run with: python -m isaacteleop.rig rigs/robot_viz.yaml # # Two panes, one declared: with no `runtime:` key the runtime pane comes from # DEFAULT_RUNTIME_COMMAND. No producers either -- the app opens the OpenXR # session itself and reads controllers straight from the runtime, so there is no # rendezvous and deliberately no `params:` / `collection_id:`. # -# `{python}` expands to the launching interpreter, so both wheels must be -# installed there rather than in a private .venv: +# `{python}` expands to the launching interpreter, so the wheel must be installed +# there rather than in a private .venv, and it must be a Televiz build -- the robot +# twin ships with BUILD_VIZ on Linux: # # uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ --reinstall -# uv pip install ./examples/mujoco_xr # same environment +# +# The example itself is pure Python and is reached from the source tree, hence +# PYTHONPATH below rather than a second wheel. # # 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 +name: robot_viz +description: CloudXR runtime + a robot twin in XR with the SO-101 leader gripper cwd: .. # -> Teleop repo root consumers: - - name: mujoco xr app (requires headset) + - name: robot viz app (requires headset) # Keep --no-launch-cloudxr-runtime. The runtime is a host singleton on WSS # port 48322, so a second one kills this rig's own pane: the headset drops # mid-session and reads as a runtime crash rather than a config edit. # find_runtime_footguns() warns but never gates. - command: "{python} -m isaacteleop_examples.mujoco_xr --no-launch-cloudxr-runtime" + # PYTHONPATH inline: the rig schema takes only {name, command}, and the example + # is pure Python read straight from the source tree rather than a second wheel. + command: "PYTHONPATH=examples/robot_viz/python {python} -m isaacteleop_examples.robot_viz --no-launch-cloudxr-runtime" diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index 8e6a7904c1..0f364aed09 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -72,6 +72,10 @@ isaacteleop = ["*.so", "*.pyd", "*.pyi", "py.typed"] # Keys for packages staged in only some configurations are inert in the rest: # package-data for a package `find` did not discover matches nothing. "isaacteleop.viz" = ["*.so", "*.pyd", "*.pyi"] +# The robot twin's backend, plus the libisaacteleop_mujoco.so it loads. Not staged on +# Windows, where the twin is not built. assets/ has no __init__.py, so its MJCF ships as +# this package's data rather than as a package of its own. +"isaacteleop.viz.robot" = ["*.so", "*.pyd", "*.pyi", "assets/*.xml"] robotic_grounding = [ "V2D_LICENSE", "assets/xmls/sharpawave/*.xml", diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 73ef7ed12b..ff2d687348 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -14,12 +14,15 @@ if(NOT BUILD_PYTHON_BINDINGS) return() endif() -# CONFIGURE_DEPENDS: adding or removing a .py re-runs configure. Globbing *.py -# rather than copying the directory keeps __pycache__ out by construction. +# CONFIGURE_DEPENDS: adding or removing a file re-runs configure. Globbing by +# extension rather than copying the directory keeps __pycache__ out by construction. +# The .xml are viz/robot/assets/'s MJCF wrappers, which assets.ensure_so101_scene() +# reads out of its own installed directory. file(GLOB_RECURSE ISAAC_TELEOP_PYTHON_SOURCES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/isaacteleop/*.py" + "${CMAKE_CURRENT_SOURCE_DIR}/isaacteleop/*.xml" ) # viz/__init__.py does a top-level `from ._viz import ...`, so without _viz it is diff --git a/src/python/isaacteleop/retargeters/SO101/clutch_retargeter.py b/src/python/isaacteleop/retargeters/SO101/clutch_retargeter.py index ec4a858999..9ec19933d0 100644 --- a/src/python/isaacteleop/retargeters/SO101/clutch_retargeter.py +++ b/src/python/isaacteleop/retargeters/SO101/clutch_retargeter.py @@ -412,7 +412,7 @@ def set_home_base_T_ee(self, home_base_T_ee: np.ndarray) -> None: # noqa: N803 ordering unambiguous. Calling it on **every** non-engaged frame of a ``RUNNING`` session is equally sound, and is what an owner whose arm moves while disengaged wants: the home stays on the arm's live pose, so an engage from anywhere is - jump-free (``examples/mujoco_xr`` does this at frame rate). + jump-free (``examples/robot_viz`` does this at frame rate). The pending latch is re-armed as a safety net rather than the call being rejected. Without it, a call made while engaged would leave the *old* controller origin latched against the diff --git a/src/python/isaacteleop/retargeters/__init__.py b/src/python/isaacteleop/retargeters/__init__.py index 5d2ca7115b..177d6e9876 100644 --- a/src/python/isaacteleop/retargeters/__init__.py +++ b/src/python/isaacteleop/retargeters/__init__.py @@ -23,6 +23,8 @@ - WujiHandRetargeter: Retargeting for the Wuji hand via wuji_sdk.retargeting - JointStateRetargeter: Generic joint-space device (leader arm, exoskeleton) -> joint or EE action - EePoseRateLimiter / JointRateLimiter: Safety-harness velocity bounds for EE-pose / joint streams + - ControllerPoseSource: A controller's grip or aim pose as an ``ee_pose``, Optional so + tracking loss survives the node - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter - Se3AbsRetargeter: Absolute EE pose control @@ -140,6 +142,9 @@ None, ), # .rate_limiter (safety harness: per-frame velocity bounds for EE / joint streams) + # .controller_pose + "ControllerPoseSource": (".controller_pose", "ControllerPoseSource", None), + "HandPose": (".controller_pose", "HandPose", None), "EePoseRateLimiter": (".rate_limiter", "EePoseRateLimiter", None), "JointRateLimiter": (".rate_limiter", "JointRateLimiter", None), "RateLimiterConfig": (".rate_limiter", "RateLimiterConfig", None), @@ -244,7 +249,9 @@ def __getattr__(name: str): "JointStateRetargeter", "JointStateRetargeterConfig", # Safety-harness rate limiters (per-frame velocity bounds) + "ControllerPoseSource", "EePoseRateLimiter", + "HandPose", "JointRateLimiter", "RateLimiterConfig", "Se3AbsRetargeter", diff --git a/src/python/isaacteleop/retargeters/controller_pose.py b/src/python/isaacteleop/retargeters/controller_pose.py new file mode 100644 index 0000000000..9626807685 --- /dev/null +++ b/src/python/isaacteleop/retargeters/controller_pose.py @@ -0,0 +1,135 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Which controller pose a consumer drives from, and that pose as a 7-D ``ee_pose``.""" + +from __future__ import annotations + +import enum + +import numpy as np +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import BaseRetargeter, RetargeterIOType +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + DLDataType, + NDArrayType, +) +from .rate_limiter import EE_POSE_KEY + + +class HandPose(enum.Enum): + """Which standard OpenXR controller pose a consumer drives from. + + Different frames for different jobs, per the OpenXR spec. Grip is the palm centroid, + for rendering a held object; its -Z runs little finger to thumb, through the fist, + and is not a pointing direction. Aim's -Z is the pointing ray. A facing read off grip + therefore turns 1:1 with the hand but has an arbitrary zero. + + The values are the strings ``SO101ClutchRetargeter(controller_pose=...)`` takes, so + one constant switches the whole app. + """ + + GRIP = "grip" + AIM = "aim" + + @property + def indices(self) -> tuple[int, int, int]: + """``(position, orientation, is_valid)`` in :func:`ControllerInput` for this pose.""" + if self is HandPose.GRIP: + return ( + ControllerInputIndex.GRIP_POSITION, + ControllerInputIndex.GRIP_ORIENTATION, + ControllerInputIndex.GRIP_IS_VALID, + ) + return ( + ControllerInputIndex.AIM_POSITION, + ControllerInputIndex.AIM_ORIENTATION, + ControllerInputIndex.AIM_IS_VALID, + ) + + +def _pose_type() -> TensorGroupType: + """The 7-D ``[x, y, z, qx, qy, qz, qw]`` contract the EE-pose nodes share.""" + return TensorGroupType( + EE_POSE_KEY, + [NDArrayType("pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32)], + ) + + +class ControllerPoseSource(BaseRetargeter): + """A controller's pose as an ``ee_pose``, so a rate limiter or gate can take it. + + Emits in whatever reference frame the controller stream is already in; a rigid + rebase downstream bounds the same metres and radians, so limiting here and + transforming afterwards is equivalent. Goes **absent** on an invalid pose rather + than holding the last one -- holding is a governor's job, and a consumer that wants + to know about tracking loss needs the gap to survive this node. + + Inputs: + - ``input_device`` -- Optional :func:`ControllerInput`. + + Outputs: + - ``ee_pose`` -- Optional 7-D ``[x, y, z, qx, qy, qz, qw]`` float32 ``NDArray``. + """ + + def __init__( + self, + name: str, + pose: HandPose = HandPose.GRIP, + input_device: str = ControllersSource.RIGHT, + ) -> None: + """Initialize the controller-pose adapter. + + Args: + name: Name identifier for this retargeter node. + pose: Which OpenXR controller pose to read. + input_device: Controller source key to read the pose from. + """ + self._input_device = input_device + self._pose = pose + super().__init__(name=name) + + @property + def pose(self) -> HandPose: + """Which OpenXR controller pose this emits.""" + return self._pose + + def input_spec(self) -> RetargeterIOType: + """Requires the configured controller (Optional).""" + return {self._input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + """Outputs an Optional absolute 7-D ``ee_pose``.""" + return {EE_POSE_KEY: OptionalType(_pose_type())} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Repacks the pose; goes absent when the controller is untracked.""" + out = outputs[EE_POSE_KEY] + inp = inputs[self._input_device] + position_index, orientation_index, valid_index = self._pose.indices + if inp.is_none or not bool(inp[valid_index]): + out.set_none() + return + + position = inp[position_index] + orientation = inp[orientation_index] + # Both orientations are already (x, y, z, w), the EE-pose convention. + out[0] = np.array( + [ + float(position[0]), + float(position[1]), + float(position[2]), + float(orientation[0]), + float(orientation[1]), + float(orientation[2]), + float(orientation[3]), + ], + dtype=np.float32, + ) diff --git a/src/python/isaacteleop/teleop_session_manager/config.py b/src/python/isaacteleop/teleop_session_manager/config.py index 59c36ac1ea..5fc3b9d3b0 100644 --- a/src/python/isaacteleop/teleop_session_manager/config.py +++ b/src/python/isaacteleop/teleop_session_manager/config.py @@ -291,6 +291,39 @@ class PluginConfig: required: bool = False +@dataclass +class TwinRenderConfig: + """How a session-owned robot twin is rendered. + + Only reaches a twin when :attr:`TeleopSessionConfig.joint_publisher` is set. + + Attributes: + near_z: Near clip plane [m]. Handed to the compositor **and** to the twin, from + here, because a twin projecting against a different pair renders geometry + the runtime then reprojects wrongly. + far_z: Far clip plane [m]. + layer_name: Name of the projection layer the twin is drawn into. + join_timeout_s: How long teardown waits for the render thread at each step + before declaring the teardown unclean and leaving the session alive. + """ + + near_z: float = 0.05 + far_z: float = 50.0 + layer_name: str = "robot_twin" + join_timeout_s: float = 5.0 + + def __post_init__(self) -> None: + """Reject clip planes no projection could be built from. + + Raises: + ValueError: If the planes are not ``0 < near_z < far_z``. + """ + if not 0.0 < self.near_z < self.far_z: + raise ValueError( + f"require 0 < near_z < far_z, got {self.near_z} and {self.far_z}" + ) + + @dataclass class TeleopSessionConfig: """Complete configuration for a teleop session. @@ -344,6 +377,15 @@ class TeleopSessionConfig: retargeting_execution: Synchronous vs. pipelined execution settings for the main retargeting pipeline. Defaults to synchronous exact-current-frame behavior; set ``mode="pipelined"`` to opt into background execution. + joint_publisher: Optional + :class:`~isaacteleop.viz.robot.RobotTwinPublisher` -- a digital twin of the + robot being teleoperated, drawn into the operator's headset. Setting it + makes the session create its own compositor session (so ``oxr_handles`` + must be left unset), own a render thread, and tear both down; no viewer + type becomes public. Publish joints onto the object you passed in, from + the control thread; the render thread draws the latest. + twin_render: Clip planes, layer name and teardown budget for that twin. Ignored + when ``joint_publisher`` is None. Example (auto-discovery): # Source creates its own tracker automatically! @@ -407,12 +449,29 @@ class TeleopSessionConfig: retargeting_execution: RetargetingExecutionConfig = field( default_factory=RetargetingExecutionConfig ) + joint_publisher: Optional[Any] = None + twin_render: TwinRenderConfig = field(default_factory=TwinRenderConfig) def __post_init__(self) -> None: """Validate configuration consistency.""" if self.mode == SessionMode.REPLAY and self.mcap_config is None: raise ValueError("mcap_config is required when mode is SessionMode.REPLAY") + if self.joint_publisher is not None: + # The twin's compositor session IS the OpenXR session, so there is nothing + # for caller-supplied handles to mean here: one of the two would have to be + # ignored, and either choice is a silently different session. + if self.oxr_handles is not None: + raise ValueError( + "joint_publisher and oxr_handles are mutually exclusive: the twin " + "creates the OpenXR session that the trackers then share." + ) + if self.mode == SessionMode.REPLAY: + raise ValueError( + "joint_publisher requires a live session; REPLAY has no headset " + "to render into." + ) + self._validate_sinks() if self.teleop_control_pipeline is None: diff --git a/src/python/isaacteleop/teleop_session_manager/teleop_session.py b/src/python/isaacteleop/teleop_session_manager/teleop_session.py index 5e6aad25a4..8903cccca4 100644 --- a/src/python/isaacteleop/teleop_session_manager/teleop_session.py +++ b/src/python/isaacteleop/teleop_session_manager/teleop_session.py @@ -193,6 +193,10 @@ def __init__(self, config: TeleopSessionConfig): self.plugin_managers: List[pm.PluginManager] = [] self.plugin_contexts: List[Any] = [] + # The robot twin's render thread, when config.joint_publisher is set. + self._twin_runner: Optional[Any] = None + self._twin_teardown_clean: Optional[bool] = None + # Exit stack for RAII resource management self._exit_stack = ExitStack() @@ -234,6 +238,48 @@ def oxr_session(self) -> Optional[oxr.OpenXRSession]: """The internal OpenXR session, or ``None`` when using external handles or after the context manager exits (read-only).""" return self._oxr_session + @property + def twin_teardown_clean(self) -> Optional[bool]: + """Did the robot twin's render thread shut down cleanly? + + ``None`` when no twin was configured or the session has not exited yet. **False + means the compositor session was not destroyed and is still live**: a thread is + somewhere inside the OpenXR runtime, and only that thread may destroy what it + made. A caller that owns the runtime -- one that launched CloudXR itself -- + must NOT stop it on that path; the non-daemon thread keeps the process alive + and the OS reaps everything at exit. + + ``__exit__`` does not raise on False. There is nothing the caller can do about + it that the caller is not already better placed to decide. + """ + return self._twin_teardown_clean + + @property + def twin_resolution(self) -> Optional[Any]: + """Per-view resolution the twin was built at, or ``None`` without one.""" + return None if self._twin_runner is None else self._twin_runner.resolution + + @property + def twin_head_pose(self) -> Optional[Any]: + """The last rendered frame's 7-D head pose ``[x, y, z, qx, qy, qz, qw]``. + + ``None`` without a twin, and before its first rendered frame. Read from the + control thread; it is whatever the render thread last saw, not a pose belonging + to this step. Anchoring content to where the operator stood is what it is for -- + an app deriving per-frame motion from it is reading the wrong clock. + """ + return None if self._twin_runner is None else self._twin_runner.head_pose + + @property + def twin_rendering(self) -> bool: + """Whether the twin's frame loop is still running. + + Goes False when the runtime asks the session to close, which is the operator + taking the headset off or the runtime going away -- an app that wants to stop + on that has to poll it, because nothing else reports it. + """ + return self._twin_runner is not None and self._twin_runner.rendering + @property def last_context(self) -> Optional[ComputeContext]: """Most recent ComputeContext produced by ``step()``, or ``None`` before first step.""" @@ -995,6 +1041,7 @@ def _enter_resources(self, stack: ExitStack) -> None: # Reset run-scoped plugin containers on each context entry. self.plugin_managers = [] self.plugin_contexts = [] + self._twin_teardown_clean = None # Auto-populate mcap_config from pipeline sources if recording or replaying. mcap_config = None @@ -1054,6 +1101,10 @@ def _add_tracker(tracker: Any) -> None: # Resolve OpenXR handles if self.config.oxr_handles is not None: handles = self.config.oxr_handles + elif self.config.joint_publisher is not None: + handles = oxr.OpenXRSessionHandles( + *self._start_twin(stack, required_extensions) + ) else: self._oxr_session = stack.enter_context( oxr.OpenXRSession(self.config.app_name, required_extensions) @@ -1070,6 +1121,15 @@ def _add_tracker(tracker: Any) -> None: # Initialize plugins (if any) self._start_configured_plugins(stack) + if self._twin_runner is not None: + # After DeviceIOSession, so the first rendered frame cannot precede a + # tracker that answers on the same handles. Registered here rather than + # inside _start_twin so the ExitStack unwinds it BEFORE DeviceIOSession: + # the loop must be out of the runtime before the trackers sharing its + # handles go away. + stack.callback(self._stop_twin_rendering) + self._twin_runner.begin_rendering() + # Initialize runtime state self.frame_count = 0 self.start_time = time.time() @@ -1079,6 +1139,53 @@ def _add_tracker(tracker: Any) -> None: self._async_runner = None self._active_retargeting_execution_mode = self.config.retargeting_execution.mode + def _start_twin( + self, stack: ExitStack, required_extensions: List[str] + ) -> Tuple[int, int, int, int]: + """Bring up the robot twin's render thread and return its OpenXR handles. + + The same aggregated ``required_extensions`` every tracker asked for: this is + the ``xrCreateInstance`` call, so an extension discovered afterwards cannot be + added and the tracker needing it would be silently dead rather than an error. + ``get_required_oxr_extensions_from_pipeline`` exists so an *external* viz owner + can precompute that list; this path already has it. + """ + # Imported here, not at module scope: it reaches isaacteleop.viz, which a build + # with BUILD_VIZ=OFF does not ship. + from .twin_runner import TwinRunner + + view = self.config.twin_render + runner = TwinRunner( + self.config.joint_publisher, + app_name=self.config.app_name, + required_extensions=required_extensions, + near_z=view.near_z, + far_z=view.far_z, + layer_name=view.layer_name, + join_timeout_s=view.join_timeout_s, + ) + # Registered before start(), so a failure part-way through creating the session + # still joins the thread; and first of everything in _enter_resources, so it + # unwinds LAST -- the compositor session outlives the trackers borrowing its + # handles. + stack.callback(self._destroy_twin) + self._twin_runner = runner + runner.start() + return runner.oxr_handles + + def _stop_twin_rendering(self) -> None: + """Leave the frame loop; record whether it got out.""" + if self._twin_runner is not None: + self._twin_teardown_clean = self._twin_runner.stop_rendering() + + def _destroy_twin(self) -> None: + """Tear the twin's session down on its own thread, and drop the runner.""" + runner, self._twin_runner = self._twin_runner, None + if runner is None: + return + joined = runner.destroy() + self._twin_teardown_clean = bool(self._twin_teardown_clean) and joined + def _start_configured_plugins(self, stack: ExitStack) -> None: """Start ``config.plugins`` and register them on ``stack`` for cleanup.""" if not self.config.plugins: diff --git a/src/python/isaacteleop/teleop_session_manager/twin_runner.py b/src/python/isaacteleop/teleop_session_manager/twin_runner.py new file mode 100644 index 0000000000..e318a9ab25 --- /dev/null +++ b/src/python/isaacteleop/teleop_session_manager/twin_runner.py @@ -0,0 +1,267 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The render thread a :class:`~isaacteleop.teleop_session_manager.TeleopSession` owns +when it is configured with a robot twin. + +Rendering gets a thread of its own rather than a graph node or a sink. Sinks run inside +``_execute_step_request``, which is the ``AsyncRetargetRunner`` worker under ``PIPELINED`` +and the app thread under ``SYNC``; a GL context is thread-affine, so its host thread +cannot depend on an execution-mode setting. + +**Everything thread-affine happens on this thread, including teardown.** The compositor +session, the projection layer and the twin's own GPU context are created here, and +destroyed here. That is also what makes the "decline to destroy" rule automatic rather +than a discipline: if the thread never leaves its loop, nothing is destroyed, because +nothing else can. + +Importing this module needs ``isaacteleop.viz``, so ``teleop_session`` imports it only +on the branch that actually has a twin. +""" + +from __future__ import annotations + +import logging +import threading +from typing import Any + +from isaacteleop.viz.robot import RobotTwinPublisher, XrTwinSession, head_pose + +logger = logging.getLogger(__name__) + + +class TwinRunner: + """Owns the compositor session, the frame loop, and the thread both live on. + + Driven in four steps by ``TeleopSession``, in this order and no other: + + 1. :meth:`start` -- spawn the thread, create the session, publish + :attr:`oxr_handles`. Blocks until that has happened or failed. + 2. :meth:`begin_rendering` -- release the frame loop, once ``DeviceIOSession`` + exists and can answer ``xrSyncActions`` on the handles this session owns. + 3. :meth:`stop_rendering` -- leave the frame loop, before ``DeviceIOSession`` goes + away underneath it. + 4. :meth:`destroy` -- tear the session down on its own thread and join. + + Steps 3 and 4 are separate because ``ExitStack`` unwinds LIFO and the two belong on + opposite sides of ``DeviceIOSession``'s own teardown. + """ + + def __init__( + self, + twin: Any, + *, + app_name: str, + required_extensions: list[str], + near_z: float, + far_z: float, + layer_name: str, + join_timeout_s: float = 5.0, + ) -> None: + """Configure the runner. Nothing is created until :meth:`start`. + + Args: + twin: A :class:`~isaacteleop.viz.robot.RobotTwinPublisher`. + app_name: OpenXR application name for the compositor session. + required_extensions: Every extension the session's trackers need. Complete + before ``start``, because it is what ``xrCreateInstance`` is given. + near_z: Near clip plane [m], shared by the compositor and the twin. + far_z: Far clip plane [m]. + layer_name: Name for the projection layer. + join_timeout_s: How long each handshake waits before giving up and + reporting an unclean teardown. + + Raises: + TypeError: If ``twin`` does not implement both halves of the protocol. + """ + if not isinstance(twin, RobotTwinPublisher): + missing = [ + name + for name in ( + "publish", + "create", + "render", + "color", + "depth", + "frustum", + "destroy", + ) + if not callable(getattr(twin, name, None)) + ] + raise TypeError( + f"{type(twin).__name__} is not a RobotTwinPublisher; it is missing " + f"{missing}. Both halves are needed: publish() is the control thread's " + "and the rest the render thread's." + ) + self._twin = twin + self._app_name = app_name + self._required_extensions = list(required_extensions) + self._near_z = near_z + self._far_z = far_z + self._layer_name = layer_name + self._join_timeout_s = join_timeout_s + + self._thread: threading.Thread | None = None + self._error: BaseException | None = None + self._handles: tuple[int, int, int, int] | None = None + self._resolution: Any = None + # Latest-wins, no lock: a 7-float array is rebound atomically under the GIL, so + # a reader gets one whole pose or the previous one, never a torn mix. + self._head_pose: Any = None + + # Each event is set by exactly one side and waited on by the other. `destroy` + # sets all three of the thread's gates, so a thread parked at any of them + # proceeds rather than stranding the join. + self._created = threading.Event() # thread -> main: session up, or _error set + self._go = threading.Event() # main -> thread: start rendering + self._stop = threading.Event() # main -> thread: leave the frame loop + self._loop_done = threading.Event() # thread -> main: out of the frame loop + self._destroy = threading.Event() # main -> thread: tear down now + + # ------------------------------------------------------------------ lifecycle + + def start(self) -> None: + """Spawn the thread and block until the compositor session exists. + + Raises: + RuntimeError: If already started. + BaseException: Whatever the thread raised while creating the session. + """ + if self._thread is not None: + raise RuntimeError("TwinRunner already started") + # Non-daemon on purpose. If this thread ever wedges inside the runtime, the + # process must stay alive rather than exit and tear a live OpenXR session out + # from under it -- the same rule camera_viz's VizRunner documents. + self._thread = threading.Thread( + target=self._run, name="isaacteleop_robot_twin", daemon=False + ) + self._thread.start() + self._created.wait() + if self._error is not None: + raise self._error + + def begin_rendering(self) -> None: + """Release the frame loop. Call once ``DeviceIOSession`` is up.""" + self._go.set() + + def stop_rendering(self) -> bool: + """Leave the frame loop and wait for the thread to be out of it. + + Returns: + True if the thread left the loop within the join budget. False means it is + still inside the runtime, and nothing downstream may be torn down. + """ + self._stop.set() + self._go.set() + if self._thread is None: + return True + self._loop_done.wait(timeout=self._join_timeout_s) + if not self._loop_done.is_set(): + logger.warning( + "robot twin render loop did not exit within %.1fs; leaving the " + "compositor session and the OpenXR runtime alive rather than tearing " + "them down under a live frame", + self._join_timeout_s, + ) + return False + return True + + def destroy(self) -> bool: + """Tear the session down on its own thread and join it. + + Idempotent, and safe when :meth:`start` never ran or raised. + + Returns: + True if the thread joined, which is also the only case in which the + compositor session was destroyed at all. + """ + thread, self._thread = self._thread, None + if thread is None: + return True + self._stop.set() + self._go.set() + self._destroy.set() + thread.join(timeout=self._join_timeout_s) + if thread.is_alive(): + logger.error( + "robot twin thread did not join within %.1fs. The compositor session " + "was NOT destroyed -- only that thread may destroy it -- and the " + "process will stay alive until the thread completes. Do not stop a " + "self-owned OpenXR runtime on this path.", + self._join_timeout_s, + ) + return False + if self._error is not None: + logger.error( + "robot twin thread failed", + exc_info=(type(self._error), self._error, self._error.__traceback__), + ) + return True + + # ------------------------------------------------------------------ state + + @property + def oxr_handles(self) -> tuple[int, int, int, int]: + """``(instance, session, space, proc_addr)``, valid after :meth:`start`.""" + if self._handles is None: + raise RuntimeError("TwinRunner has no OpenXR handles; start() it first") + return self._handles + + @property + def resolution(self) -> Any: + """The per-view resolution the layer and the twin were built at.""" + return self._resolution + + @property + def head_pose(self) -> Any: + """The most recent rendered frame's head pose, or None before the first. + + Published from the render thread because that is where ``FrameInfo`` lives. A + control loop reads the latest rather than one belonging to its own tick, which + is what keeps the two cadences independent. + """ + return self._head_pose + + @property + def rendering(self) -> bool: + """Whether the frame loop is still running.""" + return self._thread is not None and not self._loop_done.is_set() + + # ------------------------------------------------------------------ thread + + def _run(self) -> None: + try: + with XrTwinSession( + self._twin, + app_name=self._app_name, + near_z=self._near_z, + far_z=self._far_z, + required_extensions=self._required_extensions, + layer_name=self._layer_name, + ) as xr: + self._handles = xr.oxr_handles() + self._resolution = xr.resolution + self._created.set() + + self._go.wait() + try: + if not self._stop.is_set(): + for info in xr.frames(): + self._head_pose = head_pose(info) + xr.render(info) + if self._stop.is_set(): + break + finally: + self._loop_done.set() + + # Held open until DeviceIOSession is gone: it borrows these very + # handles, so destroying the session first would pull them out from + # under it. `destroy()` is what releases this. + self._destroy.wait() + except BaseException as error: # noqa: BLE001 -- reported to the owner + if self._error is None: + self._error = error + finally: + # Never strand a waiter, whatever went wrong and wherever. + self._created.set() + self._loop_done.set() diff --git a/src/python/isaacteleop/viz/robot/__init__.py b/src/python/isaacteleop/viz/robot/__init__.py new file mode 100644 index 0000000000..d48f51ff42 --- /dev/null +++ b/src/python/isaacteleop/viz/robot/__init__.py @@ -0,0 +1,142 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""A teleoperated robot's digital twin, and the affordances that depend on it. + +Rendering the twin into a Televiz XR session is the bulk of it; :mod:`.engage_gate` is +here because its reference operand is measured off the twin and the affordance it serves +is one the operator has to *see*. + +Scoped to that on purpose: this is not a general scene-graph or viewer API. The +scene backend sits behind :class:`RobotTwin`, and only :mod:`.scene` and :mod:`.frames` +reach it -- everything else here is numpy and duck typing. + +Those two are the wheel's only compiled scene code, and their backend is Linux-only -- +its OpenGL context is EGL. They are therefore resolved lazily, so this package still +imports on a Windows Televiz build and asking for :class:`SceneTwin` there is what raises. +""" + +from .clutch_phase import DROPOUT_TIMEOUT_S, ClutchPhase, PhaseMachine +from .harness import GHOST_MATERIAL, HarnessBand, InterventionMonitor, classify +from .operator_frame import MIN_HORIZONTAL, OperatorFrame +from .engage_gate import ( + KEY_ENGAGED, + KEY_ROTATION, + KEY_SETTLING, + KEY_UNJUDGED, + KEY_UNREFERENCED, + KEY_UNTRACKED, + EngageGate, + EngageGateConfig, + GateVerdict, +) +from .anchor import ( + UNIT_QUAT_TOL, + anchor_from_head, + is_unit, + yaw_of, + yaw_of_axis, + yaw_of_direction, +) +from .frame_info import assert_frustum, flatten_views, head_pose +from . import quaternion +from .quaternion import MIN_QUAT_NORM +from .joint_map import JointMap +from .session import VIEW_COUNT, WAIT_FOR_HEADSET, XrTwinSession +from .twin import RobotTwin, RobotTwinPublisher + +# Resolved on first access rather than imported: everything below reaches `frames` or +# `scene`, and those need the compiled `_robot_twin` that a Windows Televiz build has no +# copy of. `clutch_phase`, `engage_gate`, `harness`, `quaternion` and `anchor` are plain +# numpy and stay on the eager path above. +_LAZY = { + "ClutchPreview": ".clutch_preview", + "PreviewArm": ".preview_arm", + "SceneTwin": ".scene", + "assets": ".assets", + "clutch_preview": ".clutch_preview", + "deflection": ".preview_arm", + "frames": ".frames", + "preview_arm": ".preview_arm", + "scene": ".scene", + "so101_ghost": ".so101_ghost", +} +#: Which of those names are the module itself rather than something inside it. Spelled out +#: rather than inferred from the case: `deflection` is a lowercase function. +_LAZY_MODULES = frozenset( + {"assets", "clutch_preview", "frames", "preview_arm", "scene", "so101_ghost"} +) + + +def __getattr__(name: str): + module_path = _LAZY.get(name) + if module_path is None: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + import importlib + + try: + module = importlib.import_module(module_path, __name__) + except ImportError as error: + # Only a MISSING BACKEND gets re-labelled. A typo in a relative import or a broken + # sibling must surface as itself -- re-labelling everything makes every real error + # inside a lazy module read as "build with -DBUILD_VIZ=ON", which costs a debugging + # session before anyone reads the traceback's own first frame. + if "_robot_twin" not in str(error): + raise + raise ImportError( + f"isaacteleop.viz.robot.{name} needs the compiled scene backend, which this " + "build does not have. It ships with Televiz on Linux: build with " + "-DBUILD_VIZ=ON." + ) from error + value = module if name in _LAZY_MODULES else getattr(module, name) + globals()[name] = value + return value + + +__all__ = [ + "DROPOUT_TIMEOUT_S", + "GHOST_MATERIAL", + "KEY_ENGAGED", + "KEY_ROTATION", + "KEY_SETTLING", + "KEY_UNJUDGED", + "KEY_UNREFERENCED", + "KEY_UNTRACKED", + "MIN_QUAT_NORM", + "UNIT_QUAT_TOL", + "VIEW_COUNT", + "WAIT_FOR_HEADSET", + "EngageGate", + "EngageGateConfig", + "GateVerdict", + "ClutchPhase", + "ClutchPreview", + "HarnessBand", + "InterventionMonitor", + "JointMap", + "MIN_HORIZONTAL", + "OperatorFrame", + "PhaseMachine", + "PreviewArm", + "RobotTwin", + "RobotTwinPublisher", + "SceneTwin", + "XrTwinSession", + "anchor_from_head", + "assert_frustum", + "assets", + "classify", + "clutch_preview", + "deflection", + "preview_arm", + "flatten_views", + "frames", + "head_pose", + "is_unit", + "quaternion", + "scene", + "so101_ghost", + "yaw_of", + "yaw_of_axis", + "yaw_of_direction", +] diff --git a/src/python/isaacteleop/viz/robot/anchor.py b/src/python/isaacteleop/viz/robot/anchor.py new file mode 100644 index 0000000000..a88d9f7210 --- /dev/null +++ b/src/python/isaacteleop/viz/robot/anchor.py @@ -0,0 +1,106 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Horizontal bearing, and where operator-anchored content goes. + +Every pose here is in a gravity-aligned Y-up XR reference space -- ``LOCAL``, +``LOCAL_FLOOR``, ``STAGE`` or ``UNBOUNDED`` -- where +Y is world up. A ``VIEW``-space pose +yields a silently wrong bearing rather than an error. Poses stay correct across a runtime +recentre; a yaw LATCHED across one does not, so re-anchor on +``XrEventDataReferenceSpaceChangePending``. +""" + +from __future__ import annotations + +import math + +import numpy as np + +from .quaternion import rotate + + +#: How far a quaternion's norm may sit from 1 and still carry a usable rotation. Wide +#: enough for a float32 round-trip and renormalisation drift, far tighter than the +#: shrinkage :func:`~isaacteleop.viz.robot.quaternion.rotate` would otherwise apply in +#: silence. +UNIT_QUAT_TOL = 1e-3 + + +def is_unit(q: np.ndarray) -> bool: + """Whether ``q`` is finite and unit to :data:`UNIT_QUAT_TOL`. + + Layout-agnostic -- a norm does not care whether it is handed wxyz or xyzw. Every + function here that turns something rejects exactly what this rejects, so a caller + reading a quaternion off a device gets one predicate to gate on rather than a + tolerance to re-guess. + """ + q = np.asarray(q, dtype=float) + if not np.all(np.isfinite(q)): + return False + return abs(float(np.linalg.norm(q)) - 1.0) <= UNIT_QUAT_TOL + + +def yaw_of_direction(forward_xr: np.ndarray, fallback_xr: np.ndarray) -> np.ndarray: + """The horizontal bearing of an XR direction, as a wxyz quaternion about +Y. + + ``forward_xr`` must be unit length: the near-vertical test below is an absolute + 1e-6, so a magnified direction a hair off vertical never reaches the fallback and + returns a garbage bearing. + ``fallback_xr`` covers a direction within a hair of vertical, which has no bearing to + report -- callers pass the pose's own up-vector, which is what holds heading up to + and at vertical (past it, the bearing reverses). This is the single definition of + bearing, so anything built on it tracks a world-vertical turn 1:1. + """ + forward = np.asarray(forward_xr, dtype=float) + if abs(forward[0]) < 1e-6 and abs(forward[2]) < 1e-6: + # Straight up or down -- a headset face-down on a desk, a controller held + # muzzle-up. The fallback then points along the horizon: forwards when the + # direction points down, backwards when up. + forward = -math.copysign(1.0, forward[1]) * np.asarray(fallback_xr, dtype=float) + + half = 0.5 * math.atan2(-forward[0], -forward[2]) + return np.array([math.cos(half), 0.0, math.sin(half), 0.0]) + + +def yaw_of_axis(q_xyzw: np.ndarray, forward_local: np.ndarray) -> np.ndarray: + """The horizontal facing of an XR orientation, as a wxyz quaternion about +Y. + + ``forward_local`` names which axis of the pose is its facing, in the pose's own + frame. No default: each axis is blind to rotation about itself and sensitive to the + rest, so it must be chosen against the motions the reading has to ignore. + """ + q_wxyz = np.asarray(q_xyzw, dtype=float)[[3, 0, 1, 2]] + # Raise on genuinely broken input, absorb float drift. See `rotate` on what a short + # quaternion does here if it is let through. Callers reading a quaternion off a + # device gate on `is_unit` rather than catch this: it raises on the frame loop. + if not is_unit(q_wxyz): + raise ValueError( + f"q_xyzw must be a unit quaternion; norm is {float(np.linalg.norm(q_wxyz))}" + ) + q_wxyz = q_wxyz / float(np.linalg.norm(q_wxyz)) + forward = rotate(np.asarray(forward_local, dtype=float), q_wxyz) + up = rotate(np.array([0.0, 1.0, 0.0]), q_wxyz) + return yaw_of_direction(forward, up) + + +def yaw_of(q_xyzw: np.ndarray) -> np.ndarray: + """The horizontal facing of a HEAD pose, reading its -Z as the view direction.""" + return yaw_of_axis(q_xyzw, np.array([0.0, 0.0, -1.0])) + + +def anchor_from_head( + head_pose_xr: np.ndarray, offset_xr: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + """Where content anchored to the operator goes, from a 7-D head pose. + + Takes ``(position, xyzw)`` in XR and an offset in the head's yaw frame; returns the + XR position and the head's YAW as a wxyz quaternion. The same yaw does both jobs: + it carries ``offset_xr`` onto the head's facing, and the caller turns its content by + it. The returned orientation is gravity-aligned: head pitch and roll are discarded. + Content whose correct pose is not level -- an inclined base, a wall mount -- needs + full SO(3) and cannot be placed by this. + """ + pose = np.asarray(head_pose_xr, dtype=float) + q_yaw = yaw_of(pose[3:7]) + offset = rotate(np.asarray(offset_xr, dtype=float), q_yaw) + return pose[:3] + offset, q_yaw diff --git a/src/python/isaacteleop/viz/robot/assets.py b/src/python/isaacteleop/viz/robot/assets.py new file mode 100644 index 0000000000..c0a6e4c070 --- /dev/null +++ b/src/python/isaacteleop/viz/robot/assets.py @@ -0,0 +1,211 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The SO-101 scene the preview arm and the leader ghost are drawn from. + +The three MJCF files are tracked package data; the 18 MB of upstream mesh and MJCF they +name is **fetched, not vendored** -- Git LFS made every clone pay for binary that upstream +already publishes at a stable commit. :func:`ensure_so101_scene` assembles both halves into +one cache directory and returns the scene path. + +Downloads are checksum-verified against a pinned commit. A raw.githubusercontent path is +not immutable in practice, and a silently substituted mesh renders as a broken arm rather +than an error, which has already cost a debugging session. + +Everything lands **flat**: MuJoCo drops an included file's own ``meshdir``, so one +directory resolves the scene, both fragments and every mesh under any resolution rule. The +leader's servo is fetched under its own name, so it does not collide with the follower's +copy of the same part. +""" + +from __future__ import annotations + +import hashlib +import os +import shutil +import urllib.request +from pathlib import Path + +#: Bump this and the checksums together, or the download is refused. +SO_ARM_REPO = "TheRobotStudio/SO-ARM100" +SO_ARM_COMMIT = "fda892cba81032c46c40976a48c9ceadbf40a9ca" + +#: ``(upstream path, destination name, sha256)``. The destination is per entry because +#: ``sts3215_03a_v1.stl`` is fetched **twice** -- the leader fragment names its copy +#: ``STS3215_03a.stl`` -- and a single flat directory cannot alias the two. +#: +#: ``so101_new_calib.urdf`` is not drawn: it is where :mod:`.so101_ghost`'s trigger hinge +#: and its 0..100 degree travel come from, so it is on disk to check them against. +#: ``joints_properties.xml`` is deliberately absent -- upstream inlines its ```` +#: block rather than ````-ing it, so the file is never read. +SO_ARM_ASSETS: tuple[tuple[str, str, str], ...] = ( + # The leader gripper ghost. + ( + "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", + ), + # The follower arm. + ( + "Simulation/SO101/so101_new_calib.xml", + "so101_new_calib.xml", + "d75253eb568e8a7214db9c631ab7bed4217f608a26f7276ebe9a7636cac82580", + ), + ( + "Simulation/SO101/assets/base_motor_holder_so101_v1.stl", + "base_motor_holder_so101_v1.stl", + "8cd2f241037ea377af1191fffe0dd9d9006beea6dcc48543660ed41647072424", + ), + ( + "Simulation/SO101/assets/base_so101_v2.stl", + "base_so101_v2.stl", + "bb12b7026575e1f70ccc7240051f9d943553bf34e5128537de6cd86fae33924d", + ), + ( + "Simulation/SO101/assets/motor_holder_so101_base_v1.stl", + "motor_holder_so101_base_v1.stl", + "31242ae6fb59d8b15c66617b88ad8e9bded62d57c35d11c0c43a70d2f4caa95b", + ), + ( + "Simulation/SO101/assets/motor_holder_so101_wrist_v1.stl", + "motor_holder_so101_wrist_v1.stl", + "887f92e6013cb64ea3a1ab8675e92da1e0beacfd5e001f972523540545e08011", + ), + ( + "Simulation/SO101/assets/moving_jaw_so101_v1.stl", + "moving_jaw_so101_v1.stl", + "785a9dded2f474bc1d869e0d3dae398a3dcd9c0c345640040472210d2861fa9d", + ), + ( + "Simulation/SO101/assets/rotation_pitch_so101_v1.stl", + "rotation_pitch_so101_v1.stl", + "9be900cc2a2bf718102841ef82ef8d2873842427648092c8ed2ca1e2ef4ffa34", + ), + ( + "Simulation/SO101/assets/sts3215_03a_no_horn_v1.stl", + "sts3215_03a_no_horn_v1.stl", + "75ef3781b752e4065891aea855e34dc161a38a549549cd0970cedd07eae6f887", + ), + ( + "Simulation/SO101/assets/sts3215_03a_v1.stl", + "sts3215_03a_v1.stl", + "a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0", + ), + ( + "Simulation/SO101/assets/under_arm_so101_v1.stl", + "under_arm_so101_v1.stl", + "d01d1f2de365651dcad9d6669e94ff87ff7652b5bb2d10752a66a456a86dbc71", + ), + ( + "Simulation/SO101/assets/upper_arm_so101_v1.stl", + "upper_arm_so101_v1.stl", + "475056e03a17e71919b82fd88ab9a0b898ab50164f2a7943652a6b2941bb2d4f", + ), + ( + "Simulation/SO101/assets/waveshare_mounting_plate_so101_v2.stl", + "waveshare_mounting_plate_so101_v2.stl", + "e197e24005a07d01bbc06a8c42311664eaeda415bf859f68fa247884d0f1a6e9", + ), + ( + "Simulation/SO101/assets/wrist_roll_follower_so101_v1.stl", + "wrist_roll_follower_so101_v1.stl", + "4b17b410a12d64ec39554abc3e8054d8a97384b2dc4a8d95a5ecb2a93670f5f4", + ), + ( + "Simulation/SO101/assets/wrist_roll_pitch_so101_v2.stl", + "wrist_roll_pitch_so101_v2.stl", + "6c7ec5525b4d8b9e397a30ab4bb0037156a5d5f38a4adf2c7d943d6c56eda5ae", + ), +) + +#: The tracked wrappers. Re-copied on every call rather than gated on the completeness +#: marker, so editing one takes effect on the next run with no cache to clear. +SCENE_FILE = "scene.xml" +_WRAPPERS = (SCENE_FILE, "follower_arm.xml", "leader_gripper.xml") + +#: Overrides where the assets are cached. Point it at a pre-populated directory on a host +#: with no route to GitHub. +CACHE_ENV_VAR = "ISAACTELEOP_SO101_ASSETS" + + +def cache_dir() -> Path: + """Where the assembled scene lives.""" + override = os.environ.get(CACHE_ENV_VAR, "").strip() + if override: + return Path(override) + root = os.environ.get("XDG_CACHE_HOME", "").strip() or str(Path.home() / ".cache") + return Path(root) / "isaacteleop" / "so101-assets" + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as handle: + for block in iter(lambda: handle.read(1 << 20), b""): + digest.update(block) + return digest.hexdigest() + + +def ensure_so101_scene(dest: Path | str | None = None) -> Path: + """Assemble the scene into ``dest`` (default :func:`cache_dir`) and return its path. + + The completeness marker gates the download only: the MJCF's mere existence is not a + completeness signal, because an interrupted first run leaves the meshes it names + missing and would then hide that forever. Re-running repairs a partial cache; delete + the directory to force a re-download. + + Raises: + RuntimeError: If a download's checksum does not match :data:`SO_ARM_ASSETS`. + OSError: If the files cannot be fetched or written. + """ + dest = cache_dir() if dest is None else Path(dest) + dest.mkdir(parents=True, exist_ok=True) + + source = Path(__file__).parent / "assets" + for wrapper in _WRAPPERS: + shutil.copyfile(source / wrapper, dest / wrapper) + + marker = dest / ".fetch_complete" + if not marker.exists(): + for remote, name, sha in SO_ARM_ASSETS: + target = dest / name + if target.is_file() and _sha256(target) == sha: + continue + url = f"https://raw.githubusercontent.com/{SO_ARM_REPO}/{SO_ARM_COMMIT}/{remote}" + with urllib.request.urlopen(url, timeout=120) as response: # nosec B310 + payload = response.read() + if hashlib.sha256(payload).hexdigest() != sha: + raise RuntimeError( + f"robot twin: checksum mismatch for {remote}. Upstream changed, or " + "SO_ARM_COMMIT and the hashes in SO_ARM_ASSETS disagree." + ) + target.write_bytes(payload) + marker.touch() + + # Absolute: on mujoco 3.11 a relative model path mis-composes an d file's + # path and fails naming a file that exists. + return (dest / SCENE_FILE).resolve() diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower/follower_arm.xml b/src/python/isaacteleop/viz/robot/assets/follower_arm.xml similarity index 69% rename from examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower/follower_arm.xml rename to src/python/isaacteleop/viz/robot/assets/follower_arm.xml index fb98171f8b..08b46ee4c4 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/follower/follower_arm.xml +++ b/src/python/isaacteleop/viz/robot/assets/follower_arm.xml @@ -5,27 +5,25 @@ SPDX-License-Identifier: Apache-2.0 The SO-101 follower arm: what the operator drags before the clutch engages. A wrapper around upstream's own MJCF, which is fetched verbatim and NEVER -edited -- scripts/fetch-so-arm.sh checksums it, so an edit here is a fetch -failure on the next clone. +edited -- assets.py checksums it, so an edit is a fetch failure on the next run. Provenance: TheRobotStudio/SO-ARM100 at commit -fda892cba81032c46c40976a48c9ceadbf40a9ca, Apache-2.0. The 13 meshes land FLAT -beside this file, not under assets/: MuJoCo drops an included file's own -`meshdir`, so upstream's `meshdir="assets"` is inert here and the paths resolve -against this directory. +fda892cba81032c46c40976a48c9ceadbf40a9ca, Apache-2.0. The 13 meshes land FLAT beside this +file: MuJoCo drops an included file's own `meshdir`, so upstream's +`meshdir="assets"` is inert here and the paths resolve against this directory. -follower.py repoints EVERY follower geom's `geom_matid` at this material at +preview_arm.py repoints EVERY arm geom's `geom_matid` at this material at startup -- collision geoms included, so the rule has no exception to remember -- replacing upstream's thirteen. One material is what lets the whole arm change colour in one write, the same way the leader ghost's does. The authored colour is the BLOCKED one, which is also where the arm starts; -follower.py owns the engageable colour and restores this one from mat_rgba, +preview_arm.py owns the engageable colour and restores this one from mat_rgba, alpha included -- it writes rgba[:3] only, so translucency survives both states. Translucent, unlike the leader ghost: this arm is a preview the operator looks PAST, so it must not hide what it is being aimed at. The cost is the draw-order -and blending risk assets/leader/leader_gripper.xml avoids by staying opaque, over +and blending risk leader_gripper.xml avoids by staying opaque, over ~30 overlapping geoms, and only a headset shows it. --> diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml b/src/python/isaacteleop/viz/robot/assets/leader_gripper.xml similarity index 94% rename from examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml rename to src/python/isaacteleop/viz/robot/assets/leader_gripper.xml index 0f0de1cb2c..1245faac3a 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml +++ b/src/python/isaacteleop/viz/robot/assets/leader_gripper.xml @@ -4,7 +4,7 @@ SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All SPDX-License-Identifier: Apache-2.0 An SO-101 leader gripper locked to the operator's controller grip pose, and the -whole of the shipped scene. It shows app.py's grip calibration -- whether the +whole of the shipped scene. It shows so101_ghost.py's grip calibration -- whether the tool sits in the hand the way a hand holds one -- but not cpp/frames.hpp, whose constants place the eye pose through the same transform and so cancel. @@ -15,7 +15,7 @@ README.md#scene-assets. Provenance: all four meshes are TheRobotStudio/SO-ARM100 at commit fda892cba81032c46c40976a48c9ceadbf40a9ca, Apache-2.0. Three are leader-specific print parts; the fourth is the STS3215 servo, shared with the follower. Fetched -by scripts/fetch-so-arm.sh, not vendored; only this wrapper is tracked. +by assets.py into a cache directory, not vendored; only this wrapper is tracked. The STLs are not in print orientation -- they sit on a shared CAD datum, which is why the transforms below are derived rather than tuned: @@ -87,7 +87,7 @@ inside a 65 m solid, which reads as "passthrough broke". compiles and honours a written qpos, but mj_step integrates gravity into it (measured: 0.06 rad over 50 steps). - `pos`/`quat` match leader_ghost above -- app.py rotates this body about + `pos`/`quat` match leader_ghost above -- so101_ghost.py rotates this body about a point rather than placing its origin there, so the hinge lives in exactly one place. --> + This is the joint's zero pose; so101_ghost.py swings it from there. --> diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml b/src/python/isaacteleop/viz/robot/assets/scene.xml similarity index 70% rename from examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml rename to src/python/isaacteleop/viz/robot/assets/scene.xml index 9750e1bd79..252223bb6e 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml +++ b/src/python/isaacteleop/viz/robot/assets/scene.xml @@ -9,11 +9,14 @@ background. cpp/frames.hpp's constants place the ghost and the eye pose alike, so they cancel and it lands on the hand whatever they are. The follower's base is static -content and does not get that for free: follower.py authors its placement in the -XR reference frame and pushes it through mj_from_xr at startup, so the constants -cancel there too and kTransMjFromXr stays untuned. +content and does not get that for free: preview_arm.py authors its placement in +the XR reference frame and pushes it through mj_from_xr at startup, so the +constants cancel there too and kTransMjFromXr stays untuned. + +assets.py copies this file and its two fragments into the fetch cache and drops +every mesh FLAT beside them, so all three resolve against one directory. --> - + + + + + + + + + + + + + diff --git a/tests/python/viz/robot_twin/pyproject.toml b/tests/python/viz/robot_twin/pyproject.toml new file mode 100644 index 0000000000..3436b14fbc --- /dev/null +++ b/tests/python/viz/robot_twin/pyproject.toml @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Exists so `uv run` resolves here rather than walking up to the repository root project. +# Precedent: the viz leaf above. + +[project] +name = "isaacteleop-viz-robot-twin-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = [ + "pytest", + "numpy", + # Deliberately NOT the version deps/third_party builds: the test's job is to + # show a foreign mujoco can share the process, and it tells the copies apart by + # version. Bump it only to another version that is not that one. + "mujoco==3.10.0", +] diff --git a/tests/python/viz/robot_twin/test_frames.py b/tests/python/viz/robot_twin/test_frames.py new file mode 100644 index 0000000000..963876ef2e --- /dev/null +++ b/tests/python/viz/robot_twin/test_frames.py @@ -0,0 +1,99 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The XR -> scene-world crossing: the handedness map and the quaternion order. + +Both are cheap to get wrong and expensive to debug on hardware. +""" + +import math + +import numpy as np +import pytest + +from isaacteleop.viz.robot import frames + + +def test_the_backend_is_not_the_wheels(): + """The whole packaging claim, in one assertion: two MuJoCos, one process. + + ``--extra dev`` installs a version that is deliberately not the one + ``deps/third_party`` builds, so a shared copy would show up as equality. + """ + import mujoco + + from isaacteleop.viz.robot import scene + + assert scene._robot_twin.mujoco_version() != mujoco.mj_versionString() + + +def test_axis_map_is_rep103(): + """XR -Z -> MJ +x, +Y -> +z, +X -> -y, on the rotation alone. + + The workspace translation is subtracted, so re-measuring it cannot fail this. + """ + t = np.asarray(frames.TRANS_MJ_FROM_XR) + + forward = np.asarray(frames.mj_from_xr_pos([0.0, 0.0, -1.0])) - t + up = np.asarray(frames.mj_from_xr_pos([0.0, 1.0, 0.0])) - t + right = np.asarray(frames.mj_from_xr_pos([1.0, 0.0, 0.0])) - t + + np.testing.assert_allclose(forward, [1.0, 0.0, 0.0], atol=1e-12) + np.testing.assert_allclose(up, [0.0, 0.0, 1.0], atol=1e-12) + np.testing.assert_allclose(right, [0.0, -1.0, 0.0], atol=1e-12) + + +@pytest.mark.parametrize("eye_height", [0.0, 1.2, 1.6]) +def test_point_one_metre_in_front_at_eye_height(eye_height): + """frames.hpp's definition, executable: a point 1 m in front of the operator + at eye height h lands at MuJoCo (+1, 0, h), before the workspace translation. + """ + t = np.asarray(frames.TRANS_MJ_FROM_XR) + p_mj = np.asarray(frames.mj_from_xr_pos([0.0, eye_height, -1.0])) - t + np.testing.assert_allclose(p_mj, [1.0, 0.0, eye_height], atol=1e-12) + + +def test_translation_has_both_terms(): + """Neither term may be silently zeroed: x is operator standoff and z the + floor datum, and they are independent.""" + t = frames.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 = frames.mj_from_xr_quat(q_xyzw_identity) + np.testing.assert_allclose(q_wxyz, frames.QUAT_MJ_FROM_XR, atol=1e-12) + + +def test_quaternion_input_order_is_xyzw_not_wxyz(): + """A 90-degree roll about XR +Z, spelled xyzw. + + NINETY degrees, not 180: a 180-degree roll is (0, 0, 1, 0), which read as + wxyz is a roll about XR +Y, and BOTH send local +x to MuJoCo +y -- a probe + that passes whichever way the binding reads its input. The second half pins + that this probe does discriminate. + """ + import mujoco + + s = math.sin(math.radians(45.0)) + q_xyzw = [0.0, 0.0, s, s] # (x, y, z, w) = 90 deg about z_xr + q_wxyz = np.asarray(frames.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( + frames.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/tests/python/viz/robot_twin/test_projection.py b/tests/python/viz/robot_twin/test_projection.py new file mode 100644 index 0000000000..b0c82f907a --- /dev/null +++ b/tests/python/viz/robot_twin/test_projection.py @@ -0,0 +1,81 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""What the app hands mjvGLCamera, and what comes back in the depth buffer. + +mjr_render builds its own projection from the frustum fields, so the twin's share +of the clip convention is those six numbers plus the depth inversion on readback. +Neither needs a GPU, a headset or a VizSession. +""" + +import math + +import pytest + +from isaacteleop.viz.robot import frames + +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)] + +CENTER, HALF_WIDTH, BOTTOM, TOP, F_NEAR, F_FAR = range(6) + + +def test_the_frustum_is_the_fov_projected_onto_the_near_plane(): + """Also pins that there is no y flip here: angle_up lands on TOP, and the + flip happens once, on readback.""" + f = frames.frustum_from_fov(FOV, NEAR, FAR) + assert f[CENTER] - f[HALF_WIDTH] == pytest.approx(NEAR * math.tan(FOV[0])) + assert f[CENTER] + f[HALF_WIDTH] == pytest.approx(NEAR * math.tan(FOV[1])) + assert f[TOP] == pytest.approx(NEAR * math.tan(FOV[2])) + assert f[BOTTOM] == pytest.approx(NEAR * math.tan(FOV[3])) + assert (f[F_NEAR], f[F_FAR]) == pytest.approx((NEAR, FAR), rel=1e-6) + + +def test_half_width_is_set_and_not_left_to_the_aspect_fallback(): + """The load-bearing assertion. + + At zero, render_gl3.c's setView derives the horizontal extent from the + viewport aspect instead, and the drift shows on a headset as world-locked + geometry sliding sideways under head motion. + """ + f = frames.frustum_from_fov(FOV, NEAR, FAR) + assert f[HALF_WIDTH] > 0.0 + + aspect_derived = 0.5 * (f[TOP] - f[BOTTOM]) + assert f[HALF_WIDTH] != pytest.approx(aspect_derived), ( + "this fov happens to be square, so the test cannot tell the fallback apart" + ) + + +def test_a_default_constructed_fov_is_rejected_loudly(): + """A default-constructed viz::Fov is four ZEROS, and must never render. + + Zero half_width turns the aspect fallback on, so the frame comes back + looking plausible from a fov carrying nothing. The runtime fills + ``FrameInfo.views``, so the app can only refuse, not prevent. + """ + with pytest.raises(ValueError): + frames.frustum_from_fov([0.0, 0.0, 0.0, 0.0], NEAR, FAR) + + +def test_near_far_are_validated(): + with pytest.raises(ValueError): + frames.frustum_from_fov(FOV, 0.0, FAR) + with pytest.raises(ValueError): + frames.frustum_from_fov(FOV, FAR, NEAR) + + +def test_submitted_depth_is_standard_z_not_the_reverse_z_mujoco_writes(): + """near -> 0, far -> 1, monotonic between. + + mjr_render writes the opposite and gl_readback.cpp's shader subtracts it + from 1; this is the specification that subtraction implements. + """ + assert frames.submitted_depth(NEAR, NEAR, FAR) == pytest.approx(0.0, abs=1e-6) + assert frames.submitted_depth(FAR, NEAR, FAR) == pytest.approx(1.0, abs=1e-6) + + depths = [frames.submitted_depth(d, NEAR, FAR) for d in (NEAR, 0.5, 5.0, FAR)] + assert depths == sorted(depths) diff --git a/tests/python/viz/robot_twin/test_readback.py b/tests/python/viz/robot_twin/test_readback.py new file mode 100644 index 0000000000..0d6690053f --- /dev/null +++ b/tests/python/viz/robot_twin/test_readback.py @@ -0,0 +1,187 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The OpenGL -> CUDA readback, on a real GPU and with no headset. + +The only test here that touches hardware, and it skips loudly without it. What it buys +is the two conversions that are otherwise invisible until someone is wearing a headset: +the y flip and the depth inversion. It stops at ProjectionLayer.submit and covers +nothing downstream. + +The scene is one box on a mocap body, built inline: what is measured is where drawn +pixels land, and a single geom the test can park anywhere is the whole requirement. +""" + +import ctypes +import math + +import numpy as np +import pytest + +scene_module = pytest.importorskip( + "isaacteleop.viz.robot.scene", reason="the robot twin backend is not built" +) +frames = pytest.importorskip("isaacteleop.viz.robot.frames") + +SceneTwin = scene_module.SceneTwin + +NEAR_Z = 0.05 +FAR_Z = 50.0 +BOX = "box" +#: Half-extent of the cube below, in metres. +BOX_HALF_SIZE = 0.02 +# A 4 cm cube on a mocap body, so the fixture can park it anywhere by publishing. +SCENE = """ + + + + + + + +""" + +W = H = 256 +HALF_FOV = math.radians(45.0) +BOX_DISTANCE = 0.6 # metres straight ahead of the eye +BOX_OFFSET = 0.15 # metres off-axis, comfortably outside the box's own size + + +@pytest.fixture(scope="module") +def rendered(tmp_path_factory): + """A live SceneTwin plus a device-to-host copier, or a skip saying why. + + Drives the shipped twin rather than a Renderer of its own, so what is measured is + the path the app actually takes: publish, apply, forward, update_scene, render. + """ + path = tmp_path_factory.mktemp("scene") / "readback.xml" + path.write_text(SCENE) + twin = SceneTwin(path) + + try: + twin.create(W, H, 2, near_z=NEAR_Z, far_z=FAR_Z) + except Exception as exc: # noqa: BLE001 -- no GPU, no GL, or the wrong device + twin.destroy() + # Includes the multi-GPU case, where the EGL device and the process's + # CUDA device differ and the fix is GlContext(device_index=...). + pytest.skip(f"renderer unavailable: {exc}") + cuda = ctypes.CDLL("libcuda.so.1") + + def read(view, is_depth): + img = twin.depth(view) if is_depth else twin.color(view) + shape = (H, W) if is_depth else (H, W, 4) + out = np.empty(shape, dtype=np.float32 if is_depth else np.uint8) + rc = cuda.cuMemcpyDtoH_v2( + out.ctypes.data_as(ctypes.c_void_p), + ctypes.c_uint64(img.__cuda_array_interface__["data"][0]), + ctypes.c_size_t(out.nbytes), + ) + assert rc == 0, f"cuMemcpyDtoH_v2 -> {rc}" + return out + + def render(xr_offset, eye_separation=0.0): + """Park the box at `xr_offset` from the eye and draw both views.""" + twin.publish( + bodies={BOX: (frames.mj_from_xr_pos(xr_offset), (1.0, 0.0, 0.0, 0.0))} + ) + poses, fovs = [], [] + for sign in (-1.0, 1.0): + poses += [sign * eye_separation, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] + fovs += [-HALF_FOV, HALF_FOV, HALF_FOV, -HALF_FOV] + twin.render(poses, fovs) + + yield render, read + twin.destroy() + + +def _drawn_centre(color): + """(row, col) centre of the drawn pixels; alpha 0 is 'show passthrough'.""" + drawn = color[..., 3] > 0 + assert drawn.any(), "nothing was drawn" + return np.flatnonzero(drawn.any(axis=1)).mean(), np.flatnonzero( + drawn.any(axis=0) + ).mean() + + +def test_something_is_drawn_at_all(rendered): + render, read = rendered + render([0.0, 0.0, -BOX_DISTANCE]) + color = read(0, is_depth=False) + assert (color[..., 3] > 0).any(), ( + "the whole frame is transparent -- mjr_render drew into another framebuffer, " + "or the blit missed" + ) + assert set(np.unique(color[..., 3])) <= {0, 255}, ( + "alpha must be 0 (passthrough) or 255 (opaque); a partial alpha means blending " + "leaked into the readback pass" + ) + + +def test_row_zero_is_the_top_of_the_operators_view(rendered): + """The y flip: OpenGL renders bottom-up, XR swapchains are top-down, and + nothing short of a headset would show the whole scene upside down.""" + render, read = rendered + render([0.0, BOX_OFFSET, -BOX_DISTANCE]) + above, _ = _drawn_centre(read(0, is_depth=False)) + render([0.0, -BOX_OFFSET, -BOX_DISTANCE]) + below, _ = _drawn_centre(read(0, is_depth=False)) + + assert above < H / 2 < below, ( + f"XR +Y landed at row {above:.0f} and -Y at row {below:.0f}: the image is upside down" + ) + + +def test_the_image_is_not_mirrored(rendered): + """Horizontal, checked alongside the flip: mirroring one axis and not the + other is what a mistaken second flip looks like.""" + render, read = rendered + render([BOX_OFFSET, 0.0, -BOX_DISTANCE]) + _, right = _drawn_centre(read(0, is_depth=False)) + render([-BOX_OFFSET, 0.0, -BOX_DISTANCE]) + _, left = _drawn_centre(read(0, is_depth=False)) + + assert left < W / 2 < right, ( + f"XR +X landed at column {right:.0f} and -X at {left:.0f}: the image is mirrored" + ) + + +def test_depth_is_the_standard_z_projection_layer_is_promised(rendered): + """near -> 0, far -> 1, and the background is far. + + Getting it backwards leaves colour perfect and reprojection inverted, so the + values are checked against the geometry, not merely for being in range. + """ + render, read = rendered + render([0.0, 0.0, -BOX_DISTANCE]) + depth = read(0, is_depth=True) + color = read(0, is_depth=False) + drawn = color[..., 3] > 0 + + background = np.unique(depth[~drawn]) + assert background == pytest.approx([1.0]), ( + f"background depth {background}, expected exactly 1.0 (far). MuJoCo clears its " + "reverse-Z buffer to 0, so anything else means the inversion is missing." + ) + + # The box's front face is flat and square to the eye, so every drawn pixel carries + # one depth -- the FACE's, half a box nearer than the body's origin. Pinning that + # exact value is what an in-range check would not do. + expected = frames.submitted_depth(BOX_DISTANCE - BOX_HALF_SIZE, NEAR_Z, FAR_Z) + assert depth[drawn] == pytest.approx(expected, abs=1e-4), ( + f"drawn depth spans [{depth[drawn].min():.4f}, {depth[drawn].max():.4f}], " + f"expected {expected:.4f} for a face at {BOX_DISTANCE - BOX_HALF_SIZE} m" + ) + + +def test_the_eyes_see_the_object_at_different_offsets(rendered): + """Stereo parallax and its sign: an object ahead sits further left in the + RIGHT eye, and swapping the views reads as eye strain rather than as a bug.""" + render, read = rendered + render([0.0, 0.0, -BOX_DISTANCE], eye_separation=0.032) + _, left_eye = _drawn_centre(read(0, is_depth=False)) + _, right_eye = _drawn_centre(read(1, is_depth=False)) + + assert right_eye < left_eye, ( + f"left eye sees the object at column {left_eye:.0f} and the right eye at " + f"{right_eye:.0f}: the views are swapped" + ) diff --git a/tests/python/viz/robot_twin/test_scene_twin.py b/tests/python/viz/robot_twin/test_scene_twin.py new file mode 100644 index 0000000000..95219690b3 --- /dev/null +++ b/tests/python/viz/robot_twin/test_scene_twin.py @@ -0,0 +1,247 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""``SceneTwin``: the whole API an app is allowed to move the scene with. + +Every scene here is built inline, so this needs no fetched meshes and no GPU -- +``create()`` is the only method that touches one, and nothing below calls it. + +The twin's own ``_scene`` is read directly. That is a test-only liberty and the point of +the file: what is being checked is that a *name* published from one thread lands on the +right row of the scene the other thread draws. +""" + +import struct + +import numpy as np +import pytest + +scene_module = pytest.importorskip( + "isaacteleop.viz.robot.scene", reason="the robot twin backend is not built" +) + +SceneTwin = scene_module.SceneTwin + +_SCENE = """ + + + + + + + + + + + + + + + + + + + + + + + + +""" + + +@pytest.fixture +def twin(tmp_path): + path = tmp_path / "scene.xml" + path.write_text(_SCENE) + built = SceneTwin(path) + built.home([0.0, 0.0]) + return built + + +def _geom_groups(twin, name): + return twin._scene.geom_group[twin._groups[name]] + + +# ---------------------------------------------------------------- load time + + +def test_the_joint_map_is_the_scene_s_own(twin): + assert twin.joints.names == ("shoulder", "elbow") + + +def test_a_scene_that_does_not_compile_names_the_mujoco_it_was_tried_against(tmp_path): + """Upstream's parser error names neither the file's version nor ours.""" + path = tmp_path / "broken.xml" + path.write_text("") + with pytest.raises(RuntimeError, match="did not compile against MuJoCo"): + SceneTwin(path) + + +def test_body_offset_is_measured_in_the_parent_s_own_frame(twin): + """The base is authored turned 45 deg, so a world-frame answer would differ.""" + pos, quat = twin.body_offset("tip", relative_to="base") + np.testing.assert_allclose(pos, [0.5, 0.0, 0.0], atol=1e-12) + np.testing.assert_allclose(quat, [1.0, 0.0, 0.0, 0.0], atol=1e-12) + + +def test_site_offset_carries_the_site_s_orientation(twin): + pos, quat = twin.site_offset("tool", relative_to="base") + np.testing.assert_allclose(pos, [0.55, 0.0, 0.0], atol=1e-12) + np.testing.assert_allclose(quat, [1.0, 0.0, 0.0, 0.0], atol=1e-12) + + +def test_the_offsets_are_frozen_by_home(twin): + """Measured at the home posture, which is what makes them constants.""" + before = twin.body_offset("tip", relative_to="base")[0] + # The shoulder, not the elbow: the elbow sits AT `tip`, so turning it spins the + # body without moving its origin. + twin.home([np.radians(90.0), 0.0]) + after = twin.body_offset("tip", relative_to="base")[0] + assert not np.allclose(before, after), "home() did not repose the scene" + + +def test_drawn_only_leaves_the_collision_geoms_out(twin): + """Showing a subtree must not reveal geometry the scene authored hidden.""" + twin.declare_group("arm", body="base", drawn_only=True) + names = { + twin._scene.name(scene_module._robot_twin.ObjType.GEOM, int(g)) + for g in twin._groups["arm"] + } + assert names == {"base_geom", "link_geom", "tip_geom"} + + +def test_a_group_that_covers_nothing_is_refused(twin): + """An empty group is a group that silently never draws.""" + model = twin._scene + model.geom_group[twin._subtree_geoms(twin.body_id("base"))] = ( + scene_module.HIDDEN_GROUP + ) + with pytest.raises(RuntimeError, match="covers no geom"): + twin.declare_group("arm", body="base", drawn_only=True) + + +@pytest.mark.parametrize("kwargs", [{}, {"body": "base", "geoms": ("base_geom",)}]) +def test_declare_group_takes_exactly_one_selector(twin, kwargs): + with pytest.raises(ValueError, match="exactly one"): + twin.declare_group("arm", **kwargs) + + +def test_declare_material_returns_the_authored_colour(twin): + np.testing.assert_allclose( + twin.declare_material("paint"), [0.1, 0.2, 0.3, 1.0], atol=1e-7 + ) + + +def test_repaint_points_a_whole_group_at_one_material(twin): + twin.declare_group("arm", body="base") + twin.declare_material("paint") + twin.repaint("arm", "paint") + index = twin._scene.id(scene_module._robot_twin.ObjType.MATERIAL, "paint") + assert set(twin._scene.geom_matid[twin._groups["arm"]]) == {index} + + +# ---------------------------------------------------------------- publish + + +def test_nothing_published_reaches_the_scene_before_a_render(twin): + """The whole thread contract: publish records, the render thread applies.""" + twin.publish(joints=[1.0, 2.0]) + np.testing.assert_allclose(twin._scene.qpos, [0.0, 0.0]) + twin.settle() + np.testing.assert_allclose(twin._scene.qpos, [1.0, 2.0]) + + +def test_a_publish_is_merged_not_replaced(twin): + """A caller may send only what moved; what it left out must survive.""" + twin.declare_group("arm", body="base") + twin.publish(joints=[1.0, 2.0]) + twin.publish(groups={"arm": False}) + twin.settle() + np.testing.assert_allclose(twin._scene.qpos, [1.0, 2.0]) + assert set(_geom_groups(twin, "arm")) == {scene_module.HIDDEN_GROUP} + + +def test_the_latest_publish_wins(twin): + """Latest-wins, not queued: a fast control loop must not build a backlog.""" + twin.publish(joints=[1.0, 2.0]) + twin.publish(joints=[3.0, 4.0]) + twin.settle() + np.testing.assert_allclose(twin._scene.qpos, [3.0, 4.0]) + + +def test_a_mocap_body_moves_through_mocap_and_a_fixed_one_through_body_pos(twin): + """The caller names a body; it does not choose the mechanism.""" + twin.publish( + bodies={ + "floater": ((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0)), + "base": ((-1.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0)), + } + ) + twin.settle() + model, data = twin._scene, twin._scene + floater = twin.body_id("floater") + np.testing.assert_allclose( + data.mocap_pos[int(model.body_mocapid[floater])], [1.0, 2.0, 3.0] + ) + np.testing.assert_allclose(model.body_pos[twin.body_id("base")], [-1.0, 0.0, 0.0]) + # And the pose is not merely stored: forward kinematics ran on it. + np.testing.assert_allclose(data.xpos[floater], [1.0, 2.0, 3.0], atol=1e-12) + + +def test_publish_copies_so_the_caller_may_reuse_its_buffer(twin): + joints = np.array([1.0, 2.0]) + twin.publish(joints=joints) + joints[:] = 9.0 + twin.settle() + np.testing.assert_allclose(twin._scene.qpos, [1.0, 2.0]) + + +def test_visibility_and_colour_reach_the_scene(twin): + twin.declare_group("arm", body="base") + twin.declare_material("paint") + twin.publish(groups={"arm": True}, materials={"paint": (1.0, 0.0, 0.0, 0.5)}) + twin.settle() + assert set(_geom_groups(twin, "arm")) == {scene_module.DRAWN_GROUP} + np.testing.assert_allclose( + twin._scene.mat_rgba[ + twin._scene.id(scene_module._robot_twin.ObjType.MATERIAL, "paint") + ], + [1.0, 0.0, 0.0, 0.5], + atol=1e-7, + ) + + +def test_a_wrong_width_snapshot_is_refused_at_publish_time(twin): + twin.publish(joints=[1.0, 2.0, 3.0]) + with pytest.raises(ValueError, match="expected 2 joint values"): + twin.settle() + + +def test_a_mesh_scene_loads(tmp_path): + """MuJoCo's mesh decoders register through ``__attribute__((constructor))``. + + Nothing references the translation units they live in, so any packaging that drops + unreferenced objects loses them, and every mesh scene then fails with "no decoder + found" -- at load, not at link. This is what notices. + """ + # A tetrahedron as binary STL: 80-byte header, uint32 facet count, then 50 bytes + # per facet. Four vertices is MuJoCo's minimum for a mesh. + corners = [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)] + faces = [(0, 2, 1), (0, 1, 3), (0, 3, 2), (1, 2, 3)] + stl = tmp_path / "tet.stl" + body = b"".join( + struct.pack("<12fH", 0, 0, 0, *corners[a], *corners[b], *corners[c], 0) + for a, b, c in faces + ) + stl.write_bytes(b"\0" * 80 + struct.pack(" + + + + """) + twin = SceneTwin(path) + assert twin._scene.ngeom == 1 diff --git a/tests/python/viz/robot_twin/test_symbol_isolation.py b/tests/python/viz/robot_twin/test_symbol_isolation.py new file mode 100644 index 0000000000..bb48c08e3f --- /dev/null +++ b/tests/python/viz/robot_twin/test_symbol_isolation.py @@ -0,0 +1,250 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The robot twin's MuJoCo must be invisible to the process it runs in. + +Everything here is about one failure: a user's ``mujoco`` wheel and ours resolving to each +other. It is silent when it happens -- no import error, no version warning, just the wrong +library executing -- so these are the assertions that have to catch it. + +Every one of them runs against the module that ships. A test-only extension carrying a +second copy of the recipe would pass while the shipped one regressed, and its share of +these assertions would be weaker: ``--version-script`` has nothing to hide in a module +that links no static archive, and the shipped one links ``cudart_static``. +""" + +import ctypes +import os +import pathlib +import subprocess +import sys + +import pytest + +TWIN_DIR = os.environ.get("ISAACTELEOP_ROBOT_TWIN_DIR") +INTERNAL_VERSION = os.environ.get("ISAACTELEOP_MUJOCO_VERSION") +HERE = pathlib.Path(__file__).parent + +pytestmark = pytest.mark.skipif( + not TWIN_DIR or not INTERNAL_VERSION, + reason="run through ctest, which sets ISAACTELEOP_ROBOT_TWIN_DIR and ISAACTELEOP_MUJOCO_VERSION", +) + + +@pytest.fixture(scope="module") +def twin_dir(): + return pathlib.Path(TWIN_DIR) + + +@pytest.fixture(scope="module") +def twin_so(twin_dir): + matches = sorted(twin_dir.glob("_robot_twin*.so")) + assert len(matches) == 1, f"expected one twin extension, found {matches}" + return matches[0] + + +@pytest.fixture(scope="module") +def private_mujoco(twin_dir): + """The copy the twin dlopens: staged beside it by deps/third_party/Mujoco.cmake.""" + return twin_dir / "libisaacteleop_mujoco.so" + + +@pytest.fixture(scope="module") +def twin(twin_dir): + """The shipped extension, imported the way the wheel exposes it.""" + sys.path.insert(0, str(twin_dir)) + import _robot_twin + + return _robot_twin + + +def _dynamic_symbols(so): + out = subprocess.run( + ["readelf", "--dyn-syms", "-W", str(so)], + capture_output=True, + text=True, + check=True, + ).stdout + names = [] + for line in out.splitlines(): + fields = line.split() + if len(fields) >= 8 and fields[4] in ("GLOBAL", "WEAK") and fields[6] != "UND": + names.append(fields[7].split("@")[0]) + return names + + +# ---------------------------------------------------------------- what the linker did + + +def test_the_extension_exports_only_its_entry_point(twin_so): + """The one assertion that stops a silent interposition regressing back in. + + MuJoCo compiles its ~700 ``mj*`` at default visibility, so anything that put them in + this module's dynamic table would offer them to the whole process. Measured: the + version script takes this from 11 exports to 1. + """ + assert _dynamic_symbols(twin_so) == ["PyInit__robot_twin"] + + +def test_nothing_links_libmujoco(twin_so): + """The load-bearing one: an undefined mj* is what a foreign libmujoco answers. + + MuJoCo is reached through dlopen/dlsym instead, so there is no NEEDED entry and no + symbol for the global scope to resolve. + """ + out = subprocess.run( + ["readelf", "-d", "-W", str(twin_so)], + capture_output=True, + text=True, + check=True, + ).stdout + needed = [line for line in out.splitlines() if "(NEEDED)" in line] + assert not [line for line in needed if "mujoco" in line], needed + # Nor on EGL or GL: both are dlopened, so a machine without them still imports. + assert not [line for line in needed if "EGL" in line or "libGL" in line], needed + + +def test_the_private_mujoco_ships_beside_the_extension(private_mujoco): + """mj_api.cpp opens it by the module's own directory, so the wheel must stage it there. + + A plain file, not a symlink to a versioned one, because wheels do not carry symlinks. + """ + assert private_mujoco.is_file() and not private_mujoco.is_symlink() + + +def test_the_private_mujoco_ships_under_its_own_name(private_mujoco): + """A SONAME of libmujoco.so.3.x is what the wheel's own extensions resolve. + + They carry DT_NEEDED on it, and the loader satisfies that from whatever is already + loaded under that SONAME -- so an unrenamed copy of ours, loaded first, would answer + the user's ``import mujoco`` and be the only libmujoco the process maps. + """ + out = subprocess.run( + ["readelf", "-d", "-W", str(private_mujoco)], + capture_output=True, + text=True, + check=True, + ).stdout + soname = [line for line in out.splitlines() if "SONAME" in line] + assert len(soname) == 1, out + assert "libisaacteleop_mujoco.so" in soname[0], soname + + +def test_the_private_mujoco_binds_its_own_calls(private_mujoco): + """-Wl,-Bsymbolic, and nothing else here would catch its absence. + + Without it, one MuJoCo function calling another goes through the global scope, and a + user's libmujoco sitting there answers with a differently laid out mjData. Measured: + with the wheel loaded RTLD_GLOBAL, a build without this dies at import inside + MuJoCo's own resource registry -- but under the default RTLD_LOCAL it passes every + other test in this file. + """ + out = subprocess.run( + ["readelf", "-d", "-W", str(private_mujoco)], + capture_output=True, + text=True, + check=True, + ).stdout + assert "SYMBOLIC" in out, out + + +# ---------------------------------------------------------------- two copies, one process + + +def test_a_foreign_mujoco_shares_the_process(twin): + """Import order is the app's: the wheel first, as a user's own code would.""" + import mujoco + + assert mujoco.mj_versionString() != INTERNAL_VERSION, ( + "the dev extra pins the same version the twin builds, so this proves nothing" + ) + assert twin.mujoco_version() == INTERNAL_VERSION + + +def test_both_copies_still_work(twin): + import mujoco + + model = mujoco.MjModel.from_xml_path(str(HERE / "model.xml")) + mujoco.mj_forward(model, mujoco.MjData(model)) + assert twin.Scene(str(HERE / "model.xml")).ngeom > 0 + + +def test_rtld_global_does_not_interpose_us(twin_dir): + """The failure mode the hiding exists for, in the configuration that provokes it. + + Extensions load RTLD_LOCAL by default, so an unhidden build passes every test above + and only breaks once something else in the process flips the flag. + """ + script = ( + "import ctypes, sys;" + f"sys.path.insert(0, {str(twin_dir)!r});" + "sys.setdlopenflags(sys.getdlopenflags() | ctypes.RTLD_GLOBAL);" + "import mujoco, _robot_twin;" + "print(mujoco.mj_versionString(), _robot_twin.mujoco_version())" + ) + out = subprocess.run( + [sys.executable, "-c", script], capture_output=True, text=True, check=True + ).stdout.split() + assert out[1] == INTERNAL_VERSION, f"interposed by the wheel's {out[0]}" + + +def test_mj_symbols_are_absent_from_the_global_namespace(twin): + """Nothing may satisfy an mj* lookup out of our extension.""" + with pytest.raises(AttributeError): + ctypes.CDLL(None).mj_versionString + + +# ---------------------------------------------------------------- the error hooks + + +def _our_mujoco(twin_dir): + """A handle on the twin's own copy. + + The twin already dlopened this exact file, and glibc dedupes a dlopen by inode, so + this is that mapping rather than a third one. + """ + return ctypes.CDLL( + str(twin_dir / "libisaacteleop_mujoco.so"), mode=ctypes.RTLD_LOCAL + ) + + +def test_the_wheels_error_hook_does_not_bind_ours(twin, twin_dir): + """``mju_user_error`` is a plain global per libmujoco copy, not shared state. + + Ours carries mj_guard.cpp's handler because the module initialiser installed it; + the wheel's is whatever the wheel's own user set, and setting one cannot reach the + other. + """ + import mujoco + + mujoco.set_mju_user_warning(lambda _msg: None) + wheel_lib = ( + pathlib.Path(mujoco.__file__).parent + / f"libmujoco.so.{mujoco.mj_versionString()}" + ) + ours = ctypes.c_void_p.in_dll(_our_mujoco(twin_dir), "mju_user_error").value + theirs = ctypes.c_void_p.in_dll( + ctypes.CDLL(str(wheel_lib), mode=ctypes.RTLD_LOCAL), "mju_user_error" + ).value + assert ours, "install_mujoco_handlers() did not reach the twin's own copy" + assert ours != theirs + + +def test_an_unguarded_mujoco_error_aborts_rather_than_exits(twin_dir): + """MuJoCo's default handler ends in exit(EXIT_FAILURE), taking the host with it. + + mj_guard.cpp replaces it. Outside a guarded call there is nowhere to land, so it says + so and aborts -- which is what distinguishes it from the default, and from a handler + that RETURNS and lets MuJoCo resume on state it has already declared invalid. + """ + script = ( + "import ctypes, sys;" + f"sys.path.insert(0, {str(twin_dir)!r});" + "import _robot_twin;" + f"ctypes.CDLL({str(twin_dir / 'libisaacteleop_mujoco.so')!r}).mju_error(b'deliberate')" + ) + done = subprocess.run( + [sys.executable, "-c", script], capture_output=True, text=True + ) + assert "robot_twin: unguarded MuJoCo error: deliberate" in done.stderr, done.stderr + assert done.returncode != 1, "exit(1) is MuJoCo's default handler, not ours" diff --git a/tests/python/viz/test_engage_gate.py b/tests/python/viz/test_engage_gate.py new file mode 100644 index 0000000000..49ecaa6b28 --- /dev/null +++ b/tests/python/viz/test_engage_gate.py @@ -0,0 +1,304 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free unit tests for :class:`~isaacteleop.viz.robot.EngageGate`. + +Drives it through ``update`` at a synthetic cadence, so the hysteresis band and the +dwell are exercised at the frame level rather than through their internals. +""" + +import math + +import numpy as np +import pytest + +from isaacteleop.retargeters.SO101.clutch_retargeter import SO101ClutchRetargeter +from isaacteleop.viz.robot.engage_gate import ( + KEY_ENGAGED, + KEY_ROTATION, + KEY_SETTLING, + KEY_UNJUDGED, + KEY_UNREFERENCED, + KEY_UNTRACKED, + EngageGate, + EngageGateConfig, +) + +_ID_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) +_FRAME_S = 0.01 # 100 Hz, so one dwell is 10 frames at the default 0.1 s + +# Long enough to clear any dwell in one frame; the dt clamp caps the credit at max_dt. +_LONG_FRAME_S = 1.0 + + +def _quat_about_y(deg: float) -> np.ndarray: + """A rotation of ``deg`` about +Y as an ``[x, y, z, w]`` quaternion.""" + half = math.radians(deg) / 2.0 + return np.array([0.0, math.sin(half), 0.0, math.cos(half)], dtype=np.float32) + + +def _controller(*, orientation=_ID_QUAT, valid: bool = True): + """The controller orientation the caller feeds in, or ``None`` where it has none. + + An absent sample and one the runtime flagged invalid both arrive as ``None``: the + caller collapses them before the gate sees either. + """ + return np.asarray(orientation, dtype=np.float32) if valid else None + + +def _transform(rotation_3x3=None) -> np.ndarray: + """A reference pose; only its rotation block is read.""" + matrix = np.eye(4, dtype=np.float32) + if rotation_3x3 is not None: + matrix[:3, :3] = np.asarray(rotation_3x3, dtype=np.float32) + return matrix + + +class _Driver: + """Steps a gate at a fixed cadence, so a test says what changed and not how.""" + + def __init__(self, gate: EngageGate) -> None: + self._gate = gate + + def step( + self, + *, + controller=None, + reference=None, + engaged=None, + app_permitted=None, + frame_s: float = _FRAME_S, + ) -> bool: + """One frame. ``None`` leaves an operand absent; returns the emitted permission.""" + self._gate.update( + controller, + reference, + engaged=bool(engaged), + # None is the unwired app conjunct, which fails open. + app_ok=True if app_permitted is None else bool(app_permitted), + dt=frame_s, + ) + return self._gate.permitted + + def settle(self, **kwargs) -> bool: + """Step until the dwell is spent, so the next verdict is the steady-state one.""" + permitted = False + for _ in range(20): + permitted = self.step(**kwargs) + return permitted + + @property + def verdict(self): + return self._gate.verdict + + +@pytest.fixture +def driver(): + return _Driver(EngageGate()) + + +# ---------------------------------------------------------------- conjuncts + + +def test_aligned_and_settled_permits(driver): + assert driver.settle(controller=_controller(), reference=_transform()) is True + assert driver.verdict.ok + + +def test_dwell_holds_the_gate_shut_until_it_is_spent(): + """Everything passes from frame one, so only the dwell can be keeping it closed.""" + driver = _Driver(EngageGate(config=EngageGateConfig(dwell_s=0.05))) + kwargs = {"controller": _controller(), "reference": _transform()} + # 100 Hz against a 0.05 s dwell: four frames of credit are not enough, five are. + for _ in range(4): + assert driver.step(**kwargs) is False + assert driver.verdict.keys == (KEY_SETTLING,) + assert driver.step(**kwargs) is True + + +def test_an_absent_reference_blocks_rather_than_permits(driver): + """Fails closed: nothing to align against is not the same as aligned.""" + assert driver.settle(controller=_controller(), reference=None) is False + assert KEY_UNREFERENCED in driver.verdict.keys + + +def test_an_untracked_controller_blocks(driver): + """Absent and flagged-invalid arrive identically -- the caller collapses both to None.""" + assert driver.settle(controller=None, reference=_transform()) is False + assert KEY_UNTRACKED in driver.verdict.keys + + +def test_a_degenerate_quaternion_reads_as_untracked(driver): + """Valid per the runtime's flag and still carrying no orientation.""" + zero = np.zeros(4, dtype=np.float32) + assert ( + driver.settle(controller=_controller(orientation=zero), reference=_transform()) + is False + ) + assert KEY_UNTRACKED in driver.verdict.keys + + +def test_an_untracked_frame_reports_no_rotation_conjunct(driver): + """One failure, not two: the angle would be derived from the missing operand.""" + driver.step(controller=_controller(valid=False), reference=_transform()) + assert KEY_ROTATION not in driver.verdict.keys + + +def test_a_sheared_reference_reads_as_unreferenced(driver): + """A non-orthonormal block yields a plausible wrong angle, so it is refused.""" + sheared = np.eye(3) + sheared[0, 1] = 0.5 + assert ( + driver.settle(controller=_controller(), reference=_transform(sheared)) is False + ) + assert KEY_UNREFERENCED in driver.verdict.keys + + +def test_every_failing_conjunct_is_reported(driver): + """Half the truth twice is the failure mode this exists to avoid.""" + driver.step( + controller=_controller(valid=False), + reference=None, + engaged=True, + app_permitted=False, + ) + assert set(driver.verdict.keys) == { + KEY_ENGAGED, + KEY_UNREFERENCED, + KEY_UNTRACKED, + "app", + } + + +def test_the_app_conjunct_is_named_by_the_owner(): + driver = _Driver(EngageGate(app_conjunct=("limiter", "still catching up"))) + driver.step(controller=_controller(), reference=_transform(), app_permitted=False) + assert driver.verdict.keys == ("limiter",) + assert driver.verdict.blocked == ("still catching up",) + + +def test_an_unwired_app_conjunct_fails_open(driver): + assert driver.settle(controller=_controller(), reference=_transform()) is True + + +# ---------------------------------------------------------------- hysteresis + + +@pytest.mark.parametrize("deg", [0.0, 19.0, -19.0]) +def test_inside_the_enter_band_the_gate_opens(driver, deg): + assert ( + driver.settle( + controller=_controller(orientation=_quat_about_y(deg)), + reference=_transform(), + ) + is True + ) + + +@pytest.mark.parametrize("deg", [21.0, 90.0, 180.0]) +def test_outside_the_enter_band_the_gate_stays_shut(driver, deg): + assert ( + driver.settle( + controller=_controller(orientation=_quat_about_y(deg)), + reference=_transform(), + ) + is False + ) + assert KEY_ROTATION in driver.verdict.keys + + +def test_an_open_gate_holds_out_to_the_exit_band(driver): + """Enter at 20 deg, leave at 30: 25 deg keeps an open gate open and a shut one shut.""" + reference = _transform() + assert driver.settle(controller=_controller(), reference=reference) is True + assert ( + driver.step( + controller=_controller(orientation=_quat_about_y(25.0)), reference=reference + ) + is True + ) + assert ( + driver.step( + controller=_controller(orientation=_quat_about_y(35.0)), reference=reference + ) + is False + ) + # Closed now, so the tighter band applies again and 25 deg no longer qualifies. + assert ( + driver.settle( + controller=_controller(orientation=_quat_about_y(25.0)), reference=reference + ) + is False + ) + + +def test_the_reported_angle_carries_the_measurement(driver): + driver.step( + controller=_controller(orientation=_quat_about_y(90.0)), reference=_transform() + ) + assert driver.verdict.blocked == ("rotation 90 deg > 20",) + + +# ---------------------------------------------------------------- engagement + + +def test_an_engaged_clutch_is_permitted_however_the_wrist_sits(driver): + """The disjunction the module docstring is about: a dropout recovery must not stall.""" + permitted = driver.settle( + controller=_controller(orientation=_quat_about_y(180.0)), + reference=_transform(), + engaged=True, + ) + assert permitted is True + assert KEY_ENGAGED in driver.verdict.keys + + +def test_a_release_cannot_re_latch_inside_the_dwell(driver): + """The post-release debounce: the engaged conjunct zeroes the dwell every frame.""" + kwargs = {"controller": _controller(), "reference": _transform()} + driver.settle(engaged=True, **kwargs) + assert driver.step(engaged=False, **kwargs) is False + assert driver.verdict.keys == (KEY_SETTLING,) + + +def test_a_stalled_clock_cannot_credit_the_whole_stall_to_the_dwell(): + """max_dt bounds one frame's credit, so a resumed graph still serves the dwell.""" + driver = _Driver(EngageGate(config=EngageGateConfig(dwell_s=0.5, max_dt=0.1))) + kwargs = {"controller": _controller(), "reference": _transform()} + assert driver.step(frame_s=_LONG_FRAME_S, **kwargs) is False + assert driver.verdict.keys == (KEY_SETTLING,) + + +# ---------------------------------------------------------------- wiring + + +def test_the_permission_leaf_fits_the_clutch_input(): + """Structural, not by name. The gate is not a node, so the leaf carrying its answer + into the graph is the app's, and nothing else checks the two still agree.""" + clutch_preview = pytest.importorskip("isaacteleop.viz.robot.clutch_preview") + + clutch = SO101ClutchRetargeter(name="ee_pose", home_base_T_ee=np.eye(4)) + clutch.input_spec()[ + SO101ClutchRetargeter.ENGAGE_PERMITTED_INPUT + ].check_compatibility(clutch_preview.PERMITTED_TYPE) + + +@pytest.mark.parametrize( + "config", + [ + {"enter_rad": 0.0}, + {"enter_rad": math.radians(40.0)}, # wider than the exit band + {"dwell_s": -1.0}, + {"max_dt": 1e-6}, # below nominal_dt + ], +) +def test_a_nonsense_configuration_is_refused(config): + with pytest.raises(ValueError): + EngageGateConfig(**config) + + +def test_the_verdict_before_the_first_step_is_not_permission(): + """An empty verdict reads as `ok`, and a caller polling early would believe it.""" + gate = EngageGate() + assert not gate.verdict.ok + assert gate.verdict.keys == (KEY_UNJUDGED,) diff --git a/tests/python/viz/test_operator_frame.py b/tests/python/viz/test_operator_frame.py new file mode 100644 index 0000000000..ab1a4eaa5e --- /dev/null +++ b/tests/python/viz/test_operator_frame.py @@ -0,0 +1,93 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The XR-to-base yaw measurement, which no headless run of the twin exercises.""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest +from isaacteleop.viz.robot.operator_frame import OperatorFrame + +#: OpenXR (x right, y up, z back) onto REP-103 (x forward, y left, z up). +AXIS_MAP = np.array( + [ + [0.0, 0.0, -1.0, 0.0], + [-1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] +) +FORWARD_BASE = np.array([1.0, 0.0, 0.0]) + + +def bearing_xr(deg: float) -> np.ndarray: + """A horizontal XR direction at ``deg`` off forward, turning the way a yaw does.""" + t = math.radians(deg) + return np.array([-math.sin(t), 0.0, -math.cos(t)]) + + +def test_convention_until_measured(): + frame = OperatorFrame(AXIS_MAP) + assert frame.yaw_rad is None and not frame.measured + assert np.allclose(frame.transform, AXIS_MAP) + + +@pytest.mark.parametrize("deg", [0.0, 30.0, 90.0, 179.0, -45.0, -135.0]) +def test_recovers_the_yaw(deg): + """A direction seen at ``deg`` in XR, known to be base +X, means the frames differ by -deg.""" + frame = OperatorFrame(AXIS_MAP) + frame.update(bearing_xr(deg), FORWARD_BASE, engaged=False) + assert frame.measured + # Pushing along XR forward must land at -deg in the base frame. + landed = frame.transform[:3, :3] @ np.array([0.0, 0.0, -1.0]) + assert math.degrees(math.atan2(landed[1], landed[0])) == pytest.approx( + -deg, abs=1e-9 + ) + + +def test_translation_is_never_read(): + """The clutch is engage-relative, so a standoff cancels; it must not reach the output.""" + offset = AXIS_MAP.copy() + offset[:3, 3] = [3.0, -2.0, 0.5] + frame = OperatorFrame(offset) + frame.update(bearing_xr(40.0), FORWARD_BASE, engaged=False) + plain = OperatorFrame(AXIS_MAP) + plain.update(bearing_xr(40.0), FORWARD_BASE, engaged=False) + assert np.allclose(frame.transform[:3, :3], plain.transform[:3, :3]) + + +def test_held_while_engaged(): + """The frame the operator engaged under has to be the frame they finish in.""" + frame = OperatorFrame(AXIS_MAP) + frame.update(bearing_xr(20.0), FORWARD_BASE, engaged=False) + latched = frame.yaw_rad + for deg in (60.0, -80.0, 175.0): + frame.update(bearing_xr(deg), FORWARD_BASE, engaged=True) + assert frame.yaw_rad == latched + + +@pytest.mark.parametrize( + "xr, base", + [ + (None, FORWARD_BASE), + (bearing_xr(30.0), None), + (np.array([0.0, 1.0, 0.0]), FORWARD_BASE), # straight up: no bearing + (bearing_xr(30.0), np.array([0.0, 0.0, 1.0])), # base direction vertical + (np.array([np.nan, 0.0, -1.0]), FORWARD_BASE), + ], +) +def test_unusable_observation_holds(xr, base): + """Absent, vertical or non-finite holds the last yaw rather than latching noise.""" + frame = OperatorFrame(AXIS_MAP) + frame.update(bearing_xr(25.0), FORWARD_BASE, engaged=False) + latched = frame.yaw_rad + frame.update(xr, base, engaged=False) + assert frame.yaw_rad == latched + + +def test_rejects_a_bad_axis_map(): + with pytest.raises(ValueError, match="4x4"): + OperatorFrame(np.eye(3)) diff --git a/tests/python/viz/test_robot_anchor.py b/tests/python/viz/test_robot_anchor.py new file mode 100644 index 0000000000..caaa510782 --- /dev/null +++ b/tests/python/viz/test_robot_anchor.py @@ -0,0 +1,163 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Yaw and anchoring math from ``isaacteleop.viz.robot.anchor``.""" + +import math + +import numpy as np +import pytest + +from isaacteleop.viz.robot import anchor + +# Identity as xyzw: faces XR -Z. +_FACING_MINUS_Z = np.array([0.0, 0.0, 0.0, 1.0]) + + +def _bearing_deg(q_wxyz): + """The angle about +Y carried by a yaw quaternion, wrapped into (-180, 180]. + + A half turn reports +180, not -180, so a sign can be asserted at every bearing. + """ + deg = math.degrees(2.0 * math.atan2(q_wxyz[2], q_wxyz[0])) + return 180.0 - (180.0 - deg) % 360.0 + + +def _pitched(deg): + """A head orientation (xyzw) pitched about +X; positive looks up.""" + half = math.radians(deg) / 2.0 + return np.array([math.sin(half), 0.0, 0.0, math.cos(half)]) + + +@pytest.mark.parametrize( + ("forward", "expected_deg"), + [ + ((0.0, 0.0, -1.0), 0.0), # straight ahead + # +90 deg about XR +Y carries -Z onto -X, so -X is the POSITIVE bearing. + ((-1.0, 0.0, 0.0), 90.0), + ((1.0, 0.0, 0.0), -90.0), + ((0.0, 0.0, 1.0), 180.0), + ], +) +def test_yaw_of_direction_bearings(forward, expected_deg): + q = anchor.yaw_of_direction(np.array(forward), np.array([0.0, 0.0, -1.0])) + assert math.isclose(_bearing_deg(q), expected_deg, abs_tol=1e-9) + + +def test_yaw_of_direction_is_yaw_only(): + """Pitch must not leak: a direction tilted in Y keeps its horizontal bearing.""" + flat = anchor.yaw_of_direction( + np.array([1.0, 0.0, -1.0]), np.array([0.0, 0.0, -1.0]) + ) + tilted = anchor.yaw_of_direction( + np.array([1.0, 5.0, -1.0]), np.array([0.0, 0.0, -1.0]) + ) + np.testing.assert_allclose(flat, tilted, atol=1e-12) + + +def test_yaw_of_direction_negates_the_fallback_pointing_up(): + """The raw primitive, with a fallback a real caller would not pass by itself. + + Callers hand it the pose's own up-vector, which is what makes the reading + continuous through vertical -- see the heading tests below. + """ + up = anchor.yaw_of_direction(np.array([0.0, 1.0, 0.0]), np.array([0.0, 0.0, -1.0])) + assert math.isclose(_bearing_deg(up), 180.0, abs_tol=1e-9) + + down = anchor.yaw_of_direction( + np.array([0.0, -1.0, 0.0]), np.array([0.0, 0.0, -1.0]) + ) + assert math.isclose(_bearing_deg(down), 0.0, abs_tol=1e-9) + + +def test_yaw_of_reads_minus_z_of_a_head_pose(): + """A head yawed 30 deg reports 30 deg; identity alone would not show that.""" + assert math.isclose(_bearing_deg(anchor.yaw_of(_FACING_MINUS_Z)), 0.0, abs_tol=1e-9) + half = math.radians(30.0) / 2.0 + yawed = np.array([0.0, math.sin(half), 0.0, math.cos(half)]) + assert math.isclose(_bearing_deg(anchor.yaw_of(yawed)), 30.0, abs_tol=1e-9) + + +@pytest.mark.parametrize("pitch_deg", [-90.0, -89.0, 0.0, 89.0, 90.0]) +def test_yaw_of_holds_heading_up_to_vertical(pitch_deg): + """Pitching the head must not swing the twin: the up-vector fallback covers it.""" + assert math.isclose( + _bearing_deg(anchor.yaw_of(_pitched(pitch_deg))), 0.0, abs_tol=1e-9 + ) + + +@pytest.mark.parametrize("pitch_deg", [-91.0, 91.0]) +def test_yaw_of_flips_past_vertical(pitch_deg): + """Pinning a known wart, not endorsing it. + + One degree past vertical the -Z axis has crossed the horizon and the bearing + reverses. The near-vertical guard is an absolute 1e-6 and does not reach this. + """ + assert math.isclose( + _bearing_deg(anchor.yaw_of(_pitched(pitch_deg))), 180.0, abs_tol=1e-9 + ) + + +def test_anchor_from_head_carries_the_offset_onto_the_facing(): + """With the head facing -Z, the offset is applied unrotated and added to position.""" + head = np.array([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]) + offset = np.array([0.0, -0.30, -0.60]) + position, q_yaw = anchor.anchor_from_head(head, offset) + np.testing.assert_allclose(position, [1.0, 1.70, 2.40], atol=1e-12) + np.testing.assert_allclose(q_yaw, [1.0, 0.0, 0.0, 0.0], atol=1e-12) + + +def test_anchor_from_head_rotates_the_offset_with_the_head(): + """Turned 90 deg, a 'forward' offset must come out along the new facing.""" + half = math.sqrt(0.5) + head = np.array([0.0, 0.0, 0.0, 0.0, half, 0.0, half]) # +90 deg about +Y, xyzw + position, _ = anchor.anchor_from_head(head, np.array([0.0, 0.0, -1.0])) + np.testing.assert_allclose(position, [-1.0, 0.0, 0.0], atol=1e-9) + + +def test_yaw_of_axis_refuses_a_non_unit_quaternion(): + """A short quaternion shrinks the bearing silently. + + The rotation lerps toward the identity rather than scaling, so norm 0.9 turns a + 30 deg yaw into 24.4 deg with nothing to notice it by. + """ + half = math.radians(30.0) / 2.0 + q = np.array([0.0, math.sin(half), 0.0, math.cos(half)]) + forward = np.array([0.0, 0.0, -1.0]) + assert math.isclose( + _bearing_deg(anchor.yaw_of_axis(q, forward)), 30.0, abs_tol=1e-9 + ) + with pytest.raises(ValueError, match="unit quaternion"): + anchor.yaw_of_axis(0.9 * q, forward) + + +@pytest.mark.parametrize( + ("q", "expected"), + [ + ((0.0, 0.0, 0.0, 1.0), True), + ((0.0, 0.0, 0.0, 1.0 + 5e-4), True), # inside the drift tolerance + ((0.0, 0.0, 0.0, 1.0 - 5e-4), True), + ((0.0, 0.0, 0.0, 0.5), False), # finite, non-degenerate, and still unusable + ((0.0, 0.0, 0.0, 0.0), False), + ((0.0, 0.0, 0.0, float("nan")), False), + ((0.0, 0.0, 0.0, float("inf")), False), + ], +) +def test_is_unit(q, expected): + assert anchor.is_unit(np.array(q)) is expected + + +def test_is_unit_gates_exactly_what_yaw_of_axis_refuses(): + """One predicate for callers, so a norm-0.5 quaternion cannot reach the raise. + + ``yaw_of_axis`` raises on the frame loop, which ends a session; a caller reading a + quaternion off a device must be able to ask first, and get the same answer. + """ + forward = np.array([0.0, 0.0, -1.0]) + for scale in (0.0, 0.5, 0.9, 1.0 - 5e-4, 1.0, 1.0 + 5e-4, 1.1, 2.0): + q = _FACING_MINUS_Z * scale + if anchor.is_unit(q): + anchor.yaw_of_axis(q, forward) + else: + with pytest.raises(ValueError, match="unit quaternion"): + anchor.yaw_of_axis(q, forward) diff --git a/tests/python/viz/test_robot_frame_info.py b/tests/python/viz/test_robot_frame_info.py new file mode 100644 index 0000000000..71f0808a4f --- /dev/null +++ b/tests/python/viz/test_robot_frame_info.py @@ -0,0 +1,91 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""FrameInfo adapters that guard against silent-corruption bugs. + +Duck-typed stubs throughout -- no GPU, no headset, no runtime. +""" + +import numpy as np +import pytest + +from isaacteleop.viz.robot import frame_info + + +class _Pose: + def __init__(self, position, orientation): + self.position = position + self.orientation = orientation + + +class _Fov: + angle_left = -0.7 + angle_right = 0.7 + angle_up = 0.7 + angle_down = -0.7 + + +class _View: + def __init__(self, pose, fov=None): + self.pose = pose + self.fov = fov or _Fov() + + +class _Info: + def __init__(self, views=()): + self.views = list(views) + + +def _identity_view(): + return _View(_Pose((1.0, 2.0, 3.0), (1.0, 0.0, 0.0, 0.0))) + + +def test_head_pose_reorders_the_quaternion(): + """viz reports (w,x,y,z); everything downstream of this takes (x,y,z,w).""" + view = _View(_Pose((1.0, 2.0, 3.0), (0.5, 0.5, 0.5, 0.5))) + pose = frame_info.head_pose(_Info(views=[view])) + np.testing.assert_allclose(pose, [1.0, 2.0, 3.0, 0.5, 0.5, 0.5, 0.5]) + + +def test_head_pose_is_none_without_views(): + assert frame_info.head_pose(_Info()) is None + + +@pytest.mark.parametrize( + "orientation", + [ + (0.0, 0.0, 0.0, 0.0), # degenerate: carries no orientation + (float("nan"), 0.0, 0.0, 0.0), + # Finite and non-degenerate, and still unusable: everything this pose feeds + # turns a vector by it, and those raise on a non-unit quaternion. + (0.5, 0.0, 0.0, 0.0), + (2.0, 0.0, 0.0, 0.0), + ], +) +def test_head_pose_refuses_unusable_orientations(orientation): + view = _View(_Pose((0.0, 0.0, 0.0), orientation)) + assert frame_info.head_pose(_Info(views=[view])) is None + + +def test_head_pose_refuses_non_finite_position(): + view = _View(_Pose((float("inf"), 0.0, 0.0), (1.0, 0.0, 0.0, 0.0))) + assert frame_info.head_pose(_Info(views=[view])) is None + + +def test_flatten_views_keeps_wxyz_and_pairs_each_fov(): + """Stereo: two views flatten to 7 pose floats and 4 fov floats each, in order.""" + poses, fovs = frame_info.flatten_views(_Info(views=[_identity_view()] * 2)) + assert len(poses) == 14 + assert len(fovs) == 8 + # Position first, then (w,x,y,z) -- not the (x,y,z,w) head_pose emits. + assert poses[:7] == [1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0] + assert fovs[:4] == [ + _Fov.angle_left, + _Fov.angle_right, + _Fov.angle_up, + _Fov.angle_down, + ] + + +def test_flatten_views_is_empty_before_the_first_rendered_frame(): + assert frame_info.flatten_views(_Info()) == ([], []) diff --git a/tests/python/viz/test_robot_imports.py b/tests/python/viz/test_robot_imports.py new file mode 100644 index 0000000000..ab47cf88e9 --- /dev/null +++ b/tests/python/viz/test_robot_imports.py @@ -0,0 +1,106 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Static wiring checks for `isaacteleop.viz.robot`, which no import can make for us. + +Most of that package is resolved lazily, because `frames` and `scene` need the compiled +`_robot_twin` that a Windows Televiz build has no copy of. Importing a lazy module is +therefore not something a GPU-less test can do -- and a name that does not exist behind one +of those `__getattr__` entries stays invisible until a headset run reaches it. + +These read the source instead. They need no backend, no GPU and no `isaacteleop` import, so +they run everywhere and catch the whole class: a relative import naming something its +target does not define, and a lazy-table entry pointing at nothing. +""" + +from __future__ import annotations + +import ast +from pathlib import Path + +import pytest +from repo_paths import repo_root + +ROBOT = repo_root() / "src" / "python" / "isaacteleop" / "viz" / "robot" + + +def _top_level_names(path: Path) -> set[str]: + """Every name a module binds at module scope, imports included.""" + names: set[str] = set() + for node in ast.parse(path.read_text()).body: + if isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef, ast.ClassDef)): + names.add(node.name) + elif isinstance(node, ast.Assign): + names |= {t.id for t in node.targets if isinstance(t, ast.Name)} + elif isinstance(node, ast.AnnAssign) and isinstance(node.target, ast.Name): + names.add(node.target.id) + elif isinstance(node, (ast.Import, ast.ImportFrom)): + names |= {a.asname or a.name.split(".")[0] for a in node.names} + return names + + +@pytest.mark.parametrize("source", sorted(ROBOT.glob("*.py")), ids=lambda p: p.name) +def test_relative_imports_resolve(source: Path) -> None: + """Every `from .module import name` names something that module actually defines.""" + for node in ast.walk(ast.parse(source.read_text())): + if ( + not isinstance(node, ast.ImportFrom) + or node.level != 1 + or node.module is None + ): + continue + target = ROBOT / f"{node.module}.py" + if not target.exists(): + # The compiled backend, which is a build artifact rather than a source file. + assert node.module.startswith("_"), ( + f"{source.name}: no module .{node.module}" + ) + continue + defined = _top_level_names(target) + for alias in node.names: + assert alias.name == "*" or alias.name in defined, ( + f"{source.name}: `from .{node.module} import {alias.name}` but " + f"{node.module}.py defines no {alias.name}" + ) + + +def test_lazy_table_resolves() -> None: + """Every `_LAZY` entry names a real module, or a real name inside one, and is exported. + + The lazy table is the one place a typo cannot fail at import time -- `__getattr__` only + runs when somebody asks for the name, which on this package may be the first headset + run. + """ + tree = ast.parse((ROBOT / "__init__.py").read_text()) + lazy: dict[str, str] = {} + modules: set[str] = set() + exported: set[str] = set() + for node in ast.walk(tree): + if not isinstance(node, ast.Assign): + continue + target = node.targets[0] + if not isinstance(target, ast.Name): + continue + if target.id == "_LAZY": + lazy = { + k.value: v.value.lstrip(".") + for k, v in zip(node.value.keys, node.value.values) + } + elif target.id == "_LAZY_MODULES": + modules = set(ast.literal_eval(node.value.args[0])) + elif target.id == "__all__": + exported = set(ast.literal_eval(node.value)) + + assert lazy, "no _LAZY table found" + assert modules <= set(lazy), ( + f"_LAZY_MODULES names entries not in _LAZY: {modules - set(lazy)}" + ) + + for name, module in sorted(lazy.items()): + assert (ROBOT / f"{module}.py").exists(), ( + f"_LAZY[{name!r}] -> missing {module}.py" + ) + assert name in exported, f"_LAZY[{name!r}] is not in __all__" + if name not in modules: + defined = _top_level_names(ROBOT / f"{module}.py") + assert name in defined, f"_LAZY[{name!r}] -> {module}.py defines no {name}" diff --git a/tests/python/viz/test_robot_joint_map.py b/tests/python/viz/test_robot_joint_map.py new file mode 100644 index 0000000000..25f419c9be --- /dev/null +++ b/tests/python/viz/test_robot_joint_map.py @@ -0,0 +1,81 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""``isaacteleop.viz.robot.JointMap`` -- name to address, and the layout assert.""" + +import numpy as np +import pytest + +from isaacteleop.viz.robot import JointMap + +_NAMES = ("shoulder", "elbow", "gripper") + + +def _map(names=_NAMES, addresses=(0, 1, 2), width=3): + return JointMap(names, addresses, width=width) + + +def test_scatter_writes_each_name_to_its_own_address(): + """Addresses need not be contiguous, and the gaps must be left alone.""" + positions = np.full(6, -1.0) + _map(addresses=(5, 0, 3), width=6).scatter([0.1, 0.2, 0.3], positions) + np.testing.assert_allclose(positions, [0.2, -1.0, -1.0, 0.3, -1.0, 0.1]) + + +@pytest.mark.parametrize( + ("names", "addresses", "width"), + [ + (_NAMES, (0, 1), 3), # one address short + (("a", "a"), (0, 1), 3), # duplicate name + (("a", "b"), (1, 1), 3), # aliased address: the second would win + (("a", "b"), (0, 3), 3), # past the end + (("a", "b"), (0, -1), 3), # negative would index from the end + ], +) +def test_rejects_a_mapping_nothing_could_scatter_through(names, addresses, width): + with pytest.raises(ValueError): + JointMap(names, addresses, width=width) + + +def test_require_accepts_the_authored_order(): + _map().require(_NAMES) + + +@pytest.mark.parametrize( + "expected", + [ + ("shoulder", "gripper", "elbow"), # reordered: same joints, wrong angles + ("shoulder", "elbow"), # a joint the caller does not know about + ("shoulder", "elbow", "wrist"), # renamed upstream + ], +) +def test_require_rejects_anything_else(expected): + with pytest.raises(RuntimeError, match="pose the wrong joints"): + _map().require(expected) + + +def test_require_names_what_differs(): + """The message has to be actionable: an operator sees it and nothing else.""" + with pytest.raises(RuntimeError) as excinfo: + _map().require(("shoulder", "elbow", "wrist")) + assert "wrist" in str(excinfo.value) and "gripper" in str(excinfo.value) + + +@pytest.mark.parametrize("joints", [[1.0, 2.0], [1.0, 2.0, 3.0, 4.0], []]) +def test_scatter_rejects_a_snapshot_of_the_wrong_width(joints): + with pytest.raises(ValueError): + _map().scatter(joints, np.zeros(3)) + + +def test_scatter_rejects_a_state_vector_of_the_wrong_width(): + """Numpy would happily index a longer one, silently posing a different scene.""" + with pytest.raises(ValueError): + _map().scatter([0.0, 0.0, 0.0], np.zeros(9)) + + +def test_a_rigid_scene_maps_no_joints_at_all(): + """Legal, and what a prop moved only by its base pose has.""" + joints = JointMap((), (), width=0) + assert len(joints) == 0 + joints.require(()) + joints.scatter([], np.zeros(0)) diff --git a/tests/python/viz/test_robot_session.py b/tests/python/viz/test_robot_session.py new file mode 100644 index 0000000000..06bc74e7ad --- /dev/null +++ b/tests/python/viz/test_robot_session.py @@ -0,0 +1,354 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""``XrTwinSession`` lifecycle and frame-loop invariants, against fakes. + +The compositor types are replaced wholesale, so what is under test is this module's own +sequencing. Importing it still needs the built ``_viz``; the fakes replace the runtime, +not the extension. +""" + +import math + +import pytest + +from isaacteleop.viz.robot import session as session_mod +from isaacteleop.viz.robot.session import ( + VIEW_COUNT, + WAIT_FOR_HEADSET, + XrTwinSession, +) + + +_OXR_HANDLES = (1, 2, 3, 4) +_NEAR_Z = 0.05 +_FAR_Z = 50.0 + + +class _Resolution: + width = 1440 + height = 1584 + + +class _Fov: + angle_left = -0.7 + angle_right = 0.7 + angle_up = 0.7 + angle_down = -0.7 + + +class _Pose: + position = (0.0, 0.0, 0.0) + orientation = (1.0, 0.0, 0.0, 0.0) + + +class _View: + pose = _Pose() + fov = _Fov() + + +class _Info: + def __init__(self, *, should_render=True, view_count=VIEW_COUNT): + self.should_render = should_render + self.views = [_View() for _ in range(view_count)] + + +class _FakeSession: + """Records the compositor calls this module is responsible for ordering. + + One shared ``calls`` list across session, layer and twin: the invariants under test + are orderings BETWEEN them, which separate logs cannot express. + """ + + def __init__(self, frames, calls): + self._frames = list(frames) + self.calls = calls + self.destroyed = False + self.layer = None + self.handles = _OXR_HANDLES + + def get_recommended_resolution(self): + return _Resolution() + + def add_projection_layer(self, config): + self.layer = _FakeLayer(self.calls) + return self.layer + + def should_close(self): + return not self._frames + + def begin_frame(self): + self.calls.append("begin_frame") + return self._frames.pop(0) + + def end_frame(self): + self.calls.append( + "end_frame" if not self.destroyed else "end_frame_AFTER_DESTROY" + ) + + def destroy(self): + self.destroyed = True + self.calls.append("destroy") + + def get_oxr_handles(self): + return self.handles + + +class _FakeLayer: + def __init__(self, calls): + self._calls = calls + self.submitted = None + + def submit(self, *images): + self._calls.append("layer.submit") + self.submitted = images + + +class _FakeTwin: + """A RobotTwin that records its lifecycle and can be told to fail in create().""" + + def __init__(self, calls, *, fail_create=False): + self._calls = calls + self._fail_create = fail_create + self.created_with = None + + def create(self, width, height, view_count, *, near_z, far_z): + self._calls.append("twin.create") + self.created_with = (width, height, view_count, near_z, far_z) + if self._fail_create: + raise RuntimeError("backend blew up") + + def render(self, poses, fovs): + self._calls.append("twin.render") + + def color(self, view): + return f"color{view}" + + def depth(self, view): + return f"depth{view}" + + def frustum(self, view): + self._calls.append("twin.frustum") + # What assert_frustum expects for _Fov at the session's clip planes. + near, far = _NEAR_Z, _FAR_Z + left, right = ( + near * math.tan(_Fov.angle_left), + near * math.tan(_Fov.angle_right), + ) + bottom, top = near * math.tan(_Fov.angle_down), near * math.tan(_Fov.angle_up) + return ((left + right) / 2, (right - left) / 2, bottom, top, near, far) + + def destroy(self): + self._calls.append("twin.destroy") + + +@pytest.fixture +def patched(monkeypatch): + """Replace the compositor types so nothing touches Vulkan.""" + holder = {} + + class _Config: + pass + + def _make(frames, calls): + holder["session"] = _FakeSession(frames, calls) + return holder["session"] + + class _FakeVizSession: + @staticmethod + def create(config): + holder["config"] = config + return holder["pending"] + + monkeypatch.setattr(session_mod, "VizSession", _FakeVizSession) + monkeypatch.setattr(session_mod, "VizSessionConfig", _Config) + monkeypatch.setattr(session_mod, "ProjectionLayerConfig", _Config) + monkeypatch.setattr(session_mod, "DisplayMode", type("D", (), {"kXr": 0})) + monkeypatch.setattr( + session_mod, "PixelFormat", type("P", (), {"kRGBA8": 0, "kD32F": 1}) + ) + return holder, _make + + +def _build(holder, make, frames, *, fail_create=False): + calls = [] + make(frames, calls) + holder["pending"] = holder["session"] + twin = _FakeTwin(calls, fail_create=fail_create) + xr = XrTwinSession( + twin, + app_name="test", + near_z=_NEAR_Z, + far_z=_FAR_Z, + required_extensions=["XR_TEST_extension"], + layer_name="test_layer", + ) + return xr, twin, calls + + +def test_frames_before_entering_raises(patched): + holder, make = patched + xr, _, _ = _build(holder, make, []) + with pytest.raises(RuntimeError, match="Not entered"): + next(xr.frames()) + + +def test_resolution_before_entering_raises(patched): + holder, make = patched + xr, _, _ = _build(holder, make, []) + with pytest.raises(RuntimeError, match="Not entered"): + _ = xr.resolution + + +def test_twin_is_created_with_the_sessions_clip_planes(patched): + """The clip planes must reach the twin from the session, not a second source.""" + holder, make = patched + xr, twin, _ = _build(holder, make, []) + with xr: + assert twin.created_with == ( + _Resolution.width, + _Resolution.height, + VIEW_COUNT, + _NEAR_Z, + _FAR_Z, + ) + + +def test_it_waits_for_a_headset_by_default(patched): + """viz itself fails fast, which is right for CI and wrong for an app on a rig. + + Regression: NVIDIA/IsaacTeleop@32c9cd680 made oxr::OpenXRSession wait but left the viz + path alone, so the robot-twin example died with FORM_FACTOR_UNAVAILABLE the moment the headset was + a second late. + """ + holder, make = patched + xr, _, _ = _build(holder, make, []) + with xr: + pass + assert holder["config"].xr_system_wait_seconds == WAIT_FOR_HEADSET + + +def test_a_caller_can_still_ask_to_fail_fast(patched): + holder, make = patched + calls = [] + make([], calls) + holder["pending"] = holder["session"] + xr = XrTwinSession( + _FakeTwin(calls), + app_name="test", + near_z=_NEAR_Z, + far_z=_FAR_Z, + required_extensions=[], + layer_name="test_layer", + system_wait_seconds=0, + ) + with xr: + pass + assert holder["config"].xr_system_wait_seconds == 0 + + +def test_destroy_runs_even_when_create_raised(patched): + """A twin that built a context before failing must still be torn down.""" + holder, make = patched + xr, _, calls = _build(holder, make, [], fail_create=True) + with pytest.raises(RuntimeError, match="backend blew up"): + with xr: + pass + assert calls.index("twin.destroy") < calls.index("destroy") + assert holder["session"].destroyed + + +def test_teardown_order_is_twin_then_session(patched): + """The twin's GPU objects need its context, so it goes first.""" + holder, make = patched + xr, _, calls = _build(holder, make, []) + with xr: + pass + assert calls.index("twin.destroy") < calls.index("destroy") + + +def test_end_frame_runs_on_the_not_rendered_path(patched): + """Skipping end_frame on a pre-kRunning frame wedges the loop.""" + holder, make = patched + xr, _, calls = _build(holder, make, [_Info(should_render=False), _Info()]) + with xr: + list(xr.frames()) + assert calls.count("end_frame") == 2 + + +def test_end_frame_runs_when_the_loop_body_raises(patched): + """And it must land before the session is destroyed.""" + holder, make = patched + xr, _, calls = _build(holder, make, [_Info()]) + with pytest.raises(ValueError): + with xr: + for _frame in xr.frames(): + raise ValueError("caller blew up") + assert calls.index("end_frame") < calls.index("destroy") + + +def test_pre_krunning_frames_are_not_yielded(patched): + holder, make = patched + xr, _, _ = _build(holder, make, [_Info(should_render=False), _Info()]) + with xr: + assert len(list(xr.frames())) == 1 + + +def test_a_view_count_this_session_cannot_render_is_refused(patched): + """A quad-view runtime must fail loudly, not render the wrong two eyes.""" + holder, make = patched + xr, _, calls = _build(holder, make, [_Info(view_count=4)]) + with xr: + with pytest.raises(RuntimeError, match="stereo-only"): + list(xr.frames()) + # The raise happens inside the generator's try, so the frame is still closed out. + assert calls.count("end_frame") == 1 + + +def test_render_draws_then_submits(patched): + holder, make = patched + xr, _, calls = _build(holder, make, [_Info()]) + with xr: + for frame in xr.frames(): + xr.render(frame) + assert calls.index("twin.render") < calls.index("layer.submit") + + +def test_the_frustum_is_checked_on_the_first_rendered_frame_only(patched): + """Per-frame checking would cost a readback every frame for a fixed convention.""" + holder, make = patched + xr, _, calls = _build(holder, make, [_Info(), _Info()]) + with xr: + for frame in xr.frames(): + xr.render(frame) + assert calls.count("twin.frustum") == VIEW_COUNT + assert calls.count("twin.render") == 2 + + +def test_render_hands_both_eyes_over_in_order(patched): + holder, make = patched + xr, _, _ = _build(holder, make, [_Info()]) + with xr: + for frame in xr.frames(): + xr.render(frame) + assert holder["session"].layer.submitted == ("color0", "depth0", "color1", "depth1") + + +def test_render_after_exit_raises(patched): + """A frame kept past the session must not reach a destroyed layer.""" + holder, make = patched + xr, _, _ = _build(holder, make, [_Info()]) + with xr: + held = next(xr.frames()) + with pytest.raises(RuntimeError, match="Not entered"): + xr.render(held) + + +def test_oxr_handles_refuses_a_backend_that_did_not_initialize(patched): + holder, make = patched + xr, _, _ = _build(holder, make, []) + with xr: + assert xr.oxr_handles() == _OXR_HANDLES + holder["session"].handles = None + with pytest.raises(RuntimeError, match="did not initialize"): + xr.oxr_handles()