diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c12c7c8e..80af32619 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -167,6 +167,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/native_openxr) add_subdirectory(examples/mcap_record_replay) add_subdirectory(examples/haptic_feedback) + add_subdirectory(examples/g1_wuji_teleop) if(BUILD_VIZ) add_subdirectory(examples/camera_viz/tests) endif() diff --git a/examples/g1_wuji_teleop/CMakeLists.txt b/examples/g1_wuji_teleop/CMakeLists.txt new file mode 100644 index 000000000..b51ba8fdc --- /dev/null +++ b/examples/g1_wuji_teleop/CMakeLists.txt @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) +project(g1_wuji_teleop_example) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/g1_wuji_teleop/python) + +install(DIRECTORY config docs assets + DESTINATION examples/g1_wuji_teleop +) diff --git a/examples/g1_wuji_teleop/README.md b/examples/g1_wuji_teleop/README.md new file mode 100644 index 000000000..cc862fb0e --- /dev/null +++ b/examples/g1_wuji_teleop/README.md @@ -0,0 +1,45 @@ + + +# G1-Wuji Teleop Example + +This example runs a G1-Wuji Isaac Lab scene from an OpenXR headset and MANUS +gloves. Arm end-effector poses come from either AVP hand tracking or VR/PICO +controllers; finger targets come from MANUS skeletons retargeted with the +official `wuji-retargeting` package. + +## Layout + +- `assets/`: G1-Wuji robot USD and Wuji hand assets used by the example. +- `config/avp_manus.yaml`: AVP hand-wrist arm control with MANUS fingers. +- `config/vr_manus.yaml`: VR/PICO controller arm control with MANUS fingers. +- `config/wuji_retargeting/`: Wuji retargeting YAML files for both hands. +- `docs/setup.md`: host setup and launch runbook. +- `python/g1_wuji_teleop/`: Python implementation package. + +## Run + +Install IsaacTeleop, install `wuji-retargeting` into the same Python +environment, start the CloudXR runtime, and source +`~/.cloudxr/run/cloudxr.env`. See [docs/setup.md](docs/setup.md) for the full +runbook. + +AVP + MANUS: + +```bash +PYTHONPATH=examples/g1_wuji_teleop/python python -m g1_wuji_teleop \ + --config examples/g1_wuji_teleop/config/avp_manus.yaml \ + --device cuda:0 \ + --viz kit +``` + +VR/PICO + MANUS: + +```bash +PYTHONPATH=examples/g1_wuji_teleop/python python -m g1_wuji_teleop \ + --config examples/g1_wuji_teleop/config/vr_manus.yaml \ + --device cuda:0 \ + --viz kit +``` diff --git a/examples/g1_wuji_teleop/assets/g1_wuji/g1_wuji.usd b/examples/g1_wuji_teleop/assets/g1_wuji/g1_wuji.usd new file mode 100644 index 000000000..f5a8770aa --- /dev/null +++ b/examples/g1_wuji_teleop/assets/g1_wuji/g1_wuji.usd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:acc0afc786907290fcb81f796ede6fa791bbae75c37c8168f8b8dbbc9c1e3d51 +size 47977976 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1.STL new file mode 100644 index 000000000..cb3cd3a36 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b17f48ce8974b30536376ef150abc9b8abdf2b6028af52757d3ce90c3f8174be +size 349784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1_collision.STL new file mode 100644 index 000000000..6fc5ae36a --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:853c85e7149580c96d8f6b221c6132595cc4dc5fc33e73dfccfbfc240ca6b161 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2.STL new file mode 100644 index 000000000..e0d32c792 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53b16e2fc88fa152189750f7176b717cdc2b0ad8ade3e91d546355b196274205 +size 92484 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2_collision.STL new file mode 100644 index 000000000..bb5f9faa5 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:34150ded0f71fc59a028eb40d6a747618a3aa1d0c44823550a59ec1b74451b9f +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3.STL new file mode 100644 index 000000000..6f75864c7 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ebcbd6e485881a948b825c8f3c5d8d7120a5fea8d2590bf27664c4dc72940ba9 +size 87984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3_collision.STL new file mode 100644 index 000000000..23ffb3756 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:adb4f921e1ff437296077699dc770bb3a256353d53bbf6c9aa09f3d52985b3b8 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4.STL new file mode 100644 index 000000000..bfbf1c079 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0dcb58b73f8ec2a491f6044a742e8e3a0cb8a9c77c5a9a899d36537ccb9da698 +size 64584 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4_collision.STL new file mode 100644 index 000000000..552a22129 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:65ed57875ed25de2141a4a195bbaf555f510cfc44de4e99681c6957df18c8b0c +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link.STL new file mode 100644 index 000000000..593f336c7 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ac9ddb655262dfa0a339783141eb8f31387e6314d92d9a84a06e960cb64bc86 +size 96384 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link_collision.STL new file mode 100644 index 000000000..78fd167ff --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger1_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:23e9a2e6058f523437d6ad0689d54d45c04d002c1dfac85bc073e16f6e8780b6 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1.STL new file mode 100644 index 000000000..a655f0e33 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cfe8003977ed8fe69e04633d1cbfd09714f4fd1f5b6947197da3019bb7051959 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1_collision.STL new file mode 100644 index 000000000..e8d4f311b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:29d52cbef248abfc5ce8df43fcf3ffa900635127cced5bf859d9694bfd58fbc2 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2.STL new file mode 100644 index 000000000..96fc99cf6 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1131e9a85112b3ba9bc792544d8a522bec93412293668626f3d52ad9f7ffe8b6 +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2_collision.STL new file mode 100644 index 000000000..1d8dab458 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4ef6ff2efef32ccc8ff8e2eee28db7463ba9ae05b158d5666d4bd5ef16161ad0 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3.STL new file mode 100644 index 000000000..cfa21284b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5181c09e54860d98f3be64a644efdb664d134006f6c48549ed112f8f63c62fa +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3_collision.STL new file mode 100644 index 000000000..c508a882f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7990ea209c1a0456bcbc65c3b4bbf831b966635ca57cb324175c6592886e94f6 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4.STL new file mode 100644 index 000000000..659612df2 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c64e2fdc5c192f8dc327dacf60b35609304ad5cea3fc7358d3f780aac4e235a2 +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4_collision.STL new file mode 100644 index 000000000..c4a3c3578 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ce7659ecfc46967b08da7ab064cefca5c6223c7576f22c7dcaf4fd8219983ca7 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link.STL new file mode 100644 index 000000000..bb48f1adf --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1e8be5a90e81c319302053c9fa27608c900d47a5b67797fc931449885b9f86af +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link_collision.STL new file mode 100644 index 000000000..91cdf5105 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger2_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fdf002df36490d481a10593d476eb9239f666339559ed5562cb5d5ffc93a41d +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1.STL new file mode 100644 index 000000000..e885af052 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2ed2537fdea1584af89e562e9fc115c8ac06310f1c67f074ddc5a1eb57cdda6b +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1_collision.STL new file mode 100644 index 000000000..b45d2c42c --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d8027c721b5c0c69fe2f04db0533d000c46fef43dc5db8a9430a01ae8ad88476 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2.STL new file mode 100644 index 000000000..a34be67c5 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e76e1b3adbcbcb5f28fec54492a44c24d6ab9dfc6d8636c1cc4dc31f974559e +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2_collision.STL new file mode 100644 index 000000000..30301d0aa --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0302da39f9c148cffb16d680fe5399efa0a022462ab9bb3937e3146cdfe3d88e +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3.STL new file mode 100644 index 000000000..6a128f65b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e78e7bec8901a499e664b99900b96acd4b8f78a547af06753b09b90cd51e949b +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3_collision.STL new file mode 100644 index 000000000..561e5e206 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6ea57bfc0ca89c9f80e80702f5a6b22e96e38858c41b407cb390cbe081cae534 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4.STL new file mode 100644 index 000000000..f7bbb9a93 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b7a16806e231d51ff50380b3d4b25bac3f347e6f13918c7373d018bcd9da11dd +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4_collision.STL new file mode 100644 index 000000000..4bdfd294b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:281eff1ca8541cf81d932edd676a5561ceee65ac2955b3af4df3906ba71e0bd3 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link.STL new file mode 100644 index 000000000..4c08a6ffb --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b918123bb7f6dc4d94b6b62ceb01b5331d938066f5ce40b8138cb2aa8c755117 +size 113484 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link_collision.STL new file mode 100644 index 000000000..a2140b781 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger3_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b7c505a71cae1ea747f44c9a808f615cda8859ea8472370e67a64e42a600908 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1.STL new file mode 100644 index 000000000..fb3c5aadb --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0664072a389f47449eba9636583c40a2f95386ab23feacc2962fcb9a3070cb34 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1_collision.STL new file mode 100644 index 000000000..27467a46c --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ce0f42d7c684b7036bc5aeba3bd53ea7611dc4ad00592a448acaa95c8590491b +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2.STL new file mode 100644 index 000000000..8afad4fd1 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:eefec26f19d27322ccd9b2b1b080431bb6d63f3a4e1ea0b480c7d07c09eab655 +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2_collision.STL new file mode 100644 index 000000000..22b8c4b10 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bc8ccd945de619743a9e94c0fe34e56afc346758ab5ea4439c5531c71d0383ca +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3.STL new file mode 100644 index 000000000..d6ff1953a --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6d27de502b0e12cff871037a7bd206b8caaa808cb1fd580305fbacf269b50d08 +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3_collision.STL new file mode 100644 index 000000000..ab072f515 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:922b21c1e98f21b20b525f5060e07e07b7aa9f3fef0472b24c3a4d3ace7cc78b +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4.STL new file mode 100644 index 000000000..468685557 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fb629c2fe0957bb6136b62a4f67ccb25839e79ad916b67c000bb3d726d9d9c2b +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4_collision.STL new file mode 100644 index 000000000..c0c26378b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c45c282dd5e3db4b66ccc90e84529f3df698e924a2f5dca53297632cbb17bf85 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link.STL new file mode 100644 index 000000000..f11927429 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7d6739ac876ec9b7c6c36bd1fdd068389474407339cf97dbb93881b2cdf0f2cc +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link_collision.STL new file mode 100644 index 000000000..ee09d3433 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger4_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6593376f3a70d40346661f5e042903e41d3040c630bfc5a66c72f55739804567 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1.STL new file mode 100644 index 000000000..922515657 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ff604ba894a320331516f0227aed75ffdd47d9a6ec29a8e792b2e7777bbb681 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1_collision.STL new file mode 100644 index 000000000..3a73638f8 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5901c224dd0b657e62d724c6b561ae0d77b8d955ae0454e8a93985c21d14472 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2.STL new file mode 100644 index 000000000..4dd29380e --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4f01c61e81bb440c789d242c43d00c16c44590ad2a25f004f8783e7df80bb3db +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2_collision.STL new file mode 100644 index 000000000..d1fb211d1 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6136648bc9de8049d130376b049cac1f7a2deeb30534e6aa75f6ae82f89ce51e +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3.STL new file mode 100644 index 000000000..d6f2ea32b --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:20052a953fdfda74337317d939c59762bb70bb52c206eb62c4fa77e846ca948a +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3_collision.STL new file mode 100644 index 000000000..d27acdca4 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3f8e87de06a09b9451352f870bfcd3cc4120676eb5aa108aa21409c03544026c +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4.STL new file mode 100644 index 000000000..731214cd3 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9355ba84b24102ccb8251a2213c63f48acf98dce47865b4ff446cec7c33c38cc +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4_collision.STL new file mode 100644 index 000000000..dd97e3894 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3426b242ab5c4b8bba7e331a46eeaacba6094797d1fbdb4cff74cfc2cb1a564a +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link.STL new file mode 100644 index 000000000..9d38ea072 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:32636af3449202bc1689432c2f87eb5fa74b1b7a423a03aa6f850a7c829b8ea0 +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link_collision.STL new file mode 100644 index 000000000..565c9abd5 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/finger5_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ea5df32f12ccb000510c2a96ea9adbf0c4be6f2b88ee0bcdbc88a5e1fc609f22 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link.STL new file mode 100644 index 000000000..213695039 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cd1dfefc9be806bda6839cc1c787ed6e6c7cdd0773c0cf91d543877ac6a60aff +size 359284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link_collision.STL new file mode 100644 index 000000000..bd39ef195 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/left/palm_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1c5d4fe26c033a0c7820556a438825d9c206a0d63a5758e8e10ccaaf5269076f +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1.STL new file mode 100644 index 000000000..bc2da1cbe --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0e32302a46cd00d6cc1e7fb03cc483988c639271708c2300a9aecc9de537e42b +size 366584 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1_collision.STL new file mode 100644 index 000000000..e603495cd --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2adeb7b286e96c57c354c0b4d2107aa7bbd63a58b47a2fce828f313722d8eae2 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2.STL new file mode 100644 index 000000000..47ebbd0fa --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:080015cce41de31954e53bbfca45f37f0439e2e689bca49e1910c70efc4f70b7 +size 92684 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2_collision.STL new file mode 100644 index 000000000..a18af74bb --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:930360774dcefe897cafcf6145d042217a78f1b54ca98dc7a1017db889f18afc +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3.STL new file mode 100644 index 000000000..baf64130f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:52eef283d29c617fe4457151685386324bddbccf1598b8a7fc675bb8f28dc85a +size 89184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3_collision.STL new file mode 100644 index 000000000..ba9a3b435 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f2042ec400e5840bf1b3c22aebb7c50ffa466e0f23474908c40ab546eefdbc6c +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4.STL new file mode 100644 index 000000000..219cb7859 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0fb321c4abfd4ab7437caeb116f1935b94d1510fd921222eb49ba6c5a57f057 +size 65984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4_collision.STL new file mode 100644 index 000000000..65641915f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48eb6a1c4818d6be44c1264b069c06362a2edc0bb8ebacb70f195e1e3f4e9090 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link.STL new file mode 100644 index 000000000..822ade70c --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:25929e0c28a5c65afe88dbccaa697664a2385f8ead02126731a7e1b956ec1dea +size 96084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link_collision.STL new file mode 100644 index 000000000..3f650635f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger1_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1f76a0f6319bd664570fa2571e6a87db2b5b94a47045732daef486f2896dc220 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1.STL new file mode 100644 index 000000000..5652a26cc --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36e0471a0373a7634fc10911b33f3b30a5fe829f6526e656a911406cebf19731 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1_collision.STL new file mode 100644 index 000000000..223ccd7f4 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:55cf7973ab4d57119f6eb032704defddf7077a01022f99d6a9f95eaea7f28875 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2.STL new file mode 100644 index 000000000..e4ca87897 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:56bb1972b8cb1d63d48aa18283148ff6760e387dc64fe963b11209e07e243511 +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2_collision.STL new file mode 100644 index 000000000..ea3f95cc0 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:db3b779d3cf98615617f5554946205e9f07de5cd9853005305048f617dc96fdc +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3.STL new file mode 100644 index 000000000..7e270dd43 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:48d70e865e2b49139028511808ae3ad25c5cf06e36fe959c6c71b5263dfcbbde +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3_collision.STL new file mode 100644 index 000000000..12304a5c7 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:24f3464762c5ab07b4c56d3df6576ee43cb63eab0938c6366f1153323c619aa5 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4.STL new file mode 100644 index 000000000..b40fd6b23 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3eede2c8e1eb1301bc23b4b453fb7014ca5965611b44b115a7330bf33e4d922e +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4_collision.STL new file mode 100644 index 000000000..bbfd834fc --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a8708b0b03aab6a7fb5074a6c9639dee85be56415018ad16a9a04b6f43b6cc85 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link.STL new file mode 100644 index 000000000..59e46226a --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:496039ce3f4f3fbe6bac466a01567797f741af95c6f11dc61626ec2de175900b +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link_collision.STL new file mode 100644 index 000000000..b5fede500 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger2_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:53d0ef5c6346f3f5dd514646df784fa1b6b56c088164496ef661019566028c1d +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1.STL new file mode 100644 index 000000000..71d4a7917 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:558fac18be219c0e6bb6224b82c711b3efe762b547818ea074eda93dc38bad07 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1_collision.STL new file mode 100644 index 000000000..528724ca1 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:698e2327acfe737e11fb10f2fb1d36bbf8f1bc6e688f1e31e6232bfc51127581 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2.STL new file mode 100644 index 000000000..c1f3f753f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:338db71e2c1ec63fba860d5d2af52bcb1daff72349b0e03a45f82ae63539bd43 +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2_collision.STL new file mode 100644 index 000000000..ac71baf46 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:50e5f990401d1ede299adc2034c5c2c66f8b2499a8f7c38d9bbc4e26da06c3b9 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3.STL new file mode 100644 index 000000000..16cd3a6fb --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3af337085d5f6430a471462c18b9132f1447eb8c0f3c68a8b1e69318ff8c3db8 +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3_collision.STL new file mode 100644 index 000000000..29fe2892c --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ddbd96f0b25ac9582a786bc6da7b363be61804e2b02fa5da4ca17c04f1715e5f +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4.STL new file mode 100644 index 000000000..8d3580bfe --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3b8efa21bafcae21119f243decf924dac51cc55181ed540e69ce6b85ecf03790 +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4_collision.STL new file mode 100644 index 000000000..b3a3fbc83 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:fafdff7b5e8b736bde927d6b62a7c953bd1f4bf8e77b442f5a7217ae0f2fa431 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link.STL new file mode 100644 index 000000000..3fef1c5f6 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7a99f65a6a473899913d4273e61319e3724fbf81ac8532969ed5c2ff1c1bde54 +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link_collision.STL new file mode 100644 index 000000000..40cca7b11 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger3_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7e855b092faf2e2f0471109814ad9b2469485f88a47b6abd290b79b0f663ab2d +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1.STL new file mode 100644 index 000000000..db6a65028 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:508321fefaeae1ae0567f89acd2492fac27cc468a2f562d895afcff192b0d00c +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1_collision.STL new file mode 100644 index 000000000..10c8ab9c5 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cc3b2c4d5b6304520c4a0297978ffaed3005d99cd8bdf74cc631deb6f5d9f14b +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2.STL new file mode 100644 index 000000000..e2f06532f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e0b28769389c33af6af73c6dbf506be126570fa6ca7cbaf08f85a51005c48cde +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2_collision.STL new file mode 100644 index 000000000..6aea7ff97 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a68648fc10cccdc58b2db7a4d09079ba2a9fa102a04e3c6a955e13e25edb2c30 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3.STL new file mode 100644 index 000000000..5f079cde3 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b01668bab452260954faf8cbff94fb59c55ccb3e490ff6eb4b73e81e550c787f +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3_collision.STL new file mode 100644 index 000000000..c42eacf98 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8eb85ee78e3fd9e489a75e0385ea8493db0af0023286a005ac148cc26ff65b81 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4.STL new file mode 100644 index 000000000..a53f12a70 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d673bf02761f722728eb1ee0e7cc79ee6afba7693c78f623c3a4a89d5a905453 +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4_collision.STL new file mode 100644 index 000000000..bc7862bb4 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:86783162be22f7600503a0a62484314367cf0d1adc2757a68681667e8f03205d +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link.STL new file mode 100644 index 000000000..623387db6 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e8b1fbee8e019afe490e820b1cba5e02c5124bb4a09aa73896bd46db6586dfde +size 113484 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link_collision.STL new file mode 100644 index 000000000..e41cb1416 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger4_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:29ed263d5e5cb24fdede93f6a8c695c36aaf5392f7c52acd4b13dee050a98bb9 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1.STL new file mode 100644 index 000000000..1356abb04 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:85ce22b8285a7d85e4c866151c972cde0736d4f618f35015b0b96fe39e367e49 +size 56184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1_collision.STL new file mode 100644 index 000000000..39b4b4a4f --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link1_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:011e4890f925b9d3454261f4fa98f0fb1b95c3aaeec707aa3db48a305fc42243 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2.STL new file mode 100644 index 000000000..594035022 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9245362396a184e63af8d2707dc85a3250f1d7db4eeb079d5b3ee9b432939dd6 +size 51984 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2_collision.STL new file mode 100644 index 000000000..8a5064314 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link2_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bf966538aa20a9ea1ba6e58c8bf1bf09aef3ca6826f349757077d3de84ed0119 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3.STL new file mode 100644 index 000000000..e87500b5a --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1083dcec6df545d60c3f14af1113613ca89cc8964692dd624baa750491915f40 +size 91284 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3_collision.STL new file mode 100644 index 000000000..76abe5831 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link3_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:26b793372d5232de0b82dd2e74a041564f9bb22394705f2bae23366921cc56b0 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4.STL new file mode 100644 index 000000000..d8604709d --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6342d4562a8bfaef740850c02dc5418ade11dcbe04736b957d93522995080f35 +size 54884 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4_collision.STL new file mode 100644 index 000000000..8ba27a01e --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_link4_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8643d9c3d6363aa7d6652e2137b4f496a1bd350dc39a0ddcc9cc8bfdc5f34093 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link.STL new file mode 100644 index 000000000..ccd435d94 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ebbf48ca7ba3bca5f329f95277ae392d0c1a09615eda23f1bf646c5112a38cee +size 112784 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link_collision.STL new file mode 100644 index 000000000..5a4d45b41 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/finger5_tip_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4678873a3d1ca7c0a97f68cf87ebaa334484cf2d202da76b961e7c16a7a5968d +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link.STL new file mode 100644 index 000000000..7a28ced65 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1ba4495e3b7adc707d50150a8625a0ed3731fa42ab6d40e62ae7437da103ce5e +size 456184 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link_collision.STL b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link_collision.STL new file mode 100644 index 000000000..e8fe80cab --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/meshes/right/palm_link_collision.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:542614615dc46aee7df9cad628374a3aea4c07f58e6f2c716d2b4cbebe665139 +size 25084 diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/urdf/left.urdf b/examples/g1_wuji_teleop/assets/wuji_hand/urdf/left.urdf new file mode 100644 index 000000000..560769b35 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/urdf/left.urdf @@ -0,0 +1,1472 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/g1_wuji_teleop/assets/wuji_hand/urdf/right.urdf b/examples/g1_wuji_teleop/assets/wuji_hand/urdf/right.urdf new file mode 100644 index 000000000..a29f29f10 --- /dev/null +++ b/examples/g1_wuji_teleop/assets/wuji_hand/urdf/right.urdf @@ -0,0 +1,1472 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/g1_wuji_teleop/config/avp_manus.yaml b/examples/g1_wuji_teleop/config/avp_manus.yaml new file mode 100644 index 000000000..1c61e2310 --- /dev/null +++ b/examples/g1_wuji_teleop/config/avp_manus.yaml @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +session: + app_name: G1WujiAvpManusTeleop + +manus_plugin: + plugin_name: manus_hand_plugin + plugin_root_id: manus + search_paths: + - ../../../install/plugins + args: [] + +robot: + variant: g1_wuji + initial_world_position: [2.0, 2.0, 0.8] + initial_world_orientation_xyzw: [0.0, 1.0, 0.0, 0.0] + + avp_frame_binding: + enabled: true + calibration_window_s: 3.0 + robot_reference_body: torso_link + robot_reference_offset_pos: [0.0077774, 0.0000210, 0.3836842] + robot_reference_offset_quat_xyzw: [0.0, 0.0, 0.0, 1.0] + head_forward_axis_isaac: [0.0, 1.0, 0.0] + + head_view_camera: + enabled: true + activate_on_calibration: true + prim_path: /World/TeleopTargets/HeadViewCamera + head_link_prim_path: /World/G1Wuji/torso_link/head_link + translation_offset_m: [0.05, 0.0, 0.3836842] + orientation_offset_rpy_deg: [70.0, 0.0, -90.0] + horizontal_aperture_mm: 20.955 + focal_length_mm: 5.0 + clipping_range_m: [0.01, 1000.0] + + third_viewpoint: + relative_position_m: [0.0, -1.2, 0.55] + +avp: + ee_pose_source: hand_tracking + ee_hand_joint: wrist + +retargeting: + backend: official_wuji diff --git a/examples/g1_wuji_teleop/config/vr_manus.yaml b/examples/g1_wuji_teleop/config/vr_manus.yaml new file mode 100644 index 000000000..650d19e42 --- /dev/null +++ b/examples/g1_wuji_teleop/config/vr_manus.yaml @@ -0,0 +1,50 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +session: + app_name: G1WujiVrManusTeleop + +manus_plugin: + plugin_name: manus_hand_plugin + plugin_root_id: manus + search_paths: + - ../../../install/plugins + args: [] + +robot: + variant: g1_wuji + initial_world_position: [2.0, 2.0, 0.8] + initial_world_orientation_xyzw: [0.0, 1.0, 0.0, 0.0] + + avp_frame_binding: + enabled: true + calibration_window_s: 3.0 + robot_reference_body: torso_link + robot_reference_offset_pos: [0.0077774, 0.0000210, 0.3836842] + robot_reference_offset_quat_xyzw: [0.0, 0.0, 0.0, 1.0] + head_forward_axis_isaac: [0.0, 1.0, 0.0] + + head_view_camera: + enabled: true + activate_on_calibration: true + prim_path: /World/TeleopTargets/HeadViewCamera + head_link_prim_path: /World/G1Wuji/torso_link/head_link + translation_offset_m: [0.05, 0.0, 0.3836842] + orientation_offset_rpy_deg: [70.0, 0.0, -90.0] + horizontal_aperture_mm: 20.955 + focal_length_mm: 5.0 + clipping_range_m: [0.01, 1000.0] + + third_viewpoint: + relative_position_m: [0.0, -1.2, 0.55] + +avp: + ee_pose_source: controller + controller_pose_source: grip + left_controller_to_palm_pos: [-0.08, 0.0, -0.02] + left_controller_to_palm_quat_xyzw: [0.0, 0.0, 0.382683, 0.92388] + right_controller_to_palm_pos: [0.08, 0.0, -0.02] + right_controller_to_palm_quat_xyzw: [0.0, 0.0, -0.382683, 0.92388] + +retargeting: + backend: official_wuji diff --git a/examples/g1_wuji_teleop/config/wuji_retargeting/left.yaml b/examples/g1_wuji_teleop/config/wuji_retargeting/left.yaml new file mode 100644 index 000000000..9b72954c6 --- /dev/null +++ b/examples/g1_wuji_teleop/config/wuji_retargeting/left.yaml @@ -0,0 +1,39 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# G1-Wuji MANUS retargeting config backed by the official wuji-retargeting package. + +optimizer: + type: "AdaptiveOptimizerAnalytical" + urdf_path: "../../assets/wuji_hand/urdf/left.urdf" + +retarget: + huber_delta: 2.0 + huber_delta_dir: 0.5 + norm_delta: 0.04 + + w_pos: 1.0 + w_dir: 2.0 + scaling: 1.0 + project_tip_dir: false + + w_full_hand: 1.0 + segment_scaling: + thumb: [0.98, 1.0, 1.0] + index: [1.1, 0.989, 1.03] + middle: [1.10, 0.96, 0.96] + ring: [1.1, 0.97, 0.99] + pinky: [1.105, 1.15, 1.15] + + pinch_thresholds: + index: { d1: 2.0, d2: 4.0 } + middle: { d1: 2.0, d2: 4.0 } + ring: { d1: 2.0, d2: 4.0 } + pinky: { d1: 2.0, d2: 4.0 } + + mediapipe_rotation: + x: 0.0 + y: 0.0 + z: 15.0 + + lp_alpha: 0.2 diff --git a/examples/g1_wuji_teleop/config/wuji_retargeting/right.yaml b/examples/g1_wuji_teleop/config/wuji_retargeting/right.yaml new file mode 100644 index 000000000..5a3f7f8b4 --- /dev/null +++ b/examples/g1_wuji_teleop/config/wuji_retargeting/right.yaml @@ -0,0 +1,39 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# G1-Wuji MANUS retargeting config backed by the official wuji-retargeting package. + +optimizer: + type: "AdaptiveOptimizerAnalytical" + urdf_path: "../../assets/wuji_hand/urdf/right.urdf" + +retarget: + huber_delta: 2.0 + huber_delta_dir: 0.5 + norm_delta: 0.04 + + w_pos: 1.0 + w_dir: 2.0 + scaling: 1.0 + project_tip_dir: false + + w_full_hand: 1.0 + segment_scaling: + thumb: [0.98, 1.0, 1.0] + index: [1.1, 0.989, 1.03] + middle: [1.10, 0.96, 0.96] + ring: [1.1, 0.97, 0.99] + pinky: [1.105, 1.15, 1.15] + + pinch_thresholds: + index: { d1: 2.0, d2: 4.0 } + middle: { d1: 2.0, d2: 4.0 } + ring: { d1: 2.0, d2: 4.0 } + pinky: { d1: 2.0, d2: 4.0 } + + mediapipe_rotation: + x: 0.0 + y: 0.0 + z: -15.0 + + lp_alpha: 0.2 diff --git a/examples/g1_wuji_teleop/docs/setup.md b/examples/g1_wuji_teleop/docs/setup.md new file mode 100644 index 000000000..4aa040241 --- /dev/null +++ b/examples/g1_wuji_teleop/docs/setup.md @@ -0,0 +1,125 @@ + + +# G1-Wuji AVP/VR + MANUS Setup + +This runbook assumes an Isaac Lab Python environment and an IsaacTeleop build +using the same Python version. + +Replace `/path/to/IsaacTeleop` and `/path/to/wuji-retargeting` with local paths. + +## 1. Build and Install IsaacTeleop + +```bash +cd /path/to/IsaacTeleop +conda activate isaacteleop + +cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DISAAC_TELEOP_PYTHON_VERSION=3.12 \ + -DBUILD_VIZ=OFF \ + -DBUILD_PLUGIN_OAK_CAMERA=OFF \ + -DENABLE_CLANG_FORMAT_CHECK=OFF \ + -DENABLE_CLOUDXR_BUNDLE_CHECK=ON + +cmake --build build --parallel "$(nproc)" +cmake --install build + +python -m pip install -r src/core/python/requirements-retargeters.txt +python -m pip install -r src/core/python/requirements-cloudxr.txt +python -m pip install --force-reinstall install/wheels/isaacteleop-*.whl +``` + +## 2. Install Wuji Retargeting + +Keep Wuji's repository outside IsaacTeleop. Install it into the same Python +environment used to run this example. + +```bash +cd /path/to +git clone --recurse-submodules https://github.com/wuji-technology/wuji-retargeting.git + +cd /path/to/wuji-retargeting +python -m pip install -U pip +python -m pip install -r requirements.txt +python -m pip install -e . + +python -c "from wuji_retargeting.retarget import Retargeter; print(Retargeter)" +``` + +## 3. Install MANUS Support + +Run the udev step on the host. + +```bash +cd /path/to/IsaacTeleop/src/plugins/manus +./install_udev_rules.sh +./install_manus.sh +``` + +Unplug and replug the MANUS dongle. The example configs expect the plugin under +`install/plugins`; edit `manus_plugin.search_paths` if your install prefix is +different. + +## 4. Start CloudXR Runtime + +Keep this terminal running. + +```bash +cd /path/to/IsaacTeleop +conda activate isaacteleop + +echo 'NV_DEVICE_PROFILE=auto-webrtc' > custom.env +python -m isaacteleop.cloudxr --cloudxr-env-config=./custom.env --accept-eula +``` + +On the headset, open the Isaac XR Teleop Sample Client and connect to the Ubuntu +host LAN IP. + +## 5. Verify MANUS Input + +Run this in a second terminal after CloudXR creates `~/.cloudxr/run/cloudxr.env`. + +```bash +cd /path/to/IsaacTeleop +source ~/.cloudxr/run/cloudxr.env + +./install/bin/manus_hand_tracker_printer +``` + +Close the printer before starting teleop because the MANUS SDK is single-owner. + +## 6. Run G1-Wuji Teleop + +AVP hand-wrist arm control: + +```bash +cd /path/to/IsaacTeleop +source ~/.cloudxr/run/cloudxr.env +conda activate isaacteleop + +# export LD_LIBRARY_PATH="$CONDA_PREFIX/lib/python3.12/site-packages/nvidia/cu13/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +PYTHONPATH=examples/g1_wuji_teleop/python python -m g1_wuji_teleop \ + --config examples/g1_wuji_teleop/config/avp_manus.yaml \ + --device cuda:0 \ + --viz kit +``` + +VR/PICO controller arm control: + +```bash +cd /path/to/IsaacTeleop +source ~/.cloudxr/run/cloudxr.env +conda activate isaacteleop + +export LD_LIBRARY_PATH="$CONDA_PREFIX/lib/python3.12/site-packages/nvidia/cu13/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +PYTHONPATH=examples/g1_wuji_teleop/python python -m g1_wuji_teleop \ + --config examples/g1_wuji_teleop/config/vr_manus.yaml \ + --device cuda:0 \ + --viz kit +``` + +Press `B` in the Isaac Sim window to start calibration and arm teleop. Press `R` +to reset the robot and recalibrate. diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/__init__.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/__init__.py new file mode 100644 index 000000000..b1e8db1a5 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/__init__.py @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""G1-Wuji AVP/VR + MANUS teleoperation example.""" + +from .app import main + +__all__ = ["main"] diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/__main__.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/__main__.py new file mode 100644 index 000000000..520a69737 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/__main__.py @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Run the G1-Wuji teleop example as a module.""" + +from __future__ import annotations + +from .app import main + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/app.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/app.py new file mode 100644 index 000000000..accafb6a3 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/app.py @@ -0,0 +1,1530 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Minimal Isaac Lab scene for AVP + MANUS teleop on G1-Wuji. + +Run this script with an Isaac Sim / Isaac Lab Python environment. It intentionally +keeps the simulation side small: a ground plane, a light, the selected G1 USD, +and live hand joint targets from ``TeleopMain``. +""" + +from __future__ import annotations + +import argparse +import asyncio +import sys +import time +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + +from .config import default_config_path, example_path, load_yaml_file +from .input_stream import TeleopMain +from .robot import ( + AvpRobotFrameBinding, + HandTargetPostProcessor, + WujiHandTargetBackend, + as_torch, + avp_frame_binding_runtime_config, + matrix_to_quat_xyzw, + normalize_quat_xyzw, + openxr_pose_to_isaac_pose, + robot_profile_from_config, + wuji_hand_runtime_config, +) +from .scene import ( + G1WujiSceneConfig, + design_g1_wuji_scene, +) + + +ARM_JOINTS = { + "left": ( + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + ), + "right": ( + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ), +} + + +@dataclass(frozen=True) +class HeadViewCameraConfig: + enabled: bool = True + activate_on_calibration: bool = True + prim_path: str = "/World/TeleopTargets/HeadViewCamera" + head_link_prim_path: str = "/World/G1Wuji/torso_link/head_link" + translation_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + orientation_offset_rpy_deg: tuple[float, float, float] = (90.0, 0.0, 90.0) + horizontal_aperture_mm: float = 20.955 + focal_length_mm: float = 18.14756 + clipping_range_m: tuple[float, float] = (0.01, 1000.0) + + +@dataclass(frozen=True) +class ThirdViewpointConfig: + relative_position_m: tuple[float, float, float] = (0.0, -1.2, 0.55) + + +class HeadViewCameraController: + """Drive a viewport camera from the robot head pose after AVP calibration.""" + + def __init__( + self, + *, + stage: Any, + UsdGeom: Any, + Gf: Any, + viewport_api: Any, + config: HeadViewCameraConfig, + head_link_prim: Any | None, + avp_frame_binding: Any | None, + ) -> None: + self._config = config + self._viewport_api = viewport_api + self._UsdGeom = UsdGeom + self._Gf = Gf + self._head_link_prim = head_link_prim + self._avp_frame_binding = avp_frame_binding + self._active = False + self._announced_missing_head = False + self._orientation_offset_quat = self._orientation_offset_quatd(config) + + camera = UsdGeom.Camera.Define(stage, config.prim_path) + self._camera_prim = camera.GetPrim() + self._camera_xform = UsdGeom.Xformable(self._camera_prim) + self._translate_op = self._camera_xform.AddTranslateOp() + # USD xform op value types are strict. Head prim world rotations come back as + # GfQuatd here, so keep the camera orient op in double precision as well. + self._orient_op = self._camera_xform.AddOrientOp( + precision=UsdGeom.XformOp.PrecisionDouble + ) + self._scale_op = self._camera_xform.AddScaleOp() + self._camera_path = str(self._camera_prim.GetPath()) + + camera.GetHorizontalApertureAttr().Set(float(config.horizontal_aperture_mm)) + camera.GetFocalLengthAttr().Set(float(config.focal_length_mm)) + camera.GetClippingRangeAttr().Set( + Gf.Vec2f( + float(config.clipping_range_m[0]), + float(config.clipping_range_m[1]), + ) + ) + self._scale_op.Set(Gf.Vec3f(1.0, 1.0, 1.0)) + + @property + def camera_path(self) -> str: + return self._camera_path + + def activate(self) -> None: + if self._viewport_api is None: + return + self._viewport_api.camera_path = self._camera_path + self._active = True + + def deactivate(self) -> None: + self._active = False + + def update(self, *, xform_cache: Any) -> None: + head_link_prim = self._head_link_prim + if head_link_prim is None or xform_cache is None: + if not self._announced_missing_head: + print( + "Head view camera is enabled but no robot head prim is available; " + "camera updates are disabled.", + flush=True, + ) + self._announced_missing_head = True + return + + xform_cache.Clear() + world_transform = xform_cache.GetLocalToWorldTransform(head_link_prim) + world_translation = world_transform.ExtractTranslation() + world_rotation = world_transform.ExtractRotationQuat() + camera_base_rotation = world_rotation * self._orientation_offset_quat + camera_rotation = self._apply_head_pose_delta(camera_base_rotation) + + offset = self._config.translation_offset_m + offset_vec = self._Gf.Vec3d( + float(offset[0]), float(offset[1]), float(offset[2]) + ) + camera_translation = world_translation + world_transform.TransformDir( + offset_vec + ) + + self._translate_op.Set(camera_translation) + self._orient_op.Set(camera_rotation) + + def _apply_head_pose_delta(self, base_rotation: Any) -> Any: + avp_frame_binding = self._avp_frame_binding + if avp_frame_binding is None or not avp_frame_binding.calibrated: + return base_rotation + tilt_delta_rot = avp_frame_binding.latest_head_tilt_delta_rot() + camera_rotation = base_rotation + if tilt_delta_rot is not None: + tilt_delta_quat = self._matrix3_to_quatd(tilt_delta_rot) + camera_rotation = tilt_delta_quat * camera_rotation + return camera_rotation + + def _orientation_offset_quatd(self, config: HeadViewCameraConfig) -> Any: + roll_deg, pitch_deg, yaw_deg = config.orientation_offset_rpy_deg + roll_rad = np.deg2rad(float(roll_deg)) + pitch_rad = np.deg2rad(float(pitch_deg)) + yaw_rad = np.deg2rad(float(yaw_deg)) + + cr = np.cos(roll_rad * 0.5) + sr = np.sin(roll_rad * 0.5) + cp = np.cos(pitch_rad * 0.5) + sp = np.sin(pitch_rad * 0.5) + cy = np.cos(yaw_rad * 0.5) + sy = np.sin(yaw_rad * 0.5) + + quat_xyzw = normalize_quat_xyzw( + ( + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + cr * cp * cy + sr * sp * sy, + ) + ) + return self._Gf.Quatd( + float(quat_xyzw[3]), + self._Gf.Vec3d( + float(quat_xyzw[0]), + float(quat_xyzw[1]), + float(quat_xyzw[2]), + ), + ) + + def _matrix3_to_quatd(self, matrix: np.ndarray) -> Any: + quat_xyzw = normalize_quat_xyzw( + matrix_to_quat_xyzw(np.asarray(matrix, dtype=np.float64)) + ) + return self._Gf.Quatd( + float(quat_xyzw[3]), + self._Gf.Vec3d( + float(quat_xyzw[0]), + float(quat_xyzw[1]), + float(quat_xyzw[2]), + ), + ) + + +class ThirdViewpointController: + """Drive a right-side viewport viewpoint from the robot reference pose.""" + + _VIEWPOINT_PATH = "/OmniverseKit_Persp" + _WINDOW_TITLE = "G1-Wuji Third View" + + def __init__( + self, + *, + Gf: Any, + ViewportManager: Any, + viewport_utility: Any, + ui: Any, + config: ThirdViewpointConfig, + reference_prim: Any, + main_viewport_api: Any, + main_camera_path: str | None, + status_label: str, + ) -> None: + self._Gf = Gf + self._ViewportManager = ViewportManager + self._viewport_utility = viewport_utility + self._ui = ui + self._config = config + self._reference_prim = reference_prim + self._main_viewport_api = main_viewport_api + self._main_camera_path = ( + None if main_camera_path is None else str(main_camera_path) + ) + self._status_label = status_label + self._window = None + self._viewport_api = None + self._active = False + self._layout_requested = False + + def activate(self, *, xform_cache: Any) -> None: + if self._active: + return + if self._ensure_window() is None: + return + self._set_window_camera() + self._set_initial_view(xform_cache=xform_cache) + self._set_main_camera() + self._active = True + + def deactivate(self) -> None: + self._active = False + if self._window is not None: + self._window.visible = False + + def _set_initial_view(self, *, xform_cache: Any) -> None: + xform_cache.Clear() + world_transform = xform_cache.GetLocalToWorldTransform(self._reference_prim) + target = world_transform.ExtractTranslation() + offset = self._config.relative_position_m + eye = target + world_transform.TransformDir( + self._Gf.Vec3d(float(offset[0]), float(offset[1]), float(offset[2])) + ) + self._ViewportManager.set_camera_view( + self._VIEWPOINT_PATH, + eye=self._vec3_to_list(eye), + target=self._vec3_to_list(target), + ) + + def _ensure_window(self) -> Any: + if self._window is not None: + self._window.visible = True + return self._window + + self._window = self._viewport_utility.create_viewport_window( + self._WINDOW_TITLE, + width=1280, + height=720, + camera_path=self._VIEWPOINT_PATH, + ) + if self._window is None: + print( + f"[{self._status_label}] failed to create third viewpoint window.", + flush=True, + ) + return None + + self._viewport_api = self._window.viewport_api + self._set_window_camera() + self._window.visible = True + self._dock_to_main_viewport_async() + return self._window + + def _dock_to_main_viewport_async(self) -> None: + window = self._window + if window is None or self._layout_requested: + return + self._layout_requested = True + + async def dock_window() -> None: + import omni.kit.app + + await omni.kit.app.get_app().next_update_async() + target_window = self._ui.Workspace.get_window("Viewport") + if target_window is not None and window is not None: + window.dock_in(target_window, self._ui.DockPosition.RIGHT, 0.5) + self._set_window_camera() + self._set_main_camera() + + asyncio.ensure_future(dock_window()) + + def _set_main_camera(self) -> None: + if self._main_viewport_api is None or not self._main_camera_path: + return + self._main_viewport_api.camera_path = self._main_camera_path + + def _set_window_camera(self) -> None: + if self._viewport_api is None: + return + self._viewport_api.camera_path = self._VIEWPOINT_PATH + + def _vec3_to_list(self, value: Any) -> list[float]: + return [float(value[0]), float(value[1]), float(value[2])] + + +def _head_view_camera_config(config: Mapping[str, Any]) -> HeadViewCameraConfig: + robot_config = config.get("robot", {}) + if robot_config is None: + robot_config = {} + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + + camera_config = robot_config.get("head_view_camera", {}) + if camera_config is None: + camera_config = {} + if not isinstance(camera_config, Mapping): + raise ValueError("Config field 'robot.head_view_camera' must be a mapping") + + prim_path = str( + camera_config.get("prim_path", "/World/TeleopTargets/HeadViewCamera") + ).strip() + if not prim_path: + raise ValueError( + "Config field 'robot.head_view_camera.prim_path' must be non-empty" + ) + head_link_prim_path = str( + camera_config.get( + "head_link_prim_path", + "/World/G1Wuji/torso_link/head_link", + ) + ).strip() + if not head_link_prim_path: + raise ValueError( + "Config field 'robot.head_view_camera.head_link_prim_path' must be non-empty" + ) + + return HeadViewCameraConfig( + enabled=bool(camera_config.get("enabled", True)), + activate_on_calibration=bool( + camera_config.get("activate_on_calibration", True) + ), + prim_path=prim_path, + head_link_prim_path=head_link_prim_path, + translation_offset_m=_float_tuple( + camera_config.get("translation_offset_m"), + name="robot.head_view_camera.translation_offset_m", + length=3, + default=(0.0, 0.0, 0.0), + ), + orientation_offset_rpy_deg=_float_tuple( + camera_config.get("orientation_offset_rpy_deg"), + name="robot.head_view_camera.orientation_offset_rpy_deg", + length=3, + default=(90.0, 0.0, 90.0), + ), + horizontal_aperture_mm=float( + camera_config.get("horizontal_aperture_mm", 20.955) + ), + focal_length_mm=float(camera_config.get("focal_length_mm", 18.14756)), + clipping_range_m=_float_tuple( + camera_config.get("clipping_range_m"), + name="robot.head_view_camera.clipping_range_m", + length=2, + default=(0.01, 1000.0), + ), + ) + + +def _third_viewpoint_config(config: Mapping[str, Any]) -> ThirdViewpointConfig | None: + robot_config = config.get("robot", {}) + if robot_config is None: + robot_config = {} + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + if "third_viewpoint" not in robot_config: + return None + + viewpoint_config = robot_config.get("third_viewpoint", {}) + if viewpoint_config is None: + viewpoint_config = {} + if not isinstance(viewpoint_config, Mapping): + raise ValueError("Config field 'robot.third_viewpoint' must be a mapping") + + return ThirdViewpointConfig( + relative_position_m=_float_tuple( + viewpoint_config.get("relative_position_m"), + name="robot.third_viewpoint.relative_position_m", + length=3, + default=(0.0, -1.2, 0.55), + ) + ) + + +def _hide_third_viewpoint_window(viewport_utility: Any) -> None: + window = viewport_utility.get_active_viewport_window( + window_name=ThirdViewpointController._WINDOW_TITLE + ) + if window is not None: + window.visible = False + + +@dataclass(frozen=True) +class ArmIkSideConfig: + body_name: str + body_offset_pos: tuple[float, float, float] + body_offset_quat_xyzw: tuple[float, float, float, float] + target_orientation_correction_quat_xyzw: tuple[float, float, float, float] + joint_names: tuple[str, ...] + + +@dataclass(frozen=True) +class ArmIkRuntimeConfig: + status_label: str = "teleop" + enabled: bool = True + pose_scale: float = 1.0 + pose_z_offset: float = 0.0 + command_type: str = "pose" + startup_relative_position: bool = True + startup_relative_orientation: bool = True + position_error_scale: float = 1.0 + orientation_error_scale: float = 0.35 + position_deadband_m: float = 0.003 + orientation_deadband_rad: float = 0.06 + ik_method: str = "dls" + damping: float = 0.05 + smoothing_alpha: float = 0.35 + clamp_to_joint_limits: bool = True + sides: dict[str, ArmIkSideConfig] = field(default_factory=dict) + + +def _default_config_path() -> Path: + return default_config_path() + + +def _robot_initial_world_position( + config: Mapping[str, Any], +) -> tuple[float, float, float]: + robot_config = config.get("robot", {}) + if robot_config is None: + return (0.0, 0.0, 0.0) + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + + return _float_tuple( + robot_config.get("initial_world_position"), + name="robot.initial_world_position", + length=3, + default=(0.0, 0.0, 0.0), + ) + + +def _robot_initial_world_orientation_xyzw( + config: Mapping[str, Any], +) -> tuple[float, float, float, float]: + robot_config = config.get("robot", {}) + if robot_config is None: + return (0.0, 0.0, 0.0, 1.0) + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + + raw_quat = robot_config.get("initial_world_orientation_xyzw") + quat = _float_tuple( + raw_quat, + name="robot.initial_world_orientation_xyzw", + length=4, + default=(0.0, 0.0, 0.0, 1.0), + ) + norm = sum(value * value for value in quat) ** 0.5 + if norm < 1.0e-8: + raise ValueError( + "Config field 'robot.initial_world_orientation_xyzw' must be non-zero" + ) + return tuple(value / norm for value in quat) + + +def _float_tuple( + values: Any, + *, + name: str, + length: int, + default: Sequence[float], +) -> tuple[float, ...]: + raw = default if values is None else values + if not isinstance(raw, Sequence) or isinstance(raw, (str, bytes)): + raise ValueError(f"Config field {name!r} must be a {length}-element sequence") + if len(raw) != length: + raise ValueError(f"Config field {name!r} must contain exactly {length} values") + return tuple(float(value) for value in raw) + + +def _arm_ik_runtime_config(config: Mapping[str, Any]) -> ArmIkRuntimeConfig: + profile = robot_profile_from_config(config) + robot_config = config.get("robot", {}) + if robot_config is None: + robot_config = {} + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + + # Keep the arm IK body bindings pinned in code rather than mirroring them in + # the teleop YAML. These offsets are asset-specific and tuned together. + sides = { + "left": ArmIkSideConfig( + body_name=profile.ik_body_names["left"], + body_offset_pos=profile.ik_body_offset_pos["left"], + body_offset_quat_xyzw=profile.ik_body_offset_quat_xyzw["left"], + target_orientation_correction_quat_xyzw=( + profile.ik_target_orientation_correction_quat_xyzw["left"] + ), + joint_names=ARM_JOINTS["left"], + ), + "right": ArmIkSideConfig( + body_name=profile.ik_body_names["right"], + body_offset_pos=profile.ik_body_offset_pos["right"], + body_offset_quat_xyzw=profile.ik_body_offset_quat_xyzw["right"], + target_orientation_correction_quat_xyzw=( + profile.ik_target_orientation_correction_quat_xyzw["right"] + ), + joint_names=ARM_JOINTS["right"], + ), + } + + return ArmIkRuntimeConfig( + status_label=profile.status_label, + enabled=True, + pose_scale=1.0, + pose_z_offset=0.0, + command_type="pose", + startup_relative_position=True, + startup_relative_orientation=True, + position_error_scale=0.8, + orientation_error_scale=0.8, + position_deadband_m=0.004, + orientation_deadband_rad=0.08, + ik_method="dls", + damping=0.05, + smoothing_alpha=0.80, + clamp_to_joint_limits=True, + sides=sides, + ) + + +def _load_isaac_lab(): + from isaaclab.app import AppLauncher + + return AppLauncher + + +class DualArmIkController: + """Drive G1 arm joints from left/right end-effector pose targets.""" + + def __init__( + self, + robot: Any, + config: ArmIkRuntimeConfig, + *, + device: str, + pose_binding: AvpRobotFrameBinding | None, + ) -> None: + import torch + from isaaclab.controllers import DifferentialIKController + from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg + from isaaclab.utils import math as math_utils + + self._robot = robot + self._config = config + self._pose_binding = pose_binding + self._device = device + self._torch = torch + self._math_utils = math_utils + self._controllers: dict[str, Any] = {} + self._joint_ids: dict[str, Sequence[int] | slice] = {} + self._joint_id_lists: dict[str, list[int]] = {} + self._joint_names: dict[str, tuple[str, ...]] = {} + self._body_ids: dict[str, int] = {} + self._body_names: dict[str, str] = {} + self._jacobi_body_ids: dict[str, int] = {} + self._jacobi_joint_ids: dict[str, list[int]] = {} + self._offset_pos: dict[str, Any] = {} + self._offset_rot: dict[str, Any] = {} + self._target_rot_correction: dict[str, Any] = {} + self._last_targets: dict[str, Any] = {} + self._startup_target_refs: dict[str, tuple[Any, Any, Any, Any]] = {} + self._startup_ref_announced: set[str] = set() + + ik_params = self._ik_params() + for side, side_config in config.sides.items(): + joint_ids, resolved_joint_names = robot.find_joints( + list(side_config.joint_names), preserve_order=True + ) + missing = [ + name + for name in side_config.joint_names + if name not in resolved_joint_names + ] + if missing: + raise RuntimeError( + f"{self._config.status_label} USD is missing {side} arm joints: {missing[:8]}" + ) + + body_ids, body_names = robot.find_bodies(side_config.body_name) + if len(body_ids) != 1: + raise RuntimeError( + f"Expected one {side} IK body match for {side_config.body_name!r}; " + f"got {len(body_ids)}: {body_names}" + ) + + body_idx = int(body_ids[0]) + joint_id_list = [int(joint_id) for joint_id in joint_ids] + if robot.is_fixed_base: + jacobi_body_idx = body_idx - 1 + jacobi_joint_ids = joint_id_list + else: + jacobi_body_idx = body_idx + jacobi_joint_ids = [joint_id + 6 for joint_id in joint_id_list] + + controller_cfg = DifferentialIKControllerCfg( + command_type=config.command_type, + use_relative_mode=False, + ik_method=config.ik_method, + ik_params=ik_params, + ) + self._controllers[side] = DifferentialIKController( + cfg=controller_cfg, + num_envs=1, + device=device, + ) + self._joint_id_lists[side] = joint_id_list + self._joint_ids[side] = ( + slice(None) if len(joint_id_list) == robot.num_joints else joint_id_list + ) + self._joint_names[side] = tuple(str(name) for name in resolved_joint_names) + self._body_ids[side] = body_idx + self._body_names[side] = str(body_names[0]) + self._jacobi_body_ids[side] = int(jacobi_body_idx) + self._jacobi_joint_ids[side] = jacobi_joint_ids + self._offset_pos[side] = torch.tensor( + side_config.body_offset_pos, + dtype=torch.float32, + device=device, + ).unsqueeze(0) + self._offset_rot[side] = torch.tensor( + side_config.body_offset_quat_xyzw, + dtype=torch.float32, + device=device, + ).unsqueeze(0) + self._target_rot_correction[side] = self._math_utils.quat_unique( + self._math_utils.normalize( + torch.tensor( + side_config.target_orientation_correction_quat_xyzw, + dtype=torch.float32, + device=device, + ).unsqueeze(0) + ) + ) + + def _ik_params(self) -> dict[str, float]: + if self._config.ik_method == "dls": + return {"lambda_val": self._config.damping} + if self._config.ik_method == "pinv": + return {"k_val": 1.0} + if self._config.ik_method == "trans": + return {"k_val": 1.0} + if self._config.ik_method == "svd": + return {"k_val": 1.0, "min_singular_value": 1.0e-5} + return {} + + def _frame_pose_root(self, side: str) -> tuple[Any, Any]: + body_idx = self._body_ids[side] + ee_pos_w = as_torch(self._robot.data.body_pos_w)[:, body_idx] + ee_quat_w = as_torch(self._robot.data.body_quat_w)[:, body_idx] + root_pos_w = as_torch(self._robot.data.root_pos_w) + root_quat_w = as_torch(self._robot.data.root_quat_w) + ee_pos_b, ee_quat_b = self._math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, ee_pos_w, ee_quat_w + ) + return self._math_utils.combine_frame_transforms( + ee_pos_b, + ee_quat_b, + self._offset_pos[side], + self._offset_rot[side], + ) + + def _target_pose_root( + self, side: str, pose: Mapping[str, Any] + ) -> tuple[Any, Any] | None: + if self._pose_binding is not None: + target_pose = self._pose_binding.transform_pose(side, pose) + if target_pose is None: + return None + pos_w_np, quat_w_np = target_pose + else: + pos_w_np, quat_w_np = openxr_pose_to_isaac_pose( + pose, + scale=self._config.pose_scale, + offset_z=self._config.pose_z_offset, + ) + return self._target_pose_root_from_world_pose( + pos_w_np, + quat_w_np, + side=side, + apply_target_rot_correction=True, + ) + + def _target_pose_root_from_world_pose( + self, + pos_w_np: Sequence[float] | np.ndarray, + quat_w_np: Sequence[float] | np.ndarray, + *, + side: str | None = None, + apply_target_rot_correction: bool = False, + ) -> tuple[Any, Any]: + pos_w = self._torch.as_tensor( + pos_w_np, + dtype=self._torch.float32, + device=self._device, + ).unsqueeze(0) + quat_w = self._torch.as_tensor( + normalize_quat_xyzw(quat_w_np), + dtype=self._torch.float32, + device=self._device, + ).unsqueeze(0) + if apply_target_rot_correction: + if side is None: + raise ValueError( + "side is required when applying target orientation correction" + ) + quat_w = self._math_utils.normalize( + self._math_utils.quat_mul(quat_w, self._target_rot_correction[side]) + ) + else: + quat_w = self._math_utils.normalize(quat_w) + quat_w = self._math_utils.quat_unique(quat_w) + root_pos_w = as_torch(self._robot.data.root_pos_w) + root_quat_w = as_torch(self._robot.data.root_quat_w) + return self._math_utils.subtract_frame_transforms( + root_pos_w, root_quat_w, pos_w, quat_w + ) + + def _startup_relative_target( + self, + side: str, + target_pos: Any, + target_quat: Any, + ee_pos_curr: Any, + ee_quat_curr: Any, + ) -> tuple[Any, Any]: + if not ( + self._config.startup_relative_position + or self._config.startup_relative_orientation + ): + return target_pos, target_quat + + ref = self._startup_target_refs.get(side) + if ref is None: + ref = ( + target_pos.detach().clone(), + self._math_utils.quat_unique( + self._math_utils.normalize(target_quat.detach().clone()) + ), + ee_pos_curr.detach().clone(), + self._math_utils.quat_unique( + self._math_utils.normalize(ee_quat_curr.detach().clone()) + ), + ) + self._startup_target_refs[side] = ref + self._announce_startup_relative_target(side, ref) + + target_start_pos, target_start_quat, ee_start_pos, ee_start_quat = ref + adjusted_pos = target_pos + adjusted_quat = target_quat + + # AVP optical wrists and the robot EE rarely share a reliable absolute + # pose. Bind the first valid target to the current simulated EE, then + # command startup-relative translation and rotation deltas. + if self._config.startup_relative_position: + adjusted_pos = ee_start_pos + (target_pos - target_start_pos) + if self._config.startup_relative_orientation: + target_quat = self._math_utils.quat_unique( + self._math_utils.normalize(target_quat) + ) + quat_delta = self._math_utils.quat_mul( + target_quat, + self._math_utils.quat_inv(target_start_quat), + ) + adjusted_quat = self._math_utils.normalize( + self._math_utils.quat_mul(quat_delta, ee_start_quat) + ) + adjusted_quat = self._math_utils.quat_unique(adjusted_quat) + return adjusted_pos, adjusted_quat + + def _announce_startup_relative_target( + self, + side: str, + ref: tuple[Any, Any, Any, Any], + ) -> None: + if side in self._startup_ref_announced: + return + target_start_pos, target_start_quat, ee_start_pos, ee_start_quat = ref + print( + f"[{self._config.status_label}] {side} IK startup target reference calibrated " + f"relative_position={self._config.startup_relative_position} " + f"relative_orientation={self._config.startup_relative_orientation} " + f"target_pos={np.round(target_start_pos[0].detach().cpu().numpy(), 4).tolist()} " + f"ee_pos={np.round(ee_start_pos[0].detach().cpu().numpy(), 4).tolist()} " + f"target_quat_xyzw={np.round(target_start_quat[0].detach().cpu().numpy(), 4).tolist()} " + f"ee_quat_xyzw={np.round(ee_start_quat[0].detach().cpu().numpy(), 4).tolist()}", + flush=True, + ) + self._startup_ref_announced.add(side) + + def _compute_delta_joint_pos(self, delta_pose: Any, jacobian: Any) -> Any: + if self._config.ik_method == "pinv": + jacobian_pinv = self._torch.linalg.pinv(jacobian) + return (jacobian_pinv @ delta_pose.unsqueeze(-1)).squeeze(-1) + if self._config.ik_method == "svd": + min_singular_value = 1.0e-5 + U, S, Vh = self._torch.linalg.svd(jacobian) + S_inv = 1.0 / S + S_inv = self._torch.where( + min_singular_value < S, + S_inv, + self._torch.zeros_like(S_inv), + ) + jacobian_pinv = ( + self._torch.transpose(Vh, dim0=1, dim1=2)[:, :, : jacobian.shape[1]] + @ self._torch.diag_embed(S_inv) + @ self._torch.transpose(U, dim0=1, dim1=2) + ) + return (jacobian_pinv @ delta_pose.unsqueeze(-1)).squeeze(-1) + if self._config.ik_method == "trans": + jacobian_t = self._torch.transpose(jacobian, dim0=1, dim1=2) + return (jacobian_t @ delta_pose.unsqueeze(-1)).squeeze(-1) + if self._config.ik_method == "dls": + jacobian_t = self._torch.transpose(jacobian, dim0=1, dim1=2) + lambda_val = self._config.damping + lambda_matrix = (lambda_val**2) * self._torch.eye( + n=jacobian.shape[1], + device=self._device, + ) + return ( + jacobian_t + @ self._torch.inverse(jacobian @ jacobian_t + lambda_matrix) + @ delta_pose.unsqueeze(-1) + ).squeeze(-1) + raise ValueError( + f"Unsupported inverse-kinematics method: {self._config.ik_method}" + ) + + def _compute_joint_targets( + self, + side: str, + ee_pos_curr: Any, + ee_quat_curr: Any, + joint_pos: Any, + ) -> Any: + jacobian = self._frame_jacobian(side) + controller = self._controllers[side] + if self._config.command_type == "position": + return controller.compute( + ee_pos_curr, + ee_quat_curr, + jacobian, + joint_pos, + ) + + position_error, axis_angle_error = self._math_utils.compute_pose_error( + ee_pos_curr, + ee_quat_curr, + controller.ee_pos_des, + controller.ee_quat_des, + rot_error_type="axis_angle", + ) + if self._config.position_deadband_m > 0.0: + position_norm = self._torch.linalg.norm(position_error, dim=1, keepdim=True) + position_error = self._torch.where( + position_norm <= self._config.position_deadband_m, + self._torch.zeros_like(position_error), + position_error, + ) + if self._config.orientation_deadband_rad > 0.0: + orientation_norm = self._torch.linalg.norm( + axis_angle_error, dim=1, keepdim=True + ) + axis_angle_error = self._torch.where( + orientation_norm <= self._config.orientation_deadband_rad, + self._torch.zeros_like(axis_angle_error), + axis_angle_error, + ) + # Pose-mode wrist targets are much noisier in orientation than in translation. + # Down-weight angular error so small AVP wrist rotations do not yank the whole + # arm into a large reconfiguration before the translational target is reached. + position_error = position_error * self._config.position_error_scale + axis_angle_error = axis_angle_error * self._config.orientation_error_scale + pose_error = self._torch.cat((position_error, axis_angle_error), dim=1) + if bool(self._torch.all(self._torch.abs(pose_error) <= 1.0e-9)): + return joint_pos + delta_joint_pos = self._compute_delta_joint_pos(pose_error, jacobian) + return joint_pos + delta_joint_pos + + def _frame_jacobian(self, side: str): + jacobian = as_torch(self._robot.root_physx_view.get_jacobians())[ + :, self._jacobi_body_ids[side], :, self._jacobi_joint_ids[side] + ].clone() + root_quat_w = as_torch(self._robot.data.root_quat_w) + root_rot_b_w = self._math_utils.matrix_from_quat( + self._math_utils.quat_inv(root_quat_w) + ) + jacobian[:, 0:3, :] = self._torch.bmm(root_rot_b_w, jacobian[:, 0:3, :]) + jacobian[:, 3:, :] = self._torch.bmm(root_rot_b_w, jacobian[:, 3:, :]) + offset_pos = self._offset_pos[side] + offset_rot = self._offset_rot[side] + jacobian[:, 0:3, :] += self._torch.bmm( + -self._math_utils.skew_symmetric_matrix(offset_pos), + jacobian[:, 3:, :], + ) + jacobian[:, 3:, :] = self._torch.bmm( + self._math_utils.matrix_from_quat(offset_rot), + jacobian[:, 3:, :], + ) + return jacobian + + def _apply_target_pose_root( + self, + side: str, + target_pose_root: tuple[Any, Any], + ) -> None: + ee_pos_curr, ee_quat_curr = self._frame_pose_root(side) + target_pos, target_quat = target_pose_root + target_pos, target_quat = self._startup_relative_target( + side, + target_pos, + target_quat, + ee_pos_curr, + ee_quat_curr, + ) + command = ( + target_pos + if self._config.command_type == "position" + else self._torch.cat((target_pos, target_quat), dim=1) + ) + self._controllers[side].set_command(command, ee_pos_curr, ee_quat_curr) + joint_pos = as_torch(self._robot.data.joint_pos)[:, self._joint_ids[side]] + joint_targets = self._compute_joint_targets( + side, + ee_pos_curr, + ee_quat_curr, + joint_pos, + ) + + if self._config.clamp_to_joint_limits: + limits = as_torch(self._robot.data.soft_joint_pos_limits)[ + :, self._joint_ids[side], : + ] + joint_targets = self._torch.clamp( + joint_targets, limits[:, :, 0], limits[:, :, 1] + ) + + previous = self._last_targets.get(side) + if previous is not None and self._config.smoothing_alpha < 1.0: + alpha = self._config.smoothing_alpha + joint_targets = previous * (1.0 - alpha) + joint_targets * alpha + + self._last_targets[side] = joint_targets + self._robot.set_joint_position_target( + target=joint_targets, + joint_ids=self._joint_ids[side], + ) + + def update(self, sample: Mapping[str, Any]) -> None: + if not self._config.enabled: + return + + for side in ("left", "right"): + pose = sample["ee_poses"].get(side) + if pose is None: + target = self._last_targets.get(side) + if target is not None: + self._robot.set_joint_position_target( + target=target, + joint_ids=self._joint_ids[side], + ) + continue + + target_pose = self._target_pose_root(side, pose) + if target_pose is None: + continue + self._apply_target_pose_root(side, target_pose) + + def reset_startup_references(self) -> None: + self._startup_target_refs.clear() + self._startup_ref_announced.clear() + print( + f"[{self._config.status_label}] arm IK startup target references cleared; " + "waiting for re-arm calibration.", + flush=True, + ) + + +def _find_required_joint_ids( + robot: Any, + joint_names: Sequence[str], + side: str, + *, + status_label: str, +) -> list[int]: + joint_ids, resolved_names = robot.find_joints( + list(joint_names), preserve_order=True + ) + missing = [name for name in joint_names if name not in resolved_names] + if missing: + available = list(getattr(robot.data, "joint_names", []) or []) + raise RuntimeError( + f"{status_label} USD is missing {side} hand joints. " + f"Missing first entries: {missing[:8]}; available count={len(available)}" + ) + return list(joint_ids) + + +def _default_joint_positions( + robot: Any, + joint_ids: Sequence[int], +) -> np.ndarray: + default_joint_pos = as_torch(robot.data.default_joint_pos) + values = default_joint_pos[0, list(joint_ids)].detach().cpu().numpy() + return values.astype(np.float32) + + +def _soft_joint_limits(robot: Any, joint_ids: Sequence[int]) -> np.ndarray | None: + limits = as_torch(robot.data.soft_joint_pos_limits) + values = limits[0, list(joint_ids), :].detach().cpu().numpy() + if values.shape != (len(joint_ids), 2): + return None + return values.astype(np.float32) + + +def _parse_args(argv: Sequence[str]) -> argparse.Namespace: + AppLauncher = _load_isaac_lab() + parser = argparse.ArgumentParser( + description="Run a minimal G1 Isaac Lab scene driven by AVP + MANUS teleop." + ) + parser.add_argument("--config", default=str(_default_config_path())) + parser.add_argument("--robot-usd", default=None) + parser.add_argument("--robot-prim", default=None) + parser.add_argument("--duration-s", type=float, default=0.0) + parser.add_argument( + "--robot-height", + type=float, + default=None, + help="Optional override for robot.initial_world_position z.", + ) + parser.add_argument("--light-intensity", type=float, default=3000.0) + parser.add_argument( + "--scene-only", + action="store_true", + help="Load the minimal scene and robot without starting OpenXR/MANUS teleop.", + ) + AppLauncher.add_app_launcher_args(parser) + return parser.parse_args(argv) + + +def main(argv: Sequence[str] | None = None) -> int: + args = _parse_args(sys.argv[1:] if argv is None else argv) + config_path = Path(args.config).expanduser().resolve() + teleop_config = load_yaml_file(config_path) + teleop_config["_config_dir"] = str(config_path.parent) + robot_profile = robot_profile_from_config(teleop_config) + status_label = robot_profile.status_label + head_view_camera_config = _head_view_camera_config(teleop_config) + third_viewpoint_config = _third_viewpoint_config(teleop_config) + initial_world_position = _robot_initial_world_position(teleop_config) + initial_world_orientation_xyzw = _robot_initial_world_orientation_xyzw( + teleop_config + ) + wuji_hand_config = wuji_hand_runtime_config(teleop_config) + arm_ik_config = _arm_ik_runtime_config(teleop_config) + avp_frame_binding_config = avp_frame_binding_runtime_config(teleop_config) + if args.robot_height is not None: + initial_world_position = ( + initial_world_position[0], + initial_world_position[1], + float(args.robot_height), + ) + robot_usd = ( + Path(args.robot_usd).expanduser().resolve() + if args.robot_usd + else example_path(robot_profile.default_usd_relpath) + ) + if not robot_usd.is_file(): + raise FileNotFoundError(f"{status_label} USD not found: {robot_usd}") + robot_prim = str(args.robot_prim or robot_profile.robot_prim) + + AppLauncher = _load_isaac_lab() + app_launcher = AppLauncher(args) + simulation_app = app_launcher.app + + import carb.input + import omni.appwindow + import omni.kit.viewport.utility + import omni.ui + import omni.usd + import torch + from isaacsim.core.rendering_manager import ViewportManager + from pxr import Gf, UsdGeom + + import isaaclab.sim as sim_utils + from isaaclab.sim import SimulationContext + + sim_cfg = sim_utils.SimulationCfg(dt=1.0 / 60.0, device=args.device) + sim = SimulationContext(sim_cfg) + sim.set_camera_view([3.0, -4.0, 2.0], [0.0, 0.0, 0.9]) + render_enabled = not args.headless + + scene_config = G1WujiSceneConfig( + robot_prim=robot_prim, + robot_usd=robot_usd, + initial_world_position=initial_world_position, + initial_world_orientation_xyzw=initial_world_orientation_xyzw, + initial_hand_joint_positions=wuji_hand_config.initial_joint_positions, + light_intensity=args.light_intensity, + ) + robot = design_g1_wuji_scene(scene_config) + + sim.reset() + sim_dt = sim.get_physics_dt() + left_joint_ids = _find_required_joint_ids( + robot, + wuji_hand_config.left_joint_names, + "left", + status_label=status_label, + ) + right_joint_ids = _find_required_joint_ids( + robot, + wuji_hand_config.right_joint_names, + "right", + status_label=status_label, + ) + left_initial_targets = _default_joint_positions( + robot, + left_joint_ids, + ) + right_initial_targets = _default_joint_positions( + robot, + right_joint_ids, + ) + left_postprocessor = HandTargetPostProcessor( + joint_names=wuji_hand_config.left_joint_names, + initial_targets=left_initial_targets, + joint_limits=_soft_joint_limits(robot, left_joint_ids), + config=wuji_hand_config, + side="left", + ) + right_postprocessor = HandTargetPostProcessor( + joint_names=wuji_hand_config.right_joint_names, + initial_targets=right_initial_targets, + joint_limits=_soft_joint_limits(robot, right_joint_ids), + config=wuji_hand_config, + side="right", + ) + wuji_backend = WujiHandTargetBackend(wuji_hand_config) + avp_frame_binding = ( + AvpRobotFrameBinding( + robot, + arm_ik_config, + avp_frame_binding_config, + pose_scale=arm_ik_config.pose_scale, + pose_z_offset=arm_ik_config.pose_z_offset, + ) + if avp_frame_binding_config.enabled + else None + ) + arm_ik = ( + DualArmIkController( + robot, + arm_ik_config, + device=sim.device, + pose_binding=avp_frame_binding, + ) + if arm_ik_config.enabled + else None + ) + stage = omni.usd.get_context().get_stage() + head_link_prim = None + if head_view_camera_config.enabled: + head_link_prim = stage.GetPrimAtPath( + head_view_camera_config.head_link_prim_path + ) + if head_link_prim is None or not head_link_prim.IsValid(): + raise FileNotFoundError( + f"{status_label} head camera prim does not exist: " + f"{head_view_camera_config.head_link_prim_path}" + ) + print( + f"[{status_label}] head camera uses prim {head_link_prim.GetPath()}", + flush=True, + ) + xform_cache = UsdGeom.XformCache() + viewport_api = omni.kit.viewport.utility.get_active_viewport() + default_viewport_camera_path = ( + str(viewport_api.camera_path) + if viewport_api is not None and viewport_api.camera_path is not None + else None + ) + head_view_camera = ( + HeadViewCameraController( + stage=stage, + UsdGeom=UsdGeom, + Gf=Gf, + viewport_api=viewport_api, + config=head_view_camera_config, + head_link_prim=head_link_prim, + avp_frame_binding=avp_frame_binding, + ) + if head_view_camera_config.enabled and stage is not None + else None + ) + third_viewpoint_reference_prim = None + if third_viewpoint_config is not None and stage is not None: + third_viewpoint_reference_path = ( + f"{robot_prim.rstrip('/')}/{avp_frame_binding_config.robot_reference_body}" + ) + third_viewpoint_reference_prim = stage.GetPrimAtPath( + third_viewpoint_reference_path + ) + if ( + third_viewpoint_reference_prim is None + or not third_viewpoint_reference_prim.IsValid() + ): + raise FileNotFoundError( + f"{status_label} third viewpoint reference prim does not exist: " + f"{third_viewpoint_reference_path}" + ) + third_viewpoint = ( + ThirdViewpointController( + Gf=Gf, + ViewportManager=ViewportManager, + viewport_utility=omni.kit.viewport.utility, + ui=omni.ui, + config=third_viewpoint_config, + reference_prim=third_viewpoint_reference_prim, + main_viewport_api=viewport_api, + main_camera_path=( + None if head_view_camera is None else head_view_camera.camera_path + ), + status_label=status_label, + ) + if ( + third_viewpoint_config is not None + and stage is not None + and not args.headless + ) + else None + ) + if third_viewpoint is None: + _hide_third_viewpoint_window(omni.kit.viewport.utility) + teleop: TeleopMain | None = None + teleop_started = False + scene_warmed_up = False + robot_default_joint_pos = as_torch(robot.data.default_joint_pos).clone() + robot_default_joint_vel = as_torch(robot.data.default_joint_vel).clone() + robot.set_joint_position_target( + target=torch.as_tensor(left_initial_targets, device=sim.device).unsqueeze(0), + joint_ids=left_joint_ids, + ) + robot.set_joint_position_target( + target=torch.as_tensor(right_initial_targets, device=sim.device).unsqueeze(0), + joint_ids=right_joint_ids, + ) + + print( + f"{status_label} teleop scene ready: " + f"{len(left_joint_ids)} left hand joints, {len(right_joint_ids)} right hand joints, " + f"hand_retarget={wuji_hand_config.retarget_backend}, " + f"arm_ik={arm_ik_config.enabled}, " + f"avp_frame_binding={avp_frame_binding_config.enabled}, " + f"head_view_camera={head_view_camera is not None}, " + f"third_viewpoint={third_viewpoint is not None}." + ) + + def step_scene_once() -> None: + robot.write_data_to_sim() + sim.step(render=render_enabled) + robot.update(sim_dt) + if head_view_camera is not None: + head_view_camera.update(xform_cache=xform_cache) + + if args.scene_only: + print("Scene-only mode: OpenXR/MANUS teleop is not started.") + start_time = time.monotonic() + while simulation_app.is_running(): + if ( + args.duration_s > 0.0 + and time.monotonic() - start_time >= args.duration_s + ): + break + step_scene_once() + simulation_app.close() + return 0 + + teleop_armed = False + start_waiting_announced = False + keyboard_start_requested = False + keyboard_reset_requested = False + + app_window = omni.appwindow.get_default_app_window() + input_interface = carb.input.acquire_input_interface() + + def on_keyboard_event(event, *args, **kwargs): + nonlocal keyboard_reset_requested, keyboard_start_requested + if event.type == carb.input.KeyboardEventType.KEY_PRESS: + if event.input == carb.input.KeyboardInput.B: + keyboard_start_requested = True + elif event.input == carb.input.KeyboardInput.R: + keyboard_reset_requested = True + return True + + keyboard_subscription = ( + input_interface.subscribe_to_keyboard_events( + app_window.get_keyboard(), + on_keyboard_event, + ) + if app_window is not None and app_window.get_keyboard() is not None + else None + ) + + def hold_robot_targets() -> None: + robot.set_joint_position_target( + target=torch.as_tensor(left_initial_targets, device=sim.device).unsqueeze( + 0 + ), + joint_ids=left_joint_ids, + ) + robot.set_joint_position_target( + target=torch.as_tensor(right_initial_targets, device=sim.device).unsqueeze( + 0 + ), + joint_ids=right_joint_ids, + ) + + def restore_robot_initial_pose() -> None: + robot.write_joint_state_to_sim( + robot_default_joint_pos, + robot_default_joint_vel, + ) + robot.set_joint_position_target( + target=robot_default_joint_pos, + ) + if arm_ik is not None: + arm_ik._last_targets.clear() + + def reset_teleop_calibration() -> None: + if avp_frame_binding is not None: + avp_frame_binding.reset_calibration() + if arm_ik is not None: + arm_ik.reset_startup_references() + if head_view_camera is not None: + head_view_camera.deactivate() + if viewport_api is not None and default_viewport_camera_path: + viewport_api.camera_path = default_viewport_camera_path + if third_viewpoint is not None: + third_viewpoint.deactivate() + + def close_openxr_runtime() -> None: + nonlocal teleop, teleop_started + if teleop_started and teleop is not None: + teleop.__exit__(None, None, None) + teleop = None + teleop_started = False + + def start_openxr_runtime() -> None: + nonlocal teleop, teleop_started + teleop = TeleopMain(teleop_config, config_dir=config_path.parent) + teleop.__enter__() + teleop_started = True + print( + f"[{status_label}] OpenXR teleop session started.", + flush=True, + ) + + start_time = time.monotonic() + if not simulation_app.is_running(): + has_keyboard = bool( + app_window is not None and app_window.get_keyboard() is not None + ) + print( + f"[{status_label}] simulation app is not running at teleop loop start; " + "exiting before the first teleop frame. " + f"headless={args.headless} " + f"app_window={app_window is not None} " + f"keyboard={has_keyboard}", + flush=True, + ) + while simulation_app.is_running(): + if args.duration_s > 0.0 and time.monotonic() - start_time >= args.duration_s: + break + + start_button_rising = keyboard_start_requested + keyboard_start_requested = False + reset_button_rising = keyboard_reset_requested + keyboard_reset_requested = False + + if reset_button_rising: + teleop_armed = False + start_waiting_announced = False + restore_robot_initial_pose() + reset_teleop_calibration() + print( + f"[{status_label}] teleop reset; robot returned to its initial joint pose. " + "Press keyboard B to recalibrate and start again.", + flush=True, + ) + + if not scene_warmed_up: + hold_robot_targets() + step_scene_once() + scene_warmed_up = True + continue + + if teleop is None: + start_openxr_runtime() + + sample = teleop.step() + if not teleop_armed: + if not start_waiting_announced: + print( + f"[{status_label}] teleop is disarmed; press keyboard B to recalibrate " + "and start teleop. Press keyboard R to force reset back to this state.", + flush=True, + ) + start_waiting_announced = True + if start_button_rising: + teleop_armed = True + start_waiting_announced = False + reset_teleop_calibration() + print( + f"[{status_label}] teleop armed at frame={int(sample.get('frame_count', 0))}; " + "starting fresh AVP head-hand calibration " + f"(averaging {avp_frame_binding_config.calibration_window_s:.1f}s).", + flush=True, + ) + else: + hold_robot_targets() + step_scene_once() + continue + + raw_left_targets, raw_right_targets = wuji_backend.targets(sample) + + left_targets = left_postprocessor.process(raw_left_targets) + right_targets = right_postprocessor.process(raw_right_targets) + + if left_targets is not None: + robot.set_joint_position_target( + target=torch.as_tensor(left_targets, device=sim.device).unsqueeze(0), + joint_ids=left_joint_ids, + ) + if right_targets is not None: + robot.set_joint_position_target( + target=torch.as_tensor(right_targets, device=sim.device).unsqueeze(0), + joint_ids=right_joint_ids, + ) + if avp_frame_binding is not None: + avp_frame_binding.update_calibration(sample) + avp_frame_binding.update_runtime_state(sample) + if ( + head_view_camera is not None + and head_view_camera_config.activate_on_calibration + and avp_frame_binding.calibrated + ): + if third_viewpoint is not None: + third_viewpoint.activate(xform_cache=xform_cache) + head_view_camera.activate() + if arm_ik is not None: + arm_ik.update(sample) + + step_scene_once() + + if keyboard_subscription is not None: + input_interface.unsubscribe_from_keyboard_events( + app_window.get_keyboard(), + keyboard_subscription, + ) + close_openxr_runtime() + simulation_app.close() + + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/cli.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/cli.py new file mode 100644 index 000000000..d994da3b1 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/cli.py @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Thin CLI entrypoint for the G1-Wuji teleop example.""" + +from __future__ import annotations + +from collections.abc import Sequence + +from .app import main as app_main + + +def teleop_main(argv: Sequence[str] | None = None) -> int: + return app_main(argv) diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/config.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/config.py new file mode 100644 index 000000000..156731225 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/config.py @@ -0,0 +1,47 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Configuration and path helpers for the G1-Wuji teleop example.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + + +def package_root() -> Path: + return Path(__file__).resolve().parent + + +def example_root() -> Path: + return package_root().parents[1] + + +def default_config_path() -> Path: + return (example_root() / "config" / "avp_manus.yaml").resolve() + + +def default_robot_usd_path() -> Path: + return (example_root() / "assets" / "g1_wuji" / "g1_wuji.usd").resolve() + + +def example_path(relative_path: str | Path) -> Path: + path = Path(relative_path).expanduser() + if path.is_absolute(): + return path.resolve() + return (example_root() / path).resolve() + + +def load_yaml_file(path: Path) -> dict[str, Any]: + try: + import yaml + except ModuleNotFoundError as exc: + raise ModuleNotFoundError( + "PyYAML is required for G1-Wuji teleop configs." + ) from exc + + with path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) or {} + if not isinstance(data, dict): + raise ValueError(f"Config root must be a mapping: {path}") + return data diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/input_stream.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/input_stream.py new file mode 100644 index 000000000..3aff60b80 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/input_stream.py @@ -0,0 +1,506 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""OpenXR + MANUS input stream for the G1-Wuji teleop example.""" + +from __future__ import annotations + +import argparse +import sys +import time +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +from .config import default_config_path, load_yaml_file +from .transforms import transform_controller_pose_to_palm + + +SIDE_NAMES = ("left", "right") + + +@dataclass(frozen=True) +class RuntimeDeps: + DeviceIOSession: Any + ControllersSource: Any + HeadSource: Any + HandsSource: Any + OutputCombiner: Any + PluginConfig: Any + TeleopSession: Any + TeleopSessionConfig: Any + ControllerInputIndex: Any + HandInputIndex: Any + HandJointIndex: Any + HeadPoseIndex: Any + + +@dataclass(frozen=True) +class ControllerPalmOffset: + pos: tuple[float, float, float] + quat_xyzw: tuple[float, float, float, float] + + +@dataclass(frozen=True) +class EePoseConfig: + source: str + controller_pose_source: str + hand_joint: str + controller_to_palm: dict[str, ControllerPalmOffset] + + +def _load_runtime_deps() -> RuntimeDeps: + try: + import isaacteleop.deviceio as deviceio + from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + HandsSource, + HeadSource, + ) + from isaacteleop.retargeting_engine.interface import OutputCombiner + from isaacteleop.retargeting_engine.tensor_types.indices import ( + ControllerInputIndex, + HandInputIndex, + HandJointIndex, + HeadPoseIndex, + ) + from isaacteleop.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, + ) + except ModuleNotFoundError as exc: + raise ModuleNotFoundError( + f"Missing Python dependency {exc.name!r}. Activate an IsaacTeleop " + "environment with CloudXR and retargeter dependencies." + ) from exc + + return RuntimeDeps( + DeviceIOSession=deviceio.DeviceIOSession, + ControllersSource=ControllersSource, + HeadSource=HeadSource, + HandsSource=HandsSource, + OutputCombiner=OutputCombiner, + PluginConfig=PluginConfig, + TeleopSession=TeleopSession, + TeleopSessionConfig=TeleopSessionConfig, + ControllerInputIndex=ControllerInputIndex, + HandInputIndex=HandInputIndex, + HandJointIndex=HandJointIndex, + HeadPoseIndex=HeadPoseIndex, + ) + + +def _as_mapping(value: Any, name: str) -> Mapping[str, Any]: + if value is None: + return {} + if not isinstance(value, Mapping): + raise ValueError(f"Config field {name!r} must be a mapping") + return value + + +def _as_sequence(value: Any, name: str) -> Sequence[Any]: + if isinstance(value, (str, bytes)) or not isinstance(value, Sequence): + raise ValueError(f"Config field {name!r} must be a sequence") + return value + + +def _float_tuple(value: Any, *, name: str, length: int) -> tuple[float, ...]: + raw = _as_sequence(value, name) + if len(raw) != length: + raise ValueError(f"Config field {name!r} must contain {length} values") + return tuple(float(item) for item in raw) + + +def _resolve_path(path_value: Any, *, base_dir: Path, name: str) -> Path: + if path_value is None: + raise ValueError(f"Config field {name!r} is required") + path = Path(str(path_value)).expanduser() + if not path.is_absolute(): + path = base_dir / path + return path.resolve() + + +def _load_controller_palm_offsets( + avp_cfg: Mapping[str, Any], +) -> dict[str, ControllerPalmOffset]: + offsets: dict[str, ControllerPalmOffset] = {} + for side in SIDE_NAMES: + offsets[side] = ControllerPalmOffset( + pos=_float_tuple( + avp_cfg.get(f"{side}_controller_to_palm_pos"), + name=f"avp.{side}_controller_to_palm_pos", + length=3, + ), + quat_xyzw=_float_tuple( + avp_cfg.get(f"{side}_controller_to_palm_quat_xyzw"), + name=f"avp.{side}_controller_to_palm_quat_xyzw", + length=4, + ), + ) + return offsets + + +def _load_ee_pose_config(config: Mapping[str, Any]) -> EePoseConfig: + avp_cfg = _as_mapping(config.get("avp"), "avp") + source = str(avp_cfg.get("ee_pose_source", "hand_tracking")).lower() + hand_joint = str(avp_cfg.get("ee_hand_joint", "wrist")).lower() + controller_pose_source = str(avp_cfg.get("controller_pose_source", "grip")).lower() + + if source not in {"controller", "hand_tracking"}: + raise ValueError("avp.ee_pose_source must be 'controller' or 'hand_tracking'") + if hand_joint not in {"wrist", "palm"}: + raise ValueError("avp.ee_hand_joint must be 'wrist' or 'palm'") + if controller_pose_source not in {"aim", "grip"}: + raise ValueError("avp.controller_pose_source must be 'aim' or 'grip'") + + return EePoseConfig( + source=source, + controller_pose_source=controller_pose_source, + hand_joint=hand_joint, + controller_to_palm=( + _load_controller_palm_offsets(avp_cfg) if source == "controller" else {} + ), + ) + + +def _controller_pose( + group: Any, + *, + pose_source: str, + deps: RuntimeDeps, +) -> dict[str, Any] | None: + if group.is_none: + return None + + if pose_source == "aim": + position_index = deps.ControllerInputIndex.AIM_POSITION + orientation_index = deps.ControllerInputIndex.AIM_ORIENTATION + valid_index = deps.ControllerInputIndex.AIM_IS_VALID + elif pose_source == "grip": + position_index = deps.ControllerInputIndex.GRIP_POSITION + orientation_index = deps.ControllerInputIndex.GRIP_ORIENTATION + valid_index = deps.ControllerInputIndex.GRIP_IS_VALID + else: + raise ValueError(f"Unsupported controller pose source: {pose_source!r}") + + if not bool(group[valid_index]): + return None + + return { + "position": [float(value) for value in group[position_index]], + "orientation_xyzw": [float(value) for value in group[orientation_index]], + } + + +def _hand_tracking_pose( + group: Any, + *, + joint_name: str, + deps: RuntimeDeps, +) -> dict[str, Any] | None: + if group.is_none: + return None + + import numpy as np + + joint_index = int(getattr(deps.HandJointIndex, joint_name.upper())) + valid = np.from_dlpack(group[deps.HandInputIndex.JOINT_VALID]) + if not bool(valid[joint_index]): + return None + + positions = np.from_dlpack(group[deps.HandInputIndex.JOINT_POSITIONS]) + orientations = np.from_dlpack(group[deps.HandInputIndex.JOINT_ORIENTATIONS]) + return { + "position": [float(value) for value in positions[joint_index]], + "orientation_xyzw": [float(value) for value in orientations[joint_index]], + } + + +def _head_pose(group: Any, *, deps: RuntimeDeps) -> dict[str, Any] | None: + if group.is_none or not bool(group[deps.HeadPoseIndex.IS_VALID]): + return None + + return { + "position": [float(value) for value in group[deps.HeadPoseIndex.POSITION]], + "orientation_xyzw": [ + float(value) for value in group[deps.HeadPoseIndex.ORIENTATION] + ], + } + + +def _controller_scalar( + group: Any, + index_enum: Any, + field_name: str, +) -> float: + return float(group[getattr(index_enum, field_name)]) + + +def _controller_inputs(group: Any, *, deps: RuntimeDeps) -> dict[str, Any] | None: + if group.is_none: + return None + controller_index = deps.ControllerInputIndex + return { + "primary_click": _controller_scalar(group, controller_index, "PRIMARY_CLICK") + > 0.5, + "secondary_click": _controller_scalar( + group, controller_index, "SECONDARY_CLICK" + ) + > 0.5, + "thumbstick_click": _controller_scalar( + group, controller_index, "THUMBSTICK_CLICK" + ) + > 0.5, + "menu_click": _controller_scalar(group, controller_index, "MENU_CLICK") > 0.5, + "thumbstick_x": _controller_scalar(group, controller_index, "THUMBSTICK_X"), + "thumbstick_y": _controller_scalar(group, controller_index, "THUMBSTICK_Y"), + "squeeze_value": _controller_scalar(group, controller_index, "SQUEEZE_VALUE"), + "trigger_value": _controller_scalar(group, controller_index, "TRIGGER_VALUE"), + } + + +def _openxr_hand_joint_names(deps: RuntimeDeps) -> list[str]: + return [ + name.lower() + for name, _index in sorted( + deps.HandJointIndex.__members__.items(), + key=lambda item: int(item[1]), + ) + ] + + +def _hand_tracking_skeleton(group: Any, *, deps: RuntimeDeps) -> dict[str, Any] | None: + if group.is_none: + return None + + import numpy as np + + positions = np.from_dlpack(group[deps.HandInputIndex.JOINT_POSITIONS]).copy() + valid = np.from_dlpack(group[deps.HandInputIndex.JOINT_VALID]).astype(bool).copy() + if positions.ndim != 2 or positions.shape[1] != 3: + return None + if valid.ndim != 1 or positions.shape[0] != valid.shape[0]: + return None + + return { + "joint_names": _openxr_hand_joint_names(deps), + "positions": [[float(coord) for coord in position] for position in positions], + "valid": [bool(value) for value in valid], + } + + +class TeleopMain: + """Facade returning EE poses and MANUS skeletons from TeleopSession.""" + + def __init__( + self, + config: Mapping[str, Any], + *, + config_dir: Path, + oxr_handles: Any | None = None, + ) -> None: + self._config = config + self._config_dir = config_dir + self._deps = _load_runtime_deps() + self._oxr_handles = oxr_handles + self._session = None + self._session_context = None + self._ee_config = _load_ee_pose_config(config) + self._manus_plugin_configs = self._build_plugin_configs() + self._required_xr_extensions: tuple[str, ...] = () + self._session_config = self._build_session_config() + + @classmethod + def from_config_file( + cls, + path: str | Path | None = None, + *, + oxr_handles: Any | None = None, + ) -> "TeleopMain": + config_path = Path(path).expanduser() if path else default_config_path() + config_path = config_path.resolve() + return cls( + load_yaml_file(config_path), + config_dir=config_path.parent, + oxr_handles=oxr_handles, + ) + + @property + def required_xr_extensions(self) -> tuple[str, ...]: + return tuple(self._required_xr_extensions) + + def set_oxr_handles(self, oxr_handles: Any | None) -> None: + if self._session is not None or self._session_context is not None: + raise RuntimeError( + "TeleopMain.set_oxr_handles() must be called before starting the session" + ) + self._oxr_handles = oxr_handles + self._session_config = self._build_session_config() + + def __enter__(self) -> "TeleopMain": + self._session_context = self._deps.TeleopSession(self._session_config) + self._session = self._session_context.__enter__() + return self + + def __exit__(self, exc_type, exc_value, traceback) -> None: + if self._session_context is not None: + self._session_context.__exit__(exc_type, exc_value, traceback) + self._session_context = None + self._session = None + + def step(self) -> dict[str, Any]: + if self._session is None: + raise RuntimeError("TeleopMain must be entered before step()") + + result = self._session.step() + left_hand = result["openxr_hand_left"] + right_hand = result["openxr_hand_right"] + controller_outputs = self._controller_outputs(result) + return { + "timestamp_s": time.time(), + "frame_count": self._session.frame_count, + "head_pose": _head_pose(result["avp_head"], deps=self._deps), + "ee_poses": { + "left": self._ee_pose(result, "left"), + "right": self._ee_pose(result, "right"), + }, + "hand_active": { + "left": not left_hand.is_none, + "right": not right_hand.is_none, + }, + "hand_skeletons": { + "left": _hand_tracking_skeleton(left_hand, deps=self._deps), + "right": _hand_tracking_skeleton(right_hand, deps=self._deps), + }, + "hand_joint_names": {side: [] for side in SIDE_NAMES}, + "hand_joint_positions": {side: None for side in SIDE_NAMES}, + "controller_inputs": controller_outputs, + } + + def _build_session_config(self): + deps = self._deps + head = deps.HeadSource(name="avp_head") + hands = deps.HandsSource(name="manus_hands") + trackers = [head.get_tracker(), hands.get_tracker()] + output_mapping = { + "avp_head": head.output("head"), + "openxr_hand_left": hands.output(deps.HandsSource.LEFT), + "openxr_hand_right": hands.output(deps.HandsSource.RIGHT), + } + + if self._ee_config.source == "controller": + controllers = deps.ControllersSource(name="avp_controllers") + trackers.append(controllers.get_tracker()) + output_mapping.update( + { + "avp_controller_left": controllers.output( + deps.ControllersSource.LEFT + ), + "avp_controller_right": controllers.output( + deps.ControllersSource.RIGHT + ), + } + ) + + pipeline = deps.OutputCombiner(output_mapping) + self._required_xr_extensions = tuple( + deps.DeviceIOSession.get_required_extensions(trackers) + ) + session_cfg = _as_mapping(self._config.get("session"), "session") + return deps.TeleopSessionConfig( + app_name=str(session_cfg.get("app_name", "G1WujiTeleop")), + pipeline=pipeline, + plugins=self._manus_plugin_configs, + oxr_handles=self._oxr_handles, + ) + + def _build_plugin_configs(self) -> list[Any]: + plugin_cfg = _as_mapping(self._config.get("manus_plugin"), "manus_plugin") + if not bool(plugin_cfg.get("enabled", True)): + return [] + + search_paths_raw = _as_sequence( + plugin_cfg.get("search_paths"), "manus_plugin.search_paths" + ) + search_paths = [ + _resolve_path( + path, base_dir=self._config_dir, name="manus_plugin.search_paths" + ) + for path in search_paths_raw + ] + missing = [path for path in search_paths if not path.is_dir()] + if missing: + raise FileNotFoundError( + "MANUS plugin search path does not exist: " + + ", ".join(str(path) for path in missing) + ) + + return [ + self._deps.PluginConfig( + plugin_name=str(plugin_cfg.get("plugin_name", "manus_hand_plugin")), + plugin_root_id=str(plugin_cfg.get("plugin_root_id", "manus")), + search_paths=search_paths, + enabled=True, + plugin_args=[ + str(arg) + for arg in _as_sequence( + plugin_cfg.get("args", []), "manus_plugin.args" + ) + ], + ) + ] + + def _controller_outputs(self, result: Mapping[str, Any]) -> dict[str, Any]: + if self._ee_config.source != "controller": + return {side: None for side in SIDE_NAMES} + return { + side: _controller_inputs(result[f"avp_controller_{side}"], deps=self._deps) + for side in SIDE_NAMES + } + + def _ee_pose(self, result: Mapping[str, Any], side: str) -> dict[str, Any] | None: + if self._ee_config.source == "hand_tracking": + return _hand_tracking_pose( + result[f"openxr_hand_{side}"], + joint_name=self._ee_config.hand_joint, + deps=self._deps, + ) + + pose = _controller_pose( + result[f"avp_controller_{side}"], + pose_source=self._ee_config.controller_pose_source, + deps=self._deps, + ) + if pose is None: + return None + offset = self._ee_config.controller_to_palm[side] + palm_pos, palm_quat = transform_controller_pose_to_palm( + controller_pos=pose["position"], + controller_quat_xyzw=pose["orientation_xyzw"], + offset_pos=offset.pos, + offset_quat_xyzw=offset.quat_xyzw, + ) + return { + "position": [float(value) for value in palm_pos], + "orientation_xyzw": [float(value) for value in palm_quat], + } + + +def _parse_args(argv: Sequence[str]) -> argparse.Namespace: + parser = argparse.ArgumentParser(description="Run the G1-Wuji input stream.") + parser.add_argument("--config", default=str(default_config_path())) + return parser.parse_args(argv) + + +def main(argv: Sequence[str] | None = None) -> int: + args = _parse_args(sys.argv[1:] if argv is None else argv) + with TeleopMain.from_config_file(args.config) as teleop: + while True: + teleop.step() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/retargeting.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/retargeting.py new file mode 100644 index 000000000..8ed6a3679 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/retargeting.py @@ -0,0 +1,198 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Adapter for the external official ``wuji-retargeting`` package.""" + +from __future__ import annotations + +import importlib.util +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + + +@dataclass(frozen=True) +class HandRetargetStepResult: + frame_index: int + left_joint_positions: np.ndarray + right_joint_positions: np.ndarray + left_joint_names: tuple[str, ...] | None + right_joint_names: tuple[str, ...] | None + ok: bool + error: str | None = None + latency_ms: float = 0.0 + raw: dict[str, Any] = field(default_factory=dict) + + +def import_official_wuji_retargeter(): + if importlib.util.find_spec("wuji_retargeting.retarget") is None: + raise ModuleNotFoundError( + "Official wuji-retargeting is not installed in this Python environment. " + "Install it with `python -m pip install -e /path/to/wuji-retargeting` " + "before running the G1-Wuji teleop example." + ) + + from wuji_retargeting.retarget import Retargeter + + return Retargeter + + +def _as_float32_numpy(values: Any, shape: tuple[int, ...]) -> np.ndarray: + return np.asarray(values, dtype=np.float32).reshape(shape) + + +def _valid_skeleton21(skeleton: np.ndarray) -> tuple[bool, str | None]: + if skeleton.shape != (21, 3): + return False, f"unexpected shape {skeleton.shape}" + if not np.all(np.isfinite(skeleton)): + return False, "non-finite landmarks" + + centered = skeleton - skeleton[0:1] + if float(np.max(np.linalg.norm(centered, axis=1))) < 1.0e-4: + return False, "all landmarks collapsed to the wrist" + + palm_vec_a = skeleton[5] - skeleton[0] + palm_vec_b = skeleton[9] - skeleton[0] + if np.linalg.norm(palm_vec_a) < 1.0e-5 or np.linalg.norm(palm_vec_b) < 1.0e-5: + return False, "missing palm anchor landmarks" + if np.linalg.norm(np.cross(palm_vec_a, palm_vec_b)) < 1.0e-7: + return False, "degenerate palm frame" + return True, None + + +class WujiOfficialManusHandRetargeter: + """Retarget MANUS/OpenXR-style 21-point skeletons with Wuji's package.""" + + def __init__( + self, + *, + config_dir: Path, + left_config_name: str = "left.yaml", + right_config_name: str = "right.yaml", + ) -> None: + self._config_dir = Path(config_dir).expanduser().resolve() + self._config_names = {"left": left_config_name, "right": right_config_name} + self._retargeter_left: Any | None = None + self._retargeter_right: Any | None = None + self._left_joint_names: tuple[str, ...] | None = None + self._right_joint_names: tuple[str, ...] | None = None + + @property + def left_joint_names(self) -> tuple[str, ...] | None: + return self._left_joint_names + + @property + def right_joint_names(self) -> tuple[str, ...] | None: + return self._right_joint_names + + def _resolve_config_path(self, side: str) -> Path: + return (self._config_dir / self._config_names[side]).resolve() + + def _ensure_retargeters(self) -> None: + if self._retargeter_left is not None and self._retargeter_right is not None: + return + + Retargeter = import_official_wuji_retargeter() + left_yaml = self._resolve_config_path("left") + right_yaml = self._resolve_config_path("right") + for config_path in (left_yaml, right_yaml): + if not config_path.is_file(): + raise FileNotFoundError( + f"Wuji retarget config does not exist: {config_path}" + ) + + self._retargeter_left = Retargeter.from_yaml(str(left_yaml), hand_side="left") + self._retargeter_right = Retargeter.from_yaml( + str(right_yaml), hand_side="right" + ) + self._left_joint_names = tuple( + self._retargeter_left.optimizer.robot.dof_joint_names + ) + self._right_joint_names = tuple( + self._retargeter_right.optimizer.robot.dof_joint_names + ) + + @staticmethod + def _retarget_hand(side: str, skeleton: np.ndarray, retargeter: Any) -> np.ndarray: + valid, error = _valid_skeleton21(skeleton) + if not valid: + raise ValueError(f"Invalid {side} hand skeleton: {error}") + + qpos = np.asarray(retargeter.retarget(skeleton), dtype=np.float32).reshape(-1) + if qpos.shape != (int(retargeter.num_joints),): + raise RuntimeError( + f"{side} hand retarget returned {qpos.shape}, expected {(int(retargeter.num_joints),)}" + ) + if not np.all(np.isfinite(qpos)): + raise RuntimeError(f"{side} hand retarget returned non-finite values") + return qpos.copy() + + def retarget_manus_skeletons( + self, + left_skeleton: Any, + right_skeleton: Any, + *, + frame_index: int, + device: Any = None, + ) -> HandRetargetStepResult: + del device + self._ensure_retargeters() + started = time.perf_counter() + + left_qpos = self._retarget_hand( + "left", + _as_float32_numpy(left_skeleton, (21, 3)), + self._retargeter_left, + ) + right_qpos = self._retarget_hand( + "right", + _as_float32_numpy(right_skeleton, (21, 3)), + self._retargeter_right, + ) + + return HandRetargetStepResult( + frame_index=int(frame_index), + left_joint_positions=left_qpos, + right_joint_positions=right_qpos, + left_joint_names=self._left_joint_names, + right_joint_names=self._right_joint_names, + ok=True, + latency_ms=(time.perf_counter() - started) * 1000.0, + ) + + def retarget_manus_skeleton( + self, + side: str, + skeleton: Any, + *, + frame_index: int, + device: Any = None, + ) -> tuple[np.ndarray, tuple[str, ...] | None]: + del frame_index, device + self._ensure_retargeters() + if side == "left": + return ( + self._retarget_hand( + "left", + _as_float32_numpy(skeleton, (21, 3)), + self._retargeter_left, + ), + self._left_joint_names, + ) + if side == "right": + return ( + self._retarget_hand( + "right", + _as_float32_numpy(skeleton, (21, 3)), + self._retargeter_right, + ), + self._right_joint_names, + ) + raise ValueError(f"Unsupported hand side: {side!r}") + + def shutdown(self) -> None: + self._retargeter_left = None + self._retargeter_right = None diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/robot.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/robot.py new file mode 100644 index 000000000..98da94329 --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/robot.py @@ -0,0 +1,1232 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""MANUS and AVP data helpers for the G1-Wuji teleop example.""" + +from __future__ import annotations + +import time +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + +from .config import example_root + +LEFT_WUJI_JOINTS = ( + "left_finger1_joint1", + "left_finger1_joint2", + "left_finger1_joint3", + "left_finger1_joint4", + "left_finger2_joint1", + "left_finger2_joint2", + "left_finger2_joint3", + "left_finger2_joint4", + "left_finger3_joint1", + "left_finger3_joint2", + "left_finger3_joint3", + "left_finger3_joint4", + "left_finger4_joint1", + "left_finger4_joint2", + "left_finger4_joint3", + "left_finger4_joint4", + "left_finger5_joint1", + "left_finger5_joint2", + "left_finger5_joint3", + "left_finger5_joint4", +) + +RIGHT_WUJI_JOINTS = tuple( + name.replace("left_", "right_", 1) for name in LEFT_WUJI_JOINTS +) + +WUJI_SKELETON21_OPENXR_NAMES = ( + "wrist", + "thumb_metacarpal", + "thumb_proximal", + "thumb_distal", + "thumb_tip", + "index_proximal", + "index_intermediate", + "index_distal", + "index_tip", + "middle_proximal", + "middle_intermediate", + "middle_distal", + "middle_tip", + "ring_proximal", + "ring_intermediate", + "ring_distal", + "ring_tip", + "little_proximal", + "little_intermediate", + "little_distal", + "little_tip", +) + +FIXED_WUJI_HAND_CONFIG_DIR = "wuji_retargeting" + + +def _default_wuji_initial_joint_positions() -> dict[str, float]: + return { + "left_finger1_joint1": 0.059, + "left_finger1_joint2": 0.059, + "left_finger1_joint3": 0.059, + "left_finger1_joint4": 0.0, + "left_finger2_joint1": 0.0, + "left_finger2_joint2": 0.0, + "left_finger2_joint3": 0.0, + "left_finger2_joint4": 0.0, + "left_finger3_joint1": 0.0, + "left_finger3_joint2": 0.0, + "left_finger3_joint3": 0.0, + "left_finger3_joint4": 0.0, + "left_finger4_joint1": 0.0, + "left_finger4_joint2": 0.0, + "left_finger4_joint3": 0.0, + "left_finger4_joint4": 0.0, + "left_finger5_joint1": 0.0, + "left_finger5_joint2": 0.0, + "left_finger5_joint3": 0.0, + "left_finger5_joint4": 0.0, + "right_finger1_joint1": 0.037, + "right_finger1_joint2": 0.037, + "right_finger1_joint3": 0.037, + "right_finger1_joint4": 0.0, + "right_finger2_joint1": 0.0, + "right_finger2_joint2": 0.0, + "right_finger2_joint3": 0.0, + "right_finger2_joint4": 0.0, + "right_finger3_joint1": 0.0, + "right_finger3_joint2": 0.0, + "right_finger3_joint3": 0.0, + "right_finger3_joint4": 0.0, + "right_finger4_joint1": 0.0, + "right_finger4_joint2": 0.0, + "right_finger4_joint3": 0.0, + "right_finger4_joint4": 0.0, + "right_finger5_joint1": 0.0, + "right_finger5_joint2": 0.0, + "right_finger5_joint3": 0.0, + "right_finger5_joint4": 0.0, + } + + +@dataclass(frozen=True) +class RobotProfile: + variant: str + robot_prim: str + default_usd_relpath: str + left_hand_joint_names: tuple[str, ...] + right_hand_joint_names: tuple[str, ...] + initial_hand_joint_positions: dict[str, float] + hand_retarget_backend: str + hand_wuji_config_dir: str | None + ik_body_names: dict[str, str] + ik_body_offset_pos: dict[str, tuple[float, float, float]] + ik_body_offset_quat_xyzw: dict[str, tuple[float, float, float, float]] + ik_target_orientation_correction_quat_xyzw: dict[ + str, tuple[float, float, float, float] + ] + reference_body: str + reference_offset_pos: tuple[float, float, float] + reference_offset_quat_xyzw: tuple[float, float, float, float] + axis_signs: dict[str, float] + status_label: str + + +ROBOT_PROFILES: dict[str, RobotProfile] = { + "g1_wuji": RobotProfile( + variant="g1_wuji", + robot_prim="/World/G1Wuji", + default_usd_relpath="assets/g1_wuji/g1_wuji.usd", + left_hand_joint_names=LEFT_WUJI_JOINTS, + right_hand_joint_names=RIGHT_WUJI_JOINTS, + initial_hand_joint_positions=_default_wuji_initial_joint_positions(), + hand_retarget_backend="official_wuji", + hand_wuji_config_dir=FIXED_WUJI_HAND_CONFIG_DIR, + ik_body_names={ + "left": "left_wrist_yaw_link", + "right": "right_wrist_yaw_link", + }, + ik_body_offset_pos={ + "left": (0.0415, 0.003, 0.0), + "right": (0.0415, -0.003, 0.0), + }, + ik_body_offset_quat_xyzw={ + "left": (0.0, 0.0, 0.0, 1.0), + "right": (0.0, 0.0, 0.0, 1.0), + }, + ik_target_orientation_correction_quat_xyzw={ + "left": (0.5, 0.5, 0.5, 0.5), + "right": (-0.5, 0.5, 0.5, -0.5), + }, + reference_body="torso_link", + reference_offset_pos=(0.0077774, 0.0000210, 0.3836842), + reference_offset_quat_xyzw=(0.0, 0.0, 0.0, 1.0), + axis_signs={}, + status_label="G1-Wuji", + ), +} + + +def normalize_robot_variant(value: Any) -> str: + raw = str(value or "g1_wuji").strip().lower() + if raw not in ROBOT_PROFILES: + raise ValueError( + f"Unsupported robot.variant={value!r}. Expected one of {sorted(ROBOT_PROFILES)}" + ) + return raw + + +def robot_profile_from_config(config: Mapping[str, Any]) -> RobotProfile: + robot_config = config.get("robot", {}) + if robot_config is None: + robot_config = {} + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + return ROBOT_PROFILES[normalize_robot_variant(robot_config.get("variant"))] + + +@dataclass(frozen=True) +class WujiHandRuntimeConfig: + robot_variant: str = "g1_wuji" + neutral_calibration_samples: int = 30 + initial_joint_position: float = 0.0 + initial_joint_positions: dict[str, float] = field( + default_factory=_default_wuji_initial_joint_positions + ) + left_joint_names: tuple[str, ...] = LEFT_WUJI_JOINTS + right_joint_names: tuple[str, ...] = RIGHT_WUJI_JOINTS + axis_signs: dict[str, float] = field(default_factory=dict) + retarget_backend: str = "official_wuji" + status_label: str = "teleop" + wuji_config_dir: Path | None = None + retarget_output_mode: str = "absolute" + clamp_to_joint_limits: bool = True + smoothing_alpha: float = 0.8 + + +@dataclass(frozen=True) +class AvpFrameBindingRuntimeConfig: + status_label: str = "teleop" + enabled: bool = True + calibration_window_s: float = 3.0 + robot_reference_body: str = "torso_link" + robot_reference_offset_pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + robot_reference_offset_quat_xyzw: tuple[float, float, float, float] = ( + 0.0, + 0.0, + 0.0, + 1.0, + ) + head_forward_axis_isaac: tuple[float, float, float] = (0.0, 1.0, 0.0) + + +@dataclass +class _AvpHeadHandBindingState: + origin_pos: np.ndarray + frame_rot: np.ndarray + head_rot: np.ndarray + + +@dataclass +class _AvpPendingHeadHandCalibration: + start_timestamp_s: float + last_timestamp_s: float + sample_count: int = 0 + head_pos_sum: np.ndarray = field( + default_factory=lambda: np.zeros(3, dtype=np.float64) + ) + head_rot_sum: np.ndarray = field( + default_factory=lambda: np.zeros((3, 3), dtype=np.float64) + ) + left_pos_sum: np.ndarray = field( + default_factory=lambda: np.zeros(3, dtype=np.float64) + ) + right_pos_sum: np.ndarray = field( + default_factory=lambda: np.zeros(3, dtype=np.float64) + ) + + def append( + self, + *, + timestamp_s: float, + head_pos: np.ndarray, + head_rot: np.ndarray, + left_pos: np.ndarray, + right_pos: np.ndarray, + ) -> None: + self.last_timestamp_s = float(timestamp_s) + self.sample_count += 1 + self.head_pos_sum += np.asarray(head_pos, dtype=np.float64) + self.head_rot_sum += np.asarray(head_rot, dtype=np.float64) + self.left_pos_sum += np.asarray(left_pos, dtype=np.float64) + self.right_pos_sum += np.asarray(right_pos, dtype=np.float64) + + +class HandTargetPostProcessor: + """Map MANUS retarget qpos into stable USD joint position targets.""" + + def __init__( + self, + *, + joint_names: Sequence[str], + initial_targets: np.ndarray, + joint_limits: np.ndarray | None, + config: WujiHandRuntimeConfig, + side: str, + ) -> None: + self._joint_names = tuple(joint_names) + self._initial_targets = np.asarray(initial_targets, dtype=np.float32) + self._joint_limits = ( + None if joint_limits is None else np.asarray(joint_limits, dtype=np.float32) + ) + self._config = config + self._side = side + self._neutral_samples: list[np.ndarray] = [] + self._neutral: np.ndarray | None = None + self._last_target: np.ndarray | None = None + self._announced = False + + @property + def ready(self) -> bool: + return ( + self._config.neutral_calibration_samples <= 0 or self._neutral is not None + ) + + def process(self, raw_targets: np.ndarray | None) -> np.ndarray | None: + if raw_targets is None: + return None + + raw = np.asarray(raw_targets, dtype=np.float32) + if raw.shape != self._initial_targets.shape: + raise ValueError( + f"{self._side} {self._config.status_label} target shape mismatch: expected " + f"{self._initial_targets.shape}, got {raw.shape}" + ) + + if self._config.retarget_output_mode == "absolute": + target = raw + elif self._config.neutral_calibration_samples > 0 and self._neutral is None: + self._neutral_samples.append(raw.copy()) + if len(self._neutral_samples) >= self._config.neutral_calibration_samples: + self._neutral = np.mean(np.stack(self._neutral_samples), axis=0).astype( + np.float32 + ) + print( + f"[{self._config.status_label}] {self._side} neutral calibrated with " + f"{len(self._neutral_samples)} sample(s)." + ) + if self._last_target is not None: + return self._last_target + return self._initial_targets.copy() + + else: + neutral = self._neutral if self._neutral is not None else np.zeros_like(raw) + target = self._initial_targets + (raw - neutral) + + if self._config.clamp_to_joint_limits and self._joint_limits is not None: + target = np.clip(target, self._joint_limits[:, 0], self._joint_limits[:, 1]) + + if self._last_target is not None and self._config.smoothing_alpha < 1.0: + alpha = self._config.smoothing_alpha + target = self._last_target * (1.0 - alpha) + target * alpha + + self._last_target = target.astype(np.float32) + if not self._announced: + print( + f"[{self._config.status_label}] {self._side} hand postprocess active: " + f"mode={self._config.retarget_output_mode}, " + f"neutral_samples={self._config.neutral_calibration_samples}, " + f"clamp={self._config.clamp_to_joint_limits}, " + f"smoothing_alpha={self._config.smoothing_alpha}." + ) + self._announced = True + return self._last_target + + +def wuji_hand_runtime_config(config: Mapping[str, Any]) -> WujiHandRuntimeConfig: + profile = robot_profile_from_config(config) + wuji_config_dir = None + if profile.hand_wuji_config_dir is not None: + wuji_config_dir = _resolve_config_relative_path( + profile.hand_wuji_config_dir, config + ) + if wuji_config_dir is None or not wuji_config_dir.is_dir(): + raise FileNotFoundError( + f"Fixed {profile.status_label} Wuji retarget config directory does not exist: " + f"{wuji_config_dir}" + ) + + return WujiHandRuntimeConfig( + robot_variant=profile.variant, + initial_joint_positions=dict(profile.initial_hand_joint_positions), + left_joint_names=profile.left_hand_joint_names, + right_joint_names=profile.right_hand_joint_names, + axis_signs=dict(profile.axis_signs), + retarget_backend=profile.hand_retarget_backend, + status_label=profile.status_label, + wuji_config_dir=wuji_config_dir, + ) + + +def avp_frame_binding_runtime_config( + config: Mapping[str, Any], +) -> AvpFrameBindingRuntimeConfig: + profile = robot_profile_from_config(config) + robot_config = config.get("robot", {}) + if robot_config is None: + robot_config = {} + if not isinstance(robot_config, Mapping): + raise ValueError("Config field 'robot' must be a mapping") + + binding_config = robot_config.get("avp_frame_binding", {}) + if binding_config is None: + binding_config = {} + if not isinstance(binding_config, Mapping): + raise ValueError("Config field 'robot.avp_frame_binding' must be a mapping") + calibration_window_s = float(binding_config.get("calibration_window_s", 3.0)) + if calibration_window_s < 0.0: + raise ValueError( + "Config field 'robot.avp_frame_binding.calibration_window_s' must be >= 0" + ) + + return AvpFrameBindingRuntimeConfig( + status_label=profile.status_label, + enabled=bool(binding_config.get("enabled", True)), + calibration_window_s=calibration_window_s, + robot_reference_body=str( + binding_config.get("robot_reference_body", profile.reference_body) + ), + robot_reference_offset_pos=_float_tuple( + binding_config.get("robot_reference_offset_pos"), + name="robot.avp_frame_binding.robot_reference_offset_pos", + length=3, + default=profile.reference_offset_pos, + ), + robot_reference_offset_quat_xyzw=_float_tuple( + binding_config.get("robot_reference_offset_quat_xyzw"), + name="robot.avp_frame_binding.robot_reference_offset_quat_xyzw", + length=4, + default=profile.reference_offset_quat_xyzw, + ), + head_forward_axis_isaac=_float_tuple( + binding_config.get("head_forward_axis_isaac"), + name="robot.avp_frame_binding.head_forward_axis_isaac", + length=3, + default=(0.0, 1.0, 0.0), + ), + ) + + +def openxr_to_isaac_position( + position: Sequence[float], scale: float, offset_z: float +) -> np.ndarray: + x, y, z = (float(v) for v in position) + position_isaac = np.array([x, -z, y], dtype=np.float32) * float(scale) + return position_isaac + np.array([0.0, 0.0, offset_z], dtype=np.float32) + + +def quat_xyzw_to_matrix(quat: Sequence[float]) -> np.ndarray: + x, y, z, w = (float(v) for v in quat) + norm = (x * x + y * y + z * z + w * w) ** 0.5 + if norm < 1.0e-8: + return np.eye(3, dtype=np.float64) + x /= norm + y /= norm + z /= norm + w /= norm + return np.array( + [ + [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)], + [2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)], + [2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)], + ], + dtype=np.float64, + ) + + +def matrix_to_quat_xyzw(matrix: np.ndarray) -> np.ndarray: + m = np.asarray(matrix, dtype=np.float64) + trace = float(np.trace(m)) + if trace > 0.0: + s = (trace + 1.0) ** 0.5 * 2.0 + w = 0.25 * s + x = (m[2, 1] - m[1, 2]) / s + y = (m[0, 2] - m[2, 0]) / s + z = (m[1, 0] - m[0, 1]) / s + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = (1.0 + m[0, 0] - m[1, 1] - m[2, 2]) ** 0.5 * 2.0 + w = (m[2, 1] - m[1, 2]) / s + x = 0.25 * s + y = (m[0, 1] + m[1, 0]) / s + z = (m[0, 2] + m[2, 0]) / s + elif m[1, 1] > m[2, 2]: + s = (1.0 + m[1, 1] - m[0, 0] - m[2, 2]) ** 0.5 * 2.0 + w = (m[0, 2] - m[2, 0]) / s + x = (m[0, 1] + m[1, 0]) / s + y = 0.25 * s + z = (m[1, 2] + m[2, 1]) / s + else: + s = (1.0 + m[2, 2] - m[0, 0] - m[1, 1]) ** 0.5 * 2.0 + w = (m[1, 0] - m[0, 1]) / s + x = (m[0, 2] + m[2, 0]) / s + y = (m[1, 2] + m[2, 1]) / s + z = 0.25 * s + quat = np.array([x, y, z, w], dtype=np.float32) + quat_norm = np.linalg.norm(quat) + if quat_norm > 1.0e-8: + quat /= quat_norm + return quat + + +def openxr_to_isaac_orientation(orientation_xyzw: Sequence[float]) -> np.ndarray: + return matrix_to_quat_xyzw(openxr_to_isaac_rotation_matrix(orientation_xyzw)) + + +def openxr_to_isaac_rotation_matrix(orientation_xyzw: Sequence[float]) -> np.ndarray: + frame = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], + dtype=np.float64, + ) + return frame @ quat_xyzw_to_matrix(orientation_xyzw) @ frame.T + + +def normalize_quat_xyzw(quat: Sequence[float]) -> np.ndarray: + out = np.asarray(quat, dtype=np.float32) + norm = np.linalg.norm(out) + if norm < 1.0e-8: + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + return out / norm + + +def openxr_pose_to_isaac_pose( + pose: Mapping[str, Any], + *, + scale: float, + offset_z: float, +) -> tuple[np.ndarray, np.ndarray]: + return ( + openxr_to_isaac_position(pose["position"], scale, offset_z), + openxr_to_isaac_orientation(pose["orientation_xyzw"]), + ) + + +class AvpRobotFrameBinding: + """Bind a startup head-hand AVP frame to the robot reference body.""" + + def __init__( + self, + robot: Any, + arm_ik_config: Any, + config: AvpFrameBindingRuntimeConfig, + *, + pose_scale: float, + pose_z_offset: float, + ) -> None: + _ = arm_ik_config + self._robot = robot + self._config = config + self._pose_scale = float(pose_scale) + self._pose_z_offset = float(pose_z_offset) + self._reference_body_id = _find_single_body_id( + robot, + config.robot_reference_body, + "AVP robot reference body", + ) + self._reference_offset_pos = np.asarray( + config.robot_reference_offset_pos, dtype=np.float64 + ) + self._reference_offset_rot = quat_xyzw_to_matrix( + config.robot_reference_offset_quat_xyzw + ) + self._head_forward_axis = _unit_vector(config.head_forward_axis_isaac) + if self._head_forward_axis is None: + raise ValueError( + "Config field 'robot.avp_frame_binding.head_forward_axis_isaac' " + "must be non-zero" + ) + self._state: _AvpHeadHandBindingState | None = None + self._pending_calibration: _AvpPendingHeadHandCalibration | None = None + self._waiting_announced = False + self._runtime_frame_rot: np.ndarray | None = None + self._latest_head_tilt_delta_rot = np.eye(3, dtype=np.float64) + + def update_calibration(self, sample: Mapping[str, Any]) -> None: + if not self._config.enabled or self._state is not None: + return + + timestamp_s = _sample_timestamp_s(sample) + pending = self._pending_calibration + calibration = self._calibration_inputs(sample) + if pending is None and calibration is None: + self._maybe_log_waiting(sample) + return + + if pending is None: + pending = _AvpPendingHeadHandCalibration( + start_timestamp_s=timestamp_s, + last_timestamp_s=timestamp_s, + ) + self._pending_calibration = pending + print( + f"[{self._config.status_label}] collecting AVP head-hand calibration samples " + f"frame={int(sample.get('frame_count', 0))} " + f"window_s={self._config.calibration_window_s:.2f}.", + flush=True, + ) + + if calibration is not None: + head_pos, head_rot, left_pos, right_pos = calibration + pending.append( + timestamp_s=timestamp_s, + head_pos=head_pos, + head_rot=head_rot, + left_pos=left_pos, + right_pos=right_pos, + ) + + elapsed_s = max(0.0, timestamp_s - pending.start_timestamp_s) + if elapsed_s < self._config.calibration_window_s: + return + + if pending.sample_count <= 0: + self._pending_calibration = None + self._waiting_announced = False + self._maybe_log_waiting( + sample, + reason=( + "calibration window elapsed without valid head/hand samples " + f"elapsed_s={elapsed_s:.3f}" + ), + ) + return + + averaged_head_pos = pending.head_pos_sum / float(pending.sample_count) + averaged_head_rot = _average_rotation_matrix(pending.head_rot_sum) + averaged_left_pos = pending.left_pos_sum / float(pending.sample_count) + averaged_right_pos = pending.right_pos_sum / float(pending.sample_count) + if averaged_head_rot is None: + self._pending_calibration = None + self._waiting_announced = False + self._maybe_log_waiting( + sample, + reason=( + "averaged head rotation was degenerate " + f"samples={pending.sample_count} elapsed_s={elapsed_s:.3f}" + ), + ) + return + + frame_rot = _head_hand_frame_rotation( + head_rot=averaged_head_rot, + left_pos=averaged_left_pos, + right_pos=averaged_right_pos, + head_forward_axis=self._head_forward_axis, + ) + if frame_rot is None: + wrist_delta = averaged_left_pos - averaged_right_pos + self._pending_calibration = None + self._waiting_announced = False + self._maybe_log_waiting( + sample, + reason=( + "degenerate averaged head/hand axes " + f"samples={pending.sample_count} elapsed_s={elapsed_s:.3f} " + f"head={np.round(averaged_head_pos, 4).tolist()} " + f"left={np.round(averaged_left_pos, 4).tolist()} " + f"right={np.round(averaged_right_pos, 4).tolist()} " + f"left_right_norm={float(np.linalg.norm(wrist_delta)):.6f}" + ), + ) + return + + self._state = _AvpHeadHandBindingState( + origin_pos=averaged_head_pos, + frame_rot=frame_rot, + head_rot=averaged_head_rot, + ) + self._runtime_frame_rot = frame_rot.copy() + self._latest_head_yaw_delta_rad = 0.0 + self._latest_head_tilt_delta_rot = np.eye(3, dtype=np.float64) + self._pending_calibration = None + self._announce_calibration( + sample, + averaged_head_pos, + frame_rot, + elapsed_s=elapsed_s, + sample_count=pending.sample_count, + ) + + def reset_calibration(self) -> None: + self._state = None + self._pending_calibration = None + self._waiting_announced = False + self._runtime_frame_rot = None + self._latest_head_tilt_delta_rot = np.eye(3, dtype=np.float64) + print( + f"[{self._config.status_label}] AVP head-hand frame calibration cleared; " + "waiting for fresh calibration.", + flush=True, + ) + + def update_runtime_state(self, sample: Mapping[str, Any]) -> None: + state = self._state + if not self._config.enabled or state is None: + self._runtime_frame_rot = None + self._latest_head_tilt_delta_rot = np.eye(3, dtype=np.float64) + return + + if self._runtime_frame_rot is None: + self._runtime_frame_rot = state.frame_rot + + head_pose = sample.get("head_pose") + if head_pose is None: + return + + current_head_rot = openxr_to_isaac_rotation_matrix( + head_pose["orientation_xyzw"] + ) + head_tilt_delta_rot = _head_tilt_delta_rotation( + calibration_head_rot=state.head_rot, + current_head_rot=current_head_rot, + head_forward_axis=self._head_forward_axis, + ) + if head_tilt_delta_rot is not None: + self._latest_head_tilt_delta_rot = head_tilt_delta_rot + + self._runtime_frame_rot = state.frame_rot + + @property + def calibrated(self) -> bool: + return self._state is not None + + def latest_head_tilt_delta_rot(self) -> np.ndarray: + return np.array(self._latest_head_tilt_delta_rot, copy=True) + + def transform_pose( + self, + side: str, + pose: Mapping[str, Any], + ) -> tuple[np.ndarray, np.ndarray] | None: + raw_pos, raw_quat = openxr_pose_to_isaac_pose( + pose, + scale=self._pose_scale, + offset_z=self._pose_z_offset, + ) + if not self._config.enabled: + return raw_pos, raw_quat + + state = self._state + if state is None: + return None + + frame_rot = self._runtime_frame_rot + if frame_rot is None: + frame_rot = state.frame_rot + raw_rot = quat_xyzw_to_matrix(raw_quat) + local_pos = frame_rot.T @ ( + np.asarray(raw_pos, dtype=np.float64) - state.origin_pos + ) + local_rot = frame_rot.T @ raw_rot + reference_pos, reference_rot = self._current_reference_world_pose() + target_pos = reference_pos + reference_rot @ local_pos + target_rot = reference_rot @ local_rot + return target_pos.astype(np.float32), matrix_to_quat_xyzw(target_rot) + + def _current_reference_world_pose(self) -> tuple[np.ndarray, np.ndarray]: + body_pos, body_rot = _body_pose_np(self._robot, self._reference_body_id) + return ( + body_pos + body_rot @ self._reference_offset_pos, + body_rot @ self._reference_offset_rot, + ) + + def _calibration_inputs( + self, sample: Mapping[str, Any] + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] | None: + head_pose = sample.get("head_pose") + ee_poses = sample.get("ee_poses", {}) + left_pose = ee_poses.get("left") if isinstance(ee_poses, Mapping) else None + right_pose = ee_poses.get("right") if isinstance(ee_poses, Mapping) else None + if head_pose is None or left_pose is None or right_pose is None: + return None + + head_pos, head_quat = openxr_pose_to_isaac_pose( + head_pose, + scale=self._pose_scale, + offset_z=self._pose_z_offset, + ) + left_pos, _left_quat = openxr_pose_to_isaac_pose( + left_pose, + scale=self._pose_scale, + offset_z=self._pose_z_offset, + ) + right_pos, _right_quat = openxr_pose_to_isaac_pose( + right_pose, + scale=self._pose_scale, + offset_z=self._pose_z_offset, + ) + return ( + np.asarray(head_pos, dtype=np.float64), + quat_xyzw_to_matrix(head_quat), + np.asarray(left_pos, dtype=np.float64), + np.asarray(right_pos, dtype=np.float64), + ) + + def _maybe_log_waiting( + self, sample: Mapping[str, Any], *, reason: str | None = None + ) -> None: + if self._waiting_announced: + return + frame_count = int(sample.get("frame_count", 0)) + head_ready = sample.get("head_pose") is not None + ee_poses = sample.get("ee_poses", {}) + left_ready = isinstance(ee_poses, Mapping) and ee_poses.get("left") is not None + right_ready = ( + isinstance(ee_poses, Mapping) and ee_poses.get("right") is not None + ) + detail = f" reason={reason}" if reason else "" + print( + f"[{self._config.status_label}] waiting for AVP head-hand frame calibration " + f"frame={frame_count} head={head_ready} left={left_ready} " + f"right={right_ready}{detail}", + flush=True, + ) + self._waiting_announced = True + + def _announce_calibration( + self, + sample: Mapping[str, Any], + origin_pos: np.ndarray, + frame_rot: np.ndarray, + *, + elapsed_s: float, + sample_count: int, + ) -> None: + reference_body_pos, _reference_body_rot = _body_pose_np( + self._robot, self._reference_body_id + ) + reference_pos, _reference_rot = self._current_reference_world_pose() + print( + f"[{self._config.status_label}] AVP head-hand frame calibrated " + f"frame={int(sample.get('frame_count', 0))} " + f"elapsed_s={elapsed_s:.3f} samples={sample_count} " + f"origin_isaac={origin_pos.tolist()} " + f"x={frame_rot[:, 0].tolist()} y={frame_rot[:, 1].tolist()} " + f"z={frame_rot[:, 2].tolist()} " + f"robot_reference_body={self._config.robot_reference_body!r} " + f"robot_reference_body_pos={reference_body_pos.astype(np.float32).tolist()} " + f"robot_reference_pos={reference_pos.astype(np.float32).tolist()}", + flush=True, + ) + + +class WujiHandTargetBackend: + """Official Wuji retargeting from MANUS/OpenXR skeletons.""" + + def __init__(self, config: WujiHandRuntimeConfig) -> None: + self._config = config + self._retargeter: Any | None = None + self._announced = False + self._waiting_announced = False + + def _ensure_retargeter(self) -> None: + if self._retargeter is not None: + return + if self._config.retarget_backend != "official_wuji": + raise ValueError( + f"Unsupported hand retarget backend: {self._config.retarget_backend!r}" + ) + + from .retargeting import WujiOfficialManusHandRetargeter + + self._retargeter = WujiOfficialManusHandRetargeter( + config_dir=self._config.wuji_config_dir + ) + self._retargeter._ensure_retargeters() + if not self._announced: + print( + f"[{self._config.status_label}] using hand retarget backend: " + f"{self._config.retarget_backend} " + f"(left_joints={len(self._retargeter.left_joint_names or ())}, " + f"right_joints={len(self._retargeter.right_joint_names or ())}, " + f"config={self._config.wuji_config_dir})." + ) + self._announced = True + + @staticmethod + def _to_numpy(values: Any) -> np.ndarray: + if hasattr(values, "detach") and hasattr(values, "cpu"): + values = values.detach().cpu().numpy() + return np.asarray(values, dtype=np.float32).reshape(-1) + + def targets( + self, + sample: Mapping[str, Any], + ) -> tuple[np.ndarray | None, np.ndarray | None]: + left_skeleton = _wuji_skeleton21_from_openxr(sample, "left") + right_skeleton = _wuji_skeleton21_from_openxr(sample, "right") + + if left_skeleton is None and right_skeleton is None: + if not self._waiting_announced: + print( + f"[{self._config.status_label}] waiting for MANUS hand skeletons; " + "arm EE teleop remains independent.", + flush=True, + ) + self._waiting_announced = True + return None, None + + self._ensure_retargeter() + self._waiting_announced = False + frame_index = int(sample.get("frame_count", 0)) + left_targets = None + if left_skeleton is not None: + left_positions, left_joint_names = self._retargeter.retarget_manus_skeleton( + "left", + left_skeleton, + frame_index=frame_index, + device=None, + ) + left_targets = _ordered_named_targets( + values=self._to_numpy(left_positions), + source_names=left_joint_names, + target_names=self._config.left_joint_names, + side="left", + label=self._config.retarget_backend, + axis_signs=self._config.axis_signs, + status_label=self._config.status_label, + ) + + right_targets = None + if right_skeleton is not None: + right_positions, right_joint_names = ( + self._retargeter.retarget_manus_skeleton( + "right", + right_skeleton, + frame_index=frame_index, + device=None, + ) + ) + right_targets = _ordered_named_targets( + values=self._to_numpy(right_positions), + source_names=right_joint_names, + target_names=self._config.right_joint_names, + side="right", + label=self._config.retarget_backend, + axis_signs=self._config.axis_signs, + status_label=self._config.status_label, + ) + return left_targets, right_targets + + +def _resolve_config_relative_path(value: Any, config: Mapping[str, Any]) -> Path | None: + if value is None: + return None + raw = str(value).strip() + if not raw: + return None + path = Path(raw).expanduser() + if path.is_absolute(): + return path.resolve() + default_config_dir = example_root() / "config" + base_dir = Path(str(config.get("_config_dir", default_config_dir))).expanduser() + return (base_dir / path).resolve() + + +def _float_tuple( + values: Any, + *, + name: str, + length: int, + default: Sequence[float], +) -> tuple[float, ...]: + raw = default if values is None else values + if not isinstance(raw, Sequence) or isinstance(raw, (str, bytes)): + raise ValueError(f"Config field {name!r} must be a {length}-element sequence") + if len(raw) != length: + raise ValueError(f"Config field {name!r} must contain exactly {length} values") + return tuple(float(value) for value in raw) + + +def _unit_vector(values: Sequence[float] | np.ndarray) -> np.ndarray | None: + vector = np.asarray(values, dtype=np.float64).reshape(-1) + if vector.size != 3: + return None + norm = np.linalg.norm(vector) + if norm < 1.0e-8: + return None + return vector / norm + + +def _sample_timestamp_s(sample: Mapping[str, Any]) -> float: + timestamp = sample.get("timestamp_s") + if timestamp is None: + return time.monotonic() + return float(timestamp) + + +def _average_rotation_matrix(matrix_sum: np.ndarray) -> np.ndarray | None: + matrix = np.asarray(matrix_sum, dtype=np.float64) + if matrix.shape != (3, 3) or np.linalg.norm(matrix) < 1.0e-8: + return None + + u, _s, vt = np.linalg.svd(matrix) + + rotation = u @ vt + if np.linalg.det(rotation) < 0.0: + u[:, -1] *= -1.0 + rotation = u @ vt + return rotation + + +def _head_hand_frame_rotation( + *, + head_rot: np.ndarray, + left_pos: np.ndarray, + right_pos: np.ndarray, + head_forward_axis: np.ndarray, +) -> np.ndarray | None: + x_axis = _unit_vector(head_rot @ head_forward_axis) + if x_axis is None: + return None + + y_axis_raw = _unit_vector(left_pos - right_pos) + if y_axis_raw is None: + return None + else: + z_axis = _unit_vector(np.cross(x_axis, y_axis_raw)) + if z_axis is None: + return None + else: + y_axis = _unit_vector(np.cross(z_axis, x_axis)) + if y_axis is None: + return None + + z_axis = _unit_vector(np.cross(x_axis, y_axis)) + if z_axis is None: + return None + frame_rot = np.column_stack((x_axis, y_axis, z_axis)) + if np.linalg.det(frame_rot) < 0.0: + frame_rot[:, 2] *= -1.0 + return frame_rot + + +def _head_tilt_delta_rotation( + *, + calibration_head_rot: np.ndarray, + current_head_rot: np.ndarray, + head_forward_axis: np.ndarray, +) -> np.ndarray | None: + calibration_tilt = _yaw_removed_rotation( + rotation=calibration_head_rot, + forward_axis=head_forward_axis, + ) + current_tilt = _yaw_removed_rotation( + rotation=current_head_rot, + forward_axis=head_forward_axis, + ) + if calibration_tilt is None or current_tilt is None: + return None + tilt_delta = current_tilt @ calibration_tilt.T + if np.linalg.det(tilt_delta) < 0.0: + return None + return tilt_delta + + +def _yaw_removed_rotation( + *, + rotation: np.ndarray, + forward_axis: np.ndarray, +) -> np.ndarray | None: + yaw_rot = _world_z_yaw_rotation_from_forward( + rotation=rotation, + forward_axis=forward_axis, + ) + if yaw_rot is None: + return None + return yaw_rot.T @ rotation + + +def _world_z_yaw_rotation_from_forward( + *, + rotation: np.ndarray, + forward_axis: np.ndarray, +) -> np.ndarray | None: + forward_local = _unit_vector(forward_axis) + if forward_local is None: + return None + + reference_forward = _unit_vector( + np.asarray([forward_local[0], forward_local[1], 0.0], dtype=np.float64) + ) + current_forward = _unit_vector(rotation @ forward_local) + if reference_forward is None or current_forward is None: + return None + projected_forward = _unit_vector( + np.asarray([current_forward[0], current_forward[1], 0.0], dtype=np.float64) + ) + if projected_forward is None: + return None + + yaw_rad = float( + np.arctan2( + reference_forward[0] * projected_forward[1] + - reference_forward[1] * projected_forward[0], + reference_forward[0] * projected_forward[0] + + reference_forward[1] * projected_forward[1], + ) + ) + cos_yaw = float(np.cos(yaw_rad)) + sin_yaw = float(np.sin(yaw_rad)) + return np.array( + [ + [cos_yaw, -sin_yaw, 0.0], + [sin_yaw, cos_yaw, 0.0], + [0.0, 0.0, 1.0], + ], + dtype=np.float64, + ) + + +def _find_single_body_id(robot: Any, body_name: str, label: str) -> int: + body_ids, body_names = robot.find_bodies(body_name) + if len(body_ids) == 1: + return int(body_ids[0]) + + available = list(getattr(robot.data, "body_names", []) or []) + preview = available[:40] + raise RuntimeError( + f"Expected one {label} match for {body_name!r}; got {len(body_ids)}: " + f"{body_names}. Available body names first entries: {preview}" + ) + + +def as_torch(value: Any) -> Any: + """Return a torch tensor from Isaac Lab buffers or Warp arrays.""" + import torch + + if isinstance(value, torch.Tensor): + return value + import warp as wp + + return wp.to_torch(value) + + +def _first_numpy(value: Any) -> np.ndarray: + if hasattr(value, "detach"): + return value.detach().cpu().numpy() + + return as_torch(value).detach().cpu().numpy() + + +def _body_pose_np(robot: Any, body_id: int) -> tuple[np.ndarray, np.ndarray]: + body_pos = np.asarray( + _first_numpy(robot.data.body_pos_w)[0, body_id], dtype=np.float64 + ) + body_quat = np.asarray( + _first_numpy(robot.data.body_quat_w)[0, body_id], dtype=np.float64 + ) + return body_pos, quat_xyzw_to_matrix(body_quat) + + +def _ordered_named_targets( + *, + values: Any, + source_names: Sequence[str] | None, + target_names: Sequence[str], + side: str, + label: str, + axis_signs: Mapping[str, float] | None = None, + status_label: str = "teleop", +) -> np.ndarray | None: + if values is None: + return None + + values_array = np.asarray(values, dtype=np.float32).reshape(-1) + if source_names is None: + raise RuntimeError( + f"[{status_label}] retarget backend={label} side={side} did not " + "provide joint names" + ) + + values_by_name = { + str(name): float(values_array[i]) + for i, name in enumerate(source_names) + if i < values_array.size + } + resolved_values: dict[str, float] = {} + used_source_names: set[str] = set() + side_prefix = f"{side}_" + for name in target_names: + source_name = name + if source_name not in values_by_name and name.startswith(side_prefix): + source_name = name.removeprefix(side_prefix) + if source_name in values_by_name: + resolved_values[name] = values_by_name[source_name] + used_source_names.add(source_name) + + missing = [name for name in target_names if name not in resolved_values] + if missing: + extra = [name for name in values_by_name if name not in used_source_names] + raise RuntimeError( + f"[{status_label}] retarget joint-name mismatch " + f"backend={label} side={side} missing={missing[:8]} " + f"total_missing={len(missing)} extra={extra[:8]} total_extra={len(extra)}" + ) + + ordered = np.zeros(len(target_names), dtype=np.float32) + sign_map = axis_signs or {} + for i, name in enumerate(target_names): + ordered[i] = resolved_values[name] * sign_map.get(name, 1.0) + return ordered + + +def _wuji_skeleton21_from_openxr( + sample: Mapping[str, Any], side: str +) -> np.ndarray | None: + skeletons = sample.get("hand_skeletons", {}) + skeleton = skeletons.get(side) if isinstance(skeletons, Mapping) else None + if not isinstance(skeleton, Mapping): + return None + + joint_names = skeleton.get("joint_names") + positions = skeleton.get("positions") + if not isinstance(joint_names, Sequence) or positions is None: + return None + + positions_array = np.asarray(positions, dtype=np.float32) + if positions_array.ndim != 2 or positions_array.shape[1] != 3: + return None + + valid_values = skeleton.get("valid") + valid_array = ( + np.ones(positions_array.shape[0], dtype=bool) + if valid_values is None + else np.asarray(valid_values, dtype=bool).reshape(-1) + ) + name_to_index = {str(name): index for index, name in enumerate(joint_names)} + + out = np.zeros((len(WUJI_SKELETON21_OPENXR_NAMES), 3), dtype=np.float32) + missing: list[str] = [] + for out_index, name in enumerate(WUJI_SKELETON21_OPENXR_NAMES): + source_index = name_to_index.get(name) + if ( + source_index is None + or source_index >= positions_array.shape[0] + or source_index >= valid_array.size + or not bool(valid_array[source_index]) + ): + missing.append(name) + continue + out[out_index] = positions_array[source_index] + + if missing: + return None + + if not np.all(np.isfinite(out)): + return None + return out diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/scene.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/scene.py new file mode 100644 index 000000000..63054a43a --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/scene.py @@ -0,0 +1,291 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Isaac Lab scene helpers for the G1-Wuji teleop app.""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +from pathlib import Path +from typing import Any + + +@dataclass(frozen=True) +class G1WujiSceneConfig: + robot_prim: str + robot_usd: Path + initial_world_position: tuple[float, float, float] + initial_world_orientation_xyzw: tuple[float, float, float, float] + initial_hand_joint_positions: Mapping[str, float] + light_intensity: float + + +def _xyzw_to_wxyz( + quat_xyzw: tuple[float, float, float, float], +) -> tuple[float, float, float, float]: + x, y, z, w = quat_xyzw + return (w, x, y, z) + + +def make_g1_wuji_robot_cfg(config: G1WujiSceneConfig): + import isaaclab.sim as sim_utils + from isaaclab.actuators import ImplicitActuatorCfg + from isaaclab.assets import ArticulationCfg + + joint_pos = { + "right_wrist_yaw_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + ".*_wrist_pitch_joint": 0.0, + ".*_wrist_roll_joint": 0.0, + ".*_shoulder_pitch_joint": 0.0, + ".*_shoulder_roll_joint": 0.0, + ".*_shoulder_yaw_joint": 0.0, + "right_elbow_joint": 0.0, + "left_elbow_joint": 0.0, + } + joint_pos.update( + { + name: float(value) + for name, value in config.initial_hand_joint_positions.items() + } + ) + + return ArticulationCfg( + prim_path=config.robot_prim, + spawn=sim_utils.UsdFileCfg( + usd_path=str(config.robot_usd), + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=False, + max_linear_velocity=100.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=0.002, + rest_offset=0.001, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=16, + solver_velocity_iteration_count=4, + sleep_threshold=0.005, + stabilization_threshold=0.001, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=config.initial_world_position, + # Config stores xyzw; IsaacLab InitialStateCfg.rot expects wxyz. + rot=_xyzw_to_wxyz(config.initial_world_orientation_xyzw), + joint_pos=joint_pos, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.98, + actuators=_wuji_actuators(ImplicitActuatorCfg), + ) + + +def _wuji_actuators(ImplicitActuatorCfg): + common = { + "base": ImplicitActuatorCfg( + joint_names_expr=["base_.*"], + effort_limit_sim=100000.0, + velocity_limit_sim=1000.0, + stiffness=1e6, + damping=1e4, + ), + "right_arm": ImplicitActuatorCfg( + joint_names_expr=["right_shoulder_.*", "right_elbow_joint"], + effort_limit_sim=5.0, + velocity_limit_sim=3.0, + stiffness=1000.0, + damping=80.0, + ), + "left_arm": ImplicitActuatorCfg( + joint_names_expr=["left_shoulder_.*", "left_elbow_joint"], + effort_limit_sim=5.0, + velocity_limit_sim=3.0, + stiffness=1000.0, + damping=80.0, + ), + "wrist": ImplicitActuatorCfg( + joint_names_expr=[ + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + ], + effort_limit_sim=90000.0, + velocity_limit_sim=1200.0, + stiffness=1000.0, + damping=80.0, + ), + } + common["fingers"] = ImplicitActuatorCfg( + joint_names_expr=["left_finger.*", "right_finger.*"], + effort_limit_sim=300.0, + velocity_limit_sim=8.0, + stiffness=3000.0, + damping=120.0, + ) + return common + + +def _make_shape_cfg_without_visual_material(shape_cfg_type: Any, **kwargs: Any) -> Any: + kwargs.pop("visual_material", None) + return shape_cfg_type(**kwargs) + + +def _apply_shape_display_color( + prim_path: str, display_color: tuple[float, float, float] +) -> None: + import omni.usd + from pxr import Gf, UsdGeom + + stage = omni.usd.get_context().get_stage() + if stage is None: + return + mesh_prim = stage.GetPrimAtPath(f"{prim_path}/geometry/mesh") + if mesh_prim is None or not mesh_prim.IsValid(): + return + UsdGeom.Gprim(mesh_prim).CreateDisplayColorAttr( + [Gf.Vec3f(*(float(value) for value in display_color))] + ) + + +def _spawn_display_color_shape( + prim_path: str, + cfg: Any, + *, + display_color: tuple[float, float, float], + **kwargs: Any, +) -> None: + cfg.func(prim_path, cfg, **kwargs) + _apply_shape_display_color(prim_path, display_color) + + +def design_g1_wuji_scene(config: G1WujiSceneConfig) -> Any: + import isaaclab.sim as sim_utils + from isaaclab.assets import Articulation + + graspable_surface_material = sim_utils.RigidBodyMaterialCfg( + static_friction=1.3, + dynamic_friction=1.1, + restitution=0.0, + friction_combine_mode="max", + restitution_combine_mode="min", + ) + + floor_color = (0.25, 0.25, 0.25) + floor_cfg = _make_shape_cfg_without_visual_material( + sim_utils.CuboidCfg, + size=(6.0, 6.0, 0.04), + collision_props=sim_utils.CollisionPropertiesCfg(), + ) + _spawn_display_color_shape( + "/World/Floor", + floor_cfg, + translation=(0.0, 0.0, -0.02), + display_color=floor_color, + ) + + table_color = (0.54, 0.42, 0.30) + table_leg_color = (0.22, 0.22, 0.24) + table_top_size = (1.20, 0.75, 0.05) + table_top_center = (0.0, 0.5, 0.74) + table_leg_size = (0.06, 0.06, 0.72) + table_leg_x_offset = table_top_size[0] * 0.5 - 0.09 + table_leg_y_offset = table_top_size[1] * 0.5 - 0.09 + table_leg_z = table_leg_size[2] * 0.5 + + table_top_cfg = _make_shape_cfg_without_visual_material( + sim_utils.CuboidCfg, + size=table_top_size, + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=graspable_surface_material, + ) + _spawn_display_color_shape( + "/World/Props/TableTop", + table_top_cfg, + translation=table_top_center, + display_color=table_color, + ) + + table_leg_cfg = _make_shape_cfg_without_visual_material( + sim_utils.CuboidCfg, + size=table_leg_size, + collision_props=sim_utils.CollisionPropertiesCfg(), + physics_material=graspable_surface_material, + ) + for leg_index, (x_sign, y_sign) in enumerate( + ((1.0, 1.0), (1.0, -1.0), (-1.0, 1.0), (-1.0, -1.0)), start=1 + ): + _spawn_display_color_shape( + f"/World/Props/TableLeg{leg_index}", + table_leg_cfg, + translation=( + table_top_center[0] + x_sign * table_leg_x_offset, + table_top_center[1] + y_sign * table_leg_y_offset, + table_leg_z, + ), + display_color=table_leg_color, + ) + + cylinder_radius = 0.032 + cylinder_height = 0.18 + cylinder_surface_z = table_top_center[2] + table_top_size[2] * 0.5 + cylinder_color = (0.84, 0.18, 0.10) + cylinder_cfg = _make_shape_cfg_without_visual_material( + sim_utils.CylinderCfg, + radius=cylinder_radius, + height=cylinder_height, + axis="Z", + collision_props=sim_utils.CollisionPropertiesCfg( + contact_offset=0.003, + rest_offset=0.0, + ), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + disable_gravity=False, + linear_damping=0.08, + angular_damping=0.06, + max_linear_velocity=8.0, + max_angular_velocity=50.0, + max_depenetration_velocity=3.0, + solver_position_iteration_count=16, + solver_velocity_iteration_count=4, + sleep_threshold=0.002, + stabilization_threshold=0.001, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=0.18), + physics_material=sim_utils.RigidBodyMaterialCfg( + static_friction=1.4, + dynamic_friction=1.2, + restitution=0.0, + friction_combine_mode="max", + restitution_combine_mode="min", + ), + ) + _spawn_display_color_shape( + "/World/Props/GraspCylinder", + cylinder_cfg, + translation=( + table_top_center[0], + table_top_center[1] - 0.12, + cylinder_surface_z + cylinder_height * 0.5, + ), + display_color=cylinder_color, + ) + + light_cfg = sim_utils.DomeLightCfg( + intensity=config.light_intensity, + color=(0.75, 0.75, 0.75), + ) + light_cfg.func("/World/Light", light_cfg) + + return Articulation(cfg=make_g1_wuji_robot_cfg(config)) diff --git a/examples/g1_wuji_teleop/python/g1_wuji_teleop/transforms.py b/examples/g1_wuji_teleop/python/g1_wuji_teleop/transforms.py new file mode 100644 index 000000000..2560f2f6b --- /dev/null +++ b/examples/g1_wuji_teleop/python/g1_wuji_teleop/transforms.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Small transform helpers used by the G1-Wuji teleop example.""" + +from __future__ import annotations + +from collections.abc import Sequence + +import numpy as np + + +def normalize_quat_xyzw(quat: Sequence[float]) -> np.ndarray: + quat_array = np.asarray(quat, dtype=np.float64).reshape(4) + norm = float(np.linalg.norm(quat_array)) + if norm < 1.0e-12: + raise ValueError("Quaternion must be non-zero.") + return quat_array / norm + + +def quat_xyzw_to_matrix(quat: Sequence[float]) -> np.ndarray: + x, y, z, w = normalize_quat_xyzw(quat) + xx, yy, zz = x * x, y * y, z * z + xy, xz, yz = x * y, x * z, y * z + wx, wy, wz = w * x, w * y, w * z + return np.array( + [ + [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)], + [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)], + [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)], + ], + dtype=np.float64, + ) + + +def quat_xyzw_multiply( + lhs: Sequence[float], + rhs: Sequence[float], +) -> np.ndarray: + lx, ly, lz, lw = normalize_quat_xyzw(lhs) + rx, ry, rz, rw = normalize_quat_xyzw(rhs) + return normalize_quat_xyzw( + [ + lw * rx + lx * rw + ly * rz - lz * ry, + lw * ry - lx * rz + ly * rw + lz * rx, + lw * rz + lx * ry - ly * rx + lz * rw, + lw * rw - lx * rx - ly * ry - lz * rz, + ] + ) + + +def transform_controller_pose_to_palm( + *, + controller_pos: Sequence[float], + controller_quat_xyzw: Sequence[float], + offset_pos: Sequence[float], + offset_quat_xyzw: Sequence[float], +) -> tuple[np.ndarray, np.ndarray]: + controller_pos_array = np.asarray(controller_pos, dtype=np.float64).reshape(3) + offset_pos_array = np.asarray(offset_pos, dtype=np.float64).reshape(3) + controller_rot = quat_xyzw_to_matrix(controller_quat_xyzw) + palm_pos = controller_pos_array + controller_rot @ offset_pos_array + palm_quat = quat_xyzw_multiply(controller_quat_xyzw, offset_quat_xyzw) + return palm_pos, palm_quat diff --git a/examples/g1_wuji_teleop/python/pyproject.toml b/examples/g1_wuji_teleop/python/pyproject.toml new file mode 100644 index 000000000..f2bb525ab --- /dev/null +++ b/examples/g1_wuji_teleop/python/pyproject.toml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "g1-wuji-teleop-example" +version = "0.0.0" +description = "G1-Wuji AVP/VR + MANUS teleop example" +requires-python = ">=3.10,<3.14" +dependencies = [ + "isaacteleop[cloudxr,retargeters]", + "numpy>=1.23.0", + "PyYAML>=6.0", +]