From b40444db96b35b1a56abd6ee6da1eb4cd80695d3 Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Fri, 15 Aug 2025 13:30:29 -0700 Subject: [PATCH 1/9] initial commit for manus + vive extension --- source/apps/isaacsim.exp.base.kit | 1 + source/apps/isaacsim.exp.extscache.kit | 1 + .../isaacsim/core/includes/core/Platform.h | 1 + .../IsaacsimXrInputDevicesBindings.cpp | 36 +++ .../config/extension.toml | 31 ++ .../isaacsim.xr.input_devices/data/icon.png | 3 + .../data/preview.png | 3 + .../docs/CHANGELOG.md | 30 ++ .../isaacsim.xr.input_devices/docs/README.md | 181 +++++++++++ .../isaacsim/xr/input_devices/ManusTracker.h | 71 +++++ .../ManusTracker.cpp | 301 ++++++++++++++++++ .../isaacsim.xr.input_devices/premake5.lua | 70 ++++ .../python/__init__.py | 18 ++ .../python/impl/__init__.py | 26 ++ .../python/impl/extension.py | 28 ++ .../python/impl/manus_tracker.py | 93 ++++++ .../python/impl/vive_tracker.py | 105 ++++++ .../python/impl/xr_device_integration.py | 87 +++++ .../python/tests/__init__.py | 18 ++ .../python/tests/test_xr_input_devices.py | 101 ++++++ .../manus_vive_tracking_sample.py | 183 +++++++++++ 21 files changed, 1388 insertions(+) create mode 100644 source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp create mode 100644 source/extensions/isaacsim.xr.input_devices/config/extension.toml create mode 100644 source/extensions/isaacsim.xr.input_devices/data/icon.png create mode 100644 source/extensions/isaacsim.xr.input_devices/data/preview.png create mode 100644 source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md create mode 100644 source/extensions/isaacsim.xr.input_devices/docs/README.md create mode 100644 source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h create mode 100644 source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp create mode 100644 source/extensions/isaacsim.xr.input_devices/premake5.lua create mode 100644 source/extensions/isaacsim.xr.input_devices/python/__init__.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/extension.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py create mode 100644 source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py create mode 100644 source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py diff --git a/source/apps/isaacsim.exp.base.kit b/source/apps/isaacsim.exp.base.kit index 0cf7cba9..4fc7382c 100644 --- a/source/apps/isaacsim.exp.base.kit +++ b/source/apps/isaacsim.exp.base.kit @@ -44,6 +44,7 @@ keywords = ["experience", "app", "usd"] # That makes it browsable in UI with "ex "isaacsim.simulation_app" = {} "isaacsim.storage.native" = {} "isaacsim.util.debug_draw" = {} +"isaacsim.xr.input_devices" = {} "omni.isaac.core_archive" = {} "omni.isaac.ml_archive" = {} "omni.kit.loop-isaac" = {} diff --git a/source/apps/isaacsim.exp.extscache.kit b/source/apps/isaacsim.exp.extscache.kit index b1eaccf5..a988e7bc 100644 --- a/source/apps/isaacsim.exp.extscache.kit +++ b/source/apps/isaacsim.exp.extscache.kit @@ -101,6 +101,7 @@ folders = [ "isaacsim.util.debug_draw" = {} "isaacsim.util.merge_mesh" = {} "isaacsim.util.physics" = {} +"isaacsim.xr.input_devices" = {} "isaacsim.xr.openxr" = {} "omni.exporter.urdf" = {} "omni.isaac.app.selector" = {} diff --git a/source/extensions/isaacsim.core.includes/include/isaacsim/core/includes/core/Platform.h b/source/extensions/isaacsim.core.includes/include/isaacsim/core/includes/core/Platform.h index cd449a7d..805b91ab 100644 --- a/source/extensions/isaacsim.core.includes/include/isaacsim/core/includes/core/Platform.h +++ b/source/extensions/isaacsim.core.includes/include/isaacsim/core/includes/core/Platform.h @@ -15,6 +15,7 @@ #pragma once +#include #include #include diff --git a/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp b/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp new file mode 100644 index 00000000..7906d89f --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +CARB_BINDINGS("isaacsim.xr.input_devices.python") + +namespace +{ +PYBIND11_MODULE(_isaac_xr_input_devices, m) +{ + using namespace carb; + using namespace isaacsim::xr::input_devices; + + auto carbModule = py::module::import("carb"); + + py::class_(m, "IsaacSimManusTracker") + .def(py::init<>()) + .def("initialize", &IsaacSimManusTracker::initialize, R"( + Initialize Manus SDK (adapted from existing implementation). + + Returns: + bool: True if initialization was successful, False otherwise + )") + .def("get_glove_data", &IsaacSimManusTracker::get_glove_data, R"( + Get glove data in IsaacSim format. + + Returns: + Dict[str, List[float]]: Dictionary mapping glove data keys to values + )") + .def("cleanup", &IsaacSimManusTracker::cleanup, R"( + Cleanup SDK resources. + )"); +} +} \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/config/extension.toml b/source/extensions/isaacsim.xr.input_devices/config/extension.toml new file mode 100644 index 00000000..f92f0a3b --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/config/extension.toml @@ -0,0 +1,31 @@ +[package] +version = "1.0.0" +category = "XR" +title = "Isaac Sim XR Input Devices" +description = "Provides XR input device support for Manus gloves and Vive trackers" +authors = ["yuanchenl@nvidia.com"] +repository = "https://gitlab-master.nvidia.com/omniverse/isaac/omni_isaac_sim/-/tree/develop/source/extensions/isaacsim.xr.input_devices" +keywords = ["isaac", "xr", "manus", "vive", "trackers", "gloves", "input", "devices"] +changelog = "docs/CHANGELOG.md" +readme = "docs/README.md" +writeTarget.kit = true + +[dependencies] +"omni.kit.xr.core" = {} +"isaacsim.core.api" = {} +"isaacsim.core.deprecation_manager" = {} +"isaacsim.core.utils" = {} + +[[python.module]] +name = "isaacsim.xr.input_devices" + +[[test]] +dependencies = [ + "isaacsim.core.api", + "isaacsim.core.utils", +] +args = [ + "--/app/asyncRendering=0", + "--/app/fastShutdown=1", + "--vulkan", +] \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/data/icon.png b/source/extensions/isaacsim.xr.input_devices/data/icon.png new file mode 100644 index 00000000..56a758d9 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/data/icon.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:30231e6fe2c3747b0456cb696d99fb7ebce25af1c3ac08c08e44736f20916a5e +size 2698 diff --git a/source/extensions/isaacsim.xr.input_devices/data/preview.png b/source/extensions/isaacsim.xr.input_devices/data/preview.png new file mode 100644 index 00000000..02350eb3 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/data/preview.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cf6ccd9d5e1e2fbce73e99ee009a67536adac455157bb68a1e80a187b4ba313a +size 19265 diff --git a/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md b/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md new file mode 100644 index 00000000..b441c5a7 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md @@ -0,0 +1,30 @@ +# Changelog + +All notable changes to this project will be documented in this file. + +The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), +and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). + +## [1.0.0] - 2025-08-08 + +### Added +- Initial release of IsaacSim XR Input Devices extension +- Support for Manus gloves with ManusSDK (c++ with Python binding) +- Support for Vive trackers with pysurvive +- Integration with IsaacSim XR system +- Synchronized device updates +- Mock data fallback for testing without hardware +- Documentation and build configuration + +### Technical Details +- Adapted from isaac-deploy Holoscan implementations +- Rate-limited updates (60Hz default) +- Device status monitoring +- Cleanup and resource management + +### Dependencies +- IsaacSim Core API +- IsaacSim Core Utils +- Omniverse Kit XR Core +- ManusSDK (optional - falls back to mock data) +- pysurvive (optional - falls back to mock data) diff --git a/source/extensions/isaacsim.xr.input_devices/docs/README.md b/source/extensions/isaacsim.xr.input_devices/docs/README.md new file mode 100644 index 00000000..4bcec37c --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/docs/README.md @@ -0,0 +1,181 @@ +# IsaacSim XR Input Devices Extension + +This extension provides XR input device support for Manus gloves and Vive trackers, adapted from isaac-deploy Holoscan implementations. + +## Features + +- **Manus Glove Integration**: Real-time hand and finger tracking with full skeletal data +- **Vive Tracker Integration**: 6DOF pose tracking for HMDs, controllers, and trackers +- **Live Data Visualization**: Red cubes appear at device positions in Isaac Sim +- **Automatic Setup**: Library paths configured automatically +- **Device Status Monitoring**: Connection state and data freshness tracking +- **Error Handling**: Graceful degradation when hardware/SDKs are unavailable +- **Mock Data Support**: Testing without physical hardware + +## Prerequisites + +### Hardware Requirements +- **Manus Gloves**: Manus Prime or MetaGloves with license dongle +- **Vive Trackers**: SteamVR/Lighthouse tracking system with trackers + +### Software Dependencies + +#### Manus SDK +- ManusSDK is automatically included in Isaac Sim's target dependencies +- Requires Manus license dongle for operation +- Falls back to mock data if unavailable + +#### libsurvive (for Vive trackers) +Currently requires manual installation. Choose one option: + +**Option 1: System Installation** +```bash +sudo apt update +sudo apt install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev liblapacke-dev libopenblas-dev libatlas-base-dev cmake + +git clone https://github.com/cntools/libsurvive.git +cd libsurvive +sudo cp ./useful_files/81-vive.rules /etc/udev/rules.d/ +sudo udevadm control --reload-rules && sudo udevadm trigger +make && cmake . && make -j$(nproc) +sudo make install && sudo ldconfig +``` + +**Option 2: User Installation (Recommended)** +```bash +# Build in home directory (detected automatically) +git clone https://github.com/cntools/libsurvive.git ~/libsurvive +cd ~/libsurvive +sudo cp ./useful_files/81-vive.rules /etc/udev/rules.d/ +sudo udevadm control --reload-rules && sudo udevadm trigger +make && cmake . && make -j$(nproc) +``` + +## Building + +```bash +# Build Isaac Sim (includes this extension) +./build.sh +``` + +The extension is automatically built as part of Isaac Sim's build process. + +## Usage + +### Running the Sample +A complete sample script demonstrates both Manus and Vive tracking: + +```bash +cd /path/to/IsaacSim +./_build/linux-x86_64/release/python.sh source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +``` + +### Python API Usage + +```python +from isaacsim.xr.input_devices.impl.xr_device_integration import get_xr_device_integration + +# Get the extension's device integration instance +integration = get_xr_device_integration() + +# Get all device data +all_data = integration.get_all_device_data() + +# Access specific device data +manus_data = all_data.get('manus_gloves', {}) +vive_data = all_data.get('vive_trackers', {}) + +# Check device status +status = all_data.get('device_status', {}) +manus_connected = status.get('manus_gloves', {}).get('connected', False) +vive_connected = status.get('vive_trackers', {}).get('connected', False) + +print(f"Manus connected: {manus_connected}") +print(f"Vive connected: {vive_connected}") +``` + +### Data Format + +**Manus Glove Data:** +```python +{ + 'manus_gloves': { + 'left_glove': { + 'position': np.array([...]), # 75 joint positions (x,y,z per joint) + 'orientation': np.array([...]), # 75 joint orientations (w,x,y,z per joint) + 'valid': True + }, + 'right_glove': { ... } + } +} +``` + +**Vive Tracker Data:** +```python +{ + 'vive_trackers': { + 'WM0_position': [x, y, z], # Tracker position + 'WM0_orientation': [w, x, y, z], # Tracker orientation + 'LH1_position': [x, y, z], # Lighthouse position + 'LH1_orientation': [w, x, y, z] # Lighthouse orientation + } +} +``` + +## Extension Architecture + +``` +isaacsim.xr.input_devices/ +├── bindings/ # Pybind11 C++ bindings +├── include/ # C++ headers +├── plugins/ # C++ implementation +│ └── ManusTracker.cpp # Manus SDK integration +├── python/ +│ └── impl/ +│ ├── extension.py # Extension lifecycle +│ ├── xr_device_integration.py # Main orchestrator +│ ├── manus_tracker.py # Manus wrapper +│ └── vive_tracker.py # Vive wrapper +└── config/extension.toml # Extension configuration +``` + +## Troubleshooting + +### Manus Issues +- **License Error**: Unplug and replug the Manus license dongle +- **No Data**: Check if `ManusSDK_Integrated.so` is in `LD_LIBRARY_PATH` +- **Connection Failed**: Ensure Manus Core isn't running simultaneously + +### Vive Issues +- **No Trackers Found**: Verify SteamVR is not running (conflicts with libsurvive) +- **Lighthouse Only**: Check that trackers have tracking lock (LED should be solid) +- **pysurvive Import Error**: Install libsurvive (see Prerequisites section) + +### Build Issues +- Verify Isaac Sim build environment is set up correctly +- Check that Manus SDK is in `_build/target-deps/manus_sdk/` +- Ensure all C++ dependencies are available + +### Runtime Issues +```bash +# Check device connections +lsusb | grep -i manus +lsusb | grep -i lighthouse + +# Monitor Isaac Sim logs for device initialization +# Look for: "Manus glove tracker initialized with SDK" +# Look for: "Adding tracked object WM0 from HTC" +``` + +## Contributing + +This extension follows Isaac Sim's extension development guidelines: +- C++ code uses pybind11 for Python bindings +- Python code follows Isaac Sim coding standards +- All changes should maintain backward compatibility +- Include tests for new functionality + +## License + +Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +SPDX-License-Identifier: Apache-2.0 diff --git a/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h b/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h new file mode 100644 index 00000000..268a662b --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "ManusSDK.h" + +#ifdef _MSC_VER +# if ISAACSIM_XR_INPUT_DEVICES_EXPORT +# define ISAACSIM_XR_INPUT_DEVICES_DLL_EXPORT __declspec(dllexport) +# else +# define ISAACSIM_XR_INPUT_DEVICES_DLL_EXPORT __declspec(dllimport) +# endif +#else +# define ISAACSIM_XR_INPUT_DEVICES_DLL_EXPORT __attribute__((visibility("default"))) +#endif + +namespace isaacsim +{ +namespace xr +{ +namespace input_devices +{ + +class ISAACSIM_XR_INPUT_DEVICES_DLL_EXPORT IsaacSimManusTracker +{ +public: + IsaacSimManusTracker(); + ~IsaacSimManusTracker(); + + bool initialize(); + std::unordered_map> get_glove_data(); + void cleanup(); + +private: + static IsaacSimManusTracker* s_instance; + static std::mutex s_instance_mutex; + + // ManusSDK specific members + void RegisterCallbacks(); + void ConnectToGloves(); + void DisconnectFromGloves(); + + // Callback functions + static void OnSkeletonStream(const SkeletonStreamInfo* skeleton_stream_info); + static void OnLandscapeStream(const Landscape* landscape); + static void OnErgonomicsStream(const ErgonomicsStream* ergonomics_stream); + + // Data storage (following isaac-deploy pattern) + std::mutex output_map_mutex; + std::mutex landscape_mutex; + std::unordered_map> output_map; + std::optional left_glove_id; + std::optional right_glove_id; + bool is_connected = false; + + // Legacy member for compatibility + std::unordered_map> m_glove_data; + std::mutex m_data_mutex; +}; + +} // namespace input_devices +} // namespace xr +} // namespace isaacsim \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp b/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp new file mode 100644 index 00000000..282cc35d --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp @@ -0,0 +1,301 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include + +#include +#include +#include +#include +#include +#include + +#include "ManusSDK.h" +#include "ManusSDKTypeInitializers.h" + +#include + +namespace isaacsim +{ +namespace xr +{ +namespace input_devices +{ + +IsaacSimManusTracker* IsaacSimManusTracker::s_instance = nullptr; +std::mutex IsaacSimManusTracker::s_instance_mutex; + +IsaacSimManusTracker::IsaacSimManusTracker() = default; + +IsaacSimManusTracker::~IsaacSimManusTracker() { + cleanup(); +} + +bool IsaacSimManusTracker::initialize() { + { + std::lock_guard lock(s_instance_mutex); + if (s_instance != nullptr) { + CARB_LOG_ERROR("ManusTracker instance already exists - only one instance allowed"); + return false; + } + s_instance = this; + } + + CARB_LOG_INFO("Initializing Manus SDK..."); + const SDKReturnCode t_InitializeResult = CoreSdk_InitializeIntegrated(); + if (t_InitializeResult != SDKReturnCode::SDKReturnCode_Success) { + CARB_LOG_ERROR("Failed to initialize Manus SDK, error code: %d", static_cast(t_InitializeResult)); + std::lock_guard lock(s_instance_mutex); + s_instance = nullptr; + return false; + } + CARB_LOG_INFO("Manus SDK initialized successfully"); + + RegisterCallbacks(); + + CoordinateSystemVUH t_VUH; + CoordinateSystemVUH_Init(&t_VUH); + t_VUH.handedness = Side::Side_Right; + t_VUH.up = AxisPolarity::AxisPolarity_PositiveZ; + t_VUH.view = AxisView::AxisView_XFromViewer; + t_VUH.unitScale = 1.0f; + + CARB_LOG_INFO("Setting up coordinate system (Z-up, right-handed, meters)..."); + const SDKReturnCode t_CoordinateResult = CoreSdk_InitializeCoordinateSystemWithVUH(t_VUH, true); + + if (t_CoordinateResult != SDKReturnCode::SDKReturnCode_Success) { + CARB_LOG_ERROR("Failed to initialize Manus SDK coordinate system, error code: %d", static_cast(t_CoordinateResult)); + std::lock_guard lock(s_instance_mutex); + s_instance = nullptr; + return false; + } + CARB_LOG_INFO("Coordinate system initialized successfully"); + + ConnectToGloves(); + return true; +} + +std::unordered_map> IsaacSimManusTracker::get_glove_data() { + std::lock_guard lock(output_map_mutex); + return output_map; +} + +void IsaacSimManusTracker::cleanup() { + std::lock_guard lock(s_instance_mutex); + if (s_instance == this) { + CoreSdk_RegisterCallbackForRawSkeletonStream(nullptr); + CoreSdk_RegisterCallbackForLandscapeStream(nullptr); + CoreSdk_RegisterCallbackForErgonomicsStream(nullptr); + DisconnectFromGloves(); + CoreSdk_ShutDown(); + s_instance = nullptr; + } +} + +void IsaacSimManusTracker::RegisterCallbacks() { + CoreSdk_RegisterCallbackForRawSkeletonStream(OnSkeletonStream); + CoreSdk_RegisterCallbackForLandscapeStream(OnLandscapeStream); + CoreSdk_RegisterCallbackForErgonomicsStream(OnErgonomicsStream); +} + +void IsaacSimManusTracker::ConnectToGloves() { + bool connected = false; + const int max_attempts = 30; // Maximum connection attempts + const auto retry_delay = std::chrono::milliseconds(1000); // 1 second delay between attempts + int attempts = 0; + + CARB_LOG_INFO("Looking for Manus gloves..."); + + while (!connected && attempts < max_attempts) { + attempts++; + + if (const auto start_result = CoreSdk_LookForHosts(1, false); + start_result != SDKReturnCode::SDKReturnCode_Success) { + CARB_LOG_ERROR("Failed to look for hosts (attempt %d/%d)", attempts, max_attempts); + std::this_thread::sleep_for(retry_delay); + continue; + } + + uint32_t number_of_hosts_found{}; + if (const auto number_result = CoreSdk_GetNumberOfAvailableHostsFound(&number_of_hosts_found); + number_result != SDKReturnCode::SDKReturnCode_Success) { + CARB_LOG_ERROR("Failed to get number of available hosts (attempt %d/%d)", attempts, max_attempts); + std::this_thread::sleep_for(retry_delay); + continue; + } + + if (number_of_hosts_found == 0) { + CARB_LOG_ERROR("Failed to find hosts (attempt %d/%d)", attempts, max_attempts); + std::this_thread::sleep_for(retry_delay); + continue; + } + + std::vector available_hosts(number_of_hosts_found); + + if (const auto hosts_result = + CoreSdk_GetAvailableHostsFound(available_hosts.data(), number_of_hosts_found); + hosts_result != SDKReturnCode::SDKReturnCode_Success) { + CARB_LOG_ERROR("Failed to get available hosts (attempt %d/%d)", attempts, max_attempts); + std::this_thread::sleep_for(retry_delay); + continue; + } + + if (const auto connect_result = CoreSdk_ConnectToHost(available_hosts[0]); + connect_result == SDKReturnCode::SDKReturnCode_NotConnected) { + CARB_LOG_ERROR("Failed to connect to host (attempt %d/%d)", attempts, max_attempts); + std::this_thread::sleep_for(retry_delay); + continue; + } + + connected = true; + is_connected = true; + CARB_LOG_INFO("Successfully connected to Manus host after %d attempts", attempts); + } + + if (!connected) { + CARB_LOG_ERROR("Failed to connect to Manus gloves after %d attempts", max_attempts); + throw std::runtime_error("Failed to connect to Manus gloves"); + } +} + +void IsaacSimManusTracker::DisconnectFromGloves() { + if (is_connected) { + CoreSdk_Disconnect(); + is_connected = false; + CARB_LOG_INFO("Disconnected from Manus gloves"); + } +} + +void IsaacSimManusTracker::OnSkeletonStream(const SkeletonStreamInfo* skeleton_stream_info) { + CARB_LOG_INFO("OnSkeletonStream callback triggered with %u skeletons", skeleton_stream_info->skeletonsCount); + std::lock_guard instance_lock(s_instance_mutex); + if (!s_instance) { + return; + } + + std::lock_guard output_lock(s_instance->output_map_mutex); + + for (uint32_t i = 0; i < skeleton_stream_info->skeletonsCount; i++) { + RawSkeletonInfo skeleton_info; + CoreSdk_GetRawSkeletonInfo(i, &skeleton_info); + + std::vector nodes(skeleton_info.nodesCount); + skeleton_info.publishTime = skeleton_stream_info->publishTime; + CoreSdk_GetRawSkeletonData(i, nodes.data(), skeleton_info.nodesCount); + + uint32_t glove_id = skeleton_info.gloveId; + + // Check if glove ID matches any known glove + bool is_left_glove, is_right_glove; + { + std::lock_guard landscape_lock(s_instance->landscape_mutex); + is_left_glove = s_instance->left_glove_id && glove_id == *s_instance->left_glove_id; + is_right_glove = s_instance->right_glove_id && glove_id == *s_instance->right_glove_id; + } + + if (!is_left_glove && !is_right_glove) { + CARB_LOG_WARN("Skipping data from unknown glove ID: %u", glove_id); + continue; + } + + std::string prefix = is_left_glove ? "left" : "right"; + + // Store position data (3 floats per node: x, y, z) + std::string pos_key = prefix + "_position"; + s_instance->output_map[pos_key].resize(skeleton_info.nodesCount * 3); + + // Store orientation data (4 floats per node: w, x, y, z) + std::string orient_key = prefix + "_orientation"; + s_instance->output_map[orient_key].resize(skeleton_info.nodesCount * 4); + + for (uint32_t j = 0; j < skeleton_info.nodesCount; j++) { + const auto& position = nodes[j].transform.position; + s_instance->output_map[pos_key][j * 3 + 0] = position.x; + s_instance->output_map[pos_key][j * 3 + 1] = position.y; + s_instance->output_map[pos_key][j * 3 + 2] = position.z; + + const auto& orientation = nodes[j].transform.rotation; + s_instance->output_map[orient_key][j * 4 + 0] = orientation.w; + s_instance->output_map[orient_key][j * 4 + 1] = orientation.x; + s_instance->output_map[orient_key][j * 4 + 2] = orientation.y; + s_instance->output_map[orient_key][j * 4 + 3] = orientation.z; + } + + CARB_LOG_INFO("Updated %s glove data with %u nodes", prefix.c_str(), skeleton_info.nodesCount); + } +} + +void IsaacSimManusTracker::OnLandscapeStream(const Landscape* landscape) { + CARB_LOG_INFO("OnLandscapeStream callback triggered"); + std::lock_guard instance_lock(s_instance_mutex); + if (!s_instance) { + return; + } + + const auto& gloves = landscape->gloveDevices; + CARB_LOG_INFO("Processing %u gloves in landscape", gloves.gloveCount); + + std::lock_guard landscape_lock(s_instance->landscape_mutex); + + // We only support one left and one right glove + if (gloves.gloveCount > 2) { + CARB_LOG_ERROR("Invalid number of gloves detected: %u", gloves.gloveCount); + return; + } + + // Extract glove IDs from landscape data + for (uint32_t i = 0; i < gloves.gloveCount; i++) { + const GloveLandscapeData& glove = gloves.gloves[i]; + if (glove.side == Side::Side_Left) { + s_instance->left_glove_id = glove.id; + CARB_LOG_INFO("Left glove detected with ID: %u", glove.id); + } else if (glove.side == Side::Side_Right) { + s_instance->right_glove_id = glove.id; + CARB_LOG_INFO("Right glove detected with ID: %u", glove.id); + } + } +} + +void IsaacSimManusTracker::OnErgonomicsStream(const ErgonomicsStream* ergonomics_stream) { + std::lock_guard instance_lock(s_instance_mutex); + if (!s_instance) { + return; + } + + std::lock_guard output_lock(s_instance->output_map_mutex); + + for (uint32_t i = 0; i < ergonomics_stream->dataCount; i++) { + if (ergonomics_stream->data[i].isUserID) continue; + + uint32_t glove_id = ergonomics_stream->data[i].id; + + // Check if glove ID matches any known glove + bool is_left_glove, is_right_glove; + { + std::lock_guard landscape_lock(s_instance->landscape_mutex); + is_left_glove = s_instance->left_glove_id && glove_id == *s_instance->left_glove_id; + is_right_glove = s_instance->right_glove_id && glove_id == *s_instance->right_glove_id; + } + + if (!is_left_glove && !is_right_glove) { + CARB_LOG_WARN("Skipping ergonomics data from unknown glove ID: %u", glove_id); + continue; + } + + std::string prefix = is_left_glove ? "left" : "right"; + std::string angle_key = prefix + "_angle"; + s_instance->output_map[angle_key].clear(); + s_instance->output_map[angle_key].reserve(ErgonomicsDataType_MAX_SIZE); + + for (int j = 0; j < ErgonomicsDataType_MAX_SIZE; j++) { + float value = ergonomics_stream->data[i].data[j]; + s_instance->output_map[angle_key].push_back(value); + } + + CARB_LOG_INFO("Updated %s glove ergonomics data", prefix.c_str()); + } +} + +} // namespace input_devices +} // namespace xr +} // namespace isaacsim \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/premake5.lua b/source/extensions/isaacsim.xr.input_devices/premake5.lua new file mode 100644 index 00000000..03749288 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/premake5.lua @@ -0,0 +1,70 @@ +-- SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +-- SPDX-License-Identifier: Apache-2.0 +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +local ext = get_current_extension_info() +project_ext(ext) + +-- Python Bindings for Carbonite Plugin +project_ext_bindings { + ext = ext, + project_name = "isaacsim.xr.input_devices.python", + module = "_isaac_xr_input_devices", + src = "bindings", + target_subdir = "isaacsim/xr/input_devices", +} +staticruntime("Off") +add_files("impl", "plugins") +add_files("iface", "include") +defines { "ISAACSIM_XR_INPUT_DEVICES_EXPORT" } + +-- Always include the basic headers +includedirs { + "%{root}/source/extensions/isaacsim.core.includes/include", + "%{root}/source/extensions/isaacsim.xr.input_devices/include", + "%{root}/source/extensions/isaacsim.xr.input_devices/plugins", +} + +-- Include ManusSDK from target-deps +includedirs { + "%{root}/_build/target-deps/manus_sdk/include", +} +libdirs { + "%{root}/_build/target-deps/manus_sdk/lib", +} +links { + "ManusSDK_Integrated", +} + +-- Include libsurvive from target-deps +includedirs { + "%{root}/_build/target-deps/libsurvive/include", +} +libdirs { + "%{root}/_build/target-deps/libsurvive/lib", +} +links { + "survive", +} + +repo_build.prebuild_link { + { "python/impl", ext.target_dir .. "/isaacsim/xr/input_devices/impl" }, + { "python/tests", ext.target_dir .. "/isaacsim/xr/input_devices/tests" }, + { "docs", ext.target_dir .. "/docs" }, + { "data", ext.target_dir .. "/data" }, +} + +repo_build.prebuild_copy { + { "python/*.py", ext.target_dir .. "/isaacsim/xr/input_devices" }, +} \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/__init__.py new file mode 100644 index 00000000..cab54238 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/__init__.py @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .impl.extension import Extension + +__all__ = ["Extension"] \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py new file mode 100644 index 00000000..4130905b --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from .extension import Extension +from .xr_device_integration import XRDeviceIntegration +from .manus_tracker import IsaacSimManusGloveTracker +from .vive_tracker import IsaacSimViveTracker + +__all__ = [ + "Extension", + "XRDeviceIntegration", + "IsaacSimManusGloveTracker", + "IsaacSimViveTracker" +] \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py b/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py new file mode 100644 index 00000000..a7ea2209 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py @@ -0,0 +1,28 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import omni.ext +import carb +from .xr_device_integration import XRDeviceIntegration + +class Extension(omni.ext.IExt): + _instance = None + + def on_startup(self, ext_id): + carb.log_info("IsaacSim XR Input Devices extension startup") + self.xr_integration = XRDeviceIntegration() + self._register_xr_devices() + Extension._instance = self + + def on_shutdown(self): + carb.log_info("IsaacSim XR Input Devices extension shutdown") + if hasattr(self, 'xr_integration'): + self.xr_integration.cleanup() + Extension._instance = None + + def _register_xr_devices(self): + self.xr_integration.register_devices() + + @classmethod + def get_instance(cls): + return cls._instance \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py new file mode 100644 index 00000000..e8be9c5a --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py @@ -0,0 +1,93 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import carb +import numpy as np +from typing import Dict, List, Optional + +try: + from isaacsim.xr.input_devices._isaac_xr_input_devices import IsaacSimManusTracker + _manus_tracker_available = True +except ImportError: + carb.log_warn("IsaacSimManusTracker not available - using mock data for manus") + _manus_tracker_available = False + +class IsaacSimManusGloveTracker: + def __init__(self): + self.glove_data = {} + self.is_connected = False + + if _manus_tracker_available: + try: + self._manus_tracker = IsaacSimManusTracker() + success = self._manus_tracker.initialize() + if success: + self.is_connected = True + carb.log_info("Manus glove tracker initialized with SDK") + else: + carb.log_warn("Failed to initialize Manus SDK - using mock data") + self._manus_tracker = None + except Exception as e: + carb.log_warn(f"Failed to initialize Manus tracker: {e} - using mock data") + self._manus_tracker = None + else: + self._manus_tracker = None + carb.log_info("Manus glove tracker initialized (mock)") + + def update(self): + if self._manus_tracker and self.is_connected: + try: + raw_data = self._manus_tracker.get_glove_data() + self.glove_data = self._convert_to_isaacsim_format(raw_data) + except Exception as e: + carb.log_error(f"Failed to update Manus glove data: {e}") + else: + # Provide mock data + self.glove_data = { + 'left_glove': { + 'position': np.array([0.0, 0.0, 0.0]), + 'orientation': np.array([1.0, 0.0, 0.0, 0.0]), + 'valid': True + }, + 'right_glove': { + 'position': np.array([0.1, 0.0, 0.0]), + 'orientation': np.array([1.0, 0.0, 0.0, 0.0]), + 'valid': True + } + } + + def _convert_to_isaacsim_format(self, raw_data: Dict) -> Dict: + isaacsim_data = {} + + if 'left_position' in raw_data and 'left_orientation' in raw_data: + isaacsim_data['left_glove'] = { + 'position': np.array(raw_data['left_position']), + 'orientation': np.array(raw_data['left_orientation']), + 'valid': True + } + + if 'right_position' in raw_data and 'right_orientation' in raw_data: + isaacsim_data['right_glove'] = { + 'position': np.array(raw_data['right_position']), + 'orientation': np.array(raw_data['right_orientation']), + 'valid': True + } + + return isaacsim_data + + def get_glove_pose(self, hand: str) -> Optional[Dict]: + glove_key = f'{hand}_glove' + if glove_key in self.glove_data: + return self.glove_data[glove_key] + return None + + def get_all_glove_data(self) -> Dict: + return self.glove_data.copy() + + def cleanup(self): + try: + if self._manus_tracker: + self._manus_tracker.cleanup() + self.is_connected = False + except Exception as e: + carb.log_error(f"Error during Manus tracker cleanup: {e}") \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py new file mode 100644 index 00000000..8c0915c9 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -0,0 +1,105 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +import carb +import numpy as np +from typing import Dict, List, Optional + +try: + # Add libsurvive pysurvive bindings to Python path + libsurvive_python_path = "/home/yuanchenl/libsurvive/bindings/python" ###FIXME: hardcoded path + if libsurvive_python_path not in sys.path: + sys.path.insert(0, libsurvive_python_path) + + import pysurvive + from pysurvive.pysurvive_generated import survive_simple_close + PYSURVIVE_AVAILABLE = True + carb.log_info("pysurvive imported successfully from libsurvive source") +except ImportError as e: + carb.log_warn(f"pysurvive not available - using mock data for vive: {e}") + PYSURVIVE_AVAILABLE = False + +class IsaacSimViveTracker: + def __init__(self): + self.device_data = {} + self.is_connected = False + + if PYSURVIVE_AVAILABLE: + try: + self._ctx = pysurvive.SimpleContext([sys.argv[0]]) + if self._ctx is None: + raise RuntimeError('Failed to initialize Survive context.') + self.is_connected = True + carb.log_info("Vive tracker initialized with pysurvive") + except Exception as e: + carb.log_warn(f"Failed to initialize Vive tracker: {e}") + else: + self._ctx = None + self.is_connected = True + carb.log_info("Vive tracker initialized (mock)") + + def update(self): + if not PYSURVIVE_AVAILABLE: + self.device_data = { + 'tracker_1_position': [0.0, 0.0, 0.0], + 'tracker_1_orientation': [1.0, 0.0, 0.0, 0.0], + 'tracker_2_position': [0.1, 0.0, 0.0], + 'tracker_2_orientation': [1.0, 0.0, 0.0, 0.0] + } + return + if not self.is_connected: + return + + try: + output_map: Dict[str, List[float]] = {} + + max_iterations = 1000 # Prevent infinite loops + iteration = 0 + while iteration < max_iterations: + updated = self._ctx.NextUpdated() + if not updated: + break + iteration += 1 + + pose_obj, _ = updated.Pose() + pos = pose_obj.Pos # (x, y, z) + rot = pose_obj.Rot # (w, x, y, z) + + device_id = updated.Name().decode('utf-8') + + # Capture ALL devices like isaac-deploy does (no filtering) + output_map[f'{device_id}_position'] = [pos[0], pos[1], pos[2]] + output_map[f'{device_id}_orientation'] = [rot[0], rot[1], rot[2], rot[3]] + carb.log_info(f"Detected device {device_id}: pos={pos}, rot={rot}") + + if output_map: + self.device_data = output_map + carb.log_info(f"Updated Vive tracker data: {list(output_map.keys())}") + + except Exception as e: + carb.log_error(f"Failed to update Vive tracker data: {e}") + + def get_tracker_pose(self, device_id: str) -> Optional[Dict]: + position_key = f'{device_id}_position' + orientation_key = f'{device_id}_orientation' + + if position_key in self.device_data and orientation_key in self.device_data: + return { + 'position': np.array(self.device_data[position_key]), + 'orientation': np.array(self.device_data[orientation_key]), + 'valid': True + } + return None + + def get_all_tracker_data(self) -> Dict: + return self.device_data.copy() + + def cleanup(self): + try: + if PYSURVIVE_AVAILABLE and hasattr(self, '_ctx') and self._ctx is not None: + carb.log_info("Cleaning up Vive tracker context") + survive_simple_close(self._ctx.ptr) + self.is_connected = False + except Exception as e: + carb.log_error(f"Error during Vive tracker cleanup: {e}") \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py new file mode 100644 index 00000000..0c954014 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -0,0 +1,87 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import carb +import time +from typing import Dict, Optional +from .manus_tracker import IsaacSimManusGloveTracker +from .vive_tracker import IsaacSimViveTracker + +def get_xr_device_integration(): + """Get the existing XRDeviceIntegration instance from the extension.""" + try: + import omni.ext + from .extension import Extension + ext_instance = Extension.get_instance() + if ext_instance and hasattr(ext_instance, 'xr_integration'): + return ext_instance.xr_integration + else: + carb.log_warn("Extension not loaded, creating new XRDeviceIntegration instance") + return XRDeviceIntegration() + except Exception as e: + carb.log_warn(f"Failed to get extension instance: {e}, creating new XRDeviceIntegration") + return XRDeviceIntegration() + +class XRDeviceIntegration: + def __init__(self): + self.manus_tracker = IsaacSimManusGloveTracker() + self.vive_tracker = IsaacSimViveTracker() + self.last_update_time = 0 + self.update_rate = 30.0 + self.device_status = { + 'manus_gloves': {'connected': False, 'last_data_time': 0}, + 'vive_trackers': {'connected': False, 'last_data_time': 0} + } + + def register_devices(self): + try: + if self.manus_tracker.is_connected: + carb.log_info("Manus gloves registered successfully") + self.device_status['manus_gloves']['connected'] = True + else: + carb.log_warn("Failed to initialize Manus gloves") + + if self.vive_tracker.is_connected: + carb.log_info("Vive trackers registered successfully") + self.device_status['vive_trackers']['connected'] = True + else: + carb.log_warn("Failed to initialize Vive trackers") + + except Exception as e: + carb.log_error(f"Failed to register XR devices: {e}") + + def update_all_devices(self): + current_time = time.time() + + if current_time - self.last_update_time >= 1.0 / self.update_rate: + try: + self.manus_tracker.update() + self.vive_tracker.update() + + self.device_status['manus_gloves']['last_data_time'] = current_time + self.device_status['vive_trackers']['last_data_time'] = current_time + + self.last_update_time = current_time + + except Exception as e: + carb.log_error(f"Failed to update XR devices: {e}") + + def get_device_data(self, device_type: str, device_id: str) -> Optional[Dict]: + if device_type == 'manus_glove': + return self.manus_tracker.get_glove_pose(device_id) + elif device_type == 'vive_tracker': + return self.vive_tracker.get_tracker_pose(device_id) + return None + + def get_all_device_data(self) -> Dict: + return { + 'manus_gloves': self.manus_tracker.get_all_glove_data(), + 'vive_trackers': self.vive_tracker.get_all_tracker_data(), + 'device_status': self.device_status + } + + def cleanup(self): + if hasattr(self, 'manus_tracker'): + self.manus_tracker.cleanup() + if hasattr(self, 'vive_tracker'): + self.vive_tracker.cleanup() \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py new file mode 100644 index 00000000..2b26afbb --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Tests for IsaacSim XR Input Devices extension. +""" \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py new file mode 100644 index 00000000..17b22e0f --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +""" +Basic test for IsaacSim XR Input Devices extension +Tests that the extension can be imported and basic functionality works +""" + +import sys +import os + +# Add the impl directory to Python path so we can import the modules directly +impl_path = os.path.join(os.path.dirname(__file__), '..', 'impl') +sys.path.insert(0, impl_path) + +def test_extension_import(): + """Test that the extension modules can be imported""" + try: + # Test importing the modules directly + from xr_device_integration import XRDeviceIntegration + from manus_tracker import IsaacSimManusGloveTracker + from vive_tracker import IsaacSimViveTracker + from extension import Extension + + print("✅ All extension modules imported successfully") + return True + except ImportError as e: + print(f"❌ Failed to import extension modules: {e}") + return False + +def test_basic_functionality(): + """Test basic functionality with mock data""" + try: + from xr_device_integration import XRDeviceIntegration + + # Create integration + integration = XRDeviceIntegration() + + # Update devices (will use mock data) + integration.update_all_devices() + + # Get data + all_data = integration.get_all_device_data() + + print("✅ Basic functionality test passed") + print(f"📊 Data keys: {list(all_data.keys())}") + + return True + + except Exception as e: + print(f"❌ Basic functionality test failed: {e}") + return False + +def test_individual_trackers(): + """Test individual tracker functionality""" + try: + from manus_tracker import IsaacSimManusGloveTracker + from vive_tracker import IsaacSimViveTracker + + # Test Manus tracker + manus_tracker = IsaacSimManusGloveTracker() + manus_tracker.update() + manus_data = manus_tracker.get_all_glove_data() + print(f"✅ Manus tracker: {len(manus_data)} glove(s)") + + # Test Vive tracker + vive_tracker = IsaacSimViveTracker() + vive_tracker.update() + vive_data = vive_tracker.get_all_tracker_data() + print(f"✅ Vive tracker: {len(vive_data)} tracker(s)") + + return True + + except Exception as e: + print(f"❌ Individual tracker test failed: {e}") + return False + +if __name__ == "__main__": + print("🚀 Testing IsaacSim XR Input Devices Extension") + print("=" * 40) + + tests = [ + ("Extension Import", test_extension_import), + ("Basic Functionality", test_basic_functionality), + ("Individual Trackers", test_individual_trackers), + ] + + passed = 0 + for test_name, test_func in tests: + print(f"\n{test_name}:") + if test_func(): + passed += 1 + + print(f"\n📊 Results: {passed}/{len(tests)} tests passed") + + if passed == len(tests): + print("🎉 Extension is working correctly!") + print("\n💡 To test with real hardware:") + print(" - Connect Manus gloves") + print(" - Connect Vive trackers") + print(" - Run IsaacSim and load the extension") + else: + print("⚠️ Some tests failed") \ No newline at end of file diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py new file mode 100644 index 00000000..0a066076 --- /dev/null +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -0,0 +1,183 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import numpy as np +from isaacsim import SimulationApp + +# Initialize simulation app +simulation_app = SimulationApp({"headless": False}) + +import carb +import omni.usd +from isaacsim.core.api import World +from isaacsim.core.api.objects import VisualCuboid +from isaacsim.core.utils.prims import create_prim, set_prim_visibility +from omni.isaac.core.prims import XFormPrim +from pxr import Gf, Sdf, UsdGeom, UsdLux + +# Import our XR device integration +try: + from isaacsim.xr.input_devices.impl.xr_device_integration import get_xr_device_integration + carb.log_info("Successfully imported XR device integration helper") +except ImportError as e: + carb.log_error(f"Failed to import XR device integration: {e}") + simulation_app.close() + exit(1) + +# Create world and lighting +my_world = World(stage_units_in_meters=1.0) + +# Add Light Source +stage = omni.usd.get_context().get_stage() +distantLight = UsdLux.DistantLight.Define(stage, Sdf.Path("/DistantLight")) +distantLight.CreateIntensityAttr(300) + +# Create red cube prototype +hidden_prim = create_prim("/Hidden/Prototypes", "Scope") +base_cube_path = "/Hidden/Prototypes/BaseCube" +VisualCuboid( + prim_path=base_cube_path, + size=0.02, # Slightly larger cubes for better visibility + color=np.array([255, 0, 0]), # Red color +) +set_prim_visibility(hidden_prim, False) + +# Create point instancer for cubes +instancer_path = "/World/DeviceCubeInstancer" +point_instancer = UsdGeom.PointInstancer.Define(my_world.stage, instancer_path) +point_instancer.CreatePrototypesRel().SetTargets([Sdf.Path(base_cube_path)]) + +# Calculate total device count (2 manus gloves + variable vive trackers, let's assume max 4 trackers) +max_devices = 6 # 2 manus gloves + up to 4 vive trackers +device_count = max_devices + +# Initially hide all cubes until devices are tracked +point_instancer.CreateProtoIndicesAttr().Set([1 for _ in range(device_count)]) + +# Initialize positions and orientations +positions = [Gf.Vec3f(0.0, 0.0, 0.0) for i in range(device_count)] +point_instancer.CreatePositionsAttr().Set(positions) + +orientations = [Gf.Quath(1.0, 0.0, 0.0, 0.0) for _ in range(device_count)] +point_instancer.CreateOrientationsAttr().Set(orientations) + +# Add instancer to world scene +instancer_prim = XFormPrim(prim_path=instancer_path) +my_world.scene.add(instancer_prim) + +# Get XR device integration from extension (to avoid singleton conflicts) +xr_integration = get_xr_device_integration() +carb.log_info("Using XR device integration from extension") + +my_world.reset() +reset_needed = False + +# Get attribute references for faster access +positions_attr = point_instancer.GetPositionsAttr() +orientations_attr = point_instancer.GetOrientationsAttr() +proto_idx_attr = point_instancer.GetProtoIndicesAttr() + +carb.log_info("Starting Manus Glove and Vive Tracker visualization") +carb.log_info("Red cubes will appear at device positions when data is available") + +# Main simulation loop +while simulation_app.is_running(): + my_world.step(render=True) + if my_world.is_stopped() and not reset_needed: + reset_needed = True + if my_world.is_playing(): + if reset_needed: + my_world.reset() + reset_needed = False + + # Update device data + xr_integration.update_all_devices() + + # Get current cube arrays + current_positions = positions_attr.Get() + current_orientations = orientations_attr.Get() + proto_indices = proto_idx_attr.Get() + + # Initially hide all cubes + proto_indices = [1 for _ in range(device_count)] + cube_idx = 0 + + # Get all device data + all_device_data = xr_integration.get_all_device_data() + + # Update manus glove positions + manus_data = all_device_data.get('manus_gloves', {}) + + # Left glove + if 'left_glove' in manus_data and cube_idx < device_count: + glove_data = manus_data['left_glove'] + if glove_data.get('valid', False): + pos = glove_data['position'] + ori = glove_data['orientation'] + + current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) + current_orientations[cube_idx] = Gf.Quath( + float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) + ) + proto_indices[cube_idx] = 0 # Show cube + cube_idx += 1 + + # Right glove + if 'right_glove' in manus_data and cube_idx < device_count: + glove_data = manus_data['right_glove'] + if glove_data.get('valid', False): + pos = glove_data['position'] + ori = glove_data['orientation'] + + current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) + current_orientations[cube_idx] = Gf.Quath( + float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) + ) + proto_indices[cube_idx] = 0 # Show cube + cube_idx += 1 + + # Update vive tracker positions + vive_data = all_device_data.get('vive_trackers', {}) + + # Process all vive tracker devices + for device_key, device_values in vive_data.items(): + if cube_idx >= device_count: + break + + if device_key.endswith('_position'): + # Extract device name (remove '_position' suffix) + device_name = device_key[:-9] + orientation_key = f"{device_name}_orientation" + + if orientation_key in vive_data: + pos = device_values + ori = vive_data[orientation_key] + + current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) + current_orientations[cube_idx] = Gf.Quath( + float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) + ) + proto_indices[cube_idx] = 0 # Show cube + cube_idx += 1 + + # Update the instancer with new positions and orientations + positions_attr.Set(current_positions) + orientations_attr.Set(current_orientations) + proto_idx_attr.Set(proto_indices) + +# Cleanup +xr_integration.cleanup() +simulation_app.close() \ No newline at end of file From bfc6cf5abb2992e3e7c129d833e06a79a44dfc1d Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 19 Aug 2025 10:13:04 -0700 Subject: [PATCH 2/9] coordinate transformation --- .../python/impl/manus_tracker.py | 106 +++++++++++++----- .../python/impl/vive_tracker.py | 85 ++++++++++---- .../python/impl/xr_device_integration.py | 27 +++-- .../manus_vive_tracking_sample.py | 97 +++++++--------- 4 files changed, 190 insertions(+), 125 deletions(-) diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py index e8be9c5a..e48049d7 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py @@ -4,6 +4,8 @@ import carb import numpy as np from typing import Dict, List, Optional +from pxr import Gf +from omni.kit.xr.core import XRCore try: from isaacsim.xr.input_devices._isaac_xr_input_devices import IsaacSimManusTracker @@ -12,6 +14,11 @@ carb.log_warn("IsaacSimManusTracker not available - using mock data for manus") _manus_tracker_available = False +hmd_pose = ((0.996499240398407, -0.06362161040306091, 0.054237786680459976, 0), + (-0.06642970442771912, -0.20866860449314117, 0.9757277965545654, 0), + (-0.05075964331626892, -0.9759148359298706, -0.21216443181037903, 0), + (0.000521781446877867, 0.19946864247322083, 1.4816499948501587, 1)) + class IsaacSimManusGloveTracker: def __init__(self): self.glove_data = {} @@ -38,51 +45,94 @@ def update(self): if self._manus_tracker and self.is_connected: try: raw_data = self._manus_tracker.get_glove_data() - self.glove_data = self._convert_to_isaacsim_format(raw_data) + for hand in ['left', 'right']: + self._populate_glove_data(raw_data, hand) + except Exception as e: carb.log_error(f"Failed to update Manus glove data: {e}") else: # Provide mock data self.glove_data = { - 'left_glove': { + 'left_joint_0': { 'position': np.array([0.0, 0.0, 0.0]), - 'orientation': np.array([1.0, 0.0, 0.0, 0.0]), - 'valid': True + 'orientation': np.array([1.0, 0.0, 0.0, 0.0]) }, - 'right_glove': { + 'right_joint_0': { 'position': np.array([0.1, 0.0, 0.0]), - 'orientation': np.array([1.0, 0.0, 0.0, 0.0]), - 'valid': True + 'orientation': np.array([1.0, 0.0, 0.0, 0.0]) } } - def _convert_to_isaacsim_format(self, raw_data: Dict) -> Dict: - isaacsim_data = {} + def _populate_glove_data(self, raw_data: Dict, hand: str) -> None: + """ + Convert raw Manus data to Isaac Sim format with individual joint entries. + Adds to self.glove_data: {joint_name: {position: pos, orientation: ori}} + """ + if f'{hand}_position' not in raw_data or f'{hand}_orientation' not in raw_data: + # carb.log_error(f"No {hand} position or orientation data found") + return - if 'left_position' in raw_data and 'left_orientation' in raw_data: - isaacsim_data['left_glove'] = { - 'position': np.array(raw_data['left_position']), - 'orientation': np.array(raw_data['left_orientation']), - 'valid': True + position = raw_data[f'{hand}_position'] + orientation = raw_data[f'{hand}_orientation'] + joint_count = len(position) // 3 # 3 values per position + + for joint_idx in range(joint_count): + # Extract joint position (x, y, z) + pos_start = joint_idx * 3 + joint_pos = np.array([float(position[pos_start]), float(position[pos_start + 1]), float(position[pos_start + 2])]) + + # Extract joint orientation (w, x, y, z) + ori_start = joint_idx * 4 + joint_ori = np.array([float(orientation[ori_start]), float(orientation[ori_start + 1]), + float(orientation[ori_start + 2]), float(orientation[ori_start + 3])]) + + # Transform coordinates + transformed_pos, transformed_ori = self._transform_manus_coordinates(joint_pos, joint_ori) + joint_name = f"{hand}_joint_{joint_idx}" + + self.glove_data[joint_name] = { + 'position': transformed_pos, + 'orientation': transformed_ori } + + def _transform_manus_coordinates(self, manus_pos: np.ndarray, manus_ori: np.ndarray) -> tuple: + """ + Transform Manus glove coordinates to Isaac Sim coordinate system. + Uses HMD pose as reference to align with stage coordinate system. + """ + # Create transformation matrix from Manus pose + manus_quat = Gf.Quatd(manus_ori[0], Gf.Vec3d(manus_ori[1], manus_ori[2], manus_ori[3])) + manus_rot = Gf.Matrix3d().SetRotate(manus_quat) + manus_trans = Gf.Vec3d(manus_pos[0], manus_pos[1], manus_pos[2]) + manus_matrix = Gf.Matrix4d(manus_rot, manus_trans) - if 'right_position' in raw_data and 'right_orientation' in raw_data: - isaacsim_data['right_glove'] = { - 'position': np.array(raw_data['right_position']), - 'orientation': np.array(raw_data['right_orientation']), - 'valid': True - } + # Apply coordinate system transformation: Z-up (Manus) to Y-up (Isaac Sim) + coord_transform = Gf.Matrix4d().SetRotate(Gf.Quatd(0.7071068, Gf.Vec3d(0.7071068, 0.0, 0.0))) # 90° around X - return isaacsim_data - - def get_glove_pose(self, hand: str) -> Optional[Dict]: - glove_key = f'{hand}_glove' - if glove_key in self.glove_data: - return self.glove_data[glove_key] - return None + # Apply coordinate system transformation: Z-up (Manus) to Y-up (Isaac Sim) + transformed_matrix = manus_matrix * coord_transform + + # Use HMD pose to align with the same coordinate system as OpenXR hand tracking + # The HMD pose represents where the stage coordinate system is relative to our devices + hmd_transform = Gf.Matrix4d(hmd_pose) + + # Apply the same transformation that OpenXR.cpp applies internally + # This aligns our external devices with the stage coordinate system + transformed_matrix = transformed_matrix * hmd_transform + + # Extract transformed position and orientation + transformed_pos = transformed_matrix.ExtractTranslation() + transformed_rot = transformed_matrix.ExtractRotation() + transformed_quat = transformed_rot.GetQuat() + + return ( + np.array([transformed_pos[0], transformed_pos[1], transformed_pos[2]]), + np.array([transformed_quat.GetReal(), transformed_quat.GetImaginary()[0], + transformed_quat.GetImaginary()[1], transformed_quat.GetImaginary()[2]]) + ) def get_all_glove_data(self) -> Dict: - return self.glove_data.copy() + return self.glove_data def cleanup(self): try: diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index 8c0915c9..c0587f4f 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -5,6 +5,8 @@ import carb import numpy as np from typing import Dict, List, Optional +from pxr import Gf +from omni.kit.xr.core import XRCore try: # Add libsurvive pysurvive bindings to Python path @@ -20,6 +22,12 @@ carb.log_warn(f"pysurvive not available - using mock data for vive: {e}") PYSURVIVE_AVAILABLE = False +# Hardcoded HMD pose matrix from XRCore (logged from head_device.get_virtual_world_pose("")) +hmd_pose = ((0.996499240398407, -0.06362161040306091, 0.054237786680459976, 0), + (-0.06642970442771912, -0.20866860449314117, 0.9757277965545654, 0), + (-0.05075964331626892, -0.9759148359298706, -0.21216443181037903, 0), + (0.000521781446877867, 0.19946864247322083, 1.4816499948501587, 1)) + class IsaacSimViveTracker: def __init__(self): self.device_data = {} @@ -42,18 +50,20 @@ def __init__(self): def update(self): if not PYSURVIVE_AVAILABLE: self.device_data = { - 'tracker_1_position': [0.0, 0.0, 0.0], - 'tracker_1_orientation': [1.0, 0.0, 0.0, 0.0], - 'tracker_2_position': [0.1, 0.0, 0.0], - 'tracker_2_orientation': [1.0, 0.0, 0.0, 0.0] + 'tracker_1': { + 'position': [0.0, 0.0, 0.0], + 'orientation': [1.0, 0.0, 0.0, 0.0] + }, + 'tracker_2': { + 'position': [0.1, 0.0, 0.0], + 'orientation': [1.0, 0.0, 0.0, 0.0] + } } return if not self.is_connected: return try: - output_map: Dict[str, List[float]] = {} - max_iterations = 1000 # Prevent infinite loops iteration = 0 while iteration < max_iterations: @@ -68,32 +78,59 @@ def update(self): device_id = updated.Name().decode('utf-8') + # Transform Vive coordinates to Isaac Sim coordinate system + transformed_pos, transformed_ori = self._transform_vive_coordinates(pos, rot) + # Capture ALL devices like isaac-deploy does (no filtering) - output_map[f'{device_id}_position'] = [pos[0], pos[1], pos[2]] - output_map[f'{device_id}_orientation'] = [rot[0], rot[1], rot[2], rot[3]] - carb.log_info(f"Detected device {device_id}: pos={pos}, rot={rot}") + self.device_data[device_id] = { + 'position': transformed_pos, + 'orientation': transformed_ori + } - if output_map: - self.device_data = output_map - carb.log_info(f"Updated Vive tracker data: {list(output_map.keys())}") - except Exception as e: carb.log_error(f"Failed to update Vive tracker data: {e}") - def get_tracker_pose(self, device_id: str) -> Optional[Dict]: - position_key = f'{device_id}_position' - orientation_key = f'{device_id}_orientation' + + def _transform_vive_coordinates(self, position: np.ndarray, orientation: np.ndarray) -> tuple: + """ + Transform Vive tracker coordinates to Isaac Sim coordinate system. + Attempts to match the C++ OpenXR approach using available XRCore APIs. + """ + # Create transformation matrix from Vive pose + vive_quat = Gf.Quatd(orientation[0], Gf.Vec3d(orientation[1], orientation[2], orientation[3])) + vive_rot = Gf.Matrix3d().SetRotate(vive_quat) + vive_trans = Gf.Vec3d(position[0], position[1], position[2]) + vive_matrix = Gf.Matrix4d(vive_rot, vive_trans) - if position_key in self.device_data and orientation_key in self.device_data: - return { - 'position': np.array(self.device_data[position_key]), - 'orientation': np.array(self.device_data[orientation_key]), - 'valid': True - } - return None + # Apply basic transformations for Vive trackers + # Vive uses Y-up like Isaac Sim, so minimal transformation needed + scale_transform = Gf.Matrix4d().SetScale(1.0) # Adjust scale if needed + offset_transform = Gf.Matrix4d().SetTranslate(Gf.Vec3d(0.0, 0.0, 0.0)) # Adjust offset if needed + + # Apply the transformations + transformed_matrix = vive_matrix * scale_transform * offset_transform + + # Use HMD pose to align with the same coordinate system as OpenXR hand tracking + # The HMD pose represents where the stage coordinate system is relative to our devices + hmd_transform = Gf.Matrix4d(hmd_pose) + + # Apply the same transformation that OpenXR.cpp applies internally + # This aligns our external devices with the stage coordinate system + transformed_matrix = transformed_matrix * hmd_transform + + # Extract transformed position and orientation + transformed_pos = transformed_matrix.ExtractTranslation() + transformed_rot = transformed_matrix.ExtractRotation() + transformed_quat = transformed_rot.GetQuat() + + return ( + [transformed_pos[0], transformed_pos[1], transformed_pos[2]], + [transformed_quat.GetReal(), transformed_quat.GetImaginary()[0], + transformed_quat.GetImaginary()[1], transformed_quat.GetImaginary()[2]] + ) def get_all_tracker_data(self) -> Dict: - return self.device_data.copy() + return self.device_data def cleanup(self): try: diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index 0c954014..cec606da 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -27,7 +27,7 @@ def __init__(self): self.manus_tracker = IsaacSimManusGloveTracker() self.vive_tracker = IsaacSimViveTracker() self.last_update_time = 0 - self.update_rate = 30.0 + self.update_rate = 100.0 self.device_status = { 'manus_gloves': {'connected': False, 'last_data_time': 0}, 'vive_trackers': {'connected': False, 'last_data_time': 0} @@ -50,29 +50,28 @@ def register_devices(self): except Exception as e: carb.log_error(f"Failed to register XR devices: {e}") + def update_manus(self): + current_time = time.time() + self.manus_tracker.update() + self.device_status['manus_gloves']['last_data_time'] = current_time + + def update_vive(self): + current_time = time.time() + self.vive_tracker.update() + self.device_status['vive_trackers']['last_data_time'] = current_time + def update_all_devices(self): current_time = time.time() if current_time - self.last_update_time >= 1.0 / self.update_rate: try: - self.manus_tracker.update() - self.vive_tracker.update() - - self.device_status['manus_gloves']['last_data_time'] = current_time - self.device_status['vive_trackers']['last_data_time'] = current_time - + self.update_manus() + self.update_vive() self.last_update_time = current_time except Exception as e: carb.log_error(f"Failed to update XR devices: {e}") - def get_device_data(self, device_type: str, device_id: str) -> Optional[Dict]: - if device_type == 'manus_glove': - return self.manus_tracker.get_glove_pose(device_id) - elif device_type == 'vive_tracker': - return self.vive_tracker.get_tracker_pose(device_id) - return None - def get_all_device_data(self) -> Dict: return { 'manus_gloves': self.manus_tracker.get_all_glove_data(), diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 0a066076..55b41647 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -50,7 +50,7 @@ base_cube_path = "/Hidden/Prototypes/BaseCube" VisualCuboid( prim_path=base_cube_path, - size=0.02, # Slightly larger cubes for better visibility + size=0.01, color=np.array([255, 0, 0]), # Red color ) set_prim_visibility(hidden_prim, False) @@ -60,18 +60,16 @@ point_instancer = UsdGeom.PointInstancer.Define(my_world.stage, instancer_path) point_instancer.CreatePrototypesRel().SetTargets([Sdf.Path(base_cube_path)]) -# Calculate total device count (2 manus gloves + variable vive trackers, let's assume max 4 trackers) -max_devices = 6 # 2 manus gloves + up to 4 vive trackers -device_count = max_devices +max_devices = 60 # Initially hide all cubes until devices are tracked -point_instancer.CreateProtoIndicesAttr().Set([1 for _ in range(device_count)]) +point_instancer.CreateProtoIndicesAttr().Set([1 for _ in range(max_devices)]) # Initialize positions and orientations -positions = [Gf.Vec3f(0.0, 0.0, 0.0) for i in range(device_count)] +positions = [Gf.Vec3f(0.0, 0.0, 0.0) for i in range(max_devices)] point_instancer.CreatePositionsAttr().Set(positions) -orientations = [Gf.Quath(1.0, 0.0, 0.0, 0.0) for _ in range(device_count)] +orientations = [Gf.Quath(1.0, 0.0, 0.0, 0.0) for _ in range(max_devices)] point_instancer.CreateOrientationsAttr().Set(orientations) # Add instancer to world scene @@ -85,12 +83,15 @@ my_world.reset() reset_needed = False +# Frame counter for sequential device updates +frame_counter = 0 + # Get attribute references for faster access positions_attr = point_instancer.GetPositionsAttr() orientations_attr = point_instancer.GetOrientationsAttr() proto_idx_attr = point_instancer.GetProtoIndicesAttr() -carb.log_info("Starting Manus Glove and Vive Tracker visualization") +carb.log_info("Starting Manus Glove and Vive Tracker visualization with sequential updates") carb.log_info("Red cubes will appear at device positions when data is available") # Main simulation loop @@ -103,30 +104,39 @@ my_world.reset() reset_needed = False - # Update device data - xr_integration.update_all_devices() + # Sequential device updates to avoid resource contention + # Alternate between Manus and Vive every frame + update_manus = (frame_counter % 2 == 0) + frame_counter += 1 # Get current cube arrays current_positions = positions_attr.Get() current_orientations = orientations_attr.Get() proto_indices = proto_idx_attr.Get() - # Initially hide all cubes - proto_indices = [1 for _ in range(device_count)] + # Initialize all cubes as hidden (following hand_tracking_sample pattern) + proto_indices = [1 for _ in range(max_devices)] + proto_idx_attr.Set(proto_indices) cube_idx = 0 - - # Get all device data - all_device_data = xr_integration.get_all_device_data() - # Update manus glove positions - manus_data = all_device_data.get('manus_gloves', {}) + if update_manus: + # Update manus glove positions + xr_integration.update_manus() + else: + # Update vive tracker positions + xr_integration.update_vive() - # Left glove - if 'left_glove' in manus_data and cube_idx < device_count: - glove_data = manus_data['left_glove'] - if glove_data.get('valid', False): - pos = glove_data['position'] - ori = glove_data['orientation'] + all_device_data = xr_integration.get_all_device_data() + manus_data = all_device_data.get('manus_gloves', {}) + vive_data = all_device_data.get('vive_trackers', {}) + + # Process all devices + for device_data in [manus_data, vive_data]: + for joint, joint_data in device_data.items(): + if cube_idx >= max_devices: + break + pos = joint_data['position'] + ori = joint_data['orientation'] current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) current_orientations[cube_idx] = Gf.Quath( @@ -135,44 +145,13 @@ proto_indices[cube_idx] = 0 # Show cube cube_idx += 1 - # Right glove - if 'right_glove' in manus_data and cube_idx < device_count: - glove_data = manus_data['right_glove'] - if glove_data.get('valid', False): - pos = glove_data['position'] - ori = glove_data['orientation'] - - current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) - current_orientations[cube_idx] = Gf.Quath( - float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) - ) - proto_indices[cube_idx] = 0 # Show cube - cube_idx += 1 + # Debug: Log cube visibility and positions + visible_cubes = sum(1 for idx in proto_indices if idx == 0) - # Update vive tracker positions - vive_data = all_device_data.get('vive_trackers', {}) + if frame_counter % 100 == 0: + carb.log_info(f"Showing {visible_cubes} cubes at positions: {current_positions[:visible_cubes]}") + carb.log_info(f"Frame {frame_counter}, data: {all_device_data}") - # Process all vive tracker devices - for device_key, device_values in vive_data.items(): - if cube_idx >= device_count: - break - - if device_key.endswith('_position'): - # Extract device name (remove '_position' suffix) - device_name = device_key[:-9] - orientation_key = f"{device_name}_orientation" - - if orientation_key in vive_data: - pos = device_values - ori = vive_data[orientation_key] - - current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) - current_orientations[cube_idx] = Gf.Quath( - float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) - ) - proto_indices[cube_idx] = 0 # Show cube - cube_idx += 1 - # Update the instancer with new positions and orientations positions_attr.Set(current_positions) orientations_attr.Set(current_orientations) From e64a0091286ee69c278c5b50d403d3445d270148 Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 19 Aug 2025 18:19:13 -0700 Subject: [PATCH 3/9] wrist tracking --- .../python/impl/manus_tracker.py | 71 +--------- .../python/impl/vive_tracker.py | 61 +-------- .../python/impl/xr_device_integration.py | 126 +++++++++++++++++- .../manus_vive_tracking_sample.py | 64 +++++---- 4 files changed, 174 insertions(+), 148 deletions(-) diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py index e48049d7..30779152 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py @@ -2,10 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 import carb -import numpy as np -from typing import Dict, List, Optional -from pxr import Gf -from omni.kit.xr.core import XRCore +from typing import Dict try: from isaacsim.xr.input_devices._isaac_xr_input_devices import IsaacSimManusTracker @@ -14,11 +11,6 @@ carb.log_warn("IsaacSimManusTracker not available - using mock data for manus") _manus_tracker_available = False -hmd_pose = ((0.996499240398407, -0.06362161040306091, 0.054237786680459976, 0), - (-0.06642970442771912, -0.20866860449314117, 0.9757277965545654, 0), - (-0.05075964331626892, -0.9759148359298706, -0.21216443181037903, 0), - (0.000521781446877867, 0.19946864247322083, 1.4816499948501587, 1)) - class IsaacSimManusGloveTracker: def __init__(self): self.glove_data = {} @@ -54,12 +46,12 @@ def update(self): # Provide mock data self.glove_data = { 'left_joint_0': { - 'position': np.array([0.0, 0.0, 0.0]), - 'orientation': np.array([1.0, 0.0, 0.0, 0.0]) + 'position': [0.0, 0.0, 0.0], + 'orientation': [1.0, 0.0, 0.0, 0.0] }, 'right_joint_0': { - 'position': np.array([0.1, 0.0, 0.0]), - 'orientation': np.array([1.0, 0.0, 0.0, 0.0]) + 'position': [0.1, 0.0, 0.0], + 'orientation': [1.0, 0.0, 0.0, 0.0] } } @@ -69,7 +61,6 @@ def _populate_glove_data(self, raw_data: Dict, hand: str) -> None: Adds to self.glove_data: {joint_name: {position: pos, orientation: ori}} """ if f'{hand}_position' not in raw_data or f'{hand}_orientation' not in raw_data: - # carb.log_error(f"No {hand} position or orientation data found") return position = raw_data[f'{hand}_position'] @@ -77,60 +68,12 @@ def _populate_glove_data(self, raw_data: Dict, hand: str) -> None: joint_count = len(position) // 3 # 3 values per position for joint_idx in range(joint_count): - # Extract joint position (x, y, z) - pos_start = joint_idx * 3 - joint_pos = np.array([float(position[pos_start]), float(position[pos_start + 1]), float(position[pos_start + 2])]) - - # Extract joint orientation (w, x, y, z) - ori_start = joint_idx * 4 - joint_ori = np.array([float(orientation[ori_start]), float(orientation[ori_start + 1]), - float(orientation[ori_start + 2]), float(orientation[ori_start + 3])]) - - # Transform coordinates - transformed_pos, transformed_ori = self._transform_manus_coordinates(joint_pos, joint_ori) joint_name = f"{hand}_joint_{joint_idx}" - self.glove_data[joint_name] = { - 'position': transformed_pos, - 'orientation': transformed_ori + 'position': [float(position[i]) for i in range(joint_idx * 3, joint_idx * 3 + 3)], + 'orientation': [float(orientation[i]) for i in range(joint_idx * 4, joint_idx * 4 + 4)] } - def _transform_manus_coordinates(self, manus_pos: np.ndarray, manus_ori: np.ndarray) -> tuple: - """ - Transform Manus glove coordinates to Isaac Sim coordinate system. - Uses HMD pose as reference to align with stage coordinate system. - """ - # Create transformation matrix from Manus pose - manus_quat = Gf.Quatd(manus_ori[0], Gf.Vec3d(manus_ori[1], manus_ori[2], manus_ori[3])) - manus_rot = Gf.Matrix3d().SetRotate(manus_quat) - manus_trans = Gf.Vec3d(manus_pos[0], manus_pos[1], manus_pos[2]) - manus_matrix = Gf.Matrix4d(manus_rot, manus_trans) - - # Apply coordinate system transformation: Z-up (Manus) to Y-up (Isaac Sim) - coord_transform = Gf.Matrix4d().SetRotate(Gf.Quatd(0.7071068, Gf.Vec3d(0.7071068, 0.0, 0.0))) # 90° around X - - # Apply coordinate system transformation: Z-up (Manus) to Y-up (Isaac Sim) - transformed_matrix = manus_matrix * coord_transform - - # Use HMD pose to align with the same coordinate system as OpenXR hand tracking - # The HMD pose represents where the stage coordinate system is relative to our devices - hmd_transform = Gf.Matrix4d(hmd_pose) - - # Apply the same transformation that OpenXR.cpp applies internally - # This aligns our external devices with the stage coordinate system - transformed_matrix = transformed_matrix * hmd_transform - - # Extract transformed position and orientation - transformed_pos = transformed_matrix.ExtractTranslation() - transformed_rot = transformed_matrix.ExtractRotation() - transformed_quat = transformed_rot.GetQuat() - - return ( - np.array([transformed_pos[0], transformed_pos[1], transformed_pos[2]]), - np.array([transformed_quat.GetReal(), transformed_quat.GetImaginary()[0], - transformed_quat.GetImaginary()[1], transformed_quat.GetImaginary()[2]]) - ) - def get_all_glove_data(self) -> Dict: return self.glove_data diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index c0587f4f..03624937 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -3,10 +3,7 @@ import sys import carb -import numpy as np -from typing import Dict, List, Optional -from pxr import Gf -from omni.kit.xr.core import XRCore +from typing import Dict try: # Add libsurvive pysurvive bindings to Python path @@ -22,12 +19,6 @@ carb.log_warn(f"pysurvive not available - using mock data for vive: {e}") PYSURVIVE_AVAILABLE = False -# Hardcoded HMD pose matrix from XRCore (logged from head_device.get_virtual_world_pose("")) -hmd_pose = ((0.996499240398407, -0.06362161040306091, 0.054237786680459976, 0), - (-0.06642970442771912, -0.20866860449314117, 0.9757277965545654, 0), - (-0.05075964331626892, -0.9759148359298706, -0.21216443181037903, 0), - (0.000521781446877867, 0.19946864247322083, 1.4816499948501587, 1)) - class IsaacSimViveTracker: def __init__(self): self.device_data = {} @@ -74,60 +65,18 @@ def update(self): pose_obj, _ = updated.Pose() pos = pose_obj.Pos # (x, y, z) - rot = pose_obj.Rot # (w, x, y, z) - + ori = pose_obj.Rot # (w, x, y, z) device_id = updated.Name().decode('utf-8') - # Transform Vive coordinates to Isaac Sim coordinate system - transformed_pos, transformed_ori = self._transform_vive_coordinates(pos, rot) - # Capture ALL devices like isaac-deploy does (no filtering) self.device_data[device_id] = { - 'position': transformed_pos, - 'orientation': transformed_ori + 'position': [float(pos[i]) for i in range(3)], + 'orientation': [float(ori[i]) for i in range(4)] } except Exception as e: carb.log_error(f"Failed to update Vive tracker data: {e}") - - - def _transform_vive_coordinates(self, position: np.ndarray, orientation: np.ndarray) -> tuple: - """ - Transform Vive tracker coordinates to Isaac Sim coordinate system. - Attempts to match the C++ OpenXR approach using available XRCore APIs. - """ - # Create transformation matrix from Vive pose - vive_quat = Gf.Quatd(orientation[0], Gf.Vec3d(orientation[1], orientation[2], orientation[3])) - vive_rot = Gf.Matrix3d().SetRotate(vive_quat) - vive_trans = Gf.Vec3d(position[0], position[1], position[2]) - vive_matrix = Gf.Matrix4d(vive_rot, vive_trans) - - # Apply basic transformations for Vive trackers - # Vive uses Y-up like Isaac Sim, so minimal transformation needed - scale_transform = Gf.Matrix4d().SetScale(1.0) # Adjust scale if needed - offset_transform = Gf.Matrix4d().SetTranslate(Gf.Vec3d(0.0, 0.0, 0.0)) # Adjust offset if needed - - # Apply the transformations - transformed_matrix = vive_matrix * scale_transform * offset_transform - - # Use HMD pose to align with the same coordinate system as OpenXR hand tracking - # The HMD pose represents where the stage coordinate system is relative to our devices - hmd_transform = Gf.Matrix4d(hmd_pose) - - # Apply the same transformation that OpenXR.cpp applies internally - # This aligns our external devices with the stage coordinate system - transformed_matrix = transformed_matrix * hmd_transform - - # Extract transformed position and orientation - transformed_pos = transformed_matrix.ExtractTranslation() - transformed_rot = transformed_matrix.ExtractRotation() - transformed_quat = transformed_rot.GetQuat() - - return ( - [transformed_pos[0], transformed_pos[1], transformed_pos[2]], - [transformed_quat.GetReal(), transformed_quat.GetImaginary()[0], - transformed_quat.GetImaginary()[1], transformed_quat.GetImaginary()[2]] - ) + def get_all_tracker_data(self) -> Dict: return self.device_data diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index cec606da..e296d881 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -3,10 +3,21 @@ import carb import time -from typing import Dict, Optional +import numpy as np +from typing import Dict +from pxr import Gf from .manus_tracker import IsaacSimManusGloveTracker from .vive_tracker import IsaacSimViveTracker +# Left wrist pose from AVP hand tracking +scene_T_wrist_static = Gf.Matrix4d( + (0.9534056782722473, -0.30112600326538086, -0.018459070473909378, 0), + (-0.104372538626194, -0.38662755489349365, 0.9163107872009277, 0), + (-0.28306180238723755, -0.8716893196105957, -0.40004217624664307, 0), + (-0.1019858792424202, 0.3140803277492523, 1.285851240158081, 1) +) +scene_T_wrist_static = Gf.Matrix4d().SetIdentity() + def get_xr_device_integration(): """Get the existing XRDeviceIntegration instance from the extension.""" try: @@ -32,6 +43,7 @@ def __init__(self): 'manus_gloves': {'connected': False, 'last_data_time': 0}, 'vive_trackers': {'connected': False, 'last_data_time': 0} } + self.scene_T_lighthouse_static = None # Coordinate transformation matrix, will be computed from first Vive wrist position def register_devices(self): try: @@ -51,15 +63,23 @@ def register_devices(self): carb.log_error(f"Failed to register XR devices: {e}") def update_manus(self): + """Update raw Manus glove data. """ current_time = time.time() self.manus_tracker.update() self.device_status['manus_gloves']['last_data_time'] = current_time def update_vive(self): + """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update. """ current_time = time.time() self.vive_tracker.update() self.device_status['vive_trackers']['last_data_time'] = current_time - + try: + # Initialize coordinate transformation from first Vive wrist position + if self.scene_T_lighthouse_static is None: + self._initialize_coordinate_transformation() + except Exception as e: + carb.log_error(f"Vive tracker update failed: {e}") + def update_all_devices(self): current_time = time.time() @@ -71,14 +91,112 @@ def update_all_devices(self): except Exception as e: carb.log_error(f"Failed to update XR devices: {e}") + + def pose_to_matrix(self, pose: Dict) -> Gf.Matrix4d: + """Convert position and orientation to a 4x4 matrix.""" + pos, ori = pose['position'], pose['orientation'] + quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) + rot = Gf.Matrix3d().SetRotate(quat) + trans = Gf.Vec3d(pos[0], pos[1], pos[2]) + return Gf.Matrix4d(rot, trans) + + def matrix_to_pose(self, matrix: Gf.Matrix4d) -> tuple: + """Convert a 4x4 matrix to position and orientation.""" + pos = matrix.ExtractTranslation() + rot = matrix.ExtractRotation() + quat = rot.GetQuat() + return { + 'position': [pos[0], pos[1], pos[2]], + 'orientation': [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]] + } + + def _initialize_coordinate_transformation(self): + """ + Initialize the scene to lighthouse coordinate transformation. + The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. + It is computed from a hard-coded AVP wrist pose and the first Vive wrist pose. + """ + vive_data = self.vive_tracker.get_all_tracker_data() + # Look for a wrist tracker in the Vive data + left_wrist = 'WM0' # FIXME: hardcoded - assuming that WM0 is the left wrist tracker + list_trackers = [] + if left_wrist not in vive_data: + for device_key in vive_data.keys(): + list_trackers.append(device_key) + if len(device_key) >= 2 and device_key[:2] == 'WM': + left_wrist = device_key + carb.log_warn(f"Using Vive tracker '{left_wrist}' for coordinate transformation") + break + + if left_wrist not in vive_data: + carb.log_warn(f"No Vive wrist tracker found for coordinate transformation. Trackers: {list_trackers}") + return + + try: + lighthouse_T_wrist = self.pose_to_matrix(vive_data[left_wrist]) + wrist_T_lighthouse_static = lighthouse_T_wrist.GetInverse() + self.scene_T_lighthouse_static = wrist_T_lighthouse_static * scene_T_wrist_static + + except Exception as e: + carb.log_error(f"Failed to initialize coordinate transformation: {e}") + + def _transform_to_scene_coordinates(self, wrist_data: Dict) -> Dict: + """Transform vive wrist coordinates to scene coordinates using scene_T_lighthouse_static.""" + if self.scene_T_lighthouse_static is None: + return wrist_data + + try: + lighthouse_T_wrist = self.pose_to_matrix(wrist_data) + transformed_matrix = lighthouse_T_wrist * self.scene_T_lighthouse_static + return self.matrix_to_pose(transformed_matrix) + + except Exception as e: + carb.log_error(f"Failed to transform coordinates: {e}") + return wrist_data def get_all_device_data(self) -> Dict: + # Get raw data from trackers + manus_data = self.manus_tracker.get_all_glove_data() + vive_data = self.vive_tracker.get_all_tracker_data() + vive_transformed = self._transform_vive_data(vive_data) + scene_T_wrist = self._get_scene_T_wrist(vive_transformed) + return { - 'manus_gloves': self.manus_tracker.get_all_glove_data(), - 'vive_trackers': self.vive_tracker.get_all_tracker_data(), + 'manus_gloves': self._transform_manus_data(manus_data, scene_T_wrist), + 'vive_trackers': vive_transformed, 'device_status': self.device_status } + def _transform_vive_data(self, device_data: Dict) -> Dict: + """Transform all joints in vive data to scene coordinates. """ + if self.scene_T_lighthouse_static is None: + return device_data + + transformed_data = {} + for joint_name, joint_data in device_data.items(): + transformed_data[joint_name] = self._transform_to_scene_coordinates(joint_data) + + return transformed_data + + def _get_scene_T_wrist(self, vive_data: Dict) -> Gf.Matrix4d: + """Get the wrist position and orientation from vive data.""" + if 'WM0' in vive_data: + return self.pose_to_matrix(vive_data['WM0']) # FIXME: only works for left hand + else: + carb.log_warn("No Vive wrist tracker found") + return None + + def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Gf.Matrix4d) -> Dict: + """Transform Manus glove data: wrist * wrist_T_joint.""" + if scene_T_wrist is None: + return manus_data + transformed_data = {} + for joint_name, joint_data in manus_data.items(): + if joint_name.startswith('left_'): + joint_transformed = self.pose_to_matrix(joint_data) * scene_T_wrist + transformed_data[joint_name] = self.matrix_to_pose(joint_transformed) + return transformed_data + def cleanup(self): if hasattr(self, 'manus_tracker'): self.manus_tracker.cleanup() diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 55b41647..ed9256f2 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -45,25 +45,36 @@ distantLight = UsdLux.DistantLight.Define(stage, Sdf.Path("/DistantLight")) distantLight.CreateIntensityAttr(300) -# Create red cube prototype +# Create cube prototypes hidden_prim = create_prim("/Hidden/Prototypes", "Scope") -base_cube_path = "/Hidden/Prototypes/BaseCube" + +# Red cube for Manus gloves (small) +manus_cube_path = "/Hidden/Prototypes/ManusCube" VisualCuboid( - prim_path=base_cube_path, + prim_path=manus_cube_path, size=0.01, color=np.array([255, 0, 0]), # Red color ) + +# Blue cube for Vive trackers (larger) +vive_cube_path = "/Hidden/Prototypes/ViveCube" +VisualCuboid( + prim_path=vive_cube_path, + size=0.02, + color=np.array([0, 0, 255]), # Blue color +) + set_prim_visibility(hidden_prim, False) # Create point instancer for cubes instancer_path = "/World/DeviceCubeInstancer" point_instancer = UsdGeom.PointInstancer.Define(my_world.stage, instancer_path) -point_instancer.CreatePrototypesRel().SetTargets([Sdf.Path(base_cube_path)]) +point_instancer.CreatePrototypesRel().SetTargets([Sdf.Path(manus_cube_path), Sdf.Path(vive_cube_path)]) max_devices = 60 -# Initially hide all cubes until devices are tracked -point_instancer.CreateProtoIndicesAttr().Set([1 for _ in range(max_devices)]) +# Initially hide all cubes until devices are tracked (index 2 = hidden, since we have 2 prototypes: 0=manus, 1=vive) +point_instancer.CreateProtoIndicesAttr().Set([2 for _ in range(max_devices)]) # Initialize positions and orientations positions = [Gf.Vec3f(0.0, 0.0, 0.0) for i in range(max_devices)] @@ -92,7 +103,7 @@ proto_idx_attr = point_instancer.GetProtoIndicesAttr() carb.log_info("Starting Manus Glove and Vive Tracker visualization with sequential updates") -carb.log_info("Red cubes will appear at device positions when data is available") +carb.log_info("Red cubes (0.01) for Manus gloves, Blue cubes (0.02) for Vive trackers") # Main simulation loop while simulation_app.is_running(): @@ -106,7 +117,7 @@ # Sequential device updates to avoid resource contention # Alternate between Manus and Vive every frame - update_manus = (frame_counter % 2 == 0) + update_manus = (frame_counter % 2 == 1) frame_counter += 1 # Get current cube arrays @@ -114,8 +125,8 @@ current_orientations = orientations_attr.Get() proto_indices = proto_idx_attr.Get() - # Initialize all cubes as hidden (following hand_tracking_sample pattern) - proto_indices = [1 for _ in range(max_devices)] + # Initialize all cubes as hidden (index 2 = hidden, since we have 2 prototypes: 0=manus, 1=vive) + proto_indices = [2 for _ in range(max_devices)] proto_idx_attr.Set(proto_indices) cube_idx = 0 @@ -130,20 +141,25 @@ manus_data = all_device_data.get('manus_gloves', {}) vive_data = all_device_data.get('vive_trackers', {}) - # Process all devices - for device_data in [manus_data, vive_data]: - for joint, joint_data in device_data.items(): - if cube_idx >= max_devices: - break - pos = joint_data['position'] - ori = joint_data['orientation'] - - current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) - current_orientations[cube_idx] = Gf.Quath( - float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3]) - ) - proto_indices[cube_idx] = 0 # Show cube - cube_idx += 1 + # Process Manus gloves (red cubes, index 0) + for joint, joint_data in manus_data.items(): + if cube_idx >= max_devices: + break + pos, ori = joint_data['position'], joint_data['orientation'] + current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) + current_orientations[cube_idx] = Gf.Quath(float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3])) + proto_indices[cube_idx] = 0 # Red Manus cube + cube_idx += 1 + + # Process Vive trackers (blue cubes, index 1) + for joint, joint_data in vive_data.items(): + if cube_idx >= max_devices: + break + pos, ori = joint_data['position'], joint_data['orientation'] + current_positions[cube_idx] = Gf.Vec3f(float(pos[0]), float(pos[1]), float(pos[2])) + current_orientations[cube_idx] = Gf.Quath(float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3])) + proto_indices[cube_idx] = 1 # Blue Vive cube + cube_idx += 1 # Debug: Log cube visibility and positions visible_cubes = sum(1 for idx in proto_indices if idx == 0) From 106b11420c23a84d5aaa3a9fa299d548c2a49f13 Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 26 Aug 2025 10:04:52 -0700 Subject: [PATCH 4/9] calibrated coordinate transformation. Works correctly on one glove and tracker --- .../python/impl/xr_device_integration.py | 192 ++++++++++++++---- .../manus_vive_tracking_sample.py | 27 ++- 2 files changed, 175 insertions(+), 44 deletions(-) diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index e296d881..69ee8c6a 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -8,15 +8,8 @@ from pxr import Gf from .manus_tracker import IsaacSimManusGloveTracker from .vive_tracker import IsaacSimViveTracker +from omni.kit.xr.core import XR_INPUT_DEVICE_HAND_TRACKING_POSE_NAMES, XRCore, XRPoseValidityFlags -# Left wrist pose from AVP hand tracking -scene_T_wrist_static = Gf.Matrix4d( - (0.9534056782722473, -0.30112600326538086, -0.018459070473909378, 0), - (-0.104372538626194, -0.38662755489349365, 0.9163107872009277, 0), - (-0.28306180238723755, -0.8716893196105957, -0.40004217624664307, 0), - (-0.1019858792424202, 0.3140803277492523, 1.285851240158081, 1) -) -scene_T_wrist_static = Gf.Matrix4d().SetIdentity() def get_xr_device_integration(): """Get the existing XRDeviceIntegration instance from the extension.""" @@ -38,12 +31,22 @@ def __init__(self): self.manus_tracker = IsaacSimManusGloveTracker() self.vive_tracker = IsaacSimViveTracker() self.last_update_time = 0 - self.update_rate = 100.0 + self.update_rate = 60.0 self.device_status = { 'manus_gloves': {'connected': False, 'last_data_time': 0}, 'vive_trackers': {'connected': False, 'last_data_time': 0} } - self.scene_T_lighthouse_static = None # Coordinate transformation matrix, will be computed from first Vive wrist position + self.scene_T_lighthouse_static = None + # Calibration buffering for robust initialization + self._calib_candidates = [] # List[Gf.Matrix4d] + self._calib_start_time = None + self._calib_required_samples = 15 + self._calib_max_seconds = 1.0 + self._calib_max_trans_error_m = 0.03 + self._calib_max_rot_error_deg = 7.5 + # Mode (cluster) selection thresholds for robust calibration + self._mode_trans_threshold_m = 0.02 + self._mode_rot_threshold_deg = 5.0 def register_devices(self): try: @@ -64,6 +67,9 @@ def register_devices(self): def update_manus(self): """Update raw Manus glove data. """ + if self.scene_T_lighthouse_static is None: + # carb.log_warn("Manus glove data update skipped: scene_T_lighthouse_static is not initialized") + return current_time = time.time() self.manus_tracker.update() self.device_status['manus_gloves']['last_data_time'] = current_time @@ -100,7 +106,7 @@ def pose_to_matrix(self, pose: Dict) -> Gf.Matrix4d: trans = Gf.Vec3d(pos[0], pos[1], pos[2]) return Gf.Matrix4d(rot, trans) - def matrix_to_pose(self, matrix: Gf.Matrix4d) -> tuple: + def matrix_to_pose(self, matrix: Gf.Matrix4d) -> Dict: """Convert a 4x4 matrix to position and orientation.""" pos = matrix.ExtractTranslation() rot = matrix.ExtractRotation() @@ -110,45 +116,166 @@ def matrix_to_pose(self, matrix: Gf.Matrix4d) -> tuple: 'orientation': [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]] } + def _get_openxr_left_wrist_matrix(self) -> Gf.Matrix4d: + """Get OpenXR left wrist world matrix if valid, else None.""" + try: + left_hand_device = XRCore.get_singleton().get_input_device("/user/hand/left") + if left_hand_device is None: + return None + left_joints = left_hand_device.get_all_virtual_world_poses() + if 'wrist' not in left_joints: + return None + joint = left_joints['wrist'] + required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID + if (joint.validity_flags & required) != required: + return None + return joint.pose_matrix + except Exception as e: + carb.log_warn(f"OpenXR wrist fetch failed: {e}") + return None + + def _compute_delta_errors(self, a: Gf.Matrix4d, b: Gf.Matrix4d) -> (float, float): + """Return (translation_error_m, rotation_error_deg) between transforms a and b.""" + try: + delta = a * b.GetInverse() + t = delta.ExtractTranslation() + trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) + q = delta.ExtractRotation().GetQuat() + w = float(max(min(q.GetReal(), 1.0), -1.0)) + ang = 2.0 * float(np.arccos(w)) + ang_deg = float(np.degrees(ang)) + if ang_deg > 180.0: + ang_deg = 360.0 - ang_deg + return trans_err, ang_deg + except Exception: + return float('inf'), float('inf') + + def _average_transforms(self, mats: list) -> Gf.Matrix4d: + """Average a list of rigid transforms (simple mean of translation and normalized mean of quaternions).""" + if not mats: + return None + # Use the first as reference for quaternion sign + ref_quat = mats[0].ExtractRotation().GetQuat() + ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) + acc_q = np.zeros(4, dtype=np.float64) + acc_t = np.zeros(3, dtype=np.float64) + for m in mats: + t = m.ExtractTranslation() + acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) + q = m.ExtractRotation().GetQuat() + qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) + # Align sign to reference to avoid cancellation + if np.dot(qi, ref) < 0.0: + qi = -qi + acc_q += qi + mean_t = acc_t / float(len(mats)) + # Normalize quaternion + norm = np.linalg.norm(acc_q) + if norm <= 1e-12: + # Fallback to identity rotation + quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) + else: + qn = acc_q / norm + quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) + rot3 = Gf.Matrix3d().SetRotate(quat_avg) + trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) + return Gf.Matrix4d(rot3, trans) + + def _select_mode_cluster(self, mats: list) -> list: + """Select the largest cluster (mode) of transforms under proximity thresholds.""" + if not mats: + return [] + best_cluster = [] + for i, center in enumerate(mats): + cluster = [] + for m in mats: + trans_err, rot_err = self._compute_delta_errors(m, center) + if trans_err <= self._mode_trans_threshold_m and rot_err <= self._mode_rot_threshold_deg: + cluster.append(m) + if len(cluster) > len(best_cluster): + best_cluster = cluster + return best_cluster + def _initialize_coordinate_transformation(self): """ Initialize the scene to lighthouse coordinate transformation. The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. - It is computed from a hard-coded AVP wrist pose and the first Vive wrist pose. + It is computed from multiple frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. """ vive_data = self.vive_tracker.get_all_tracker_data() - # Look for a wrist tracker in the Vive data - left_wrist = 'WM0' # FIXME: hardcoded - assuming that WM0 is the left wrist tracker - list_trackers = [] - if left_wrist not in vive_data: - for device_key in vive_data.keys(): - list_trackers.append(device_key) - if len(device_key) >= 2 and device_key[:2] == 'WM': - left_wrist = device_key - carb.log_warn(f"Using Vive tracker '{left_wrist}' for coordinate transformation") - break - - if left_wrist not in vive_data: - carb.log_warn(f"No Vive wrist tracker found for coordinate transformation. Trackers: {list_trackers}") + left_wrist = self._get_init_ref_wrist_id(vive_data) + if left_wrist is None: return + # Start/continue calibration window + if self._calib_start_time is None: + self._calib_start_time = time.time() + self._calib_candidates = [] + try: lighthouse_T_wrist = self.pose_to_matrix(vive_data[left_wrist]) - wrist_T_lighthouse_static = lighthouse_T_wrist.GetInverse() - self.scene_T_lighthouse_static = wrist_T_lighthouse_static * scene_T_wrist_static - + openxr_scene_T_wrist = self._get_openxr_left_wrist_matrix() + if openxr_scene_T_wrist is None: + # No valid AVP wrist this frame + pass + else: + # Candidate scene_T_lighthouse from this frame + wrist_T_lighthouse = lighthouse_T_wrist.GetInverse() + candidate = wrist_T_lighthouse * openxr_scene_T_wrist + # Validate by checking that lighthouse * candidate ~ openxr + pred_scene_T_wrist = lighthouse_T_wrist * candidate + trans_err, rot_err = self._compute_delta_errors(pred_scene_T_wrist, openxr_scene_T_wrist) + if trans_err <= self._calib_max_trans_error_m and rot_err <= self._calib_max_rot_error_deg: + self._calib_candidates.append(candidate) + else: + carb.log_warn(f"Calibration sample rejected: trans_err={trans_err:.3f} m, rot_err={rot_err:.1f} deg") + + # Decide to finalize + enough_samples = len(self._calib_candidates) >= self._calib_required_samples + timed_out = (time.time() - self._calib_start_time) >= self._calib_max_seconds + if enough_samples or (timed_out and len(self._calib_candidates) >= max(3, self._calib_required_samples // 2)): + cluster = self._select_mode_cluster(self._calib_candidates) + if cluster and len(cluster) < len(self._calib_candidates): + carb.log_warn(f"Calibration mode selected: {len(cluster)}/{len(self._calib_candidates)} candidates") + averaged = self._average_transforms(cluster if cluster else self._calib_candidates) + if averaged is not None: + self.scene_T_lighthouse_static = averaged + carb.log_info(f"Calibrated scene_T_lighthouse with {len(cluster) if cluster else len(self._calib_candidates)} samples") + else: + carb.log_warn("Calibration averaging failed; keeping transform uninitialized") + # Reset calibration state + self._calib_candidates = [] + self._calib_start_time = None + elif timed_out and not self._calib_candidates: + # No good samples; restart window + self._calib_start_time = time.time() except Exception as e: carb.log_error(f"Failed to initialize coordinate transformation: {e}") + def _get_init_ref_wrist_id(self, vive_data: Dict) -> str: + """Get the wrist id for the initial reference wrist.""" + left_wrist = 'WM0' # FIXME: hardcoded - assuming that WM0 is the left wrist tracker + if left_wrist in vive_data: + return left_wrist + + devices = [] + for device_key in vive_data.keys(): + devices.append(device_key) + if len(device_key) >= 2 and device_key[:2] == 'WM': + left_wrist = device_key + return left_wrist + def _transform_to_scene_coordinates(self, wrist_data: Dict) -> Dict: """Transform vive wrist coordinates to scene coordinates using scene_T_lighthouse_static.""" if self.scene_T_lighthouse_static is None: return wrist_data try: - lighthouse_T_wrist = self.pose_to_matrix(wrist_data) - transformed_matrix = lighthouse_T_wrist * self.scene_T_lighthouse_static - return self.matrix_to_pose(transformed_matrix) + transformed_matrix = self.pose_to_matrix(wrist_data) * self.scene_T_lighthouse_static + # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis + # 10 cm offset on Y-axis for change in vive tracker position after flipping the hand + Rcorr = Gf.Matrix4d(Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))), Gf.Vec3d(0, -0.1, 0)) + return self.matrix_to_pose(Rcorr * transformed_matrix) except Exception as e: carb.log_error(f"Failed to transform coordinates: {e}") @@ -182,9 +309,6 @@ def _get_scene_T_wrist(self, vive_data: Dict) -> Gf.Matrix4d: """Get the wrist position and orientation from vive data.""" if 'WM0' in vive_data: return self.pose_to_matrix(vive_data['WM0']) # FIXME: only works for left hand - else: - carb.log_warn("No Vive wrist tracker found") - return None def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Gf.Matrix4d) -> Dict: """Transform Manus glove data: wrist * wrist_T_joint.""" diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index ed9256f2..305df723 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -13,12 +13,28 @@ # See the License for the specific language governing permissions and # limitations under the License. +""" +Manus Glove and Vive Tracker Visualization Sample + +This sample demonstrates simultaneous tracking of Manus gloves and Vive trackers +in Isaac Sim. The devices are visualized as colored cubes: +- Red cubes (0.01 size): Manus glove joints +- Blue cubes (0.02 size): Vive tracker positions + +IMPORTANT: To avoid resource contention and crashes, ensure Manus and Vive devices +are connected to different USB controllers/buses. Use 'lsusb -t' to identify +different buses and connect devices accordingly. +""" + import os import numpy as np from isaacsim import SimulationApp # Initialize simulation app -simulation_app = SimulationApp({"headless": False}) +simulation_app = SimulationApp( + {"headless": False}, + experience=f'/home/yuanchenl/Desktop/IsaacSim/source/apps/isaacsim.exp.base.xr.openxr.kit' +) import carb import omni.usd @@ -131,10 +147,8 @@ cube_idx = 0 if update_manus: - # Update manus glove positions xr_integration.update_manus() else: - # Update vive tracker positions xr_integration.update_vive() all_device_data = xr_integration.get_all_device_data() @@ -160,13 +174,6 @@ current_orientations[cube_idx] = Gf.Quath(float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3])) proto_indices[cube_idx] = 1 # Blue Vive cube cube_idx += 1 - - # Debug: Log cube visibility and positions - visible_cubes = sum(1 for idx in proto_indices if idx == 0) - - if frame_counter % 100 == 0: - carb.log_info(f"Showing {visible_cubes} cubes at positions: {current_positions[:visible_cubes]}") - carb.log_info(f"Frame {frame_counter}, data: {all_device_data}") # Update the instancer with new positions and orientations positions_attr.Set(current_positions) From a2ca6d054ea8283acfda76d5645833b650c3db8c Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 26 Aug 2025 10:54:29 -0700 Subject: [PATCH 5/9] enable two hands --- .../python/impl/vive_tracker.py | 13 +- .../python/impl/xr_device_integration.py | 304 +++++++----------- .../python/impl/xr_utils.py | 127 ++++++++ .../python/tests/test_xr_input_devices.py | 23 -- .../manus_vive_tracking_sample.py | 2 +- 5 files changed, 246 insertions(+), 223 deletions(-) create mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index 03624937..97d03929 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -4,6 +4,7 @@ import sys import carb from typing import Dict +from pxr import Gf try: # Add libsurvive pysurvive bindings to Python path @@ -55,28 +56,26 @@ def update(self): return try: - max_iterations = 1000 # Prevent infinite loops + max_iterations = 10 # Prevent infinite loops iteration = 0 while iteration < max_iterations: updated = self._ctx.NextUpdated() if not updated: break iteration += 1 - + pose_obj, _ = updated.Pose() pos = pose_obj.Pos # (x, y, z) ori = pose_obj.Rot # (w, x, y, z) device_id = updated.Name().decode('utf-8') - - # Capture ALL devices like isaac-deploy does (no filtering) + self.device_data[device_id] = { - 'position': [float(pos[i]) for i in range(3)], - 'orientation': [float(ori[i]) for i in range(4)] + 'position': [float(pos[0]), float(-pos[2]), float(pos[1])], # x, z, -y + 'orientation': [float(ori[0]), float(ori[1]), float(-ori[3]), float(ori[2])] } except Exception as e: carb.log_error(f"Failed to update Vive tracker data: {e}") - def get_all_tracker_data(self) -> Dict: return self.device_data diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index 69ee8c6a..e21d2910 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -3,12 +3,11 @@ import carb import time -import numpy as np from typing import Dict from pxr import Gf from .manus_tracker import IsaacSimManusGloveTracker from .vive_tracker import IsaacSimViveTracker -from omni.kit.xr.core import XR_INPUT_DEVICE_HAND_TRACKING_POSE_NAMES, XRCore, XRPoseValidityFlags +from .xr_utils import * def get_xr_device_integration(): @@ -30,23 +29,23 @@ class XRDeviceIntegration: def __init__(self): self.manus_tracker = IsaacSimManusGloveTracker() self.vive_tracker = IsaacSimViveTracker() - self.last_update_time = 0 - self.update_rate = 60.0 self.device_status = { 'manus_gloves': {'connected': False, 'last_data_time': 0}, - 'vive_trackers': {'connected': False, 'last_data_time': 0} + 'vive_trackers': {'connected': False, 'last_data_time': 0}, + 'left_hand_connected': False, + 'right_hand_connected': False } + self.last_update_time = 0 + self.update_rate = 100.0 self.scene_T_lighthouse_static = None - # Calibration buffering for robust initialization - self._calib_candidates = [] # List[Gf.Matrix4d] - self._calib_start_time = None - self._calib_required_samples = 15 - self._calib_max_seconds = 1.0 - self._calib_max_trans_error_m = 0.03 - self._calib_max_rot_error_deg = 7.5 - # Mode (cluster) selection thresholds for robust calibration - self._mode_trans_threshold_m = 0.02 - self._mode_rot_threshold_deg = 5.0 + self._vive_left_id = None + self._vive_right_id = None + self._pairA_candidates = [] # Pair A: WM0->Left, WM1->Right + self._pairB_candidates = [] # Pair B: WM1->Left, WM0->Right + self._pairA_trans_errs = [] + self._pairA_rot_errs = [] + self._pairB_trans_errs = [] + self._pairB_rot_errs = [] def register_devices(self): try: @@ -67,16 +66,22 @@ def register_devices(self): def update_manus(self): """Update raw Manus glove data. """ - if self.scene_T_lighthouse_static is None: - # carb.log_warn("Manus glove data update skipped: scene_T_lighthouse_static is not initialized") - return current_time = time.time() + if current_time - self.device_status['manus_gloves']['last_data_time'] < 1.0 / self.update_rate: + return + self.manus_tracker.update() self.device_status['manus_gloves']['last_data_time'] = current_time + manus_data = self.manus_tracker.get_all_glove_data() + self.device_status['left_hand_connected'] = 'left_joint_0' in manus_data + self.device_status['right_hand_connected'] = 'right_joint_0' in manus_data def update_vive(self): """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update. """ current_time = time.time() + if current_time - self.device_status['vive_trackers']['last_data_time'] < 1.0 / self.update_rate: + return + self.vive_tracker.update() self.device_status['vive_trackers']['last_data_time'] = current_time try: @@ -86,196 +91,106 @@ def update_vive(self): except Exception as e: carb.log_error(f"Vive tracker update failed: {e}") - def update_all_devices(self): - current_time = time.time() - - if current_time - self.last_update_time >= 1.0 / self.update_rate: - try: - self.update_manus() - self.update_vive() - self.last_update_time = current_time - - except Exception as e: - carb.log_error(f"Failed to update XR devices: {e}") - - def pose_to_matrix(self, pose: Dict) -> Gf.Matrix4d: - """Convert position and orientation to a 4x4 matrix.""" - pos, ori = pose['position'], pose['orientation'] - quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) - rot = Gf.Matrix3d().SetRotate(quat) - trans = Gf.Vec3d(pos[0], pos[1], pos[2]) - return Gf.Matrix4d(rot, trans) - - def matrix_to_pose(self, matrix: Gf.Matrix4d) -> Dict: - """Convert a 4x4 matrix to position and orientation.""" - pos = matrix.ExtractTranslation() - rot = matrix.ExtractRotation() - quat = rot.GetQuat() - return { - 'position': [pos[0], pos[1], pos[2]], - 'orientation': [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]] - } - - def _get_openxr_left_wrist_matrix(self) -> Gf.Matrix4d: - """Get OpenXR left wrist world matrix if valid, else None.""" - try: - left_hand_device = XRCore.get_singleton().get_input_device("/user/hand/left") - if left_hand_device is None: - return None - left_joints = left_hand_device.get_all_virtual_world_poses() - if 'wrist' not in left_joints: - return None - joint = left_joints['wrist'] - required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID - if (joint.validity_flags & required) != required: - return None - return joint.pose_matrix - except Exception as e: - carb.log_warn(f"OpenXR wrist fetch failed: {e}") - return None - - def _compute_delta_errors(self, a: Gf.Matrix4d, b: Gf.Matrix4d) -> (float, float): - """Return (translation_error_m, rotation_error_deg) between transforms a and b.""" - try: - delta = a * b.GetInverse() - t = delta.ExtractTranslation() - trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) - q = delta.ExtractRotation().GetQuat() - w = float(max(min(q.GetReal(), 1.0), -1.0)) - ang = 2.0 * float(np.arccos(w)) - ang_deg = float(np.degrees(ang)) - if ang_deg > 180.0: - ang_deg = 360.0 - ang_deg - return trans_err, ang_deg - except Exception: - return float('inf'), float('inf') - - def _average_transforms(self, mats: list) -> Gf.Matrix4d: - """Average a list of rigid transforms (simple mean of translation and normalized mean of quaternions).""" - if not mats: - return None - # Use the first as reference for quaternion sign - ref_quat = mats[0].ExtractRotation().GetQuat() - ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) - acc_q = np.zeros(4, dtype=np.float64) - acc_t = np.zeros(3, dtype=np.float64) - for m in mats: - t = m.ExtractTranslation() - acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) - q = m.ExtractRotation().GetQuat() - qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) - # Align sign to reference to avoid cancellation - if np.dot(qi, ref) < 0.0: - qi = -qi - acc_q += qi - mean_t = acc_t / float(len(mats)) - # Normalize quaternion - norm = np.linalg.norm(acc_q) - if norm <= 1e-12: - # Fallback to identity rotation - quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) - else: - qn = acc_q / norm - quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) - rot3 = Gf.Matrix3d().SetRotate(quat_avg) - trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) - return Gf.Matrix4d(rot3, trans) - - def _select_mode_cluster(self, mats: list) -> list: - """Select the largest cluster (mode) of transforms under proximity thresholds.""" - if not mats: - return [] - best_cluster = [] - for i, center in enumerate(mats): - cluster = [] - for m in mats: - trans_err, rot_err = self._compute_delta_errors(m, center) - if trans_err <= self._mode_trans_threshold_m and rot_err <= self._mode_rot_threshold_deg: - cluster.append(m) - if len(cluster) > len(best_cluster): - best_cluster = cluster - return best_cluster - def _initialize_coordinate_transformation(self): """ Initialize the scene to lighthouse coordinate transformation. The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. It is computed from multiple frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. """ + params = { + 'mapping_margin_trans_m': 0.015, + 'mapping_margin_rot_deg': 3.0, + 'mapping_min_pair_frames': 6 + } vive_data = self.vive_tracker.get_all_tracker_data() - left_wrist = self._get_init_ref_wrist_id(vive_data) - if left_wrist is None: + wm0_id, wm1_id = get_vive_wrist_ids(vive_data) + if wm0_id is None and wm1_id is None: return - # Start/continue calibration window - if self._calib_start_time is None: - self._calib_start_time = time.time() - self._calib_candidates = [] - try: - lighthouse_T_wrist = self.pose_to_matrix(vive_data[left_wrist]) - openxr_scene_T_wrist = self._get_openxr_left_wrist_matrix() - if openxr_scene_T_wrist is None: - # No valid AVP wrist this frame - pass - else: - # Candidate scene_T_lighthouse from this frame - wrist_T_lighthouse = lighthouse_T_wrist.GetInverse() - candidate = wrist_T_lighthouse * openxr_scene_T_wrist - # Validate by checking that lighthouse * candidate ~ openxr - pred_scene_T_wrist = lighthouse_T_wrist * candidate - trans_err, rot_err = self._compute_delta_errors(pred_scene_T_wrist, openxr_scene_T_wrist) - if trans_err <= self._calib_max_trans_error_m and rot_err <= self._calib_max_rot_error_deg: - self._calib_candidates.append(candidate) - else: - carb.log_warn(f"Calibration sample rejected: trans_err={trans_err:.3f} m, rot_err={rot_err:.1f} deg") + # Fetch OpenXR wrists + L, R, gloves = None, None, [] + if self.device_status['left_hand_connected']: + gloves.append('left') + L = get_openxr_wrist_matrix('left') + if self.device_status['right_hand_connected']: + gloves.append('right') + R = get_openxr_wrist_matrix('right') + + M0, M1, vives = None, None, [] + if wm0_id is not None: + vives.append(wm0_id) + M0 = pose_to_matrix(vive_data[wm0_id]) + if wm1_id is not None: + vives.append(wm1_id) + M1 = pose_to_matrix(vive_data[wm1_id]) - # Decide to finalize - enough_samples = len(self._calib_candidates) >= self._calib_required_samples - timed_out = (time.time() - self._calib_start_time) >= self._calib_max_seconds - if enough_samples or (timed_out and len(self._calib_candidates) >= max(3, self._calib_required_samples // 2)): - cluster = self._select_mode_cluster(self._calib_candidates) - if cluster and len(cluster) < len(self._calib_candidates): - carb.log_warn(f"Calibration mode selected: {len(cluster)}/{len(self._calib_candidates)} candidates") - averaged = self._average_transforms(cluster if cluster else self._calib_candidates) - if averaged is not None: + TL0, TL1, TR0, TR1 = None, None, None, None + # Compute transforms for available pairs + if wm0_id is not None and L is not None: + TL0 = M0.GetInverse() * L + self._pairA_candidates.append(TL0) + if wm1_id is not None and L is not None: + TL1 = M1.GetInverse() * L + self._pairB_candidates.append(TL1) + if wm1_id is not None and R is not None: + TR1 = M1.GetInverse() * R + self._pairA_candidates.append(TR1) + if wm0_id is not None and R is not None: + TR0 = M0.GetInverse() * R + self._pairB_candidates.append(TR0) + + # Per-frame pairing error if both candidates present + if len(gloves) == 2 and len(vives) == 2: + eT, eR = compute_delta_errors(TL0, TR1) + self._pairA_trans_errs.append(eT) + self._pairA_rot_errs.append(eR) + eT, eR = compute_delta_errors(TL1, TR0) + self._pairB_trans_errs.append(eT) + self._pairB_rot_errs.append(eR) + + # Choose a mapping + enough_pair_frames = (len(self._pairA_trans_errs) >= params['mapping_min_pair_frames'] and + len(self._pairB_trans_errs) >= params['mapping_min_pair_frames']) + + choose_A = len(self._pairA_candidates) > len(self._pairB_candidates) + if enough_pair_frames: + errA = get_pairing_error(self._pairA_trans_errs, self._pairA_rot_errs) + errB = get_pairing_error(self._pairB_trans_errs, self._pairB_rot_errs) + choose_A = errA <= errB + + if choose_A: + chosen_list = self._pairA_candidates + self._vive_left_id, self._vive_right_id = wm0_id, wm1_id + else: + chosen_list = self._pairB_candidates + self._vive_left_id, self._vive_right_id = wm1_id, wm0_id + + if len(chosen_list) >= params['mapping_min_pair_frames']: + cluster = select_mode_cluster(chosen_list) + carb.log_error(f"Wrist calibration: formed size {len(cluster)} cluster from {len(chosen_list)} samples") + if len(cluster) >= params['mapping_min_pair_frames'] // 2: + averaged = average_transforms(cluster) self.scene_T_lighthouse_static = averaged - carb.log_info(f"Calibrated scene_T_lighthouse with {len(cluster) if cluster else len(self._calib_candidates)} samples") + carb.log_error(f"Resolved mapping: {self._vive_left_id}->Left, {self._vive_right_id}->Right") else: - carb.log_warn("Calibration averaging failed; keeping transform uninitialized") - # Reset calibration state - self._calib_candidates = [] - self._calib_start_time = None - elif timed_out and not self._calib_candidates: - # No good samples; restart window - self._calib_start_time = time.time() + carb.log_error(f"Waiting for more samples to average wrist transforms") + except Exception as e: carb.log_error(f"Failed to initialize coordinate transformation: {e}") - def _get_init_ref_wrist_id(self, vive_data: Dict) -> str: - """Get the wrist id for the initial reference wrist.""" - left_wrist = 'WM0' # FIXME: hardcoded - assuming that WM0 is the left wrist tracker - if left_wrist in vive_data: - return left_wrist - - devices = [] - for device_key in vive_data.keys(): - devices.append(device_key) - if len(device_key) >= 2 and device_key[:2] == 'WM': - left_wrist = device_key - return left_wrist - + def _transform_to_scene_coordinates(self, wrist_data: Dict) -> Dict: """Transform vive wrist coordinates to scene coordinates using scene_T_lighthouse_static.""" if self.scene_T_lighthouse_static is None: return wrist_data try: - transformed_matrix = self.pose_to_matrix(wrist_data) * self.scene_T_lighthouse_static + transformed_matrix = pose_to_matrix(wrist_data) * self.scene_T_lighthouse_static # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis - # 10 cm offset on Y-axis for change in vive tracker position after flipping the hand + # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm Rcorr = Gf.Matrix4d(Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))), Gf.Vec3d(0, -0.1, 0)) - return self.matrix_to_pose(Rcorr * transformed_matrix) + return matrix_to_pose(Rcorr * transformed_matrix) except Exception as e: carb.log_error(f"Failed to transform coordinates: {e}") @@ -305,20 +220,25 @@ def _transform_vive_data(self, device_data: Dict) -> Dict: return transformed_data - def _get_scene_T_wrist(self, vive_data: Dict) -> Gf.Matrix4d: + def _get_scene_T_wrist(self, vive_data: Dict) -> Dict: """Get the wrist position and orientation from vive data.""" - if 'WM0' in vive_data: - return self.pose_to_matrix(vive_data['WM0']) # FIXME: only works for left hand + scene_T_wrist = {'left': Gf.Matrix4d().SetIdentity(), 'right': Gf.Matrix4d().SetIdentity()} + if self._vive_left_id is not None: + scene_T_wrist['left'] = pose_to_matrix(vive_data[self._vive_left_id]) + if self._vive_right_id is not None: + scene_T_wrist['right'] = pose_to_matrix(vive_data[self._vive_right_id]) + return scene_T_wrist - def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Gf.Matrix4d) -> Dict: + def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Dict) -> Dict: """Transform Manus glove data: wrist * wrist_T_joint.""" - if scene_T_wrist is None: - return manus_data transformed_data = {} for joint_name, joint_data in manus_data.items(): if joint_name.startswith('left_'): - joint_transformed = self.pose_to_matrix(joint_data) * scene_T_wrist - transformed_data[joint_name] = self.matrix_to_pose(joint_transformed) + joint_transformed = pose_to_matrix(joint_data) * scene_T_wrist['left'] + transformed_data[joint_name] = matrix_to_pose(joint_transformed) + elif joint_name.startswith('right_'): + joint_transformed = pose_to_matrix(joint_data) * scene_T_wrist['right'] + transformed_data[joint_name] = matrix_to_pose(joint_transformed) return transformed_data def cleanup(self): diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py new file mode 100644 index 00000000..0b84fdd3 --- /dev/null +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py @@ -0,0 +1,127 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import carb +import numpy as np +from pxr import Gf +from omni.kit.xr.core import XRCore, XRPoseValidityFlags +from typing import Dict, List, Tuple + + +def compute_delta_errors(a: Gf.Matrix4d, b: Gf.Matrix4d) -> Tuple[float, float]: + """Return (translation_error_m, rotation_error_deg) between transforms a and b.""" + try: + delta = a * b.GetInverse() + t = delta.ExtractTranslation() + trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) + q = delta.ExtractRotation().GetQuat() + w = float(max(min(q.GetReal(), 1.0), -1.0)) + ang = 2.0 * float(np.arccos(w)) + ang_deg = float(np.degrees(ang)) + if ang_deg > 180.0: + ang_deg = 360.0 - ang_deg + return trans_err, ang_deg + except Exception: + return float('inf'), float('inf') + + +def average_transforms(mats: List[Gf.Matrix4d]) -> Gf.Matrix4d: + """Average rigid transforms: mean translation and normalized mean quaternion (with sign alignment).""" + if not mats: + return None + ref_quat = mats[0].ExtractRotation().GetQuat() + ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) + acc_q = np.zeros(4, dtype=np.float64) + acc_t = np.zeros(3, dtype=np.float64) + for m in mats: + t = m.ExtractTranslation() + acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) + q = m.ExtractRotation().GetQuat() + qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) + if np.dot(qi, ref) < 0.0: + qi = -qi + acc_q += qi + mean_t = acc_t / float(len(mats)) + norm = np.linalg.norm(acc_q) + if norm <= 1e-12: + quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) + else: + qn = acc_q / norm + quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) + rot3 = Gf.Matrix3d().SetRotate(quat_avg) + trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) + return Gf.Matrix4d(rot3, trans) + + +def select_mode_cluster(mats: List[Gf.Matrix4d], trans_thresh_m: float = 0.02, rot_thresh_deg: float = 5.0) -> List[Gf.Matrix4d]: + """Return the largest cluster (mode) under proximity thresholds.""" + if not mats: + return [] + best_cluster: List[Gf.Matrix4d] = [] + for center in mats: + cluster: List[Gf.Matrix4d] = [] + for m in mats: + trans_err, rot_err = compute_delta_errors(m, center) + if trans_err <= trans_thresh_m and rot_err <= rot_thresh_deg: + cluster.append(m) + if len(cluster) > len(best_cluster): + best_cluster = cluster + return best_cluster + + +def get_openxr_wrist_matrix(hand: str) -> Gf.Matrix4d: + """Get OpenXR wrist world matrix if valid, else None.""" + hand = hand.lower() + try: + hand_device = XRCore.get_singleton().get_input_device(f"/user/hand/{hand}") + if hand_device is None: + return None + joints = hand_device.get_all_virtual_world_poses() + if 'wrist' not in joints: + return None + joint = joints['wrist'] + required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID + if (joint.validity_flags & required) != required: + return None + return joint.pose_matrix + except Exception as e: + carb.log_warn(f"OpenXR {hand} wrist fetch failed: {e}") + return None + + +def get_vive_wrist_ids(vive_data: Dict): + """Return tuple (wm0_id, wm1_id) if available, else Nones. Sort by key for stability.""" + wm_ids = [k for k in vive_data.keys() if len(k) >= 2 and k[:2] == 'WM'] + wm_ids.sort() + if len(wm_ids) >= 2: + return wm_ids[0], wm_ids[1] + if len(wm_ids) == 1: + return wm_ids[0], None + return None, None + + +def pose_to_matrix(pose: Dict) -> Gf.Matrix4d: + """Convert position and orientation to a 4x4 matrix.""" + pos, ori = pose['position'], pose['orientation'] + quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) + rot = Gf.Matrix3d().SetRotate(quat) + trans = Gf.Vec3d(pos[0], pos[1], pos[2]) + return Gf.Matrix4d(rot, trans) + +def matrix_to_pose(matrix: Gf.Matrix4d) -> Dict: + """Convert a 4x4 matrix to position and orientation.""" + pos = matrix.ExtractTranslation() + rot = matrix.ExtractRotation() + quat = rot.GetQuat() + return { + 'position': [pos[0], pos[1], pos[2]], + 'orientation': [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]] + } + +def get_pairing_error(trans_errs: list, rot_errs: list) -> float: + def _median(values: list) -> float: + try: + return float(np.median(np.asarray(values, dtype=np.float64))) + except Exception: + return float('inf') + return _median(trans_errs) + 0.01 * _median(rot_errs) diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py index 17b22e0f..dde64b08 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py @@ -26,29 +26,6 @@ def test_extension_import(): print(f"❌ Failed to import extension modules: {e}") return False -def test_basic_functionality(): - """Test basic functionality with mock data""" - try: - from xr_device_integration import XRDeviceIntegration - - # Create integration - integration = XRDeviceIntegration() - - # Update devices (will use mock data) - integration.update_all_devices() - - # Get data - all_data = integration.get_all_device_data() - - print("✅ Basic functionality test passed") - print(f"📊 Data keys: {list(all_data.keys())}") - - return True - - except Exception as e: - print(f"❌ Basic functionality test failed: {e}") - return False - def test_individual_trackers(): """Test individual tracker functionality""" try: diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 305df723..431a6024 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -133,7 +133,7 @@ # Sequential device updates to avoid resource contention # Alternate between Manus and Vive every frame - update_manus = (frame_counter % 2 == 1) + update_manus = (frame_counter % 2 == 0) frame_counter += 1 # Get current cube arrays From 5757de78e63dda7455c07b1508e1fffcaca9c1d0 Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Fri, 29 Aug 2025 15:59:01 -0700 Subject: [PATCH 6/9] fix orientations --- .../IsaacsimXrInputDevicesBindings.cpp | 2 +- .../config/extension.toml | 2 +- .../docs/CHANGELOG.md | 39 ++-- .../isaacsim.xr.input_devices/docs/README.md | 196 ++++++++---------- .../isaacsim/xr/input_devices/ManusTracker.h | 2 +- .../ManusTracker.cpp | 2 +- .../isaacsim.xr.input_devices/premake5.lua | 2 +- .../python/__init__.py | 2 +- .../python/impl/__init__.py | 2 +- .../python/impl/extension.py | 2 +- .../python/impl/manus_tracker.py | 8 +- .../python/impl/vive_tracker.py | 2 +- .../python/impl/xr_device_integration.py | 159 ++++++++------ .../python/impl/xr_utils.py | 9 +- .../python/tests/__init__.py | 2 +- .../python/tests/test_xr_input_devices.py | 119 +++++++---- .../manus_vive_tracking_sample.py | 15 +- 17 files changed, 298 insertions(+), 267 deletions(-) diff --git a/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp b/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp index 7906d89f..caa9b0b3 100644 --- a/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp +++ b/source/extensions/isaacsim.xr.input_devices/bindings/isaacsim.xr.input_devices/IsaacsimXrInputDevicesBindings.cpp @@ -33,4 +33,4 @@ PYBIND11_MODULE(_isaac_xr_input_devices, m) Cleanup SDK resources. )"); } -} \ No newline at end of file +} diff --git a/source/extensions/isaacsim.xr.input_devices/config/extension.toml b/source/extensions/isaacsim.xr.input_devices/config/extension.toml index f92f0a3b..d9800eb4 100644 --- a/source/extensions/isaacsim.xr.input_devices/config/extension.toml +++ b/source/extensions/isaacsim.xr.input_devices/config/extension.toml @@ -28,4 +28,4 @@ args = [ "--/app/asyncRendering=0", "--/app/fastShutdown=1", "--vulkan", -] \ No newline at end of file +] diff --git a/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md b/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md index b441c5a7..5fa488fc 100644 --- a/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md +++ b/source/extensions/isaacsim.xr.input_devices/docs/CHANGELOG.md @@ -8,23 +8,28 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [1.0.0] - 2025-08-08 ### Added -- Initial release of IsaacSim XR Input Devices extension -- Support for Manus gloves with ManusSDK (c++ with Python binding) -- Support for Vive trackers with pysurvive -- Integration with IsaacSim XR system -- Synchronized device updates -- Mock data fallback for testing without hardware -- Documentation and build configuration +- Initial release of `isaacsim.xr.input_devices` extension +- Manus gloves integration via C++ tracker with Python bindings +- Vive tracker integration via `pysurvive` (libsurvive) with mock fallback +- Unified Python API: `get_xr_device_integration().get_all_device_data()` +- Left/right wrist mapping resolution between Vive trackers and OpenXR wrists +- Static scene-to-lighthouse transform estimation with clustering and averaging +- Per-device connection status and last-data timestamps +- Sample visualization script for Manus and Vive devices +- Documentation and simple CLI-style tests -### Technical Details -- Adapted from isaac-deploy Holoscan implementations -- Rate-limited updates (60Hz default) -- Device status monitoring -- Cleanup and resource management +### Details +- Update cadence is rate-limited to 100 Hz +- Wrist mapping considers both pairings (WM0→L/WM1→R vs WM1→L/WM0→R), + accumulates per-frame translation/rotation errors, and chooses the better pairing +- Chosen pairing candidates are clustered (position/orientation margins) and averaged + to produce a stable `scene_T_lighthouse_static` transform +- Vive tracker poses are transformed into scene coordinates using the static transform +- Manus joint poses (relative to the wrist) and transformed into scene coordinates + using the resolved wrist mapping from vive tracker poses ### Dependencies -- IsaacSim Core API -- IsaacSim Core Utils -- Omniverse Kit XR Core -- ManusSDK (optional - falls back to mock data) -- pysurvive (optional - falls back to mock data) +- Isaac Sim Core APIs +- Omniverse Kit runtime (for logging and extension lifecycle) +- Manus SDK (optional; otherwise uses mock data) +- libsurvive / pysurvive (optional; otherwise uses mock data) diff --git a/source/extensions/isaacsim.xr.input_devices/docs/README.md b/source/extensions/isaacsim.xr.input_devices/docs/README.md index 4bcec37c..0457b347 100644 --- a/source/extensions/isaacsim.xr.input_devices/docs/README.md +++ b/source/extensions/isaacsim.xr.input_devices/docs/README.md @@ -1,56 +1,56 @@ # IsaacSim XR Input Devices Extension -This extension provides XR input device support for Manus gloves and Vive trackers, adapted from isaac-deploy Holoscan implementations. +This extension provides XR input device support for Manus gloves and Vive trackers, with a unified Python API and automatic wrist mapping between trackers and OpenXR wrists. ## Features -- **Manus Glove Integration**: Real-time hand and finger tracking with full skeletal data -- **Vive Tracker Integration**: 6DOF pose tracking for HMDs, controllers, and trackers -- **Live Data Visualization**: Red cubes appear at device positions in Isaac Sim -- **Automatic Setup**: Library paths configured automatically -- **Device Status Monitoring**: Connection state and data freshness tracking -- **Error Handling**: Graceful degradation when hardware/SDKs are unavailable -- **Mock Data Support**: Testing without physical hardware +- **Manus gloves**: Real-time hand/finger joint poses via C++ tracker with Python bindings +- **Vive trackers**: 6DOF poses via libsurvive (`pysurvive`) with mock fallback +- **Left/Right mapping**: Resolves which Vive tracker corresponds to left/right wrist +- **Scene alignment**: Estimates a stable scene↔lighthouse transform from early samples +- **Unified API**: Single call to fetch all device data and device status +- **Visualization sample**: Renders gloves/trackers as cubes in Isaac Sim ## Prerequisites -### Hardware Requirements -- **Manus Gloves**: Manus Prime or MetaGloves with license dongle -- **Vive Trackers**: SteamVR/Lighthouse tracking system with trackers +### Hardware +- **Manus Gloves**: Manus Prime/MetaGloves with license dongle +- **Vive Trackers**: Lighthouse tracking (SteamVR base stations + trackers) -### Software Dependencies +### Software +- **Manus SDK**: Bundled in Isaac Sim target-deps (mock used if missing) +- **libsurvive / pysurvive**: Required for real Vive tracking; otherwise mock data is used -#### Manus SDK -- ManusSDK is automatically included in Isaac Sim's target dependencies -- Requires Manus license dongle for operation -- Falls back to mock data if unavailable +Install libsurvive (either system-wide or in your home directory): -#### libsurvive (for Vive trackers) -Currently requires manual installation. Choose one option: - -**Option 1: System Installation** +System-wide: ```bash sudo apt update -sudo apt install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev liblapacke-dev libopenblas-dev libatlas-base-dev cmake +sudo apt install -y build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev \ + freeglut3-dev liblapacke-dev libopenblas-dev libatlas-base-dev cmake git clone https://github.com/cntools/libsurvive.git cd libsurvive sudo cp ./useful_files/81-vive.rules /etc/udev/rules.d/ sudo udevadm control --reload-rules && sudo udevadm trigger -make && cmake . && make -j$(nproc) +make && cmake . && make -j"$(nproc)" sudo make install && sudo ldconfig ``` -**Option 2: User Installation (Recommended)** +User (home) install (recommended during development): ```bash -# Build in home directory (detected automatically) git clone https://github.com/cntools/libsurvive.git ~/libsurvive cd ~/libsurvive sudo cp ./useful_files/81-vive.rules /etc/udev/rules.d/ sudo udevadm control --reload-rules && sudo udevadm trigger -make && cmake . && make -j$(nproc) +make && cmake . && make -j"$(nproc)" + +# Ensure Python can find pysurvive bindings +export PYTHONPATH="$HOME/libsurvive/bindings/python:$PYTHONPATH" ``` +Note: The Vive tracker wrapper currently prepends a libsurvive Python path. Adjust it as needed for your environment or set `PYTHONPATH` as shown above. + ## Building ```bash @@ -58,124 +58,106 @@ make && cmake . && make -j$(nproc) ./build.sh ``` -The extension is automatically built as part of Isaac Sim's build process. - ## Usage -### Running the Sample -A complete sample script demonstrates both Manus and Vive tracking: +### Sample +Run the visualization sample that renders Manus joints (red cubes) and Vive trackers (blue cubes): ```bash cd /path/to/IsaacSim -./_build/linux-x86_64/release/python.sh source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +./_build/linux-x86_64/release/python.sh \ + source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py ``` -### Python API Usage +### Python API ```python from isaacsim.xr.input_devices.impl.xr_device_integration import get_xr_device_integration -# Get the extension's device integration instance +# Obtain the shared integration instance from the extension integration = get_xr_device_integration() -# Get all device data +# Fetch all device data all_data = integration.get_all_device_data() -# Access specific device data manus_data = all_data.get('manus_gloves', {}) vive_data = all_data.get('vive_trackers', {}) - -# Check device status status = all_data.get('device_status', {}) -manus_connected = status.get('manus_gloves', {}).get('connected', False) -vive_connected = status.get('vive_trackers', {}).get('connected', False) -print(f"Manus connected: {manus_connected}") -print(f"Vive connected: {vive_connected}") +print(f"Manus connected: {status.get('manus_gloves', {}).get('connected', False)}") +print(f"Vive connected: {status.get('vive_trackers', {}).get('connected', False)}") ``` ### Data Format -**Manus Glove Data:** ```python { - 'manus_gloves': { - 'left_glove': { - 'position': np.array([...]), # 75 joint positions (x,y,z per joint) - 'orientation': np.array([...]), # 75 joint orientations (w,x,y,z per joint) - 'valid': True - }, - 'right_glove': { ... } - } + 'manus_gloves': { + 'left_0': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + 'left_1': { ... }, + 'right_0': { ... }, + # ... per-joint entries + }, + 'vive_trackers': { + '': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + # e.g., 'WM0', 'WM1', or device names from libsurvive + }, + 'device_status': { + 'manus_gloves': {'connected': bool, 'last_data_time': float}, + 'vive_trackers': {'connected': bool, 'last_data_time': float}, + 'left_hand_connected': bool, + 'right_hand_connected': bool + } } ``` -**Vive Tracker Data:** -```python -{ - 'vive_trackers': { - 'WM0_position': [x, y, z], # Tracker position - 'WM0_orientation': [w, x, y, z], # Tracker orientation - 'LH1_position': [x, y, z], # Lighthouse position - 'LH1_orientation': [w, x, y, z] # Lighthouse orientation - } -} -``` +## How left/right mapping is determined -## Extension Architecture - -``` -isaacsim.xr.input_devices/ -├── bindings/ # Pybind11 C++ bindings -├── include/ # C++ headers -├── plugins/ # C++ implementation -│ └── ManusTracker.cpp # Manus SDK integration -├── python/ -│ └── impl/ -│ ├── extension.py # Extension lifecycle -│ ├── xr_device_integration.py # Main orchestrator -│ ├── manus_tracker.py # Manus wrapper -│ └── vive_tracker.py # Vive wrapper -└── config/extension.toml # Extension configuration -``` +- Detect connected OpenXR wrists (left/right) and available Vive wrist markers (e.g., WM0/WM1) +- For each frame, compute candidate transforms for both pairings: + - Pair A: WM0→Left, WM1→Right + - Pair B: WM1→Left, WM0→Right +- Accumulate translation/rotation deltas per pairing when both wrists and trackers are present +- Choose the pairing: + - Prefer the pairing with more samples initially + - Once there are enough paired frames, choose the one with lower accumulated error +- Cluster the chosen pairing’s transforms and average them to estimate a stable + scene↔lighthouse transform (`scene_T_lighthouse_static`) +- Use the resolved mapping and transform to place all Vive and Manus data in scene coordinates ## Troubleshooting -### Manus Issues -- **License Error**: Unplug and replug the Manus license dongle -- **No Data**: Check if `ManusSDK_Integrated.so` is in `LD_LIBRARY_PATH` -- **Connection Failed**: Ensure Manus Core isn't running simultaneously - -### Vive Issues -- **No Trackers Found**: Verify SteamVR is not running (conflicts with libsurvive) -- **Lighthouse Only**: Check that trackers have tracking lock (LED should be solid) -- **pysurvive Import Error**: Install libsurvive (see Prerequisites section) +- Manus license issues: replug license dongle; ensure SDK libraries are discoverable +- libsurvive conflicts: ensure SteamVR is NOT running concurrently +- No Vive devices: verify udev rules and USB connections (solid tracker LED) +- Python import for `pysurvive`: set `PYTHONPATH` to libsurvive bindings path -### Build Issues -- Verify Isaac Sim build environment is set up correctly -- Check that Manus SDK is in `_build/target-deps/manus_sdk/` -- Ensure all C++ dependencies are available +## Extension Layout -### Runtime Issues -```bash -# Check device connections -lsusb | grep -i manus -lsusb | grep -i lighthouse - -# Monitor Isaac Sim logs for device initialization -# Look for: "Manus glove tracker initialized with SDK" -# Look for: "Adding tracked object WM0 from HTC" ``` - -## Contributing - -This extension follows Isaac Sim's extension development guidelines: -- C++ code uses pybind11 for Python bindings -- Python code follows Isaac Sim coding standards -- All changes should maintain backward compatibility -- Include tests for new functionality +isaacsim.xr.input_devices/ +├── bindings/ # Pybind11 bindings +├── include/ # C++ headers +├── plugins/ # C++ implementations +├── python/ +│ └── impl/ +│ ├── extension.py # Extension lifecycle +│ ├── xr_device_integration.py # Orchestrates devices & transforms +│ ├── manus_tracker.py # Manus wrapper +│ └── vive_tracker.py # Vive wrapper +└── docs/ + ├── README.md + └── CHANGELOG.md +``` ## License -Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. + SPDX-License-Identifier: Apache-2.0 diff --git a/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h b/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h index 268a662b..bba10dff 100644 --- a/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h +++ b/source/extensions/isaacsim.xr.input_devices/include/isaacsim/xr/input_devices/ManusTracker.h @@ -68,4 +68,4 @@ class ISAACSIM_XR_INPUT_DEVICES_DLL_EXPORT IsaacSimManusTracker } // namespace input_devices } // namespace xr -} // namespace isaacsim \ No newline at end of file +} // namespace isaacsim diff --git a/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp b/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp index 282cc35d..02a137e8 100644 --- a/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp +++ b/source/extensions/isaacsim.xr.input_devices/plugins/isaacsim.xr.input_devices/ManusTracker.cpp @@ -298,4 +298,4 @@ void IsaacSimManusTracker::OnErgonomicsStream(const ErgonomicsStream* ergonomics } // namespace input_devices } // namespace xr -} // namespace isaacsim \ No newline at end of file +} // namespace isaacsim diff --git a/source/extensions/isaacsim.xr.input_devices/premake5.lua b/source/extensions/isaacsim.xr.input_devices/premake5.lua index 03749288..6e933eeb 100644 --- a/source/extensions/isaacsim.xr.input_devices/premake5.lua +++ b/source/extensions/isaacsim.xr.input_devices/premake5.lua @@ -67,4 +67,4 @@ repo_build.prebuild_link { repo_build.prebuild_copy { { "python/*.py", ext.target_dir .. "/isaacsim/xr/input_devices" }, -} \ No newline at end of file +} diff --git a/source/extensions/isaacsim.xr.input_devices/python/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/__init__.py index cab54238..d4e49660 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/__init__.py +++ b/source/extensions/isaacsim.xr.input_devices/python/__init__.py @@ -15,4 +15,4 @@ from .impl.extension import Extension -__all__ = ["Extension"] \ No newline at end of file +__all__ = ["Extension"] diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py index 4130905b..cdbf4d2a 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/__init__.py @@ -23,4 +23,4 @@ "XRDeviceIntegration", "IsaacSimManusGloveTracker", "IsaacSimViveTracker" -] \ No newline at end of file +] diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py b/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py index a7ea2209..3414dd48 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/extension.py @@ -25,4 +25,4 @@ def _register_xr_devices(self): @classmethod def get_instance(cls): - return cls._instance \ No newline at end of file + return cls._instance diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py index 30779152..b51a326f 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py @@ -45,11 +45,11 @@ def update(self): else: # Provide mock data self.glove_data = { - 'left_joint_0': { + 'left_0': { 'position': [0.0, 0.0, 0.0], 'orientation': [1.0, 0.0, 0.0, 0.0] }, - 'right_joint_0': { + 'right_0': { 'position': [0.1, 0.0, 0.0], 'orientation': [1.0, 0.0, 0.0, 0.0] } @@ -68,7 +68,7 @@ def _populate_glove_data(self, raw_data: Dict, hand: str) -> None: joint_count = len(position) // 3 # 3 values per position for joint_idx in range(joint_count): - joint_name = f"{hand}_joint_{joint_idx}" + joint_name = f"{hand}_{joint_idx}" self.glove_data[joint_name] = { 'position': [float(position[i]) for i in range(joint_idx * 3, joint_idx * 3 + 3)], 'orientation': [float(orientation[i]) for i in range(joint_idx * 4, joint_idx * 4 + 4)] @@ -83,4 +83,4 @@ def cleanup(self): self._manus_tracker.cleanup() self.is_connected = False except Exception as e: - carb.log_error(f"Error during Manus tracker cleanup: {e}") \ No newline at end of file + carb.log_error(f"Error during Manus tracker cleanup: {e}") diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index 97d03929..506fddb6 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -87,4 +87,4 @@ def cleanup(self): survive_simple_close(self._ctx.ptr) self.is_connected = False except Exception as e: - carb.log_error(f"Error during Vive tracker cleanup: {e}") \ No newline at end of file + carb.log_error(f"Error during Vive tracker cleanup: {e}") diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index e21d2910..877711a1 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -35,8 +35,12 @@ def __init__(self): 'left_hand_connected': False, 'right_hand_connected': False } + self.default_pose = {'position': [0, 0, 0],'orientation': [1, 0, 0, 0]} self.last_update_time = 0 self.update_rate = 100.0 + # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis + self.rot_adjust = Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))) + self.manus_update_frame = True self.scene_T_lighthouse_static = None self._vive_left_id = None self._vive_right_id = None @@ -64,6 +68,55 @@ def register_devices(self): except Exception as e: carb.log_error(f"Failed to register XR devices: {e}") + def get_all_device_data(self) -> Dict: + """ + Get all device data. + Format: { + 'manus_gloves': { + '{left/right}_{joint_index}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + }, + 'vive_trackers': { + '{vive_tracker_id}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + }, + 'device_status': { + 'manus_gloves': {'connected': bool, 'last_data_time': float}, + 'vive_trackers': {'connected': bool, 'last_data_time': float}, + 'left_hand_connected': bool, + 'right_hand_connected': bool + } + } + """ + if self.manus_update_frame: + self.update_manus() + else: + self.update_vive() + + # Get raw data from trackers + manus_data = self.manus_tracker.get_all_glove_data() + vive_data = self.vive_tracker.get_all_tracker_data() + vive_transformed = self._transform_vive_data(vive_data) + scene_T_wrist = self._get_scene_T_wrist_matrix(vive_transformed) + + return { + 'manus_gloves': self._transform_manus_data(manus_data, scene_T_wrist), + 'vive_trackers': vive_transformed, + 'device_status': self.device_status + } + + def cleanup(self): + if hasattr(self, 'manus_tracker'): + self.manus_tracker.cleanup() + if hasattr(self, 'vive_tracker'): + self.vive_tracker.cleanup() + def update_manus(self): """Update raw Manus glove data. """ current_time = time.time() @@ -73,8 +126,9 @@ def update_manus(self): self.manus_tracker.update() self.device_status['manus_gloves']['last_data_time'] = current_time manus_data = self.manus_tracker.get_all_glove_data() - self.device_status['left_hand_connected'] = 'left_joint_0' in manus_data - self.device_status['right_hand_connected'] = 'right_joint_0' in manus_data + self.device_status['left_hand_connected'] = 'left_0' in manus_data + self.device_status['right_hand_connected'] = 'right_0' in manus_data + self.manus_update_frame = False def update_vive(self): """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update. """ @@ -84,6 +138,7 @@ def update_vive(self): self.vive_tracker.update() self.device_status['vive_trackers']['last_data_time'] = current_time + self.manus_update_frame = True try: # Initialize coordinate transformation from first Vive wrist position if self.scene_T_lighthouse_static is None: @@ -97,11 +152,8 @@ def _initialize_coordinate_transformation(self): The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. It is computed from multiple frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. """ - params = { - 'mapping_margin_trans_m': 0.015, - 'mapping_margin_rot_deg': 3.0, - 'mapping_min_pair_frames': 6 - } + min_frames = 6 + tolerance = 3.0 vive_data = self.vive_tracker.get_all_tracker_data() wm0_id, wm1_id = get_vive_wrist_ids(vive_data) if wm0_id is None and wm1_id is None: @@ -141,23 +193,29 @@ def _initialize_coordinate_transformation(self): self._pairB_candidates.append(TR0) # Per-frame pairing error if both candidates present - if len(gloves) == 2 and len(vives) == 2: + if TL0 is not None and TR1 is not None and TL1 is not None and TR0 is not None: eT, eR = compute_delta_errors(TL0, TR1) self._pairA_trans_errs.append(eT) self._pairA_rot_errs.append(eR) eT, eR = compute_delta_errors(TL1, TR0) self._pairB_trans_errs.append(eT) self._pairB_rot_errs.append(eR) - # Choose a mapping - enough_pair_frames = (len(self._pairA_trans_errs) >= params['mapping_min_pair_frames'] and - len(self._pairB_trans_errs) >= params['mapping_min_pair_frames']) - - choose_A = len(self._pairA_candidates) > len(self._pairB_candidates) - if enough_pair_frames: + choose_A = None + if len(self._pairA_candidates) == 0 and len(self._pairB_candidates) >= min_frames: + choose_A = False + elif len(self._pairB_candidates) == 0 and len(self._pairA_candidates) >= min_frames: + choose_A = True + elif len(self._pairA_trans_errs) >= min_frames and len(self._pairB_trans_errs) >= min_frames: errA = get_pairing_error(self._pairA_trans_errs, self._pairA_rot_errs) errB = get_pairing_error(self._pairB_trans_errs, self._pairB_rot_errs) - choose_A = errA <= errB + carb.log_error(f"Wrist calibration: errA {errA}, errB {errB}") + if errA <= errB and errA < tolerance: + choose_A = True + elif errB <= errA and errB < tolerance: + choose_A = False + if choose_A is None: + return if choose_A: chosen_list = self._pairA_candidates @@ -166,61 +224,31 @@ def _initialize_coordinate_transformation(self): chosen_list = self._pairB_candidates self._vive_left_id, self._vive_right_id = wm1_id, wm0_id - if len(chosen_list) >= params['mapping_min_pair_frames']: + if len(chosen_list) >= min_frames: cluster = select_mode_cluster(chosen_list) carb.log_error(f"Wrist calibration: formed size {len(cluster)} cluster from {len(chosen_list)} samples") - if len(cluster) >= params['mapping_min_pair_frames'] // 2: + if len(cluster) >= min_frames // 2: averaged = average_transforms(cluster) self.scene_T_lighthouse_static = averaged carb.log_error(f"Resolved mapping: {self._vive_left_id}->Left, {self._vive_right_id}->Right") - else: - carb.log_error(f"Waiting for more samples to average wrist transforms") except Exception as e: carb.log_error(f"Failed to initialize coordinate transformation: {e}") - - def _transform_to_scene_coordinates(self, wrist_data: Dict) -> Dict: - """Transform vive wrist coordinates to scene coordinates using scene_T_lighthouse_static.""" - if self.scene_T_lighthouse_static is None: - return wrist_data - - try: - transformed_matrix = pose_to_matrix(wrist_data) * self.scene_T_lighthouse_static - # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis - # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm - Rcorr = Gf.Matrix4d(Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))), Gf.Vec3d(0, -0.1, 0)) - return matrix_to_pose(Rcorr * transformed_matrix) - - except Exception as e: - carb.log_error(f"Failed to transform coordinates: {e}") - return wrist_data - - def get_all_device_data(self) -> Dict: - # Get raw data from trackers - manus_data = self.manus_tracker.get_all_glove_data() - vive_data = self.vive_tracker.get_all_tracker_data() - vive_transformed = self._transform_vive_data(vive_data) - scene_T_wrist = self._get_scene_T_wrist(vive_transformed) - - return { - 'manus_gloves': self._transform_manus_data(manus_data, scene_T_wrist), - 'vive_trackers': vive_transformed, - 'device_status': self.device_status - } - def _transform_vive_data(self, device_data: Dict) -> Dict: """Transform all joints in vive data to scene coordinates. """ - if self.scene_T_lighthouse_static is None: - return device_data - + # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, -0.1, 0)) transformed_data = {} for joint_name, joint_data in device_data.items(): - transformed_data[joint_name] = self._transform_to_scene_coordinates(joint_data) - + transformed_pose = self.default_pose + if self.scene_T_lighthouse_static is not None: + transformed_matrix = pose_to_matrix(joint_data) * self.scene_T_lighthouse_static + transformed_pose = matrix_to_pose(Rcorr * transformed_matrix) + transformed_data[joint_name] = transformed_pose return transformed_data - def _get_scene_T_wrist(self, vive_data: Dict) -> Dict: + def _get_scene_T_wrist_matrix(self, vive_data: Dict) -> Dict: """Get the wrist position and orientation from vive data.""" scene_T_wrist = {'left': Gf.Matrix4d().SetIdentity(), 'right': Gf.Matrix4d().SetIdentity()} if self._vive_left_id is not None: @@ -230,19 +258,14 @@ def _get_scene_T_wrist(self, vive_data: Dict) -> Dict: return scene_T_wrist def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Dict) -> Dict: - """Transform Manus glove data: wrist * wrist_T_joint.""" + """Transform Manus glove data: relative joint poses to wrist -> joint in scene coordinates""" + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, 0, 0)).GetInverse() transformed_data = {} for joint_name, joint_data in manus_data.items(): - if joint_name.startswith('left_'): - joint_transformed = pose_to_matrix(joint_data) * scene_T_wrist['left'] - transformed_data[joint_name] = matrix_to_pose(joint_transformed) - elif joint_name.startswith('right_'): - joint_transformed = pose_to_matrix(joint_data) * scene_T_wrist['right'] - transformed_data[joint_name] = matrix_to_pose(joint_transformed) + hand, _ = joint_name.split('_') + joint_mat = Rcorr * pose_to_matrix(joint_data) * scene_T_wrist[hand] + transformed_data[joint_name] = matrix_to_pose(joint_mat) + # Calculate palm with middle metacarpal and proximal + transformed_data['left_25'] = get_palm(transformed_data, 'left') + transformed_data['right_25'] = get_palm(transformed_data, 'right') return transformed_data - - def cleanup(self): - if hasattr(self, 'manus_tracker'): - self.manus_tracker.cleanup() - if hasattr(self, 'vive_tracker'): - self.vive_tracker.cleanup() \ No newline at end of file diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py index 0b84fdd3..59aee075 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py @@ -53,7 +53,7 @@ def average_transforms(mats: List[Gf.Matrix4d]) -> Gf.Matrix4d: return Gf.Matrix4d(rot3, trans) -def select_mode_cluster(mats: List[Gf.Matrix4d], trans_thresh_m: float = 0.02, rot_thresh_deg: float = 5.0) -> List[Gf.Matrix4d]: +def select_mode_cluster(mats: List[Gf.Matrix4d], trans_thresh_m: float = 0.03, rot_thresh_deg: float = 10.0) -> List[Gf.Matrix4d]: """Return the largest cluster (mode) under proximity thresholds.""" if not mats: return [] @@ -125,3 +125,10 @@ def _median(values: list) -> float: except Exception: return float('inf') return _median(trans_errs) + 0.01 * _median(rot_errs) + +def get_palm(transformed_data: Dict, hand: str) -> Dict: + """Get palm position and orientation from transformed data.""" + metacarpal = transformed_data[f'{hand}_6'] + proximal = transformed_data[f'{hand}_7'] + pos = (np.array(metacarpal['position']) + np.array(proximal['position'])) / 2.0 + return {'position': [pos[0], pos[1], pos[2]], 'orientation': metacarpal['orientation']} diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py b/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py index 2b26afbb..a946756e 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/__init__.py @@ -15,4 +15,4 @@ """ Tests for IsaacSim XR Input Devices extension. -""" \ No newline at end of file +""" diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py index dde64b08..e39f61ca 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py @@ -1,78 +1,105 @@ #!/usr/bin/env python3 """ -Basic test for IsaacSim XR Input Devices extension -Tests that the extension can be imported and basic functionality works +Basic tests for isaacsim.xr.input_devices +- Verifies modules import +- Exercises individual trackers (mock-safe) +- Exercises XRDeviceIntegration end-to-end structure """ -import sys import os +import sys # Add the impl directory to Python path so we can import the modules directly impl_path = os.path.join(os.path.dirname(__file__), '..', 'impl') -sys.path.insert(0, impl_path) +sys.path.insert(0, os.path.abspath(impl_path)) def test_extension_import(): - """Test that the extension modules can be imported""" + """Test that core modules can be imported.""" try: - # Test importing the modules directly - from xr_device_integration import XRDeviceIntegration - from manus_tracker import IsaacSimManusGloveTracker - from vive_tracker import IsaacSimViveTracker - from extension import Extension - - print("✅ All extension modules imported successfully") + from xr_device_integration import XRDeviceIntegration, get_xr_device_integration # noqa: F401 + from manus_tracker import IsaacSimManusGloveTracker # noqa: F401 + from vive_tracker import IsaacSimViveTracker # noqa: F401 + print("Modules imported successfully") return True - except ImportError as e: - print(f"❌ Failed to import extension modules: {e}") + except Exception as e: + print(f"Import failed: {e}") return False def test_individual_trackers(): - """Test individual tracker functionality""" + """Test individual tracker classes (mock-friendly).""" try: from manus_tracker import IsaacSimManusGloveTracker from vive_tracker import IsaacSimViveTracker - - # Test Manus tracker - manus_tracker = IsaacSimManusGloveTracker() - manus_tracker.update() - manus_data = manus_tracker.get_all_glove_data() - print(f"✅ Manus tracker: {len(manus_data)} glove(s)") - - # Test Vive tracker - vive_tracker = IsaacSimViveTracker() - vive_tracker.update() - vive_data = vive_tracker.get_all_tracker_data() - print(f"✅ Vive tracker: {len(vive_data)} tracker(s)") - + + manus = IsaacSimManusGloveTracker() + manus.update() + manus_data = manus.get_all_glove_data() + print(f"Manus tracker: {len(manus_data)} joint entries") + + vive = IsaacSimViveTracker() + vive.update() + vive_data = vive.get_all_tracker_data() + print(f"Vive tracker: {len(vive_data)} device entries") + return True - except Exception as e: - print(f"❌ Individual tracker test failed: {e}") + print(f"Individual tracker test failed: {e}") + return False + +def test_basic_functionality(): + """Test XRDeviceIntegration orchestration and data schema.""" + try: + from xr_device_integration import XRDeviceIntegration + integration = XRDeviceIntegration() + + # Register and perform at least one update on both sources + integration.register_devices() + integration.update_manus() + integration.update_vive() + + all_data = integration.get_all_device_data() + assert isinstance(all_data, dict) + assert 'manus_gloves' in all_data + assert 'vive_trackers' in all_data + assert 'device_status' in all_data + + # Validate structure if entries are present + for group_key in ('manus_gloves', 'vive_trackers'): + for _, pose in all_data[group_key].items(): + pos = pose.get('position') + ori = pose.get('orientation') + assert isinstance(pos, (list, tuple)) and len(pos) == 3 + assert isinstance(ori, (list, tuple)) and len(ori) == 4 + + print("Integration produced structured device data") + integration.cleanup() + return True + except AssertionError as e: + print(f"Data shape assertion failed: {e}") + return False + except Exception as e: + print(f"Integration test failed: {e}") return False if __name__ == "__main__": - print("🚀 Testing IsaacSim XR Input Devices Extension") + print("Testing isaacsim.xr.input_devices") print("=" * 40) - + tests = [ - ("Extension Import", test_extension_import), + ("Module Imports", test_extension_import), ("Basic Functionality", test_basic_functionality), ("Individual Trackers", test_individual_trackers), ] - + passed = 0 - for test_name, test_func in tests: - print(f"\n{test_name}:") - if test_func(): + for name, fn in tests: + print(f"\n{name}:") + if fn(): passed += 1 - - print(f"\n📊 Results: {passed}/{len(tests)} tests passed") - + + print(f"\nResults: {passed}/{len(tests)} tests passed") if passed == len(tests): - print("🎉 Extension is working correctly!") - print("\n💡 To test with real hardware:") - print(" - Connect Manus gloves") - print(" - Connect Vive trackers") - print(" - Run IsaacSim and load the extension") + print("All tests passed") + print("\nFor hardware testing: connect Manus gloves and Vive trackers, then run the sample.") else: - print("⚠️ Some tests failed") \ No newline at end of file + print("Some tests failed") \ No newline at end of file diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 431a6024..4dd39bb0 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -110,9 +110,6 @@ my_world.reset() reset_needed = False -# Frame counter for sequential device updates -frame_counter = 0 - # Get attribute references for faster access positions_attr = point_instancer.GetPositionsAttr() orientations_attr = point_instancer.GetOrientationsAttr() @@ -131,11 +128,6 @@ my_world.reset() reset_needed = False - # Sequential device updates to avoid resource contention - # Alternate between Manus and Vive every frame - update_manus = (frame_counter % 2 == 0) - frame_counter += 1 - # Get current cube arrays current_positions = positions_attr.Get() current_orientations = orientations_attr.Get() @@ -146,11 +138,6 @@ proto_idx_attr.Set(proto_indices) cube_idx = 0 - if update_manus: - xr_integration.update_manus() - else: - xr_integration.update_vive() - all_device_data = xr_integration.get_all_device_data() manus_data = all_device_data.get('manus_gloves', {}) vive_data = all_device_data.get('vive_trackers', {}) @@ -182,4 +169,4 @@ # Cleanup xr_integration.cleanup() -simulation_app.close() \ No newline at end of file +simulation_app.close() From fa86e7fde1fbd748216a521aa6053cf47e7751ad Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 2 Sep 2025 15:52:15 -0700 Subject: [PATCH 7/9] move logic to isaaclab --- .../python/impl/manus_tracker.py | 2 +- .../python/impl/vive_tracker.py | 19 +- .../python/impl/xr_device_integration.py | 215 ------------------ .../python/tests/test_xr_input_devices.py | 4 +- .../manus_vive_tracking_sample.py | 8 +- 5 files changed, 18 insertions(+), 230 deletions(-) diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py index b51a326f..8c075fe2 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/manus_tracker.py @@ -74,7 +74,7 @@ def _populate_glove_data(self, raw_data: Dict, hand: str) -> None: 'orientation': [float(orientation[i]) for i in range(joint_idx * 4, joint_idx * 4 + 4)] } - def get_all_glove_data(self) -> Dict: + def get_data(self) -> Dict: return self.glove_data def cleanup(self): diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index 506fddb6..cb841d47 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -3,19 +3,24 @@ import sys import carb +import os from typing import Dict from pxr import Gf try: - # Add libsurvive pysurvive bindings to Python path - libsurvive_python_path = "/home/yuanchenl/libsurvive/bindings/python" ###FIXME: hardcoded path - if libsurvive_python_path not in sys.path: - sys.path.insert(0, libsurvive_python_path) - + # Optionally add libsurvive pysurvive bindings to Python path via env var + libsurvive_python_path = os.environ.get("LIBSURVIVE_PYTHON_PATH") + if libsurvive_python_path: + if libsurvive_python_path not in sys.path: + sys.path.insert(0, libsurvive_python_path) + carb.log_info(f"Using LIBSURVIVE_PYTHON_PATH={libsurvive_python_path}") + else: + carb.log_warn("LIBSURVIVE_PYTHON_PATH is not set; trying to import pysurvive from default sys.path") + import pysurvive from pysurvive.pysurvive_generated import survive_simple_close PYSURVIVE_AVAILABLE = True - carb.log_info("pysurvive imported successfully from libsurvive source") + carb.log_info("pysurvive imported successfully") except ImportError as e: carb.log_warn(f"pysurvive not available - using mock data for vive: {e}") PYSURVIVE_AVAILABLE = False @@ -77,7 +82,7 @@ def update(self): except Exception as e: carb.log_error(f"Failed to update Vive tracker data: {e}") - def get_all_tracker_data(self) -> Dict: + def get_data(self) -> Dict: return self.device_data def cleanup(self): diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py index 877711a1..3fdf603b 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_device_integration.py @@ -2,12 +2,8 @@ # SPDX-License-Identifier: Apache-2.0 import carb -import time -from typing import Dict -from pxr import Gf from .manus_tracker import IsaacSimManusGloveTracker from .vive_tracker import IsaacSimViveTracker -from .xr_utils import * def get_xr_device_integration(): @@ -35,21 +31,6 @@ def __init__(self): 'left_hand_connected': False, 'right_hand_connected': False } - self.default_pose = {'position': [0, 0, 0],'orientation': [1, 0, 0, 0]} - self.last_update_time = 0 - self.update_rate = 100.0 - # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis - self.rot_adjust = Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))) - self.manus_update_frame = True - self.scene_T_lighthouse_static = None - self._vive_left_id = None - self._vive_right_id = None - self._pairA_candidates = [] # Pair A: WM0->Left, WM1->Right - self._pairB_candidates = [] # Pair B: WM1->Left, WM0->Right - self._pairA_trans_errs = [] - self._pairA_rot_errs = [] - self._pairB_trans_errs = [] - self._pairB_rot_errs = [] def register_devices(self): try: @@ -68,204 +49,8 @@ def register_devices(self): except Exception as e: carb.log_error(f"Failed to register XR devices: {e}") - def get_all_device_data(self) -> Dict: - """ - Get all device data. - Format: { - 'manus_gloves': { - '{left/right}_{joint_index}': { - 'position': [x, y, z], - 'orientation': [w, x, y, z] - }, - ... - }, - 'vive_trackers': { - '{vive_tracker_id}': { - 'position': [x, y, z], - 'orientation': [w, x, y, z] - }, - ... - }, - 'device_status': { - 'manus_gloves': {'connected': bool, 'last_data_time': float}, - 'vive_trackers': {'connected': bool, 'last_data_time': float}, - 'left_hand_connected': bool, - 'right_hand_connected': bool - } - } - """ - if self.manus_update_frame: - self.update_manus() - else: - self.update_vive() - - # Get raw data from trackers - manus_data = self.manus_tracker.get_all_glove_data() - vive_data = self.vive_tracker.get_all_tracker_data() - vive_transformed = self._transform_vive_data(vive_data) - scene_T_wrist = self._get_scene_T_wrist_matrix(vive_transformed) - - return { - 'manus_gloves': self._transform_manus_data(manus_data, scene_T_wrist), - 'vive_trackers': vive_transformed, - 'device_status': self.device_status - } - def cleanup(self): if hasattr(self, 'manus_tracker'): self.manus_tracker.cleanup() if hasattr(self, 'vive_tracker'): self.vive_tracker.cleanup() - - def update_manus(self): - """Update raw Manus glove data. """ - current_time = time.time() - if current_time - self.device_status['manus_gloves']['last_data_time'] < 1.0 / self.update_rate: - return - - self.manus_tracker.update() - self.device_status['manus_gloves']['last_data_time'] = current_time - manus_data = self.manus_tracker.get_all_glove_data() - self.device_status['left_hand_connected'] = 'left_0' in manus_data - self.device_status['right_hand_connected'] = 'right_0' in manus_data - self.manus_update_frame = False - - def update_vive(self): - """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update. """ - current_time = time.time() - if current_time - self.device_status['vive_trackers']['last_data_time'] < 1.0 / self.update_rate: - return - - self.vive_tracker.update() - self.device_status['vive_trackers']['last_data_time'] = current_time - self.manus_update_frame = True - try: - # Initialize coordinate transformation from first Vive wrist position - if self.scene_T_lighthouse_static is None: - self._initialize_coordinate_transformation() - except Exception as e: - carb.log_error(f"Vive tracker update failed: {e}") - - def _initialize_coordinate_transformation(self): - """ - Initialize the scene to lighthouse coordinate transformation. - The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. - It is computed from multiple frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. - """ - min_frames = 6 - tolerance = 3.0 - vive_data = self.vive_tracker.get_all_tracker_data() - wm0_id, wm1_id = get_vive_wrist_ids(vive_data) - if wm0_id is None and wm1_id is None: - return - - try: - # Fetch OpenXR wrists - L, R, gloves = None, None, [] - if self.device_status['left_hand_connected']: - gloves.append('left') - L = get_openxr_wrist_matrix('left') - if self.device_status['right_hand_connected']: - gloves.append('right') - R = get_openxr_wrist_matrix('right') - - M0, M1, vives = None, None, [] - if wm0_id is not None: - vives.append(wm0_id) - M0 = pose_to_matrix(vive_data[wm0_id]) - if wm1_id is not None: - vives.append(wm1_id) - M1 = pose_to_matrix(vive_data[wm1_id]) - - TL0, TL1, TR0, TR1 = None, None, None, None - # Compute transforms for available pairs - if wm0_id is not None and L is not None: - TL0 = M0.GetInverse() * L - self._pairA_candidates.append(TL0) - if wm1_id is not None and L is not None: - TL1 = M1.GetInverse() * L - self._pairB_candidates.append(TL1) - if wm1_id is not None and R is not None: - TR1 = M1.GetInverse() * R - self._pairA_candidates.append(TR1) - if wm0_id is not None and R is not None: - TR0 = M0.GetInverse() * R - self._pairB_candidates.append(TR0) - - # Per-frame pairing error if both candidates present - if TL0 is not None and TR1 is not None and TL1 is not None and TR0 is not None: - eT, eR = compute_delta_errors(TL0, TR1) - self._pairA_trans_errs.append(eT) - self._pairA_rot_errs.append(eR) - eT, eR = compute_delta_errors(TL1, TR0) - self._pairB_trans_errs.append(eT) - self._pairB_rot_errs.append(eR) - # Choose a mapping - choose_A = None - if len(self._pairA_candidates) == 0 and len(self._pairB_candidates) >= min_frames: - choose_A = False - elif len(self._pairB_candidates) == 0 and len(self._pairA_candidates) >= min_frames: - choose_A = True - elif len(self._pairA_trans_errs) >= min_frames and len(self._pairB_trans_errs) >= min_frames: - errA = get_pairing_error(self._pairA_trans_errs, self._pairA_rot_errs) - errB = get_pairing_error(self._pairB_trans_errs, self._pairB_rot_errs) - carb.log_error(f"Wrist calibration: errA {errA}, errB {errB}") - if errA <= errB and errA < tolerance: - choose_A = True - elif errB <= errA and errB < tolerance: - choose_A = False - if choose_A is None: - return - - if choose_A: - chosen_list = self._pairA_candidates - self._vive_left_id, self._vive_right_id = wm0_id, wm1_id - else: - chosen_list = self._pairB_candidates - self._vive_left_id, self._vive_right_id = wm1_id, wm0_id - - if len(chosen_list) >= min_frames: - cluster = select_mode_cluster(chosen_list) - carb.log_error(f"Wrist calibration: formed size {len(cluster)} cluster from {len(chosen_list)} samples") - if len(cluster) >= min_frames // 2: - averaged = average_transforms(cluster) - self.scene_T_lighthouse_static = averaged - carb.log_error(f"Resolved mapping: {self._vive_left_id}->Left, {self._vive_right_id}->Right") - - except Exception as e: - carb.log_error(f"Failed to initialize coordinate transformation: {e}") - - def _transform_vive_data(self, device_data: Dict) -> Dict: - """Transform all joints in vive data to scene coordinates. """ - # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm - Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, -0.1, 0)) - transformed_data = {} - for joint_name, joint_data in device_data.items(): - transformed_pose = self.default_pose - if self.scene_T_lighthouse_static is not None: - transformed_matrix = pose_to_matrix(joint_data) * self.scene_T_lighthouse_static - transformed_pose = matrix_to_pose(Rcorr * transformed_matrix) - transformed_data[joint_name] = transformed_pose - return transformed_data - - def _get_scene_T_wrist_matrix(self, vive_data: Dict) -> Dict: - """Get the wrist position and orientation from vive data.""" - scene_T_wrist = {'left': Gf.Matrix4d().SetIdentity(), 'right': Gf.Matrix4d().SetIdentity()} - if self._vive_left_id is not None: - scene_T_wrist['left'] = pose_to_matrix(vive_data[self._vive_left_id]) - if self._vive_right_id is not None: - scene_T_wrist['right'] = pose_to_matrix(vive_data[self._vive_right_id]) - return scene_T_wrist - - def _transform_manus_data(self, manus_data: Dict, scene_T_wrist: Dict) -> Dict: - """Transform Manus glove data: relative joint poses to wrist -> joint in scene coordinates""" - Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, 0, 0)).GetInverse() - transformed_data = {} - for joint_name, joint_data in manus_data.items(): - hand, _ = joint_name.split('_') - joint_mat = Rcorr * pose_to_matrix(joint_data) * scene_T_wrist[hand] - transformed_data[joint_name] = matrix_to_pose(joint_mat) - # Calculate palm with middle metacarpal and proximal - transformed_data['left_25'] = get_palm(transformed_data, 'left') - transformed_data['right_25'] = get_palm(transformed_data, 'right') - return transformed_data diff --git a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py index e39f61ca..18a51034 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py +++ b/source/extensions/isaacsim.xr.input_devices/python/tests/test_xr_input_devices.py @@ -33,12 +33,12 @@ def test_individual_trackers(): manus = IsaacSimManusGloveTracker() manus.update() - manus_data = manus.get_all_glove_data() + manus_data = manus.get_data() print(f"Manus tracker: {len(manus_data)} joint entries") vive = IsaacSimViveTracker() vive.update() - vive_data = vive.get_all_tracker_data() + vive_data = vive.get_data() print(f"Vive tracker: {len(vive_data)} device entries") return True diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 4dd39bb0..1ef350e6 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -21,9 +21,8 @@ - Red cubes (0.01 size): Manus glove joints - Blue cubes (0.02 size): Vive tracker positions -IMPORTANT: To avoid resource contention and crashes, ensure Manus and Vive devices -are connected to different USB controllers/buses. Use 'lsusb -t' to identify -different buses and connect devices accordingly. +DEPRECATED: This sample is deprecated. The coordinate transformation logic has +been moved to IsaacLab. """ import os @@ -32,8 +31,7 @@ # Initialize simulation app simulation_app = SimulationApp( - {"headless": False}, - experience=f'/home/yuanchenl/Desktop/IsaacSim/source/apps/isaacsim.exp.base.xr.openxr.kit' + {"headless": False}, experience=f'{os.environ["EXP_PATH"]}/isaacsim.exp.base.xr.openxr.kit' ) import carb From dac28e3cd4e1557e6960bdb9c33bd7e01aa40613 Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Tue, 2 Sep 2025 17:02:54 -0700 Subject: [PATCH 8/9] clean up --- .../python/impl/xr_utils.py | 134 ------------------ 1 file changed, 134 deletions(-) delete mode 100644 source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py b/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py deleted file mode 100644 index 59aee075..00000000 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/xr_utils.py +++ /dev/null @@ -1,134 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -import carb -import numpy as np -from pxr import Gf -from omni.kit.xr.core import XRCore, XRPoseValidityFlags -from typing import Dict, List, Tuple - - -def compute_delta_errors(a: Gf.Matrix4d, b: Gf.Matrix4d) -> Tuple[float, float]: - """Return (translation_error_m, rotation_error_deg) between transforms a and b.""" - try: - delta = a * b.GetInverse() - t = delta.ExtractTranslation() - trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) - q = delta.ExtractRotation().GetQuat() - w = float(max(min(q.GetReal(), 1.0), -1.0)) - ang = 2.0 * float(np.arccos(w)) - ang_deg = float(np.degrees(ang)) - if ang_deg > 180.0: - ang_deg = 360.0 - ang_deg - return trans_err, ang_deg - except Exception: - return float('inf'), float('inf') - - -def average_transforms(mats: List[Gf.Matrix4d]) -> Gf.Matrix4d: - """Average rigid transforms: mean translation and normalized mean quaternion (with sign alignment).""" - if not mats: - return None - ref_quat = mats[0].ExtractRotation().GetQuat() - ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) - acc_q = np.zeros(4, dtype=np.float64) - acc_t = np.zeros(3, dtype=np.float64) - for m in mats: - t = m.ExtractTranslation() - acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) - q = m.ExtractRotation().GetQuat() - qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) - if np.dot(qi, ref) < 0.0: - qi = -qi - acc_q += qi - mean_t = acc_t / float(len(mats)) - norm = np.linalg.norm(acc_q) - if norm <= 1e-12: - quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) - else: - qn = acc_q / norm - quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) - rot3 = Gf.Matrix3d().SetRotate(quat_avg) - trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) - return Gf.Matrix4d(rot3, trans) - - -def select_mode_cluster(mats: List[Gf.Matrix4d], trans_thresh_m: float = 0.03, rot_thresh_deg: float = 10.0) -> List[Gf.Matrix4d]: - """Return the largest cluster (mode) under proximity thresholds.""" - if not mats: - return [] - best_cluster: List[Gf.Matrix4d] = [] - for center in mats: - cluster: List[Gf.Matrix4d] = [] - for m in mats: - trans_err, rot_err = compute_delta_errors(m, center) - if trans_err <= trans_thresh_m and rot_err <= rot_thresh_deg: - cluster.append(m) - if len(cluster) > len(best_cluster): - best_cluster = cluster - return best_cluster - - -def get_openxr_wrist_matrix(hand: str) -> Gf.Matrix4d: - """Get OpenXR wrist world matrix if valid, else None.""" - hand = hand.lower() - try: - hand_device = XRCore.get_singleton().get_input_device(f"/user/hand/{hand}") - if hand_device is None: - return None - joints = hand_device.get_all_virtual_world_poses() - if 'wrist' not in joints: - return None - joint = joints['wrist'] - required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID - if (joint.validity_flags & required) != required: - return None - return joint.pose_matrix - except Exception as e: - carb.log_warn(f"OpenXR {hand} wrist fetch failed: {e}") - return None - - -def get_vive_wrist_ids(vive_data: Dict): - """Return tuple (wm0_id, wm1_id) if available, else Nones. Sort by key for stability.""" - wm_ids = [k for k in vive_data.keys() if len(k) >= 2 and k[:2] == 'WM'] - wm_ids.sort() - if len(wm_ids) >= 2: - return wm_ids[0], wm_ids[1] - if len(wm_ids) == 1: - return wm_ids[0], None - return None, None - - -def pose_to_matrix(pose: Dict) -> Gf.Matrix4d: - """Convert position and orientation to a 4x4 matrix.""" - pos, ori = pose['position'], pose['orientation'] - quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) - rot = Gf.Matrix3d().SetRotate(quat) - trans = Gf.Vec3d(pos[0], pos[1], pos[2]) - return Gf.Matrix4d(rot, trans) - -def matrix_to_pose(matrix: Gf.Matrix4d) -> Dict: - """Convert a 4x4 matrix to position and orientation.""" - pos = matrix.ExtractTranslation() - rot = matrix.ExtractRotation() - quat = rot.GetQuat() - return { - 'position': [pos[0], pos[1], pos[2]], - 'orientation': [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]] - } - -def get_pairing_error(trans_errs: list, rot_errs: list) -> float: - def _median(values: list) -> float: - try: - return float(np.median(np.asarray(values, dtype=np.float64))) - except Exception: - return float('inf') - return _median(trans_errs) + 0.01 * _median(rot_errs) - -def get_palm(transformed_data: Dict, hand: str) -> Dict: - """Get palm position and orientation from transformed data.""" - metacarpal = transformed_data[f'{hand}_6'] - proximal = transformed_data[f'{hand}_7'] - pos = (np.array(metacarpal['position']) + np.array(proximal['position'])) / 2.0 - return {'position': [pos[0], pos[1], pos[2]], 'orientation': metacarpal['orientation']} From c0e51d9d14f8657c640413c50b3dec4d05a9d07d Mon Sep 17 00:00:00 2001 From: Cathy Li Date: Wed, 3 Sep 2025 16:19:23 -0700 Subject: [PATCH 9/9] build libsurvive --- build.sh | 5 ++ .../isaacsim.xr.input_devices/premake5.lua | 49 +++++--------- .../python/impl/vive_tracker.py | 29 +++------ source/scripts/build_libsurvive.sh | 65 +++++++++++++++++++ .../manus_vive_tracking_sample.py | 16 +++-- 5 files changed, 109 insertions(+), 55 deletions(-) create mode 100755 source/scripts/build_libsurvive.sh diff --git a/build.sh b/build.sh index 6df7acee..09bc4429 100755 --- a/build.sh +++ b/build.sh @@ -25,5 +25,10 @@ if [ $EULA_STATUS -ne 0 ]; then exit 1 fi +# Provision libsurvive into _build/target-deps on Linux (no-op if already installed) +if [[ "$(uname -s)" == "Linux" ]]; then + bash "${SCRIPT_DIR}/source/scripts/build_libsurvive.sh" "${SCRIPT_DIR}" +fi + set -e source "$SCRIPT_DIR/repo.sh" build $@ || exit $? diff --git a/source/extensions/isaacsim.xr.input_devices/premake5.lua b/source/extensions/isaacsim.xr.input_devices/premake5.lua index 6e933eeb..560df14c 100644 --- a/source/extensions/isaacsim.xr.input_devices/premake5.lua +++ b/source/extensions/isaacsim.xr.input_devices/premake5.lua @@ -18,53 +18,38 @@ project_ext(ext) -- Python Bindings for Carbonite Plugin project_ext_bindings { - ext = ext, - project_name = "isaacsim.xr.input_devices.python", - module = "_isaac_xr_input_devices", - src = "bindings", - target_subdir = "isaacsim/xr/input_devices", + ext = ext, + project_name = "isaacsim.xr.input_devices.python", + module = "_isaac_xr_input_devices", + src = "bindings", + target_subdir = "isaacsim/xr/input_devices", } staticruntime("Off") add_files("impl", "plugins") add_files("iface", "include") defines { "ISAACSIM_XR_INPUT_DEVICES_EXPORT" } --- Always include the basic headers -includedirs { - "%{root}/source/extensions/isaacsim.core.includes/include", - "%{root}/source/extensions/isaacsim.xr.input_devices/include", - "%{root}/source/extensions/isaacsim.xr.input_devices/plugins", -} - --- Include ManusSDK from target-deps includedirs { + "%{root}/source/extensions/isaacsim.core.includes/include", + "%{root}/source/extensions/isaacsim.xr.input_devices/include", + "%{root}/source/extensions/isaacsim.xr.input_devices/plugins", "%{root}/_build/target-deps/manus_sdk/include", -} -libdirs { - "%{root}/_build/target-deps/manus_sdk/lib", -} -links { - "ManusSDK_Integrated", -} - --- Include libsurvive from target-deps -includedirs { "%{root}/_build/target-deps/libsurvive/include", } libdirs { - "%{root}/_build/target-deps/libsurvive/lib", -} -links { - "survive", + "%{root}/_build/target-deps/manus_sdk/lib", + "%{root}/_build/target-deps/libsurvive/lib", } +links {"ManusSDK_Integrated", "survive"} repo_build.prebuild_link { - { "python/impl", ext.target_dir .. "/isaacsim/xr/input_devices/impl" }, - { "python/tests", ext.target_dir .. "/isaacsim/xr/input_devices/tests" }, - { "docs", ext.target_dir .. "/docs" }, - { "data", ext.target_dir .. "/data" }, + { "python/impl", ext.target_dir .. "/isaacsim/xr/input_devices/impl" }, + { "python/tests", ext.target_dir .. "/isaacsim/xr/input_devices/tests" }, + { "docs", ext.target_dir .. "/docs" }, + { "data", ext.target_dir .. "/data" }, } repo_build.prebuild_copy { - { "python/*.py", ext.target_dir .. "/isaacsim/xr/input_devices" }, + { "python/*.py", ext.target_dir .. "/isaacsim/xr/input_devices" }, + { "%{root}/_build/target-deps/libsurvive/bindings/python/**", ext.target_dir .. "/pysurvive" }, } diff --git a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py index cb841d47..3ea16592 100644 --- a/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py +++ b/source/extensions/isaacsim.xr.input_devices/python/impl/vive_tracker.py @@ -8,21 +8,21 @@ from pxr import Gf try: - # Optionally add libsurvive pysurvive bindings to Python path via env var - libsurvive_python_path = os.environ.get("LIBSURVIVE_PYTHON_PATH") - if libsurvive_python_path: - if libsurvive_python_path not in sys.path: - sys.path.insert(0, libsurvive_python_path) - carb.log_info(f"Using LIBSURVIVE_PYTHON_PATH={libsurvive_python_path}") - else: - carb.log_warn("LIBSURVIVE_PYTHON_PATH is not set; trying to import pysurvive from default sys.path") + if 'pysurvive' not in sys.modules: + ext_root = os.path.abspath(os.path.join(__file__, "../../../../..")) + vendor_path = os.path.join(ext_root, 'pysurvive') + if os.path.isdir(vendor_path) and vendor_path not in sys.path: + sys.path.insert(0, vendor_path) + carb.log_info(f"Using vendored pysurvive at {vendor_path}") + else: + carb.log_warn(f"Failed to add vendored pysurvive: {vendor_path}") import pysurvive from pysurvive.pysurvive_generated import survive_simple_close PYSURVIVE_AVAILABLE = True carb.log_info("pysurvive imported successfully") except ImportError as e: - carb.log_warn(f"pysurvive not available - using mock data for vive: {e}") + carb.log_error(f"pysurvive not available") PYSURVIVE_AVAILABLE = False class IsaacSimViveTracker: @@ -46,16 +46,7 @@ def __init__(self): def update(self): if not PYSURVIVE_AVAILABLE: - self.device_data = { - 'tracker_1': { - 'position': [0.0, 0.0, 0.0], - 'orientation': [1.0, 0.0, 0.0, 0.0] - }, - 'tracker_2': { - 'position': [0.1, 0.0, 0.0], - 'orientation': [1.0, 0.0, 0.0, 0.0] - } - } + raise RuntimeError("pysurvive not available") return if not self.is_connected: return diff --git a/source/scripts/build_libsurvive.sh b/source/scripts/build_libsurvive.sh new file mode 100755 index 00000000..1b264acf --- /dev/null +++ b/source/scripts/build_libsurvive.sh @@ -0,0 +1,65 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Usage: build_libsurvive.sh +# If REPO_ROOT is not provided, infer it from this script's location. + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="${1:-$(cd "$SCRIPT_DIR/../.." && pwd)}" + +TP_DIR="$REPO_ROOT/_build/target-deps/libsurvive" +SRC_DIR="$REPO_ROOT/_build/third_party/libsurvive" +BUILD_DIR="$SRC_DIR/build" + +# Skip if already built (lib + include present) +if [[ -f "$TP_DIR/lib/libsurvive.so" || -f "$TP_DIR/lib/libsurvive.dylib" || -f "$TP_DIR/lib/survive.lib" ]] && \ + [[ -f "$TP_DIR/include/survive.h" ]]; then + echo "[libsurvive] Found existing install under $TP_DIR, skipping build." + exit 0 +fi + +mkdir -p "$TP_DIR" "$SRC_DIR" + +# Clone source if missing +if [[ ! -d "$SRC_DIR/.git" ]]; then + echo "[libsurvive] Cloning libsurvive into $SRC_DIR" + git clone --depth=1 https://github.com/cntools/libsurvive.git "$SRC_DIR" +fi + +# Configure and build with RPATH so Python bindings can locate libsurvive without LD_LIBRARY_PATH +mkdir -p "$BUILD_DIR" +echo "[libsurvive] Configuring CMake (install prefix $TP_DIR)" +cmake -S "$SRC_DIR" -B "$BUILD_DIR" \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX="$TP_DIR" \ + -DCMAKE_INSTALL_RPATH='$$ORIGIN/../../lib' \ + -DCMAKE_BUILD_WITH_INSTALL_RPATH=ON \ + -DCMAKE_INSTALL_RPATH_USE_LINK_PATH=ON + +echo "[libsurvive] Building and installing" +cmake --build "$BUILD_DIR" --target install -j"$(nproc || echo 4)" + +# Copy Python bindings for runtime import (pysurvive) +if [[ -d "$SRC_DIR/bindings/python" ]]; then + mkdir -p "$TP_DIR/bindings/python" + rsync -a --delete "$SRC_DIR/bindings/python/" "$TP_DIR/bindings/python/" + echo "[libsurvive] Copied Python bindings to $TP_DIR/bindings/python" +else + echo "[libsurvive] WARNING: Python bindings directory not found; pysurvive may not import" +fi + +# If compiled extension .so files exist under bindings/python, ensure their RUNPATH points to ../../lib +if command -v patchelf >/dev/null 2>&1; then + shopt -s nullglob + for so in "$TP_DIR"/bindings/python/**/*.so "$TP_DIR"/bindings/python/*.so; do + echo "[libsurvive] Setting RUNPATH on $(basename "$so") to \$ORIGIN/../../lib" + patchelf --set-rpath "\$ORIGIN/../../lib" "$so" || true + done + shopt -u nullglob +else + echo "[libsurvive] patchelf not found; assuming CMake RPATH on extension is sufficient" +fi + +# Print hints +echo "[libsurvive] Install complete at: $TP_DIR" +echo "[libsurvive] Python bindings at: $TP_DIR/bindings/python" diff --git a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py index 1ef350e6..43aaa84a 100644 --- a/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py +++ b/source/standalone_examples/api/isaacsim.xr.input_devices/manus_vive_tracking_sample.py @@ -107,6 +107,7 @@ my_world.reset() reset_needed = False +frame_count = 0 # Get attribute references for faster access positions_attr = point_instancer.GetPositionsAttr() @@ -136,10 +137,17 @@ proto_idx_attr.Set(proto_indices) cube_idx = 0 - all_device_data = xr_integration.get_all_device_data() - manus_data = all_device_data.get('manus_gloves', {}) - vive_data = all_device_data.get('vive_trackers', {}) - + if frame_count % 2 == 0: + xr_integration.manus_tracker.update() + else: + xr_integration.vive_tracker.update() + frame_count += 1 + manus_data = xr_integration.manus_tracker.get_data() + vive_data = xr_integration.vive_tracker.get_data() + if frame_count % 100 == 0: + carb.log_warn(f"Manus data: {manus_data}") + carb.log_warn(f"Vive data: {vive_data}") + # Process Manus gloves (red cubes, index 0) for joint, joint_data in manus_data.items(): if cube_idx >= max_devices: