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..1aa3cfd646 --- /dev/null +++ b/examples/jmex/README.md @@ -0,0 +1,258 @@ + + +# 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 + +Set up a venv as in [Running an example](../README.md#running-an-example) and activate it in each +terminal below. 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 +``` + +The install replaces the venv's `isaaccapture` with an editable build of this checkout. After a C++ +change to `isaaccapture`, rerun it with `--reinstall-package isaaccapture`. + +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. In a venv set up as in +[Running an example](../README.md#running-an-example): + +```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..8a1e494472 --- /dev/null +++ b/examples/jmex/pyproject.toml @@ -0,0 +1,35 @@ +# 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] +only-include = ["python/isaaccapture_examples/jmex"] + +[tool.hatch.build.targets.wheel] +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..e99f5cf7c4 --- /dev/null +++ b/src/plugins/jmex/README.md @@ -0,0 +1,239 @@ + + +# 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. In a venv set up as + in [Running an example](../../../examples/README.md#running-an-example), install the example + once, then start the runtime: + + ```bash + uv pip install -e ./examples/jmex + 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 46847194f9..c06e2b7ead 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -16,3 +16,7 @@ endif() if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() + +if(TARGET jmex_plugin_core) + add_subdirectory(jmex) +endif() 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]), + )