From 8dcc95c530b462210c18f6e685a131661e608cae Mon Sep 17 00:00:00 2001 From: j-mex Date: Fri, 30 Oct 2026 14:48:45 +0800 Subject: [PATCH] feat(jmex): add AgileMaster full-body plugin and G1 teleop example Adds `jmex`, a plugin for the j-mex AgileMaster motion capture suit, and an example, in two parts, that consumes what it publishes. The plugin reads one MOXI Player channel through the MOXI Receiver SDK (0.8.0 or newer, obtained from j-mex) and runs on Linux only. It fills the 24 `FullBodyPose` slots from the standard human skeleton Player has loaded, one slot per joint, already in OpenXR axes, and pushes them under the `body.jmex` vendor on every frame. A slot with no joint, or with no usable value in a frame, goes out invalid; nothing is inferred from its neighbors. The raw device clock carries the time Player stamped on the frame (`--device-clock=local` uses the pickup time instead), and if the CloudXR runtime restarts underneath the plugin, the plugin reconnects rather than ending the run. An argument it cannot use stops it with exit code 1, naming the argument. Everything but the receiver is a library that builds without the SDK, so on Linux its unit tests run in every build that has tests enabled; without the SDK only the plugin executable is skipped. Isaac Capture learns `body.jmex` as a full-body vendor: a live tracker that reads the plugin's collection, with a `collection_id` parameter, its entry in the DeviceIO factory, the vendor validation cases, and a row in `devices.yaml`. An SDK unpacked into the plugin directory, under any versioned name, is kept out of git and the clang-format check. `isaaccapture_examples.jmex.jmex_g1_tasks` registers a task that inherits Isaac Lab's `LocomanipulationG1EnvCfg`, on Isaac Lab 3.0.0 EA or 3.0.0 beta2, and takes its action from the operator; nothing in Isaac Lab is modified. It is upper-body teleop. Each arm is rebuilt segment by segment onto the robot's own shoulder, so the operator's arm angles survive while the lengths stay the robot's, and the elbows and shoulders get IK targets of their own from that same chain. The wrist's orientation is the operator's hand, carried across through a forearm frame. The operator's torso turns the robot's shoulders about its waist, so leaning, bending sideways and twisting come through; their heading does not, so the operator can turn to face a screen without moving the robot. Only a frame that carries all 24 slots drives the robot, and slots that stay missing are named on the console. `isaaccapture_examples.jmex.record_jmex_full_body` writes the same stream to a standard full-body MCAP. Tests cover the plugin's argument parsing, slot mapping, change of axes, device clock choice and session rebuild after a failed push, the vendor's parameters, and the example's action source, including the geometry of the rebuilt arm and the orientation carried through the forearm frame. Co-Authored-By: Claude Opus 5.5 Signed-off-by: j-mex --- cmake/ClangFormat.cmake | 1 + docs/source/_data/devices.yaml | 14 + examples/jmex/.gitignore | 1 + examples/jmex/README.md | 252 +++ examples/jmex/pyproject.toml | 33 + .../isaaccapture_examples/jmex/__init__.py | 2 + .../jmex/jmex_g1_retargeting.py | 932 ++++++++++ .../jmex/jmex_g1_tasks.py | 797 +++++++++ .../jmex/record_jmex_full_body.py | 194 +++ src/core/live_trackers/cpp/CMakeLists.txt | 2 + .../live_trackers/live_deviceio_factory.hpp | 1 + .../cpp/live_deviceio_factory.cpp | 26 + .../cpp/live_full_body_tracker_jmex_impl.cpp | 79 + .../cpp/live_full_body_tracker_jmex_impl.hpp | 64 + src/plugins/CMakeLists.txt | 1 + src/plugins/jmex/.gitignore | 5 + src/plugins/jmex/CMakeLists.txt | 52 + src/plugins/jmex/README.md | 238 +++ src/plugins/jmex/app/CMakeLists.txt | 17 + src/plugins/jmex/app/main.cpp | 131 ++ src/plugins/jmex/app/receiver_session.cpp | 235 +++ src/plugins/jmex/app/receiver_session.hpp | 90 + src/plugins/jmex/core/CMakeLists.txt | 43 + src/plugins/jmex/core/body_joint_mapping.cpp | 167 ++ src/plugins/jmex/core/device_clock.cpp | 63 + src/plugins/jmex/core/full_body_publisher.cpp | 224 +++ .../jmex/core/inc/jmex/body_joint_mapping.hpp | 95 + .../jmex/core/inc/jmex/device_clock.hpp | 35 + .../jmex/core/inc/jmex/full_body_frame.hpp | 27 + .../core/inc/jmex/full_body_publisher.hpp | 115 ++ .../jmex/core/inc/jmex/openxr_convention.hpp | 25 + .../jmex/core/inc/jmex/plugin_args.hpp | 33 + .../jmex/core/inc/jmex/receiver_config.hpp | 49 + src/plugins/jmex/core/openxr_convention.cpp | 48 + src/plugins/jmex/core/plugin_args.cpp | 123 ++ src/plugins/jmex/plugin.yaml | 14 + .../live_trackers/test_vendor_validation.cpp | 38 + tests/cpp/plugins/CMakeLists.txt | 3 +- tests/cpp/plugins/jmex/CMakeLists.txt | 26 + .../plugins/jmex/test_jmex_plugin_core.cpp | 634 +++++++ tests/cpp/plugins/jmex/test_plugin_args.cpp | 111 ++ tests/python/examples/CMakeLists.txt | 1 + tests/python/examples/jmex/CMakeLists.txt | 24 + tests/python/examples/jmex/conftest.py | 20 + tests/python/examples/jmex/pyproject.toml | 21 + .../jmex/test_jmex_g1_action_source.py | 1541 +++++++++++++++++ 46 files changed, 6646 insertions(+), 1 deletion(-) create mode 100644 examples/jmex/.gitignore create mode 100644 examples/jmex/README.md create mode 100644 examples/jmex/pyproject.toml create mode 100644 examples/jmex/python/isaaccapture_examples/jmex/__init__.py create mode 100644 examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_retargeting.py create mode 100644 examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_tasks.py create mode 100644 examples/jmex/python/isaaccapture_examples/jmex/record_jmex_full_body.py create mode 100644 src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.cpp create mode 100644 src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.hpp create mode 100644 src/plugins/jmex/.gitignore create mode 100644 src/plugins/jmex/CMakeLists.txt create mode 100644 src/plugins/jmex/README.md create mode 100644 src/plugins/jmex/app/CMakeLists.txt create mode 100644 src/plugins/jmex/app/main.cpp create mode 100644 src/plugins/jmex/app/receiver_session.cpp create mode 100644 src/plugins/jmex/app/receiver_session.hpp create mode 100644 src/plugins/jmex/core/CMakeLists.txt create mode 100644 src/plugins/jmex/core/body_joint_mapping.cpp create mode 100644 src/plugins/jmex/core/device_clock.cpp create mode 100644 src/plugins/jmex/core/full_body_publisher.cpp create mode 100644 src/plugins/jmex/core/inc/jmex/body_joint_mapping.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/device_clock.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/full_body_frame.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/full_body_publisher.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/openxr_convention.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/plugin_args.hpp create mode 100644 src/plugins/jmex/core/inc/jmex/receiver_config.hpp create mode 100644 src/plugins/jmex/core/openxr_convention.cpp create mode 100644 src/plugins/jmex/core/plugin_args.cpp create mode 100644 src/plugins/jmex/plugin.yaml create mode 100644 tests/cpp/plugins/jmex/CMakeLists.txt create mode 100644 tests/cpp/plugins/jmex/test_jmex_plugin_core.cpp create mode 100644 tests/cpp/plugins/jmex/test_plugin_args.cpp create mode 100644 tests/python/examples/jmex/CMakeLists.txt create mode 100644 tests/python/examples/jmex/conftest.py create mode 100644 tests/python/examples/jmex/pyproject.toml create mode 100644 tests/python/examples/jmex/test_jmex_g1_action_source.py diff --git a/cmake/ClangFormat.cmake b/cmake/ClangFormat.cmake index b0500093e1..d33088304c 100644 --- a/cmake/ClangFormat.cmake +++ b/cmake/ClangFormat.cmake @@ -40,6 +40,7 @@ set(_cf_exclude_patterns ".*/deps/.*" ".*/manus/ManusSDK/.*" ".*/wuji_glove/wuji-sdk-c-.*" + ".*/jmex/MOXIReceiverSDK[^/]*/.*" ".*/node_modules/.*" ".*/third_party/.*" ".*/venv/.*" diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index 92facf6c24..4540c3038a 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -284,6 +284,20 @@ devices: - label: Noitom Mocap Plugin url: https://github.com/NVIDIA/IsaacTeleop/tree/main/src/plugins/noitom_mocap + - id: jmex-agilemaster + name: j-mex AgileMaster + url: https://jmex.com.tw/agilemaster/ + group: peripheral + modes: Full-body motion capture + since: main + details: + setup: + - label: j-mex AgileMaster Plugin + url: https://github.com/NVIDIA/IsaacCapture/tree/main/src/plugins/jmex + requirements: + - MOXI Receiver SDK 0.8.0 or newer (on request from j-mex) + - MOXI Player with the standard human skeleton loaded, on this machine or the same subnet + - id: logitech-rudder-pedals name: Logitech Rudder Pedals url: https://www.logitechg.com/en-us/products/flight/flight-simulator-rudder-pedals.html diff --git a/examples/jmex/.gitignore b/examples/jmex/.gitignore new file mode 100644 index 0000000000..d1b5f6ca79 --- /dev/null +++ b/examples/jmex/.gitignore @@ -0,0 +1 @@ +recordings diff --git a/examples/jmex/README.md b/examples/jmex/README.md new file mode 100644 index 0000000000..b2a851da44 --- /dev/null +++ b/examples/jmex/README.md @@ -0,0 +1,252 @@ + + +# j-mex AgileMaster full body + +An example in two parts, both reading one [j-mex AgileMaster](https://jmex.com.tw/agilemaster/) +motion capture channel through the [`jmex` plugin](../../src/plugins/jmex/README.md): a teleop task +that drives a G1 in Isaac Lab from the operator's body pose, and a script that records the same body +pose to a standard Isaac Capture full-body MCAP. + +## Prerequisites + +1. **The plugin, installed.** `cmake --install build` — a plugin that has only been built is not + discoverable. The [plugin README](../../src/plugins/jmex/README.md) lists the SDK it needs and + the build prerequisites. +2. **MOXI Player**, with the **standard human skeleton** loaded, on a known channel. The plugin's + default is channel 1; Player's file replay is fixed on 255. +3. **A CloudXR runtime.** It is required even though no headset is. +4. **Isaac Lab 3.0.0 EA or 3.0.0 beta2**, for the teleop half. [Verified on](#verified-on) gives + the exact versions. +5. **This repository's `isaaccapture`, in Isaac Lab's Python**, also for the teleop half: no + released package carries the `body.jmex` vendor yet. Build it as in + [Build from source](https://nvidia.github.io/IsaacCapture/main/development/build_from_source/index.html), + with `-DISAAC_TELEOP_PYTHON_VERSION=3.12`, the Python Isaac Sim 6 runs. Then install the wheel + into Isaac Lab's Python instead of a venv, with `X.Y` the first two numbers of `VERSION`: + + ```bash + cd + ./isaaclab.sh -p -m pip install "isaaccapture[cloudxr]==X.Y+local" \ + --find-links=/install/wheels + ``` + + After a rebuild, replace it without touching the packages Isaac Lab pins: + + ```bash + ./isaaclab.sh -p -m pip install "isaaccapture==X.Y+local" \ + --find-links=/install/wheels --force-reinstall --no-deps + ``` + + pip reports conflicts with versions Isaac Lab pins, such as `isaacteleop` or `viser`'s + `websockets`. That is expected: the teleop steps below work with them. `./isaaclab.sh --install` + puts the pinned packages back, so repeat this step after it. +6. **This example, in Isaac Lab's Python**, for the teleop half as well. Install it editable — the + task finds `jmex_plugin` under this checkout's `install/plugins`: + + ```bash + cd + ./isaaclab.sh -p -m pip install -e /examples/jmex + ``` + +## Teleop: driving the G1 + +`jmex_g1_tasks.py` registers `Isaac-PickPlace-Locomanipulation-G1-Jmex-Abs-v0`: Isaac Lab's +locomanipulation G1 task, with its action taken from the operator instead of from XR controllers. +Nothing in Isaac Lab is modified. + +This is **upper-body teleop**: the operator's arms drive the G1's arms, and their torso — leaning, +bending sideways, twisting — drives its waist. Turning on the spot does not move the robot, and the +legs belong to the locomotion policy. The scene is the G1 on its own. + +Running it takes four steps: + +1. **Start the runtime** in a terminal of its own, and leave it running; `Ctrl-C` there stops it + once Isaac Lab has exited. It runs on Isaac Lab's Python, which has this repository's + `isaaccapture` from [Prerequisites](#prerequisites) step 5: + + ```bash + cd + ./isaaclab.sh -p -m isaaccapture.cloudxr.service run + ``` + +2. **Load the environment it wrote**, in the terminal you start Isaac Lab from: + + ```bash + source ~/.cloudxr/run/cloudxr.env + ``` + +3. **Start the task** in that terminal, with `--cloudxr_env none`: + + ```bash + cd + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-Locomanipulation-G1-Jmex-Abs-v0 \ + --visualizer kit \ + --xr \ + --cloudxr_env none \ + --external_callback isaaccapture_examples.jmex.jmex_g1_tasks.register_tasks + ``` + +4. **Press Start XR** in Kit's XR panel. Nothing moves until then, and Isaac Lab logs + `OpenXR handles not yet available (waiting for XR session)`. Without `--visualizer kit` Isaac Lab + runs headless, and the XR session starts by itself. + +Things to know: + +- **Keep `--cloudxr_env none`.** Without it, Isaac Lab launches a runtime of its own with the + `auto-webrtc` profile, which waits for a headset: no data arrives, and that runtime is still + running after Isaac Lab exits. +- **The callback path uses a dot, not a colon**, and resolves only once this example is installed in + Isaac Lab's Python ([Prerequisites](#prerequisites) step 6). +- **The task launches `jmex_plugin` itself.** `JMEX_PLUGIN_AUTO_LAUNCH=0` leaves that to you, and + `JMEX_CHANNEL` and `JMEX_COLLECTION_ID` change the channel and the collection. +- **Until Player pairs**, the G1 holds its own rest pose, and the console says so once and then + every 300 frames. +- **If Player disconnects mid-run**, the G1 holds the last pose it received: the collection keeps its + last frame. The plugin logs `MOXI Player disconnected; waiting for it to come back`, and the G1 + follows again once Player pairs. +- **Load the standard human skeleton in Player.** Only a frame that carries all 24 slots moves the + robot. A robot URDF loaded by mistake fills only some of them; when slots stay missing for 30 + frames in a row — a few seconds when driven live — the console names them, again every 300 frames. + +### What is connected, and what is not + +| | Source | +| --- | --- | +| Wrist position | The operator's arm, rebuilt with the G1's arm lengths from the G1's shoulder | +| Wrist orientation | The operator's hand | +| Elbow position | The operator's elbow, on the same rebuilt arm | +| Shoulder position | The G1's shoulder, turned about its waist by the operator's torso | +| Fingers | Zero | +| Legs, base motion | A fixed standing command; the locomotion policy keeps the G1 standing | + +Ten of the 24 slots reach the action: the shoulders, elbows and wrists for the arms, the two hips +for the operator's heading, and `SPINE1` and `SPINE3` for the torso. The rest are drawn in the +overlay and drive nothing. + +### Reading the overlay + +The task draws two figures over the robot. Once XR is running, Kit's main viewport shows the XR +camera's view; open **Window → Viewport 2** to watch them: + +| Color | What it is | Should the robot line up with it? | +| --- | --- | --- | +| Blue | The body pose as it arrived, turned to face the way the robot does, with its pelvis on the robot's | No. It has the operator's proportions | +| Orange | Each arm as the IK is asked to place it, with the shoulder, elbow and wrist drawn as large dots | Yes. The robot's shoulders, elbows and wrists should sit on the large dots | + +Before Player pairs, orange is a dot on each of the robot's resting wrists, elbows and shoulders, +and there is no blue figure. Without Kit's debug draw extension nothing is drawn, and the console +says so once. + +## Recording a body pose + +Install the example once, then start the runtime in its own terminal and leave it running: + +```bash +uv pip install -e ./examples/jmex +python -m isaaccapture.cloudxr.service run +``` + +Then, in the terminal you record from, load the environment it wrote and let the script attach to +what is already up: + +```bash +source ~/.cloudxr/run/cloudxr.env + +python -m isaaccapture_examples.jmex.record_jmex_full_body \ + 10 examples/jmex/recordings/jmex_full_body.mcap \ + --no-launch-cloudxr-runtime +``` + +This records ten seconds to that path on the plugin's default channel. Add `--channel=255` to +record from Player's file replay, and `--collection-id=` if this machine runs more than one of +these. + +The script starts `jmex_plugin` itself and stops it on exit. `--no-plugin` leaves starting it to +you, which is also how to give the plugin any of its [other +flags](../../src/plugins/jmex/README.md#running). Start the runtime first, as above: if the script +starts a runtime itself and that runtime is still coming up, the script can wait at +`OpenXR HMD form factor is unavailable; waiting for a system...` with no timeout, and `Ctrl-C` does +not stop it. + +Each progress line reports how many of the 24 slots the frame carried: + +```text +[record] t= 1.03s frame=60 joints=24/24 +``` + +`00/24` on every line means no sample has arrived: Player has not paired, or the channel is wrong. + +## Replaying + +There is no j-mex replay script. The recording uses the standard `core.FullBodyPoseRecord` schema on +the standard `full_body` channel, so the shared script plays it back: + +```bash +uv pip install -e ./examples/mcap_record_replay +python -m isaaccapture_examples.mcap_record_replay.replay_full_body \ + examples/jmex/recordings/jmex_full_body.mcap +``` + +## What is in the recording + +| Channel | Schema | Contents | +| --- | --- | --- | +| `full_body/full_body` | `core.FullBodyPoseRecord` | 24 body-pose slots, OpenXR axes, meters | +| `controllers/left_controller` | `core.ControllerSnapshotRecord` | No tracked pose on a mocap-only session | +| `controllers/right_controller` | `core.ControllerSnapshotRecord` | No tracked pose on a mocap-only session | + +The two controller channels are recorded because the shared replay pipeline expects them. Near the +end of each pass, replay logs `left controller data not found` and `right controller data not found` +once each; neither is a fault. + +## Known limitations + +- **Nothing can be picked up.** The body pose carries no finger joints, so the fingers stay at zero, + and the table and its object are moved away. +- **The feet shuffle.** The pelvis is pinned, and the locomotion policy keeps counterbalancing + whatever the arms do. +- **An arm thrown out to the shoulder's joint limit can stay there.** Isaac Lab's "Press 'R'" does + not reset it; restart Isaac Lab. Short visits to the limit that clear on their own are more + common. +- **Stay inside the waist's range**: ±15° of lean and side bend, ±30° of twist, as logged at + startup. Past them the waist stops at the limit and the G1 stops following, arms included. +- **The G1 runs behind the operator.** With XR on, the simulation runs well below real time. +- **Stand upright before starting.** For the first couple of seconds the robot catches up to + wherever the operator already is. + +## Troubleshooting + +| Symptom | What to check | +| --- | --- | +| Isaac Lab fails to import `pinocchio`, or its `pip install` lands in another environment | The shell has a venv active or a ROS environment sourced. Put `env -u VIRTUAL_ENV -u LD_LIBRARY_PATH -u PYTHONPATH` in front of Isaac Lab's commands | +| On Isaac Lab 3.0.0 EA, `Ctrl-C` returns to the prompt but Isaac Lab keeps running | Isaac Lab 3.0.0 EA does the same with its own tasks. Stop it with `pkill -f teleop_se3_agent.py` | + +## Verified on + +- Isaac Lab v3.0.0-EA on Isaac Sim 6.1.0 +- Isaac Lab v3.0.0-beta2.patch1 on Isaac Sim 6.0.1 + +Both with the `LocomanipulationG1EnvCfg` base task and the Pink IK controller that ships with it. + +## Running the offline tests + +The retargeting tests need only the staged `isaaccapture` package: no Isaac Sim, Isaac Lab, device +or MOXI Player. They carry the ctest label `jmex`, as do the plugin's C++ tests, which need no MOXI +Receiver SDK. A build registers the Python ones only with `BUILD_EXAMPLES` and `BUILD_VIZ` on: + +```bash +ctest --test-dir build -L jmex --output-on-failure +``` + +To run them under `pytest` from the repository root instead, put the staged package on `PYTHONPATH` +and use the Python the build was configured for, 3.11 unless `ISAAC_TELEOP_PYTHON_VERSION` set +another: + +```bash +PYTHONPATH=build/python_package/Release \ + uv run --project tests/python/examples/jmex --python 3.11 --extra dev \ + pytest tests/python/examples/jmex/ -v +``` diff --git a/examples/jmex/pyproject.toml b/examples/jmex/pyproject.toml new file mode 100644 index 0000000000..375536fa37 --- /dev/null +++ b/examples/jmex/pyproject.toml @@ -0,0 +1,33 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[project] +# The dist name mirrors the import path, so an installed example claims no bare +# top-level name in site-packages. +name = "isaaccapture-examples-jmex" +version = "0.0.0" # Internal example - not versioned +description = "j-mex AgileMaster full body: G1 upper-body teleop in Isaac Lab, MCAP recording" +requires-python = ">=3.11,<3.14" +# No version on isaaccapture: the example runs against the one built from this +# checkout. isaaclab, gymnasium and pink are left out on purpose: jmex_g1_tasks +# runs inside the Isaac Lab environment, which provides them, and README.md +# installs this example into that interpreter. +dependencies = [ + "isaaccapture[cloudxr]", + "numpy>=1.23.0", + "scipy>=1.15.0", +] + +# `only-include` + `sources`, not `packages`, and no __init__.py above +# jmex -- see the packaging rules in examples/README.md. +[tool.hatch.build.targets.wheel] +only-include = ["python/isaaccapture_examples/jmex"] +sources = ["python"] + +[tool.uv.sources] +# Editable, so edits under src/python/ need no rebuild. +isaaccapture = { path = "../..", editable = true } diff --git a/examples/jmex/python/isaaccapture_examples/jmex/__init__.py b/examples/jmex/python/isaaccapture_examples/jmex/__init__.py new file mode 100644 index 0000000000..e91b7aeac3 --- /dev/null +++ b/examples/jmex/python/isaaccapture_examples/jmex/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 diff --git a/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_retargeting.py b/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_retargeting.py new file mode 100644 index 0000000000..e9b8bcf7ea --- /dev/null +++ b/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_retargeting.py @@ -0,0 +1,932 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Turn the operator's body pose into G1 locomanipulation IK targets. + +Each arm is rebuilt from the robot's shoulder with the robot's segment lengths and the operator's +segment directions. Do not import ``isaaclab`` here: the tests run this module without Isaac Sim, +a GPU or the device. The task, its config and the robot's measurements are in ``jmex_g1_tasks.py``. +""" + +from __future__ import annotations + +import logging +from dataclasses import dataclass +from itertools import combinations +from typing import Any, Callable, Sequence + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaaccapture.logging_config._core import logging_enabled +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPoseTracked, + IDeviceIOSource, +) +from isaaccapture.retargeting_engine.interface import ( + ComputeContext, + RetargeterIO, + RetargeterIOType, + TensorGroup, + TensorGroupType, +) +from isaaccapture.retargeting_engine.tensor_types import DLDataType, NDArrayType +from isaaccapture.schema import BodyJoint, FullBodyPose + +logger = logging.getLogger("isaaccapture.jmex.jmex_g1_retargeting") + + +def info_or_print(log: logging.Logger, message: str, *args: object) -> None: + """Log at info, or print under ``ISAACCAPTURE_LOGGING=off``, where nothing shows an info record.""" + if logging_enabled(): + log.info(message, *args) + else: + print(message % args if args else message) + + +#: Input this source polls from DeviceIO. +FULL_BODY_INPUT = "deviceio_full_body" +#: The one output. Isaac Lab's teleop device looks for an output literally named "action". +ACTION_OUTPUT = "action" + +#: Position (3) plus orientation (4) per Pink frame task, in the order the action term reads. +POSITION_DIM = 3 +ORIENTATION_DIM = 4 +POSE_DIM = POSITION_DIM + ORIENTATION_DIM + +#: The lower-body policy's command, ``[vx, vy, wz, hip_height]`` in the robot's body frame, in +#: m/s, m/s, rad/s and m. It is a policy input, not a set-point: under a constant command the +#: legs still counterbalance the arms. 0.72 is Isaac Lab's +#: ``LocomotionRootCmdRetargeterConfig(initial_hip_height=0.72)`` in +#: isaaclab_tasks/.../locomanipulation/pick_place/locomanipulation_g1_env_cfg.py. +STANDING_LOCOMOTION_COMMAND: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 0.72) + +#: Change of basis from the plugin's OpenXR axes (+X the operator's right, +Y up, -Z the way +#: they face; src/plugins/jmex/core/openxr_convention.cpp) to Isaac's (+X forward, +Y left, +#: +Z up). A proper rotation (determinant +1), which ``_hand_orientation`` relies on. +_OPENXR_TO_ISAAC = np.array( + [ + [0.0, 0.0, -1.0], + [-1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + ] +) + +#: The ``forearm_frame`` in the axes of the standard human skeleton's ``WRIST`` slot, per arm. +#: Columns: x out along the forearm, y the elbow's hinge, z = x cross y. In the slot's axes x is +#: -X on the left arm and +X on the right, and the hinge is -Y and +Y. Measured: the wrist-to-hand +#: offset is rigid in these axes, 55.2 mm (sd 0.000 mm), along x to 0.00 deg. A skeleton whose +#: ``WRIST`` slot is laid out differently needs a new matrix, or the hands come out a quarter turn +#: off; nothing in the stream reports it. +_FOREARM_FRAME_IN_PUBLISHED_WRIST = { + int(BodyJoint.LEFT_WRIST): np.array( + [ + [-1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + [0.0, 0.0, 1.0], + ] + ), + int(BodyJoint.RIGHT_WRIST): np.array( + [ + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + ] + ), +} + +#: Below this, a segment carries no usable direction and the frame is treated as unusable. +_MIN_SEGMENT_LENGTH_M = 1e-4 +#: Anchors and link lengths closer than this are one measurement, taken twice. +_SAME_MEASUREMENT_TOLERANCE_M = 1e-9 + +#: Frames in a row that may miss a slot before the model, not a bad frame, is blamed. One frame +#: is one simulation step, so live this is a few seconds: long enough that one dropped slot +#: never says it, short enough that the wrong model is named while the operator is still looking. +_INCOMPLETE_FRAMES_BEFORE_WARNING = 30 +#: How often a condition that persists is repeated on the console. +_REPORT_EVERY_FRAMES = 300 + + +def left_wrist_chain() -> tuple[int, ...]: + """Body slots from the left shoulder out to the left wrist, anchor first.""" + return ( + int(BodyJoint.LEFT_SHOULDER), + int(BodyJoint.LEFT_ELBOW), + int(BodyJoint.LEFT_WRIST), + ) + + +def right_wrist_chain() -> tuple[int, ...]: + """Body slots from the right shoulder out to the right wrist, anchor first.""" + return ( + int(BodyJoint.RIGHT_SHOULDER), + int(BodyJoint.RIGHT_ELBOW), + int(BodyJoint.RIGHT_WRIST), + ) + + +def forearm_frame( + shoulder: Sequence[float], + elbow: Sequence[float], + wrist: Sequence[float], + *, + elbow_axis: Sequence[float], +) -> Rotation: + """The frame a hand is turned relative to, built from an arm resting with its elbow bent. + + x runs elbow to wrist; y is ``elbow_axis`` made square to x and signed along + (upper arm x forearm), whichever way the model signs its joint; z completes it. + + Raises: + ValueError: The forearm has no length, the elbow is straight, or the elbow's axis + runs along the forearm. + """ + shoulder, elbow, wrist = ( + np.asarray(point, dtype=np.float64) for point in (shoulder, elbow, wrist) + ) + forearm = wrist - elbow + if np.linalg.norm(forearm) < _MIN_SEGMENT_LENGTH_M: + raise ValueError("the forearm has no length") + distal = forearm / np.linalg.norm(forearm) + bend = np.cross(elbow - shoulder, distal) + if np.linalg.norm(bend) < _MIN_SEGMENT_LENGTH_M: + raise ValueError("the elbow rests straight, so which way it bends is unknown") + hinge = np.asarray(elbow_axis, dtype=np.float64) + hinge = hinge - np.dot(hinge, distal) * distal + if np.linalg.norm(hinge) < _MIN_SEGMENT_LENGTH_M: + raise ValueError("the elbow's axis runs along the forearm") + hinge = hinge / np.linalg.norm(hinge) + if np.dot(hinge, bend) < 0.0: + hinge = -hinge + return Rotation.from_matrix( + np.column_stack([distal, hinge, np.cross(distal, hinge)]) + ) + + +@dataclass(frozen=True) +class PelvisPose: + """The robot's pelvis, in the per-env world frame Isaac hands the action term. + + ``orientation_xyzw`` is scalar-last, as Isaac Lab 3.0 hands it over; a scalar-first one + raises nothing and arrives rotated. + """ + + position: np.ndarray + orientation_xyzw: np.ndarray + + @property + def rotation(self) -> Rotation: + """``orientation_xyzw`` as a ``Rotation``.""" + return Rotation.from_quat(np.asarray(self.orientation_xyzw, dtype=np.float64)) + + def place(self, points_in_pelvis: np.ndarray) -> np.ndarray: + """Re-express pelvis-frame points in the world frame.""" + return np.asarray(self.position, dtype=np.float64) + self.rotation.apply( + points_in_pelvis + ) + + def compose( + self, offset_in_pelvis: np.ndarray, orientation_in_pelvis: Rotation + ) -> tuple[np.ndarray, np.ndarray]: + """Re-express a pelvis-frame target in the world frame. + + The quaternion is the one of its two signs with w >= 0, so one orientation is always + written the same way. + """ + return ( + self.place(offset_in_pelvis), + (self.rotation * orientation_in_pelvis).as_quat(canonical=True), + ) + + +def _unit_quaternion(value: Sequence[float], *, what: str) -> np.ndarray: + quaternion = np.asarray(value, dtype=np.float64) + if quaternion.shape != (ORIENTATION_DIM,): + raise ValueError( + f"{what} must be four numbers (x, y, z, w), got {quaternion.shape}" + ) + norm = float(np.linalg.norm(quaternion)) + # The action term builds its rotation matrix by dividing by the squared norm, so a zero + # -- the obvious filler for a task whose orientation weight is zero -- reaches the IK + # solver as NaN rather than as "unconstrained". + if norm < _MIN_SEGMENT_LENGTH_M: + raise ValueError( + f"{what} must be a non-zero quaternion, got {quaternion.tolist()}" + ) + return quaternion / norm + + +@dataclass(frozen=True) +class FrameTargetCfg: + """One Pink ``FrameTask``, and the chain of body slots whose last point is its target. + + The robot's numbers are measured off the live pinocchio model by the wiring module, in + the pelvis frame at the robot's rest configuration, so switching robots changes no + constant here. + """ + + name: str + #: Body slots from the anchor out to this task's own (see ``arm_frame_targets``). + chain: tuple[int, ...] + #: The robot's own point that ``chain[0]`` stands for: its shoulder, for an arm. + anchor_position_pelvis: tuple[float, float, float] + #: Meters between consecutive frames of ``chain`` on the robot. + robot_segment_lengths: tuple[float, ...] + #: Where the task's frame rests: the target until a frame drives the chain. + neutral_position_pelvis: tuple[float, float, float] + #: How the task's frame rests. A task without a forearm frame never turns from it. + neutral_orientation_pelvis_xyzw: tuple[float, float, float, float] + #: The robot's ``forearm_frame`` in the task frame's own axes, which carries the + #: operator's hand onto the task. Exactly the chains that end at a wrist have one. + forearm_frame_in_task_xyzw: tuple[float, float, float, float] | None = None + + def __post_init__(self) -> None: + if not self.chain: + raise ValueError(f"{self.name}: a chain needs at least its anchor slot") + if np.shape(self.anchor_position_pelvis) != (POSITION_DIM,): + raise ValueError( + f"{self.name}: the anchor must be three numbers (x, y, z), got " + f"{np.shape(self.anchor_position_pelvis)}" + ) + if len(self.robot_segment_lengths) != len(self.chain) - 1: + raise ValueError( + f"{self.name}: {len(self.chain)} slots need " + f"{len(self.chain) - 1} link lengths, got {len(self.robot_segment_lengths)}" + ) + if any(length <= 0.0 for length in self.robot_segment_lengths): + raise ValueError(f"{self.name}: link lengths must be positive") + # Raises here, at construction, rather than emitting a NaN target later. + _unit_quaternion( + self.neutral_orientation_pelvis_xyzw, + what=f"{self.name}: neutral orientation", + ) + ends_at_wrist = self.chain[-1] in _FOREARM_FRAME_IN_PUBLISHED_WRIST + if ends_at_wrist and self.forearm_frame_in_task_xyzw is None: + raise ValueError( + f"{self.name}: a chain that ends at a wrist needs a forearm frame to carry " + "the operator's hand onto its task" + ) + if not ends_at_wrist and self.forearm_frame_in_task_xyzw is not None: + raise ValueError( + f"{self.name}: only a chain that ends at a wrist takes a forearm frame; this " + f"one ends at {BodyJoint(self.chain[-1]).name}" + ) + if self.forearm_frame_in_task_xyzw is not None: + _unit_quaternion( + self.forearm_frame_in_task_xyzw, what=f"{self.name}: forearm frame" + ) + + @property + def neutral_rotation(self) -> Rotation: + """``neutral_orientation_pelvis_xyzw`` as a ``Rotation``.""" + return Rotation.from_quat( + _unit_quaternion( + self.neutral_orientation_pelvis_xyzw, + what=f"{self.name}: neutral orientation", + ) + ) + + @property + def forearm_frame_in_task(self) -> Rotation: + """``forearm_frame_in_task_xyzw`` as a ``Rotation``; ``ValueError`` if None.""" + return Rotation.from_quat( + _unit_quaternion( + self.forearm_frame_in_task_xyzw, + what=f"{self.name}: forearm frame", + ) + ) + + +def arm_frame_targets( + chain: tuple[int, ...], + *, + anchor_position_pelvis: Sequence[float], + robot_segment_lengths: Sequence[float], + rest_poses_pelvis: Sequence[tuple[Sequence[float], Sequence[float]]], + forearm_frame_in_task_xyzw: Sequence[float], +) -> tuple[FrameTargetCfg, ...]: + """One task per slot of an arm's ``chain``, anchor first: the chain cut short at that slot. + + ``rest_poses_pelvis`` holds each slot's frame at rest as (position, xyzw). The task that + ends the chain -- the wrist -- is the one that takes the forearm frame. + """ + + if len(rest_poses_pelvis) != len(chain): + raise ValueError( + f"{len(chain)} slots need {len(chain)} rest poses, got {len(rest_poses_pelvis)}" + ) + + def floats(values: Sequence[float]) -> tuple[float, ...]: + return tuple(float(value) for value in values) + + return tuple( + FrameTargetCfg( + name=BodyJoint(chain[depth - 1]).name.lower(), + chain=chain[:depth], + anchor_position_pelvis=floats(anchor_position_pelvis), + robot_segment_lengths=floats(robot_segment_lengths[: depth - 1]), + neutral_position_pelvis=floats(position), + neutral_orientation_pelvis_xyzw=floats(orientation), + forearm_frame_in_task_xyzw=( + floats(forearm_frame_in_task_xyzw) if depth == len(chain) else None + ), + ) + for depth, (position, orientation) in enumerate(rest_poses_pelvis, start=1) + ) + + +@dataclass(frozen=True) +class G1ActionLayout: + """How the flat action tensor is laid out, and how long it is. + + The length is counted from the frame tasks, not hard-coded, so adding a task resizes the + action. Frame tasks come first, in the controller's order, then everything else; + ``_leading_frame_tasks`` in ``jmex_g1_tasks.py`` enforces that order. + """ + + frame_targets: tuple[FrameTargetCfg, ...] + #: The point the robot's upper body turns about, in the pelvis frame at rest: where every + #: anchor is turned through the operator's torso pose. + waist_pivot_pelvis: tuple[float, float, float] + num_hand_joints: int = 14 + num_locomotion: int = len(STANDING_LOCOMOTION_COMMAND) + locomotion_command: tuple[float, ...] = STANDING_LOCOMOTION_COMMAND + + def __post_init__(self) -> None: + if not self.frame_targets: + raise ValueError("a layout needs at least one frame task") + if np.shape(self.waist_pivot_pelvis) != (POSITION_DIM,): + raise ValueError( + "the waist pivot must be three numbers (x, y, z), got " + f"{np.shape(self.waist_pivot_pelvis)}" + ) + if len(self.locomotion_command) != self.num_locomotion: + raise ValueError( + f"locomotion command is {len(self.locomotion_command)} long, " + f"but {self.num_locomotion} slots were declared" + ) + # Slots two chains share are the same points of one arm. Rebuilt from different + # anchors or lengths, an elbow and a wrist target need not be reachable together. + for target, other in combinations(self.frame_targets, 2): + shared = _shared_prefix_length(target.chain, other.chain) + if shared and not ( + _same_measurement( + target.anchor_position_pelvis, other.anchor_position_pelvis + ) + and _same_measurement( + target.robot_segment_lengths[: shared - 1], + other.robot_segment_lengths[: shared - 1], + ) + ): + raise ValueError( + f"{target.name} and {other.name} share the arm out of " + f"{BodyJoint(target.chain[0]).name} but not its anchor and link lengths" + ) + + @property + def action_dim(self) -> int: + """(frame tasks) x 7 + fingers + locomotion.""" + return ( + len(self.frame_targets) * POSE_DIM + + self.num_hand_joints + + self.num_locomotion + ) + + +@dataclass(frozen=True) +class JmexG1Settings: + """Defaults for the j-mex G1 retargeting example.""" + + #: The collection the plugin publishes the body pose on, matching its plugin.yaml. + #: The two ends are paired by this string alone: an id that agrees with nothing simply + #: never delivers a sample. + collection_id: str = "jmex_full_body" + #: MOXI Player channel the plugin pairs on. + channel: int = 1 + plugin_auto_launch: bool = True + status_period_s: float = 1.0 + + +DEFAULT_JMEX_G1_SETTINGS = JmexG1Settings() + + +def g1_locomanipulation_action_type(layout: G1ActionLayout) -> TensorGroupType: + """The flat action tensor this source produces.""" + return TensorGroupType( + "g1_locomanipulation_action", + [ + NDArrayType( + ACTION_OUTPUT, + shape=(layout.action_dim,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ) + ], + ) + + +@dataclass(frozen=True) +class ReferenceOverlay: + """What one frame draws over the robot, in the per-env world frame. + + ``skeleton`` is the body pose as it arrived, per slot, with the operator's heading taken + out and its pelvis placed on the robot's -- neither rebuilt nor scaled. ``chains`` maps + each frame task to its rebuilt chain, anchor first and the IK target last. + """ + + skeleton: dict[int, np.ndarray] + chains: dict[str, np.ndarray] + + +class JmexG1ActionSource(IDeviceIOSource): + """One frame of the operator's body pose in, one flat G1 action out.""" + + def __init__( + self, + name: str = "jmex_g1_action", + *, + layout: G1ActionLayout, + pelvis_pose_provider: Callable[[], PelvisPose], + settings: JmexG1Settings = DEFAULT_JMEX_G1_SETTINGS, + overlay_sink: Callable[[ReferenceOverlay], None] | None = None, + ) -> None: + """Wire the source to a DeviceIO vendor, a layout, and a live pelvis. + + Args: + name: Node name inside the retargeting pipeline. + layout: Frame tasks, their chains, and the robot numbers behind them. + pelvis_pose_provider: Called once per frame for the robot pelvis's live pose in + the per-env world frame. Targets are composed onto it, so they turn with + the robot's spawn yaw. + settings: Collection id and the rest of the pairing configuration. + overlay_sink: Called once per frame with what to draw over the robot. + """ + import isaaccapture.deviceio as deviceio + + self._layout = layout + self._settings = settings + self._pelvis_pose_provider = pelvis_pose_provider + self._overlay_sink = overlay_sink + self._tracker = deviceio.FullBodyTracker() + self._operator_segment_lengths: dict[str, tuple[float, ...]] | None = None + # Held between frames so an unusable frame changes nothing rather than jumping. + # Pelvis-frame, so a moving pelvis still carries the held target with it. Each is + # the rebuilt chain, target last; before the first pairing it is the rest pose alone. + self._held_chains: dict[str, np.ndarray] = { + target.name: np.asarray([target.neutral_position_pelvis], dtype=np.float64) + for target in layout.frame_targets + } + # Updated only together with the chain, so the two are always held as a pair. + self._held_orientations: dict[str, Rotation] = { + target.name: target.neutral_rotation for target in layout.frame_targets + } + self._rebuilt_once = False + self._frame_count = 0 + self._unpaired_frames = 0 + self._incomplete_frames = 0 + self._last_status_s = 0.0 + self._paired = False + super().__init__( + name, + vendor=deviceio.TrackerVendor( + "body.jmex", + {"collection_id": settings.collection_id}, + ), + ) + + @property + def settings(self) -> JmexG1Settings: + """The settings this source was built with.""" + return self._settings + + @property + def layout(self) -> G1ActionLayout: + """The action layout this source packs.""" + return self._layout + + @property + def operator_segment_lengths(self) -> dict[str, tuple[float, ...]] | None: + """Per-chain operator segment lengths in meters, measured once per pairing. + + None until the pairing's first usable frame. Printed for diagnosis; the reconstruction + uses the robot's lengths, not these. + """ + return self._operator_segment_lengths + + def get_tracker(self) -> Any: + """The full-body tracker TeleopSession initializes for this source.""" + return self._tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + """Read one body pose from the active DeviceIO session.""" + group = TensorGroup(self.input_spec()[FULL_BODY_INPUT]) + group[0] = self._tracker.get_body_pose(deviceio_session) + return {FULL_BODY_INPUT: group} + + def input_spec(self) -> RetargeterIOType: + """Declare the raw full-body DeviceIO input.""" + return {FULL_BODY_INPUT: DeviceIOFullBodyPoseTracked()} + + def output_spec(self) -> RetargeterIOType: + """Declare the flat G1 action output.""" + return {ACTION_OUTPUT: g1_locomanipulation_action_type(self._layout)} + + def _compute_fn( + self, + inputs: RetargeterIO, + outputs: RetargeterIO, + context: ComputeContext, + ) -> None: + """Reconstruct each frame task's target and pack the flat action.""" + self._frame_count += 1 + frame: FullBodyPose | None = inputs[FULL_BODY_INPUT][0] + skeleton: dict[int, np.ndarray] = {} + + if frame is None: + self._on_unpaired() + else: + self._announce_pairing() + positions, valid = _joint_positions_in_isaac_axes(frame) + whole = self._is_whole_skeleton(valid) + heading = _heading(positions, valid) + # No heading, nothing to follow and no way to face the skeleton on the robot. + if heading is not None: + # From here on every direction is the operator's own: turning is taken out. + positions = heading.inv().apply(positions) + torso = _torso_pose(positions, valid) + if whole and torso is not None: + self._measure_operator(positions, valid) + pivot = np.asarray(self._layout.waist_pivot_pelvis) + chains = [ + _reconstruct( + target, + positions, + valid, + anchor=pivot + + torso.apply( + np.asarray(target.anchor_position_pelvis) - pivot + ), + ) + for target in self._layout.frame_targets + ] + # All or none: an elbow that moved while its wrist held would no longer + # be a point of the same arm. + if all(chain is not None for chain in chains): + self._rebuilt_once = True + for target, chain in zip(self._layout.frame_targets, chains): + self._held_chains[target.name] = chain + self._held_orientations[target.name] = _hand_orientation( + target, frame, heading + ) + skeleton = _received_skeleton(positions, valid) + + pelvis = self._pelvis_pose_provider() + outputs[ACTION_OUTPUT][0] = self._pack(pelvis) + if self._overlay_sink is not None: + self._overlay_sink( + ReferenceOverlay( + skeleton={ + slot: pelvis.place(point) for slot, point in skeleton.items() + }, + chains={ + name: pelvis.place(chain) + for name, chain in self._held_chains.items() + }, + ) + ) + self._report_status(context, pelvis) + + def _pack(self, pelvis: PelvisPose) -> np.ndarray: + action = np.zeros(self._layout.action_dim, dtype=np.float32) + for index, target in enumerate(self._layout.frame_targets): + position, orientation_xyzw = pelvis.compose( + self._held_chains[target.name][-1], + self._held_orientations[target.name], + ) + start = index * POSE_DIM + action[start : start + POSITION_DIM] = position + action[start + POSITION_DIM : start + POSE_DIM] = orientation_xyzw + # Fingers stay zero: the body pose's slots carry no finger joints. See the README. + if self._layout.num_locomotion: + action[-self._layout.num_locomotion :] = self._layout.locomotion_command + return action + + def _measure_operator(self, positions: np.ndarray, valid: np.ndarray) -> None: + """Take the operator's segment lengths off the first usable frame of a pairing.""" + if self._operator_segment_lengths is not None: + return + measured: dict[str, tuple[float, ...]] = {} + for target in self._layout.frame_targets: + lengths = _segment_lengths(target.chain, positions, valid) + if lengths is None: + # Wait for a frame that carries the whole chain rather than record part of it. + return + measured[target.name] = lengths + self._operator_segment_lengths = measured + for target in self._layout.frame_targets: + if _extended_by_another(target, self._layout.frame_targets): + continue # The longer chain's log line already has these lengths. + info_or_print( + logger, + "measured operator %s chain %s m; retargeting onto robot %s m", + target.name, + _fmt_lengths(measured[target.name]), + _fmt_lengths(target.robot_segment_lengths), + ) + + def _announce_pairing(self) -> None: + if self._paired: + return + self._paired = True + self._unpaired_frames = 0 + info_or_print( + logger, + "paired -- first body pose received on collection '%s'", + self._settings.collection_id, + ) + + def _is_whole_skeleton(self, valid: np.ndarray) -> bool: + """Does this frame carry every slot? Name the missing ones if that keeps not happening. + + Only the standard human skeleton is driven from, and it fills every slot, so a frame + short of any is a bad frame or another model -- and neither is followed, even where + an arm's own slots all arrived. The README says what a robot URDF fills. + """ + if all(valid): + self._incomplete_frames = 0 + return True + self._incomplete_frames += 1 + if not _report_due(self._incomplete_frames, _INCOMPLETE_FRAMES_BEFORE_WARNING): + return False + missing = [BodyJoint(slot).name for slot, ok in enumerate(valid) if not ok] + logger.warning( + "the body pose on collection '%s' has been missing %d of %d slots for %d frames " + "(%s), so MOXI Player is not streaming the whole standard human skeleton. %s " + "Check that the standard human skeleton is the model loaded in Player.", + self._settings.collection_id, + len(missing), + len(valid), + self._incomplete_frames, + ", ".join(missing), + self._holding(), + ) + return False + + def _holding(self) -> str: + """What the robot is doing while nothing drives it, for the console.""" + if self._rebuilt_once: + return "The robot is holding the last target it was given." + return "The robot is holding its own rest pose." + + def _on_unpaired(self) -> None: + """No sample on the collection. Say so, and forget what the last pairing measured. + + A Player disconnect does not land here: the plugin keeps the collection, so the tracker + goes on handing out the last frame. + """ + # Deliberately not restoring the held targets: dropping the robot's arms back to + # rest on a lost link is a larger movement than leaving them where they were told + # to go. Before the first pairing the held targets already are the rest pose. + self._paired = False + self._operator_segment_lengths = None + self._incomplete_frames = 0 + self._unpaired_frames += 1 + if not _report_due(self._unpaired_frames, 1): + return + logger.warning( + "the jmex plugin is not paired -- no body pose on collection '%s' after %d/%d " + "frames. %s Start MOXI Player with the standard human skeleton loaded, on channel " + "%d; Player is the client and connects to the plugin's receiver, so nothing " + "happens until it does.", + self._settings.collection_id, + self._unpaired_frames, + self._frame_count, + self._holding(), + self._settings.channel, + ) + + def _report_status(self, context: ComputeContext, pelvis: PelvisPose) -> None: + if self._settings.status_period_s <= 0.0 or not self._paired: + return + now_s = context.graph_time.real_time_ns * 1.0e-9 + if now_s - self._last_status_s < self._settings.status_period_s: + return + self._last_status_s = now_s + targets = " ".join( + f"{target.name}={_fmt_vec(self._held_chains[target.name][-1])}" + for target in self._layout.frame_targets + ) + info_or_print( + logger, + "frame=%d pelvis=%s targets(pelvis frame) %s", + self._frame_count, + _fmt_vec(np.asarray(pelvis.position)), + targets, + ) + + +def _report_due(frames: int, first: int) -> bool: + """Say it on frame ``first`` of a condition, then every ``_REPORT_EVERY_FRAMES``.""" + return frames >= first and (frames - first) % _REPORT_EVERY_FRAMES == 0 + + +def _joint_positions_in_isaac_axes( + frame: FullBodyPose, +) -> tuple[np.ndarray, np.ndarray]: + """Read the 24 slots out of the frame and rebase them into Isaac's axes.""" + joints = frame.joints + if joints is None: + count = int(BodyJoint.NUM_JOINTS) + return np.zeros((count, 3)), np.zeros(count, dtype=np.uint8) + positions = np.asarray(joints.positions, dtype=np.float64) @ _OPENXR_TO_ISAAC.T + return positions, np.asarray(joints.is_valid, dtype=np.uint8) + + +def _heading(positions: np.ndarray, valid: np.ndarray) -> Rotation | None: + """The way the operator faces, as a turn about Isaac's vertical from +X, or None. + + Read off the line between the hips, leveled, and not off the pelvis slot's orientation: + the standard human skeleton's pelvis axes have never been measured. None when a hip is + missing or the two have no level distance between them, one above the other. + """ + left, right = int(BodyJoint.LEFT_HIP), int(BodyJoint.RIGHT_HIP) + if not (valid[left] and valid[right]): + return None + across = positions[left] - positions[right] + if np.linalg.norm(across[:2]) < _MIN_SEGMENT_LENGTH_M: + return None + # Isaac's +Y is the left of whoever faces +X, so the operator faces a quarter turn short + # of the way their left hip lies. + return Rotation.from_euler("z", np.arctan2(across[1], across[0]) - np.pi / 2.0) + + +def _torso_pose(positions: np.ndarray, valid: np.ndarray) -> Rotation | None: + """The operator's upper body, turned from upright and facing their heading, or None. + + ``positions`` have the heading taken out, so upright and facing ahead -- MOXI Player's + reference pose -- is the identity. Read positions only: the spine slots' orientations + are unverified. None when a slot is missing, the spine has no length, or the shoulders + have no line across it. + """ + spine1, spine3 = int(BodyJoint.SPINE1), int(BodyJoint.SPINE3) + left, right = int(BodyJoint.LEFT_SHOULDER), int(BodyJoint.RIGHT_SHOULDER) + if not all(valid[slot] for slot in (spine1, spine3, left, right)): + return None + up = positions[spine3] - positions[spine1] + if np.linalg.norm(up) < _MIN_SEGMENT_LENGTH_M: + return None + up = up / np.linalg.norm(up) + across = positions[left] - positions[right] + across = across - np.dot(across, up) * up + if np.linalg.norm(across) < _MIN_SEGMENT_LENGTH_M: + return None + across = across / np.linalg.norm(across) + # Isaac's axes: +X ahead, +Y to the left, +Z up. + return Rotation.from_matrix(np.column_stack([np.cross(across, up), across, up])) + + +def _shared_prefix_length(chain: tuple[int, ...], other: tuple[int, ...]) -> int: + """How many slots ``chain`` and ``other`` have in common before they part.""" + shared = 0 + for slot, other_slot in zip(chain, other): + if slot != other_slot: + break + shared += 1 + return shared + + +def _same_measurement(values: Sequence[float], other: Sequence[float]) -> bool: + return bool( + np.allclose(values, other, rtol=0.0, atol=_SAME_MEASUREMENT_TOLERANCE_M) + ) + + +def _extended_by_another( + target: FrameTargetCfg, targets: Sequence[FrameTargetCfg] +) -> bool: + """Does another task's chain run on past the end of ``target``'s?""" + return any( + len(other.chain) > len(target.chain) + and _shared_prefix_length(target.chain, other.chain) == len(target.chain) + for other in targets + ) + + +def _segment_lengths( + chain: tuple[int, ...], positions: np.ndarray, valid: np.ndarray +) -> tuple[float, ...] | None: + """Meters between consecutive slots of ``chain``, or None if the chain is unusable.""" + if not all(valid[slot] for slot in chain): + return None + lengths = [] + for start, end in zip(chain, chain[1:]): + length = float(np.linalg.norm(positions[end] - positions[start])) + if length < _MIN_SEGMENT_LENGTH_M: + return None + lengths.append(length) + return tuple(lengths) + + +def _received_skeleton( + positions: np.ndarray, valid: np.ndarray +) -> dict[int, np.ndarray]: + """Each usable slot as it arrived, relative to the operator's pelvis, in the pelvis frame. + + ``positions`` already have the operator's heading taken out; the caller draws nothing when + there is none. Empty when the pelvis itself is unusable: it is what the skeleton is placed + by. + """ + pelvis = int(BodyJoint.PELVIS) + if not valid[pelvis]: + return {} + return { + slot: positions[slot] - positions[pelvis] + for slot in range(len(positions)) + if valid[slot] + } + + +def _reconstruct( + target: FrameTargetCfg, + positions: np.ndarray, + valid: np.ndarray, + *, + anchor: np.ndarray, +) -> np.ndarray | None: + """Walk ``target``'s chain out from ``anchor``: operator directions, robot lengths. + + ``anchor`` is already turned with the operator's torso; ``positions`` have the heading + taken out, so their directions read as the robot's pelvis frame. Returns the chain's + points in that frame, anchor first and target last, or None if this frame cannot drive it. + """ + if not all(valid[slot] for slot in target.chain): + return None + points = [np.asarray(anchor, dtype=np.float64)] + for index, (start, end) in enumerate(zip(target.chain, target.chain[1:])): + segment = positions[end] - positions[start] + length = float(np.linalg.norm(segment)) + if length < _MIN_SEGMENT_LENGTH_M: + return None + points.append( + points[-1] + (segment / length) * target.robot_segment_lengths[index] + ) + return np.asarray(points) + + +def _hand_orientation( + target: FrameTargetCfg, frame: FullBodyPose, heading: Rotation +) -> Rotation: + """The operator's hand, turned onto ``target``'s task frame, in the pelvis frame. + + ``heading`` is taken out, as from the positions. The ``WRIST`` slot is read whole. Do not + build the orientation from the rebuilt forearm's direction plus a twist: a bent wrist + takes the hand off the elbow-to-wrist line. + """ + if target.forearm_frame_in_task_xyzw is None: + # Uncosted, but not inert: Pink's frame error is an SE(3) log, whose position part an + # orientation difference turns by up to half its angle. It steers the approach, never + # where the task settles. + return target.neutral_rotation + wrist = target.chain[-1] + published = Rotation.from_quat( + np.asarray(frame.joints.orientations[wrist], dtype=np.float64) + ).as_matrix() + # The WRIST rotation in Isaac's axes is M R M^T, and the constant's columns move by M, so + # the forearm frame in Isaac's world is (M R M^T)(M F) = M R F. + forearm = _OPENXR_TO_ISAAC @ published @ _FOREARM_FRAME_IN_PUBLISHED_WRIST[wrist] + return ( + heading.inv() + * Rotation.from_matrix(forearm) + * target.forearm_frame_in_task.inv() + ) + + +def _fmt_vec(vec: np.ndarray) -> str: + return "[" + ", ".join(f"{value:+.3f}" for value in np.asarray(vec).ravel()) + "]" + + +def _fmt_lengths(lengths: Sequence[float]) -> str: + return "[" + ", ".join(f"{value:.3f}" for value in lengths) + "]" + + +__all__ = [ + "ACTION_OUTPUT", + "DEFAULT_JMEX_G1_SETTINGS", + "FULL_BODY_INPUT", + "ORIENTATION_DIM", + "POSE_DIM", + "POSITION_DIM", + "STANDING_LOCOMOTION_COMMAND", + "FrameTargetCfg", + "G1ActionLayout", + "JmexG1ActionSource", + "JmexG1Settings", + "PelvisPose", + "ReferenceOverlay", + "arm_frame_targets", + "forearm_frame", + "g1_locomanipulation_action_type", + "left_wrist_chain", + "right_wrist_chain", +] diff --git a/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_tasks.py b/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_tasks.py new file mode 100644 index 0000000000..3858b72a22 --- /dev/null +++ b/examples/jmex/python/isaaccapture_examples/jmex/jmex_g1_tasks.py @@ -0,0 +1,797 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Register the j-mex G1 locomanipulation task and wire it to the retargeting pipeline. + +Subclasses ``LocomanipulationG1EnvCfg``, adds the elbows and shoulders to its Pink IK targets, +reads the robot's shoulders, link lengths and waist off the live pinocchio model for +``jmex_g1_retargeting``, and draws that module's reference overlay. README.md ("Teleop: +driving the G1") has the launch command. +""" + +from __future__ import annotations + +import logging +import os +from dataclasses import dataclass, replace +from pathlib import Path +from typing import TYPE_CHECKING, Any + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import registry +from scipy.spatial.transform import Rotation + +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.schema import BodyJoint +from isaaccapture.teleop_session_manager import PluginConfig, RetargetingExecutionConfig +from isaaclab.controllers.pink_ik import FrameTaskCfg, LocalFrameTaskCfg +from isaaclab.utils.configclass import configclass + +try: + from isaaclab_tasks.contrib.locomanip_pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, + ) +except ModuleNotFoundError as error: + # Isaac Lab 3.0.0 beta2 keeps it under manager_based. + if not (error.name or "").startswith("isaaclab_tasks.contrib"): + raise + from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, + ) + +from .jmex_g1_retargeting import ( + ACTION_OUTPUT, + DEFAULT_JMEX_G1_SETTINGS, + POSE_DIM, + FrameTargetCfg, + G1ActionLayout, + JmexG1ActionSource, + JmexG1Settings, + PelvisPose, + ReferenceOverlay, + arm_frame_targets, + forearm_frame, + info_or_print, + left_wrist_chain, + right_wrist_chain, +) + +if TYPE_CHECKING: + import torch + from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.mdp.actions.pink_actions_cfg import ( + PinkInverseKinematicsActionCfg, + ) + from isaaclab.envs.mdp.actions.pink_task_space_actions import ( + PinkInverseKinematicsAction, + ) + +logger = logging.getLogger("isaaccapture.jmex.jmex_g1_tasks") + +TASK_ID = "Isaac-PickPlace-Locomanipulation-G1-Jmex-Abs-v0" + +# Teleoperation runs one environment: one operator produces one action, which the action +# manager broadcasts to every environment there is. +_TELEOP_ENV_INDEX = 0 + +_PLUGIN_NAME = "jmex" +_PLUGIN_ROOT_ID = "jmex" + +# Pink's frame names, which are the URDF's and carry the asset's prefix. Isaac Sim's body +# names for the same links do not -- the two namespaces are not interchangeable, and the +# prefix is not universal within the URDF either. The wrist names are the ones Isaac Lab's +# G1 pink controller config already targets (isaaclab_tasks/.../configs/pink_controller_cfg.py); +# the shoulder and elbow links follow the same naming. +_PINK_PELVIS_LINK = "g1_29dof_with_hand_rev_1_0_pelvis" +_PINK_LEFT_SHOULDER_LINK = "g1_29dof_with_hand_rev_1_0_left_shoulder_pitch_link" +_PINK_LEFT_ELBOW_LINK = "g1_29dof_with_hand_rev_1_0_left_elbow_link" +_PINK_LEFT_WRIST_LINK = "g1_29dof_with_hand_rev_1_0_left_wrist_yaw_link" +_PINK_RIGHT_SHOULDER_LINK = "g1_29dof_with_hand_rev_1_0_right_shoulder_pitch_link" +_PINK_RIGHT_ELBOW_LINK = "g1_29dof_with_hand_rev_1_0_right_elbow_link" +_PINK_RIGHT_WRIST_LINK = "g1_29dof_with_hand_rev_1_0_right_wrist_yaw_link" +# The waist joints the upper body tips on. The yaw joint only reports how far its axis passes +# from the point these two share -- a few millimeters on the G1, which the IK takes up. +_PINK_WAIST_ROLL_JOINT = "waist_roll_joint" +_PINK_WAIST_PITCH_JOINT = "waist_pitch_joint" +_PINK_WAIST_YAW_JOINT = "waist_yaw_joint" +# Further apart than this, the roll and pitch axes do not cross and there is no one point the +# upper body tips about. +_WAIST_AXES_CROSS_WITHIN_M = 1e-3 +# Below this, |roll x pitch| says the two axes run parallel. +_WAIST_AXES_PARALLEL_BELOW = 1e-6 + +# Each arm's body slots and the Pink frames standing for them, one for one, shoulder first. The +# action is laid out in the controller's task order, which this table does not set. +# +# The wrist frame is wrist_yaw_link, after all three of the wrist's roll, pitch and yaw joints. +# The operator's WRIST slot observes all three, so Isaac Lab's scalar orientation_cost applies +# to every axis. +_ARM_SLOTS_AND_FRAMES = ( + ( + left_wrist_chain(), + (_PINK_LEFT_SHOULDER_LINK, _PINK_LEFT_ELBOW_LINK, _PINK_LEFT_WRIST_LINK), + ), + ( + right_wrist_chain(), + (_PINK_RIGHT_SHOULDER_LINK, _PINK_RIGHT_ELBOW_LINK, _PINK_RIGHT_WRIST_LINK), + ), +) + +# The frame tasks put behind the base task's wrists, in this order: the frame, the wrist task +# whose gain and damping it copies, and its share of that wrist's position cost, so the IK +# still places the hand first. Position only: the chain places these frames but does not +# turn them. +_ADDED_FRAME_TASKS = ( + (_PINK_LEFT_ELBOW_LINK, _PINK_LEFT_WRIST_LINK, 1.0 / 2.0), + (_PINK_RIGHT_ELBOW_LINK, _PINK_RIGHT_WRIST_LINK, 1.0 / 2.0), + (_PINK_LEFT_SHOULDER_LINK, _PINK_LEFT_WRIST_LINK, 1.0 / 3.0), + (_PINK_RIGHT_SHOULDER_LINK, _PINK_RIGHT_WRIST_LINK, 1.0 / 3.0), +) + +# The body slots' joint tree, as chains out from the root; consecutive slots are one bone. +_BODY_CHAINS = ( + ( + BodyJoint.PELVIS, + BodyJoint.SPINE1, + BodyJoint.SPINE2, + BodyJoint.SPINE3, + BodyJoint.NECK, + BodyJoint.HEAD, + ), + ( + BodyJoint.SPINE3, + BodyJoint.LEFT_COLLAR, + BodyJoint.LEFT_SHOULDER, + BodyJoint.LEFT_ELBOW, + BodyJoint.LEFT_WRIST, + BodyJoint.LEFT_HAND, + ), + ( + BodyJoint.SPINE3, + BodyJoint.RIGHT_COLLAR, + BodyJoint.RIGHT_SHOULDER, + BodyJoint.RIGHT_ELBOW, + BodyJoint.RIGHT_WRIST, + BodyJoint.RIGHT_HAND, + ), + ( + BodyJoint.PELVIS, + BodyJoint.LEFT_HIP, + BodyJoint.LEFT_KNEE, + BodyJoint.LEFT_ANKLE, + BodyJoint.LEFT_FOOT, + ), + ( + BodyJoint.PELVIS, + BodyJoint.RIGHT_HIP, + BodyJoint.RIGHT_KNEE, + BodyJoint.RIGHT_ANKLE, + BodyJoint.RIGHT_FOOT, + ), +) + +# The overlay's two colors (RGBA): blue is the body pose as it arrived, orange the arms +# rebuilt from it. The README says how to read them. +_RECEIVED_COLOR = (0.25, 0.65, 1.0, 1.0) +_REBUILT_COLOR = (1.0, 0.55, 0.1, 1.0) +# Debug draw sizes are screen pixels, so they stay legible however far the camera is. +_LINE_WIDTH_PX = 2.0 +_JOINT_POINT_PX = 6.0 +_IK_TARGET_POINT_PX = 14.0 + +# How far along +Y, in meters, the packing table and the steering wheel on it are parked. Both +# move together, so the wheel still rests on the table. +_PARK_OFFSET_Y = 5.0 + + +class _JmexRuntime: + """What the pipeline needs from the running robot: its layout, env and live pelvis. + + The layout is only measurable once Isaac Lab has built the Pink IK controller, and the + pipeline is built later still, when the teleop session starts. + """ + + def __init__(self) -> None: + self._layout: G1ActionLayout | None = None + self._env: Any | None = None + self._pelvis = PelvisPose( + position=np.zeros(3), orientation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]) + ) + + def publish_env(self, env: Any) -> None: + self._env = env + + def env(self) -> Any | None: + return self._env + + def env_origin(self) -> np.ndarray: + """The teleoperated environment's origin in the simulation world.""" + if self._env is None: + return np.zeros(3) + origin = self._env.scene.env_origins[_TELEOP_ENV_INDEX] + return origin.detach().cpu().numpy().astype(np.float64) + + def publish_layout(self, layout: G1ActionLayout) -> None: + self._layout = layout + + def layout(self) -> G1ActionLayout: + """The action layout, sized from the robot the environment actually built.""" + if self._layout is None: + raise RuntimeError( + "The j-mex pipeline was built before the Pink IK action term measured the " + "robot. Launch through the registered task id so the environment is " + f"constructed first: --task {TASK_ID}" + ) + return self._layout + + def publish_pelvis( + self, position: np.ndarray, orientation_xyzw: np.ndarray + ) -> None: + self._pelvis = PelvisPose( + position=np.asarray(position, dtype=np.float64), + orientation_xyzw=np.asarray(orientation_xyzw, dtype=np.float64), + ) + + def pelvis(self) -> PelvisPose: + return self._pelvis + + +_RUNTIME = _JmexRuntime() + + +class _ReferenceOverlayDrawer: + """Draws each frame's ``ReferenceOverlay`` with Kit's debug draw. + + Where there is no Kit debug draw -- a visualizer other than Kit, or no extension -- or + drawing fails, it says so once and draws nothing more; the task runs the same either way. + """ + + def __init__(self) -> None: + self._draw: Any | None = None + self._off = False + + def __call__(self, overlay: ReferenceOverlay) -> None: + draw = self._debug_draw() + if draw is None: + return + # The overlay is in the per-env world frame; debug draw is in the simulation's. + origin = _RUNTIME.env_origin() + starts: list[list[float]] = [] + ends: list[list[float]] = [] + line_colors: list[tuple[float, ...]] = [] + points: list[list[float]] = [] + point_colors: list[tuple[float, ...]] = [] + point_sizes: list[float] = [] + + def line(start: np.ndarray, end: np.ndarray, color: tuple[float, ...]) -> None: + starts.append((start + origin).tolist()) + ends.append((end + origin).tolist()) + line_colors.append(color) + + def point(at: np.ndarray, color: tuple[float, ...], size: float) -> None: + points.append((at + origin).tolist()) + point_colors.append(color) + point_sizes.append(size) + + skeleton = overlay.skeleton + for chain in _BODY_CHAINS: + for parent, child in zip(chain, chain[1:]): + if int(parent) in skeleton and int(child) in skeleton: + line(skeleton[int(parent)], skeleton[int(child)], _RECEIVED_COLOR) + for at in skeleton.values(): + point(at, _RECEIVED_COLOR, _JOINT_POINT_PX) + for chain in overlay.chains.values(): + for start, end in zip(chain, chain[1:]): + line(start, end, _REBUILT_COLOR) + for at in chain[:-1]: + point(at, _REBUILT_COLOR, _JOINT_POINT_PX) + point(chain[-1], _REBUILT_COLOR, _IK_TARGET_POINT_PX) + + try: + draw.clear_lines() + draw.clear_points() + if starts: + draw.draw_lines( + starts, ends, line_colors, [_LINE_WIDTH_PX] * len(starts) + ) + draw.draw_points(points, point_colors, point_sizes) + except Exception as exc: + self._turn_off(f"drawing failed ({type(exc).__name__}: {exc})") + + def _debug_draw(self) -> Any | None: + if self._draw is not None or self._off: + return self._draw + try: + import omni.kit.app + + omni.kit.app.get_app().get_extension_manager().set_extension_enabled_immediate( + "isaacsim.util.debug_draw", True + ) + from isaacsim.util.debug_draw import _debug_draw + + draw = _debug_draw.acquire_debug_draw_interface() + # Outside Kit any of the three steps can fail, each in its own way. + except Exception as exc: + self._turn_off(f"no Kit debug draw ({type(exc).__name__}: {exc})") + return None + if draw is None: + self._turn_off("no Kit debug draw") + return None + self._draw = draw + info_or_print( + logger, + "drawing the reference overlay -- blue is the body pose as received, in the " + "operator's proportions, and is not meant to line up with the robot; orange is each " + "arm rebuilt in the robot's proportions, and its large dots are what the IK is told " + "to reach -- the robot's wrist, elbow and shoulder should sit on them.", + ) + return draw + + def _turn_off(self, reason: str) -> None: + self._off = True + self._draw = None + logger.warning("%s; the reference overlay is off", reason) + + +@dataclass(frozen=True) +class JmexG1TaskSettings: + """Everything the task reads at startup.""" + + device: JmexG1Settings = DEFAULT_JMEX_G1_SETTINGS + teleoperation_active_default: bool = True + + +DEFAULT_JMEX_G1_TASK_SETTINGS = JmexG1TaskSettings() + + +def _env_bool(name: str, default: bool) -> bool: + value = os.environ.get(name) + if value is None: + return default + return value.lower() not in {"0", "false", "no", "off"} + + +def _settings_from_env() -> JmexG1TaskSettings: + default = DEFAULT_JMEX_G1_TASK_SETTINGS + device = replace( + default.device, + collection_id=os.environ.get( + "JMEX_COLLECTION_ID", default.device.collection_id + ), + channel=int(os.environ.get("JMEX_CHANNEL", default.device.channel)), + plugin_auto_launch=_env_bool( + "JMEX_PLUGIN_AUTO_LAUNCH", default.device.plugin_auto_launch + ), + ) + return replace(default, device=device) + + +def _plugin_search_paths() -> list[Path]: + # The repository root: this file sits in examples/jmex/python/isaaccapture_examples/jmex/. + base = Path(__file__).resolve().parents[5] + candidates = [base / "plugins", base / "install" / "plugins"] + return [path for path in candidates if path.exists()] + + +def _plugin_configs(settings: JmexG1TaskSettings) -> list[PluginConfig]: + if not settings.device.plugin_auto_launch: + return [] + search_paths = _plugin_search_paths() + if not search_paths: + raise RuntimeError( + "jmex plugin directory not found. Run `cmake --install build`, or set " + "JMEX_PLUGIN_AUTO_LAUNCH=0 to drive a manually started plugin." + ) + return [ + # The rest of the receiver's settings live in src/plugins/jmex/plugin.yaml. These + # two are appended after it, and the last value of a flag wins. + PluginConfig( + plugin_name=_PLUGIN_NAME, + plugin_root_id=_PLUGIN_ROOT_ID, + search_paths=search_paths, + plugin_args=[ + f"--channel={settings.device.channel}", + f"--collection-id={settings.device.collection_id}", + ], + # Refuse to start rather than wait for a pairing that cannot happen: a build without + # the SDK installs the other plugins but not this one. + required=True, + ) + ] + + +def _measure_frame_targets( + pink_configuration: Any, frame_names: list[str] +) -> tuple[FrameTargetCfg, ...]: + """Read each named frame task's arm off Pink's pinocchio model, in the order named. + + Reads a private ``Data`` at the model's rest configuration, leaving the controller's live + state alone. Each shoulder read there is its arm's anchor before the operator's torso pose + turns it about the waist (``_measure_waist_pivot``). + """ + import pinocchio as pin + + model, data = _rest_kinematics(pink_configuration) + + def placement(name: str) -> pin.SE3: + if not model.existFrame(name): + raise RuntimeError( + f"Frame '{name}' is not in the Pink kinematics model. Pink uses the URDF's " + "prefixed names, which are not Isaac Sim's body names." + ) + return data.oMf[model.getFrameId(name)] + + def axis_moving(name: str) -> np.ndarray: + """World axis of the joint that moves frame ``name``: its Jacobian's angular column.""" + joint = model.frames[model.getFrameId(name)].parentJoint + jacobian = pin.getJointJacobian( + model, data, joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + ) + return np.asarray(jacobian[3:, model.joints[joint].idx_v], dtype=np.float64) + + pelvis = placement(_PINK_PELVIS_LINK) + by_frame: dict[str, FrameTargetCfg] = {} + for chain, frames in _ARM_SLOTS_AND_FRAMES: + placements = [placement(frame) for frame in frames] + lengths = tuple( + float(np.linalg.norm(nxt.translation - cur.translation)) + for cur, nxt in zip(placements, placements[1:]) + ) + # The action term expresses every target in the pelvis frame before handing it to + # the task, so the anchor, the rest poses and the forearm are measured there too. + at_rest = [pelvis.actInv(p) for p in placements] + shoulder, elbow, wrist = at_rest + forearm_in_task = Rotation.from_matrix(wrist.rotation).inv() * forearm_frame( + shoulder.translation, + elbow.translation, + wrist.translation, + elbow_axis=pelvis.rotation.T @ axis_moving(frames[1]), + ) + arm = arm_frame_targets( + chain, + anchor_position_pelvis=shoulder.translation, + robot_segment_lengths=lengths, + rest_poses_pelvis=[ + (rest.translation, Rotation.from_matrix(rest.rotation).as_quat()) + for rest in at_rest + ], + forearm_frame_in_task_xyzw=forearm_in_task.as_quat(), + ) + by_frame.update(zip(frames, arm)) + anchor_m = [round(float(v), 4) for v in shoulder.translation] + axes = forearm_in_task.as_matrix() + info_or_print( + logger, + "robot %s hangs from %s m (pelvis frame), chain %s m (reach %s m from there); in " + "its task frame the forearm runs %s and the elbow hinges about %s", + BodyJoint(chain[-1]).name.lower(), + anchor_m, + [round(v, 4) for v in lengths], + round(sum(lengths), 4), + np.round(axes[:, 0], 3).tolist(), + np.round(axes[:, 1], 3).tolist(), + ) + unknown = [name for name in frame_names if name not in by_frame] + if unknown: + raise RuntimeError( + f"The Pink controller has frame tasks on {unknown}, which no body slot drives. " + "Each frame task takes seven numbers of the action, so none can be left out." + ) + return tuple(by_frame[name] for name in frame_names) + + +def _rest_kinematics(pink_configuration: Any) -> tuple[Any, Any]: + """Pink's pinocchio model, and a private ``Data`` for it at the model's rest configuration.""" + import pinocchio as pin + + model = pink_configuration.full_model + data = model.createData() + q_rest = np.asarray(pink_configuration.robot_wrapper.q0, dtype=np.float64) + pin.forwardKinematics(model, data, q_rest) + pin.updateFramePlacements(model, data) + pin.computeJointJacobians(model, data, q_rest) + return model, data + + +def _measure_waist_pivot(pink_configuration: Any) -> tuple[float, float, float]: + """Where the waist's roll and pitch axes cross, in the pelvis frame at rest. + + It is the point the upper body tips about, measured off the same rest configuration as the + shoulders and the link lengths. + """ + import pinocchio as pin + + model, data = _rest_kinematics(pink_configuration) + pelvis = data.oMf[model.getFrameId(_PINK_PELVIS_LINK)] + + def axis_line(name: str) -> tuple[np.ndarray, np.ndarray]: + """A point on joint ``name``'s axis and the axis's direction, in the pelvis frame.""" + if not model.existJointName(name): + raise RuntimeError( + f"Joint '{name}' is not in the Pink kinematics model, so there is no waist " + "for the operator's torso to turn." + ) + joint = model.getJointId(name) + jacobian = pin.getJointJacobian( + model, data, joint, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED + ) + direction = np.asarray( + jacobian[3:, model.joints[joint].idx_v], dtype=np.float64 + ) + return ( + pelvis.actInv(data.oMi[joint]).translation, + pelvis.rotation.T @ (direction / np.linalg.norm(direction)), + ) + + roll_at, roll = axis_line(_PINK_WAIST_ROLL_JOINT) + pitch_at, pitch = axis_line(_PINK_WAIST_PITCH_JOINT) + square = np.cross(roll, pitch) + if np.linalg.norm(square) < _WAIST_AXES_PARALLEL_BELOW: + raise RuntimeError( + "The waist's roll and pitch axes are parallel, so they share no point to tip " + "about." + ) + # The two points, one on each axis, joined by a line square to both. + along = np.linalg.solve(np.column_stack([roll, -pitch, square]), pitch_at - roll_at) + on_roll = roll_at + along[0] * roll + on_pitch = pitch_at + along[1] * pitch + if np.linalg.norm(on_roll - on_pitch) > _WAIST_AXES_CROSS_WITHIN_M: + raise RuntimeError( + "The waist's roll and pitch axes pass " + f"{np.linalg.norm(on_roll - on_pitch) * 1000.0:.1f} mm apart, so there is no one " + "point the upper body tips about." + ) + pivot = (on_roll + on_pitch) / 2.0 + yaw_at, yaw = axis_line(_PINK_WAIST_YAW_JOINT) + off_yaw = (pivot - yaw_at) - np.dot(pivot - yaw_at, yaw) * yaw + + def limits_deg(name: str) -> str: + """Joint ``name``'s range in the model the IK solves on, which is what bounds it.""" + index = model.joints[model.getJointId(name)].idx_q + lower, upper = np.degrees( + [model.lowerPositionLimit[index], model.upperPositionLimit[index]] + ) + return f"[{lower:+.1f}, {upper:+.1f}] deg" + + info_or_print( + logger, + "robot upper body turns about %s m (pelvis frame), where the waist's roll and pitch " + "axes cross; the yaw axis passes %.1f mm from it, which the IK takes up. The IK keeps " + "the waist's pitch within %s, its roll within %s and its yaw within %s; a torso pose " + "past them is not followed further", + [round(float(v), 4) for v in pivot], + np.linalg.norm(off_yaw) * 1000.0, + limits_deg(_PINK_WAIST_PITCH_JOINT), + limits_deg(_PINK_WAIST_ROLL_JOINT), + limits_deg(_PINK_WAIST_YAW_JOINT), + ) + return tuple(float(v) for v in pivot) + + +def _insert_arm_frame_tasks(controller: Any) -> None: + """Put the elbow and shoulder tasks right behind the controller's own frame tasks.""" + tasks = list(controller.variable_input_tasks) + leading = _leading_frame_tasks(tasks, (FrameTaskCfg, LocalFrameTaskCfg)) + by_frame = {task.frame: task for task in leading} + added = [] + for frame, wrist_frame, share in _ADDED_FRAME_TASKS: + wrist = by_frame.get(wrist_frame) + if not isinstance(wrist, LocalFrameTaskCfg): + raise RuntimeError( + f"The base Pink controller has no leading LocalFrameTask on {wrist_frame}, " + f"which the task on {frame} takes its settings from." + ) + added.append( + LocalFrameTaskCfg( + frame=frame, + base_link_frame_name=wrist.base_link_frame_name, + position_cost=float(wrist.position_cost) * share, + orientation_cost=0.0, + lm_damping=wrist.lm_damping, + gain=wrist.gain, + ) + ) + controller.variable_input_tasks = leading + added + tasks[len(leading) :] + + +def _leading_frame_tasks( + tasks: list[Any], frame_task_cls: type | tuple[type, ...] +) -> list[Any]: + """The controller's frame tasks, which must be a prefix of its task list. + + The action term locates each task's slot with ``enumerate`` over the whole list and + does not compress the index, so a frame task sitting behind a non-frame one reads the + wrong seven numbers first and only raises later, if at all. + """ + frame_indices = [ + index for index, task in enumerate(tasks) if isinstance(task, frame_task_cls) + ] + if frame_indices != list(range(len(frame_indices))): + raise RuntimeError( + "Pink frame tasks must occupy the front of variable_input_tasks; found them " + f"at {frame_indices} of {len(tasks)} tasks." + ) + return tasks[: len(frame_indices)] + + +def _assert_action_dim(layout: G1ActionLayout) -> None: + """Check the pipeline's action length against the environment's, before the first frame. + + Unchecked, a mismatch surfaces on the first step as a bare shape ``ValueError`` from the + action manager, which does not say which side is wrong. Skipped without a running + environment, as in the tests. + """ + env = _RUNTIME.env() + if env is None: + return + total = env.action_manager.total_action_dim + if layout.action_dim != total: + raise RuntimeError( + "j-mex action size disagrees with the environment: the pipeline produces " + f"{layout.action_dim} from {len(layout.frame_targets)} frame tasks, " + f"{layout.num_hand_joints} finger joints and {layout.num_locomotion} " + f"locomotion elements, but the action manager expects {total}. A task added to " + "the Pink controller without a matching frame target lands here." + ) + + +def _build_jmex_pink_ik_action_class() -> type[PinkInverseKinematicsAction]: + """Built lazily, so importing this module does not pull in Isaac Lab's action stack.""" + from isaaclab.envs.mdp.actions.pink_task_space_actions import ( + PinkInverseKinematicsAction, + ) + + # The class the action term counts to give a task seven numbers of the action. + from pink.tasks import FrameTask + + class JmexPinkInverseKinematicsAction(PinkInverseKinematicsAction): + """Isaac Lab's Pink IK action, plus the two things the j-mex pipeline needs from it. + + It measures the robot once at startup and republishes the pelvis's live world pose + every step. Both go to ``_RUNTIME``; neither changes how the IK behaves. + """ + + def __init__( + self, cfg: PinkInverseKinematicsActionCfg, env: ManagerBasedEnv + ) -> None: + super().__init__(cfg, env) + controller = self._ik_controllers[0] + frame_tasks = _leading_frame_tasks( + controller.cfg.variable_input_tasks, FrameTask + ) + _RUNTIME.publish_env(env) + _RUNTIME.publish_layout( + G1ActionLayout( + frame_targets=_measure_frame_targets( + controller.pink_configuration, + [task.frame for task in frame_tasks], + ), + waist_pivot_pelvis=_measure_waist_pivot( + controller.pink_configuration + ), + ) + ) + self._publish_pelvis() + + def _publish_pelvis(self) -> None: + """Hand the pipeline the pelvis in the per-env world frame the action is read in. + + The action term subtracts the env origin from the pelvis before inverting it, so + subtract it here too; otherwise every target is off by the env origin, which is + zero for a single environment but not in general. + """ + data = self._env.scene[self.cfg.controller.articulation_name].data + pose = data.body_link_pose_w.torch[_TELEOP_ENV_INDEX, self._base_link_idx] + pose = pose.detach().cpu().numpy() + _RUNTIME.publish_pelvis(pose[:3] - _RUNTIME.env_origin(), pose[3:7]) + + def process_actions(self, actions: torch.Tensor) -> None: + # Read before the base class consumes the action, so the pose the pipeline + # composes onto is at most one step behind the pose it is decomposed from. + self._publish_pelvis() + super().process_actions(actions) + + return JmexPinkInverseKinematicsAction + + +def build_jmex_g1_locomanipulation_pipeline( + settings: JmexG1TaskSettings = DEFAULT_JMEX_G1_TASK_SETTINGS, +) -> OutputCombiner: + """Build the one-source Isaac Teleop pipeline this task takes its action from.""" + layout = _RUNTIME.layout() + _assert_action_dim(layout) + info_or_print( + logger, + "the action is %d long -- %d for each of %s, then %d fingers and %d locomotion", + layout.action_dim, + POSE_DIM, + ", ".join(target.name for target in layout.frame_targets), + layout.num_hand_joints, + layout.num_locomotion, + ) + source = JmexG1ActionSource( + layout=layout, + pelvis_pose_provider=_RUNTIME.pelvis, + settings=settings.device, + overlay_sink=_ReferenceOverlayDrawer(), + ) + return OutputCombiner({ACTION_OUTPUT: source.output(ACTION_OUTPUT)}) + + +def register_tasks() -> None: + """Register the j-mex G1 locomanipulation task with Gymnasium. + + It parses no arguments and leaves them all to the launcher. + """ + if TASK_ID not in registry: + gym.register( + id=TASK_ID, + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={"env_cfg_entry_point": f"{__name__}:JmexLocomanipulationG1EnvCfg"}, + disable_env_checker=True, + ) + + +@configclass +class JmexLocomanipulationG1EnvCfg(LocomanipulationG1EnvCfg): + """Isaac Lab's G1 locomanipulation task, driven from a j-mex AgileMaster suit. + + The elbows and shoulders join the wrists as Pink IK tasks; the factory tasks and their weights, + the finger channel and the lower-body policy stay as Isaac Lab ships them. + """ + + def __post_init__(self) -> None: + """Take the base configuration first, then swap in the j-mex pipeline.""" + super().__post_init__() + settings = _settings_from_env() + + # Without this the agile lower-body policy crouches over the first steps, and the + # IK then cannot reach the targets it is being given. + self.scene.robot.spawn.articulation_props.fix_root_link = True + + # The robot rests with its hands over the table, and hands following an operator's + # lowered arms turn down into it and stay caught there. Park the table and the wheel + # instead of deleting them: the observations read the wheel. + for asset in (self.scene.packing_table, self.scene.object): + x, y, z = asset.init_state.pos + asset.init_state.pos = (x, y + _PARK_OFFSET_Y, z) + # The wheel is now more than 1 m from the robot, which ends the episode on the first + # step, and the pick-and-place checks have nothing left to judge. The time limit would + # reset the robot every 20 s. + self.terminations.object_dropping = None + self.terminations.object_too_far = None + self.terminations.success = None + self.terminations.time_out = None + + # Seed the pelvis from the spawn pose, so the frames before the first physics step + # already carry the robot's yaw instead of an assumed identity. `rot` is xyzw. + _RUNTIME.publish_pelvis( + np.asarray(self.scene.robot.init_state.pos, dtype=np.float64), + np.asarray(self.scene.robot.init_state.rot, dtype=np.float64), + ) + + _insert_arm_frame_tasks(self.actions.upper_body_ik.controller) + self.actions.upper_body_ik.class_type = _build_jmex_pink_ik_action_class() + + self.isaac_teleop.pipeline_builder = lambda: ( + build_jmex_g1_locomanipulation_pipeline(settings) + ) + self.isaac_teleop.plugins = _plugin_configs(settings) + self.isaac_teleop.retargeting_execution = RetargetingExecutionConfig( + mode="sync" + ) + self.isaac_teleop.teleoperation_active_default = ( + settings.teleoperation_active_default + ) + self.isaac_teleop.control_channel_uuid = None + self.isaac_teleop.app_name = "IsaacLabJmexG1" + + +__all__ = [ + "TASK_ID", + "JmexG1TaskSettings", + "JmexLocomanipulationG1EnvCfg", + "build_jmex_g1_locomanipulation_pipeline", + "register_tasks", +] diff --git a/examples/jmex/python/isaaccapture_examples/jmex/record_jmex_full_body.py b/examples/jmex/python/isaaccapture_examples/jmex/record_jmex_full_body.py new file mode 100644 index 0000000000..8047e17671 --- /dev/null +++ b/examples/jmex/python/isaaccapture_examples/jmex/record_jmex_full_body.py @@ -0,0 +1,194 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Record the operator's body pose to a standard Isaac Capture full-body MCAP. + +The recording is the standard ``core.FullBodyPoseRecord`` schema on the standard ``full_body`` +channel, so ``python -m isaaccapture_examples.mcap_record_replay.replay_full_body`` plays it back +unmodified. +""" + +from __future__ import annotations + +import argparse +import sys +import time +from datetime import datetime +from pathlib import Path + +import numpy as np + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio import McapRecordingConfig, TrackerVendor +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + FullBodySource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.retargeting_engine.tensor_types import FullBodyInputIndex +from isaaccapture.schema import BodyJoint +from isaaccapture.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, +) + + +PLUGIN_NAME = "jmex" +PLUGIN_ROOT_ID = "jmex" +JMEX_VENDOR_ID = "body.jmex" + +# plugin.yaml's defaults. +DEFAULT_COLLECTION_ID = "jmex_full_body" +DEFAULT_CHANNEL = 1 + + +def _plugin_search_paths() -> list[Path]: + # The repository root: this file sits in examples/jmex/python/isaaccapture_examples/jmex/. + base = Path(__file__).resolve().parents[5] + candidates = [ + base / "plugins", + base / "install" / "plugins", + ] + return [path for path in candidates if path.exists()] + + +def _build_pipeline(collection_id: str) -> OutputCombiner: + controllers = ControllersSource(name="controllers") + full_body = FullBodySource( + name="full_body", + vendor=TrackerVendor(JMEX_VENDOR_ID, {"collection_id": collection_id}), + ) + # The same three outputs common.build_full_body_pipeline() declares, so the pipeline the replay + # script builds finds every channel this recorded. + return OutputCombiner( + { + "controller_left": controllers.output(ControllersSource.LEFT), + "controller_right": controllers.output(ControllersSource.RIGHT), + "full_body": full_body.output(FullBodySource.FULL_BODY), + } + ) + + +def _resolve_output(path_arg: str | None) -> Path: + if path_arg: + path = Path(path_arg) + else: + # Under the working directory, as mcap_record_replay does: next to this file is inside + # the package, and inside site-packages once installed. + out_dir = Path.cwd() / "recordings" + path = out_dir / f"jmex_full_body_{datetime.now():%Y%m%d_%H%M%S}.mcap" + path.parent.mkdir(parents=True, exist_ok=True) + return path + + +def main(argv: list[str]) -> int: + """Parse ``argv``, record for the requested duration, and return the exit status.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "duration", nargs="?", type=float, default=5.0, help="Recording duration (s)" + ) + parser.add_argument( + "output", + nargs="?", + help="Output .mcap path (default: ./recordings/jmex_full_body_.mcap)", + ) + parser.add_argument( + "--collection-id", + default=DEFAULT_COLLECTION_ID, + help=( + "The collection the plugin publishes the body pose on, and the one this " + f"reads from (default: {DEFAULT_COLLECTION_ID})" + ), + ) + parser.add_argument( + "--channel", + type=int, + default=DEFAULT_CHANNEL, + help=( + "MOXI Player channel the plugin pairs on. Player's file replay is fixed on 255 " + f"(default: {DEFAULT_CHANNEL})" + ), + ) + parser.add_argument( + "--no-plugin", + action="store_true", + help=( + "Do not auto-launch jmex_plugin; use an already running one. Also how to pass the " + "receiver's other settings (--bind-ip, --broadcast-ip, --tcp-port, " + "--udp-start-port), which this script does not forward." + ), + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args(argv[1:]) + + mcap_path = _resolve_output(args.output) + plugins: list[PluginConfig] = [] + if not args.no_plugin: + search_paths = _plugin_search_paths() + if not search_paths: + sys.exit( + "[record] error: no installed plugin directory found. " + "Run `cmake --install build` or pass `--no-plugin` for a manually " + "started plugin." + ) + plugins.append( + PluginConfig( + plugin_name=PLUGIN_NAME, + plugin_root_id=PLUGIN_ROOT_ID, + search_paths=search_paths, + plugin_args=[ + f"--channel={args.channel}", + f"--collection-id={args.collection_id}", + ], + required=True, + ) + ) + + config = TeleopSessionConfig( + app_name="JmexFullBodyRecordExample", + pipeline=_build_pipeline(args.collection_id), + plugins=plugins, + mcap_config=McapRecordingConfig(str(mcap_path)), + ) + + joint_count = int(BodyJoint.NUM_JOINTS) + print(f"[record] writing {mcap_path} for {args.duration:.1f}s") + print(f"[record] collection_id={args.collection_id}") + + with CloudXRLauncher.launch_context(args) as launcher: + if launcher.owns_runtime: + print( + f"[record] CloudXR runtime started (WSS log: {launcher.wss_log_path})" + ) + with TeleopSession(config) as session: + start = time.time() + while time.time() - start < args.duration: + result = session.step() + if session.frame_count % 60 == 0: + full_body = result["full_body"] + n_valid = ( + 0 + if full_body.is_none + else int( + np.count_nonzero( + np.asarray( + full_body[FullBodyInputIndex.JOINT_VALID], + dtype=np.uint8, + ) + ) + ) + ) + print( + f"[record] t={time.time() - start:5.2f}s " + f"frame={session.frame_count} " + f"joints={n_valid:02d}/{joint_count}" + ) + time.sleep(1 / 60) + + print(f"[record] done - {mcap_path}") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index 6f2788cbe3..1f90929ec0 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -14,6 +14,7 @@ add_library(live_trackers STATIC live_full_body_tracker_pico_impl.cpp live_full_body_tracker_noitom_impl.cpp live_full_body_tracker_xsens_impl.cpp + live_full_body_tracker_jmex_impl.cpp live_tensor_push_tracker_impl.cpp live_haptic_command_reader_tracker_impl.cpp live_keyboard_tracker_impl.cpp @@ -29,6 +30,7 @@ add_library(live_trackers STATIC live_full_body_tracker_pico_impl.hpp live_full_body_tracker_noitom_impl.hpp live_full_body_tracker_xsens_impl.hpp + live_full_body_tracker_jmex_impl.hpp live_tensor_push_tracker_impl.hpp live_haptic_command_reader_tracker_impl.hpp live_keyboard_tracker_impl.hpp diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index 080ea5a4e4..e7be035fdb 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -87,6 +87,7 @@ class LiveDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTracker* tracker); std::unique_ptr create_full_body_tracker_noitom_impl(const FullBodyTracker* tracker); std::unique_ptr create_full_body_tracker_xsens_impl(const FullBodyTracker* tracker); + std::unique_ptr create_full_body_tracker_jmex_impl(const FullBodyTracker* tracker); std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); std::unique_ptr create_haptic_command_reader_tracker_impl( const HapticCommandReaderTracker* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index b0008ac1e5..bd5aa6dd3c 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -5,6 +5,7 @@ #include "generated_live_includes.inc" #include "live_controller_tracker_impl.hpp" +#include "live_full_body_tracker_jmex_impl.hpp" #include "live_full_body_tracker_noitom_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" #include "live_full_body_tracker_xsens_impl.hpp" @@ -101,6 +102,12 @@ std::unique_ptr try_create_full_body_xsens_impl(LiveDeviceIOFactor return typed ? factory.create_full_body_tracker_xsens_impl(typed) : nullptr; } +std::unique_ptr try_create_full_body_jmex_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_jmex_impl(typed) : nullptr; +} + std::unique_ptr try_create_tensor_push_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -161,6 +168,8 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { &try_create_full_body_noitom_impl, LiveFullBodyTrackerNoitomImpl::VENDOR_ID), make_dispatch_entry( &try_create_full_body_xsens_impl, LiveFullBodyTrackerXsensImpl::VENDOR_ID), + make_dispatch_entry( + &try_create_full_body_jmex_impl, LiveFullBodyTrackerJmexImpl::VENDOR_ID), make_dispatch_entry(&try_create_tensor_push_impl), make_dispatch_entry( &try_create_haptic_command_reader_impl), @@ -298,6 +307,10 @@ void validate_vendor_selections(const std::vector LiveDeviceIOFactory::create_full_body_trac return std::make_unique(handles_, *vendor, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_jmex_impl(const FullBodyTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFullBodyTrackerPicoImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + + const TrackerVendor* vendor = find_vendor(tracker); + assert(vendor && vendor->id == LiveFullBodyTrackerJmexImpl::VENDOR_ID); + return std::make_unique(handles_, *vendor, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_tensor_push_tracker_impl(const TensorPushTracker* tracker) { return std::make_unique(handles_, tracker); diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.cpp b/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.cpp new file mode 100644 index 0000000000..cf39921047 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.cpp @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_full_body_tracker_jmex_impl.hpp" + +#include +#include +#include + +namespace core +{ + +namespace +{ + +constexpr std::string_view COLLECTION_ID_PARAM = "collection_id"; + +SchemaTrackerConfig make_jmex_tensor_config(const TrackerVendor& vendor) +{ + if (vendor.id != LiveFullBodyTrackerJmexImpl::VENDOR_ID) + { + throw std::invalid_argument("j-mex full-body vendor id must be '" + + std::string(LiveFullBodyTrackerJmexImpl::VENDOR_ID) + "'"); + } + + for (const auto& [key, value] : vendor.params) + { + (void)value; + if (key != COLLECTION_ID_PARAM) + { + throw std::invalid_argument("j-mex full-body vendor does not support parameter '" + key + "'"); + } + } + + SchemaTrackerConfig config; + config.collection_id = std::string(LiveFullBodyTrackerJmexImpl::DEFAULT_COLLECTION_ID); + config.max_flatbuffer_size = LiveFullBodyTrackerJmexImpl::MAX_FLATBUFFER_SIZE; + config.tensor_identifier = std::string(LiveFullBodyTrackerJmexImpl::TENSOR_IDENTIFIER); + config.localized_name = "j-mex Full Body"; + + if (auto it = vendor.params.find(std::string(COLLECTION_ID_PARAM)); it != vendor.params.end()) + { + if (it->second.empty()) + { + throw std::invalid_argument("j-mex full-body collection_id must not be empty"); + } + config.collection_id = it->second; + } + + return config; +} + +} // namespace + +void LiveFullBodyTrackerJmexImpl::validate_vendor(const TrackerVendor& vendor) +{ + (void)make_jmex_tensor_config(vendor); +} + +LiveFullBodyTrackerJmexImpl::LiveFullBodyTrackerJmexImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + schema_reader_(handles, make_jmex_tensor_config(vendor), mcap_channels_.get(), /*mcap_channel_index=*/0) +{ +} + +void LiveFullBodyTrackerJmexImpl::update(int64_t /*monotonic_time_ns*/) +{ + schema_reader_.update(tracked_); +} + +const Serialized& LiveFullBodyTrackerJmexImpl::get_body_pose() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.hpp new file mode 100644 index 0000000000..fc8d400b91 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_jmex_impl.hpp @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using FullBodyMcapChannels = McapTrackerChannels; +using FullBodyJmexSchemaTracker = SchemaTracker; + +// Live full-body impl for the "body.jmex" vendor: the j-mex plugin resolves the 24 slots from the +// standard human skeleton MOXI Player has loaded and pushes them on a tensor collection, already +// in OpenXR axes, so this side only reads. +class LiveFullBodyTrackerJmexImpl : public IFullBodyTrackerImpl +{ +public: + static constexpr std::string_view VENDOR_ID = "body.jmex"; + // The plugin's own --collection-id default, used verbatim on both sides. + static constexpr std::string_view DEFAULT_COLLECTION_ID = "jmex_full_body"; + static constexpr std::string_view TENSOR_IDENTIFIER = "full_body"; + static constexpr size_t MAX_FLATBUFFER_SIZE = 16 * 1024; + + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + + static void validate_vendor(const TrackerVendor& vendor); + + LiveFullBodyTrackerJmexImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels); + + LiveFullBodyTrackerJmexImpl(const LiveFullBodyTrackerJmexImpl&) = delete; + LiveFullBodyTrackerJmexImpl& operator=(const LiveFullBodyTrackerJmexImpl&) = delete; + LiveFullBodyTrackerJmexImpl(LiveFullBodyTrackerJmexImpl&&) = delete; + LiveFullBodyTrackerJmexImpl& operator=(LiveFullBodyTrackerJmexImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const Serialized& get_body_pose() const override; + +private: + std::unique_ptr mcap_channels_; + FullBodyJmexSchemaTracker schema_reader_; + Serialized tracked_; +}; + +} // namespace core diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 4e5fa231f1..b72b5359ac 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -25,6 +25,7 @@ add_subdirectory(so101_leader) add_subdirectory(rebot_devarm_leader) add_subdirectory(manus) add_subdirectory(haptikos) +add_subdirectory(jmex) if(BUILD_PLUGIN_NOITOM_MOCAP) add_subdirectory(noitom_mocap) diff --git a/src/plugins/jmex/.gitignore b/src/plugins/jmex/.gitignore new file mode 100644 index 0000000000..c8133e0670 --- /dev/null +++ b/src/plugins/jmex/.gitignore @@ -0,0 +1,5 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Vendor MOXI Receiver SDK (user-provided; do not commit) +MOXIReceiverSDK*/ diff --git a/src/plugins/jmex/CMakeLists.txt b/src/plugins/jmex/CMakeLists.txt new file mode 100644 index 0000000000..d5d9c626d8 --- /dev/null +++ b/src/plugins/jmex/CMakeLists.txt @@ -0,0 +1,52 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping jmex plugin (Linux only)") + return() +endif() + +# The MOXI Receiver SDK is obtained from j-mex rather than fetched; README.md says where to put it. +set(_JMEX_LOCAL_SDK_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/MOXIReceiverSDK") +set(_JMEX_MIN_SDK_VERSION 0.8.0) + +if(NOT DEFINED JMEX_SDK_ROOT AND DEFINED ENV{JMEX_SDK_ROOT}) + set(JMEX_SDK_ROOT "$ENV{JMEX_SDK_ROOT}" CACHE PATH "Path to the MOXI Receiver SDK platform directory") +endif() + +# An override is searched on its own first: a stale or mistyped one must be reported, not degrade +# into a silent build against whatever is unpacked in the plugin directory. The cached result of an +# earlier configure is dropped, or a since-edited JMEX_SDK_ROOT would never be looked at again. +if(JMEX_SDK_ROOT) + unset(MOXIReceiverSDK_DIR CACHE) + find_package(MOXIReceiverSDK ${_JMEX_MIN_SDK_VERSION} QUIET NO_DEFAULT_PATH PATHS "${JMEX_SDK_ROOT}") + if(NOT TARGET Moxi::Receiver) + message(WARNING "JMEX_SDK_ROOT='${JMEX_SDK_ROOT}' holds no MOXIReceiverSDK ${_JMEX_MIN_SDK_VERSION} or newer; " + "ignoring it and searching the usual paths") + endif() +endif() + +if(NOT TARGET Moxi::Receiver) + find_package(MOXIReceiverSDK ${_JMEX_MIN_SDK_VERSION} QUIET PATHS + "${_JMEX_LOCAL_SDK_ROOT}/sdk/linux-x64" + "${_JMEX_LOCAL_SDK_ROOT}") +endif() + +# core/ builds with or without the SDK; app/ is the part that needs it. +add_subdirectory(core) + +if(NOT TARGET Moxi::Receiver) + message(STATUS "Skipping jmex plugin build: MOXIReceiverSDK ${_JMEX_MIN_SDK_VERSION} or newer not found " + "(unpack the SDK into ${_JMEX_LOCAL_SDK_ROOT}, or set CMAKE_PREFIX_PATH or " + "JMEX_SDK_ROOT to /sdk/linux-x64)") + return() +endif() + +# Report which SDK answered: a stale CMAKE_PREFIX_PATH looks the same as a deliberate one. +message(STATUS "jmex plugin: MOXIReceiverSDK ${MOXIReceiverSDK_VERSION} from ${MOXIReceiverSDK_DIR}") + +add_subdirectory(app) + +install(FILES plugin.yaml README.md DESTINATION plugins/jmex) diff --git a/src/plugins/jmex/README.md b/src/plugins/jmex/README.md new file mode 100644 index 0000000000..0929736cd8 --- /dev/null +++ b/src/plugins/jmex/README.md @@ -0,0 +1,238 @@ + + +# j-mex AgileMaster plugin + +Streams a [j-mex AgileMaster](https://jmex.com.tw/agilemaster/) motion capture channel into Isaac +Capture as the operator's body pose: `FullBodyPose` under the `body.jmex` vendor, so anything that +consumes `FullBodyPose` can use it. Linux x86_64 only. + +AgileMaster has three parts: the **j-mex Mocap Suit** the operator wears, **MOXI Player**, which +solves the suit onto a skeleton and streams it on a numbered channel, and the **MOXI Receiver SDK** +this plugin links against. + +```text +Mocap Suit -> MOXI Player -- channel --> jmex_plugin -- "jmex_full_body" --> FullBodySource + (standard human skeleton) (FullBodyPose, body.jmex) +``` + +## Prerequisites + +1. **MOXI Receiver SDK** 0.8.0 or newer. It is Apache-2.0 but not a public download: request it, + with MOXI Player and its license key, through the + [AgileMaster product page](https://jmex.com.tw/agilemaster/). +2. **MOXI Player for Robot**, with the **standard human skeleton** loaded, on this machine or + another on the same subnet. Only MOXI Player for Robot works with this plugin. +3. **A CloudXR runtime.** It is required even though no headset is. + +## Building + +Unpack the SDK into this directory as `MOXIReceiverSDK/`, so that +`src/plugins/jmex/MOXIReceiverSDK/sdk/linux-x64/lib/cmake/MOXIReceiverSDK/` exists. No build flags +are then needed: + +```bash +cmake -B build +cmake --build build --parallel +cmake --install build +``` + +To keep the SDK elsewhere, point `JMEX_SDK_ROOT` (a CMake or environment variable) or +`CMAKE_PREFIX_PATH` at the SDK's platform directory, the one containing `lib/cmake/MOXIReceiverSDK`: + +```bash +cmake -B build -DJMEX_SDK_ROOT=/path/to/MOXIReceiverSDK-0.8.0/sdk/linux-x64 +``` + +The configure output names the SDK it found: `jmex plugin: MOXIReceiverSDK from `. +Without one, the plugin is skipped with `Skipping jmex plugin build:` and the rest of the tree still +builds. The project's own build prerequisites still apply; see +[Build from Source](../../../docs/source/getting_started/build_from_source/index.rst). + +## Running + +1. **Start the CloudXR runtime** in a terminal of its own and leave it running. Start it from a + Python that has this repository's `isaaccapture` — `uv pip install -e ./examples/jmex` puts it + there: + + ```bash + python -m isaaccapture.cloudxr.service run + ``` + + The runtime needs a concrete device profile, and the default, `Quest3`, is one. To use another, + hand the service an env file: + + ```bash + echo 'NV_DEVICE_PROFILE=' > cloudxr.env + python -m isaaccapture.cloudxr.service run --cloudxr-env-config cloudxr.env + ``` + +2. **Load the runtime's environment** in every terminal you start the plugin from: + + ```bash + source ~/.cloudxr/run/cloudxr.env + ``` + +3. **Start the plugin:** + + ```bash + # Defaults: channel 1, collection id "jmex_full_body" + ./install/plugins/jmex/jmex_plugin + + # Player's file replay is fixed on channel 255 + ./install/plugins/jmex/jmex_plugin --channel=255 + ``` + + It logs `Waiting for MOXI Player to pair on channel ...` and waits. If Player disconnects, the + plugin waits for it again, and Player pairs again on its own. + +4. **Stream from MOXI Player** on the same channel, with the standard human skeleton loaded. + Player's own README covers its setup and calibration. Once paired, the log reads + `channel full body filled 24 of 24 slots`. + +5. **Check the stream** with no simulator in the way: with the plugin running, record a few seconds + as under [Recording a body pose](../../../examples/jmex/README.md#recording-a-body-pose), adding + `--no-plugin` so that the script does not start a second plugin. `joints=24/24` on the progress + lines means frames from the standard human skeleton have arrived. + +### Choosing where the receiver listens + +| Flag | Default | When to set it | +| --- | --- | --- | +| `--channel=` | `1` | Player streams on another channel. Its file replay is fixed on 255 | +| `--collection-id=` | `jmex_full_body` | More than one of these plugins runs on one machine. Consumers must name the same id | +| `--bind-ip=` | every interface | Rarely. It narrows only the TCP server, so a Player answering from an interface it leaves out cannot connect | +| `--broadcast-ip=` | `255.255.255.255` | Another MOXI Player on the subnet would otherwise contend for the same channel | +| `--tcp-port=` | `7000` | A second plugin on this machine; with the default, the second one to start finds the port taken | +| `--udp-start-port=` | `10100` | A firewall rule needs predictable data ports. Channel N receives on this + 1 + N | + +One plugin receives one channel. A second plugin on this machine needs only its own `--tcp-port=`. +Every flag takes its value after `=`. An argument the plugin does not know, or a value it cannot +use, stops it with exit code 1 and names the argument. + +```bash +./install/plugins/jmex/jmex_plugin --channel=255 --bind-ip=192.0.2.10 --broadcast-ip=192.0.2.10 +``` + +### Choosing the device clock + +Each sample's common clock is the moment the plugin picked the frame up. `--device-clock=` chooses +what goes in its raw device clock: + +| `--device-clock=` | Raw device clock | +| --- | --- | +| `device` (default) | The time MOXI Player stamped on the frame, in nanoseconds since the Unix epoch on Player's machine | +| `local` | The same pickup time as the common clock | + +`JMEX_DEVICE_CLOCK` sets the same from the environment, for a launcher that cannot add plugin +arguments; the flag wins when both are given. Any other value stops the plugin at startup. + +### Letting Isaac Teleop launch it + +`TeleopSession` can start the plugin when the session starts and stop it when the session ends. Add +a `PluginConfig` that names the plugin and points at an installed tree; a plugin that has only been +built is not found: + +```python +from pathlib import Path +from isaaccapture.teleop_session_manager import PluginConfig, TeleopSessionConfig + +session_config = TeleopSessionConfig( + app_name="AgileMasterTeleop", + pipeline=pipeline, + plugins=[ + PluginConfig( + plugin_name="jmex", # plugin.yaml's name + plugin_root_id="jmex", + search_paths=[Path("install/plugins")], + plugin_args=["--channel=255"], # optional; the last value of a flag wins + required=True, # refuse to start, rather than wait in silence, if it is not found + ) + ], +) +``` + +While bringing a device up, starting the plugin by hand is easier: it can be restarted without +restarting the simulator, and its log stays separate. + +### Logs and stopping + +Started by hand, the plugin logs to stderr and to a file under `/tmp/isaaccapture-/logs/` +(`ISAACCAPTURE_LOG_DIR` moves it). Started by `TeleopSession`, its log lines go to that session's +console and log file, and anything else the process prints goes to the session's capture file in the +same directory, `.isaaccapture..native.log`. + +`Ctrl-C` or `SIGTERM` stops it. Once it is waiting for Player it stops cleanly, and the last line +is the run's counters, starting `stopping -- published`. Before it reaches that state it exits at +once, without the counters. If the CloudXR runtime restarts underneath it, the plugin reconnects +within about 23 seconds and carries on; frames sent during the outage are lost. A runtime that +does not come back in that time ends the run with exit code 1. + +## Consuming the data + +Wire a `FullBodySource` into your retargeting graph with the `body.jmex` vendor: + +```python +from isaaccapture.deviceio_session import TrackerVendor +from isaaccapture.retargeting_engine.deviceio_source_nodes import FullBodySource + +source = FullBodySource( + name="agilemaster", # this node's name in your graph + vendor=TrackerVendor("body.jmex"), +) +``` + +The vendor reads `jmex_full_body` unless told otherwise. A plugin started with another +`--collection-id=` needs the consumer to name the same id. A mismatch raises no error anywhere; the +source simply never delivers a sample: + +```python +TrackerVendor("body.jmex", {"collection_id": "mysuit_full_body"}) +``` + +| Item | Value | +| --- | --- | +| Collection | `jmex_full_body`, or whatever `--collection-id=` names | +| Schema | `FullBodyPose` | +| Vendor | `body.jmex` | +| Consumer | `FullBodySource`, or a `FullBodyTracker` with that vendor | +| Contents | The 24 `BodyJoint` slots: position in meters and orientation, in OpenXR axes | + +- With the standard human skeleton loaded, all 24 slots arrive on every frame. Any other model fills + only the slots whose joint names match; on a humanoid URDF, typically the knees and elbows. +- A slot with no joint, or with no usable value in a frame, goes out with `is_valid` false. + `all_joint_poses_tracked` is true only when all 24 are valid. +- When Player disconnects, the plugin stops publishing but keeps the collection, so a consumer goes + on reading the last frame until Player pairs again. The plugin logs + `MOXI Player disconnected; waiting for it to come back` when that happens. +- The spine, foot, neck, collar and hand slots have no sensor of their own: read them for position, + not orientation. +- The skeleton has fixed dimensions, so positions describe the operator's posture, not their size. + +For a complete consumer, which drives a Unitree G1 in Isaac Lab from the body pose and records it to +MCAP, see the [example](../../../examples/jmex/README.md). + +## Troubleshooting + +| Symptom | What to check | +| --- | --- | +| Stuck at `Waiting for MOXI Player to pair` | Player is streaming, and **on the same channel**: a mismatch looks exactly like Player not running. With two hosts, check they share a subnet and that the wireless access point does not isolate clients | +| Stuck at `OpenXR HMD form factor is unavailable; waiting for a system...` | The runtime hands out no system. Use a concrete device profile such as the default `Quest3`: an `auto-*` profile waits for a client to connect, and nothing times that wait out | +| `Environment variable NV_CXR_RUNTIME_DIR is not set` | `~/.cloudxr/run/cloudxr.env` has not been sourced in this terminal | +| `Failed to create OpenXR instance: -51` | The environment is loaded, but the runtime it names is not running | +| `OpenXR session unrecoverable after` … | The runtime went away and did not come back within about 23 seconds. Start it again, then the plugin | +| `Skipping jmex plugin build:` at configure time | No SDK 0.8.0 or newer in `MOXIReceiverSDK/`, and neither `JMEX_SDK_ROOT` nor `CMAKE_PREFIX_PATH` points at one. An older SDK is skipped the same way | +| `no joint on this channel is named for a body slot` | Player has a model loaded whose joint names match no slot. Load the standard human skeleton | +| Fewer than 24 of 24 slots filled at pairing | Player has another model loaded. The lines after the count name each empty slot | +| A consumer never receives a sample while the plugin is paired | The consumer's `collection_id` is not the one `--collection-id=` names | + +Everything before the channel — the suit, the gloves, the dongle, Player's setup and calibration — +is covered by Player's own README and glove guide. + +## License + +The source files here are Apache-2.0, as is the MOXI Receiver SDK they link against. This project +does not redistribute the SDK; it is obtained from j-mex. MOXI Player is a separate proprietary +application under its own terms. diff --git a/src/plugins/jmex/app/CMakeLists.txt b/src/plugins/jmex/app/CMakeLists.txt new file mode 100644 index 0000000000..76785c0c2b --- /dev/null +++ b/src/plugins/jmex/app/CMakeLists.txt @@ -0,0 +1,17 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(jmex_plugin + main.cpp + receiver_session.cpp + receiver_session.hpp +) + +target_link_libraries(jmex_plugin + PRIVATE + isaac::jmex_plugin_core + Moxi::Receiver + log_bridge::log_bridge_core +) + +install(TARGETS jmex_plugin RUNTIME DESTINATION plugins/jmex) diff --git a/src/plugins/jmex/app/main.cpp b/src/plugins/jmex/app/main.cpp new file mode 100644 index 0000000000..e00f1277c7 --- /dev/null +++ b/src/plugins/jmex/app/main.cpp @@ -0,0 +1,131 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +/*! + * @file main.cpp + * @brief j-mex AgileMaster device plugin: streams one MOXI channel over the OpenXR tensor + * transport as ``FullBodyPose``. Flags and setup: README.md. + */ + +#include "receiver_session.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::jmex; + +namespace +{ + +constexpr const char* LOGGER_NAME = "isaaccapture.plugins.jmex.main"; + +// MOXI Player sends at 60 Hz; polling at four times that picks each frame up within about 4 ms. +constexpr int kPollHz = 240; + +//! Set by SIGINT/SIGTERM so the poll loop returns and ~ReceiverSession() shuts the SDK down in +//! order, instead of the process being killed mid-stride. +std::atomic g_stop{ false }; +static_assert(std::atomic::is_always_lock_free, "the signal handler stores to this"); + +extern "C" void on_stop_signal(int /*signum*/) +{ + // Everything a signal handler is allowed to do here: one lock-free store, no I/O. + g_stop.store(true, std::memory_order_relaxed); +} + +} // namespace + +int main(int argc, char** argv) +try +{ + const auto logger = isaaccapture::Logger::get(LOGGER_NAME); + const PluginArgs args = parse_plugin_args(argc, argv, std::getenv("JMEX_DEVICE_CLOCK")); + + const int channel = args.receiver.channel; + const std::string collection_id = args.collection_id; + + logger->info("j-mex AgileMaster plugin (channel: {}, collection: {}, device clock: {}, SDK: {})", channel, + collection_id, device_clock_name(args.device_clock), moxi_sdk_version()); + logger->info("receiver on tcp {}:{}, advertising to {}, channel data from port {}", + args.receiver.bind_ip.empty() ? "every interface" : args.receiver.bind_ip, args.receiver.tcp_port, + args.receiver.broadcast_ip.empty() ? "255.255.255.255" : args.receiver.broadcast_ip, + args.receiver.udp_start_port + 1 + channel); + + // OpenXR first: a missing runtime is the common setup mistake, and failing on it before the + // MOXI receiver starts means we never take the process-global TCP port just to give it back. + FullBodyPublisher full_body_publisher(collection_id, args.device_clock); + + // Only now: the publisher's startup wait for an OpenXR system reads no stop flag, so until it + // returns, SIGINT and SIGTERM keep their default and end the process. Nothing needs undoing + // yet. + std::signal(SIGINT, on_stop_signal); + std::signal(SIGTERM, on_stop_signal); + + ReceiverSession receiver(args.receiver); + + logger->info("Waiting for MOXI Player to pair on channel {}...", channel); + + const auto poll_interval = std::chrono::nanoseconds(1000000000 / kPollHz); + auto next_poll = std::chrono::steady_clock::now(); + int64_t published = 0; + + int exit_code = 0; + try + { + while (!g_stop.load(std::memory_order_relaxed)) + { + auto frame = receiver.poll(); + if (frame && full_body_publisher.publish(*frame, g_stop)) + { + if (published == 0) + { + logger->info("full body on collection '{}'", collection_id); + } + ++published; + } + + // After a stall, re-base rather than catch up: the missed polls would all be empty. + const auto now = std::chrono::steady_clock::now(); + next_poll += poll_interval; + if (next_poll < now) + { + next_poll = now + poll_interval; + } + std::this_thread::sleep_until(next_poll); + } + } + catch (const std::exception& e) + { + logger->error("{}: {}", argv[0], e.what()); + exit_code = 1; + } + + // Log the counters on every exit: after a fatal outage they are the only record of it. + logger->info( + "stopping -- published {} frames, dropped {}, push failures {}, sessions re-established {}, recoveries " + "abandoned {}", + published, receiver.dropped_frames(), full_body_publisher.push_failures(), + full_body_publisher.session_recoveries(), full_body_publisher.recoveries_abandoned()); + return exit_code; +} +catch (const std::exception& e) +{ + isaaccapture::Logger::get(LOGGER_NAME)->error("{}: {}", argv[0], e.what()); + return 1; +} +catch (...) +{ + isaaccapture::Logger::get(LOGGER_NAME)->error("{}: Unknown error", argv[0]); + return 1; +} diff --git a/src/plugins/jmex/app/receiver_session.cpp b/src/plugins/jmex/app/receiver_session.cpp new file mode 100644 index 0000000000..43dfa069e7 --- /dev/null +++ b/src/plugins/jmex/app/receiver_session.cpp @@ -0,0 +1,235 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "receiver_session.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +// The SDK's entry points are free functions over one internal receiver, so only one session may +// exist per process. +std::atomic g_session_alive{ false }; + +spdlog::logger& logger() +{ + static const std::shared_ptr instance = + isaaccapture::Logger::get("isaaccapture.plugins.jmex.ReceiverSession"); + return *instance; +} + +//! Report which of the 24 slots this pairing's model fills. +void log_slot_coverage(int channel, const BodyJointMapping& mapping) +{ + // A shortfall is a warning, the count as much as the per-slot lines after it: a console that + // shows only warnings should still show the count those lines explain. + const bool every_slot_filled = mapping.filled_slot_count() == static_cast(core::BodyJoint_NUM_JOINTS); + logger().log(every_slot_filled ? spdlog::level::info : spdlog::level::warn, + "channel {} full body filled {} of {} slots", channel, mapping.filled_slot_count(), + static_cast(core::BodyJoint_NUM_JOINTS)); + + // Log the empty slots here: nothing downstream can name them, since samples carry poses, not + // joint names. + if (mapping.filled_slot_count() == 0) + { + logger().warn( + "no joint on this channel is named for a body slot, so the full body channel will publish 24 invalid " + "slots -- is the standard human skeleton loaded?"); + } + else + { + for (size_t slot = 0; slot < k_standard_human_skeleton_joint_names.size(); ++slot) + { + const auto body_joint = static_cast(slot); + if (mapping.joint_index(body_joint) < 0) + { + logger().warn("no joint named '{}' for slot {}", k_standard_human_skeleton_joint_names[slot], + core::EnumNameBodyJoint(body_joint)); + } + } + } + + for (const std::string& name : mapping.contested_joint_names()) + { + logger().warn("joint '{}' matches a slot an earlier joint already fills; the earlier one keeps it", name); + } +} + +} // namespace + +std::string moxi_sdk_version() +{ + const char* version = MxGetVersion(); + return (version != nullptr) ? version : "unknown"; +} + +ReceiverSession::ReceiverSession(const ReceiverConfig& config) : config_(config) +{ + bool expected = false; + if (!g_session_alive.compare_exchange_strong(expected, true)) + { + throw std::runtime_error( + "ReceiverSession: a session already exists in this process; the MOXI " + "receiver is process-global and cannot be opened twice"); + } + + // The SDK's "no preference" is NULL, not an empty string. + const char* bind_ip = config_.bind_ip.empty() ? nullptr : config_.bind_ip.c_str(); + const char* broadcast_ip = config_.broadcast_ip.empty() ? nullptr : config_.broadcast_ip.c_str(); + + // The robot dialect, although nothing here reads robot joints: the change of basis in + // openxr_convention.hpp was measured on it. Re-measure before switching dialects. + if (!MxRStartSystem(bind_ip, config_.tcp_port, broadcast_ip, config_.udp_start_port, MOXIMocapType::ROBOT)) + { + g_session_alive.store(false); + throw std::runtime_error("ReceiverSession: MxRStartSystem failed binding " + + (config_.bind_ip.empty() ? std::string("every interface") : config_.bind_ip) + + " on TCP port " + std::to_string(config_.tcp_port) + + " -- the port is already taken, or that is not an address this machine has"); + } + + if (!MxROpenChannel(config_.channel)) + { + MxRFinishSystem(); + g_session_alive.store(false); + throw std::runtime_error("ReceiverSession: MxROpenChannel(" + std::to_string(config_.channel) + + ") failed; nothing would ever arrive on this channel"); + } +} + +ReceiverSession::~ReceiverSession() +{ + MxRCloseChannel(config_.channel); + MxRFinishSystem(); + g_session_alive.store(false); +} + +bool ReceiverSession::is_streaming() const +{ + return MxCheckChannelConnected(config_.channel); +} + +std::optional ReceiverSession::poll() +{ + MxRUpdateSystem(); + + if (!is_streaming()) + { + // Logged on the channel's own transition, not on the SDK's disconnect event: that one also + // fires when a connection that never held this channel ends, such as one from a second + // Player the SDK refused. + if (was_streaming_) + { + logger().info("MOXI Player disconnected; waiting for it to come back"); + } + was_streaming_ = false; + + // A new pairing may bring a different model, so the joint table is re-read next time + // rather than carried across the gap. + joints_discovered_ = false; + joint_names_.clear(); + sequence_id_ = 0; + return std::nullopt; + } + was_streaming_ = true; + + const uint32_t sequence_id = MxRGetCurrentDataSequenceID(config_.channel); + if (sequence_id == sequence_id_) + { + return std::nullopt; // no new frame; the values on the wire are the ones already handed out + } + + // Ids step by one per frame, so a larger step counts frames that never reached us; the first + // frame of a pairing has nothing to compare against. No branch for an id going backwards: only + // a Player restart resets the count, and that drops the pairing first (!is_streaming() above). + if (sequence_id_ != 0 && sequence_id > sequence_id_ + 1) + { + dropped_frames_ += sequence_id - sequence_id_ - 1; + } + sequence_id_ = sequence_id; + const uint64_t player_time_ns = MxRGetCurrentDataTimestamp(config_.channel); + + if (!joints_discovered_) + { + discover_joints(); + } + if (!joints_discovered_) + { + return std::nullopt; + } + + unconstrained_global_poses(joint_poses_); + + FullBodyFrame frame; + frame.all_joint_poses_tracked = + mapping_.apply(joint_poses_, frame.joints) == static_cast(core::BodyJoint_NUM_JOINTS); + frame.sequence_id = sequence_id; + frame.player_time_ns = player_time_ns; + return frame; +} + +void ReceiverSession::discover_joints() +{ + const int joint_count = MxRGetBoneJointCount(config_.channel); + if (joint_count <= 0) + { + return; // the initial pose has not landed yet; try again on the next frame + } + + joint_names_.clear(); + joint_names_.reserve(static_cast(joint_count)); + for (int bone = 0; bone < joint_count; ++bone) + { + // Every joint gets an entry, named or not: the table is indexed the way the per-joint SDK + // calls are, and a name no slot carries simply fills nothing. + const char* raw_name = MxRGetBoneJointName(config_.channel, bone); + joint_names_.emplace_back((raw_name != nullptr) ? raw_name : ""); + } + + mapping_ = BodyJointMapping::resolve(joint_names_); + joints_discovered_ = true; + logger().info("channel {} paired, {} joints", config_.channel, joint_count); + log_slot_coverage(config_.channel, mapping_); +} + +void ReceiverSession::unconstrained_global_poses(std::vector& out) const +{ + out.resize(joint_names_.size()); + + // Seed with NaN, not identity: the SDK's read returns void, so a joint the frame did not carry + // keeps the seed, and an identity pose at the origin would go out as a valid slot. NaN survives + // the change of basis and is_usable() in body_joint_mapping.cpp rejects it. + const float nan = std::numeric_limits::quiet_NaN(); + for (size_t bone = 0; bone < joint_names_.size(); ++bone) + { + float quat_wxyz[4] = { nan, nan, nan, nan }; + float position[3] = { nan, nan, nan }; + MxRGetBoneJointMotionData(config_.channel, static_cast(bone), quat_wxyz, position, MOXI_DATA_TYPE_GLOBAL); + + // The only conversion from the SDK's conventions to OpenXR's: FullBodyPose consumers expect + // OpenXR axes from every producer. + out[bone] = moxi_pose_to_openxr(position, quat_wxyz); + } +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/app/receiver_session.hpp b/src/plugins/jmex/app/receiver_session.hpp new file mode 100644 index 0000000000..ffa163a0a8 --- /dev/null +++ b/src/plugins/jmex/app/receiver_session.hpp @@ -0,0 +1,90 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +//! Version of the MOXI Receiver SDK this plugin is linked against. +std::string moxi_sdk_version(); + +/*! + * @brief One robot-dialect MOXI channel, from pairing to each frame's 24 slots. + * + * MOXI Player is the TCP *client*: the SDK listens and the Player connects in, so a freshly + * constructed session is normally not streaming yet, and neither is one whose Player disconnected. + * Callers poll and wait rather than treating silence as a failure. + */ +class ReceiverSession +{ +public: + /*! + * @throws std::runtime_error if another ReceiverSession already exists in this process, or if + * the SDK refuses to start -- the port is already taken, or `bind_ip` is not an address + * this machine has. + */ + explicit ReceiverSession(const ReceiverConfig& config); + ~ReceiverSession(); + + ReceiverSession(const ReceiverSession&) = delete; + ReceiverSession& operator=(const ReceiverSession&) = delete; + + /*! + * @brief Pump the receiver once and hand out the frame it delivered, if that frame is new. + * + * A frame comes out only when the channel's sequence id moved *and* the joint table has been + * read, so a caller publishes at most one sample per delivered frame instead of re-publishing + * stale values at the poll rate. The joint table is read on the first frame of a pairing that + * carries one. + */ + std::optional poll(); + + //! Frames the Player sent that never reached us, counted from sequence id gaps. + int64_t dropped_frames() const + { + return dropped_frames_; + } + +private: + //! Is the channel currently delivering motion data? False for a channel that is merely open, or + //! paired but not yet sending. + bool is_streaming() const; + + //! Read the channel's joint names and match them against the slots. Idempotent per pairing. + void discover_joints(); + + /*! + * @brief Every joint's unconstrained global pose, in joint table order. + * + * @param out Resized to the joint table's size and overwritten. + */ + void unconstrained_global_poses(std::vector& out) const; + + ReceiverConfig config_; + uint32_t sequence_id_ = 0; + int64_t dropped_frames_ = 0; + //! Whether the channel was streaming at the previous poll, so each disconnect is logged once. + bool was_streaming_ = false; + bool joints_discovered_ = false; + //! Every joint on the channel, in the index order the per-joint SDK calls take. Empty before + //! the first frame of a pairing. + std::vector joint_names_; + BodyJointMapping mapping_; + //! This frame's joint poses, kept as a member so it is not reallocated per frame. + std::vector joint_poses_; +}; + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/CMakeLists.txt b/src/plugins/jmex/core/CMakeLists.txt new file mode 100644 index 0000000000..b887b7837a --- /dev/null +++ b/src/plugins/jmex/core/CMakeLists.txt @@ -0,0 +1,43 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(jmex_plugin_core STATIC + openxr_convention.cpp + body_joint_mapping.cpp + full_body_publisher.cpp + device_clock.cpp + plugin_args.cpp + inc/jmex/openxr_convention.hpp + inc/jmex/plugin_args.hpp + inc/jmex/receiver_config.hpp + inc/jmex/full_body_frame.hpp + inc/jmex/body_joint_mapping.hpp + inc/jmex/full_body_publisher.hpp + inc/jmex/device_clock.hpp +) + +target_include_directories(jmex_plugin_core + INTERFACE + $ +) + +# Public headers include the generated full-body schema, so isaacteleop_schema propagates. Nothing +# else does: no public header includes OpenXR or the pusher -- the transport the publisher owns is +# behind this library's own headers. +target_link_libraries(jmex_plugin_core + PUBLIC + isaacteleop_schema + PRIVATE + pusherio::pusherio + oxr::oxr_core + oxr::oxr_utils + log_bridge::log_bridge_core +) + +set_target_properties(jmex_plugin_core PROPERTIES + POSITION_INDEPENDENT_CODE ON + CXX_VISIBILITY_PRESET hidden + VISIBILITY_INLINES_HIDDEN YES +) + +add_library(isaac::jmex_plugin_core ALIAS jmex_plugin_core) diff --git a/src/plugins/jmex/core/body_joint_mapping.cpp b/src/plugins/jmex/core/body_joint_mapping.cpp new file mode 100644 index 0000000000..73de6cf976 --- /dev/null +++ b/src/plugins/jmex/core/body_joint_mapping.cpp @@ -0,0 +1,167 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/jmex/body_joint_mapping.hpp" + +#include +#include + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +// The skeleton's root and finger joints fill no slot, so they have no row here. +constexpr SlotNameTable make_standard_human_skeleton_joint_names() +{ + SlotNameTable names{}; + names[core::BodyJoint_PELVIS] = "pelvis_joint"; + names[core::BodyJoint_LEFT_HIP] = "left_hip_joint"; + names[core::BodyJoint_RIGHT_HIP] = "right_hip_joint"; + names[core::BodyJoint_SPINE1] = "spine1_joint"; + names[core::BodyJoint_LEFT_KNEE] = "left_knee_joint"; + names[core::BodyJoint_RIGHT_KNEE] = "right_knee_joint"; + names[core::BodyJoint_SPINE2] = "spine2_joint"; + names[core::BodyJoint_LEFT_ANKLE] = "left_ankle_joint"; + names[core::BodyJoint_RIGHT_ANKLE] = "right_ankle_joint"; + names[core::BodyJoint_SPINE3] = "spine3_joint"; + names[core::BodyJoint_LEFT_FOOT] = "left_foot_joint"; + names[core::BodyJoint_RIGHT_FOOT] = "right_foot_joint"; + names[core::BodyJoint_NECK] = "neck_joint"; + names[core::BodyJoint_LEFT_COLLAR] = "left_collar_joint"; + names[core::BodyJoint_RIGHT_COLLAR] = "right_collar_joint"; + names[core::BodyJoint_HEAD] = "head_joint"; + names[core::BodyJoint_LEFT_SHOULDER] = "left_shoulder_joint"; + names[core::BodyJoint_RIGHT_SHOULDER] = "right_shoulder_joint"; + names[core::BodyJoint_LEFT_ELBOW] = "left_elbow_joint"; + names[core::BodyJoint_RIGHT_ELBOW] = "right_elbow_joint"; + names[core::BodyJoint_LEFT_WRIST] = "left_wrist_joint"; + names[core::BodyJoint_RIGHT_WRIST] = "right_wrist_joint"; + names[core::BodyJoint_LEFT_HAND] = "left_hand_joint"; + names[core::BodyJoint_RIGHT_HAND] = "right_hand_joint"; + return names; +} + +constexpr JointPose k_identity_pose{ { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f, 1.0f } }; + +// How far a quaternion may drift from unit length and still be treated as a rotation. Wide enough +// to absorb float32 round-off down the hierarchy, narrow enough that an uninitialized or partly +// written frame does not pass. +constexpr float k_quaternion_norm_tolerance = 1e-3f; + +//! Whether a pose can go out valid: a finite position and a unit quaternion. +bool is_usable(const JointPose& pose) +{ + for (float component : pose.position) + { + if (!std::isfinite(component)) + { + return false; + } + } + + float norm_squared = 0.0f; + for (float component : pose.orientation_xyzw) + { + if (!std::isfinite(component)) + { + return false; + } + norm_squared += component * component; + } + + return std::fabs(std::sqrt(norm_squared) - 1.0f) <= k_quaternion_norm_tolerance; +} + +core::BodyJointPose to_schema(const JointPose& pose, bool is_valid) +{ + return core::BodyJointPose(core::Pose(core::Point(pose.position[0], pose.position[1], pose.position[2]), + core::Quaternion(pose.orientation_xyzw[0], pose.orientation_xyzw[1], + pose.orientation_xyzw[2], pose.orientation_xyzw[3])), + is_valid); +} + +//! Slot a joint name fills, or -1 for a name the table does not carry. +int body_slot_for_joint_name(std::string_view joint_name) +{ + if (joint_name.empty()) + { + return -1; + } + for (size_t slot = 0; slot < k_standard_human_skeleton_joint_names.size(); ++slot) + { + if (k_standard_human_skeleton_joint_names[slot] == joint_name) + { + return static_cast(slot); + } + } + return -1; +} + +} // namespace + +const SlotNameTable k_standard_human_skeleton_joint_names = make_standard_human_skeleton_joint_names(); + +BodyJointMapping BodyJointMapping::resolve(const std::vector& joint_names) +{ + BodyJointMapping mapping; + mapping.slot_to_joint_.fill(-1); + + // Walked joint-first rather than slot-first so that a second joint claiming a taken slot is + // seen at all: a slot-first lookup would find the first match and never know about the rest. + for (size_t joint = 0; joint < joint_names.size(); ++joint) + { + const int slot = body_slot_for_joint_name(joint_names[joint]); + if (slot < 0) + { + continue; + } + if (mapping.slot_to_joint_[static_cast(slot)] >= 0) + { + mapping.contested_joint_names_.push_back(joint_names[joint]); + continue; + } + mapping.slot_to_joint_[static_cast(slot)] = static_cast(joint); + ++mapping.filled_slot_count_; + } + + return mapping; +} + +int BodyJointMapping::joint_index(core::BodyJoint slot) const +{ + if (static_cast(slot) >= slot_to_joint_.size()) + { + return -1; + } + return slot_to_joint_[static_cast(slot)]; +} + +size_t BodyJointMapping::apply(const std::vector& joint_poses, core::BodyJoints& out) const +{ + size_t valid_slots = 0; + for (size_t slot = 0; slot < slot_to_joint_.size(); ++slot) + { + core::BodyJointPose sample = to_schema(k_identity_pose, false); + + const int joint = slot_to_joint_[slot]; + if (joint >= 0 && static_cast(joint) < joint_poses.size()) + { + const JointPose& pose = joint_poses[static_cast(joint)]; + if (is_usable(pose)) + { + sample = to_schema(pose, true); + ++valid_slots; + } + } + + out.mutable_joints()->Mutate(static_cast(slot), sample); + } + return valid_slots; +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/device_clock.cpp b/src/plugins/jmex/core/device_clock.cpp new file mode 100644 index 0000000000..d05d12ce37 --- /dev/null +++ b/src/plugins/jmex/core/device_clock.cpp @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/jmex/device_clock.hpp" + +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +DeviceClock parse_device_clock(const std::string& source, const std::string& value) +{ + if (value == "device") + { + return DeviceClock::DEVICE; + } + if (value == "local") + { + return DeviceClock::LOCAL; + } + throw std::runtime_error(source + " takes 'device' or 'local', got '" + value + "'"); +} + +} // namespace + +DeviceClock choose_device_clock(const char* flag_value, const char* env_value) +{ + DeviceClock clock = DeviceClock::DEVICE; + if (env_value != nullptr && *env_value != '\0') + { + clock = parse_device_clock("JMEX_DEVICE_CLOCK", env_value); + } + if (flag_value != nullptr) + { + clock = parse_device_clock("--device-clock=", flag_value); + } + return clock; +} + +const char* device_clock_name(DeviceClock clock) +{ + return clock == DeviceClock::DEVICE ? "device" : "local"; +} + +int64_t raw_device_time_ns(DeviceClock clock, int64_t sample_time_ns, uint64_t player_time_ns) +{ + if (clock == DeviceClock::DEVICE && player_time_ns <= static_cast(std::numeric_limits::max())) + { + return static_cast(player_time_ns); + } + return sample_time_ns; +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/full_body_publisher.cpp b/src/plugins/jmex/core/full_body_publisher.cpp new file mode 100644 index 0000000000..d74e468c23 --- /dev/null +++ b/src/plugins/jmex/core/full_body_publisher.cpp @@ -0,0 +1,224 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/jmex/full_body_publisher.hpp" + +#include "inc/jmex/device_clock.hpp" +#include "inc/jmex/full_body_frame.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +//! Names the OpenXR instance and the collection's pushing application. One constant because the +//! runtime reports them separately and two spellings of the same plugin read as two plugins. +constexpr const char* APP_NAME = "JmexPlugin"; + +//! How long to wait before each attempt to re-establish a lost session, in milliseconds: about +//! 23 s in total, short first, then 4 s. +constexpr int SESSION_BACKOFF_MS[] = { 500, 1000, 2000, 4000, 4000, 4000, 4000, 4000 }; + +//! How many times a lost session is re-established before the plugin gives up: one per wait. +constexpr int SESSION_RECOVERY_ATTEMPTS = static_cast(std::size(SESSION_BACKOFF_MS)); + +//! A backoff is slept in slices this long, so a stop request costs at most one of them. +constexpr int BACKOFF_SLICE_MS = 50; + +spdlog::logger& logger() +{ + static const std::shared_ptr instance = + isaaccapture::Logger::get("isaaccapture.plugins.jmex.FullBodyPublisher"); + return *instance; +} + +//! The whole schedule added up, for the message that reports giving up. +int backoff_budget_ms() +{ + int total = 0; + for (const int wait_ms : SESSION_BACKOFF_MS) + { + total += wait_ms; + } + return total; +} + +//! The BackoffWait a publisher gets when none is passed in: @a total_ms, slept a slice at a time. +bool wait_unless_stopped(int total_ms, const std::atomic& stop) +{ + for (int remaining = total_ms; remaining > 0; remaining -= BACKOFF_SLICE_MS) + { + if (stop.load(std::memory_order_relaxed)) + { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(remaining < BACKOFF_SLICE_MS ? remaining : BACKOFF_SLICE_MS)); + } + return !stop.load(std::memory_order_relaxed); +} + +//! An OpenXR session and the pusher on it. The pusher is declared last so it is destroyed first: +//! its collection handle lives inside the session, and once the runtime has gone, destroying the +//! two the other way round would destroy the handle against a session that no longer answers. +struct OpenXRTransport +{ + OpenXRTransport(const std::string& collection_id, bool wait_for_system) + : session(APP_NAME, core::SchemaPusher::get_required_extensions(), wait_for_system), + pusher(session.get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = FullBodyPublisher::MAX_FLATBUFFER_SIZE, + // Must match what FullBodyTracker reads. + .tensor_identifier = "full_body", + .localized_name = "j-mex Full Body", + .app_name = APP_NAME }) + { + } + + core::OpenXRSession session; + core::SchemaPusher pusher; +}; + +FullBodyPublisher::TransportFactory openxr_transport_factory(const std::string& collection_id) +{ + return [collection_id](bool wait_for_system) -> FullBodyPublisher::PushBufferFunction + { + // Shared only because std::function needs a callable it can copy. + const auto transport = std::make_shared(collection_id, wait_for_system); + return [transport](const uint8_t* buffer, size_t size, int64_t sample_time_ns, int64_t raw_device_time_ns) + { transport->pusher.push_buffer(buffer, size, sample_time_ns, raw_device_time_ns); }; + }; +} + +} // namespace + +FullBodyPublisher::FullBodyPublisher(const std::string& collection_id, DeviceClock device_clock) + : FullBodyPublisher(openxr_transport_factory(collection_id), device_clock) +{ +} + +FullBodyPublisher::FullBodyPublisher(TransportFactory create_transport, DeviceClock device_clock, BackoffWait wait) + : create_transport_(std::move(create_transport)), + wait_(wait ? std::move(wait) : BackoffWait(wait_unless_stopped)), + device_clock_(device_clock) +{ + establish_session(/*wait_for_system=*/true); +} + +void FullBodyPublisher::establish_session(bool wait_for_system) +{ + // Let go of the old transport before asking for a new one, so two sessions never stand at + // once. Assigning the new one straight over it would bring the new one up first. + push_buffer_ = nullptr; + push_buffer_ = create_transport_(wait_for_system); +} + +bool FullBodyPublisher::recover_session(const std::atomic& stop) +{ + for (int attempt = 0; attempt < SESSION_RECOVERY_ATTEMPTS; ++attempt) + { + if (!wait_(SESSION_BACKOFF_MS[attempt], stop)) + { + return false; + } + + try + { + establish_session(/*wait_for_system=*/false); + ++session_recoveries_; + logger().info("OpenXR session re-established after {} attempt(s); publishing again", attempt + 1); + return true; + } + catch (const std::exception& e) + { + // Expected while the runtime is down, so only the first and last attempts are + // reported rather than every one. + if (attempt == 0 || attempt == SESSION_RECOVERY_ATTEMPTS - 1) + { + logger().warn( + "session re-establish attempt {}/{} failed: {}", attempt + 1, SESSION_RECOVERY_ATTEMPTS, e.what()); + } + } + } + + ++recoveries_abandoned_; + return false; +} + +bool FullBodyPublisher::publish(const FullBodyFrame& frame, const std::atomic& stop) +{ + // Empty only after a recovery that gave up with a stop already pending, when the caller's loop + // is about to leave on the same flag. + if (!push_buffer_) + { + return false; + } + + const int64_t sample_time_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(MAX_FLATBUFFER_SIZE); + builder.Finish(core::CreateFullBodyPose(builder, &frame.joints, frame.all_joint_poses_tracked)); + + if (builder.GetSize() > MAX_FLATBUFFER_SIZE) + { + logger().warn( + "serialized FullBodyPose is {} bytes, over the {}-byte budget shared with the consumer's " + "FullBodyTracker. Sample dropped.", + builder.GetSize(), MAX_FLATBUFFER_SIZE); + return false; + } + + // push_buffer throws when the push fails, and a restarted CloudXR runtime arrives as exactly + // that rather than as anything session-shaped. Not rate limited: every failure costs a backoff + // before anything is retried, so this cannot produce more than a line or two a second. + try + { + push_buffer_(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, + raw_device_time_ns(device_clock_, sample_time_ns, frame.player_time_ns)); + return true; + } + catch (const std::exception& e) + { + ++push_failures_; + logger().warn( + "push failed at sequence id {}: {} -- re-establishing the OpenXR session", frame.sequence_id, e.what()); + + if (!recover_session(stop)) + { + if (stop.load(std::memory_order_relaxed)) + { + return false; // stopping anyway; main's loop exits on the same flag + } + throw std::runtime_error("OpenXR session unrecoverable after " + std::to_string(SESSION_RECOVERY_ATTEMPTS) + + " attempts over " + std::to_string(backoff_budget_ms() / 1000) + + " s -- restart the CloudXR runtime, then restart this plugin"); + } + // Recovered. This frame's pose is stale by a backoff now, so it is not re-pushed; the next + // one Player delivers goes out on the new session. + return false; + } +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/body_joint_mapping.hpp b/src/plugins/jmex/core/inc/jmex/body_joint_mapping.hpp new file mode 100644 index 0000000000..6ef55f6a8b --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/body_joint_mapping.hpp @@ -0,0 +1,95 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +//! A joint's unconstrained pose: the quaternion the motion frame carried, composed down the +//! hierarchy from the base. Meters, in OpenXR axes (see openxr_convention.hpp). +struct JointPose +{ + float position[3]; + float orientation_xyzw[4]; +}; + +//! The joint name for each FullBodyPose slot, indexed by core::BodyJoint. +using SlotNameTable = std::array; + +/*! + * @brief The joint name MOXI Player reports for each slot of the standard human skeleton. This + * table is the whole slot mapping. + * + * Only a joint carrying a controller rotates. The spine, foot, neck, collar and hand joints move + * with the hierarchy: read them for position, not rotation. + */ +extern const SlotNameTable k_standard_human_skeleton_joint_names; + +/*! + * @brief Which joint fills each of the 24 slots. + * + * Split by lifetime: `resolve()` matches joint names against the table once per pairing, so + * `apply()` does no lookups at all. Pure -- no SDK state and no vendor header. + */ +class BodyJointMapping +{ +public: + /*! + * @brief Match this pairing's joint table against the name table. + * + * @param joint_names Every joint on the channel, in index order. + */ + static BodyJointMapping resolve(const std::vector& joint_names); + + /*! + * @brief Fill the 24 slots from one frame's poses. + * + * A slot's `is_valid` says only whether it carries a usable pose on this frame; it does not + * describe the device's sensing coverage. + * + * @param joint_poses This frame's poses, indexed as `resolve()`'s joint_names were. A frame + * that is short, empty, or carries values a consumer cannot use leaves the affected + * slots invalid; it never reads out of range and never ends the pairing. + * @return How many slots went out valid. + */ + size_t apply(const std::vector& joint_poses, core::BodyJoints& out) const; + + //! Index of the joint filling a slot, or -1 when this model has no joint of that name. + int joint_index(core::BodyJoint slot) const; + + //! How many of the 24 slots this model fills. 24 for the standard human skeleton. + size_t filled_slot_count() const + { + return filled_slot_count_; + } + + /*! + * @brief Joint names that matched a slot already taken by an earlier joint. + * + * Non-empty means the model's joint names are not the keys the table assumes. The earlier + * joint keeps the slot; the pairing log names the rest. + */ + const std::vector& contested_joint_names() const + { + return contested_joint_names_; + } + +private: + std::array slot_to_joint_{}; + std::vector contested_joint_names_; + size_t filled_slot_count_ = 0; +}; + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/device_clock.hpp b/src/plugins/jmex/core/inc/jmex/device_clock.hpp new file mode 100644 index 0000000000..cc9dde7c71 --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/device_clock.hpp @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugins +{ +namespace jmex +{ + +//! What goes in a sample's raw device clock. +enum class DeviceClock +{ + //! The time MOXI Player stamped on the frame. + DEVICE, + //! The local time the frame was picked up: the same value as the common clock. + LOCAL, +}; + +//! The clock ``--device-clock=`` names, else ``JMEX_DEVICE_CLOCK``, else DEVICE. Null means not +//! given, and so does an empty variable. Any value but ``device`` or ``local`` throws +//! std::runtime_error naming where it came from, even one the flag overrides. +DeviceClock choose_device_clock(const char* flag_value, const char* env_value); + +//! The clock's name as ``--device-clock=`` spells it, for the startup line. +const char* device_clock_name(DeviceClock clock); + +//! What a sample carries as its raw device clock, given the local time it was picked up at and the +//! Player time on its frame. A Player time past int64 gives the sample time. +int64_t raw_device_time_ns(DeviceClock clock, int64_t sample_time_ns, uint64_t player_time_ns); + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/full_body_frame.hpp b/src/plugins/jmex/core/inc/jmex/full_body_frame.hpp new file mode 100644 index 0000000000..0b3a553677 --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/full_body_frame.hpp @@ -0,0 +1,27 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include + +namespace plugins +{ +namespace jmex +{ + +//! One frame from MOXI Player, in the 24 slots of ``FullBodyPose``. +struct FullBodyFrame +{ + core::BodyJoints joints; + //! Whether every slot is valid on this frame. + bool all_joint_poses_tracked = false; + uint32_t sequence_id = 0; + //! Unix epoch nanoseconds on Player's machine. + uint64_t player_time_ns = 0; +}; + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/full_body_publisher.hpp b/src/plugins/jmex/core/inc/jmex/full_body_publisher.hpp new file mode 100644 index 0000000000..f8be604bfc --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/full_body_publisher.hpp @@ -0,0 +1,115 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "device_clock.hpp" +#include "full_body_frame.hpp" + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +/*! + * @brief Publishes the operator's body pose as ``FullBodyPose`` on the generic full-body path. + * + * Each frame arrives from ReceiverSession already in the 24 slots. The transport is owned here so + * that publish() can replace it after a runtime restart. + */ +class FullBodyPublisher +{ +public: + //! Pushes one serialized sample: the buffer, its size, the sample time on the common clock and + //! the raw device clock. Throws when the push fails. + using PushBufferFunction = std::function; + + //! Brings a transport up and returns the function that pushes on it, which owns it: the + //! transport goes when the function does. Throws if no transport can be brought up. + //! @a wait_for_system: see establish_session(). + using TransportFactory = std::function; + + //! Waits @a wait_ms before a recovery attempt. Returns false, without waiting the rest, once + //! @a stop is set. + using BackoffWait = std::function& stop)>; + + //! The collection's sample size. Must not exceed + //! LiveFullBodyTrackerJmexImpl::MAX_FLATBUFFER_SIZE, or the reader throws. + static constexpr size_t MAX_FLATBUFFER_SIZE = 16 * 1024; + + //! @param collection_id The collection to publish on, used verbatim: what the operator passed + //! to ``--collection-id``. + //! @param device_clock What each sample carries as its raw device clock. + //! @throws std::runtime_error if the OpenXR session cannot be created. + FullBodyPublisher(const std::string& collection_id, DeviceClock device_clock); + + //! A transport passed in keeps publishing and recovery testable without an OpenXR runtime. + //! @param create_transport Called here, and again for each recovery attempt. Must not be empty. + //! @param wait Waits out each backoff. Empty means sleeping it, as the constructor above does. + //! @throws whatever @a create_transport throws. + FullBodyPublisher(TransportFactory create_transport, DeviceClock device_clock, BackoffWait wait = {}); + + FullBodyPublisher(const FullBodyPublisher&) = delete; + FullBodyPublisher& operator=(const FullBodyPublisher&) = delete; + FullBodyPublisher(FullBodyPublisher&&) = delete; + FullBodyPublisher& operator=(FullBodyPublisher&&) = delete; + + /*! + * @brief Serialize one frame from ReceiverSession::poll() and push it as one sample. + * + * @param stop Checked while a recovery backoff is waited out. + * @return Whether the frame went out. A frame whose push failed is not sent again, even when + * the session comes back. + * @throws std::runtime_error when the OpenXR session could not be re-established within its + * retry budget; the caller should report the counters and exit. + */ + [[nodiscard]] bool publish(const FullBodyFrame& frame, const std::atomic& stop); + + //! Pushes that threw, each of which cost the frame it was carrying. + uint64_t push_failures() const + { + return push_failures_; + } + + //! Sessions re-established in response to one of those. Fewer than push_failures() means some + //! failure did not lead to a recovery -- the plugin was stopping, or it gave up. + uint64_t session_recoveries() const + { + return session_recoveries_; + } + + //! Recovery episodes that spent the whole budget without getting a session back. At most one, + //! since the next thing that happens is the throw that ends the run. + uint64_t recoveries_abandoned() const + { + return recoveries_abandoned_; + } + +private: + //! Bring a transport up in place of the one held. Also the recovery path. + //! @param wait_for_system True at startup: wait untimed for an OpenXR system, which gives an + //! operator time to connect a headset. False when recovering, where an untimed wait + //! would ignore Ctrl-C: the backoff decides how long to keep trying. + void establish_session(bool wait_for_system); + + //! Re-establish the session after a push threw, waiting out a bounded backoff before each + //! attempt. \return false if \a stop was set or the budget ran out. + bool recover_session(const std::atomic& stop); + + TransportFactory create_transport_; + BackoffWait wait_; + PushBufferFunction push_buffer_; + DeviceClock device_clock_; + uint64_t push_failures_ = 0; + uint64_t session_recoveries_ = 0; + uint64_t recoveries_abandoned_ = 0; +}; + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/openxr_convention.hpp b/src/plugins/jmex/core/inc/jmex/openxr_convention.hpp new file mode 100644 index 0000000000..eb3016a9cd --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/openxr_convention.hpp @@ -0,0 +1,25 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "body_joint_mapping.hpp" + +namespace plugins +{ +namespace jmex +{ + +/*! + * @brief Re-express one MOXI-space pose in the axis convention this channel's consumers expect: + * OpenXR's +X right, +Y up, -Z forward, left and right being the operator's. + * + * MOXI's global frame is +X to the operator's left, +Y behind them, +Z up, right-handed. + * + * @param moxi_position Joint position in MOXI space, meters. + * @param moxi_orientation_wxyz Joint orientation in MOXI space, scalar first, as the SDK answers. + */ +JointPose moxi_pose_to_openxr(const float moxi_position[3], const float moxi_orientation_wxyz[4]); + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/plugin_args.hpp b/src/plugins/jmex/core/inc/jmex/plugin_args.hpp new file mode 100644 index 0000000000..222357e771 --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/plugin_args.hpp @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "device_clock.hpp" +#include "receiver_config.hpp" + +#include + +namespace plugins +{ +namespace jmex +{ + +//! What the plugin's command line sets. +struct PluginArgs +{ + ReceiverConfig receiver; + std::string collection_id = "jmex_full_body"; + DeviceClock device_clock = DeviceClock::DEVICE; +}; + +/*! + * @brief Parse the plugin's named arguments; the last occurrence of a flag wins. + * + * ``device_clock_env`` is ``JMEX_DEVICE_CLOCK``, which ``--device-clock=`` overrides. Throws + * std::runtime_error naming the argument it cannot use. + */ +PluginArgs parse_plugin_args(int argc, const char* const* argv, const char* device_clock_env); + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/inc/jmex/receiver_config.hpp b/src/plugins/jmex/core/inc/jmex/receiver_config.hpp new file mode 100644 index 0000000000..0c0542cca3 --- /dev/null +++ b/src/plugins/jmex/core/inc/jmex/receiver_config.hpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugins +{ +namespace jmex +{ + +/*! + * @brief What the SDK needs from the host to bring a receiver up: the fields of the SDK's sample + * config (MOXIReceiverSDK/samples/MOXIReceiverCfg.h), all properties of this machine + * rather than of the device. + */ +struct ReceiverConfig +{ + //! MOXI channel to open, one skeleton per channel. 1..10 is what a live capture assigns; 255 + //! is what Player uses when it replays a recording. + int channel = 1; + + /*! + * @brief Local address the receiver's TCP server binds. Empty means every interface. + * + * Advertisements go out from every broadcast-capable interface whatever this is set to, and + * Player connects back to the address it heard one from: bind an address Player can reach. + */ + std::string bind_ip; + + //! Where channel advertisements are broadcast. Empty means 255.255.255.255, which every MOXI + //! Player on the subnet hears -- including ones on other machines, which then contend for the + //! same channel. Naming this host keeps a pairing local. + std::string broadcast_ip; + + //! TCP port Player connects back on, advertised in every broadcast. Each receiver on one + //! machine needs its own. + int tcp_port = 7000; + + //! Base of the per-channel data ports -- channel N receives on this + 1 + N. Only worth moving + //! to make those ports predictable, for a firewall rule or to tell two receivers apart; a + //! collision already relocates itself and announces where it landed. It does not move where + //! advertisements are sent, which Player fixes at 10100. + int udp_start_port = 10100; +}; + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/openxr_convention.cpp b/src/plugins/jmex/core/openxr_convention.cpp new file mode 100644 index 0000000000..4fd5f6a404 --- /dev/null +++ b/src/plugins/jmex/core/openxr_convention.cpp @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/jmex/openxr_convention.hpp" + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +//! The change of basis on a plain 3-vector: a half turn about (0, 1, 1)/sqrt(2), determinant +1. +void moxi_axes_to_openxr(const float moxi[3], float openxr[3]) +{ + openxr[0] = -moxi[0]; + openxr[1] = moxi[2]; + openxr[2] = moxi[1]; +} + +} // namespace + +JointPose moxi_pose_to_openxr(const float moxi_position[3], const float moxi_orientation_wxyz[4]) +{ + JointPose pose{ { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f, 1.0f } }; + + moxi_axes_to_openxr(moxi_position, pose.position); + + // Carried across the change of basis C, a rotation q becomes C q C^-1: the scalar part stays + // and the vector part moves by C, so the axis map above serves the orientation too. + const float moxi_vector_part[3] = { moxi_orientation_wxyz[1], moxi_orientation_wxyz[2], moxi_orientation_wxyz[3] }; + float openxr_vector_part[3] = { 0.0f, 0.0f, 0.0f }; + moxi_axes_to_openxr(moxi_vector_part, openxr_vector_part); + + // Deliberately a separate step: the SDK answers scalar-first and the schema stores it last. + // Doing the reshuffle and the change of basis in the wrong order raises nothing at all -- it + // just tilts every published pose. + pose.orientation_xyzw[0] = openxr_vector_part[0]; + pose.orientation_xyzw[1] = openxr_vector_part[1]; + pose.orientation_xyzw[2] = openxr_vector_part[2]; + pose.orientation_xyzw[3] = moxi_orientation_wxyz[0]; + + return pose; +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/core/plugin_args.cpp b/src/plugins/jmex/core/plugin_args.cpp new file mode 100644 index 0000000000..ea0a79cd85 --- /dev/null +++ b/src/plugins/jmex/core/plugin_args.cpp @@ -0,0 +1,123 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/jmex/plugin_args.hpp" + +#include +#include +#include + +namespace plugins +{ +namespace jmex +{ + +namespace +{ + +constexpr int k_max_port = 65535; +// The SDK keeps a channel in one byte; 0 is no channel. +constexpr int k_max_channel = 255; + +bool starts_with(const std::string& value, const std::string& prefix) +{ + return value.size() >= prefix.size() && value.compare(0, prefix.size(), prefix) == 0; +} + +//! The flag's value as a whole number in [min, max], or an error naming the flag and the value. +//! Partially parsed values are rejected: "2.9" would otherwise select channel 2. +int parse_whole_number(const std::string& flag, const std::string& value, int min, int max) +{ + const std::string rejection = flag + " takes a whole number from " + std::to_string(min) + " to " + + std::to_string(max) + ", got '" + value + "'"; + int parsed = 0; + try + { + std::size_t consumed = 0; + parsed = std::stoi(value, &consumed); + if (consumed != value.size()) + { + throw std::invalid_argument("trailing characters after the number"); + } + } + catch (const std::exception&) + { + throw std::runtime_error(rejection); + } + if (parsed < min || parsed > max) + { + throw std::runtime_error(rejection); + } + return parsed; +} + +} // namespace + +PluginArgs parse_plugin_args(int argc, const char* const* argv, const char* device_clock_env) +{ + PluginArgs args; + + args.device_clock = choose_device_clock(nullptr, device_clock_env); + + // Do not add positional arguments: the PluginManager injects --plugin-root-id= ahead of + // plugin.yaml's and PluginConfig's. + for (int i = 1; i < argc; ++i) + { + const std::string arg = argv[i]; + if (starts_with(arg, "--channel=")) + { + args.receiver.channel = + parse_whole_number("--channel=", arg.substr(std::string("--channel=").size()), 1, k_max_channel); + } + else if (starts_with(arg, "--tcp-port=")) + { + args.receiver.tcp_port = + parse_whole_number("--tcp-port=", arg.substr(std::string("--tcp-port=").size()), 1, k_max_port); + } + else if (starts_with(arg, "--udp-start-port=")) + { + args.receiver.udp_start_port = parse_whole_number( + "--udp-start-port=", arg.substr(std::string("--udp-start-port=").size()), 1, k_max_port); + } + else if (starts_with(arg, "--bind-ip=")) + { + args.receiver.bind_ip = arg.substr(std::string("--bind-ip=").size()); + } + else if (starts_with(arg, "--broadcast-ip=")) + { + args.receiver.broadcast_ip = arg.substr(std::string("--broadcast-ip=").size()); + } + else if (starts_with(arg, "--collection-id=")) + { + args.collection_id = arg.substr(std::string("--collection-id=").size()); + } + else if (starts_with(arg, "--device-clock=")) + { + args.device_clock = + choose_device_clock(arg.substr(std::string("--device-clock=").size()).c_str(), device_clock_env); + } + else if (starts_with(arg, "--plugin-root-id=")) + { + // Not used by this plugin. + } + else + { + throw std::runtime_error("unknown argument '" + arg + "': every option is --name=value"); + } + } + + // Checked once both are final, whichever came last. Both are range-checked above, so the sum + // cannot overflow. + const int data_port = args.receiver.udp_start_port + 1 + args.receiver.channel; + if (data_port > k_max_port) + { + throw std::runtime_error("--udp-start-port=" + std::to_string(args.receiver.udp_start_port) + " puts channel " + + std::to_string(args.receiver.channel) + "'s data port at " + + std::to_string(data_port) + ", past " + std::to_string(k_max_port)); + } + + return args; +} + +} // namespace jmex +} // namespace plugins diff --git a/src/plugins/jmex/plugin.yaml b/src/plugins/jmex/plugin.yaml new file mode 100644 index 0000000000..a4d28061bc --- /dev/null +++ b/src/plugins/jmex/plugin.yaml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: jmex +description: "j-mex AgileMaster motion capture: the operator's body pose as FullBodyPose" +command: "./jmex_plugin" +version: "1.0.0" +args: + - "--channel=1" + - "--collection-id=jmex_full_body" +devices: + - path: "/mocap/jmex" + type: "full_body" + description: "MOXI Player channel as a 24-slot body pose, OpenXR axes (published on )" diff --git a/tests/cpp/core/live_trackers/test_vendor_validation.cpp b/tests/cpp/core/live_trackers/test_vendor_validation.cpp index cad155b9b2..c32fa6bb9f 100644 --- a/tests/cpp/core/live_trackers/test_vendor_validation.cpp +++ b/tests/cpp/core/live_trackers/test_vendor_validation.cpp @@ -112,6 +112,14 @@ TEST_CASE("vendor validation: accepted configurations resolve extensions", "[liv REQUIRE_FALSE(contains(extensions, "XR_BD_body_tracking")); } + SECTION("j-mex vendor selects tensor data extensions") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.jmex" } } }; + const auto extensions = core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors }); + REQUIRE(contains(extensions, "XR_NVX1_tensor_data")); + REQUIRE_FALSE(contains(extensions, "XR_BD_body_tracking")); + } + SECTION("Xsens vendor accepts collection and sample-size parameters") { VendorList vendors{ { body.get(), core::TrackerVendor{ "body.xsens", @@ -119,6 +127,12 @@ TEST_CASE("vendor validation: accepted configurations resolve extensions", "[liv { "max_flatbuffer_size", "32768" } } } } }; REQUIRE_NOTHROW(core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); } + + SECTION("j-mex vendor accepts a collection parameter") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.jmex", { { "collection_id", "custom_jmex" } } } } }; + REQUIRE_NOTHROW(core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); + } } TEST_CASE("vendor validation: invalid configurations are rejected", "[live_trackers][vendor]") @@ -180,6 +194,14 @@ TEST_CASE("vendor validation: invalid configurations are rejected", "[live_track REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support parameter")); } + SECTION("an unsupported j-mex vendor parameter is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.jmex", { { "unsupported", "value" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support parameter")); + } + SECTION("an invalid Xsens max flatbuffer size is rejected") { VendorList vendors{ @@ -188,6 +210,14 @@ TEST_CASE("vendor validation: invalid configurations are rejected", "[live_track REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must be a positive integer")); } + SECTION("a j-mex max flatbuffer size is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.jmex", { { "max_flatbuffer_size", "16384" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support parameter")); + } + SECTION("an empty Xsens collection_id is rejected rather than silently never connecting") { // The pusher and reader rendezvous on collection_id, so a mismatch fails silently and @@ -198,6 +228,14 @@ TEST_CASE("vendor validation: invalid configurations are rejected", "[live_track REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must not be empty")); } + SECTION("an empty j-mex collection_id is rejected rather than silently never connecting") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.jmex", { { "collection_id", "" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must not be empty")); + } + SECTION("a duplicate selection for the same tracker is rejected") { VendorList vendors{ diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 1fd14aa203..81622e6146 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -3,10 +3,11 @@ add_subdirectory(plugin_utils) -# The SpaceMouse and gamepad plugins (and their core libraries) are Linux only. +# The SpaceMouse, gamepad and jmex plugins (and their core libraries) are Linux only. if(CMAKE_SYSTEM_NAME STREQUAL "Linux") add_subdirectory(spacemouse) add_subdirectory(gamepad) + add_subdirectory(jmex) endif() if(BUILD_PLUGIN_OGLO) diff --git a/tests/cpp/plugins/jmex/CMakeLists.txt b/tests/cpp/plugins/jmex/CMakeLists.txt new file mode 100644 index 0000000000..7baa992b62 --- /dev/null +++ b/tests/cpp/plugins/jmex/CMakeLists.txt @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The real recovery backoff sleeps, and its test sets the stop flag from a second thread. +find_package(Threads REQUIRED) + +add_executable(jmex_plugin_tests + test_jmex_plugin_core.cpp + test_plugin_args.cpp +) + +# jmex_plugin_core brings in the publisher's own OpenXR transport. The tests pass a transport in +# instead, so that one is linked but never brought up, and no OpenXR runtime is needed. +target_link_libraries(jmex_plugin_tests PRIVATE + Catch2::Catch2WithMain + Threads::Threads + isaac::jmex_plugin_core + oxr::oxr_utils +) + +# Each [jmex] tag becomes the ctest label the example's Python tests carry, so -L jmex runs both. +# A failed push is logged, and the first logger builds this process's own sinks: keep their file in +# the build tree, and off a forwarding socket an interactive shell may be exporting. +catch_discover_tests(jmex_plugin_tests ADD_TAGS_AS_LABELS + PROPERTIES ENVIRONMENT "ISAACCAPTURE_LOG_DIR=${CMAKE_CURRENT_BINARY_DIR}/logs;ISAACCAPTURE_LOG_SOCKET=" +) diff --git a/tests/cpp/plugins/jmex/test_jmex_plugin_core.cpp b/tests/cpp/plugins/jmex/test_jmex_plugin_core.cpp new file mode 100644 index 0000000000..5ff3228007 --- /dev/null +++ b/tests/cpp/plugins/jmex/test_jmex_plugin_core.cpp @@ -0,0 +1,634 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the jmex plugin's SDK-free parts. The name table is checked for the invariants +// the mapping relies on, not row by row. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Catch::Approx; +using Catch::Matchers::ContainsSubstring; +using namespace plugins::jmex; + +namespace +{ + +constexpr size_t k_slot_count = core::BodyJoint_NUM_JOINTS; + +//! The joint table of a channel carrying a joint for every slot, in slot order. +std::vector complete_skeleton() +{ + std::vector names; + for (const std::string_view name : k_standard_human_skeleton_joint_names) + { + names.emplace_back(name); + } + return names; +} + +//! A frame in which every joint carries a distinct, valid pose: position (i, 2i, 3i), identity +//! rotation. The values only have to be distinguishable. +std::vector frame_for(size_t joint_count) +{ + std::vector poses; + for (size_t i = 0; i < joint_count; ++i) + { + const float f = static_cast(i); + poses.push_back(JointPose{ { f, 2.0f * f, 3.0f * f }, { 0.0f, 0.0f, 0.0f, 1.0f } }); + } + return poses; +} + +const core::BodyJointPose& slot_of(const core::BodyJoints& joints, core::BodyJoint slot) +{ + return *joints.joints()->Get(slot); +} + +size_t valid_count(const core::BodyJoints& joints) +{ + size_t valid = 0; + for (flatbuffers::uoffset_t slot = 0; slot < joints.joints()->size(); ++slot) + { + valid += joints.joints()->Get(slot)->is_valid() ? 1 : 0; + } + return valid; +} + +//! A frame as ReceiverSession hands it out, filled from frame_for(): slot i carries joint i. Frames +//! are told apart by their Player time, which the device clock carries through. +FullBodyFrame frame_at(uint64_t player_time_ns) +{ + FullBodyFrame frame; + BodyJointMapping::resolve(complete_skeleton()).apply(frame_for(k_slot_count), frame.joints); + frame.all_joint_poses_tracked = true; + frame.player_time_ns = player_time_ns; + return frame; +} + +//! One sample as the transport received it. +struct PushedSample +{ + std::vector buffer; + int64_t sample_time_ns = 0; + int64_t raw_device_time_ns = 0; +}; + +/*! + * @brief Stands in for the CloudXR runtime behind the publisher's transport. + * + * Records each transport the publisher asks for, in order. After restart() the transports already + * up refuse every push, as a restarted runtime's do; after go_down() no new one comes up either. + */ +class FakeRuntime +{ +public: + struct Transport + { + bool wait_for_system = false; + //! How many of the publisher's transports were still up when it asked for this one. + int others_up = 0; + bool came_up = false; + bool refuses_pushes = false; + std::vector samples; + }; + + FullBodyPublisher::TransportFactory factory() + { + return [this](bool wait_for_system) -> FullBodyPublisher::PushBufferFunction + { + transports_.push_back(Transport{ .wait_for_system = wait_for_system, .others_up = up_ }); + if (down_) + { + throw std::runtime_error("the runtime is down"); + } + transports_.back().came_up = true; + + const size_t index = transports_.size() - 1; + // Counted as up until the publisher lets go of its push function. + const auto up = std::make_shared(up_); + return + [this, index, up](const uint8_t* buffer, size_t size, int64_t sample_time_ns, int64_t raw_device_time_ns) + { + Transport& transport = transports_[index]; + if (transport.refuses_pushes) + { + throw std::runtime_error("the runtime restarted"); + } + transport.samples.push_back(PushedSample{ { buffer, buffer + size }, sample_time_ns, raw_device_time_ns }); + }; + }; + } + + void restart() + { + for (Transport& transport : transports_) + { + transport.refuses_pushes = true; + } + } + + void go_down() + { + restart(); + down_ = true; + } + + const std::vector& transports() const + { + return transports_; + } + +private: + struct UpWhileHeld + { + explicit UpWhileHeld(int& up) : up_(up) + { + ++up_; + } + ~UpWhileHeld() + { + --up_; + } + UpWhileHeld(const UpWhileHeld&) = delete; + UpWhileHeld& operator=(const UpWhileHeld&) = delete; + + int& up_; + }; + + std::vector transports_; + int up_ = 0; + bool down_ = false; +}; + +} // namespace + +TEST_CASE("openxr convention: MOXI axes become OpenXR axes", "[unit][jmex][convention]") +{ + const float identity[4] = { 1.0f, 0.0f, 0.0f, 0.0f }; + + SECTION("each MOXI axis lands on the OpenXR axis that means the same thing") + { + const float left[3] = { 1.0f, 0.0f, 0.0f }; + const float behind[3] = { 0.0f, 1.0f, 0.0f }; + const float up[3] = { 0.0f, 0.0f, 1.0f }; + + const JointPose to_left = moxi_pose_to_openxr(left, identity); + REQUIRE(to_left.position[0] == Approx(-1.0f)); // the operator's left is OpenXR -X + REQUIRE(to_left.position[1] == Approx(0.0f)); + REQUIRE(to_left.position[2] == Approx(0.0f)); + + const JointPose to_behind = moxi_pose_to_openxr(behind, identity); + REQUIRE(to_behind.position[0] == Approx(0.0f)); + REQUIRE(to_behind.position[1] == Approx(0.0f)); + REQUIRE(to_behind.position[2] == Approx(1.0f)); // behind is OpenXR +Z, forward being -Z + + const JointPose to_up = moxi_pose_to_openxr(up, identity); + REQUIRE(to_up.position[0] == Approx(0.0f)); + REQUIRE(to_up.position[1] == Approx(1.0f)); // up is OpenXR +Y + REQUIRE(to_up.position[2] == Approx(0.0f)); + } + + SECTION("the map is a rotation, not a reflection") + { + // Both frames are right-handed, so the image of a right-handed triad must stay one: + // x_image cross y_image == z_image. A single flipped sign would mirror every pose -- + // left and right limbs swapped -- and nothing downstream would report an error. + const float x[3] = { 1.0f, 0.0f, 0.0f }; + const float y[3] = { 0.0f, 1.0f, 0.0f }; + const float z[3] = { 0.0f, 0.0f, 1.0f }; + + const JointPose ix = moxi_pose_to_openxr(x, identity); + const JointPose iy = moxi_pose_to_openxr(y, identity); + const JointPose iz = moxi_pose_to_openxr(z, identity); + + const float cross[3] = { + ix.position[1] * iy.position[2] - ix.position[2] * iy.position[1], + ix.position[2] * iy.position[0] - ix.position[0] * iy.position[2], + ix.position[0] * iy.position[1] - ix.position[1] * iy.position[0], + }; + REQUIRE(cross[0] == Approx(iz.position[0])); + REQUIRE(cross[1] == Approx(iz.position[1])); + REQUIRE(cross[2] == Approx(iz.position[2])); + } + + SECTION("the quaternion is reordered scalar-last and its axis carried by the same map") + { + // A quarter turn about MOXI's up axis, scalar-first as the SDK answers. + const float half = 0.7071068f; + const float quarter_turn_about_up[4] = { half, 0.0f, 0.0f, half }; + const float origin[3] = { 0.0f, 0.0f, 0.0f }; + + const JointPose pose = moxi_pose_to_openxr(origin, quarter_turn_about_up); + + REQUIRE(pose.orientation_xyzw[0] == Approx(0.0f)); + REQUIRE(pose.orientation_xyzw[1] == Approx(half)); // the axis moved to OpenXR +Y, as up does + REQUIRE(pose.orientation_xyzw[2] == Approx(0.0f)); + REQUIRE(pose.orientation_xyzw[3] == Approx(half)); // the scalar, unchanged and now last + } + + SECTION("a rotation about MOXI's lateral axis keeps its angle and flips its axis") + { + // The lateral axis is the only one whose sign flips. + const float half = 0.7071068f; + const float quarter_turn_about_left[4] = { half, half, 0.0f, 0.0f }; + const float origin[3] = { 0.0f, 0.0f, 0.0f }; + + const JointPose pose = moxi_pose_to_openxr(origin, quarter_turn_about_left); + + REQUIRE(pose.orientation_xyzw[0] == Approx(-half)); + REQUIRE(pose.orientation_xyzw[1] == Approx(0.0f)); + REQUIRE(pose.orientation_xyzw[2] == Approx(0.0f)); + REQUIRE(pose.orientation_xyzw[3] == Approx(half)); + } +} + +TEST_CASE("body joint mapping: the name table covers all 24 slots exactly once", "[unit][jmex][mapping]") +{ + std::set seen; + for (const std::string_view name : k_standard_human_skeleton_joint_names) + { + INFO("joint " << name); + REQUIRE_FALSE(name.empty()); + // A repeated name would leave one slot unfilled while another was contested. + REQUIRE(seen.insert(name).second); + } + + // Resolving is the table read backwards; the two must agree for every slot. + const BodyJointMapping mapping = BodyJointMapping::resolve(complete_skeleton()); + for (size_t slot = 0; slot < k_slot_count; ++slot) + { + INFO("slot " << slot); + REQUIRE(mapping.joint_index(static_cast(slot)) == static_cast(slot)); + } +} + +TEST_CASE("body joint mapping: a name the table does not carry fills no slot", "[unit][jmex][mapping]") +{ + const BodyJointMapping mapping = + BodyJointMapping::resolve({ "left_shoulder_pitch_joint", "left_thumb_proximal_joint", "" }); + + REQUIRE(mapping.filled_slot_count() == 0); + REQUIRE(mapping.contested_joint_names().empty()); +} + +TEST_CASE("body joint mapping: a model carrying every joint fills every slot", "[unit][jmex][mapping]") +{ + const std::vector names = complete_skeleton(); + const BodyJointMapping mapping = BodyJointMapping::resolve(names); + + REQUIRE(mapping.filled_slot_count() == k_slot_count); + REQUIRE(mapping.contested_joint_names().empty()); + + core::BodyJoints joints; + REQUIRE(mapping.apply(frame_for(names.size()), joints) == k_slot_count); + + REQUIRE(valid_count(joints) == k_slot_count); + // complete_skeleton() is in slot order, so slot i carries joint i's position (i, 2i, 3i). + const core::Point& head = slot_of(joints, core::BodyJoint_HEAD).pose().position(); + REQUIRE(head.x() == Approx(static_cast(core::BodyJoint_HEAD))); + REQUIRE(head.y() == Approx(2.0f * static_cast(core::BodyJoint_HEAD))); +} + +TEST_CASE("body joint mapping: joints the model does not carry leave their slots invalid", "[unit][jmex][mapping]") +{ + std::vector names = complete_skeleton(); + names[core::BodyJoint_LEFT_HAND] = "something_else"; + + const BodyJointMapping mapping = BodyJointMapping::resolve(names); + + REQUIRE(mapping.filled_slot_count() == k_slot_count - 1); + REQUIRE(mapping.joint_index(core::BodyJoint_LEFT_HAND) == -1); + + core::BodyJoints joints; + REQUIRE(mapping.apply(frame_for(names.size()), joints) == k_slot_count - 1); + + REQUIRE_FALSE(slot_of(joints, core::BodyJoint_LEFT_HAND).is_valid()); + REQUIRE(slot_of(joints, core::BodyJoint_RIGHT_HAND).is_valid()); + REQUIRE(valid_count(joints) == k_slot_count - 1); +} + +TEST_CASE("body joint mapping: a second joint claiming a filled slot is reported, not swallowed", "[unit][jmex][mapping]") +{ + std::vector names = complete_skeleton(); + const std::string contested(k_standard_human_skeleton_joint_names[core::BodyJoint_NECK]); + names.push_back(contested); + + const BodyJointMapping mapping = BodyJointMapping::resolve(names); + + // The earlier joint keeps the slot. + REQUIRE(mapping.joint_index(core::BodyJoint_NECK) == static_cast(core::BodyJoint_NECK)); + REQUIRE(mapping.filled_slot_count() == k_slot_count); + REQUIRE(mapping.contested_joint_names() == std::vector{ contested }); +} + +TEST_CASE("body joint mapping: a bad frame costs its own slots and nothing else", "[unit][jmex][mapping]") +{ + const std::vector names = complete_skeleton(); + const BodyJointMapping mapping = BodyJointMapping::resolve(names); + core::BodyJoints joints; + + SECTION("a joint whose position is not a number") + { + std::vector frame = frame_for(names.size()); + frame[core::BodyJoint_LEFT_ELBOW].position[1] = std::numeric_limits::quiet_NaN(); + + mapping.apply(frame, joints); + + REQUIRE_FALSE(slot_of(joints, core::BodyJoint_LEFT_ELBOW).is_valid()); + REQUIRE(valid_count(joints) == k_slot_count - 1); + } + + SECTION("a joint whose quaternion is not a rotation") + { + std::vector frame = frame_for(names.size()); + frame[core::BodyJoint_RIGHT_KNEE].orientation_xyzw[3] = 0.0f; // all four components now zero + + mapping.apply(frame, joints); + + REQUIRE_FALSE(slot_of(joints, core::BodyJoint_RIGHT_KNEE).is_valid()); + REQUIRE(valid_count(joints) == k_slot_count - 1); + } + + SECTION("float round-off in the quaternion is not a bad frame") + { + std::vector frame = frame_for(names.size()); + frame[core::BodyJoint_PELVIS].orientation_xyzw[3] = 1.0f + 1e-5f; + + mapping.apply(frame, joints); + + REQUIRE(slot_of(joints, core::BodyJoint_PELVIS).is_valid()); + } + + SECTION("a frame that carried nothing invalidates every slot and ends no pairing") + { + REQUIRE(mapping.apply(std::vector{}, joints) == 0); + + REQUIRE(valid_count(joints) == 0); + + // The mapping holds no per-frame state, so the next frame fills every slot again. + mapping.apply(frame_for(names.size()), joints); + REQUIRE(valid_count(joints) == k_slot_count); + } +} + +TEST_CASE("device clock: device carries the Player time unchanged", "[unit][jmex][device_clock]") +{ + // Unix epoch nanoseconds on Player's machine: 2026-10-07 04:20:00.123456789 UTC. + const uint64_t player_time_ns = 1791346800123456789; + + // Whatever the local time the frame was picked up at. + REQUIRE(raw_device_time_ns(DeviceClock::DEVICE, 123456789012345, player_time_ns) == 1791346800123456789); + REQUIRE(raw_device_time_ns(DeviceClock::DEVICE, 987654321098765, player_time_ns) == 1791346800123456789); +} + +TEST_CASE("device clock: a Player time past int64 falls back to the sample time", "[unit][jmex][device_clock]") +{ + const uint64_t past_int64 = static_cast(std::numeric_limits::max()) + 1; + + REQUIRE(raw_device_time_ns(DeviceClock::DEVICE, 123456789012345, past_int64) == 123456789012345); + REQUIRE(raw_device_time_ns(DeviceClock::DEVICE, 123456789012345, std::numeric_limits::max()) == + 123456789012345); + REQUIRE(raw_device_time_ns(DeviceClock::DEVICE, 123456789012345, past_int64 - 1) == + std::numeric_limits::max()); +} + +TEST_CASE("device clock: local carries the sample time unchanged", "[unit][jmex][device_clock]") +{ + // Both clocks carry the same value, whatever Player stamped on the frame. + REQUIRE(raw_device_time_ns(DeviceClock::LOCAL, 123456789012345, 1791346800123456789) == 123456789012345); + REQUIRE(raw_device_time_ns(DeviceClock::LOCAL, 123456789012345, 0) == 123456789012345); +} + +TEST_CASE("device clock: the flag wins over the environment, which wins over the default", "[unit][jmex][device_clock]") +{ + REQUIRE(choose_device_clock(nullptr, nullptr) == DeviceClock::DEVICE); + REQUIRE(choose_device_clock(nullptr, "local") == DeviceClock::LOCAL); + REQUIRE(choose_device_clock("local", nullptr) == DeviceClock::LOCAL); + REQUIRE(choose_device_clock("device", "local") == DeviceClock::DEVICE); + REQUIRE(choose_device_clock("local", "device") == DeviceClock::LOCAL); + + // An environment variable set to nothing is how a shell unsets one for a single command. + REQUIRE(choose_device_clock(nullptr, "") == DeviceClock::DEVICE); +} + +TEST_CASE("device clock: a value it cannot read stops the plugin and says where it came from", + "[unit][jmex][device_clock]") +{ + REQUIRE_THROWS_WITH(choose_device_clock("Device", nullptr), ContainsSubstring("--device-clock=")); + REQUIRE_THROWS_WITH(choose_device_clock("", nullptr), ContainsSubstring("--device-clock=")); + REQUIRE_THROWS_WITH(choose_device_clock(nullptr, "loc"), ContainsSubstring("JMEX_DEVICE_CLOCK")); + REQUIRE_THROWS_WITH(choose_device_clock("sequence", nullptr), ContainsSubstring("--device-clock=")); + REQUIRE_THROWS_WITH(choose_device_clock(nullptr, "sequence"), ContainsSubstring("JMEX_DEVICE_CLOCK")); + + // Even when the flag would have won: a misspelt environment variable is left in place for the + // next launch, which has no flag to cover for it. + REQUIRE_THROWS_WITH(choose_device_clock("local", "dev"), ContainsSubstring("JMEX_DEVICE_CLOCK")); +} + +TEST_CASE("device clock: the startup line names it the way the flag spells it", "[unit][jmex][device_clock]") +{ + REQUIRE(std::string(device_clock_name(DeviceClock::DEVICE)) == "device"); + REQUIRE(std::string(device_clock_name(DeviceClock::LOCAL)) == "local"); + + // What the log says can be pasted back in. + for (DeviceClock clock : { DeviceClock::DEVICE, DeviceClock::LOCAL }) + { + REQUIRE(choose_device_clock(device_clock_name(clock), nullptr) == clock); + } +} + +TEST_CASE("full body publisher: a frame goes out as one FullBodyPose", "[unit][jmex][publisher]") +{ + FakeRuntime runtime; + const std::atomic stop{ false }; + // Unix epoch nanoseconds on Player's machine, nothing like what the local clock reads. + const uint64_t player_time_ns = 1791346800123456789; + + SECTION("with the device clock, the sample carries the frame's Player time") + { + FullBodyPublisher publisher(runtime.factory(), DeviceClock::DEVICE); + + // Up before the first frame, and brought up the one way that waits for the OpenXR system. + REQUIRE(runtime.transports().size() == 1); + CHECK(runtime.transports()[0].wait_for_system); + + const int64_t before_ns = core::os_monotonic_now_ns(); + CHECK(publisher.publish(frame_at(player_time_ns), stop)); + const int64_t after_ns = core::os_monotonic_now_ns(); + + REQUIRE(runtime.transports()[0].samples.size() == 1); + const PushedSample& sample = runtime.transports()[0].samples[0]; + + flatbuffers::Verifier verifier(sample.buffer.data(), sample.buffer.size()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + const core::FullBodyPose& pose = *flatbuffers::GetRoot(sample.buffer.data()); + + CHECK(pose.all_joint_poses_tracked()); + REQUIRE(pose.joints() != nullptr); + for (size_t i = 0; i < k_slot_count; ++i) + { + INFO("slot " << i); + const core::BodyJointPose& slot = slot_of(*pose.joints(), static_cast(i)); + const float f = static_cast(i); + CHECK(slot.is_valid()); + CHECK(slot.pose().position().x() == f); + CHECK(slot.pose().position().y() == 2.0f * f); + CHECK(slot.pose().position().z() == 3.0f * f); + CHECK(slot.pose().orientation().x() == 0.0f); + CHECK(slot.pose().orientation().y() == 0.0f); + CHECK(slot.pose().orientation().z() == 0.0f); + CHECK(slot.pose().orientation().w() == 1.0f); + } + + // Picked up while publish() ran, on the clock every source on this machine shares. + CHECK(sample.sample_time_ns >= before_ns); + CHECK(sample.sample_time_ns <= after_ns); + CHECK(sample.raw_device_time_ns == 1791346800123456789); + } + + SECTION("with the local clock, the raw device clock is the sample time") + { + FullBodyPublisher publisher(runtime.factory(), DeviceClock::LOCAL); + CHECK(publisher.publish(frame_at(player_time_ns), stop)); + + REQUIRE(runtime.transports()[0].samples.size() == 1); + const PushedSample& sample = runtime.transports()[0].samples[0]; + CHECK(sample.raw_device_time_ns == sample.sample_time_ns); + } +} + +TEST_CASE("full body publisher: after a push throws, the transport comes back or the run ends", "[unit][jmex][recovery]") +{ + FakeRuntime runtime; + std::atomic stop{ false }; + // Every backoff the publisher asked for, in milliseconds. None is slept. + std::vector waits; + const auto no_sleep = [&waits](int wait_ms, const std::atomic& stop_flag) + { + waits.push_back(wait_ms); + return !stop_flag.load(std::memory_order_relaxed); + }; + + SECTION("a push that throws once costs that frame, and the next goes out on a new transport") + { + FullBodyPublisher publisher(runtime.factory(), DeviceClock::DEVICE, no_sleep); + CHECK(publisher.publish(frame_at(1000), stop)); + + runtime.restart(); + CHECK_FALSE(publisher.publish(frame_at(2000), stop)); // recovered, but this frame is gone + + REQUIRE(runtime.transports().size() == 2); + CHECK(runtime.transports()[1].came_up); + CHECK_FALSE(runtime.transports()[1].wait_for_system); + CHECK(runtime.transports()[1].others_up == 0); // the dead one was let go first + CHECK(runtime.transports()[1].samples.empty()); // the frame that failed is not pushed again + + CHECK(publisher.publish(frame_at(3000), stop)); + + REQUIRE(runtime.transports().size() == 2); + REQUIRE(runtime.transports()[1].samples.size() == 1); + CHECK(runtime.transports()[1].samples[0].raw_device_time_ns == 3000); + CHECK(runtime.transports()[0].samples.size() == 1); + + CHECK(publisher.push_failures() == 1); + CHECK(publisher.session_recoveries() == 1); + CHECK(publisher.recoveries_abandoned() == 0); + } + + SECTION("a runtime that never comes back ends the run once the backoff is spent") + { + FullBodyPublisher publisher(runtime.factory(), DeviceClock::DEVICE, no_sleep); + runtime.go_down(); + + REQUIRE_THROWS_WITH(publisher.publish(frame_at(1000), stop), ContainsSubstring("restart the CloudXR runtime")); + + CHECK(runtime.transports().size() == 1 + 8); // startup, then eight attempts + CHECK(publisher.push_failures() == 1); + CHECK(publisher.session_recoveries() == 0); + CHECK(publisher.recoveries_abandoned() == 1); + + // One backoff before each attempt. + REQUIRE(waits.size() == 8); + CHECK(waits.front() <= 500); // so a blip costs a blip + for (size_t attempt = 1; attempt < waits.size(); ++attempt) + { + INFO("attempt " << attempt); + // A long outage is not retried at the rate of a short one. + CHECK(waits[attempt] >= waits[attempt - 1]); + } + int total_ms = 0; + for (const int wait_ms : waits) + { + total_ms += wait_ms; + } + CHECK(total_ms >= 20000); // outlasts a CloudXR runtime restart + } + + SECTION("a stop during a backoff ends the recovery, and the run, without an error") + { + // Ctrl-C lands during the third backoff, after two attempts have failed. + const auto stop_during_third_wait = [&waits, &stop](int wait_ms, const std::atomic&) + { + waits.push_back(wait_ms); + if (waits.size() == 3) + { + stop.store(true, std::memory_order_relaxed); + } + return !stop.load(std::memory_order_relaxed); + }; + FullBodyPublisher publisher(runtime.factory(), DeviceClock::DEVICE, stop_during_third_wait); + runtime.go_down(); + + bool pushed = true; + REQUIRE_NOTHROW(pushed = publisher.publish(frame_at(1000), stop)); + CHECK_FALSE(pushed); + + CHECK(publisher.push_failures() == 1); + CHECK(publisher.session_recoveries() == 0); + CHECK(publisher.recoveries_abandoned() == 0); + } + + SECTION("the real backoff is slept in slices, so a stop lands inside the first one") + { + FullBodyPublisher publisher(runtime.factory(), DeviceClock::DEVICE); + runtime.go_down(); + + std::jthread signaller( + [&stop] + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + stop.store(true, std::memory_order_relaxed); + }); + + const auto started = std::chrono::steady_clock::now(); + REQUIRE_NOTHROW(publisher.publish(frame_at(1000), stop)); + const auto took = + std::chrono::duration_cast(std::chrono::steady_clock::now() - started); + + CHECK(took.count() < 500); // the first backoff + } +} diff --git a/tests/cpp/plugins/jmex/test_plugin_args.cpp b/tests/cpp/plugins/jmex/test_plugin_args.cpp new file mode 100644 index 0000000000..912af52f11 --- /dev/null +++ b/tests/cpp/plugins/jmex/test_plugin_args.cpp @@ -0,0 +1,111 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the plugin's command line. + +#include +#include +#include + +#include +#include + +using Catch::Matchers::ContainsSubstring; +using namespace plugins::jmex; + +namespace +{ + +//! Parse as the plugin would, with the PluginManager's injected root id ahead of the arguments. +PluginArgs parse(const std::vector& args, const char* device_clock_env = nullptr) +{ + std::vector argv{ "jmex_plugin", "--plugin-root-id=jmex" }; + for (const std::string& arg : args) + { + argv.push_back(arg.c_str()); + } + return parse_plugin_args(static_cast(argv.size()), argv.data(), device_clock_env); +} + +} // namespace + +TEST_CASE("plugin args: no flags give the defaults", "[unit][jmex][args]") +{ + const PluginArgs args = parse({}); + const ReceiverConfig defaults; + REQUIRE(args.receiver.channel == defaults.channel); + REQUIRE(args.receiver.tcp_port == defaults.tcp_port); + REQUIRE(args.receiver.udp_start_port == defaults.udp_start_port); + REQUIRE(args.receiver.bind_ip.empty()); + REQUIRE(args.receiver.broadcast_ip.empty()); + REQUIRE(args.collection_id == "jmex_full_body"); + REQUIRE(args.device_clock == DeviceClock::DEVICE); +} + +TEST_CASE("plugin args: every flag sets its field, and the last occurrence wins", "[unit][jmex][args]") +{ + const PluginArgs args = + parse({ "--channel=2", "--channel=255", "--tcp-port=7001", "--udp-start-port=20000", "--bind-ip=192.0.2.10", + "--broadcast-ip=192.0.2.10", "--collection-id=suit_b", "--device-clock=local" }); + REQUIRE(args.receiver.channel == 255); + REQUIRE(args.receiver.tcp_port == 7001); + REQUIRE(args.receiver.udp_start_port == 20000); + REQUIRE(args.receiver.bind_ip == "192.0.2.10"); + REQUIRE(args.receiver.broadcast_ip == "192.0.2.10"); + REQUIRE(args.collection_id == "suit_b"); + REQUIRE(args.device_clock == DeviceClock::LOCAL); +} + +TEST_CASE("plugin args: --device-clock= overrides JMEX_DEVICE_CLOCK", "[unit][jmex][args]") +{ + REQUIRE(parse({}, "local").device_clock == DeviceClock::LOCAL); + REQUIRE(parse({ "--device-clock=device" }, "local").device_clock == DeviceClock::DEVICE); +} + +TEST_CASE("plugin args: an argument it does not know stops the plugin", "[unit][jmex][args]") +{ + // A space instead of '=': "--channel" is unknown and "2" is positional, so channel 1 would stay. + REQUIRE_THROWS_WITH(parse({ "--channel", "2" }), ContainsSubstring("'--channel'")); + REQUIRE_THROWS_WITH(parse({ "--chanel=2" }), ContainsSubstring("'--chanel=2'")); + REQUIRE_THROWS_WITH(parse({ "2" }), ContainsSubstring("'2'")); +} + +TEST_CASE("plugin args: a value that is not a whole number stops the plugin", "[unit][jmex][args]") +{ + REQUIRE_THROWS_WITH(parse({ "--channel=2.9" }), ContainsSubstring("--channel=")); + REQUIRE_THROWS_WITH(parse({ "--channel=" }), ContainsSubstring("--channel=")); + REQUIRE_THROWS_WITH(parse({ "--tcp-port=7000x" }), ContainsSubstring("--tcp-port=")); + REQUIRE_THROWS_WITH(parse({ "--udp-start-port=port" }), ContainsSubstring("--udp-start-port=")); +} + +TEST_CASE("plugin args: a channel outside 1..255 stops the plugin", "[unit][jmex][args]") +{ + REQUIRE(parse({ "--channel=1" }).receiver.channel == 1); + REQUIRE(parse({ "--channel=11" }).receiver.channel == 11); + REQUIRE(parse({ "--channel=255" }).receiver.channel == 255); + + REQUIRE_THROWS_WITH(parse({ "--channel=0" }), ContainsSubstring("--channel=")); + REQUIRE_THROWS_WITH(parse({ "--channel=-1" }), ContainsSubstring("--channel=")); + REQUIRE_THROWS_WITH(parse({ "--channel=256" }), ContainsSubstring("--channel=")); + REQUIRE_THROWS_WITH(parse({ "--channel=300" }), ContainsSubstring("--channel=")); +} + +TEST_CASE("plugin args: a port outside 1..65535 stops the plugin", "[unit][jmex][args]") +{ + REQUIRE(parse({ "--tcp-port=65535" }).receiver.tcp_port == 65535); + + REQUIRE_THROWS_WITH(parse({ "--tcp-port=0" }), ContainsSubstring("--tcp-port=")); + REQUIRE_THROWS_WITH(parse({ "--tcp-port=70000" }), ContainsSubstring("--tcp-port=")); + REQUIRE_THROWS_WITH(parse({ "--udp-start-port=0" }), ContainsSubstring("--udp-start-port=")); + REQUIRE_THROWS_WITH(parse({ "--udp-start-port=99999999999" }), ContainsSubstring("--udp-start-port=")); +} + +TEST_CASE("plugin args: a channel data port past 65535 stops the plugin", "[unit][jmex][args]") +{ + // 65279 + 1 + 255 is the last port there is. + REQUIRE(parse({ "--udp-start-port=65279", "--channel=255" }).receiver.udp_start_port == 65279); + + REQUIRE_THROWS_WITH(parse({ "--udp-start-port=65280", "--channel=255" }), ContainsSubstring("65536")); + REQUIRE_THROWS_WITH(parse({ "--channel=255", "--udp-start-port=65280" }), ContainsSubstring("65536")); + REQUIRE_THROWS_WITH(parse({ "--udp-start-port=65535" }), ContainsSubstring("--udp-start-port=")); +} diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index a0cc7b40cc..e571591063 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -3,5 +3,6 @@ add_subdirectory(camera_viz) add_subdirectory(deviceio_live_view) +add_subdirectory(jmex) add_subdirectory(soma_body_publisher) add_subdirectory(soma_hand_publisher) diff --git a/tests/python/examples/jmex/CMakeLists.txt b/tests/python/examples/jmex/CMakeLists.txt new file mode 100644 index 0000000000..d187d00b74 --- /dev/null +++ b/tests/python/examples/jmex/CMakeLists.txt @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# The j-mex G1 retargeting example's Python tests. They need only the staged isaaccapture +# package -- no Isaac Sim, no Isaac Lab, no device. + +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "jmex_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short "${test_file}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + set_tests_properties("jmex_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "jmex;unit" + ) +endforeach() diff --git a/tests/python/examples/jmex/conftest.py b/tests/python/examples/jmex/conftest.py new file mode 100644 index 0000000000..f32f630135 --- /dev/null +++ b/tests/python/examples/jmex/conftest.py @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Import isaaccapture_examples.jmex from this checkout rather than any installed +# copy, so a bare `pytest` works and the ctest entry needs no example path. +# +# The path entry is the example's namespace root, python/. Do not give +# isaaccapture_examples/ an __init__.py to make an import work -- see +# examples/README.md. + +import sys +from pathlib import Path + +_tests_python = Path(__file__).resolve().parents[2] +if str(_tests_python) not in sys.path: + sys.path.insert(0, str(_tests_python)) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert(0, str(repo_root() / "examples" / "jmex" / "python")) diff --git a/tests/python/examples/jmex/pyproject.toml b/tests/python/examples/jmex/pyproject.toml new file mode 100644 index 0000000000..6589a42932 --- /dev/null +++ b/tests/python/examples/jmex/pyproject.toml @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "jmex-example-tests" +version = "0.0.0" # Internal tests - not versioned +description = "Tests for the j-mex G1 retargeting example" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = [ + "pytest", + "numpy", + "scipy", +] + +[tool.pytest.ini_options] +pythonpath = ["."] +python_files = ["test_*.py"] +python_functions = ["test_*"] +norecursedirs = [".git", ".venv", "build", "dist", "*.egg", "__pycache__"] diff --git a/tests/python/examples/jmex/test_jmex_g1_action_source.py b/tests/python/examples/jmex/test_jmex_g1_action_source.py new file mode 100644 index 0000000000..8fb4a1d290 --- /dev/null +++ b/tests/python/examples/jmex/test_jmex_g1_action_source.py @@ -0,0 +1,1541 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 j-mex. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The IO contract of the j-mex G1 action source, on synthetic frames.""" + +from __future__ import annotations + +import logging + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from isaaccapture.retargeting_engine.interface import ComputeContext, TensorGroup +from isaaccapture.schema import BodyJoint, BodyJoints, FullBodyPose + +from isaaccapture_examples.jmex.jmex_g1_retargeting import ( + ACTION_OUTPUT, + FULL_BODY_INPUT, + POSE_DIM, + STANDING_LOCOMOTION_COMMAND, + FrameTargetCfg, + G1ActionLayout, + JmexG1ActionSource, + PelvisPose, + arm_frame_targets, + forearm_frame, + left_wrist_chain, + right_wrist_chain, +) + +# Human proportions, meters. Deliberately unlike the robot lengths below: every assertion +# about the reconstructed target has to come out the same whatever these are. +_HUMAN_PELVIS_TO_SHOULDER = 0.55 +_HUMAN_SHOULDER_TO_ELBOW = 0.31 +_HUMAN_ELBOW_TO_WRIST = 0.27 +# Each hip this far to its side of the pelvis and a little below it; each shoulder this far to +# its side of the torso's line, per meter up it. The hips are what the heading is read off. +_HUMAN_HIP_HALF_WIDTH = 0.09 +_HUMAN_HIP_DROP = 0.06 +_HUMAN_SHOULDER_SPREAD = 0.35 +# Each slot's distance up the torso's line, as a fraction of the pelvis-to-shoulder distance. +_HUMAN_SPINE = ( + (BodyJoint.SPINE1, 0.2), + (BodyJoint.SPINE2, 0.45), + (BodyJoint.SPINE3, 0.7), + (BodyJoint.NECK, 0.9), + (BodyJoint.HEAD, 1.2), +) + +# Synthetic robot upper arm and forearm, and where the robot's shoulders sit in its own +# pelvis frame. Not the G1's -- the point is that the test needs no asset. +_ROBOT_SEGMENTS = (0.22, 0.19) +_ROBOT_LEFT_SHOULDER = (0.02, 0.12, 0.27) +_ROBOT_RIGHT_SHOULDER = (0.02, -0.12, 0.27) +# What the robot's upper body turns about, in the same frame: off the pelvis origin, and off the +# line under the shoulders, so a target turned about the wrong point cannot pass. +_ROBOT_WAIST_PIVOT = (-0.01, 0.0, 0.05) +# The robot's forearm frame in its wrist task's axes. Deliberately not the identity, so a +# target that forgets to carry it cannot pass. +_ROBOT_FOREARM_IN_TASK = tuple( + Rotation.from_euler("xyz", [15.0, -25.0, 40.0], degrees=True).as_quat() +) + +# A G1-like rest arm, in its pelvis frame: upper arm down and splayed out, forearm ahead. +_REST_SHOULDER = np.array([0.0, 0.10, 0.29]) +_REST_ELBOW = np.array([0.016, 0.147, 0.105]) +_REST_WRIST = np.array([0.20, 0.149, 0.095]) + +# Every slot above the pelvis that is not a leg: what turns when the operator twists. +_UPPER_BODY = ( + BodyJoint.SPINE1, + BodyJoint.SPINE2, + BodyJoint.SPINE3, + BodyJoint.NECK, + BodyJoint.HEAD, + BodyJoint.LEFT_COLLAR, + BodyJoint.RIGHT_COLLAR, + BodyJoint.LEFT_SHOULDER, + BodyJoint.RIGHT_SHOULDER, + BodyJoint.LEFT_ELBOW, + BodyJoint.RIGHT_ELBOW, + BodyJoint.LEFT_WRIST, + BodyJoint.RIGHT_WRIST, + BodyJoint.LEFT_HAND, + BodyJoint.RIGHT_HAND, +) + +# The operator's arm slots, shoulder first, as the plugin fills them. +_OPERATOR_ARMS = ( + (BodyJoint.LEFT_SHOULDER, BodyJoint.LEFT_ELBOW, BodyJoint.LEFT_WRIST), + (BodyJoint.RIGHT_SHOULDER, BodyJoint.RIGHT_ELBOW, BodyJoint.RIGHT_WRIST), +) + +# How the standard human skeleton's WRIST slot is turned, in the axes it is published in: X +# runs from the elbow out through the wrist, and Y lies along the elbow's hinge -- the +# direction of (upper arm x forearm). Both are negative on the left arm and positive on the +# right, so one sign per arm carries the slot's whole layout. +_WRIST_SLOT_SIGN = {BodyJoint.LEFT_WRIST: -1.0, BodyJoint.RIGHT_WRIST: 1.0} + +# OpenXR body axes, as the plugin publishes them: +X the operator's right, +Y up, +Z behind. +_OPENXR_FORWARD = np.array([0.0, 0.0, -1.0]) +_OPENXR_UP = np.array([0.0, 1.0, 0.0]) +_OPENXR_RIGHT = np.array([1.0, 0.0, 0.0]) + +# The same three, after the change of basis into Isaac's +X forward, +Y left, +Z up. +_ISAAC_FORWARD = np.array([1.0, 0.0, 0.0]) +_ISAAC_UP = np.array([0.0, 0.0, 1.0]) +_ISAAC_RIGHT = np.array([0.0, -1.0, 0.0]) +_ISAAC_LEFT = -_ISAAC_RIGHT + +# The torso's moves, as axes of turns in Isaac's: leaning forward tips up toward forward, bending +# to the right tips it toward the right, twisting to the left turns forward toward the left. +_LEANING_FORWARD = _ISAAC_LEFT +_BENDING_RIGHT = _ISAAC_FORWARD +_TWISTING_LEFT = _ISAAC_UP + + +def _identity_pelvis() -> PelvisPose: + return PelvisPose( + position=np.zeros(3), orientation_xyzw=np.array([0.0, 0.0, 0.0, 1.0]) + ) + + +def _yawed_pelvis(yaw_deg: float, height: float = 0.75) -> PelvisPose: + """The G1's spawn pose shape: a yaw about world Z at a standing height.""" + return PelvisPose( + position=np.array([0.0, 0.0, height]), + orientation_xyzw=Rotation.from_euler("z", yaw_deg, degrees=True).as_quat(), + ) + + +def _robot_shoulders_in_world( + pelvis: PelvisPose, torso: Rotation | None = None +) -> tuple[np.ndarray, np.ndarray]: + """Where the configured shoulders are once the pelvis is placed, in task order. + + With ``torso``, a turn in the pelvis frame, each is first turned that far about the waist. + """ + rotation = Rotation.from_quat(pelvis.orientation_xyzw) + turn = Rotation.identity() if torso is None else torso + pivot = np.asarray(_ROBOT_WAIST_PIVOT) + return tuple( + pelvis.position + + rotation.apply(pivot + turn.apply(np.asarray(shoulder) - pivot)) + for shoulder in (_ROBOT_LEFT_SHOULDER, _ROBOT_RIGHT_SHOULDER) + ) + + +def _torso_turn(axis_isaac: np.ndarray, degrees: float) -> Rotation: + """A turn of ``degrees`` about ``axis_isaac``, in Isaac's axes.""" + return Rotation.from_rotvec(np.radians(degrees) * _unit(axis_isaac)) + + +def _frame_target( + name: str, + chain: tuple[int, ...], + *, + anchor: tuple[float, float, float], + forearm_frame_in_task_xyzw: tuple[float, float, float, float] | None, + segments: tuple[float, ...] | None = None, + neutral_orientation_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), +) -> FrameTargetCfg: + """One task over ``chain``, with the robot's lengths along it unless told otherwise.""" + return FrameTargetCfg( + name=name, + chain=chain, + anchor_position_pelvis=anchor, + robot_segment_lengths=( + _ROBOT_SEGMENTS[: len(chain) - 1] if segments is None else segments + ), + neutral_position_pelvis=(0.2, 0.2, 0.1), + neutral_orientation_pelvis_xyzw=neutral_orientation_xyzw, + forearm_frame_in_task_xyzw=forearm_frame_in_task_xyzw, + ) + + +def _g1_layout( + neutral_position: tuple[float, float, float] = (0.2, 0.2, 0.1), + neutral_orientation_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), +) -> G1ActionLayout: + """The G1 task's frame tasks in its controller's order, every frame resting alike.""" + rest = (neutral_position, neutral_orientation_xyzw) + arms = [ + arm_frame_targets( + chain, + anchor_position_pelvis=anchor, + robot_segment_lengths=_ROBOT_SEGMENTS, + rest_poses_pelvis=[rest] * len(chain), + forearm_frame_in_task_xyzw=_ROBOT_FOREARM_IN_TASK, + ) + for chain, anchor in ( + (left_wrist_chain(), _ROBOT_LEFT_SHOULDER), + (right_wrist_chain(), _ROBOT_RIGHT_SHOULDER), + ) + ] + # Each arm comes shoulder first; the controller lists both wrists, both elbows, then + # both shoulders, so tasks 0 and 1 are the wrists. + return G1ActionLayout( + frame_targets=tuple(arm[depth] for depth in (2, 1, 0) for arm in arms), + waist_pivot_pelvis=_ROBOT_WAIST_PIVOT, + ) + + +def _task_index(layout: G1ActionLayout, name: str) -> int: + return [target.name for target in layout.frame_targets].index(name) + + +def _unit(vector: np.ndarray) -> np.ndarray: + vector = np.asarray(vector, dtype=np.float64) + return vector / np.linalg.norm(vector) + + +def _in_isaac_axes(vector_openxr: np.ndarray) -> np.ndarray: + """An OpenXR direction in Isaac's axes, built from the three pairs declared above.""" + x, y, z = np.asarray(vector_openxr, dtype=np.float64) + return x * _ISAAC_RIGHT + y * _ISAAC_UP - z * _ISAAC_FORWARD + + +def _in_openxr_axes(vector_isaac: np.ndarray) -> np.ndarray: + """An Isaac direction in OpenXR's axes: ``_in_isaac_axes`` the other way round.""" + x, y, z = np.asarray(vector_isaac, dtype=np.float64) + return x * _OPENXR_FORWARD - y * _OPENXR_RIGHT + z * _OPENXR_UP + + +def _in_openxr(turn_isaac: Rotation) -> Rotation: + """A turn given in Isaac's axes, as the same turn in OpenXR's.""" + return Rotation.from_rotvec(_in_openxr_axes(turn_isaac.as_rotvec())) + + +def _swung_bent_arm(swing_deg: float) -> tuple[np.ndarray, np.ndarray]: + """An upper arm and forearm (OpenXR) with the elbow bent, swung ``swing_deg``. + + The swing is about the operator's shoulder-to-wrist line: the hand stays where it was. + """ + upper_arm = _unit(-_OPENXR_UP + 0.4 * _OPENXR_RIGHT) + forearm = _unit(_OPENXR_FORWARD + 0.3 * _OPENXR_UP) + to_wrist = _unit( + _HUMAN_SHOULDER_TO_ELBOW * upper_arm + _HUMAN_ELBOW_TO_WRIST * forearm + ) + swing = Rotation.from_rotvec(np.radians(swing_deg) * to_wrist) + return swing.apply(upper_arm), swing.apply(forearm) + + +def _wrist_orientation( + wrist_slot: BodyJoint, forearm_openxr: np.ndarray, hinge_openxr: np.ndarray +) -> np.ndarray: + """The WRIST slot's published orientation (xyzw) for a forearm and hinge in OpenXR axes.""" + distal = _unit(forearm_openxr) + hinge = _unit(hinge_openxr - (np.dot(hinge_openxr, distal)) * distal) + sign = _WRIST_SLOT_SIGN[wrist_slot] + along_x, along_y = sign * distal, sign * hinge + matrix = np.column_stack([along_x, along_y, np.cross(along_x, along_y)]) + return Rotation.from_matrix(matrix).as_quat() + + +def _straight_arm_frame(direction_openxr: np.ndarray, **kwargs) -> FullBodyPose: + """One frame whose two arms run straight out from the shoulder along ``direction_openxr``.""" + return _arm_frame(direction_openxr, direction_openxr, **kwargs) + + +def _arm_frame( + upper_arm_openxr: np.ndarray, + forearm_openxr: np.ndarray, + *, + human_segments: tuple[float, float, float] = ( + _HUMAN_PELVIS_TO_SHOULDER, + _HUMAN_SHOULDER_TO_ELBOW, + _HUMAN_ELBOW_TO_WRIST, + ), + pelvis_position_openxr: np.ndarray | None = None, + shoulder_direction_openxr: np.ndarray = _OPENXR_UP, + hinge_openxr: np.ndarray | None = None, +) -> FullBodyPose: + """One frame whose two arms run along ``upper_arm_openxr``, then ``forearm_openxr``. + + The operator faces -Z. The spine runs ``shoulder_direction_openxr`` from the pelvis, each + shoulder spread to its side of that line at the pelvis-to-shoulder distance; the hips stay + level either side of the pelvis. Each WRIST is turned as the skeleton turns it for that + forearm, its hinge toward ``hinge_openxr``. + """ + upper = _unit(upper_arm_openxr) + arm = _unit(forearm_openxr) + if hinge_openxr is None: + hinge_openxr = ( + _OPENXR_RIGHT if abs(np.dot(arm, _OPENXR_RIGHT)) < 0.9 else _OPENXR_FORWARD + ) + torso = _unit(shoulder_direction_openxr) + pelvis = ( + np.zeros(3) + if pelvis_position_openxr is None + else np.asarray(pelvis_position_openxr, dtype=np.float64) + ) + to_shoulder, upper_arm, forearm = human_segments + + joints = BodyJoints() + positions = np.zeros((int(BodyJoint.NUM_JOINTS), 3), dtype=np.float32) + orientations = np.zeros((int(BodyJoint.NUM_JOINTS), 4), dtype=np.float32) + orientations[:, 3] = 1.0 + positions[int(BodyJoint.PELVIS)] = pelvis + for slot, side in ((BodyJoint.LEFT_HIP, -1.0), (BodyJoint.RIGHT_HIP, 1.0)): + positions[int(slot)] = ( + pelvis + + side * _HUMAN_HIP_HALF_WIDTH * _OPENXR_RIGHT + - _HUMAN_HIP_DROP * _OPENXR_UP + ) + for slot, fraction in _HUMAN_SPINE: + positions[int(slot)] = pelvis + torso * to_shoulder * fraction + across = _unit(_OPENXR_RIGHT - np.dot(_OPENXR_RIGHT, torso) * torso) + for (shoulder_slot, elbow_slot, wrist_slot), side in zip( + _OPERATOR_ARMS, (-1.0, 1.0) + ): + shoulder = pelvis + to_shoulder * _unit( + torso + side * _HUMAN_SHOULDER_SPREAD * across + ) + elbow = shoulder + upper * upper_arm + positions[int(shoulder_slot)] = shoulder + positions[int(elbow_slot)] = elbow + positions[int(wrist_slot)] = elbow + arm * forearm + orientations[int(wrist_slot)] = _wrist_orientation( + wrist_slot, arm, hinge_openxr + ) + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[:] = 1 + return FullBodyPose(joints, True) + + +def _twist_wrists(frame: FullBodyPose, degrees: float) -> FullBodyPose: + """Turn ``frame``'s two WRIST slots in place about their own forearms, and return it.""" + positions = np.asarray(frame.joints.positions, dtype=np.float64) + for _, elbow, wrist in _OPERATOR_ARMS: + forearm = _unit(positions[int(wrist)] - positions[int(elbow)]) + twisted = Rotation.from_rotvec( + np.radians(degrees) * forearm + ) * Rotation.from_quat(frame.joints.orientations[int(wrist)]) + frame.joints.orientations[int(wrist)] = twisted.as_quat() + return frame + + +def _turned( + frame: FullBodyPose, degrees: float, *, about_openxr: np.ndarray | None = None +) -> FullBodyPose: + """Turn the whole of ``frame``, hips and all, about the vertical through ``about_openxr``. + + Positions go round that line and every slot's orientation turns with them, the way an + operator turning on the spot -- or walking in a circle -- arrives. Returns ``frame``. + """ + turn = Rotation.from_rotvec(np.radians(degrees) * _OPENXR_UP) + about = np.zeros(3) if about_openxr is None else np.asarray(about_openxr) + positions = np.asarray(frame.joints.positions, dtype=np.float64) + frame.joints.positions[:] = about + turn.apply(positions - about) + frame.joints.orientations[:] = ( + turn * Rotation.from_quat(np.asarray(frame.joints.orientations)) + ).as_quat() + return frame + + +def _tilted( + frame: FullBodyPose, + turn_isaac: Rotation, + *, + slots: tuple[BodyJoint, ...] | None = None, +) -> FullBodyPose: + """Turn ``slots`` of ``frame`` -- all of them unless told -- about the operator's pelvis. + + ``turn_isaac`` is given in Isaac's axes; positions and orientations turn with it alike. + Returns ``frame``. + """ + turn = _in_openxr(turn_isaac) + indices = [int(slot) for slot in (slots or range(int(BodyJoint.NUM_JOINTS)))] + positions = np.asarray(frame.joints.positions, dtype=np.float64) + orientations = np.asarray(frame.joints.orientations, dtype=np.float64) + about = positions[int(BodyJoint.PELVIS)].copy() + positions[indices] = about + turn.apply(positions[indices] - about) + orientations[indices] = (turn * Rotation.from_quat(orientations[indices])).as_quat() + frame.joints.positions[:] = positions + frame.joints.orientations[:] = orientations + return frame + + +def _run(source: JmexG1ActionSource, frame: FullBodyPose | None) -> np.ndarray: + group = TensorGroup(source.input_spec()[FULL_BODY_INPUT]) + group[0] = frame + outputs = source({FULL_BODY_INPUT: group}, ComputeContext()) + return np.asarray(outputs[ACTION_OUTPUT][0]) + + +def _target_pose(action: np.ndarray, task_index: int) -> tuple[np.ndarray, np.ndarray]: + start = task_index * POSE_DIM + return action[start : start + 3], action[start + 3 : start + POSE_DIM] + + +def _robot_forearm_axes( + action: np.ndarray, task_index: int, layout: G1ActionLayout, *, wxyz: bool = False +) -> tuple[np.ndarray, np.ndarray]: + """Where the target puts the robot's forearm and elbow hinge, read from the action. + + ``wxyz`` reads the orientation field scalar-first instead, as an Isaac Lab 2.x copy would. + """ + _, quaternion = _target_pose(action, task_index) + if wxyz: + quaternion = np.roll(quaternion, -1) + target = Rotation.from_quat(quaternion) * Rotation.from_quat( + layout.frame_targets[task_index].forearm_frame_in_task_xyzw + ) + matrix = target.as_matrix() + return matrix[:, 0], matrix[:, 1] + + +def test_a_straight_arm_reaches_exactly_the_robots_arm_length_from_its_shoulder(): + """Per-segment reconstruction: the operator's angles, the robot's lengths.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + + for task_index, shoulder in enumerate(_robot_shoulders_in_world(pelvis)): + position, _ = _target_pose(action, task_index) + reach = np.linalg.norm(position - shoulder) + assert reach == pytest.approx(sum(_ROBOT_SEGMENTS), abs=1e-5) + + +def test_the_arm_hangs_from_the_robots_own_shoulder_never_from_the_operators(): + """The torso lies straight ahead, so the operator's shoulder is nowhere near the robot's.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run( + source, + _straight_arm_frame(_OPENXR_FORWARD, shoulder_direction_openxr=_OPENXR_FORWARD), + ) + + lying_forward = _torso_turn(_LEANING_FORWARD, 90.0) + for side, turned in zip( + ("left", "right"), _robot_shoulders_in_world(pelvis, lying_forward) + ): + shoulder, _ = _target_pose(action, _task_index(layout, f"{side}_shoulder")) + wrist, _ = _target_pose(action, _task_index(layout, f"{side}_wrist")) + np.testing.assert_allclose(shoulder, turned, atol=1e-5) + assert np.linalg.norm(wrist - shoulder) <= sum(_ROBOT_SEGMENTS) + 1e-6 + + +@pytest.mark.parametrize("swing_deg", [0.0, 90.0, -60.0]) +def test_the_elbow_target_follows_the_operators_upper_arm_through_the_swing(swing_deg): + """With the hand held still the operator's elbow can still go round; the target follows it.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + upper_arm, forearm = _swung_bent_arm(swing_deg) + + action = _run(source, _arm_frame(upper_arm, forearm)) + + turn = Rotation.from_quat(pelvis.orientation_xyzw) + for side, shoulder in zip(("left", "right"), _robot_shoulders_in_world(pelvis)): + elbow, _ = _target_pose(action, _task_index(layout, f"{side}_elbow")) + np.testing.assert_allclose( + elbow, + shoulder + _ROBOT_SEGMENTS[0] * turn.apply(_in_isaac_axes(upper_arm)), + atol=1e-5, + ) + + +def test_the_wrist_target_is_one_robot_forearm_on_from_the_elbow_target(): + """Both are points of one rebuilt arm, so the IK is never handed a pair it cannot reach.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + upper_arm, forearm = _swung_bent_arm(30.0) + + action = _run(source, _arm_frame(upper_arm, forearm)) + + turn = Rotation.from_quat(pelvis.orientation_xyzw) + for side in ("left", "right"): + elbow, _ = _target_pose(action, _task_index(layout, f"{side}_elbow")) + wrist, _ = _target_pose(action, _task_index(layout, f"{side}_wrist")) + np.testing.assert_allclose( + wrist - elbow, + _ROBOT_SEGMENTS[1] * turn.apply(_in_isaac_axes(forearm)), + atol=1e-5, + ) + + +def test_standing_in_the_reference_pose_hangs_each_arm_from_where_the_robots_shoulder_rests(): + """Upright and facing ahead is a torso pose of zero.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + upper_arm, forearm = _swung_bent_arm(30.0) + + action = _run(source, _arm_frame(upper_arm, forearm)) + + _assert_arms_hang_from_shoulders_turned_by( + action, layout, pelvis, Rotation.identity(), upper_arm, forearm + ) + for side, rest in zip(("left", "right"), _robot_shoulders_in_world(pelvis)): + shoulder, _ = _target_pose(action, _task_index(layout, f"{side}_shoulder")) + np.testing.assert_allclose(shoulder, rest, atol=1e-6) + + +def _assert_arms_hang_from_shoulders_turned_by( + action: np.ndarray, + layout: G1ActionLayout, + pelvis: PelvisPose, + torso: Rotation, + upper_arm_openxr: np.ndarray, + forearm_openxr: np.ndarray, +) -> None: + """Each shoulder target is the robot's, turned ``torso`` about the waist; its elbow and + wrist are the robot's arm hung from there along the operator's own segments.""" + turn = Rotation.from_quat(pelvis.orientation_xyzw) + for side, turned in zip( + ("left", "right"), _robot_shoulders_in_world(pelvis, torso) + ): + shoulder, _ = _target_pose(action, _task_index(layout, f"{side}_shoulder")) + elbow, _ = _target_pose(action, _task_index(layout, f"{side}_elbow")) + wrist, _ = _target_pose(action, _task_index(layout, f"{side}_wrist")) + np.testing.assert_allclose(shoulder, turned, atol=1e-5) + np.testing.assert_allclose( + elbow, + shoulder + + _ROBOT_SEGMENTS[0] * turn.apply(_in_isaac_axes(_unit(upper_arm_openxr))), + atol=1e-5, + ) + np.testing.assert_allclose( + wrist, + elbow + + _ROBOT_SEGMENTS[1] * turn.apply(_in_isaac_axes(_unit(forearm_openxr))), + atol=1e-5, + ) + + +@pytest.mark.parametrize( + ("axis", "degrees"), + [ + (_LEANING_FORWARD, 25.0), + (_LEANING_FORWARD, -15.0), + (_BENDING_RIGHT, 20.0), + (_BENDING_RIGHT, -20.0), + ], + ids=["leaning-forward", "leaning-back", "bending-right", "bending-left"], +) +def test_leaning_turns_the_shoulders_about_the_waist_and_the_arms_hang_from_them( + axis, degrees +): + """The whole operator tips, pelvis and all, the way a bend from the hips does.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + torso = _torso_turn(axis, degrees) + upper_arm, forearm = _swung_bent_arm(30.0) + + action = _run(source, _tilted(_arm_frame(upper_arm, forearm), torso)) + + tipped = _in_openxr(torso) + _assert_arms_hang_from_shoulders_turned_by( + action, layout, pelvis, torso, tipped.apply(upper_arm), tipped.apply(forearm) + ) + + +@pytest.mark.parametrize( + ("axis", "degrees"), + [(_LEANING_FORWARD, 30.0), (_BENDING_RIGHT, -25.0)], + ids=["leaning-forward", "bending-left"], +) +def test_a_lean_from_the_spine_alone_counts_the_same_as_one_from_the_hips( + axis, degrees +): + """Leaning is measured from upright, not from the operator's pelvis.""" + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + torso = _torso_turn(axis, degrees) + tipped = _in_openxr(torso) + upper_arm, forearm = _swung_bent_arm(30.0) + + from_the_hips = _run(source, _tilted(_arm_frame(upper_arm, forearm), torso)) + from_the_spine = _run( + source, + _arm_frame( + tipped.apply(upper_arm), + tipped.apply(forearm), + shoulder_direction_openxr=tipped.apply(_OPENXR_UP), + ), + ) + + for task_index in range(len(layout.frame_targets)): + np.testing.assert_allclose( + _target_pose(from_the_spine, task_index)[0], + _target_pose(from_the_hips, task_index)[0], + atol=1e-5, + ) + + +@pytest.mark.parametrize("degrees", [35.0, -50.0]) +def test_twisting_turns_the_shoulders_about_the_vertical(degrees): + """The hips stay; the shoulders and everything above them turn.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + torso = _torso_turn(_TWISTING_LEFT, degrees) + upper_arm, forearm = _swung_bent_arm(30.0) + + action = _run( + source, _tilted(_arm_frame(upper_arm, forearm), torso, slots=_UPPER_BODY) + ) + + turned = _in_openxr(torso) + _assert_arms_hang_from_shoulders_turned_by( + action, layout, pelvis, torso, turned.apply(upper_arm), turned.apply(forearm) + ) + + +@pytest.mark.parametrize("degrees", [45.0, 80.0]) +def test_a_lean_past_the_waists_range_is_passed_on_whole(degrees): + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + torso = _torso_turn(_LEANING_FORWARD, degrees) + + action = _run(source, _tilted(_straight_arm_frame(-_OPENXR_UP), torso)) + + pivot = pelvis.place(np.asarray(_ROBOT_WAIST_PIVOT)) + axis = pelvis.rotation.apply(_LEANING_FORWARD) + + def square_to_the_axis(vector: np.ndarray) -> np.ndarray: + return _unit(vector - np.dot(vector, axis) * axis) + + for side, rest in zip(("left", "right"), _robot_shoulders_in_world(pelvis)): + shoulder, _ = _target_pose(action, _task_index(layout, f"{side}_shoulder")) + swept = np.arccos( + np.dot( + square_to_the_axis(shoulder - pivot), square_to_the_axis(rest - pivot) + ) + ) + assert np.degrees(swept) == pytest.approx(degrees, abs=1e-3) + hanging = _in_openxr(torso).apply(-_OPENXR_UP) + _assert_arms_hang_from_shoulders_turned_by( + action, layout, pelvis, torso, hanging, hanging + ) + + +def test_a_lean_after_a_quarter_turn_is_still_a_lean(): + """Leaning is read against the operator's own heading, so it never becomes a side bend.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + torso = _torso_turn(_LEANING_FORWARD, 25.0) + upper_arm, forearm = _swung_bent_arm(30.0) + + facing = _run(source, _tilted(_arm_frame(upper_arm, forearm), torso)) + turned = _run(source, _turned(_tilted(_arm_frame(upper_arm, forearm), torso), 90.0)) + + np.testing.assert_allclose(turned, facing, atol=1e-6) + tipped = _in_openxr(torso) + _assert_arms_hang_from_shoulders_turned_by( + turned, layout, pelvis, torso, tipped.apply(upper_arm), tipped.apply(forearm) + ) + + +def test_reach_does_not_move_when_the_operator_gets_longer_arms(): + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + short = _run( + source, _straight_arm_frame(_OPENXR_FORWARD, human_segments=(0.4, 0.2, 0.2)) + ) + tall = _run( + source, _straight_arm_frame(_OPENXR_FORWARD, human_segments=(0.8, 0.5, 0.45)) + ) + + np.testing.assert_allclose(short, tall, atol=1e-6) + + +def test_an_operator_standing_away_from_the_world_origin_retargets_to_the_same_target(): + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + centred = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + displaced = _run( + source, + _straight_arm_frame( + _OPENXR_FORWARD, pelvis_position_openxr=np.array([1.5, 0.0, -2.0]) + ), + ) + + np.testing.assert_allclose(centred, displaced, atol=1e-6) + + +def _swung_arms_twisted_hands() -> FullBodyPose: + return _twist_wrists(_arm_frame(*_swung_bent_arm(50.0)), 35.0) + + +def _reaching_across_with_a_tilted_hinge() -> FullBodyPose: + return _straight_arm_frame( + _OPENXR_FORWARD - 0.6 * _OPENXR_UP + 0.3 * _OPENXR_RIGHT, + hinge_openxr=_OPENXR_RIGHT + 0.4 * _OPENXR_UP, + ) + + +@pytest.mark.parametrize( + "pose", [_swung_arms_twisted_hands, _reaching_across_with_a_tilted_hinge] +) +@pytest.mark.parametrize("degrees", [37.0, 90.0, 180.0, -120.0]) +@pytest.mark.parametrize( + "about_openxr", + [None, np.array([1.5, 0.0, -2.0])], + ids=["on-the-spot", "round-a-point-across-the-room"], +) +def test_turning_moves_no_target(pose, degrees, about_openxr): + """Turning round a point across the room also covers walking in a circle.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + facing = _run(source, pose()) + turned = _run(source, _turned(pose(), degrees, about_openxr=about_openxr)) + + np.testing.assert_allclose(turned, facing, atol=1e-6) + + +@pytest.mark.parametrize( + "hips", + [ + {BodyJoint.LEFT_HIP: [0.0, 0.1, 0.0], BodyJoint.RIGHT_HIP: [0.0, -0.1, 0.0]}, + {BodyJoint.LEFT_HIP: [0.02, 0.0, 0.0], BodyJoint.RIGHT_HIP: [0.02, 0.0, 0.0]}, + ], + ids=["one-above-the-other", "one-point"], +) +def test_a_frame_whose_hips_give_no_heading_holds_every_target(hips): + """No level line between the hips, no heading: nothing moves, orientations included.""" + seen = [] + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + + tracked = _run(source, _straight_arm_frame(_OPENXR_UP)) + frame = _swung_arms_twisted_hands() + for slot, position in hips.items(): + frame.joints.positions[int(slot)] = position + held = _run(source, frame) + + np.testing.assert_allclose(held, tracked, atol=1e-6) + # Nor is there a way to face the skeleton on the robot. + assert len(seen[-1].skeleton) == 0 + + +def _spine_folded(frame: FullBodyPose) -> None: + frame.joints.positions[int(BodyJoint.SPINE3)] = frame.joints.positions[ + int(BodyJoint.SPINE1) + ] + + +def _shoulders_one_point(frame: FullBodyPose) -> None: + frame.joints.positions[int(BodyJoint.RIGHT_SHOULDER)] = frame.joints.positions[ + int(BodyJoint.LEFT_SHOULDER) + ] + + +def _shoulders_on_the_spines_line(frame: FullBodyPose) -> None: + pelvis = np.asarray(frame.joints.positions[int(BodyJoint.PELVIS)]) + frame.joints.positions[int(BodyJoint.LEFT_SHOULDER)] = pelvis + 0.6 * _OPENXR_UP + frame.joints.positions[int(BodyJoint.RIGHT_SHOULDER)] = pelvis + 0.5 * _OPENXR_UP + + +@pytest.mark.parametrize( + "spoil", [_spine_folded, _shoulders_one_point, _shoulders_on_the_spines_line] +) +def test_a_frame_whose_upper_body_gives_no_torso_pose_holds_every_target(spoil): + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + tracked = _run(source, _straight_arm_frame(_OPENXR_UP)) + frame = _swung_arms_twisted_hands() + spoil(frame) + held = _run(source, frame) + + np.testing.assert_allclose(held, tracked, atol=1e-6) + + +@pytest.mark.parametrize( + ("direction_openxr", "expected_isaac"), + [ + (_OPENXR_FORWARD, _ISAAC_FORWARD), + (_OPENXR_UP, _ISAAC_UP), + (_OPENXR_RIGHT, _ISAAC_RIGHT), + ], +) +def test_arm_direction_is_preserved_through_the_change_of_basis( + direction_openxr, expected_isaac +): + layout = _g1_layout() + pelvis = _identity_pelvis() + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run(source, _straight_arm_frame(direction_openxr)) + + for task_index, shoulder in enumerate(_robot_shoulders_in_world(pelvis)): + position, _ = _target_pose(action, task_index) + np.testing.assert_allclose( + _unit(position - shoulder), expected_isaac, atol=1e-5 + ) + + +def test_an_arm_hanging_at_the_side_points_the_robots_forearm_down(): + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run(source, _straight_arm_frame(-_OPENXR_UP)) + + for task_index in range(2): + forearm, _ = _robot_forearm_axes(action, task_index, layout) + np.testing.assert_allclose(forearm, _ISAAC_UP * -1.0, atol=1e-5) + + +def test_the_robots_forearm_and_hinge_turn_where_the_operators_do_and_the_field_is_xyzw(): + """Asymmetric on every axis, on a robot turned 90 degrees, so neither the pelvis nor a + scalar-first reading of the field can pass by accident.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + reaching = _unit(_OPENXR_FORWARD - 0.6 * _OPENXR_UP + 0.3 * _OPENXR_RIGHT) + hinge = _OPENXR_RIGHT + 0.4 * _OPENXR_UP + + action = _run(source, _straight_arm_frame(reaching, hinge_openxr=hinge)) + + turn = Rotation.from_quat(pelvis.orientation_xyzw) + hinge_across = _unit(hinge - np.dot(hinge, reaching) * reaching) + for task_index in range(2): + _, quaternion = _target_pose(action, task_index) + assert np.linalg.norm(quaternion) == pytest.approx(1.0, abs=1e-6) + forearm, robot_hinge = _robot_forearm_axes(action, task_index, layout) + np.testing.assert_allclose( + forearm, turn.apply(_in_isaac_axes(reaching)), atol=1e-5 + ) + np.testing.assert_allclose( + robot_hinge, turn.apply(_in_isaac_axes(hinge_across)), atol=1e-5 + ) + misread, _ = _robot_forearm_axes(action, task_index, layout, wxyz=True) + assert not np.allclose(misread, forearm, atol=1e-2) + + +def test_turning_the_forearm_turns_the_target_about_it_and_nothing_else(): + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + reaching = _OPENXR_FORWARD - _OPENXR_UP + + before = _run(source, _straight_arm_frame(reaching)) + after = _run(source, _twist_wrists(_straight_arm_frame(reaching), 40.0)) + + for task_index in range(2): + np.testing.assert_allclose( + _target_pose(after, task_index)[0], + _target_pose(before, task_index)[0], + atol=1e-6, + ) + axis = Rotation.from_quat(pelvis.orientation_xyzw).apply( + _in_isaac_axes(_unit(reaching)) + ) + for task_index in range(2): + turned = Rotation.from_quat(_target_pose(after, task_index)[1]) * ( + Rotation.from_quat(_target_pose(before, task_index)[1]).inv() + ) + np.testing.assert_allclose( + turned.as_rotvec(), np.radians(40.0) * axis, atol=1e-4 + ) + np.testing.assert_allclose( + _robot_forearm_axes(after, task_index, layout)[0], + _robot_forearm_axes(before, task_index, layout)[0], + atol=1e-5, + ) + + +def test_bending_the_wrist_bends_the_target_off_the_forearms_line(): + """WRIST is read whole: a hand bent across the forearm is followed, not straightened.""" + layout = _g1_layout() + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + reaching = _OPENXR_FORWARD - _OPENXR_UP + bend = Rotation.from_rotvec(np.radians(20.0) * _OPENXR_RIGHT) + frame = _straight_arm_frame(reaching) + for _, _, wrist in _OPERATOR_ARMS: + frame.joints.orientations[int(wrist)] = ( + bend * Rotation.from_quat(frame.joints.orientations[int(wrist)]) + ).as_quat() + + action = _run(source, frame) + + hand = Rotation.from_quat(pelvis.orientation_xyzw).apply( + _in_isaac_axes(bend.apply(_unit(reaching))) + ) + for task_index in range(2): + forearm, _ = _robot_forearm_axes(action, task_index, layout) + np.testing.assert_allclose(forearm, hand, atol=1e-5) + + +def test_the_hand_slots_orientation_does_not_reach_the_target(): + """HAND is rigid on WRIST in the skeleton; whatever it says, WRIST already said.""" + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + plain = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + frame = _straight_arm_frame(_OPENXR_FORWARD) + for slot in (BodyJoint.LEFT_HAND, BodyJoint.RIGHT_HAND): + frame.joints.orientations[int(slot)] = Rotation.from_euler( + "xyz", [70.0, -40.0, 25.0], degrees=True + ).as_quat() + turned_hands = _run(source, frame) + + np.testing.assert_allclose(turned_hands, plain, atol=1e-6) + + +def test_elbow_and_shoulder_targets_keep_the_robots_rest_orientation_whatever_the_hand_does(): + rest = Rotation.from_euler("xyz", [10.0, -20.0, 30.0], degrees=True) + layout = _g1_layout(neutral_orientation_xyzw=tuple(rest.as_quat())) + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run( + source, _twist_wrists(_straight_arm_frame(_OPENXR_FORWARD - _OPENXR_UP), 40.0) + ) + + expected = Rotation.from_quat(pelvis.orientation_xyzw) * rest + for name in ("left_elbow", "right_elbow", "left_shoulder", "right_shoulder"): + _, quaternion = _target_pose(action, _task_index(layout, name)) + assert (Rotation.from_quat(quaternion) * expected.inv()).magnitude() < 1e-6 + + +def test_every_orientation_field_is_a_unit_quaternion_even_where_the_ik_ignores_it(): + """The action term divides every field by its squared norm, position-only tasks' too.""" + layout = _g1_layout(neutral_orientation_xyzw=(0.0, 0.0, 3.0, 3.0)) + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + for frame in (None, _straight_arm_frame(_OPENXR_FORWARD)): + action = _run(source, frame) + for task_index in range(len(layout.frame_targets)): + _, quaternion = _target_pose(action, task_index) + assert np.linalg.norm(quaternion) == pytest.approx(1.0, abs=1e-6) + + +@pytest.mark.parametrize( + "field", ["neutral_orientation_xyzw", "forearm_frame_in_task_xyzw"] +) +def test_a_zero_orientation_is_refused_at_construction(field): + orientations = {"forearm_frame_in_task_xyzw": _ROBOT_FOREARM_IN_TASK} + orientations[field] = (0.0, 0.0, 0.0, 0.0) + with pytest.raises(ValueError, match="non-zero quaternion"): + _frame_target( + "left_wrist", + left_wrist_chain(), + anchor=_ROBOT_LEFT_SHOULDER, + **orientations, + ) + + +@pytest.mark.parametrize( + "elbow_differs", + [{"anchor": (0.02, 0.15, 0.27)}, {"segments": (0.25,)}], + ids=["another-shoulder", "another-upper-arm"], +) +def test_an_elbow_that_disagrees_with_its_wrist_about_the_arm_is_refused(elbow_differs): + """Cut from two different arms, the two targets need not be reachable together.""" + wrist = _frame_target( + "left_wrist", + left_wrist_chain(), + anchor=_ROBOT_LEFT_SHOULDER, + forearm_frame_in_task_xyzw=_ROBOT_FOREARM_IN_TASK, + ) + elbow = _frame_target( + "left_elbow", + left_wrist_chain()[:2], + forearm_frame_in_task_xyzw=None, + **{"anchor": _ROBOT_LEFT_SHOULDER, **elbow_differs}, + ) + + with pytest.raises(ValueError, match="left_wrist and left_elbow"): + G1ActionLayout( + frame_targets=(wrist, elbow), waist_pivot_pelvis=_ROBOT_WAIST_PIVOT + ) + + +@pytest.mark.parametrize( + ("chain", "forearm_frame_in_task_xyzw"), + [ + (left_wrist_chain(), None), + (left_wrist_chain()[:2], _ROBOT_FOREARM_IN_TASK), + ], + ids=["wrist-without", "elbow-with"], +) +def test_a_forearm_frame_belongs_to_a_chain_that_ends_at_a_wrist_and_no_other( + chain, forearm_frame_in_task_xyzw +): + with pytest.raises(ValueError, match="forearm frame"): + _frame_target( + "arm", + chain, + anchor=_ROBOT_LEFT_SHOULDER, + forearm_frame_in_task_xyzw=forearm_frame_in_task_xyzw, + ) + + +@pytest.mark.parametrize( + ("task_count", "action_length"), + [(2, 32), (4, 46), (6, 60)], + ids=["wrists", "and-elbows", "and-shoulders"], +) +def test_the_action_grows_by_seven_with_each_frame_task(task_count, action_length): + """Then 14 fingers and 4 locomotion: the G1's 32 and 60 are what that comes to.""" + layout = G1ActionLayout( + frame_targets=_g1_layout().frame_targets[:task_count], + waist_pivot_pelvis=_ROBOT_WAIST_PIVOT, + ) + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + + assert action.shape == (action_length,) + + +def test_a_waist_pivot_is_a_point(): + with pytest.raises(ValueError, match="waist pivot"): + G1ActionLayout( + frame_targets=_g1_layout().frame_targets, waist_pivot_pelvis=(0.0, 0.0) + ) + + +def test_an_arm_needs_a_rest_pose_for_every_slot(): + with pytest.raises(ValueError, match="rest poses"): + arm_frame_targets( + left_wrist_chain(), + anchor_position_pelvis=_ROBOT_LEFT_SHOULDER, + robot_segment_lengths=_ROBOT_SEGMENTS, + rest_poses_pelvis=[((0.2, 0.2, 0.1), (0.0, 0.0, 0.0, 1.0))] * 2, + forearm_frame_in_task_xyzw=_ROBOT_FOREARM_IN_TASK, + ) + + +def test_a_layout_without_fingers_leaves_them_out_of_the_action(): + layout = G1ActionLayout( + frame_targets=_g1_layout().frame_targets[:2], + waist_pivot_pelvis=_ROBOT_WAIST_PIVOT, + num_hand_joints=0, + ) + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + + assert action.shape == (18,) + + +def test_lower_body_is_the_agreed_standing_command(): + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + + np.testing.assert_allclose( + action[-layout.num_locomotion :], STANDING_LOCOMOTION_COMMAND, atol=0.0 + ) + assert STANDING_LOCOMOTION_COMMAND[:3] == (0.0, 0.0, 0.0) + assert STANDING_LOCOMOTION_COMMAND[3] > 0.0 + + +def test_fingers_are_zero_and_the_action_is_finite(): + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + + assert action.dtype == np.float32 + assert np.all(np.isfinite(action)) + finger_start = len(layout.frame_targets) * POSE_DIM + np.testing.assert_array_equal( + action[finger_start : finger_start + layout.num_hand_joints], 0.0 + ) + + +def test_before_pairing_the_targets_are_the_robots_own_neutral_pose(): + """No frame has arrived yet: hold where the robot already is, not the world origin.""" + neutral = Rotation.from_euler("xyz", [20.0, -35.0, 50.0], degrees=True) + layout = _g1_layout( + neutral_position=(0.21, 0.19, 0.11), + neutral_orientation_xyzw=tuple(neutral.as_quat()), + ) + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + action = _run(source, None) + + turn = Rotation.from_quat(pelvis.orientation_xyzw) + expected = pelvis.position + turn.apply((0.21, 0.19, 0.11)) + for task_index in range(len(layout.frame_targets)): + position, quaternion = _target_pose(action, task_index) + np.testing.assert_allclose(position, expected, atol=1e-5) + assert ( + Rotation.from_quat(quaternion) * (turn * neutral).inv() + ).magnitude() < 1e-5 + + +def _told(caplog, level: int = logging.NOTSET) -> list[str]: + """Messages at ``level`` or above under ``isaaccapture.``, the tree the console handler is on.""" + return [ + record.getMessage() + for record in caplog.records + if record.name.startswith("isaaccapture.") and record.levelno >= level + ] + + +def test_with_the_logging_system_off_the_info_lines_still_print(monkeypatch, capsys): + """``ISAACCAPTURE_LOGGING=off`` attaches no handler, so an info record shows nowhere.""" + monkeypatch.setenv("ISAACCAPTURE_LOGGING", "off") + source = JmexG1ActionSource( + layout=_g1_layout(), pelvis_pose_provider=_identity_pelvis + ) + + _run(source, _straight_arm_frame(_OPENXR_UP)) + + printed = capsys.readouterr().out + assert "paired" in printed + assert "measured operator" in printed + + +def test_the_operator_is_told_the_plugin_has_not_paired(caplog): + source = JmexG1ActionSource( + layout=_g1_layout(), pelvis_pose_provider=_identity_pelvis + ) + + _run(source, None) + + warnings = "\n".join(_told(caplog, logging.WARNING)) + assert "not paired" in warnings + assert source.settings.collection_id in warnings + + +@pytest.mark.parametrize( + "tasks", [slice(0, 2), slice(4, 6)], ids=["wrists", "shoulders-alone"] +) +def test_a_lost_pairing_keeps_saying_the_last_target_is_held(caplog, tasks): + """The repeat 300 frames later still describes what the robot is doing, not rest. + + Shoulder chains are one point whether rebuilt or at rest: chain length cannot tell which. + """ + source = JmexG1ActionSource( + layout=G1ActionLayout( + frame_targets=_g1_layout().frame_targets[tasks], + waist_pivot_pelvis=_ROBOT_WAIST_PIVOT, + ), + pelvis_pose_provider=_identity_pelvis, + ) + + _run(source, _straight_arm_frame(_OPENXR_UP)) + for _ in range(301): + _run(source, None) + + reports = [line for line in _told(caplog, logging.WARNING) if "not paired" in line] + assert len(reports) == 2 + assert all("last target" in line for line in reports) + + +def test_operator_segment_lengths_are_measured_from_the_first_frame(): + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + assert source.operator_segment_lengths is None + + _run( + source, _straight_arm_frame(_OPENXR_FORWARD, human_segments=(0.61, 0.33, 0.26)) + ) + + measured = source.operator_segment_lengths + assert measured is not None + # Upper arm and forearm: the chain starts at the shoulder. + np.testing.assert_allclose(measured["left_wrist"], (0.33, 0.26), atol=1e-4) + # And they are not what drove the output. + assert not np.allclose(measured["left_wrist"], _ROBOT_SEGMENTS) + + +def test_losing_the_pairing_holds_the_last_target_rather_than_dropping_the_arms(): + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + tracked = _run(source, _straight_arm_frame(_OPENXR_UP)) + unpaired = _run(source, None) + + np.testing.assert_allclose(unpaired, tracked, atol=1e-6) + + +def test_the_measurement_is_taken_again_after_a_re_pairing(): + """Measured per pairing, not once per process -- Player may come back with another skeleton.""" + source = JmexG1ActionSource( + layout=_g1_layout(), pelvis_pose_provider=_identity_pelvis + ) + + _run( + source, _straight_arm_frame(_OPENXR_FORWARD, human_segments=(0.61, 0.33, 0.26)) + ) + _run(source, None) + assert source.operator_segment_lengths is None + + _run( + source, _straight_arm_frame(_OPENXR_FORWARD, human_segments=(0.50, 0.28, 0.22)) + ) + np.testing.assert_allclose( + source.operator_segment_lengths["left_wrist"], (0.28, 0.22), atol=1e-4 + ) + + +def test_a_frame_missing_any_slot_holds_every_target(): + """A missing foot holds both arms, orientation and all, though their own slots arrived.""" + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + + tracked = _run(source, _straight_arm_frame(_OPENXR_UP)) + held = _run(source, _twist_wrists(_model_without(BodyJoint.LEFT_FOOT), 60.0)) + + np.testing.assert_allclose(held, tracked, atol=1e-6) + + +def test_a_wrist_that_cannot_be_rebuilt_holds_its_elbow_too(): + """Every slot valid, the upper arms moved, the forearms folded to nothing.""" + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, pelvis_pose_provider=lambda: _yawed_pelvis(90.0) + ) + tracked = _run(source, _arm_frame(*_swung_bent_arm(0.0))) + + folded = _arm_frame(*_swung_bent_arm(90.0)) + for _, elbow, wrist in _OPERATOR_ARMS: + folded.joints.positions[int(wrist)] = folded.joints.positions[int(elbow)] + held = _run(source, folded) + + np.testing.assert_allclose(held, tracked, atol=1e-6) + + +def _model_without(*slots: BodyJoint) -> FullBodyPose: + """A frame from a model that has no joint for ``slots`` -- a robot URDF, say.""" + frame = _straight_arm_frame(_OPENXR_FORWARD) + for slot in slots: + frame.joints.is_valid[int(slot)] = 0 + return frame + + +# The torso slots a G1 URDF loaded in Player leaves empty; its arm slots come through. +_ROBOT_URDF_GAPS = ( + BodyJoint.PELVIS, + BodyJoint.SPINE1, + BodyJoint.SPINE2, + BodyJoint.SPINE3, + BodyJoint.NECK, + BodyJoint.HEAD, +) + + +def test_a_model_that_never_fills_every_slot_is_named_on_the_console(caplog): + source = JmexG1ActionSource( + layout=_g1_layout(), pelvis_pose_provider=_identity_pelvis + ) + caplog.clear() + + for _ in range(60): + _run(source, _model_without(*_ROBOT_URDF_GAPS)) + + (warning,) = [line for line in _told(caplog, logging.WARNING) if "skeleton" in line] + assert "standard human skeleton" in warning + for slot in _ROBOT_URDF_GAPS: + assert slot.name in warning + # The slots that did arrive are not on the list. + assert BodyJoint.LEFT_WRIST.name not in warning + + +def test_the_wrong_model_leaves_the_robot_at_rest_even_with_its_arms_complete(): + layout = _g1_layout(neutral_position=(0.21, 0.19, 0.11)) + pelvis = _identity_pelvis() + source = JmexG1ActionSource(layout=layout, pelvis_pose_provider=lambda: pelvis) + + for _ in range(60): + action = _run(source, _model_without(*_ROBOT_URDF_GAPS)) + + for task_index in range(len(layout.frame_targets)): + position, quaternion = _target_pose(action, task_index) + np.testing.assert_allclose(position, (0.21, 0.19, 0.11), atol=1e-6) + np.testing.assert_allclose(quaternion, (0.0, 0.0, 0.0, 1.0), atol=1e-6) + assert source.operator_segment_lengths is None + + +def test_a_single_bad_frame_is_not_reported_as_the_wrong_model(caplog): + source = JmexG1ActionSource( + layout=_g1_layout(), pelvis_pose_provider=_identity_pelvis + ) + + _run(source, _straight_arm_frame(_OPENXR_UP)) + _run(source, _model_without(BodyJoint.LEFT_ELBOW)) + for _ in range(60): + _run(source, _straight_arm_frame(_OPENXR_UP)) + + assert not [line for line in _told(caplog) if "standard human skeleton" in line] + + +def test_the_overlay_draws_the_rebuilt_arm_from_the_robots_shoulder_to_the_ik_target(): + seen = [] + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: pelvis, + overlay_sink=seen.append, + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD - _OPENXR_UP)) + + (overlay,) = seen + shoulders = _robot_shoulders_in_world(pelvis) + for task_index, name in enumerate(("left_wrist", "right_wrist")): + shoulder, elbow, wrist = overlay.chains[name] + np.testing.assert_allclose(shoulder, shoulders[task_index], atol=1e-6) + assert np.linalg.norm(elbow - shoulder) == pytest.approx(_ROBOT_SEGMENTS[0]) + np.testing.assert_allclose( + wrist, _target_pose(action, task_index)[0], atol=1e-5 + ) + + +def test_the_overlay_ends_every_tasks_chain_on_what_the_ik_is_told(): + """Wrist, elbow and shoulder alike, so each large dot drawn is an IK target.""" + seen = [] + layout = _g1_layout() + source = JmexG1ActionSource( + layout=layout, + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + + action = _run(source, _arm_frame(*_swung_bent_arm(30.0))) + + chains = seen[-1].chains + for task_index, target in enumerate(layout.frame_targets): + np.testing.assert_allclose( + chains[target.name][-1], _target_pose(action, task_index)[0], atol=1e-5 + ) + + +def test_the_overlay_draws_the_received_skeleton_unscaled_on_the_robots_pelvis(): + seen = [] + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: pelvis, + overlay_sink=seen.append, + ) + + _run( + source, + _straight_arm_frame( + _OPENXR_FORWARD, pelvis_position_openxr=np.array([1.5, 0.0, -2.0]) + ), + ) + + skeleton = seen[-1].skeleton + operator_pelvis = skeleton[int(BodyJoint.PELVIS)] + np.testing.assert_allclose(operator_pelvis, pelvis.position, atol=1e-5) + for shoulder, elbow, wrist in _OPERATOR_ARMS: + shoulder, elbow, wrist = ( + skeleton[int(slot)] for slot in (shoulder, elbow, wrist) + ) + assert np.linalg.norm(shoulder - operator_pelvis) == pytest.approx( + _HUMAN_PELVIS_TO_SHOULDER, abs=1e-5 + ) + assert np.linalg.norm(elbow - shoulder) == pytest.approx( + _HUMAN_SHOULDER_TO_ELBOW, abs=1e-5 + ) + assert np.linalg.norm(wrist - elbow) == pytest.approx( + _HUMAN_ELBOW_TO_WRIST, abs=1e-5 + ) + + +@pytest.mark.parametrize("degrees", [0.0, 90.0, -150.0]) +def test_the_overlay_draws_the_operator_facing_the_way_the_robot_does(degrees): + """However the operator is turned, their left hip lands on the robot's left side.""" + seen = [] + pelvis = _yawed_pelvis(90.0) + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: pelvis, + overlay_sink=seen.append, + ) + + _run(source, _swung_arms_twisted_hands()) + _run(source, _turned(_swung_arms_twisted_hands(), degrees)) + + facing, turned = seen + assert facing.skeleton.keys() == turned.skeleton.keys() + for slot, point in facing.skeleton.items(): + np.testing.assert_allclose(turned.skeleton[slot], point, atol=1e-5) + robots_left = Rotation.from_quat(pelvis.orientation_xyzw).apply(_ISAAC_RIGHT * -1.0) + across = ( + turned.skeleton[int(BodyJoint.LEFT_HIP)] + - turned.skeleton[int(BodyJoint.RIGHT_HIP)] + ) + np.testing.assert_allclose( + across, 2.0 * _HUMAN_HIP_HALF_WIDTH * robots_left, atol=1e-5 + ) + + +def test_each_rebuilt_segment_points_where_its_received_segment_points(): + seen = [] + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + bent = _straight_arm_frame(_OPENXR_FORWARD - _OPENXR_UP) + forearm = _HUMAN_ELBOW_TO_WRIST * _unit(_OPENXR_RIGHT - _OPENXR_UP) + for _, elbow, wrist in _OPERATOR_ARMS: + bent.joints.positions[int(wrist)] = bent.joints.positions[int(elbow)] + forearm + + _run(source, bent) + + overlay = seen[-1] + for name, arm in zip(("left_wrist", "right_wrist"), _OPERATOR_ARMS): + received = np.diff([overlay.skeleton[int(slot)] for slot in arm], axis=0) + rebuilt = np.diff(overlay.chains[name], axis=0) + assert not np.allclose(_unit(received[0]), _unit(received[1]), atol=1e-2) + for got, want in zip(rebuilt, received): + np.testing.assert_allclose(_unit(got), _unit(want), atol=1e-5) + + +def test_the_overlay_draws_no_skeleton_while_nothing_arrives(): + """Before the first pairing and after losing one; the held target is still drawn.""" + seen = [] + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + + for frame in (None, _straight_arm_frame(_OPENXR_UP), None): + action = _run(source, frame) + overlay = seen[-1] + assert (len(overlay.skeleton) == 0) == (frame is None) + for task_index, name in enumerate(("left_wrist", "right_wrist")): + np.testing.assert_allclose( + overlay.chains[name][-1], + _target_pose(action, task_index)[0], + atol=1e-5, + ) + + +def test_an_unusable_slot_is_left_out_of_the_skeleton(): + seen = [] + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + frame = _straight_arm_frame(_OPENXR_FORWARD) + frame.joints.is_valid[int(BodyJoint.LEFT_ELBOW)] = 0 + + _run(source, frame) + + skeleton = seen[-1].skeleton + assert int(BodyJoint.LEFT_ELBOW) not in skeleton + assert int(BodyJoint.RIGHT_ELBOW) in skeleton + + +def test_without_the_operators_pelvis_there_is_no_skeleton_to_place(): + """The skeleton is placed by its pelvis; the held arms are drawn all the same.""" + seen = [] + source = JmexG1ActionSource( + layout=_g1_layout(), + pelvis_pose_provider=lambda: _yawed_pelvis(90.0), + overlay_sink=seen.append, + ) + + action = _run(source, _straight_arm_frame(_OPENXR_FORWARD)) + frame = _straight_arm_frame(_OPENXR_UP) + frame.joints.is_valid[int(BodyJoint.PELVIS)] = 0 + _run(source, frame) + + overlay = seen[-1] + assert len(overlay.skeleton) == 0 + for task_index, name in enumerate(("left_wrist", "right_wrist")): + np.testing.assert_allclose( + overlay.chains[name][-1], _target_pose(action, task_index)[0], atol=1e-5 + ) + + +@pytest.mark.parametrize("axis_sign", [1.0, -1.0]) +def test_the_robots_forearm_frame_runs_out_the_forearm_with_its_hinge_toward_the_bend( + axis_sign, +): + """The hinge is the elbow joint's own axis, whichever way the model happens to sign it.""" + frame = forearm_frame( + _REST_SHOULDER, + _REST_ELBOW, + _REST_WRIST, + elbow_axis=axis_sign * np.array([0.0, 1.0, 0.0]), + ).as_matrix() + + np.testing.assert_allclose(frame[:, 0], _unit(_REST_WRIST - _REST_ELBOW), atol=1e-9) + bend = np.cross(_REST_ELBOW - _REST_SHOULDER, _REST_WRIST - _REST_ELBOW) + assert np.dot(frame[:, 1], bend) > 0.0 + # The joint's axis, not the bend's direction: the splayed upper arm tilts the latter. + assert abs(frame[1, 1]) > 0.999 + + +def test_a_robot_resting_with_a_straight_elbow_has_no_forearm_frame(): + """Which way the elbow bends is read off the rest pose; a straight one does not say.""" + with pytest.raises(ValueError): + forearm_frame( + _REST_SHOULDER, + _REST_ELBOW, + _REST_ELBOW + (_REST_ELBOW - _REST_SHOULDER), + elbow_axis=np.array([0.0, 1.0, 0.0]), + )