diff --git a/REUSE.toml b/REUSE.toml index c8040799d..49a87ba37 100644 --- a/REUSE.toml +++ b/REUSE.toml @@ -35,3 +35,9 @@ path = ".cursor/**" precedence = "override" SPDX-FileCopyrightText = "Copyright (c) NVIDIA CORPORATION & AFFILIATES. All rights reserved." SPDX-License-Identifier = "Apache-2.0" + +[[annotations]] +path = "examples/cloudxr_mujoco_teleop/**" +precedence = "override" +SPDX-FileCopyrightText = "Copyright (c) 2026 HTC Corporation" +SPDX-License-Identifier = "Apache-2.0" diff --git a/examples/cloudxr_mujoco_teleop/README.md b/examples/cloudxr_mujoco_teleop/README.md new file mode 100644 index 000000000..f384b2362 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/README.md @@ -0,0 +1,146 @@ + + +# CloudXR MuJoCo Teleop Visualization Examples + +Live MuJoCo viewers for the OpenXR pose streams arriving from IsaacTeleop — +typically while a Vive headset streams through NVIDIA CloudXR. The samples +ship with a generic HMD and Vive Focus 3 (controllers) meshes and textures +in `vive_assets/`, but the pipeline they consume is runtime-agnostic and +works with any CloudXR / OpenXR-compatible HMD. + +| Example | What it shows | +|---------|---------------| +| `visualize_poses_mujoco_example.py` | HMD + left/right controller as textured mocap bodies. | +| `visualize_hands_mujoco_example.py` | Both hands as 26-joint skeletons (spheres + capsule bones), with the HMD as optional context. | + +## Prerequisites + +Assumes IsaacTeleop is already built and installed in this tree. + +1. **[`uv`](https://docs.astral.sh/uv/)** (one-time install): + ```bash + curl -LsSf https://astral.sh/uv/install.sh | sh + ``` + On first launch, `uv run` fetches this example's Python dependencies + (`mujoco`, `numpy`, `scipy` — see `pyproject.toml`) into an isolated + environment. + +2. **CloudXR environment loaded**, so OpenXR resolves to the CloudXR + runtime. Source the setup script that ships with your CloudXR install: + ```bash + source ~/.cloudxr/run/cloudxr.env + ``` + The CloudXR runtime should be running with the headset connected before + launching the examples below. + +Close the viewer window or press `Ctrl+C` to quit either example. No time +limit, no recording. + +## Examples + +### `visualize_poses_mujoco_example.py` + +HMD + left + right controller, each rendered as a textured mesh mocap body. + +```bash +uv run visualize_poses_mujoco_example.py +``` + +| Flag | Default | Description | +|------|---------|-------------| +| `--assets-dir DIR` | `./vive_assets` | Where to find the device meshes / textures. | +| `--pose {grip,aim}` | `grip` | Which controller pose drives the mesh — natural "hold" pose or the aim ray. | +| `--debug` | off | Print `mocap_pos` for all three devices once per second. | + +### `visualize_hands_mujoco_example.py` + +Both hands as 26-joint skeletons drawn directly into `viewer.user_scn` each +frame (left = cyan, right = warm orange) — avoids paying for 52 mocap bodies +in the model. The HMD is shown as context using the same mesh + texture as +the poses example. + +```bash +uv run visualize_hands_mujoco_example.py +``` + +| Flag | Default | Description | +|------|---------|-------------| +| `--no-head` | off | Skip the head mesh — only draw the hand skeletons. | +| `--debug` | off | Print per-hand valid-joint counts once per second. | + +Hand tracking obviously requires hand poses to actually arrive in the +stream. If only controllers are tracked, both hands stay empty and only the +HMD will be visible. + +## Viewer controls (both examples) + +- **Mouse wheel** — zoom. The free camera's `lookat` is rewritten to the + headset position every frame, so zoom always converges on the head. +- **Left-click drag** — orbit around the headset. +- **Right-click drag** — pan (the next frame's auto-recentre snaps back). +- `[` / `]` — cycle cameras: `free` → fixed cameras (the poses example + also ships `play_area` and a `follow_head` camera that rides behind/above + the HMD via `mode="trackcom"`). +- Side panels are hidden by default; press `Tab` to toggle them back if you + need MuJoCo's built-in inspector / rendering options. + +## Directory layout + +``` +cloudxr_mujoco_teleop/ +├── README.md +├── pyproject.toml # uv dependency declaration +├── visualize_poses_mujoco_example.py +├── visualize_hands_mujoco_example.py +└── vive_assets/ + ├── generic_hmd.obj # HMD mesh (~500 KB) + ├── generic_hmd_color.png # HMD texture + ├── vive_focus3_controller_left.obj + ├── vive_focus3_controller_left_color.png + ├── vive_focus3_controller_right.obj + └── vive_focus3_controller_right_color.png +``` + +The bundled meshes are a generic HMD and the Vive Focus 3 controllers. +Each OBJ carries UV coordinates; the matching `*_color.png` is bound via +MJCF ``, so missing `.mtl` files are not a +problem. + +Both examples share `vive_assets/` — the hands example reuses the HMD mesh ++ texture for its context body. + +If any asset is missing each example degrades gracefully: a mesh-less +device falls back to a coloured primitive box; a textureless mesh falls +back to a solid colour. + +## Notes for adaptation + +These are also documented at the top of each script. + +- **Coordinate frames.** OpenXR is right-handed, Y-up, -Z forward; MuJoCo's + default is right-handed, Z-up. Every incoming pose is rotated by +90° + about X, and the quaternion (for the pose mocap bodies) is reordered + from `xyzw` (OpenXR / scipy) to `wxyz` (MuJoCo). +- **Mesh-frame fix.** OBJ files are not modelled in the OpenXR pose frame. + Both scripts apply a per-device static rotation (the geom's local `quat`) + to align each mesh with its tracked pose. The shipped values + (`(0, 0, 180)` for the HMD, `(0, 90, 0)` for the controllers) were tuned + visually for the bundled generic HMD and Vive Focus 3 controllers — + **re-tune them for your own meshes.** +- **Reference space.** Both samples assume a stage / floor-relative + reference space, so the headset reports its real height (`Y ≈ 1.5 m`). + With a local / head-relative reference space the headset always reads as + the origin and everything will collapse to floor level. + +## Trademarks + +VIVE, VIVE Focus 3, and HTC are trademarks of HTC Corporation, registered +in the U.S. and other countries. The 3D models and textures in this +directory depict HTC hardware and are provided by HTC Corporation under the +Apache-2.0 license for use with the IsaacTeleop examples. +The Apache-2.0 license does NOT grant any right to use HTC's trademarks, +trade names, or product names. Use of these names in this repository is for +identification only and does not imply endorsement by HTC. diff --git a/examples/cloudxr_mujoco_teleop/pyproject.toml b/examples/cloudxr_mujoco_teleop/pyproject.toml new file mode 100644 index 000000000..d0604d459 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/pyproject.toml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "cloudxr-mujoco-teleop-example" +version = "0.1.0" +description = "Visualize OpenXR / CloudXR headset + controller poses in MuJoCo." +requires-python = ">=3.10,<3.14" +dependencies = [ + "isaacteleop", + "mujoco>=3.0", + "numpy>=1.22.2", + "scipy>=1.10", +] diff --git a/examples/cloudxr_mujoco_teleop/visualize_hands_mujoco_example.py b/examples/cloudxr_mujoco_teleop/visualize_hands_mujoco_example.py new file mode 100644 index 000000000..1ffb9b949 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/visualize_hands_mujoco_example.py @@ -0,0 +1,350 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Visualize Hand Joints with MuJoCo +================================= + +Subscribes to IsaacTeleop's ``HandTracker`` (+ ``HeadTracker`` for context) and +renders both hands live in the MuJoCo passive viewer as 26-joint skeletons: +each joint is a small sphere, and OpenXR's standard bone topology is drawn as +capsule "bones" between parents and children. + +Coordinate frame +---------------- +IsaacTeleop delivers poses in the OpenXR convention (right-handed, Y-up, +-Z forward); MuJoCo's default is right-handed, Z-up. Every joint position is +rotated by +90 deg about X before being handed to MuJoCo. The head mesh +follows the same rule as ``visualize_poses_mujoco_example.py``. + +Usage +----- + python visualize_hands_mujoco_example.py + python visualize_hands_mujoco_example.py --no-head --debug +""" + +import argparse +import os +import signal +import sys +import time + +import mujoco +import mujoco.viewer +import numpy as np +from scipy.spatial.transform import Rotation as R + +import isaacteleop.deviceio as deviceio +import isaacteleop.oxr as oxr + + +# Route MuJoCo warnings to stderr instead of writing MUJOCO_LOG.TXT in CWD. +def _mujoco_warning(msg) -> None: + text = msg.decode("utf-8", errors="replace") if isinstance(msg, bytes) else msg + print(f"[MuJoCo] {text}", file=sys.stderr) + + +mujoco.set_mju_user_warning(_mujoco_warning) + + +# OpenXR -> MuJoCo world rotation (Rx +90 deg): (x, y, z) -> (x, -z, y). +R_XR_TO_MJ = R.from_euler("x", 90, degrees=True) + +# Head mesh + texture (shared with the poses example) so the hands have a +# body anchor. If either file is missing the head degrades gracefully: +# mesh-only -> rgba fallback; mesh missing -> box fallback. +ASSETS_DIR = os.path.join(os.path.dirname(__file__), "vive_assets") +HEAD_MESH = "generic_hmd.obj" +HEAD_TEXTURE = "generic_hmd_color.png" +HEAD_FIX_EULER_DEG = (0.0, 0.0, 180.0) +HEAD_FALLBACK_RGBA = "0.4 0.4 0.4 1" # used if HEAD_TEXTURE is missing +HEAD_BOX_HALF_SIZE = "0.10 0.10 0.16" # used if HEAD_MESH is missing + +# OpenXR XR_EXT_hand_tracking joint indices (also exposed as +# ``deviceio.HandJoint.*``). Listed here so the bone table is readable. +WRIST = 1 +THUMB = (2, 3, 4, 5) # metacarpal, proximal, distal, tip +INDEX = (6, 7, 8, 9, 10) # metacarpal, proximal, intermediate, distal, tip +MIDDLE = (11, 12, 13, 14, 15) +RING = (16, 17, 18, 19, 20) +LITTLE = (21, 22, 23, 24, 25) + + +def _finger_chain(root: int, joints: tuple) -> list: + """[(root,j0), (j0,j1), (j1,j2), ...] — adjacent-pair bone segments.""" + chain = [(root, joints[0])] + for a, b in zip(joints[:-1], joints[1:]): + chain.append((a, b)) + return chain + + +# Parent -> child bone list (24 segments per hand). +HAND_BONES = ( + _finger_chain(WRIST, THUMB) + + _finger_chain(WRIST, INDEX) + + _finger_chain(WRIST, MIDDLE) + + _finger_chain(WRIST, RING) + + _finger_chain(WRIST, LITTLE) +) + +# Render colours: left = cyan, right = warm orange. +HAND_COLOURS = { + "left": {"joint": (0.30, 0.85, 1.00, 1.0), "bone": (0.20, 0.55, 0.95, 1.0)}, + "right": {"joint": (1.00, 0.65, 0.30, 1.0), "bone": (0.95, 0.40, 0.20, 1.0)}, +} + +JOINT_RADIUS = 0.008 # m +BONE_RADIUS = 0.004 # m + + +# ---------------------------------------------------------------------------- +# Pose helpers +# ---------------------------------------------------------------------------- +def _xr_pos_to_mj(p) -> np.ndarray: + return R_XR_TO_MJ.apply([p.x, p.y, p.z]) + + +def _xr_pose_to_mj(position, orientation): + pos = _xr_pos_to_mj(position) + q = ( + R_XR_TO_MJ + * R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w]) + ).as_quat() + return pos, np.array([q[3], q[0], q[1], q[2]]) # MuJoCo wants wxyz + + +def _euler_to_wxyz(euler_deg) -> str: + q = R.from_euler("xyz", euler_deg, degrees=True).as_quat() + return f"{q[3]} {q[0]} {q[1]} {q[2]}" + + +# ---------------------------------------------------------------------------- +# MJCF: floor + optional head mocap body. Hand joints/bones are drawn at +# runtime via viewer.user_scn so we don't need 52 mocap bodies in the model. +# ---------------------------------------------------------------------------- +def _build_mjcf(show_head: bool) -> str: + head_mesh_path = os.path.join(ASSETS_DIR, HEAD_MESH) + head_tex_path = os.path.join(ASSETS_DIR, HEAD_TEXTURE) + quat_attr = ( + f'quat="{_euler_to_wxyz(HEAD_FIX_EULER_DEG)}" contype="0" conaffinity="0"' + ) + + if show_head and os.path.exists(head_mesh_path) and os.path.exists(head_tex_path): + head_asset = ( + f'\n ' + f'\n ' + f'' + ) + head_geom = ( + f'' + ) + elif show_head and os.path.exists(head_mesh_path): + head_asset = f'' + head_geom = f'' + elif show_head: + head_asset = "" + head_geom = f'' + else: + head_asset = "" + head_geom = "" + + head_body = ( + f'{head_geom}' + if show_head + else "" + ) + + return f""" + + + +""" + + +# ---------------------------------------------------------------------------- +# user_scn helpers — append-only renderable geoms refilled each frame. +# ---------------------------------------------------------------------------- +_IDENT3 = np.eye(3).flatten() + + +def _add_sphere(scn, pos, radius, rgba) -> None: + if scn.ngeom >= scn.maxgeom: + return + mujoco.mjv_initGeom( + scn.geoms[scn.ngeom], + type=mujoco.mjtGeom.mjGEOM_SPHERE, + size=np.array([radius, 0.0, 0.0]), + pos=np.asarray(pos, dtype=np.float64), + mat=_IDENT3, + rgba=np.asarray(rgba, dtype=np.float32), + ) + scn.ngeom += 1 + + +def _add_capsule_between(scn, p0, p1, radius, rgba) -> None: + if scn.ngeom >= scn.maxgeom: + return + g = scn.geoms[scn.ngeom] + mujoco.mjv_initGeom( + g, + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + size=np.zeros(3), + pos=np.zeros(3), + mat=_IDENT3, + rgba=np.asarray(rgba, dtype=np.float32), + ) + mujoco.mjv_connector( + g, + mujoco.mjtGeom.mjGEOM_CAPSULE, + radius, + np.asarray(p0, dtype=np.float64), + np.asarray(p1, dtype=np.float64), + ) + scn.ngeom += 1 + + +def _draw_hand(scn, hand_data, colours) -> int: + """Append spheres + capsule bones for one hand. Returns # of valid joints.""" + if hand_data is None: + return 0 + + n = deviceio.NUM_JOINTS + positions = [None] * n + valid = 0 + for i in range(n): + jp = hand_data.joints.poses(i) + if not jp.is_valid: + continue + valid += 1 + positions[i] = _xr_pos_to_mj(jp.pose.position) + _add_sphere(scn, positions[i], JOINT_RADIUS, colours["joint"]) + + for a, b in HAND_BONES: + if positions[a] is None or positions[b] is None: + continue + _add_capsule_between( + scn, positions[a], positions[b], BONE_RADIUS, colours["bone"] + ) + + return valid + + +# ---------------------------------------------------------------------------- +# Main loop +# ---------------------------------------------------------------------------- +def _mocap_id(model, body_name: str): + body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name) + if body_id < 0: + return None + mid = int(model.body_mocapid[body_id]) + return mid if mid >= 0 else None + + +def main() -> int: + ap = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + ap.add_argument( + "--no-head", + action="store_true", + help="Skip the head mesh — only draw the hands.", + ) + ap.add_argument( + "--debug", + action="store_true", + help="Print per-hand valid-joint counts once per second.", + ) + args = ap.parse_args() + + show_head = not args.no_head + model = mujoco.MjModel.from_xml_string(_build_mjcf(show_head)) + data = mujoco.MjData(model) + head_mid = _mocap_id(model, "headset") if show_head else None + + head_tracker = deviceio.HeadTracker() if show_head else None + hand_tracker = deviceio.HandTracker() + trackers = [t for t in (head_tracker, hand_tracker) if t is not None] + extensions = deviceio.DeviceIOSession.get_required_extensions(trackers) + + print("Visualize Hands (MuJoCo)") + print(f"Trackers: {[t.get_name() for t in trackers]}") + print(f"Extensions: {extensions}") + print("Close the viewer window or Ctrl+C to quit.") + + with ( + oxr.OpenXRSession("VisualizeHandsMuJoCo", extensions) as oxr_session, + deviceio.DeviceIOSession.run(trackers, oxr_session.get_handles()) as session, + mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False + ) as viewer, + ): + # Restore Python's default SIGINT handler -- MuJoCo's viewer / GLFW main + # loop installs its own that can swallow Ctrl+C, leaving the user unable + # to exit the script. This makes Ctrl+C raise KeyboardInterrupt again. + signal.signal(signal.SIGINT, signal.default_int_handler) + + last_debug = 0.0 + try: + while viewer.is_running(): + session.update() + + if head_tracker is not None: + head = head_tracker.get_head(session).data + if head is not None and head.is_valid and head_mid is not None: + pos, quat = _xr_pose_to_mj( + head.pose.position, head.pose.orientation + ) + data.mocap_pos[head_mid] = pos + data.mocap_quat[head_mid] = quat + viewer.cam.lookat[:] = pos + + viewer.user_scn.ngeom = 0 + left = hand_tracker.get_left_hand(session).data + right = hand_tracker.get_right_hand(session).data + n_l = _draw_hand(viewer.user_scn, left, HAND_COLOURS["left"]) + n_r = _draw_hand(viewer.user_scn, right, HAND_COLOURS["right"]) + + mujoco.mj_forward(model, data) + viewer.sync() + + if args.debug and time.time() - last_debug >= 1.0: + last_debug = time.time() + print( + f"hands: L {n_l:2d}/{deviceio.NUM_JOINTS} " + f"R {n_r:2d}/{deviceio.NUM_JOINTS} valid joints" + ) + + time.sleep(1 / 60) + except KeyboardInterrupt: + print() + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/examples/cloudxr_mujoco_teleop/visualize_poses_mujoco_example.py b/examples/cloudxr_mujoco_teleop/visualize_poses_mujoco_example.py new file mode 100644 index 000000000..a04f29e17 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/visualize_poses_mujoco_example.py @@ -0,0 +1,305 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 HTC CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Visualize Headset + Controller Poses with MuJoCo +================================================ + +Subscribes to IsaacTeleop's ``HeadTracker`` and ``ControllerTracker`` and drives +three mocap bodies in a generated MJCF scene, rendered live in the MuJoCo +passive viewer. Closes when the viewer window is closed or Ctrl+C. + +The bundled meshes (in ``vive_assets/``) are a generic HMD and a pair of Vive +Focus 3 controllers, each with its matching texture. If any mesh / texture is +missing, the corresponding device falls back to a coloured primitive box, so +the sample still runs without the binary assets. + +Notes for adaptation +-------------------- +* **Coordinate frames.** IsaacTeleop delivers poses in the OpenXR convention + (right-handed, Y-up, -Z forward); MuJoCo's default is right-handed, Z-up. We + rotate every incoming pose by +90 deg about X and reorder the quaternion + from ``xyzw`` (OpenXR / scipy) to ``wxyz`` (MuJoCo). +* **Mesh-frame fix.** OBJ files are not modelled in the OpenXR pose frame; + ``DEVICES[*]["fix_euler_deg"]`` below is a per-device static rotation + (applied as ````) that aligns each mesh with its tracked + pose. **Tune these for your own meshes** -- the values shipped here were set + visually for the bundled generic HMD and Vive Focus 3 controllers. +* **Camera tracking.** The free camera's ``lookat`` is rewritten to the head + position every frame, so mouse-wheel zoom always converges on the headset. + +Usage +----- + python visualize_poses_mujoco_example.py + python visualize_poses_mujoco_example.py --pose aim --debug +""" + +import argparse +import os +import signal +import sys +import time + +import mujoco +import mujoco.viewer +import numpy as np +from scipy.spatial.transform import Rotation as R + +import isaacteleop.deviceio as deviceio +import isaacteleop.oxr as oxr + + +# Route MuJoCo warnings to stderr instead of writing MUJOCO_LOG.TXT in CWD. +def _mujoco_warning(msg) -> None: + text = msg.decode("utf-8", errors="replace") if isinstance(msg, bytes) else msg + print(f"[MuJoCo] {text}", file=sys.stderr) + + +mujoco.set_mju_user_warning(_mujoco_warning) + + +# ---------------------------------------------------------------------------- +# Asset locations +# ---------------------------------------------------------------------------- +ASSETS_DIR = os.path.join(os.path.dirname(__file__), "vive_assets") + + +# ---------------------------------------------------------------------------- +# Per-device configuration. Tune ``fix_euler_deg`` if a mesh looks rotated +# relative to its real motion; tune ``box_half_size`` to match the silhouette +# of your fallback geometry. +# ---------------------------------------------------------------------------- +DEVICES = { + "headset": { + "mesh": "generic_hmd.obj", + "texture": "generic_hmd_color.png", + "rgba": "0.4 0.4 0.4 1", # only used if texture missing + "fix_euler_deg": (0.0, 0.0, 180.0), + "init_pos": "0 0 1.5", + "box_half_size": "0.10 0.10 0.16", + }, + "controller_left": { + "mesh": "vive_focus3_controller_left.obj", + "texture": "vive_focus3_controller_left_color.png", + "rgba": "0.3 0.5 1.0 1", # box fallback colour + "fix_euler_deg": (0.0, 90.0, 0.0), + "init_pos": "-0.25 0 1.2", + "box_half_size": "0.08 0.06 0.05", + }, + "controller_right": { + "mesh": "vive_focus3_controller_right.obj", + "texture": "vive_focus3_controller_right_color.png", + "rgba": "1.0 0.4 0.3 1", + "fix_euler_deg": (0.0, 90.0, 0.0), + "init_pos": "0.25 0 1.2", + "box_half_size": "0.08 0.06 0.05", + }, +} + +# OpenXR -> MuJoCo world rotation (Rx +90 deg): (x, y, z) -> (x, -z, y). +R_XR_TO_MJ = R.from_euler("x", 90, degrees=True) + + +# ---------------------------------------------------------------------------- +# Pose / quaternion helpers +# ---------------------------------------------------------------------------- +def _euler_to_wxyz(euler_deg) -> str: + """scipy returns ``xyzw``; MJCF ``quat=`` is ``wxyz``.""" + q = R.from_euler("xyz", euler_deg, degrees=True).as_quat() + return f"{q[3]} {q[0]} {q[1]} {q[2]}" + + +def _convert_pose(position, orientation): + """OpenXR pose -> MJ-frame ``(pos[3], quat_wxyz[4])``.""" + p = R_XR_TO_MJ.apply([position.x, position.y, position.z]) + q = ( + R_XR_TO_MJ + * R.from_quat([orientation.x, orientation.y, orientation.z, orientation.w]) + ).as_quat() + return p, np.array([q[3], q[0], q[1], q[2]]) + + +# ---------------------------------------------------------------------------- +# MJCF generation -- writes one of {textured mesh, coloured mesh, box} per +# device depending on which assets are present on disk. +# ---------------------------------------------------------------------------- +def _device_geom_xml(name: str, dev: dict, assets_dir: str) -> str: + quat = _euler_to_wxyz(dev["fix_euler_deg"]) + mesh_path = os.path.join(assets_dir, dev["mesh"]) if dev["mesh"] else "" + tex_path = os.path.join(assets_dir, dev["texture"]) if dev["texture"] else "" + common = f'quat="{quat}" contype="0" conaffinity="0"' + + if mesh_path and os.path.exists(mesh_path): + if tex_path and os.path.exists(tex_path): + return ( + f'' + ) + return f'' + return f'' + + +def _device_asset_xml(name: str, dev: dict, assets_dir: str) -> str: + mesh_path = os.path.join(assets_dir, dev["mesh"]) if dev["mesh"] else "" + if not (mesh_path and os.path.exists(mesh_path)): + return "" + tex_path = os.path.join(assets_dir, dev["texture"]) if dev["texture"] else "" + blocks = [f''] + if tex_path and os.path.exists(tex_path): + blocks.append(f'') + blocks.append( + f'' + ) + return "\n ".join(blocks) + + +def _build_mjcf(assets_dir: str) -> str: + device_assets = "\n ".join( + filter( + None, + (_device_asset_xml(name, dev, assets_dir) for name, dev in DEVICES.items()), + ) + ) + + bodies = "\n".join( + f' \n' + f" {_device_geom_xml(name, dev, assets_dir)}\n" + f" " + for name, dev in DEVICES.items() + ) + + return f""" + + + +""" + + +# ---------------------------------------------------------------------------- +# Main loop +# ---------------------------------------------------------------------------- +def _mocap_id(model, body_name: str) -> int: + body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name) + mid = int(model.body_mocapid[body_id]) + if mid < 0: + raise RuntimeError(f"body '{body_name}' is not a mocap body") + return mid + + +def main() -> int: + ap = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + ap.add_argument( + "--assets-dir", + default=ASSETS_DIR, + help="Directory holding the device meshes and textures " + "(default: ./vive_assets).", + ) + ap.add_argument( + "--pose", + choices=["grip", "aim"], + default="grip", + help="Which controller pose drives the mesh (default: grip).", + ) + ap.add_argument( + "--debug", action="store_true", help="Print mocap_pos values once per second." + ) + args = ap.parse_args() + + model = mujoco.MjModel.from_xml_string(_build_mjcf(args.assets_dir)) + data = mujoco.MjData(model) + mocap_ids = {nm: _mocap_id(model, nm) for nm in DEVICES} + + head_tracker = deviceio.HeadTracker() + controller_tracker = deviceio.ControllerTracker() + trackers = [head_tracker, controller_tracker] + extensions = deviceio.DeviceIOSession.get_required_extensions(trackers) + + print(f"Visualize Poses (MuJoCo) | assets: {os.path.normpath(args.assets_dir)}") + print("Close the viewer window or Ctrl+C to quit.") + + with ( + oxr.OpenXRSession("VisualizePosesMuJoCo", extensions) as oxr_session, + deviceio.DeviceIOSession.run(trackers, oxr_session.get_handles()) as session, + mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False + ) as viewer, + ): + # Restore Python's default SIGINT handler -- MuJoCo's viewer / GLFW main + # loop installs its own that can swallow Ctrl+C, leaving the user unable + # to exit the script. This makes Ctrl+C raise KeyboardInterrupt again. + signal.signal(signal.SIGINT, signal.default_int_handler) + + last_debug = 0.0 + try: + while viewer.is_running(): + session.update() + + head = head_tracker.get_head(session).data + if head is not None and head.is_valid: + pos, quat = _convert_pose(head.pose.position, head.pose.orientation) + data.mocap_pos[mocap_ids["headset"]] = pos + data.mocap_quat[mocap_ids["headset"]] = quat + viewer.cam.lookat[:] = pos # keep mouse-wheel zoom centred on head + + for label, getter in ( + ("controller_left", controller_tracker.get_left_controller), + ("controller_right", controller_tracker.get_right_controller), + ): + ctrl = getter(session).data + if ctrl is None: + continue + cpose = ctrl.aim_pose if args.pose == "aim" else ctrl.grip_pose + if not cpose.is_valid: + continue + pos, quat = _convert_pose( + cpose.pose.position, cpose.pose.orientation + ) + data.mocap_pos[mocap_ids[label]] = pos + data.mocap_quat[mocap_ids[label]] = quat + + mujoco.mj_forward(model, data) + viewer.sync() + + if args.debug and time.time() - last_debug >= 1.0: + last_debug = time.time() + print("mocap_pos (MJ frame, Z-up):") + for nm, mid in mocap_ids.items(): + p = data.mocap_pos[mid] + print(f" {nm:18s} [{p[0]:+.3f}, {p[1]:+.3f}, {p[2]:+.3f}]") + + time.sleep(1 / 60) + except KeyboardInterrupt: + print() + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd.obj b/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd.obj new file mode 100644 index 000000000..3d24e6bb8 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:073dff292bbe9e29a90169f5b2e0af2b79159a3a2a90fa1c5de71ad67708578a +size 511614 diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd_color.png b/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd_color.png new file mode 100644 index 000000000..2bce2ba82 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/generic_hmd_color.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b0ff80b1cc37091fd045c7ef801774ac9087dce7690267a81269e8ef5c31e088 +size 50635 diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left.obj b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left.obj new file mode 100644 index 000000000..78f477177 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1a51280bccdab08261f17f5aac0d1789b6ee2556df8b52756cd6cd602aa63d73 +size 1412617 diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left_color.png b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left_color.png new file mode 100644 index 000000000..c5fa69a2b --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_left_color.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1870a2669a3e6d5c828130843a2aabb4cde22d1354d4fd768a833e311c7e7de6 +size 1240137 diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right.obj b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right.obj new file mode 100644 index 000000000..095a35144 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right.obj @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:231f385654dd86cf633150d7ad7a7dcbd5c6534c1fc2a2708437d7377ffd355b +size 1415197 diff --git a/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right_color.png b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right_color.png new file mode 100644 index 000000000..8156f48f4 --- /dev/null +++ b/examples/cloudxr_mujoco_teleop/vive_assets/vive_focus3_controller_right_color.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e85d1140e122c5f8c0e75a2eff99cda2935bb8b92042276b6ff1bfea7abb3df0 +size 1255625