diff --git a/.gitignore b/.gitignore index acff933ac..d67f5fd08 100644 --- a/.gitignore +++ b/.gitignore @@ -69,13 +69,14 @@ deps/v2d/wheels/ # Runtime device files injected by MCP server infrastructure .mcp.json -# SO-101 leader-gripper assets, fetched by -# examples/mujoco_xr/scripts/fetch-so-arm.sh into the package's own assets -# directory (which is what makes them package data), so only the authored -# wrapper XML beside them is tracked. +# Gripper-ghost assets, fetched by examples/mujoco_xr/scripts/fetch-*.sh into the +# package's own assets directory (which is what makes them package data), so only +# the authored wrapper XML beside them is tracked. # -# Keep this rule here, not in examples/mujoco_xr/.gitignore: scikit-build-core +# Keep these rules here, not in examples/mujoco_xr/.gitignore: scikit-build-core # resolves .gitignore against the project root, so a rule there would strip the # meshes out of the wheel too. /examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/* !/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml +/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot/* +!/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot/rebot_gripper.xml diff --git a/examples/mujoco_xr/README.md b/examples/mujoco_xr/README.md index b9446689e..e9cc44097 100644 --- a/examples/mujoco_xr/README.md +++ b/examples/mujoco_xr/README.md @@ -6,7 +6,8 @@ SPDX-License-Identifier: Apache-2.0 # MuJoCo XR A MuJoCo scene rendered stereoscopically into an Isaac Teleop Televiz XR -session, with an SO-101 leader gripper locked to the operator's right hand. +session, with a gripper locked to the operator's right hand — an SO-101 leader +gripper or a reBot DevArm gripper, picked with `--robot`. Single process, single thread, **one** OpenXR session: @@ -47,9 +48,9 @@ extension and asserts both report the same version. | | | |---|---| -| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the projection convention, the clock, the ghost overlay and its jaw channel. All **pure CPU**: no GPU, no headset, no runtime, no window system. | +| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the projection convention, the clock, both ghost overlays and their jaw channel. All **pure CPU**: no GPU, no headset, no runtime, no window system. | | **Never executed anywhere** | **The app itself.** `kXr` is the only display mode and it needs a headset plus a CloudXR runtime, so the frame loop, the renderer, OpenXR session sharing via `oxr_handles`, controllers on a shared session, the Vulkan→CUDA→`submit()` path and whether the runtime accepts the depth layer are run by no test and by no developer here. | -| **Wrong by construction until calibrated** | The workspace translation, for any scene that adds static content — see [Frames](#frames-cppframeshpp). The shipped ghost-only scene does not show it. | +| **Wrong by construction until calibrated** | The workspace translation, for any scene that adds static content — see [Frames](#frames-cppframeshpp). Neither ghost-only scene shows it. | Nothing in `.github/workflows/` installs `mujoco`, so the example is never configured and **not one of its tests has ever run in CI**. Green means one @@ -58,31 +59,42 @@ developer ran it locally. Wiring examples into CI is ## Scope -Renderer + MuJoCo + rig, and one scene: `assets/scene.xml` — an **SO-101 -leader gripper ghost** locked to the right controller's grip pose, and nothing -else. No table, no blocks, no ground plane: this is an AR scene and passthrough -is the background. - -The ghost is not decoration. It is a real mesh assembly (4 fetched STLs, so it -exercises the `mjGEOM_MESH` path), and locking it to the hand makes the *grip* -calibration visible — whether the tool sits in the hand the way a hand holds -one. It cannot show a wrong `cpp/frames.hpp`: those constants place it and the -renderer undoes them folding it back into the XR reference space, so the ghost -lands in the hand whatever they say. Only static content shows them, and the -shipped scene has none. - -**Its trigger is driven by the shipped `SO101GripperRetargeter`, as a graph -edge** — the retargeter is a `BaseRetargeter` node inside `_build_pipeline()`, -not a library call beside it, and its closedness output reaches `mjData` and -therefore the screen. There is no robot in the scene, so the jaw it drives is -the operator's own trigger; that is enough to show the edge is live, and the -SO-101 that will read the same output arrives with the scene catalogue. +Renderer + MuJoCo + rig, and one **gripper ghost** locked to the right +controller's grip pose, with nothing else in the scene. No table, no blocks, no +ground plane: this is an AR scene and passthrough is the background. + +| `--robot` | ghost | scene | fetch | +|---|---|---|---| +| `so101` (default) | SO-101 **leader** gripper — the handheld device itself, so the ghost is the tool the fist is closed around. One trigger on a hinge. | `assets/so101_scene.xml` | `scripts/fetch-so-arm.sh` | +| `rebot` | reBot DevArm gripper — the **follower's** parallel jaw. The reBot leader is a back-driven arm on a table, so what the controller commands is this. Two jaws on one rack. | `assets/rebot_scene.xml` | `scripts/fetch-rebot-arm.sh` | + +Everything that differs between them lives in `robots.py` — scene, meshes, +mocap bodies, how each moving part is driven, where it sits on the hand — so +`app.py` holds only the machinery and adding a robot is a new `Robot` plus its +two authored XML files. A moving part is a **hinge** or a **slide**, and those +two cases are the whole of `_update_ghost`. + +The ghost is not decoration. It is a real mesh assembly (4 fetched STLs for the +SO-101, 7 for the reBot, so it exercises the `mjGEOM_MESH` path), and locking it +to the hand makes the *grip* calibration visible — whether the tool sits in the +hand the way a hand holds one. It cannot show a wrong `cpp/frames.hpp`: those +constants place it and the renderer undoes them folding it back into the XR +reference space, so the ghost lands in the hand whatever they say. Only static +content shows them, and neither shipped scene has any. + +**The jaw is driven by the shipped `SO101GripperRetargeter`, as a graph edge** — +the retargeter is a `BaseRetargeter` node inside `_build_pipeline()`, not a +library call beside it, and its closedness output reaches `mjData` and therefore +the screen. It is the repository's only proportional trigger-to-closedness node; +the SO-101 in its name is where it came from, not a claim about which gripper +reads it, and both ghosts do. There is no robot in either scene, so the jaw it +drives is the operator's own trigger. Two calibrations, and they are different in kind. `cpp/frames.hpp` is a -*convention* fixed by two specs and cannot be wrong at runtime. -`_QUAT_GRIP_FROM_GHOST` / `_POS_GRIP_FROM_GHOST` in `app.py` are a *measurement* -of how a hand holds a tool — where the fist sits on the handle — derived from -the mesh but only checkable on a headset. See [Frames](#frames-cppframeshpp). +*convention* fixed by two specs and cannot be wrong at runtime. The +`quat_grip_from_ghost` / `pos_grip_from_ghost` on each `Robot` place the tool on +the hand — a *measurement* for the SO-101, a *convention* for the reBot, and the +difference matters. See [Frames](#frames-cppframeshpp). ## Build @@ -202,11 +214,21 @@ running and you pass it anyway, the failure comes out of `VizSession.create` as an OpenXR error before any of this example's code runs — **no `[mujoco_xr]` lines at all** is the tell. -There is one scene and no flag to change it: `assets/scene.xml` is package data -beside the module, and editing it is how you load something else. There is no -desktop or headless mode either; without a headset the only verification path is +`--robot` picks the ghost; each entry's scene is package data beside the module, +and editing it is how you load something else into that scene. There is no +desktop or headless mode; without a headset the only verification path is [`ctest -L mujoco_xr`](#tests), which exercises no GPU code at all. +```bash +examples/mujoco_xr/scripts/fetch-rebot-arm.sh # once +uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr +python -m isaacteleop_examples.mujoco_xr --robot=rebot +``` + +Only the selected robot's meshes have to be fetched — the app checks that one +list and names that one script. The `robot:` line in the startup log says which +ghost it bound. + ## Conventions you can break ### Frames (`cpp/frames.hpp`) @@ -229,7 +251,7 @@ scene that puts static content on the work surface owns re-tuning it. It places static content only. The ghost goes out through `mj_from_xr` and the renderer folds it back through `xr_from_mj`, so both constants cancel on it and -the shipped scene — which is the ghost and nothing else — is blind to a wrong +each shipped scene — which is a ghost and nothing else — is blind to a wrong value. Judging one means a scene with something world-locked in it. There is no recentre keypress and no runtime override: changing the datum means @@ -241,14 +263,22 @@ startup log, compare the virtual surface against the real one, and adjust `z`. A a Python-side offset would move the gripper and leave the scene put, which is precisely the symptom this example exists to disambiguate. -### Where the ghost sits on the hand (`app.py`) +### Where the ghost sits on the hand (`robots.py`) + +A *second* calibration, and a different kind: each `Robot`'s +`quat_grip_from_ghost` / `pos_grip_from_ghost` place its gripper on the +operator's hand. Without them the gripper's body origin — a CAD datum up at the +wrist, or out at the fingertips — lands on the grip pose, so the tool hangs off +the hand at an arbitrary angle. -A *second* calibration, and a different kind: `_EULER_GRIP_FROM_GHOST_DEG` and -`_POS_GRIP_FROM_GHOST` place the leader gripper on the operator's hand. Without -them the gripper's body origin — the follower's `gripper` datum, up at the wrist -— lands on the grip pose, so the tool hangs off the hand at an arbitrary angle. +**The two robots' values have different provenance, and conflating them is the +mistake to avoid.** The SO-101's are measured; the reBot's are derived, because +nobody holds a reBot gripper. -**These are measured on a headset, not derived.** That is the whole provenance: +#### SO-101 — measured + +`SO101_EULER_GRIP_FROM_GHOST_DEG` and the position beside it are **measured on a +headset, not derived.** That is the whole provenance: it is a claim about how a gripper should look in a hand that is actually holding a *controller*, and nothing headless can settle it. @@ -272,8 +302,8 @@ convention as a MuJoCo `euler=` attribute, pinned by a test against a compiled model rather than asserted here. Change one angle, `uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr`, relaunch: `Rz` spins the gripper about its own long axis, `Rx` / `Ry` tilt it in -the hand, and `_POS_GRIP_FROM_GHOST` slides it along the grip axes if the angle -is right but the placement is not. **No test asserts a posture**, deliberately — +the hand, and the position slides it along the grip axes if the angle is right +but the placement is not. **No test asserts an SO-101 posture**, deliberately — they cover the machinery, so re-tuning cannot turn them red. The one that matters asserts the ghost is *rigidly attached* to the grip frame, which is true of any calibration and false if the correction is composed on the wrong @@ -284,7 +314,27 @@ every mesh into its inertial frame, so recovering an STL's own axes needs `mesh_pos` / `mesh_quat`. Skip that and you get the *handle's* axis back instead of the jaws', which is self-consistent, passes an axis-only check, and is wrong by 60°. The shank's own principal axis is no substitute either — it is a -near-isotropic blob (σ₀/σ₁ = 1.26), so its principal direction is noise. +near-isotropic blob (σ₀/σ₁ = 1.26), so its principal direction is noise. The same +trap bites when checking the reBot jaw frames against their URDF — reading +`geom_pos` / `geom_quat` raw is off by up to 37 mm and 132° there. + +#### reBot — a convention, so a test may pin it + +Nobody holds a reBot gripper: it is the follower's jaw drawn at the hand, so +there is no fist-on-a-handle claim to measure and nothing to tune on a headset. +`REBOT_EULER_GRIP_FROM_GHOST_DEG` is the OpenXR **grip** frame's own definition +instead — `+X` into the palm, `−Z` forward through the tube-shaped fist (where a +pen tip would point), `+Y` out of the fist toward the thumb: + +- ghost `+x`, the approach axis with the jaws reaching forward → grip `−Z` +- ghost `±y`, the jaw opening axis → grip `±X`, so the jaws open the way the + index finger squeezes + +which is `Rx(90) Ry(0) Rz(270)`. `REBOT_POS_GRIP_FROM_GHOST` then slides the fist +onto the gripper's **drive motor** rather than onto the link origin, which is out +at the fingertips — `motor_7` spans x ∈ [−0.1572, −0.0927], so its centre is at +−0.125 m. Both claims are asserted by `test_ghost.py`, which is the difference +from the SO-101 above: a change here is a change of convention, not a re-tune. ### Scene assets @@ -304,33 +354,55 @@ instead area-averaged over the faces round each corner that lie within `kCreaseCos`. The measured counts are in `cpp/mesh_buffers.hpp`, and `test_ghost.py` fails if anyone reverts to `mjModel`'s. -The ghost's four STLs are **fetched, not vendored** — 2.3 MB of binary in a +Every ghost's STLs are **fetched, not vendored** — megabytes of binary in a source tree is a poor trade when upstream publishes them at a stable commit, and -Git LFS made every clone pay for them. Run it once, then reinstall, because they -are package data: +Git LFS made every clone pay for them. One script per robot; run the one you +need, then reinstall, because they are package data: ```bash examples/mujoco_xr/scripts/fetch-so-arm.sh # from the repository root +examples/mujoco_xr/scripts/fetch-rebot-arm.sh # --robot=rebot uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr ``` Nothing fetches at build time: an isolated PEP-517 wheel build must not reach -the network, so the app fails at startup naming the script and `test_ghost.py` -**skips** with the same reason. Downloads are checksum-verified against a pinned -commit — a silently substituted mesh renders as a broken gripper rather than an -error, which has already cost a debugging session. - -The script also pulls `so101_new_calib.urdf`, which is where the trigger's hinge -and its 0..100° travel come from, so it is on disk to check them against. Three of the four -meshes are leader-specific print parts; the fourth is the **STS3215 servo**, -shared with the follower. It is not decoration — `wrist_roll` is a C-shaped -bracket that wraps the servo, so without it the assembly has an open notch where -the motor belongs and reads as a broken asset. - -It declares **two** mocap bodies — the gripper and its trigger — because the -trigger articulates; a jointed child of a mocap body would be a dynamic joint -that `mj_step` integrates gravity into, and a mocap body is kinematic by -construction. +the network, so the app fails at startup naming the *selected* robot's script and +`test_ghost.py` **skips** that robot with the same reason. Downloads are +checksum-verified against a pinned commit — a silently substituted mesh renders +as a broken gripper rather than an error, which has already cost a debugging +session. + +Each script also pulls the URDF the ghost's transforms were read out of, so +`test_ghost.py` can check them against their source rather than against +themselves. + +| | SO-101 | reBot | +|---|---|---| +| upstream | `TheRobotStudio/SO-ARM100`, Apache-2.0 | `Seeed-Projects/reBotArm_control_py`, `urdf/00-arm-rs_asm-v3/` | +| licence | `LICENSE` fetched beside the meshes | declared MIT in `README.md`, which is fetched in its place — **upstream ships no `LICENSE` file** at the pinned commit. The hardware is `Seeed-Projects/reBot-DevArm`, CERN-OHL-W-2.0. | +| meshes | 4, 2.3 MB, 45k triangles | 7, 5.8 MB, 115k triangles | +| units | print STLs in **millimetres** (`scale="0.001"`); the servo in metres and carries none | all metres, no `scale` | +| checked against the URDF | the trigger hinge and its 0..100° travel | both jaw frames, the slide axes, and the travel | + +Three of the SO-101's four meshes are leader-specific print parts; the fourth is +the **STS3215 servo**, shared with the follower. It is not decoration — +`wrist_roll` is a C-shaped bracket that wraps the servo, so without it the +assembly has an open notch where the motor belongs and reads as a broken asset. + +**The reBot travel is derived, because upstream disagrees with itself.** +`joint_left` says `upper="0.05"` and `joint_right` says `0.0715` for the same +rack-driven pair. The tie-breaker is the `cnc7` rail plate: at `0.05` the +carriage's outer edge stops 3.4 mm inside its end, and at `0.0715` it hangs +18.1 mm past it. 0.05 per +jaw is a 100 mm opening, and both upstream numbers are read out of the fetched +URDF by a test, so a corrected export fails rather than drifting. The joints' +authored zero is the **closed** end — the opposite polarity to the SO-101 +trigger, whose zero is squeezed only by coincidence of sign. + +Each fragment declares one mocap body per moving part — the SO-101's trigger, +the reBot's two jaws — because they articulate; a jointed child of a mocap body +would be a dynamic joint that `mj_step` integrates gravity into, and a mocap body +is kinematic by construction. The ghost is **opaque**, and `test_ghost.py` asserts it. That removes the draw-order constraint (at alpha 1.0 the depth test decides everything), the @@ -343,8 +415,8 @@ the scene that needs it. **Pass MuJoCo an absolute scene path.** Measured on mujoco 3.11.0, a *relative* model path mis-composes the mesh paths of an ``d file in a -subdirectory and fails with `Error opening file ''`. -`DEFAULT_SCENE` in `app.py` is absolute for this reason. +subdirectory and fails with `Error opening file ''`. Every +`Robot.scene` in `robots.py` is absolute for this reason. ### Culling @@ -364,8 +436,8 @@ ctest --test-dir build/cmake-cpython-312 -L mujoco_xr --output-on-failure |---|---| | `test_frames.py` | the XR→MuJoCo axis map and quaternion order | | `test_projection.py` | the clip-space convention (Y flip, standard Z, degenerate-fov rejection) | -| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, the single near/far pair, and that the first-frame projection assertion actually fires | -| `test_ghost.py` | the overlay: that the ghost is opaque, collision-free and carries no mass, that both its bodies are kinematic mocap bodies with no joint anywhere, that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, that the print STLs are scaled from millimetres and the servo is not, that every corner normal the renderer builds faces the same way as its own triangle (mjModel's do not, and that is what made the ghost render as shattered facets), that the ghost is *rigidly attached* to the grip frame whatever the calibration, that squeezing swings the trigger monotonically from the URDF joint's upper limit to its authored zero without driving the lever through the body, that the shipped `SO101GripperRetargeter` really is the thing driving that channel (built as a real pipeline and fed synthetic DeviceIO snapshots), and that an untracked controller freezes the whole gripper rather than parking it at the scene origin | +| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, the single near/far pair, that the first-frame projection assertion actually fires, and that `--robot` rejects an unknown value and reports the *selected* robot's fetch script | +| `test_ghost.py` | the overlay. **Every robot**: that the ghost is opaque, collision-free and carries no mass, that every one of its bodies is a kinematic mocap body with no joint anywhere, that every mesh is hand-sized (the units trap, from both directions), that every corner normal the renderer builds faces the same way as its own triangle (mjModel's do not, and that is what made the ghost render as shattered facets), that the ghost is *rigidly attached* to the grip frame whatever the calibration, that closedness drives every part monotonically from the catalogue's released end to its squeezed end, and that an untracked controller freezes the whole gripper rather than parking it at the scene origin. **SO-101**: that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, and that the trigger swings from the URDF joint's upper limit to its authored zero without driving the lever through the body. **reBot**: that both jaw frames and slide axes are Seeed's URDF (undoing MuJoCo's rewrite of the mesh into its inertial frame), that the travel is the limit the rail plate supports and upstream still disagrees with itself, that squeezing brings the fingertips together symmetrically from a 100 mm opening without pushing a carriage into the body, and that the placement follows the OpenXR grip convention with the fist on the drive motor. Plus the shipped `SO101GripperRetargeter` really is the thing driving that channel (built as a real pipeline and fed synthetic DeviceIO snapshots) | Every one runs on a CPU with no GPU, no headset, no CloudXR runtime and no window system. Keep it that way: a permanently-skipping test reports green while @@ -377,11 +449,17 @@ covering nothing. `ProjectionLayer.submit()`, the frame loop that sequences them, OpenXR session sharing via `oxr_handles`, whether the runtime accepts the depth layer, and **controllers on a shared session** — none of it is executed by any test or on -any machine here. The grip-to-gripper calibration is a headset-only judgement -by construction: it is a claim about how a hand holds a tool, and no headless -test can confirm it — `tests/test_ghost.py` pins the *machinery* against a -reference calibration and deliberately leaves the shipped constants free to be -tuned. +any machine here. The SO-101 grip-to-gripper calibration is a headset-only +judgement by construction: it is a claim about how a hand holds a tool, and no +headless test can confirm it — `tests/test_ghost.py` pins the *machinery* against +a reference calibration and deliberately leaves those constants free to be tuned. + +**Neither ghost has been seen through a headset since `--robot` was added, and +the reBot one never has.** Its placement is derived rather than measured (see +[reBot — a convention](#rebot--a-convention-so-a-test-may-pin-it)), so what is +untested is whether a 190 mm gripper reaching 250 mm out of the fist is *usable*, +not whether it is where the convention says. That is the judgement to make first +on a headset. Controllers on a shared session have no precedent elsewhere in this repository: `xrAttachSessionActionSets` is legal once per `XrSession`, Teleop sidesteps it diff --git a/examples/mujoco_xr/cpp/mesh_buffers.hpp b/examples/mujoco_xr/cpp/mesh_buffers.hpp index b2d79b400..1f42695c3 100644 --- a/examples/mujoco_xr/cpp/mesh_buffers.hpp +++ b/examples/mujoco_xr/cpp/mesh_buffers.hpp @@ -9,7 +9,7 @@ // Normals are computed here, not taken from mjModel: MuJoCo welds an STL's // vertices and stores one averaged normal per welded vertex (mesh_normalnum == // mesh_vertnum, mesh_facenormal == mesh_face), so on a CAD part every crease -// gets a normal smeared across it. Measured on the shipped scene under +// gets a normal smeared across it. Measured on the SO-101 scene under // test_ghost.py's own predicate (dot(corner normal, own face normal) <= 0): // 2138 of Wrist_Roll_SO101's 18474 face corners point away from their own // face, and 9489 of the STS3215's 57240. Lit one-sided those corners drop to diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py index 7e5292029..483c3cd88 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py @@ -7,6 +7,10 @@ (input); the scene is drawn by Vulkan into images viz owns and reaches ProjectionLayer.submit() by CUDA pointer, never through host memory. +`--robot` picks which gripper ghost is drawn. This file holds only the machinery; +everything that differs between them -- scene, meshes, mocap bodies, how each +moving part is driven, where it sits on the hand -- is in robots.py. + VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession │ │ │ vk_device / vk_physical_device │ controller grip poses @@ -27,7 +31,6 @@ import argparse import importlib.metadata import logging -import math import sys from pathlib import Path from typing import NamedTuple @@ -52,6 +55,7 @@ ) from . import _mujoco_xr +from .robots import DEFAULT_ROBOT, ROBOTS, PartKind, Robot LOG = logging.getLogger("mujoco_xr") @@ -79,95 +83,10 @@ "not sampled as 0" ) -# Package data, so it resolves identically from the wheel and the source tree. -# Must stay absolute: scene.xml s a fragment in a subdirectory, and on -# mujoco 3.11.0 a relative model path mis-composes that fragment's mesh paths -# and fails naming a file that is right there on disk. -DEFAULT_SCENE = Path(__file__).parent / "assets" / "scene.xml" - -# The meshes scene.xml s, fetched rather than vendored (see -# scripts/fetch-so-arm.sh). Checked by name before MuJoCo sees the scene, because -# MjModel.from_xml_path's failure for a missing target is a bare -# "Error opening file .stl" naming a file nobody asked for. -FETCH_SCRIPT = "examples/mujoco_xr/scripts/fetch-so-arm.sh" -_LEADER_ASSETS = Path(__file__).parent / "assets" / "leader" -_LEADER_MESHES = ( - "Wrist_Roll_SO101.stl", - "Trigger_SO101.stl", - "Handle_SO101.stl", - "STS3215_03a.stl", -) - - -def _missing_leader_assets() -> list[str]: - """Names of the fetched meshes that are not on disk. Empty when fetched.""" - return [n for n in _LEADER_MESHES if not (_LEADER_ASSETS / n).is_file()] - - -# One hand and no flag: the ghost is a right-handed leader gripper, and the left -# controller draws nothing. +# One hand and no flag: every ghost is right-handed, and the left controller +# draws nothing. GHOST_HAND = ControllersSource.RIGHT -# The two mocap bodies leader_gripper.xml declares. -GHOST_BODY = "leader_ghost" -GHOST_JAW_BODY = "leader_ghost_jaw" - -# ── Where the ghost sits on the hand ─────────────────────────────────────── -# Measured on a headset, not derived: this is a claim about a hand holding a -# CONTROLLER, so do not re-derive it from the mesh -- a model assuming the hand -# passes through the handle loop puts the loop centroid 56 mm from the palm. -# -# Euler degrees, intrinsic XYZ, i.e. MuJoCo's `euler=` (pinned by a test). To -# re-tune, change one angle and reinstall: Rz spins the gripper about its long -# axis, Rx/Ry tilt it, _POS_GRIP_FROM_GHOST slides it along the grip axes -# (-Z little finger -> thumb, +X into the palm, +Y through the knuckles). No -# test asserts a posture, so re-tuning cannot turn them red. -_EULER_GRIP_FROM_GHOST_DEG = (60, 180, 270) -_POS_GRIP_FROM_GHOST = np.array((0, 0.02, -0.025)) - -# ── The trigger hinge ────────────────────────────────────────────────────── -# The follower's `gripper` revolute joint, from SO-ARM100's -# so101_new_calib.urdf: origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 0 0", -# axis "0 0 1". The right source even for the LEADER's trigger, which is -# mounted in the follower's moving-jaw slot and shares the hinge. The axis -# below is that "0 0 1" carried through the joint frame's 90-degree roll. -# -# Do not re-derive either from the meshes: a pivot from the nearest -# trigger-to-shank vertex pair and an axis from the grip frame both look right -# at the joint's zero and are wrong by the far end of its travel. -_TRIGGER_HINGE_POS = np.array((0.0202, 0.0188, -0.0234)) # metres, ghost frame -_TRIGGER_HINGE_AXIS = np.array((0.0, -1.0, 0.0)) # unit, ghost frame - -# The travel is the URDF joint's own: `upper="1.74533"` is 100.0 degrees, and -# squeezed is its authored zero. A released end short of that does not read as -# an OPEN gripper on a headset, which is the only place this can be judged. -# Do not extend to the joint's lower limit (-10 deg): that end swings the lever -# 0.4 mm into the servo. The tightest pass across 0..100 is 2.1 mm, at the -# squeezed end. -_TRIGGER_RELEASED_RAD = math.radians(100.0) # closedness 0, jaw wide open -_TRIGGER_SQUEEZED_RAD = 0.0 # closedness 1, tucked to the authored pose - - -def _quat_from_euler_deg(angles_deg) -> np.ndarray: - """Intrinsic X-then-Y-then-Z degrees -> a wxyz quaternion. - - Right-multiplication is what makes it intrinsic, and is the convention - MuJoCo's `euler=` uses. Spelled out rather than calling mju_euler2Quat so - the sequence is visible at the point of use. - """ - quat = np.array((1.0, 0.0, 0.0, 0.0)) - for axis, angle in zip(np.eye(3), angles_deg): - step = np.empty(4) - mujoco.mju_axisAngle2Quat(step, axis, math.radians(angle)) - composed = np.empty(4) - mujoco.mju_mulQuat(composed, quat, step) - quat = composed - return quat - - -# ── Derived below; nothing from here on is authored ──────────────────────── -_QUAT_GRIP_FROM_GHOST = _quat_from_euler_deg(_EULER_GRIP_FROM_GHOST_DEG) - def _clamp_dt(dt: float) -> float: """NaN-safe clamp into [0, MAX_DT_S]. @@ -181,12 +100,13 @@ def _clamp_dt(dt: float) -> float: def _build_pipeline() -> OutputCombiner: - """Controllers, plus the shipped SO-101 jaw retargeter as a graph edge. + """Controllers, plus the shipped jaw retargeter as a graph edge. The retargeter is a BaseRetargeter node in the pipeline rather than a - library call beside it. The shipped scene has no robot, so the jaw it drives - is the operator's own trigger; the SO-101 arrives with the scene catalogue and - reads the same output. + library call beside it. It is the repository's only proportional + trigger-to-closedness node; the SO-101 in its name is where it came from, not + a claim about which gripper reads it, and both ghosts do. The shipped scenes + have no robot, so the jaw it drives is the operator's own trigger. """ controllers = ControllersSource(name="controllers") jaw = SO101GripperRetargeter(name="ghost_jaw", input_device=GHOST_HAND).connect( @@ -254,7 +174,7 @@ def _assert_projection(p: list[float], near: float, far: float) -> None: ) -def _log_startup(resolution) -> None: +def _log_startup(robot: Robot, resolution) -> None: """One block naming every assumption that is invisible at runtime.""" try: version = importlib.metadata.version("isaacteleop") @@ -262,7 +182,8 @@ def _log_startup(resolution) -> None: version = "" trans = _mujoco_xr.TRANS_MJ_FROM_XR - LOG.info("scene: %s", DEFAULT_SCENE) + LOG.info("robot: %s (%s)", robot.key, robot.description) + LOG.info("scene: %s", robot.scene) # Cross-example venv collisions are real: several examples here ship their # own .venv, and picking up the wrong isaacteleop is invisible otherwise. LOG.info( @@ -301,37 +222,40 @@ def _log_startup(resolution) -> None: class _GhostChannels(NamedTuple): - """The two mocap rows the ghost writes, resolved once at startup. + """The mocap rows the ghost writes, resolved once at startup. Mocap indices, not body ids: mocap_pos/mocap_quat are indexed by - body_mocapid, and a body id there writes into another body's row. + body_mocapid, and a body id there writes into another body's row. `parts` is + parallel to Robot.parts. """ body: int - jaw: int + parts: tuple[int, ...] -def _resolve_ghost(model) -> _GhostChannels: - """Both ghost mocap rows. The shipped scene always declares them.""" - body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_BODY) - jaw = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_JAW_BODY) - if body < 0 or jaw < 0: +def _resolve_ghost(model, robot: Robot) -> _GhostChannels: + """Every ghost mocap row. The robot's scene always declares them.""" + names = (robot.body, *(part.body for part in robot.parts)) + ids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, n) for n in names] + missing = [n for n, i in zip(names, ids) if i < 0] + if missing: raise RuntimeError( - f"mujoco_xr: {DEFAULT_SCENE} declares no `{GHOST_BODY}` / " - f"`{GHOST_JAW_BODY}` pair; it must assets/leader/leader_gripper.xml." + f"mujoco_xr: {robot.scene} declares no `{'` / `'.join(missing)}`; it must " + f" the {robot.key} gripper fragment." ) - return _GhostChannels(int(model.body_mocapid[body]), int(model.body_mocapid[jaw])) + rows = [int(model.body_mocapid[i]) for i in ids] + return _GhostChannels(rows[0], tuple(rows[1:])) -def _update_ghost(data, ghost: _GhostChannels, result) -> None: - """Lock the leader gripper to the GHOST_HAND grip pose; swing its trigger. +def _update_ghost(data, ghost: _GhostChannels, robot: Robot, result) -> None: + """Lock the gripper to the GHOST_HAND grip pose; drive its moving parts. Keep the validity gate: an untracked controller leaves the grip pose at (0, 0, 0), which is the MuJoCo scene origin and a place a legitimate pose could put it. Freezing where it was last seen is the honest rendering of "tracking lost", so there is no else branch. - _QUAT_GRIP_FROM_GHOST right-multiplies because it is fixed in the gripper's + quat_grip_from_ghost right-multiplies because it is fixed in the gripper's own frame; left-multiplying swings the ghost around the room as the operator turns. """ @@ -354,36 +278,39 @@ def _update_ghost(data, ghost: _GhostChannels, result) -> None: p_grip = np.array(_mujoco_xr.mj_from_xr_pos(p_xr), dtype=float) q_body = np.empty(4) - mujoco.mju_mulQuat(q_body, q_grip, _QUAT_GRIP_FROM_GHOST) + mujoco.mju_mulQuat(q_body, q_grip, robot.quat_grip_from_ghost) p_offset = np.empty(3) - mujoco.mju_rotVecQuat(p_offset, _POS_GRIP_FROM_GHOST, q_grip) + mujoco.mju_rotVecQuat(p_offset, robot.pos_grip_from_ghost, q_grip) p_body = p_grip + p_offset data.mocap_pos[ghost.body] = p_body data.mocap_quat[ghost.body] = q_body # Closedness comes through the pipeline output, so the deadzone and clamp - # are the retargeter's contract rather than this app's. Rotated ABOUT the - # hinge, not placed at it: the jaw body's XML rest pose equals the ghost's, - # so the pivot lives in exactly one place. + # are the retargeter's contract rather than this app's. closedness = float(result[GRIPPER_COMMAND_KEY][0]) - angle = _TRIGGER_RELEASED_RAD + closedness * ( - _TRIGGER_SQUEEZED_RAD - _TRIGGER_RELEASED_RAD - ) - q_hinge = np.empty(4) - mujoco.mju_axisAngle2Quat(q_hinge, _TRIGGER_HINGE_AXIS, angle) - q_jaw = np.empty(4) - mujoco.mju_mulQuat(q_jaw, q_body, q_hinge) - - # Where the jaw body's origin lands: rotating the ghost frame about the - # hinge maps 0 to (pivot - R_hinge . pivot). - swung = np.empty(3) - mujoco.mju_rotVecQuat(swung, _TRIGGER_HINGE_POS, q_hinge) - offset = np.empty(3) - mujoco.mju_rotVecQuat(offset, _TRIGGER_HINGE_POS - swung, q_body) - - data.mocap_pos[ghost.jaw] = p_body + offset - data.mocap_quat[ghost.jaw] = q_jaw + for part, row in zip(robot.parts, ghost.parts): + value = part.released + closedness * (part.squeezed - part.released) + if part.kind is PartKind.HINGE: + # Rotated ABOUT the pivot, not placed at it: the part's XML rest pose + # equals the root's, so the pivot lives in exactly one place. Where + # the origin lands: rotating the ghost frame about the pivot maps 0 + # to (pivot - R . pivot). + q_local = np.empty(4) + mujoco.mju_axisAngle2Quat(q_local, part.axis, value) + swung = np.empty(3) + mujoco.mju_rotVecQuat(swung, part.pivot, q_local) + local_offset = part.pivot - swung + q_part = np.empty(4) + mujoco.mju_mulQuat(q_part, q_body, q_local) + else: # PartKind.SLIDE + local_offset = part.axis * value + q_part = q_body + + offset = np.empty(3) + mujoco.mju_rotVecQuat(offset, local_offset, q_body) + data.mocap_pos[row] = p_body + offset + data.mocap_quat[row] = q_part def _frame_clock(info) -> float | None: @@ -399,8 +326,8 @@ def _frame_clock(info) -> float | None: return info.predicted_display_time / 1e9 -def run() -> int: - model = mujoco.MjModel.from_xml_path(str(DEFAULT_SCENE)) +def run(robot: Robot) -> int: + model = mujoco.MjModel.from_xml_path(str(robot.scene)) data = mujoco.MjData(model) # Order is load-bearing: build the pipeline, aggregate the OpenXR extensions @@ -448,17 +375,16 @@ def run() -> int: model_address=model._address, ) - _log_startup(resolution) + _log_startup(robot, resolution) # After the startup block, so its line reads as part of the same report. - ghost = _resolve_ghost(model) + ghost = _resolve_ghost(model, robot) LOG.info( - "leader ghost: bound to mocap %d (body) / %d (trigger); trigger driven by " - "SO101GripperRetargeter, %.0f deg released to %.0f deg squeezed", + "ghost: bound to mocap %d (body) / %s (moving); %s, driven by the " + "controller trigger through SO101GripperRetargeter", ghost.body, - ghost.jaw, - math.degrees(_TRIGGER_RELEASED_RAD), - math.degrees(_TRIGGER_SQUEEZED_RAD), + ", ".join(str(row) for row in ghost.parts), + robot.drive, ) oxr = viz_session.get_oxr_handles() @@ -474,7 +400,9 @@ def run() -> int: oxr_handles=OpenXRSessionHandles(*oxr), ) with TeleopSession(teleop_config) as teleop_session: - _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) + _loop( + viz_session, layer, renderer, model, data, teleop_session, ghost, robot + ) finally: # The renderer borrows viz_session's device: it must go first. if renderer is not None: @@ -483,7 +411,9 @@ def run() -> int: return 0 -def _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) -> None: +def _loop( + viz_session, layer, renderer, model, data, teleop_session, ghost, robot +) -> None: view_count = renderer.view_count previous_clock: float | None = None # Fixed-step accumulator. NOT reset or drained on a non-render frame: the @@ -511,7 +441,7 @@ def _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) -> N will_step = accumulator >= model.opt.timestep if will_step or info.should_render: result = teleop_session.step() - _update_ghost(data, ghost, result) + _update_ghost(data, ghost, robot, result) steps = 0 while accumulator >= model.opt.timestep and steps < 64: @@ -568,6 +498,14 @@ def main(argv: list[str]) -> int: parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter ) + parser.add_argument( + "--robot", + choices=sorted(ROBOTS), + default=DEFAULT_ROBOT, + help="Which gripper ghost to draw. " + + "; ".join(f"{k}: {r.description}" for k, r in sorted(ROBOTS.items())) + + f" (default: {DEFAULT_ROBOT})", + ) parser.add_argument("--verbose", action="store_true", help="Debug-level logging.") CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) @@ -577,14 +515,19 @@ def main(argv: list[str]) -> int: format="[mujoco_xr] %(message)s", ) + robot = ROBOTS[args.robot] + # Before launch_context, which starts a runtime process on entry and tears # it down on exit: checked here, an unfetched checkout says so plainly - # instead of landing buried in the runtime's own startup logging. - missing = _missing_leader_assets() + # instead of landing buried in the runtime's own startup logging. Only the + # selected robot's meshes are needed, so the other one's absence is not an + # error. + missing = robot.missing_meshes() if missing: raise SystemExit( - f"mujoco_xr: the leader gripper meshes are not fetched ({', '.join(missing)}).\n" - f" Run {FETCH_SCRIPT} from the repository root, then reinstall:\n" + f"mujoco_xr: the {robot.description} meshes are not fetched " + f"({', '.join(missing)}).\n" + f" Run {robot.fetch_script} from the repository root, then reinstall:\n" " uv pip install --reinstall-package isaacteleop-examples-mujoco-xr " "./examples/mujoco_xr" ) @@ -593,7 +536,7 @@ def main(argv: list[str]) -> int: if launcher is not None: LOG.info("CloudXR runtime started (WSS log: %s)", launcher.wss_log_path) try: - return run() + return run(robot) except KeyboardInterrupt: LOG.info("interrupted") return 0 diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml index a7cc98c70..96e04f7a5 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml @@ -4,7 +4,7 @@ SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All SPDX-License-Identifier: Apache-2.0 The leader gripper ghost: an SO-101 leader gripper locked to the operator's -controller grip pose, and the whole of the shipped scene. It shows the grip +controller grip pose, and the whole of `--robot=so101`. It shows the grip calibration in app.py -- whether the tool sits in the hand the way a hand holds one. It cannot show cpp/frames.hpp: those constants place it and are undone when the renderer folds it back into the XR reference space. diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot/rebot_gripper.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot/rebot_gripper.xml new file mode 100644 index 000000000..4dbd4be7e --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot/rebot_gripper.xml @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot_scene.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot_scene.xml new file mode 100644 index 000000000..b9a78a1ef --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/rebot_scene.xml @@ -0,0 +1,17 @@ + + + + + diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/so101_scene.xml similarity index 85% rename from examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml rename to examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/so101_scene.xml index e3897e8f9..dddf0fe87 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/so101_scene.xml @@ -3,9 +3,9 @@ SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. SPDX-License-Identifier: Apache-2.0 -The shipped scene: the SO-101 leader gripper ghost, and nothing else. No ground -plane and no static furniture -- this is an AR scene and passthrough is the -background. +`--robot=so101`, the default: the SO-101 leader gripper ghost, and nothing else. +No ground plane and no static furniture -- this is an AR scene and passthrough is +the background. What a ghost-only scene cannot show: the ghost is placed from the controller pose through mj_from_xr and rendered back through xr_from_mj, so both diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/robots.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/robots.py new file mode 100644 index 000000000..f665c76d6 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/robots.py @@ -0,0 +1,240 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The ghost catalogue: one entry per ``--robot`` value. + +Everything a gripper ghost needs that is not shared machinery -- its scene, its +fetched meshes, its mocap bodies, how each moving part is driven by one +closedness scalar, and where it sits on the operator's hand. app.py holds the +machinery and reads it from here, so adding a robot is a new ``Robot`` and its +two authored files (a scene and a mesh wrapper) rather than a branch in the +frame loop. + +Both ghosts are driven by ONE scalar in ``[0, 1]``: 0 released, 1 squeezed. A +part is a hinge (a lever swinging about a pivot) or a slide (a jaw translating +along an axis); those two cases are the whole of ``app._update_ghost``. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import StrEnum +from pathlib import Path + +import mujoco +import numpy as np + +# Package data, so paths resolve identically from the wheel and the source tree. +# Must stay absolute: a scene s a fragment in a subdirectory, and on +# mujoco 3.11.0 a relative model path mis-composes that fragment's mesh paths +# and fails naming a file that is right there on disk. +_ASSETS = Path(__file__).parent / "assets" + + +class PartKind(StrEnum): + """The two ways one closedness scalar can move a part. + + These are the whole of ``app._update_ghost``. A third would need a branch + there, which is what ``test_every_robot_declares_its_own_assets`` guards by + enumerating the members a robot is allowed to use. + """ + + HINGE = "hinge" + """A lever swinging about a pivot; the value is an angle in radians.""" + SLIDE = "slide" + """A jaw translating along an axis; the value is a displacement in metres.""" + + +def quat_from_euler_deg(angles_deg) -> np.ndarray: + """Intrinsic X-then-Y-then-Z degrees -> a wxyz quaternion. + + Right-multiplication is what makes it intrinsic, and is the convention + MuJoCo's `euler=` uses. Spelled out rather than calling mju_euler2Quat so + the sequence is visible at the point of use. + """ + quat = np.array((1.0, 0.0, 0.0, 0.0)) + for axis, angle in zip(np.eye(3), angles_deg): + step = np.empty(4) + mujoco.mju_axisAngle2Quat(step, axis, math.radians(angle)) + composed = np.empty(4) + mujoco.mju_mulQuat(composed, quat, step) + quat = composed + return quat + + +@dataclass(frozen=True) +class Part: + """One moving mocap body, driven from closedness by a single scalar. + + ``released`` / ``squeezed`` are the joint values at closedness 0 and 1, in + the unit ``kind`` names. Both are expressed in the ghost root's frame, and + every part's XML rest pose equals the root's, so a pivot lives in exactly one + place (see the ghost fragments). + """ + + body: str + kind: PartKind + axis: np.ndarray + released: float + squeezed: float + # Hinge only; a slide has no pivot and leaves this None. + pivot: np.ndarray | None = None + + +@dataclass(frozen=True) +class Robot: + """One selectable ghost.""" + + key: str + description: str + scene: Path + assets: Path + meshes: tuple[str, ...] + fetch_script: str + body: str + parts: tuple[Part, ...] + # How the ghost sits on the hand. `pos` is in the GRIP frame and `quat` + # right-multiplies the grip orientation; see app._update_ghost. + pos_grip_from_ghost: np.ndarray + quat_grip_from_ghost: np.ndarray + # What drives the jaw, for the startup log. The node itself is shared. + drive: str + + def missing_meshes(self) -> list[str]: + """Names of the fetched meshes that are not on disk. Empty when fetched.""" + return [n for n in self.meshes if not (self.assets / n).is_file()] + + +# ── SO-101 leader gripper ────────────────────────────────────────────────── +# The handheld leader device itself, so the ghost is the tool the operator's +# fist is actually closed around. +# +# The trigger hinge is the follower's `gripper` revolute joint, from SO-ARM100's +# so101_new_calib.urdf: origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 0 0", +# axis "0 0 1". The right source even for the LEADER's trigger, which is mounted +# in the follower's moving-jaw slot and shares the hinge. The axis below is that +# "0 0 1" carried through the joint frame's 90-degree roll. +# +# Do not re-derive either from the meshes: a pivot from the nearest +# trigger-to-shank vertex pair and an axis from the grip frame both look right at +# the joint's zero and are wrong by the far end of its travel. +# +# The travel is the URDF joint's own: `upper="1.74533"` is 100.0 degrees, and +# squeezed is its authored zero. A released end short of that does not read as an +# OPEN gripper on a headset, which is the only place this can be judged. Do not +# extend to the joint's lower limit (-10 deg): that end swings the lever 0.4 mm +# into the servo. The tightest pass across 0..100 is 2.1 mm, at the squeezed end. +_SO101_TRIGGER = Part( + body="leader_ghost_jaw", + kind=PartKind.HINGE, + axis=np.array((0.0, -1.0, 0.0)), + pivot=np.array((0.0202, 0.0188, -0.0234)), # metres, ghost frame + released=math.radians(100.0), # closedness 0, jaw wide open + squeezed=0.0, # closedness 1, tucked to the authored pose +) + +# Where the ghost sits on the hand. MEASURED ON A HEADSET, not derived: this is +# a claim about a hand holding a CONTROLLER, so do not re-derive it from the +# mesh -- a model assuming the hand passes through the handle loop puts the loop +# centroid 56 mm from the palm. +# +# Euler degrees, intrinsic XYZ, i.e. MuJoCo's `euler=` (pinned by a test). To +# re-tune, change one angle and reinstall: Rz spins the gripper about its long +# axis, Rx/Ry tilt it, the position slides it along the grip axes. No test +# asserts a posture, so re-tuning cannot turn them red. +SO101_EULER_GRIP_FROM_GHOST_DEG = (60, 180, 270) + +SO101 = Robot( + key="so101", + description="SO-101 leader gripper", + scene=_ASSETS / "so101_scene.xml", + assets=_ASSETS / "leader", + meshes=( + "Wrist_Roll_SO101.stl", + "Trigger_SO101.stl", + "Handle_SO101.stl", + "STS3215_03a.stl", + ), + fetch_script="examples/mujoco_xr/scripts/fetch-so-arm.sh", + body="leader_ghost", + parts=(_SO101_TRIGGER,), + pos_grip_from_ghost=np.array((0.0, 0.02, -0.025)), + quat_grip_from_ghost=quat_from_euler_deg(SO101_EULER_GRIP_FROM_GHOST_DEG), + drive="trigger hinge, 100 deg released to 0 deg squeezed", +) + + +# ── reBot DevArm gripper ─────────────────────────────────────────────────── +# The FOLLOWER's parallel jaw, not a leader handle: the reBot leader is a +# back-driven arm on a table, so what the controller commands is this. Two jaws +# on one rack, so both parts read the same closedness and travel together. +# +# From Seeed's own 00-arm-rs_asm-v3.urdf. joint_left / joint_right sit at +# xyz="-0.041939 -+7.3385e-05 0" with rpy="+-1.5708 -1.5708 0" and axis "0 0 1", +# which carries to -+y in the gripper_end frame -- so positive travel SEPARATES +# the jaws and the joints' authored zero is the CLOSED end (fingertips 0.15 mm +# apart). That is the opposite polarity to the SO-101 trigger above. +# +# Travel is joint_left's `upper="0.05"`, taken for BOTH jaws. Upstream disagrees +# with itself -- joint_right says 0.0715 for the same rack-driven pair -- and +# 0.05 is the one the geometry supports: at 0.05 the carriage's outer edge stops +# 3.4 mm inside the end of the cnc7 rail plate, and at 0.0715 it hangs 18.1 mm +# past it. 0.05 per jaw is a 100 mm opening. +_REBOT_TRAVEL_M = 0.05 +_REBOT_JAWS = tuple( + Part( + body=f"rebot_ghost_{side}", + kind=PartKind.SLIDE, + axis=np.array((0.0, sign, 0.0)), + released=_REBOT_TRAVEL_M, # closedness 0, jaws wide open + squeezed=0.0, # closedness 1, fingertips together + ) + for side, sign in (("left", -1.0), ("right", 1.0)) +) + +# A CONVENTION, not a headset measurement, and that is the difference from the +# SO-101 above: nobody holds this gripper, so there is no fist-on-a-handle claim +# to measure. It is placed by the OpenXR grip frame's own definition -- +X into +# the palm, -Z forward through the tube-shaped fist (where a pen tip would +# point), +Y out of the fist toward the thumb: +# +# ghost +x (the approach axis, jaws reaching forward) -> grip -Z +# ghost +-y (the jaw opening axis) -> grip +-X, so the jaws +# open the way the index +# finger squeezes +# +# which is exactly Rx(90) Ry(0) Rz(270). +REBOT_EULER_GRIP_FROM_GHOST_DEG = (90, 0, 270) + +# Slides the fist onto the gripper's drive motor rather than onto the link +# origin, which is out at the fingertips. Derived: motor_7 spans x in +# [-0.1572, -0.0927] in the ghost frame, so its centre is at -0.125, and +# Rz(270)-through-Rx(90) carries a ghost +x offset onto grip -Z. +REBOT_POS_GRIP_FROM_GHOST = np.array((0.0, 0.0, -0.125)) + +REBOT = Robot( + key="rebot", + description="reBot DevArm gripper", + scene=_ASSETS / "rebot_scene.xml", + assets=_ASSETS / "rebot", + meshes=( + "pla7_green.STL", + "cnc7.STL", + "motor_7.STL", + "pla_left.STL", + "cnc_left.STL", + "pla_right.STL", + "cnc_right.STL", + ), + fetch_script="examples/mujoco_xr/scripts/fetch-rebot-arm.sh", + body="rebot_ghost", + parts=_REBOT_JAWS, + pos_grip_from_ghost=REBOT_POS_GRIP_FROM_GHOST, + quat_grip_from_ghost=quat_from_euler_deg(REBOT_EULER_GRIP_FROM_GHOST_DEG), + drive=f"two rack-driven jaws, {_REBOT_TRAVEL_M * 2000:.0f} mm open to closed", +) + + +ROBOTS = {robot.key: robot for robot in (SO101, REBOT)} +DEFAULT_ROBOT = SO101.key diff --git a/examples/mujoco_xr/scripts/fetch-rebot-arm.sh b/examples/mujoco_xr/scripts/fetch-rebot-arm.sh new file mode 100755 index 000000000..abe16f538 --- /dev/null +++ b/examples/mujoco_xr/scripts/fetch-rebot-arm.sh @@ -0,0 +1,76 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Fetches the reBot DevArm gripper assets `--robot=rebot` draws. Same contract as +# fetch-so-arm.sh beside it: nothing calls this at build time, the files land in +# package data, and the app fails at startup naming this script when they are +# absent. +# +# Fetch, then install: +# uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr +# +# TODO: the download loop below is a copy of the one in fetch-so-arm.sh -- extract +# it into a shared helper once a third robot or asset set needs it. +set -euo pipefail + +# The pin. Everything below is reproducible from this one line; bump it and the +# checksums together or the script refuses the download. +COMMIT="e56b035e335b2e8b8b333ed94860ac9e936b67ba" +REPO="Seeed-Projects/reBotArm_control_py" +SUBDIR="urdf/00-arm-rs_asm-v3" + +DEST="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)/python/isaacteleop_examples/mujoco_xr/assets/rebot" + +# upstream path local name sha256 +# +# Seven meshes, which is the gripper_end link and both jaws -- the rest of the +# arm is not drawn. meshes/gripper_end.STL is deliberately absent: it is 4 MB +# that the URDF's gripper_end link does not reference. +# +# The URDF is not decoration: it is where rebot_gripper.xml's two jaw transforms +# and robots.py's travel come from, and having it on disk is what lets +# test_ghost.py check them against their source. +# +# README.md stands in for the LICENSE this repository does not have. Upstream +# declares MIT there ("This project is open source under the MIT License"); the +# hardware is Seeed-Projects/reBot-DevArm, CERN-OHL-W-2.0. +ASSETS=( + "${SUBDIR}/meshes/pla7_green.STL pla7_green.STL 00b3c3d51f6f756ab96fc65a229610b952ce4a8be927e2ec404bacbcf43f4afc" + "${SUBDIR}/meshes/cnc7.STL cnc7.STL 2d6088744c7d5195fc3f78d930dba9a9843ad208b2a6243fade1aaf17fecc782" + "${SUBDIR}/meshes/motor_7.STL motor_7.STL 4e019eae9e44376b9d876eae995535bcca7761e4083a3ed0d8705d25cffa9dc1" + "${SUBDIR}/meshes/pla_left.STL pla_left.STL 0e774ab0bf2975792982adf646465c7ed1ff35b9ed0ad9bf3fb14c9d7480ebef" + "${SUBDIR}/meshes/cnc_left.STL cnc_left.STL 8f8ed951fad05dd5e3fec04f697864a64e394db9138d72fdfa62f8f047388aa6" + "${SUBDIR}/meshes/pla_right.STL pla_right.STL 43556446abbbecd847bb09a4dc13d4ef2aa266a9ba904e5ac9ccff89a3c8280d" + "${SUBDIR}/meshes/cnc_right.STL cnc_right.STL d7be82297465675b19b5651f3e25c5bab82d33a20bab2aa48ddffbbcf8c65072" + "${SUBDIR}/urdf/00-arm-rs_asm-v3.urdf 00-arm-rs_asm-v3.urdf 2012b5aa3b58878109cb9e3c5deef919a87bd09a67d561662b2904a30dd4397e" + "README.md README.md 52d59f12192379b5b4b2dadb712b8b0dd48431f3bb5ae1152ea7e05afd7055a0" +) + +mkdir -p "$DEST" +echo "Fetching reBot DevArm assets at ${COMMIT:0:12} into ${DEST}" + +for entry in "${ASSETS[@]}"; do + read -r remote local sha <<<"$entry" + target="${DEST}/${local}" + if [[ -f "$target" ]] && echo "${sha} ${target}" | sha256sum --check --status; then + echo " ok ${local}" + continue + fi + url="https://raw.githubusercontent.com/${REPO}/${COMMIT}/${remote}" + echo " fetching ${local}" + curl -fsSL "$url" -o "${target}.part" + # A raw.githubusercontent path is not immutable in practice, and a silently + # substituted mesh renders as a broken gripper rather than an error. + if ! echo "${sha} ${target}.part" | sha256sum --check --status; then + rm -f "${target}.part" + echo "ERROR: checksum mismatch for ${remote}." >&2 + echo " Upstream changed, or COMMIT and the hashes above disagree." >&2 + exit 1 + fi + mv "${target}.part" "$target" +done + +echo +echo "Done. These are package data, so install before running:" +echo " uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr" diff --git a/examples/mujoco_xr/scripts/fetch-so-arm.sh b/examples/mujoco_xr/scripts/fetch-so-arm.sh index adc98916f..8211b74b5 100755 --- a/examples/mujoco_xr/scripts/fetch-so-arm.sh +++ b/examples/mujoco_xr/scripts/fetch-so-arm.sh @@ -12,6 +12,9 @@ # Fetch, then install. The files land in package data, so they only reach # site-packages on the next `uv pip install ./examples/mujoco_xr` -- skip the # reinstall and the ghost works from the source tree and fails from the wheel. +# +# TODO: the download loop below is a copy of the one in fetch-rebot-arm.sh -- +# extract it into a shared helper once a third robot or asset set needs it. set -euo pipefail # The pin. Everything below is reproducible from this one line; bump it and the diff --git a/examples/mujoco_xr/tests/test_app_helpers.py b/examples/mujoco_xr/tests/test_app_helpers.py index 6ca92b9d8..87633d5fe 100644 --- a/examples/mujoco_xr/tests/test_app_helpers.py +++ b/examples/mujoco_xr/tests/test_app_helpers.py @@ -3,11 +3,14 @@ """Pure helpers from the app that guard against silent-corruption bugs.""" +import dataclasses + import pytest app = pytest.importorskip( "isaacteleop_examples.mujoco_xr.app", reason="isaacteleop is not on PYTHONPATH" ) +robots = pytest.importorskip("isaacteleop_examples.mujoco_xr.robots") @pytest.mark.parametrize( @@ -74,6 +77,33 @@ def test_assert_projection_rejects_a_lost_y_flip(): app._assert_projection(flipped, app.NEAR_Z, app.FAR_Z) +def test_an_unknown_robot_is_rejected_before_anything_starts(): + """argparse's own gate, and it has to fire before the CloudXR runtime does.""" + with pytest.raises(SystemExit) as exit_info: + app.main(["mujoco_xr", "--robot", "not-a-robot"]) + assert exit_info.value.code == 2 + + +def test_the_unfetched_message_names_the_SELECTED_robots_script(monkeypatch, tmp_path): + """One robot's meshes must not satisfy the check for the other's. + + The check runs before CloudXRLauncher.launch_context, so getting it wrong + means the failure lands buried in a started runtime's own logging instead. + """ + unfetched = dataclasses.replace( + robots.SO101, + key="unfetched", + assets=tmp_path, + fetch_script="scripts/fetch-nothing.sh", + ) + monkeypatch.setattr(app, "ROBOTS", {**app.ROBOTS, "unfetched": unfetched}) + with pytest.raises(SystemExit) as exit_info: + app.main(["mujoco_xr", "--robot", "unfetched"]) + message = str(exit_info.value) + assert "fetch-nothing.sh" in message + assert robots.SO101.meshes[0] in message + + def test_assert_projection_rejects_reverse_z(): from isaacteleop_examples.mujoco_xr import _mujoco_xr diff --git a/examples/mujoco_xr/tests/test_ghost.py b/examples/mujoco_xr/tests/test_ghost.py index c9006dcd3..b7ecc32db 100644 --- a/examples/mujoco_xr/tests/test_ghost.py +++ b/examples/mujoco_xr/tests/test_ghost.py @@ -1,11 +1,16 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""The leader-gripper ghost: the overlay, its geometry, and when it is written. +"""The gripper ghosts: the overlay, its geometry, and when it is written. -Everything here is headless. The one thing it cannot check is what the ghost +Everything here is headless. The one thing it cannot check is what a ghost looks like through a headset, which is also the only thing that can settle the -two residual risks named in ``assets/leader/leader_gripper.xml``. +residual risks named in the two gripper fragments under ``assets/``. + +The generic assertions run against every entry in ``robots.ROBOTS``; the +per-robot ones live in their own sections below, because what makes an SO-101 +leader gripper right (a lever that clears the body) and what makes a reBot +gripper right (two jaws that meet) are different claims. """ import math @@ -18,6 +23,7 @@ "isaacteleop_examples.mujoco_xr.app", reason="isaacteleop is not on PYTHONPATH", ) +robots = pytest.importorskip("isaacteleop_examples.mujoco_xr.robots") _mujoco_xr = pytest.importorskip("isaacteleop_examples.mujoco_xr._mujoco_xr") mujoco = pytest.importorskip("mujoco") @@ -25,7 +31,10 @@ ControllerInputIndex, ) -GHOST_GEOMS = ( +SO101 = robots.SO101 +REBOT = robots.REBOT + +SO101_GEOMS = ( "leader_ghost_wrist_roll", "leader_ghost_motor", "leader_ghost_trigger", @@ -33,18 +42,25 @@ ) -def _default_scene(): - """The shipped default scene, which is the one that includes the ghost. +@pytest.fixture(params=sorted(robots.ROBOTS), ids=sorted(robots.ROBOTS)) +def robot(request): + """Every catalogue entry, for the assertions that hold of all of them.""" + return robots.ROBOTS[request.param] + - Skips on an unfetched checkout: the meshes come from - scripts/fetch-so-arm.sh, and saying so beats an "Error opening file". +def _model(robot): + """The robot's shipped scene, compiled. + + Skips on an unfetched checkout: the meshes come from the robot's fetch + script, and saying so beats an "Error opening file". """ - missing = app._missing_leader_assets() + missing = robot.missing_meshes() if missing: pytest.skip( - f"leader meshes not fetched ({', '.join(missing)}); run {app.FETCH_SCRIPT}" + f"{robot.key} meshes not fetched ({', '.join(missing)}); " + f"run {robot.fetch_script}" ) - return mujoco.MjModel.from_xml_path(str(app.DEFAULT_SCENE)) + return mujoco.MjModel.from_xml_path(str(robot.scene)) def _scene(model, data): @@ -60,6 +76,18 @@ def _scene(model, data): return scene +def _ghost_bodies(model, robot): + return [ + mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, n) + for n in (robot.body, *(part.body for part in robot.parts)) + ] + + +def _ghost_geoms(model, robot): + bodies = set(_ghost_bodies(model, robot)) + return [g for g in range(model.ngeom) if model.geom_bodyid[g] in bodies] + + def _geom_verts_world(model, data, name): gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) mesh = model.geom_dataid[gid] @@ -79,6 +107,56 @@ def _nearest_gap(a, b, stride=7, block=200): return best +def _mat(quat): + rot = np.empty(9) + mujoco.mju_quat2Mat(rot, np.asarray(quat, dtype=float)) + return rot.reshape(3, 3) + + +def _authored_geom_frame(model, gid): + """A mesh geom's transform as the XML wrote it, in its body's frame. + + MuJoCo rewrites every mesh into its inertial frame and folds that into + geom_pos / geom_quat, so those fields alone are NOT what the XML says -- + reading them raw is off by 18 to 37 mm and 74 to 132 degrees on the reBot + jaws. Undoing it needs mesh_pos / mesh_quat, which is the same trap the + SO-101 derivation documents in README.md. + """ + mesh = model.geom_dataid[gid] + rot = _mat(model.geom_quat[gid]) @ _mat(model.mesh_quat[mesh]).T + return rot, np.array(model.geom_pos[gid]) - rot @ np.array(model.mesh_pos[mesh]) + + +def _rpy_to_mat(roll, pitch, yaw): + """URDF `rpy`: fixed-axis Rz * Ry * Rx. + + Not MuJoCo's `euler=`, which is intrinsic Rx * Ry * Rz. The two agree only + when at most one angle is non-zero, which is exactly why the reBot jaw + origins are written as quaternions rather than copied across. + """ + out = np.eye(3) + for axis, angle in zip(np.eye(3), (roll, pitch, yaw)): + rot = np.empty(9) + quat = np.empty(4) + mujoco.mju_axisAngle2Quat(quat, axis, angle) + mujoco.mju_quat2Mat(rot, quat) + out = rot.reshape(3, 3) @ out + return out + + +def _urdf_joint(path, name): + joint = next( + j for j in ElementTree.parse(path).iter("joint") if j.get("name") == name + ) + origin = joint.find("origin") + return dict( + xyz=np.array([float(v) for v in origin.get("xyz").split()]), + rot=_rpy_to_mat(*(float(v) for v in origin.get("rpy").split())), + axis=np.array([float(v) for v in joint.find("axis").get("xyz").split()]), + limit=joint.find("limit"), + ) + + # --------------------------------------------------------------------------- # A pipeline step result, stubbed. ``app._update_ghost`` reads exactly three # fields through the mapping protocol, and supplying them here rather than @@ -123,19 +201,77 @@ def _result(controller, closedness=0.0): } +def _drive(model, data, robot, closedness, controller=None): + """Write the ghost at one closedness and return the compiled scene.""" + app._update_ghost( + data, + app._resolve_ghost(model, robot), + robot, + _result(controller or _Controller(True, (0.0, 1.2, -0.5)), closedness), + ) + mujoco.mj_forward(model, data) + return data + + +def _part_value(data, ghost, robot, index): + """Recover a part's joint value from the mocap rows the app wrote. + + The inverse of ``app._update_ghost``: a hinge is read back as a signed angle + about its own axis, a slide as a displacement along its own axis. + """ + part = robot.parts[index] + row = ghost.parts[index] + quat_body = np.array(data.mocap_quat[ghost.body]) + if part.kind is robots.PartKind.HINGE: + inverse, local = np.empty(4), np.empty(4) + mujoco.mju_negQuat(inverse, quat_body) + mujoco.mju_mulQuat(local, inverse, np.array(data.mocap_quat[row])) + # Signed against the axis, so a rotation the wrong way reads negative + # rather than folding onto the same magnitude. + turn = 2.0 * math.atan2(float(np.linalg.norm(local[1:])), float(local[0])) + return -turn if float(np.dot(local[1:], part.axis)) < 0 else turn + offset = np.array(data.mocap_pos[row]) - np.array(data.mocap_pos[ghost.body]) + return float(_mat(quat_body).T @ offset @ part.axis) + + +# --------------------------------------------------------------------------- +# The catalogue itself. +# --------------------------------------------------------------------------- + + +def test_every_robot_declares_its_own_assets(robot): + """A typo in one entry must not silently borrow the other one's meshes.""" + assert robot.scene.is_file(), f"{robot.key}: {robot.scene} is not shipped" + assert robot.meshes and robot.parts + assert all( + part.kind in (robots.PartKind.HINGE, robots.PartKind.SLIDE) + for part in robot.parts + ) + others = [r for r in robots.ROBOTS.values() if r is not robot] + assert all(robot.assets != o.assets and robot.scene != o.scene for o in others) + # A HINGE needs its pivot; a SLIDE must not carry one, since nothing reads it. + for part in robot.parts: + assert (part.pivot is not None) == (part.kind is robots.PartKind.HINGE) + assert np.isclose(np.linalg.norm(robot.quat_grip_from_ghost), 1.0) + + +def test_the_default_robot_is_in_the_catalogue(): + assert robots.DEFAULT_ROBOT in robots.ROBOTS + + # --------------------------------------------------------------------------- # The transparency design. This is the claim that replaced a second Vulkan # pipeline, so it is the one that has to be asserted rather than believed. # --------------------------------------------------------------------------- -def test_the_ghost_is_opaque_and_collides_with_nothing(): +def test_the_ghost_is_opaque_and_collides_with_nothing(robot): """Opaque, so draw order and the blending risks stop mattering. Read off the SCENE geom: model.geom_rgba still holds MuJoCo's default, so asserting that would pass on a translucent ghost too. """ - model = _default_scene() + model = _model(robot) data = mujoco.MjData(model) scene = _scene(model, data) by_objid = { @@ -143,8 +279,9 @@ def test_the_ghost_is_opaque_and_collides_with_nothing(): for i in range(scene.ngeom) if scene.geoms[i].objtype == mujoco.mjtObj.mjOBJ_GEOM } - for name in GHOST_GEOMS: - gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + geoms = _ghost_geoms(model, robot) + assert geoms, f"{robot.key}: the scene draws nothing" + for gid in geoms: assert scene.geoms[by_objid[gid]].rgba[3] == pytest.approx(1.0) # Contact would let the operator's hand shove scene content around, # which is the opposite of an overlay. @@ -154,80 +291,28 @@ def test_the_ghost_is_opaque_and_collides_with_nothing(): # geom is checked where it lands. A mocap body is kinematic either way, but a # non-zero mass here would change the model's total and, through it, any # inertia-derived diagnostic somebody later writes. - for body_name in (app.GHOST_BODY, app.GHOST_JAW_BODY): - body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name) + for body in _ghost_bodies(model, robot): assert model.body_mass[body] == 0.0 -def test_both_ghost_bodies_are_mocap_and_kinematic(): - """Two mocap bodies, no joints, parented to world. +def test_every_ghost_body_is_mocap_and_kinematic(robot): + """Mocap bodies, no joints, parented to world. - The trigger is a second mocap body rather than a jointed child so it can + Each moving part is its own mocap body rather than a jointed child so it can articulate without physics: mj_step integrates gravity into a joint (measured: 0.06 rad over 50 steps). """ - model = _default_scene() - bodies = [ - mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, n) - for n in (app.GHOST_BODY, app.GHOST_JAW_BODY) - ] + model = _model(robot) + bodies = _ghost_bodies(model, robot) assert all(b >= 0 for b in bodies) + assert len(bodies) == 1 + len(robot.parts) for body in bodies: assert model.body_mocapid[body] >= 0 assert model.body_parentid[body] == 0, "a mocap body must be a child of world" - # No joint anywhere on the ghost. A jointed child of a mocap body - # compiles and honours a written qpos, but mj_step then integrates - # gravity into it -- see the comment in leader_gripper.xml. assert model.body_jntnum[body] == 0 -# --------------------------------------------------------------------------- -# The geometry. All three transforms are DERIVED, and this is the derivation -# checking itself. -# --------------------------------------------------------------------------- - - -def test_the_three_leader_parts_form_one_assembly(): - """Sub-mm where the parts bolt, mm of running clearance where one pivots. - - An STL refresh that broke the shared CAD datum opens these gaps rather - than quietly rendering three pieces near each other. - """ - model = _default_scene() - data = mujoco.MjData(model) - mujoco.mj_forward(model, data) - verts = {n: _geom_verts_world(model, data, n) for n in GHOST_GEOMS} - - bolted = _nearest_gap( - verts["leader_ghost_wrist_roll"], verts["leader_ghost_handle"] - ) - assert bolted < 1e-3, f"shank-to-handle gap {bolted * 1000:.2f} mm" - for other in ("leader_ghost_trigger",): - for part in ("leader_ghost_wrist_roll", "leader_ghost_handle"): - gap = _nearest_gap(verts[part], verts[other]) - assert gap < 5e-3, f"{part} to {other} gap {gap * 1000:.2f} mm" - - -def test_the_servo_fills_the_notch_in_the_wrist_bracket(): - """`wrist_roll` is a C-shaped bracket; the servo is what sits in it. - - Asserted as contact plus the size of a real STS3215, which catches the - units trap: this mesh is Menagerie's, in metres, while its neighbours are - print STLs in millimetres. - """ - model = _default_scene() - data = mujoco.MjData(model) - mujoco.mj_forward(model, data) - servo = _geom_verts_world(model, data, "leader_ghost_motor") - bracket = _geom_verts_world(model, data, "leader_ghost_wrist_roll") - assert _nearest_gap(servo, bracket) < 1e-3, "the servo is not seated in the bracket" - extent = np.ptp(servo, axis=0) - assert np.allclose(np.sort(extent), (0.0248, 0.0396, 0.0454), atol=2e-3), ( - f"servo spans {np.round(extent * 1000, 1)} mm -- an STS3215 is 45x25x40" - ) - - -def test_the_renderers_normals_agree_with_the_geometry_they_shade(): +def test_the_renderers_normals_agree_with_the_geometry_they_shade(robot): """Every corner normal must face the same way as its own triangle. The renderer computes these; mjModel's own normals are smeared across each @@ -237,9 +322,9 @@ def test_the_renderers_normals_agree_with_the_geometry_they_shade(): The bound is the crease angle itself: smoothing may tilt a corner normal toward its neighbours, but never past 90 degrees from its own face. """ - model = _default_scene() - for name in ("leader_wrist_roll", "leader_trigger", "leader_handle"): - mesh = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_MESH, name) + model = _model(robot) + for mesh in range(model.nmesh): + name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_MESH, mesh) pos, normal = _mujoco_xr.mesh_triangles(model._address, mesh) pos = np.asarray(pos, dtype=float).reshape(-1, 3, 3) normal = np.asarray(normal, dtype=float).reshape(-1, 3, 3) @@ -256,16 +341,17 @@ def test_the_renderers_normals_agree_with_the_geometry_they_shade(): ) -def test_the_leader_meshes_are_scaled_from_millimetres(): - """`scale="0.001"`, and getting it wrong does not read as "a big mesh". +def test_every_ghost_mesh_is_hand_sized(robot): + """The units trap, from both directions. - The camera ends up inside a 65 m solid. The servo is deliberately absent - from this list: it is authored in metres and carries no scale. + SO-ARM's print STLs are in MILLIMETRES and carry `scale="0.001"`; the servo + beside them and every reBot mesh are in metres and must not. Either mistake + reads as "passthrough broke" rather than as a scale error -- the camera ends + up inside a 65 m solid -- or the part shrinks to microns and vanishes. """ - model = _default_scene() - for name in ("leader_wrist_roll", "leader_trigger", "leader_handle"): - mesh = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_MESH, name) - assert mesh >= 0 + model = _model(robot) + for mesh in range(model.nmesh): + name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_MESH, mesh) adr, num = model.mesh_vertadr[mesh], model.mesh_vertnum[mesh] verts = np.array(model.mesh_vert[adr : adr + num], dtype=float) extent = float(np.ptp(verts, axis=0).max()) @@ -277,7 +363,7 @@ def test_the_leader_meshes_are_scaled_from_millimetres(): # --------------------------------------------------------------------------- -def test_the_ghost_is_rigidly_attached_to_the_grip_frame(): +def test_the_ghost_is_rigidly_attached_to_the_grip_frame(robot): """The contract the calibration must satisfy, whatever its value. The correction is fixed in the GRIPPER's frame, so it right-multiplies. @@ -288,9 +374,9 @@ def test_the_ghost_is_rigidly_attached_to_the_grip_frame(): Asserted as invariance rather than a posture, so tuning the shipped constants on a headset cannot turn it red. """ - model = _default_scene() + model = _model(robot) data = mujoco.MjData(model) - ghost = app._resolve_ghost(model) + ghost = app._resolve_ghost(model, robot) seen = [] for grip_pos, grip_quat_xyzw in ( @@ -299,19 +385,17 @@ def test_the_ghost_is_rigidly_attached_to_the_grip_frame(): ((-0.2, 0.9, -0.8), (0.5, 0.5, 0.5, 0.5)), ): app._update_ghost( - data, ghost, _result(_Controller(True, grip_pos, grip_quat_xyzw)) + data, ghost, robot, _result(_Controller(True, grip_pos, grip_quat_xyzw)) ) q_world_from_grip = np.array(_mujoco_xr.mj_from_xr_quat(list(grip_quat_xyzw))) inverse, relative = np.empty(4), np.empty(4) mujoco.mju_negQuat(inverse, q_world_from_grip) mujoco.mju_mulQuat(relative, inverse, np.array(data.mocap_quat[ghost.body])) - rot = np.empty(9) - mujoco.mju_quat2Mat(rot, q_world_from_grip) offset = ( np.array(data.mocap_pos[ghost.body]) - np.array(_mujoco_xr.mj_from_xr_pos(list(grip_pos))) - ) @ rot.reshape(3, 3) + ) @ _mat(q_world_from_grip) seen.append((relative, offset)) for relative, offset in seen[1:]: @@ -324,53 +408,167 @@ def test_the_ghost_is_rigidly_attached_to_the_grip_frame(): "orientation -- the translation is not being rotated with the grip" ) # And it is the configured correction, not some other rigid attachment. - assert np.allclose(seen[0][0], app._QUAT_GRIP_FROM_GHOST, atol=1e-6) - assert np.allclose(seen[0][1], app._POS_GRIP_FROM_GHOST, atol=1e-6) + assert np.allclose(seen[0][0], robot.quat_grip_from_ghost, atol=1e-6) + assert np.allclose(seen[0][1], robot.pos_grip_from_ghost, atol=1e-6) + + +def test_closedness_drives_every_part_from_released_to_squeezed(robot): + """0 and 1 must land exactly on the catalogue's endpoints, monotonically. + + Recovered from the mocap rows rather than from where a point ends up: over a + large sweep a point on a lever traces an arc, so its position along any fixed + axis rises before it falls. + """ + model = _model(robot) + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model, robot) + + for index, part in enumerate(robot.parts): + values = [] + for closedness in (0.0, 0.25, 0.5, 0.75, 1.0): + _drive(model, data, robot, closedness) + values.append(_part_value(data, ghost, robot, index)) + assert values[0] == pytest.approx(part.released, abs=1e-6) + assert values[-1] == pytest.approx(part.squeezed, abs=1e-6) + step = 1 if part.squeezed > part.released else -1 + assert all(step * (b - a) > 0 for a, b in zip(values, values[1:])), ( + f"{part.body} does not move monotonically: {np.round(values, 5)}" + ) -def test_squeezing_drives_the_jaw_from_released_to_squeezed(): - """Closedness 0..1 must drive the hinge from released to squeezed. +def test_an_untracked_controller_freezes_the_whole_gripper(robot): + """(0, 0, 0) in MuJoCo world is the scene origin -- a legitimate pose. - Asserted on the recovered hinge ANGLE, not on where a point ends up: over - a large sweep a point on the lever traces an arc, so its position along any - fixed axis rises before it falls. + Freezing where it was last seen is the honest rendering of "tracking + lost", and every moving part freezes with the body rather than articulating + on a stale pose. """ - model = _default_scene() + model = _model(robot) data = mujoco.MjData(model) - ghost = app._resolve_ghost(model) - controller = _Controller(True, (0.0, 1.2, -0.5)) - - def hinge_angle_at(closedness): - app._update_ghost(data, ghost, _result(controller, closedness)) - mujoco.mj_forward(model, data) - inverse, hinge = np.empty(4), np.empty(4) - mujoco.mju_negQuat(inverse, np.array(data.mocap_quat[ghost.body])) - mujoco.mju_mulQuat(hinge, inverse, np.array(data.mocap_quat[ghost.jaw])) - # Signed against the hinge axis, so a rotation the wrong way reads - # negative rather than folding onto the same magnitude. - turn = 2.0 * math.atan2(float(np.linalg.norm(hinge[1:])), float(hinge[0])) - if float(np.dot(hinge[1:], app._TRIGGER_HINGE_AXIS)) < 0: - turn = -turn - return turn - - angles = [hinge_angle_at(c) for c in (0.0, 0.25, 0.5, 0.75, 1.0)] - assert angles[0] == pytest.approx(app._TRIGGER_RELEASED_RAD, abs=1e-6) - assert angles[-1] == pytest.approx(app._TRIGGER_SQUEEZED_RAD, abs=1e-6) - assert all(b < a for a, b in zip(angles, angles[1:])), ( - f"squeezing did not close the jaw monotonically: {np.round(angles, 4)}" + ghost = app._resolve_ghost(model, robot) + app._update_ghost( + data, + ghost, + robot, + _result(_Controller(True, (0.2, 1.3, -0.5)), closedness=0.0), ) + seen = data.mocap_pos.copy(), data.mocap_quat.copy() - # And it is big enough to see: the far end of the lever sweeps ~90 mm. - def trigger_at(closedness): - app._update_ghost(data, ghost, _result(controller, closedness)) - mujoco.mj_forward(model, data) - return _geom_verts_world(model, data, "leader_ghost_trigger") + for controller in (_Controller(False, (9.0, 9.0, 9.0)), _NoController()): + for _ in range(3): + app._update_ghost(data, ghost, robot, _result(controller, closedness=1.0)) + assert np.array_equal(data.mocap_pos, seen[0]) + assert np.array_equal(data.mocap_quat, seen[1]) + + +def test_a_scene_without_the_ghost_fragment_is_rejected(robot): + """Every scene must declare its own mocap bodies; say so if one stops.""" + model = mujoco.MjModel.from_xml_string( + '' + ) + with pytest.raises(RuntimeError, match=robot.body): + app._resolve_ghost(model, robot) + + +def test_the_shipped_retargeter_drives_the_jaw_channel(): + """The graph edge itself: trigger -> SO101GripperRetargeter -> combiner key. + + Builds the real pipeline and drives it with synthetic DeviceIO snapshots, + so the key, the indexing and the deadzone are checked against the shipped + retargeter rather than this file's idea of it. Robot-independent: one + pipeline serves every ghost. + """ + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup + from isaacteleop.schema import ( + ControllerInputState, + ControllerPose, + ControllerSnapshot, + ControllerSnapshotTrackedT, + Point, + Pose, + Quaternion, + ) + + def snapshot(trigger): + pose = ControllerPose( + Pose(Point(0.1, 1.2, -0.4), Quaternion(0.0, 0.0, 0.0, 1.0)), True + ) + state = ControllerInputState( + primary_click=False, + secondary_click=False, + thumbstick_click=False, + menu_click=False, + thumbstick_x=0.0, + thumbstick_y=0.0, + squeeze_value=0.0, + trigger_value=trigger, + ) + return ControllerSnapshotTrackedT(ControllerSnapshot(pose, pose, state)) + + pipeline = app._build_pipeline() + spec = ControllersSource(name="controllers").input_spec() + + def closedness(trigger): + inputs = {} + for name in spec: + group = TensorGroup(spec[name]) + group[0] = snapshot(trigger) + inputs[name] = group + out = pipeline.execute_pipeline({"controllers": inputs}) + assert app.GRIPPER_COMMAND_KEY in out + return float(out[app.GRIPPER_COMMAND_KEY][0]) + + assert closedness(0.0) == pytest.approx(0.0) + assert closedness(1.0) == pytest.approx(1.0) + # The retargeter's own released-end deadzone, not this app's: (0.5 - 0.05) / 0.95. + assert closedness(0.5) == pytest.approx(0.4737, abs=1e-4) + + +# --------------------------------------------------------------------------- +# SO-101: the leader gripper's own geometry. All three transforms are DERIVED, +# and this is the derivation checking itself. +# --------------------------------------------------------------------------- - travel = float(np.linalg.norm(trigger_at(1.0) - trigger_at(0.0), axis=1).max()) - # 84.5 mm at the lever tip across the joint's 0..100 degrees; a bound of - # 50 mm is the point below which "released" stops reading as OPEN, which is - # the whole reason the range is the joint's and not a comfortable subset. - assert travel > 0.05, f"the trigger moves {travel * 1000:.1f} mm -- not visible" + +def test_the_three_leader_parts_form_one_assembly(): + """Sub-mm where the parts bolt, mm of running clearance where one pivots. + + An STL refresh that broke the shared CAD datum opens these gaps rather + than quietly rendering three pieces near each other. + """ + model = _model(SO101) + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + verts = {n: _geom_verts_world(model, data, n) for n in SO101_GEOMS} + + bolted = _nearest_gap( + verts["leader_ghost_wrist_roll"], verts["leader_ghost_handle"] + ) + assert bolted < 1e-3, f"shank-to-handle gap {bolted * 1000:.2f} mm" + for other in ("leader_ghost_trigger",): + for part in ("leader_ghost_wrist_roll", "leader_ghost_handle"): + gap = _nearest_gap(verts[part], verts[other]) + assert gap < 5e-3, f"{part} to {other} gap {gap * 1000:.2f} mm" + + +def test_the_servo_fills_the_notch_in_the_wrist_bracket(): + """`wrist_roll` is a C-shaped bracket; the servo is what sits in it. + + Asserted as contact plus the size of a real STS3215, which catches the + units trap: this mesh is Menagerie's, in metres, while its neighbours are + print STLs in millimetres. + """ + model = _model(SO101) + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + servo = _geom_verts_world(model, data, "leader_ghost_motor") + bracket = _geom_verts_world(model, data, "leader_ghost_wrist_roll") + assert _nearest_gap(servo, bracket) < 1e-3, "the servo is not seated in the bracket" + extent = np.ptp(servo, axis=0) + assert np.allclose(np.sort(extent), (0.0248, 0.0396, 0.0454), atol=2e-3), ( + f"servo spans {np.round(extent * 1000, 1)} mm -- an STS3215 is 45x25x40" + ) def test_the_released_end_is_the_urdf_joints_upper_limit(): @@ -379,19 +577,36 @@ def test_the_released_end_is_the_urdf_joints_upper_limit(): Read out of the fetched so101_new_calib.urdf rather than restated, so the constant is checked against its source instead of against itself. """ - urdf = app._LEADER_ASSETS / "so101_new_calib.urdf" + urdf = SO101.assets / "so101_new_calib.urdf" if not urdf.is_file(): - pytest.skip(f"{urdf.name} not fetched; run {app.FETCH_SCRIPT}") - tree = ElementTree.parse(urdf) - joint = next(j for j in tree.iter("joint") if j.get("name") == "gripper") - upper = float(joint.find("limit").get("upper")) - assert app._TRIGGER_RELEASED_RAD == pytest.approx(upper, abs=1e-4) + pytest.skip(f"{urdf.name} not fetched; run {SO101.fetch_script}") + trigger = _urdf_joint(urdf, "gripper") + upper = float(trigger["limit"].get("upper")) + part = SO101.parts[0] + assert part.released == pytest.approx(upper, abs=1e-4) # The other end is the joint's authored zero, NOT its lower limit, which # swings the lever into the servo. - assert app._TRIGGER_SQUEEZED_RAD == 0.0 - assert float(joint.find("limit").get("lower")) == pytest.approx( + assert part.squeezed == 0.0 + assert float(trigger["limit"].get("lower")) == pytest.approx( math.radians(-10.0), abs=1e-4 ) + assert np.allclose(part.pivot, trigger["xyz"], atol=1e-9) + + +def test_the_trigger_moves_far_enough_to_read_as_an_open_gripper(): + """The far end of the lever sweeps ~90 mm across the driven range.""" + model = _model(SO101) + data = mujoco.MjData(model) + + def trigger_at(closedness): + _drive(model, data, SO101, closedness) + return _geom_verts_world(model, data, "leader_ghost_trigger") + + travel = float(np.linalg.norm(trigger_at(1.0) - trigger_at(0.0), axis=1).max()) + # 84.5 mm at the lever tip across the joint's 0..100 degrees; a bound of + # 50 mm is the point below which "released" stops reading as OPEN, which is + # the whole reason the range is the joint's and not a comfortable subset. + assert travel > 0.05, f"the trigger moves {travel * 1000:.1f} mm -- not visible" def test_the_trigger_clears_the_whole_gripper_across_its_driven_range(): @@ -409,9 +624,8 @@ def test_the_trigger_clears_the_whole_gripper_across_its_driven_range(): -10 degree limit closes to 0.4 mm. Nearest-vertex distance cannot go negative, so interpenetration shows up as a small positive number. """ - model = _default_scene() + model = _model(SO101) data = mujoco.MjData(model) - ghost = app._resolve_ghost(model) others = ( "leader_ghost_wrist_roll", "leader_ghost_motor", @@ -421,10 +635,7 @@ def test_the_trigger_clears_the_whole_gripper_across_its_driven_range(): worst = (0.0, "", 1e9) for step in range(9): closedness = step / 8 - app._update_ghost( - data, ghost, _result(_Controller(True, (0.0, 1.2, -0.5)), closedness) - ) - mujoco.mj_forward(model, data) + _drive(model, data, SO101, closedness) trigger = _geom_verts_world(model, data, "leader_ghost_trigger") for part in others: gap = _nearest_gap(trigger, _geom_verts_world(model, data, part)) @@ -436,87 +647,217 @@ def test_the_trigger_clears_the_whole_gripper_across_its_driven_range(): ) -def test_the_shipped_retargeter_drives_the_jaw_channel(): - """The graph edge itself: trigger -> SO101GripperRetargeter -> combiner key. +# --------------------------------------------------------------------------- +# reBot: a parallel jaw, so the claims are different in kind. Nothing here is +# tuned -- the transforms come out of Seeed's URDF and the travel out of the +# rail the carriage runs on. +# --------------------------------------------------------------------------- - Builds the real pipeline and drives it with synthetic DeviceIO snapshots, - so the key, the indexing and the deadzone are checked against the shipped - retargeter rather than this file's idea of it. +_REBOT_JAW_GEOMS = { + "left": ("rebot_ghost_jaw_left", "rebot_ghost_finger_left"), + "right": ("rebot_ghost_jaw_right", "rebot_ghost_finger_right"), +} +_REBOT_BODY_GEOMS = ("rebot_ghost_cover", "rebot_ghost_rail", "rebot_ghost_motor") + + +def _rebot_urdf(): + urdf = REBOT.assets / "00-arm-rs_asm-v3.urdf" + if not urdf.is_file(): + pytest.skip(f"{urdf.name} not fetched; run {REBOT.fetch_script}") + return urdf + + +def _rebot_jaw_verts(model, data, side): + return np.vstack( + [_geom_verts_world(model, data, n) for n in _REBOT_JAW_GEOMS[side]] + ) + + +def _rebot_fingertips(model, data, side, ghost): + """The forward-most 15 mm of a jaw, along the gripper's approach axis. + + What "open" means has to be measured here and not on the whole jaw: the two + carriages stay 14 mm apart behind the fingers at every closedness, so the + nearest-vertex distance between the full assemblies never reports the + opening. """ - from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource - from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup - from isaacteleop.schema import ( - ControllerInputState, - ControllerPose, - ControllerSnapshot, - ControllerSnapshotTrackedT, - Point, - Pose, - Quaternion, + approach = _mat(data.mocap_quat[ghost.body])[:, 0] + verts = _rebot_jaw_verts(model, data, side) + along = verts @ approach + return verts[along > along.max() - 0.015] + + +def test_the_rebot_jaw_frames_are_seeeds_urdf(): + """Both jaw transforms, checked against the file they were read out of. + + This is the test that makes them derived rather than tuned, and it is also + the one that catches the rpy/euler trap: URDF rpy is fixed-axis Rz*Ry*Rx + while MuJoCo `euler=` is intrinsic Rx*Ry*Rz, and these origins have two + non-zero angles, so copying the three numbers across is a different + rotation. The 1e-5 bound is upstream's own rounding -- 1.5708 for pi/2 -- + which the exact quaternions in the fragment do not carry. + """ + urdf = _rebot_urdf() + model = _model(REBOT) + for side, part in zip(("left", "right"), REBOT.parts): + joint = _urdf_joint(urdf, f"joint_{side}") + for name in _REBOT_JAW_GEOMS[side]: + gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + rot, pos = _authored_geom_frame(model, gid) + assert np.allclose(pos, joint["xyz"], atol=1e-9), name + assert np.allclose(rot, joint["rot"], atol=1e-5), name + # The slide axis app.py uses is the URDF axis carried into the ghost frame. + assert np.allclose(part.axis, joint["rot"] @ joint["axis"], atol=1e-5) + # The gripper_end link's own visuals are at the URDF's identity origin, so + # the fragment gives them no pos/quat at all. + for name in _REBOT_BODY_GEOMS: + gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + rot, pos = _authored_geom_frame(model, gid) + assert np.allclose(pos, 0.0, atol=1e-9), name + assert np.allclose(rot, np.eye(3), atol=1e-9), name + + +def test_the_rebot_travel_is_the_limit_the_rail_supports(): + """Upstream disagrees with itself; the rail plate is the tie-breaker. + + joint_left says 0.05 and joint_right says 0.0715 for the same rack-driven + pair, so one of them is an export artifact. At 0.05 the carriage's outer edge + stops 3.4 mm inside the end of the cnc7 rail plate; at 0.0715 it hangs + 18.1 mm past it. That rules out 0.0715 and leaves 0.05 -- a 100 mm opening. + Both upstream numbers are read here rather than restated, so a corrected URDF + fails this instead of drifting. + """ + urdf = _rebot_urdf() + assert float( + _urdf_joint(urdf, "joint_left")["limit"].get("upper") + ) == pytest.approx(REBOT.parts[0].released) + assert float( + _urdf_joint(urdf, "joint_right")["limit"].get("upper") + ) == pytest.approx(0.0715), ( + "upstream agrees with itself now -- re-derive the travel" ) + for part in REBOT.parts: + assert part.squeezed == 0.0, "the URDF's authored zero is the CLOSED end" + assert part.released == REBOT.parts[0].released, "one rack, one travel" - def snapshot(trigger): - pose = ControllerPose( - Pose(Point(0.1, 1.2, -0.4), Quaternion(0.0, 0.0, 0.0, 1.0)), True + model = _model(REBOT) + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model, REBOT) + _drive(model, data, REBOT, 0.0) # fully open + rail = _geom_verts_world(model, data, "rebot_ghost_rail") + for side, part in zip(_REBOT_JAW_GEOMS, REBOT.parts): + # Measured as EXTENT along the slide axis, not as proximity: the + # carriage still touches the plate at 0.0715, it just hangs 18 mm past + # its edge, so a nearest-vertex check passes at both candidate travels + # and would not discriminate. + axis = _mat(data.mocap_quat[ghost.body]) @ part.axis + past = float( + (_rebot_jaw_verts(model, data, side) @ axis).max() - (rail @ axis).max() ) - state = ControllerInputState( - primary_click=False, - secondary_click=False, - thumbstick_click=False, - menu_click=False, - thumbstick_x=0.0, - thumbstick_y=0.0, - squeeze_value=0.0, - trigger_value=trigger, + assert past <= 1e-3, ( + f"the {side} carriage hangs {past * 1000:.1f} mm past the end of the " + "rail plate at the open end -- the travel is longer than the rail" ) - return ControllerSnapshotTrackedT(ControllerSnapshot(pose, pose, state)) - pipeline = app._build_pipeline() - spec = ControllersSource(name="controllers").input_spec() - def closedness(trigger): - inputs = {} - for name in spec: - group = TensorGroup(spec[name]) - group[0] = snapshot(trigger) - inputs[name] = group - out = pipeline.execute_pipeline({"controllers": inputs}) - assert app.GRIPPER_COMMAND_KEY in out - return float(out[app.GRIPPER_COMMAND_KEY][0]) +def test_squeezing_closes_the_rebot_jaws_onto_each_other(): + """Closed means the fingertips MEET, which is the point of a parallel jaw. - assert closedness(0.0) == pytest.approx(0.0) - assert closedness(1.0) == pytest.approx(1.0) - # The retargeter's own released-end deadzone, not this app's: (0.5 - 0.05) / 0.95. - assert closedness(0.5) == pytest.approx(0.4737, abs=1e-4) + Both jaws are driven by one rack, so they must also stay symmetric about the + gripper's own centre plane at every closedness -- a sign flip on one axis + passes a monotonicity check and drives both jaws the same way. + """ + model = _model(REBOT) + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model, REBOT) + + gaps = [] + for step in range(5): + _drive(model, data, REBOT, step / 4) + left = _rebot_jaw_verts(model, data, "left") + right = _rebot_jaw_verts(model, data, "right") + gaps.append( + _nearest_gap( + _rebot_fingertips(model, data, "left", ghost), + _rebot_fingertips(model, data, "right", ghost), + stride=1, + ) + ) + centre = np.array(data.mocap_pos[ghost.body]) + assert ( + abs( + np.linalg.norm(left.mean(0) - centre) + - np.linalg.norm(right.mean(0) - centre) + ) + < 1e-6 + ), "the jaws are not symmetric about the gripper's centre" + + # 0.15 mm as authored; 1 mm is what this subsampled nearest-vertex measure + # can carry, and it is two orders below the open end. + assert gaps[-1] < 1e-3, f"squeezed leaves a {gaps[-1] * 1000:.1f} mm gap" + assert all(b < a for a, b in zip(gaps, gaps[1:])), ( + f"squeezing did not close the jaws monotonically: {np.round(gaps, 4)}" + ) + # Both jaws travel, so the opening is twice the catalogue's -- 100 mm. A + # bound of 50 mm is the point below which "released" stops reading as an OPEN + # gripper on a headset. + assert gaps[0] == pytest.approx(2 * REBOT.parts[0].released, abs=2e-3) + assert gaps[0] > 0.05, f"the jaws open only {gaps[0] * 1000:.1f} mm" -def test_an_untracked_controller_freezes_the_whole_gripper(): - """(0, 0, 0) in MuJoCo world is the scene origin -- a legitimate pose. +def test_the_rebot_jaws_clear_the_gripper_body_across_the_driven_range(): + """A carriage driven into its own housing reads as a broken asset. - Freezing where it was last seen is the honest rendering of "tracking - lost", and the jaw freezes with the body rather than articulating on a - stale pose. + The tightest legitimate pass is 0.25 mm at the open end, where the carriage + runs past a feature on the rail plate; nearest-vertex distance cannot go + negative, so interpenetration shows up as a small positive number. """ - model = _default_scene() + model = _model(REBOT) data = mujoco.MjData(model) - ghost = app._resolve_ghost(model) - app._update_ghost( - data, ghost, _result(_Controller(True, (0.2, 1.3, -0.5)), closedness=0.0) + worst = (0.0, "", 1e9) + for step in range(5): + closedness = step / 4 + _drive(model, data, REBOT, closedness) + for side in _REBOT_JAW_GEOMS: + jaw = _rebot_jaw_verts(model, data, side) + for name in _REBOT_BODY_GEOMS: + gap = _nearest_gap(jaw, _geom_verts_world(model, data, name), stride=11) + if gap < worst[2]: + worst = (closedness, f"{side}/{name}", gap) + assert worst[2] > 0.1e-3, ( + f"the {worst[1]} pair closes to {worst[2] * 1000:.2f} mm at closedness " + f"{worst[0]:.2f} -- the driven range pushes a jaw through the body" ) - seen_body = data.mocap_pos[ghost.body].copy() - seen_jaw = data.mocap_quat[ghost.jaw].copy() - for controller in (_Controller(False, (9.0, 9.0, 9.0)), _NoController()): - for _ in range(3): - app._update_ghost(data, ghost, _result(controller, closedness=1.0)) - assert np.array_equal(data.mocap_pos[ghost.body], seen_body) - assert np.array_equal(data.mocap_quat[ghost.jaw], seen_jaw) +def test_the_rebot_placement_is_the_openxr_grip_convention(): + """A convention, not a headset measurement -- so a test may pin it. -def test_a_scene_without_the_ghost_fragment_is_rejected(): - """The shipped scene must declare both mocap bodies; say so if it stops.""" - model = mujoco.MjModel.from_xml_string( - '' + Nobody holds this gripper: it is the follower's jaw drawn at the hand, so + unlike the SO-101 there is nothing to tune and the placement is the grip + frame's own definition. Two claims: + + ghost +x (approach) -> grip -Z, the pen-tip axis through the fist + ghost +-y (opening) -> grip +-X, the palm normal, so the jaws open the way + the index finger squeezes + + and the fist lands on the drive motor rather than on the link origin, which + is out at the fingertips. + """ + rot = _mat(REBOT.quat_grip_from_ghost) + assert np.allclose(rot[:, 0], (0.0, 0.0, -1.0), atol=1e-9) + assert np.allclose(rot[:, 1], (1.0, 0.0, 0.0), atol=1e-9) + + # And the fist itself: drive the ghost from a known grip pose and measure + # where the motor ends up, rather than re-deriving the offset's algebra here. + model = _model(REBOT) + data = mujoco.MjData(model) + grip_xr = (0.0, 1.2, -0.5) + _drive(model, data, REBOT, 0.0, _Controller(True, grip_xr)) + ghost = app._resolve_ghost(model, REBOT) + approach = _mat(data.mocap_quat[ghost.body])[:, 0] + motor = _geom_verts_world(model, data, "rebot_ghost_motor") @ approach + grip = np.array(_mujoco_xr.mj_from_xr_pos(list(grip_xr))) @ approach + assert 0.5 * (motor.min() + motor.max()) == pytest.approx(grip, abs=1e-3), ( + "the fist is not on the drive motor" ) - with pytest.raises(RuntimeError, match=app.GHOST_BODY): - app._resolve_ghost(model) diff --git a/rigs/mujoco_xr.yaml b/rigs/mujoco_xr.yaml index 2f5a6ee5a..6e40a5c94 100644 --- a/rigs/mujoco_xr.yaml +++ b/rigs/mujoco_xr.yaml @@ -22,10 +22,14 @@ # # See rigs/se3_tracker.yaml for the fully annotated exemplar of every key. name: mujoco_xr -description: CloudXR runtime + MuJoCo scene in XR with the SO-101 leader gripper +description: CloudXR runtime + MuJoCo gripper ghost in XR cwd: .. # -> Teleop repo root consumers: - name: mujoco xr app (requires headset) + # Add `--robot=rebot` below for the reBot DevArm gripper; the default is the + # SO-101 leader gripper. Each robot has its own fetch script, so switching + # needs examples/mujoco_xr/scripts/fetch-rebot-arm.sh and a reinstall first. + # # Keep --no-launch-cloudxr-runtime. Without it the app starts a second # CloudXR runtime, and the runtime is a host singleton on WSS port 48322, so # the second one kills this rig's own runtime pane: the headset drops