diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 03d9bb902..c1c468fc4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -8,12 +8,16 @@ repos: # Run the linter - id: ruff args: ["--fix"] + # The bundled Noitom MocapApi SDK is vendor code; keep it byte-stable. + exclude: ^src/plugins/noitom_mocap/third_party/mocap_api/ # Run the formatter - id: ruff-format + exclude: ^src/plugins/noitom_mocap/third_party/mocap_api/ - repo: https://github.com/pre-commit/pre-commit-hooks rev: v6.0.0 hooks: - id: trailing-whitespace + exclude: ^src/plugins/noitom_mocap/third_party/mocap_api/ - id: check-symlinks - id: destroyed-symlinks - id: check-added-large-files @@ -24,6 +28,7 @@ repos: - id: check-executables-have-shebangs - id: check-toml - id: end-of-file-fixer + exclude: ^src/plugins/noitom_mocap/third_party/mocap_api/ - id: check-shebang-scripts-are-executable - id: detect-private-key - id: debug-statements diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c12c7c8e..9f8220818 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,6 +59,7 @@ endif() # Options option(BUILD_PLUGINS "Build plugins" ON) option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for DepthAI v3.x)" OFF) +option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (uses bundled MocapApi SDK)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 example (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) @@ -187,6 +188,9 @@ if(BUILD_PLUGINS) if(BUILD_PLUGIN_OAK_CAMERA) add_subdirectory(src/plugins/oak) endif() + if(BUILD_PLUGIN_NOITOM_MOCAP) + add_subdirectory(src/plugins/noitom_mocap) + endif() endif() # Formatting enforcement (runs on Linux by default) diff --git a/LICENSES/LicenseRef-Noitom-MocapApi-SDK.txt b/LICENSES/LicenseRef-Noitom-MocapApi-SDK.txt new file mode 100644 index 000000000..366cc8745 --- /dev/null +++ b/LICENSES/LicenseRef-Noitom-MocapApi-SDK.txt @@ -0,0 +1,9 @@ +Noitom MocapApi SDK License Reference + +The files under src/plugins/noitom_mocap/third_party/mocap_api/ are bundled +third-party Noitom MocapApi SDK artifacts. + +The copied SDK drop in this repository does not include a redistributable +license text or copyright notice. Keep those files annotated with +LicenseRef-Noitom-MocapApi-SDK in REUSE.toml instead of assigning NVIDIA +copyright or Apache-2.0 metadata to the vendor SDK. diff --git a/REUSE.toml b/REUSE.toml index 49a87ba37..bb1e5354a 100644 --- a/REUSE.toml +++ b/REUSE.toml @@ -41,3 +41,12 @@ path = "examples/cloudxr_mujoco_teleop/**" precedence = "override" SPDX-FileCopyrightText = "Copyright (c) 2026 HTC Corporation" SPDX-License-Identifier = "Apache-2.0" + +# The bundled Noitom MocapApi SDK does not ship SPDX metadata in the copied +# headers/bindings; keep the vendor path annotated instead of adding NVIDIA +# copyright headers to third-party SDK files. +[[annotations]] +path = "src/plugins/noitom_mocap/third_party/mocap_api/**" +precedence = "override" +SPDX-FileCopyrightText = "NOASSERTION" +SPDX-License-Identifier = "LicenseRef-Noitom-MocapApi-SDK" diff --git a/applicationInfo.apiVersion: b/applicationInfo.apiVersion: new file mode 100644 index 000000000..e69de29bb diff --git a/applicationInfo.applicationName: b/applicationInfo.applicationName: new file mode 100644 index 000000000..e69de29bb diff --git a/applicationInfo.applicationVersion: b/applicationInfo.applicationVersion: new file mode 100644 index 000000000..e69de29bb diff --git a/applicationInfo.engineName: b/applicationInfo.engineName: new file mode 100644 index 000000000..e69de29bb diff --git a/applicationInfo.engineVersion: b/applicationInfo.engineVersion: new file mode 100644 index 000000000..e69de29bb diff --git a/docs/source/_static/haptikos-exoskeletons.gif b/docs/source/_static/haptikos-exoskeletons.gif new file mode 100644 index 000000000..d838ba383 Binary files /dev/null and b/docs/source/_static/haptikos-exoskeletons.gif differ diff --git a/docs/source/device/haptikos.rst b/docs/source/device/haptikos.rst index f8373e7eb..39fba12d4 100644 --- a/docs/source/device/haptikos.rst +++ b/docs/source/device/haptikos.rst @@ -4,6 +4,19 @@ Haptikos Exoskeletons ===================== +.. raw:: html + +
+ +.. image:: ../_static/haptikos-exoskeletons.gif + :alt: Haptikos exoskeletons used with Isaac Teleop + :class: align-center + :width: 100% + +.. raw:: html + +
+ Use the `Haptikos `_ Exoskeletons with the Isaac Teleop framework. Currently only Linux is supported. Tested on Meta Quest headsets. Other headsets with controllers may work as well. .. contents:: On this page diff --git a/examples/noitom/README.md b/examples/noitom/README.md new file mode 100644 index 000000000..7ca8f8ee8 --- /dev/null +++ b/examples/noitom/README.md @@ -0,0 +1,203 @@ + + +# Noitom G1 Test Task + +This example registers an external Isaac Lab task that is based on +`Isaac-PickPlace-Locomanipulation-G1-Abs-v0` and swaps its IsaacTeleop pipeline +to read the `noitom_mocap` OpenXR tensor collection as `FullBodyPosePN` data +defined in `src/core/schema/fbs/full_body_pn.fbs`. + +Noitom data is converted before it reaches this task. The plugin publishes the +Perception Neuron full-body schema on tensor identifier `full_body_pn`; this +example consumes it with `FullBodyTrackerPN("noitom_mocap")`. + +The registered task id is: + +```bash +Isaac-PickPlace-Locomanipulation-G1-Noitom-Abs-v0 +``` + +## Examples + +This README has two workflows: + +- Teleop: stream live Noitom full-body data into the G1 IsaacLab task. +- Record and replay: capture the same `full_body_pn` stream to MCAP and play it + back without running the teleop task. + +### Example 1: Teleop + +![Noitom G1 live teleoperation](assets/teleop.gif) + +Terminal 1, keep CloudXR running: + +```bash +conda activate teleop +cd ~/IsaacTeleop +python -m isaacteleop.cloudxr +``` + +Terminal 2, publish Noitom data into CloudXR/OpenXR: + +```bash +source ~/.cloudxr/run/cloudxr.env +cd ~/IsaacTeleop +./install/plugins/noitom_mocap/noitom_mocap_plugin --host= --port= +``` + +The default plugin collection id is `noitom_mocap`, which matches +`NoitomG1Settings.collection_id`. If you pass `--collection-id` to the plugin, +update `NoitomG1Settings.collection_id` to the same value. + +Terminal 3, run IsaacLab with the external task registration callback: + +```bash +conda activate teleop +cd ~/dependence/IsaacLab3-0 +PYTHONPATH=~/IsaacTeleop/examples/noitom:$PYTHONPATH \ +./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-Locomanipulation-G1-Noitom-Abs-v0 \ + --visualizer kit \ + --xr \ + --external_callback noitom_tasks.register_tasks +``` + +If CloudXR is already running in Terminal 1 and IsaacLab should not auto-launch +another runtime, add: + +```bash +--cloudxr_env none --no-auto_launch_cloudxr +``` + +### Example 2: Record and replay + +Record and replay use the same Noitom OpenXR tensor collection as the teleop +task. Keep CloudXR and `noitom_mocap_plugin` running as in Example 1. + +Before recording from a development checkout, rebuild the Python package and +make sure this shell imports the rebuilt package, not an older package installed +in the conda environment: + +```bash +cd ~/IsaacTeleop +cmake --build build --target python_package noitom_mocap_plugin --parallel + +PYTHONPATH=$PWD/build/python_package/Release \ +python - <<'PY' +import isaacteleop.deviceio as deviceio +deviceio.FullBodyTrackerPN("noitom_mocap", 16 * 1024) +print(deviceio.__file__) +PY +``` + +Record the Noitom full-body stream: + +![Noitom full-body recording](assets/record.gif) + +```bash +PYTHONPATH=$PWD/build/python_package/Release \ +python examples/noitom/record_noitom_full_body.py --duration 10 +``` + +By default the recorder reads collection `noitom_mocap`, writes MCAP channel +base `full_body_pn`, and stores the file under `examples/noitom/recordings/`. +Override paths or ids as needed: + +```bash +python examples/noitom/record_noitom_full_body.py \ + --collection-id noitom_mocap \ + --channel-name full_body_pn \ + --duration 10 \ + --output /tmp/noitom_full_body.mcap +``` + +Replay the newest recording: + +![Noitom MCAP replay](assets/replay.gif) + +```bash +PYTHONPATH=$PWD/build/python_package/Release \ +python examples/noitom/replay_noitom_full_body.py +``` + +Replay an explicit file: + +```bash +python examples/noitom/replay_noitom_full_body.py /tmp/noitom_full_body.mcap +``` + +`replay_noitom_full_body.py` uses the Python `mcap` package to infer playback +duration from the file summary. If that package is unavailable in your test +environment, pass a duration explicitly: + +```bash +python examples/noitom/replay_noitom_full_body.py /tmp/noitom_full_body.mcap --duration 10 +``` + +The resulting MCAP stores `core.FullBodyPosePNRecord` on the `full_body_pn` +channel path. Replay it with `examples/noitom/replay_noitom_full_body.py`, not +the PICO `FullBodyPosePicoRecord` replay path. + +If the recorder prints a "waiting for Noitom frames" message, check that the +plugin is still running in the same OpenXR/CloudXR runtime and that both sides +use the same `collection_id`. + +## Behavior + +Retargeting lives in `noitom_retargeting.py` and is wired by `noitom_tasks.py`. +It maps Noitom `FullBodyPosePN` shoulder / elbow / wrist bones into G1 **arm joint +deltas** for direct joint-position teleop (no Pink IK). + +Pipeline: + +```text +FullBodyPosePN + → torso frame (pelvis, SPINE3, shoulders) + → left/right wrist SE3 deltas (shoulder, elbow, wrist) + → arm-length scale + two-link reach clamp + → left/right arm joint deltas (shoulder, elbow, wrist; 7+7) + → action [left_arm(7), right_arm(7), hand_joints(14), locomotion(4)] +``` + +Upper body uses :class:`~isaaclab.envs.mdp.actions.JointPositionAction` (not Pink IK). +Each arm joint is retargeted from the corresponding mocap bone using the same +torso-relative delta idea as the legacy wrist-only path. + +The G1 action layout is: + +```text +[left_arm(7), right_arm(7), hand_joints(14), locomotion(4)] +``` + +Arm deltas use shoulder / elbow / wrist mocap in a torso-relative frame (position +for shoulder–elbow–wrist chain, orientation for wrist). Hand joints and +locomotion stay fixed for bring-up unless extended later. +Finger retargeting is intentionally omitted. + +Calibration: + +1. After teleop **reset** (`execution_events.reset`), retargeting clears its + neutral reference. +2. Hold a stable upper-body pose; the next valid frame becomes the neutral + reference (`calibrated=ready` in logs). +3. Motion is applied relative to that neutral pose. + +Default settings: + +- `NoitomG1Settings` in `noitom_tasks.py` (collection, visualization) +- `NoitomRetargetingSettings` in `noitom_retargeting.py` (scale, smoothing, + robot arm lengths, workspace clamps) + +When Kit visualization is enabled, the Noitom frame is shown as a cyan stick +figure anchored to the robot pelvis (orange crosses mark retargeted wrist +targets). The visualizer uses the same Y-up to Z-up conversion as the retargeter +(`noitom_position_to_isaac`). + +**Calibration tip:** At reset, hold a pose close to the G1 default upper-body +posture (elbows bent, hands near chest height). Calibration maps your neutral +mocap pose to the robot nominal wrist targets; a large mismatch at calibration +will show the cyan skeleton far from the orange wrist targets even when you are +not moving. diff --git a/examples/noitom/assets/record.gif b/examples/noitom/assets/record.gif new file mode 100644 index 000000000..016b47a00 --- /dev/null +++ b/examples/noitom/assets/record.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:388972ec86663d67b232aff3f192fd928752612f5b68d3cc85110dacdea3a921 +size 1775108 diff --git a/examples/noitom/assets/replay.gif b/examples/noitom/assets/replay.gif new file mode 100644 index 000000000..055089594 --- /dev/null +++ b/examples/noitom/assets/replay.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:55417b73d0cb585137ddfa2047b1e4478f8e0e0451b5ccff62d607fa41f0a026 +size 806637 diff --git a/examples/noitom/assets/teleop.gif b/examples/noitom/assets/teleop.gif new file mode 100644 index 000000000..2bc89a05d --- /dev/null +++ b/examples/noitom/assets/teleop.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a9ad5aa77d996f38b4b5952e04f9df432f67456523f3bb3e7e2343daccf8dd88 +size 2678115 diff --git a/examples/noitom/noitom_reference_draw.py b/examples/noitom/noitom_reference_draw.py new file mode 100644 index 000000000..c90493da2 --- /dev/null +++ b/examples/noitom/noitom_reference_draw.py @@ -0,0 +1,445 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Shared Noitom mocap skeleton placement (cyan debug draw + wrist retargeting).""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaacteleop.schema import BodyJointPN + +from noitom_retargeting import noitom_position_to_isaac + +# G1_29DOF_CFG spawn rot (0,0,0.7071,0.7071): pelvis +X aligns with world +Y (table forward). +G1_ROBOT_FORWARD_XY = np.array([0.0, 1.0], dtype=np.float64) + + +@dataclass(frozen=True) +class ReferenceSkeletonLengths: + """G1 link lengths for posture-based reference skeleton (meters).""" + + upper_arm: float = 0.28 + forearm: float = 0.26 + torso_segment: float = 0.07 + neck: float = 0.08 + head: float = 0.12 + hand_extension: float = 0.05 + left_shoulder_offset: tuple[float, float, float] = (0.05, 0.19, 0.30) + right_shoulder_offset: tuple[float, float, float] = (0.05, -0.19, 0.30) + + @classmethod + def from_retargeting_settings(cls, settings: Any) -> ReferenceSkeletonLengths: + return cls( + upper_arm=float(settings.robot_upper_arm_length), + forearm=float(settings.robot_forearm_length), + left_shoulder_offset=tuple( + float(v) for v in settings.robot_left_shoulder_offset + ), + right_shoulder_offset=tuple( + float(v) for v in settings.robot_right_shoulder_offset + ), + ) + + +ARM_CHAIN_JOINTS = frozenset( + { + int(BodyJointPN.LEFT_COLLAR), + int(BodyJointPN.LEFT_SHOULDER), + int(BodyJointPN.LEFT_ELBOW), + int(BodyJointPN.LEFT_WRIST), + int(BodyJointPN.LEFT_HAND), + int(BodyJointPN.RIGHT_COLLAR), + int(BodyJointPN.RIGHT_SHOULDER), + int(BodyJointPN.RIGHT_ELBOW), + int(BodyJointPN.RIGHT_WRIST), + int(BodyJointPN.RIGHT_HAND), + } +) + + +def extract_raw_yup_positions( + frame: Any, +) -> tuple[dict[int, np.ndarray], np.ndarray | None]: + """Read valid Noitom joint positions (Y-up meters) from a FullBodyPosePN frame.""" + if frame.joints is None: + return {}, None + positions: dict[int, np.ndarray] = {} + for index in range(int(BodyJointPN.NUM_JOINTS)): + joint = frame.joints.joints(index) + if not joint.is_valid: + continue + point = joint.pose.position + positions[int(index)] = np.array([point.x, point.y, point.z], dtype=np.float64) + return positions, positions.get(int(BodyJointPN.PELVIS)) + + +def reference_joint_scale( + joint_index: int, + draw_scale: float, + calib_view: Any | None, + arm_chain_joints: frozenset[int] = ARM_CHAIN_JOINTS, +) -> float: + if calib_view is None: + return draw_scale + if joint_index in arm_chain_joints: + return draw_scale * float(calib_view.arm_length_scale) + return draw_scale * float(calib_view.body_height_scale) + + +def build_reference_skeleton_positions( + raw_positions: dict[int, np.ndarray], + pelvis_raw: np.ndarray, + pelvis_anchor: np.ndarray, + draw_scale: float, + calib_view: Any | None, + arm_chain_joints: frozenset[int] = ARM_CHAIN_JOINTS, +) -> dict[int, np.ndarray]: + """Place Noitom joints at the robot pelvis anchor (Isaac Z-up, pelvis-relative).""" + pelvis_isaac = noitom_position_to_isaac(pelvis_raw) + anchor = np.asarray(pelvis_anchor, dtype=np.float64) + positions: dict[int, np.ndarray] = {} + for index, point_raw in raw_positions.items(): + joint_scale = reference_joint_scale( + int(index), draw_scale, calib_view, arm_chain_joints + ) + rel = (noitom_position_to_isaac(point_raw) - pelvis_isaac) * joint_scale + positions[int(index)] = anchor + rel + return positions + + +def reference_torso_forward_xy( + positions: dict[int, np.ndarray], + *, + pelvis_index: int = int(BodyJointPN.PELVIS), + spine3_index: int = int(BodyJointPN.SPINE3), + left_shoulder_index: int = int(BodyJointPN.LEFT_SHOULDER), + right_shoulder_index: int = int(BodyJointPN.RIGHT_SHOULDER), +) -> np.ndarray | None: + pelvis = positions.get(pelvis_index) + spine3 = positions.get(spine3_index) + left_shoulder = positions.get(left_shoulder_index) + right_shoulder = positions.get(right_shoulder_index) + if ( + pelvis is None + or spine3 is None + or left_shoulder is None + or right_shoulder is None + ): + return None + up = spine3 - pelvis + right = right_shoulder - left_shoulder + if float(np.linalg.norm(up)) < 1e-5 or float(np.linalg.norm(right)) < 1e-5: + return None + up = up / np.linalg.norm(up) + right = right / np.linalg.norm(right) + forward = np.cross(up, right) + forward_xy = forward[:2] + norm = float(np.linalg.norm(forward_xy)) + if norm < 1e-5: + return None + return forward_xy / norm + + +def signed_yaw_xy(from_xy: np.ndarray, to_xy: np.ndarray) -> float: + source = np.asarray(from_xy, dtype=np.float64)[:2] + target = np.asarray(to_xy, dtype=np.float64)[:2] + source_norm = float(np.linalg.norm(source)) + target_norm = float(np.linalg.norm(target)) + if source_norm < 1e-8 or target_norm < 1e-8: + return 0.0 + source = source / source_norm + target = target / target_norm + cross = source[0] * target[1] - source[1] * target[0] + dot = float(np.clip(np.dot(source, target), -1.0, 1.0)) + return float(np.arctan2(cross, dot)) + + +def rotate_reference_about_anchor( + positions: dict[int, np.ndarray], + anchor: np.ndarray, + yaw: float, +) -> dict[int, np.ndarray]: + if abs(yaw) < 1e-6: + return positions + rot = Rotation.from_euler("z", yaw) + anchor_vec = np.asarray(anchor, dtype=np.float64) + return { + index: anchor_vec + rot.apply(pos - anchor_vec) + for index, pos in positions.items() + } + + +def _unit_direction( + vector: np.ndarray, + fallback: np.ndarray | None = None, +) -> np.ndarray: + vec = np.asarray(vector, dtype=np.float64) + norm = float(np.linalg.norm(vec)) + if norm < 1e-6: + if fallback is not None: + return _unit_direction(fallback) + return np.array([1.0, 0.0, 0.0], dtype=np.float64) + return vec / norm + + +def _pelvis_frame_offset_to_world(offset: np.ndarray) -> np.ndarray: + """Map G1 pelvis-frame offset to world when the robot faces scene +Y.""" + ox, oy, oz = np.asarray(offset, dtype=np.float64) + return np.array([-oy, ox, oz], dtype=np.float64) + + +def apply_robot_link_lengths( + positions: dict[int, np.ndarray], + pelvis_anchor: np.ndarray, + lengths: ReferenceSkeletonLengths, + *, + length_scale: float = 1.0, + arm_length_scale: float = 1.0, + shoulder_span_scale: float = 1.0, +) -> dict[int, np.ndarray]: + """Rebuild upper body in order: head match -> shoulder width -> arm segment lengths.""" + anchor = np.asarray(pelvis_anchor, dtype=np.float64) + scale = float(length_scale) + arm_scale = float(arm_length_scale) + span_scale = float(shoulder_span_scale) + out = dict(positions) + out[int(BodyJointPN.PELVIS)] = anchor.copy() + + torso_nominal_total = ( + 3.0 * lengths.torso_segment + lengths.neck + lengths.head + ) * scale + torso_scale = 1.0 + head_src = positions.get(int(BodyJointPN.HEAD)) + pelvis_src = positions.get(int(BodyJointPN.PELVIS)) + if head_src is not None and pelvis_src is not None and torso_nominal_total > 1e-6: + src_head_dist = float(np.linalg.norm(head_src - pelvis_src)) + torso_scale = float(np.clip(src_head_dist / torso_nominal_total, 0.6, 1.8)) + + torso_chain = ( + (int(BodyJointPN.SPINE1), lengths.torso_segment), + (int(BodyJointPN.SPINE2), lengths.torso_segment), + (int(BodyJointPN.SPINE3), lengths.torso_segment), + (int(BodyJointPN.NECK), lengths.neck), + (int(BodyJointPN.HEAD), lengths.head), + ) + parent_robot = anchor + parent_mocap = positions.get(int(BodyJointPN.PELVIS), anchor) + up_fallback = np.array([0.0, 0.0, 1.0], dtype=np.float64) + for joint_index, segment_length in torso_chain: + child_mocap = positions.get(joint_index) + if child_mocap is None: + continue + direction = _unit_direction(child_mocap - parent_mocap, fallback=up_fallback) + robot_pos = parent_robot + direction * (segment_length * scale * torso_scale) + out[joint_index] = robot_pos + parent_robot = robot_pos + parent_mocap = child_mocap + + left_shoulder_src = positions.get(int(BodyJointPN.LEFT_SHOULDER)) + right_shoulder_src = positions.get(int(BodyJointPN.RIGHT_SHOULDER)) + spine3_robot = out.get(int(BodyJointPN.SPINE3)) + spine3_src = positions.get(int(BodyJointPN.SPINE3)) + + default_left = anchor + _pelvis_frame_offset_to_world( + np.asarray(lengths.left_shoulder_offset, dtype=np.float64) + ) + default_right = anchor + _pelvis_frame_offset_to_world( + np.asarray(lengths.right_shoulder_offset, dtype=np.float64) + ) + default_span = float(np.linalg.norm(default_left - default_right)) + mocap_span_scale = 1.0 + shoulder_axis = np.array([0.0, 1.0, 0.0], dtype=np.float64) + shoulder_center = 0.5 * (default_left + default_right) + if left_shoulder_src is not None and right_shoulder_src is not None: + src_vec = left_shoulder_src - right_shoulder_src + src_span = float(np.linalg.norm(src_vec)) + if src_span > 1e-6: + shoulder_axis = src_vec / src_span + if default_span > 1e-6: + mocap_span_scale = float(np.clip(src_span / default_span, 0.7, 1.15)) + src_center = 0.5 * (left_shoulder_src + right_shoulder_src) + if spine3_robot is not None and spine3_src is not None: + shoulder_center = spine3_robot + (src_center - spine3_src) + else: + shoulder_center = src_center.copy() + # Follow mocap shoulder height more aggressively; keep only a loose guard band + # so shoulders can actually track instead of being locked near nominal. + nominal_shoulder_center_z = float(0.5 * (default_left[2] + default_right[2])) + blended_center_z = ( + 0.85 * float(shoulder_center[2]) + 0.15 * nominal_shoulder_center_z + ) + shoulder_center_z = float( + np.clip( + blended_center_z, + nominal_shoulder_center_z - 0.12, + nominal_shoulder_center_z + 0.10, + ) + ) + shoulder_center = shoulder_center.copy() + shoulder_center[2] = shoulder_center_z + + shoulder_half_span = 0.5 * default_span * mocap_span_scale * scale * span_scale + left_shoulder_robot = shoulder_center + shoulder_axis * shoulder_half_span + right_shoulder_robot = shoulder_center - shoulder_axis * shoulder_half_span + + arm_specs = ( + ( + left_shoulder_robot, + int(BodyJointPN.LEFT_SHOULDER), + int(BodyJointPN.LEFT_ELBOW), + int(BodyJointPN.LEFT_WRIST), + int(BodyJointPN.LEFT_HAND), + ), + ( + right_shoulder_robot, + int(BodyJointPN.RIGHT_SHOULDER), + int(BodyJointPN.RIGHT_ELBOW), + int(BodyJointPN.RIGHT_WRIST), + int(BodyJointPN.RIGHT_HAND), + ), + ) + for shoulder, shoulder_index, elbow_index, wrist_index, hand_index in arm_specs: + elbow_mocap = positions.get(elbow_index) + wrist_mocap = positions.get(wrist_index) + shoulder_mocap = positions.get(shoulder_index, shoulder) + if elbow_mocap is None or wrist_mocap is None: + out[shoulder_index] = shoulder + continue + upper_dir = _unit_direction( + elbow_mocap - shoulder_mocap, + fallback=np.array([0.0, 0.0, -1.0], dtype=np.float64), + ) + elbow = shoulder + upper_dir * (lengths.upper_arm * scale * arm_scale) + fore_dir = _unit_direction(wrist_mocap - elbow_mocap, fallback=upper_dir) + wrist = elbow + fore_dir * (lengths.forearm * scale * arm_scale) + out[shoulder_index] = shoulder + out[elbow_index] = elbow + out[wrist_index] = wrist + out[hand_index] = wrist + fore_dir * ( + lengths.hand_extension * scale * arm_scale + ) + + spine3 = out.get(int(BodyJointPN.SPINE3)) + if spine3 is not None: + for collar_index, shoulder_index in ( + (int(BodyJointPN.LEFT_COLLAR), int(BodyJointPN.LEFT_SHOULDER)), + (int(BodyJointPN.RIGHT_COLLAR), int(BodyJointPN.RIGHT_SHOULDER)), + ): + shoulder_pos = out.get(shoulder_index) + if shoulder_pos is not None: + out[collar_index] = 0.5 * (spine3 + shoulder_pos) + + return out + + +def align_reference_skeleton_to_robot( + positions: dict[int, np.ndarray], + pelvis_anchor: np.ndarray, + *, + pelvis_index: int = int(BodyJointPN.PELVIS), + spine3_index: int = int(BodyJointPN.SPINE3), + left_shoulder_index: int = int(BodyJointPN.LEFT_SHOULDER), + right_shoulder_index: int = int(BodyJointPN.RIGHT_SHOULDER), + robot_forward_xy: np.ndarray | None = None, +) -> dict[int, np.ndarray]: + """Rotate the skeleton about pelvis so it faces the G1 robot (+Y in scene).""" + forward_xy = reference_torso_forward_xy( + positions, + pelvis_index=pelvis_index, + spine3_index=spine3_index, + left_shoulder_index=left_shoulder_index, + right_shoulder_index=right_shoulder_index, + ) + if forward_xy is None: + return positions + target_xy = G1_ROBOT_FORWARD_XY if robot_forward_xy is None else robot_forward_xy + yaw = signed_yaw_xy(forward_xy, target_xy) + return rotate_reference_about_anchor(positions, pelvis_anchor, yaw) + + +def place_aligned_reference_skeleton( + raw_positions: dict[int, np.ndarray], + pelvis_raw: np.ndarray, + pelvis_anchor: np.ndarray, + draw_scale: float = 1.0, + calib_view: Any | None = None, + *, + use_robot_link_lengths: bool = True, + link_lengths: ReferenceSkeletonLengths | None = None, + length_scale: float = 1.0, + arm_length_scale: float = 1.0, + shoulder_span_scale: float = 1.0, +) -> dict[int, np.ndarray]: + """Build pelvis-anchored skeleton and align facing with the G1 locomanipulation robot.""" + direction_scale = 1.0 if use_robot_link_lengths else draw_scale + direction_calib = None if use_robot_link_lengths else calib_view + positions = build_reference_skeleton_positions( + raw_positions, + pelvis_raw, + pelvis_anchor, + direction_scale, + direction_calib, + ) + positions = align_reference_skeleton_to_robot(positions, pelvis_anchor) + if use_robot_link_lengths: + lengths = link_lengths or ReferenceSkeletonLengths() + positions = apply_robot_link_lengths( + positions, + pelvis_anchor, + lengths, + length_scale=length_scale * draw_scale, + arm_length_scale=arm_length_scale, + shoulder_span_scale=shoulder_span_scale, + ) + return positions + + +def aligned_reference_skeleton_from_frame( + frame: Any, + pelvis_anchor: np.ndarray, + draw_scale: float = 1.0, + calib_view: Any | None = None, + *, + use_robot_link_lengths: bool = True, + link_lengths: ReferenceSkeletonLengths | None = None, + length_scale: float = 1.0, + arm_length_scale: float = 1.0, + shoulder_span_scale: float = 1.0, +) -> dict[int, np.ndarray]: + """Full pipeline: FullBodyPosePN -> robot-aligned joint positions in Isaac world.""" + raw_positions, pelvis_raw = extract_raw_yup_positions(frame) + if pelvis_raw is None or not raw_positions: + return {} + return place_aligned_reference_skeleton( + raw_positions, + pelvis_raw, + pelvis_anchor, + draw_scale, + calib_view, + use_robot_link_lengths=use_robot_link_lengths, + link_lengths=link_lengths, + length_scale=length_scale, + arm_length_scale=arm_length_scale, + shoulder_span_scale=shoulder_span_scale, + ) + + +__all__ = [ + "ARM_CHAIN_JOINTS", + "G1_ROBOT_FORWARD_XY", + "ReferenceSkeletonLengths", + "align_reference_skeleton_to_robot", + "aligned_reference_skeleton_from_frame", + "apply_robot_link_lengths", + "build_reference_skeleton_positions", + "extract_raw_yup_positions", + "place_aligned_reference_skeleton", + "reference_torso_forward_xy", + "rotate_reference_about_anchor", + "signed_yaw_xy", +] diff --git a/examples/noitom/noitom_retargeting.py b/examples/noitom/noitom_retargeting.py new file mode 100644 index 000000000..0885630dc --- /dev/null +++ b/examples/noitom/noitom_retargeting.py @@ -0,0 +1,1851 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Noitom full-body to G1 wrist SE3 retargeting for locomanipulation teleop. + +Uses **posture-based** arm retargeting: mocap bone *directions* with G1 link +lengths (not scaled human joint positions). Wrist SE(3) targets feed Pink IK. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any + +import numpy as np +from scipy.spatial.transform import Rotation, Slerp + +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + ParameterState, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import ( + ComputeContext, + RetargeterIO, +) +from isaacteleop.retargeting_engine.interface.tunable_parameter import FloatParameter +from isaacteleop.retargeting_engine.interface.tensor_group_type import TensorGroupType +from isaacteleop.retargeting_engine.tensor_types import DLDataType, NDArrayType +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPosePNTracked, +) +from isaacteleop.schema import BodyJointPN + +# PNS/Noitom Y-up -> Isaac Z-up. PNS forward is -Z; axis remap maps it to Isaac -Y. +# Retarget / debug draw then apply operator_faces_robot (+180 deg Z) to align with G1 (+Y). +_NOITOM_TO_ISAAC = np.array( + [[-1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0]], + dtype=np.float64, +) +_COORD_ROT = Rotation.from_matrix(_NOITOM_TO_ISAAC) + +_DEFAULT_LEFT_WRIST_POS = np.array([-0.18, 0.1, 0.8], dtype=np.float64) +_DEFAULT_RIGHT_WRIST_POS = np.array([0.18, 0.1, 0.8], dtype=np.float64) +# G1 locomanipulation VR teleop wrist orientations (see locomanipulation_g1_env_cfg). +_DEFAULT_LEFT_WRIST_QUAT = np.array([-0.2706, 0.6533, 0.2706, 0.6533], dtype=np.float64) +_DEFAULT_RIGHT_WRIST_QUAT = np.array([-0.7071, 0.0, 0.7071, 0.0], dtype=np.float64) +_DEFAULT_ROBOT_PELVIS = np.array([0.0, 0.0, 0.72], dtype=np.float64) +# Approximate G1 shoulder origins in pelvis frame (Isaac Z-up, +Y left). +_ROBOT_LEFT_SHOULDER_OFFSET = np.array([0.05, 0.19, 0.30], dtype=np.float64) +_ROBOT_RIGHT_SHOULDER_OFFSET = np.array([0.05, -0.19, 0.30], dtype=np.float64) +# Torso segment lengths for posture-based reference skeleton (meters, pelvis frame). +_ROBOT_TORSO_SEGMENT_Z = 0.07 +_ROBOT_NECK_SEGMENT = 0.08 +_ROBOT_HEAD_SEGMENT = 0.12 +_ROBOT_HAND_EXTENSION = 0.05 +_DELTA_LIMIT = np.array([0.65, 0.65, 0.65], dtype=np.float64) + + +@dataclass +class NoitomRetargetingSettings: + """Tunable retargeting parameters for Noitom-driven G1 wrists.""" + + # Fraction of mocap pose applied relative to calibrated neutral (not link length). + motion_scale: float = 0.55 + # Reduce motion when the solved arm chain is nearly straight (elbow singularity). + arm_extension_soft_limit: float = 0.72 + # Minimum interior elbow angle (rad) when reconstructing the forearm direction. + min_elbow_interior_angle: float = 0.65 + # Scale motion when the operator arm span exceeds the G1 link lengths. + human_reach_margin: float = 0.96 + # Cap how much operator torso twist rotates arm targets (radians). + max_torso_yaw_delta: float = 0.22 + # Fraction of torso yaw applied to arms (lower = arms ignore body twist). + torso_yaw_arm_influence: float = 0.35 + position_smoothing: float = 0.85 + rotation_smoothing: float = 0.75 + robot_upper_arm_length: float = 0.28 + robot_forearm_length: float = 0.26 + arm_scale_min: float = 0.5 + arm_scale_max: float = 1.5 + robot_pelvis_world: np.ndarray = field( + default_factory=lambda: _DEFAULT_ROBOT_PELVIS.copy() + ) + robot_left_shoulder_offset: np.ndarray = field( + default_factory=lambda: _ROBOT_LEFT_SHOULDER_OFFSET.copy() + ) + robot_right_shoulder_offset: np.ndarray = field( + default_factory=lambda: _ROBOT_RIGHT_SHOULDER_OFFSET.copy() + ) + nominal_left_wrist_pos: np.ndarray = field( + default_factory=lambda: _DEFAULT_LEFT_WRIST_POS.copy() + ) + nominal_right_wrist_pos: np.ndarray = field( + default_factory=lambda: _DEFAULT_RIGHT_WRIST_POS.copy() + ) + nominal_left_wrist_quat_xyzw: np.ndarray = field( + default_factory=lambda: _DEFAULT_LEFT_WRIST_QUAT.copy() + ) + nominal_right_wrist_quat_xyzw: np.ndarray = field( + default_factory=lambda: _DEFAULT_RIGHT_WRIST_QUAT.copy() + ) + # Mocap wrist orientation does not match G1 wrist_yaw_link; lock by default. + track_wrist_orientation: bool = False + # Operator stands facing the robot (mirror L/R in horizontal plane). + operator_faces_robot: bool = True + # Drive Pink IK wrists to the cyan skeleton wrist joints (shared placement frame). + track_aligned_mocap_wrists: bool = True + # Rebuild cyan skeleton with G1 link lengths (bone directions, not human joint spacing). + reference_use_robot_link_lengths: bool = True + # Global multiplier on G1 segment lengths for the shared reference skeleton. + reference_length_scale: float = 1.0 + # Additional scale applied only to upper-arm/forearm segments in reference skeleton. + reference_arm_length_scale: float = 0.7 + # Scale left-right shoulder span in the reference skeleton (lower = narrower shoulders). + reference_shoulder_span_scale: float = 0.82 + # Retain mocap pose via bone directions + robot link lengths (not joint positions). + use_posture_based_arms: bool = True + # Blend G1 nominal wrist quat with forearm-aligned frame (helps Pink IK converge). + wrist_orientation_forearm_blend: float = 0.35 + # Feed posture-based elbow positions into Pink IK (narrows shoulder/elbow null space). + track_elbow_ik_targets: bool = True + # Feed posture-based shoulder positions into Pink IK (locks shoulder placement). + track_shoulder_ik_targets: bool = True + delta_limit: np.ndarray = field(default_factory=lambda: _DELTA_LIMIT.copy()) + sync_nominal_at_calibration: bool = True + + +@dataclass +class SE3Pose: + """Pose in Isaac Z-up world frame (position meters, quaternion xyzw).""" + + position: np.ndarray + quaternion_xyzw: np.ndarray + + def as_action_pose(self) -> np.ndarray: + return np.concatenate( + [ + self.position.astype(np.float32), + self.quaternion_xyzw.astype(np.float32), + ] + ) + + @staticmethod + def from_nominal(position: np.ndarray, quaternion_xyzw: np.ndarray) -> SE3Pose: + return SE3Pose(position.copy(), quaternion_xyzw.copy()) + + +@dataclass +class ArmIkTargets: + """Pink IK frame targets for one update (wrist, elbow, shoulder per arm).""" + + left_wrist: SE3Pose + right_wrist: SE3Pose + left_elbow: SE3Pose + right_elbow: SE3Pose + left_shoulder: SE3Pose + right_shoulder: SE3Pose + + +@dataclass +class NoitomCalibrationView: + """Read-only calibration snapshot for debug visualization alignment.""" + + pelvis_world: np.ndarray + body_yaw_isaac: float + arm_length_scale: float + body_height_scale: float + + +@dataclass +class _TorsoFrame: + origin: np.ndarray + rotation: Rotation + + +@dataclass +class _ArmCalibration: + shoulder_torso: np.ndarray + shoulder_world: np.ndarray + elbow_world: np.ndarray + wrist_pos_torso: np.ndarray + wrist_rot_torso: Rotation + wrist_rel_pelvis: np.ndarray + wrist_world: np.ndarray + upper_arm_length: float + forearm_length: float + + +@dataclass +class _CalibrationState: + torso: _TorsoFrame + left: _ArmCalibration + right: _ArmCalibration + arm_length_scale: float + body_height_scale: float + body_yaw_isaac: float + pelvis_world: np.ndarray + nominal_left: SE3Pose + nominal_right: SE3Pose + nominal_left_elbow: SE3Pose + nominal_right_elbow: SE3Pose + nominal_left_shoulder: SE3Pose + nominal_right_shoulder: SE3Pose + + +class NoitomG1Retargeter(BaseRetargeter): + """Retarget Noitom upper-body motion to G1 arm SE3 targets for Pink IK.""" + + def __init__( + self, + settings: NoitomRetargetingSettings | None = None, + name: str = "noitom_g1_retargeter", + ) -> None: + self._settings = settings or NoitomRetargetingSettings() + self._nominal_left = SE3Pose.from_nominal( + self._settings.nominal_left_wrist_pos, + self._settings.nominal_left_wrist_quat_xyzw, + ) + self._nominal_right = SE3Pose.from_nominal( + self._settings.nominal_right_wrist_pos, + self._settings.nominal_right_wrist_quat_xyzw, + ) + self._calibration: _CalibrationState | None = None + self._latest_torso: _TorsoFrame | None = None + self._smoothed_left = SE3Pose.from_nominal( + self._nominal_left.position, self._nominal_left.quaternion_xyzw + ) + self._smoothed_right = SE3Pose.from_nominal( + self._nominal_right.position, self._nominal_right.quaternion_xyzw + ) + self._smoothed_left_elbow = self._default_elbow_pose(is_left=True) + self._smoothed_right_elbow = self._default_elbow_pose(is_left=False) + self._smoothed_left_shoulder = self._default_shoulder_pose(is_left=True) + self._smoothed_right_shoulder = self._default_shoulder_pose(is_left=False) + + param_state = ParameterState( + name, + parameters=[ + FloatParameter( + "motion_scale", + "Pose amplitude vs calibrated neutral (0=hold, 1=full mocap posture).", + default_value=self._settings.motion_scale, + min_value=0.0, + max_value=1.0, + step_size=0.05, + sync_fn=lambda v: setattr(self._settings, "motion_scale", v), + ), + FloatParameter( + "position_smoothing", + "Position smoothing alpha (0=hold last, 1=instant).", + default_value=self._settings.position_smoothing, + min_value=0.0, + max_value=1.0, + step_size=0.05, + sync_fn=lambda v: setattr(self._settings, "position_smoothing", v), + ), + FloatParameter( + "rotation_smoothing", + "Rotation smoothing alpha (0=hold last, 1=instant).", + default_value=self._settings.rotation_smoothing, + min_value=0.0, + max_value=1.0, + step_size=0.05, + sync_fn=lambda v: setattr(self._settings, "rotation_smoothing", v), + ), + FloatParameter( + "robot_upper_arm_length", + "Robot upper arm length for reach clamping [m].", + default_value=self._settings.robot_upper_arm_length, + min_value=0.05, + max_value=0.5, + step_size=0.01, + sync_fn=lambda v: setattr( + self._settings, "robot_upper_arm_length", v + ), + ), + FloatParameter( + "robot_forearm_length", + "Robot forearm length for reach clamping [m].", + default_value=self._settings.robot_forearm_length, + min_value=0.05, + max_value=0.5, + step_size=0.01, + sync_fn=lambda v: setattr( + self._settings, "robot_forearm_length", v + ), + ), + FloatParameter( + "arm_scale_min", + "Minimum arm length scale (robot/human ratio floor).", + default_value=self._settings.arm_scale_min, + min_value=0.1, + max_value=1.0, + step_size=0.05, + sync_fn=lambda v: setattr(self._settings, "arm_scale_min", v), + ), + FloatParameter( + "arm_scale_max", + "Maximum arm length scale (robot/human ratio ceiling).", + default_value=self._settings.arm_scale_max, + min_value=1.0, + max_value=3.0, + step_size=0.05, + sync_fn=lambda v: setattr(self._settings, "arm_scale_max", v), + ), + ], + ) + + super().__init__(name=name, parameter_state=param_state) + + @property + def is_calibrated(self) -> bool: + return self._calibration is not None + + @property + def awaiting_calibration(self) -> bool: + return self._calibration is None + + @property + def current_left(self) -> SE3Pose: + return self._smoothed_left + + @property + def current_right(self) -> SE3Pose: + return self._smoothed_right + + @property + def current_left_elbow(self) -> SE3Pose: + return self._smoothed_left_elbow + + @property + def current_right_elbow(self) -> SE3Pose: + return self._smoothed_right_elbow + + @property + def current_left_shoulder(self) -> SE3Pose: + return self._smoothed_left_shoulder + + @property + def current_right_shoulder(self) -> SE3Pose: + return self._smoothed_right_shoulder + + @property + def current_arm_targets(self) -> ArmIkTargets: + return ArmIkTargets( + left_wrist=self._smoothed_left, + right_wrist=self._smoothed_right, + left_elbow=self._smoothed_left_elbow, + right_elbow=self._smoothed_right_elbow, + left_shoulder=self._smoothed_left_shoulder, + right_shoulder=self._smoothed_right_shoulder, + ) + + @property + def calibration_view(self) -> NoitomCalibrationView | None: + if self._calibration is None: + return None + calib = self._calibration + return NoitomCalibrationView( + pelvis_world=calib.pelvis_world.copy(), + body_yaw_isaac=calib.body_yaw_isaac, + arm_length_scale=calib.arm_length_scale, + body_height_scale=calib.body_height_scale, + ) + + @property + def body_yaw_isaac(self) -> float: + if self._latest_torso is None: + return 0.0 + return _compute_torso_yaw(self._latest_torso) + + @property + def body_yaw_delta(self) -> float: + if self._calibration is None: + return 0.0 + return self.body_yaw_isaac - self._calibration.body_yaw_isaac + + @property + def retargeting_settings(self) -> NoitomRetargetingSettings: + return self._settings + + @property + def neutral_arms(self) -> tuple[_ArmCalibration, _ArmCalibration] | None: + if self._calibration is None: + return None + return self._calibration.left, self._calibration.right + + def clear_calibration(self) -> None: + self._calibration = None + self._latest_torso = None + self._smoothed_left = SE3Pose.from_nominal( + self._nominal_left.position, self._nominal_left.quaternion_xyzw + ) + self._smoothed_right = SE3Pose.from_nominal( + self._nominal_right.position, self._nominal_right.quaternion_xyzw + ) + self._smoothed_left_elbow = self._default_elbow_pose(is_left=True) + self._smoothed_right_elbow = self._default_elbow_pose(is_left=False) + self._smoothed_left_shoulder = self._default_shoulder_pose(is_left=True) + self._smoothed_right_shoulder = self._default_shoulder_pose(is_left=False) + + def _default_shoulder_pose(self, is_left: bool) -> SE3Pose: + shoulder = _shoulder_world_robot(self._settings, 0.0, is_left) + upper_dir = np.array([0.0, 0.0, -1.0], dtype=np.float64) + nominal_quat = ( + self._settings.nominal_left_wrist_quat_xyzw + if is_left + else self._settings.nominal_right_wrist_quat_xyzw + ) + quat = _elbow_quat_for_ik(upper_dir, nominal_quat, self._settings) + return SE3Pose(shoulder, quat) + + def _default_elbow_pose(self, is_left: bool) -> SE3Pose: + shoulder = _shoulder_world_robot(self._settings, 0.0, is_left) + upper_dir = np.array([0.0, 0.0, -1.0], dtype=np.float64) + elbow = shoulder + upper_dir * self._settings.robot_upper_arm_length + nominal_quat = ( + self._settings.nominal_left_wrist_quat_xyzw + if is_left + else self._settings.nominal_right_wrist_quat_xyzw + ) + quat = _elbow_quat_for_ik(upper_dir, nominal_quat, self._settings) + return SE3Pose(elbow, quat) + + def calibrate(self, frame: Any) -> bool: + self._sync_parameters_from_state() + parsed = _parse_upper_body(frame) + if parsed is None: + return False + torso, left, right, pelvis_world = parsed + self._latest_torso = torso + human_arm = ( + left.upper_arm_length + + left.forearm_length + + right.upper_arm_length + + right.forearm_length + ) * 0.5 + robot_arm = ( + self._settings.robot_upper_arm_length + self._settings.robot_forearm_length + ) + if human_arm < 1e-4: + return False + raw_scale = robot_arm / human_arm + arm_scale = float( + np.clip( + raw_scale, self._settings.arm_scale_min, self._settings.arm_scale_max + ) + ) + shoulder_world = torso.origin + torso.rotation.apply(left.shoulder_torso) + shoulder_rel_z = float((shoulder_world - pelvis_world)[2]) + robot_shoulder_z = float(self._settings.robot_left_shoulder_offset[2]) + if shoulder_rel_z > 0.1: + raw_body_scale = robot_shoulder_z / shoulder_rel_z + else: + raw_body_scale = arm_scale + body_height_scale = float(np.clip(raw_body_scale, 0.45, 1.15)) + + if self._settings.sync_nominal_at_calibration: + if self._settings.track_aligned_mocap_wrists: + nominal_left, nominal_right = _nominal_wrists_from_aligned_frame( + frame, + self._settings, + arm_scale, + body_height_scale, + pelvis_world, + ) + if nominal_left is None or nominal_right is None: + return False + elif self._settings.use_posture_based_arms: + nominal_left = _wrist_pose_from_posture( + left, + self._settings, + is_left=True, + yaw_delta=0.0, + nominal_quat=self._settings.nominal_left_wrist_quat_xyzw, + ) + nominal_right = _wrist_pose_from_posture( + right, + self._settings, + is_left=False, + yaw_delta=0.0, + nominal_quat=self._settings.nominal_right_wrist_quat_xyzw, + ) + else: + nominal_left = SE3Pose.from_nominal( + self._nominal_left.position, self._nominal_left.quaternion_xyzw + ) + nominal_right = SE3Pose.from_nominal( + self._nominal_right.position, self._nominal_right.quaternion_xyzw + ) + else: + nominal_left = SE3Pose.from_nominal( + self._nominal_left.position, self._nominal_left.quaternion_xyzw + ) + nominal_right = SE3Pose.from_nominal( + self._nominal_right.position, self._nominal_right.quaternion_xyzw + ) + + nominal_left_elbow = self._default_elbow_pose(is_left=True) + nominal_right_elbow = self._default_elbow_pose(is_left=False) + if ( + self._settings.track_elbow_ik_targets + and self._settings.sync_nominal_at_calibration + ): + elbow_pair = _nominal_elbows_from_aligned_frame( + frame, + self._settings, + arm_scale, + body_height_scale, + pelvis_world, + ) + if elbow_pair is not None: + nominal_left_elbow, nominal_right_elbow = elbow_pair + + nominal_left_shoulder = self._default_shoulder_pose(is_left=True) + nominal_right_shoulder = self._default_shoulder_pose(is_left=False) + if ( + self._settings.track_shoulder_ik_targets + and self._settings.sync_nominal_at_calibration + ): + shoulder_pair = _nominal_shoulders_from_aligned_frame( + frame, + self._settings, + arm_scale, + body_height_scale, + pelvis_world, + ) + if shoulder_pair is not None: + nominal_left_shoulder, nominal_right_shoulder = shoulder_pair + + self._calibration = _CalibrationState( + torso=torso, + left=left, + right=right, + arm_length_scale=arm_scale, + body_height_scale=body_height_scale, + body_yaw_isaac=_compute_torso_yaw(torso), + pelvis_world=pelvis_world, + nominal_left=nominal_left, + nominal_right=nominal_right, + nominal_left_elbow=nominal_left_elbow, + nominal_right_elbow=nominal_right_elbow, + nominal_left_shoulder=nominal_left_shoulder, + nominal_right_shoulder=nominal_right_shoulder, + ) + self._smoothed_left = nominal_left + self._smoothed_right = nominal_right + self._smoothed_left_elbow = nominal_left_elbow + self._smoothed_right_elbow = nominal_right_elbow + self._smoothed_left_shoulder = nominal_left_shoulder + self._smoothed_right_shoulder = nominal_right_shoulder + return True + + def retarget(self, frame: Any) -> ArmIkTargets | None: + self._sync_parameters_from_state() + if self._calibration is None: + return None + parsed = _parse_upper_body(frame) + if parsed is None: + return None + + torso, left, right, pelvis_world = parsed + self._latest_torso = torso + calib = self._calibration + if self._settings.track_aligned_mocap_wrists: + left_target = _wrist_target_from_aligned_skeleton( + frame, + calib, + self._settings, + is_left=True, + ) + right_target = _wrist_target_from_aligned_skeleton( + frame, + calib, + self._settings, + is_left=False, + ) + if left_target is None or right_target is None: + return None + else: + left_target = _solve_wrist_target( + torso=torso, + arm=left, + pelvis_world=pelvis_world, + neutral=calib.left, + neutral_torso=calib.torso, + nominal=calib.nominal_left, + calib_yaw=calib.body_yaw_isaac, + arm_length_scale=calib.arm_length_scale, + settings=self._settings, + is_left=True, + ) + right_target = _solve_wrist_target( + torso=torso, + arm=right, + pelvis_world=pelvis_world, + neutral=calib.right, + neutral_torso=calib.torso, + nominal=calib.nominal_right, + calib_yaw=calib.body_yaw_isaac, + arm_length_scale=calib.arm_length_scale, + settings=self._settings, + is_left=False, + ) + self._smoothed_left = _smooth_pose( + self._smoothed_left, + left_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + self._smoothed_right = _smooth_pose( + self._smoothed_right, + right_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + left_elbow_target = self._smoothed_left_elbow + right_elbow_target = self._smoothed_right_elbow + if self._settings.track_elbow_ik_targets: + if self._settings.track_aligned_mocap_wrists: + left_elbow_target = _elbow_target_from_aligned_skeleton( + frame, calib, self._settings, is_left=True + ) + right_elbow_target = _elbow_target_from_aligned_skeleton( + frame, calib, self._settings, is_left=False + ) + else: + yaw_delta = _resolve_yaw_delta( + _compute_torso_yaw(torso) - calib.body_yaw_isaac, self._settings + ) + left_elbow_target = _solve_elbow_target( + arm=left, + neutral=calib.left, + nominal=calib.nominal_left_elbow, + settings=self._settings, + yaw_delta=yaw_delta, + is_left=True, + ) + right_elbow_target = _solve_elbow_target( + arm=right, + neutral=calib.right, + nominal=calib.nominal_right_elbow, + settings=self._settings, + yaw_delta=yaw_delta, + is_left=False, + ) + if left_elbow_target is None or right_elbow_target is None: + return None + self._smoothed_left_elbow = _smooth_pose( + self._smoothed_left_elbow, + left_elbow_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + self._smoothed_right_elbow = _smooth_pose( + self._smoothed_right_elbow, + right_elbow_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + left_shoulder_target = self._smoothed_left_shoulder + right_shoulder_target = self._smoothed_right_shoulder + if self._settings.track_shoulder_ik_targets: + if self._settings.track_aligned_mocap_wrists: + left_shoulder_target = _shoulder_target_from_aligned_skeleton( + frame, calib, self._settings, is_left=True + ) + right_shoulder_target = _shoulder_target_from_aligned_skeleton( + frame, calib, self._settings, is_left=False + ) + else: + yaw_delta = _resolve_yaw_delta( + _compute_torso_yaw(torso) - calib.body_yaw_isaac, self._settings + ) + left_shoulder_target = _solve_shoulder_target( + arm=left, + neutral=calib.left, + nominal=calib.nominal_left_shoulder, + settings=self._settings, + yaw_delta=yaw_delta, + is_left=True, + ) + right_shoulder_target = _solve_shoulder_target( + arm=right, + neutral=calib.right, + nominal=calib.nominal_right_shoulder, + settings=self._settings, + yaw_delta=yaw_delta, + is_left=False, + ) + if left_shoulder_target is None or right_shoulder_target is None: + return None + self._smoothed_left_shoulder = _smooth_pose( + self._smoothed_left_shoulder, + left_shoulder_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + self._smoothed_right_shoulder = _smooth_pose( + self._smoothed_right_shoulder, + right_shoulder_target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + return self.current_arm_targets + + def input_spec(self) -> RetargeterIOType: + return {"full_body_tracked": DeviceIOFullBodyPosePNTracked()} + + def output_spec(self) -> RetargeterIOType: + wrist_type = NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + return { + "left_wrist": TensorGroupType("left_wrist", [wrist_type]), + "right_wrist": TensorGroupType("right_wrist", [wrist_type]), + "body_yaw_delta": TensorGroupType( + "body_yaw_delta", + [NDArrayType("yaw", shape=(1,), dtype=DLDataType.FLOAT, dtype_bits=32)], + ), + } + + def _compute_fn( + self, + inputs: RetargeterIO, + outputs: RetargeterIO, + context: ComputeContext, + ) -> None: + if context.execution_events.reset: + self.clear_calibration() + + tracked = inputs["full_body_tracked"][0] + frame = tracked.data + + if frame is not None: + if self._calibration is None: + self.calibrate(frame) + else: + self.retarget(frame) + + outputs["left_wrist"][0] = self._smoothed_left.as_action_pose() + outputs["right_wrist"][0] = self._smoothed_right.as_action_pose() + outputs["body_yaw_delta"][0] = np.float32(self.body_yaw_delta) + + +def noitom_position_to_isaac(position: np.ndarray) -> np.ndarray: + """Convert a Noitom Y-up position vector to Isaac Z-up.""" + return (_NOITOM_TO_ISAAC @ np.asarray(position, dtype=np.float64)).astype( + np.float64 + ) + + +def noitom_quaternion_to_isaac(quaternion_xyzw: np.ndarray) -> np.ndarray: + """Convert a Noitom Y-up quaternion (xyzw) to Isaac Z-up.""" + rot = Rotation.from_quat(_normalize_quat(quaternion_xyzw)) + return _normalize_quat((_COORD_ROT * rot * _COORD_ROT.inv()).as_quat()) + + +def map_point_to_robot_frame( + noitom_point_yup: np.ndarray, + pelvis_yup: np.ndarray, + calib: NoitomCalibrationView | None, + current_yaw: float, + robot_pelvis: np.ndarray, + draw_scale: float = 1.0, + *, + operator_faces_robot: bool = True, + length_scale: float | None = None, +) -> np.ndarray: + """Map a Noitom joint position into the robot simulation frame for debug draw.""" + point = noitom_position_to_isaac(noitom_point_yup) + pelvis = noitom_position_to_isaac(pelvis_yup) + rel = point - pelvis + if calib is None: + offset = rel * draw_scale + if operator_faces_robot: + offset = Rotation.from_euler("z", np.pi).apply(offset) + return robot_pelvis + offset + yaw_delta = current_yaw - calib.body_yaw_isaac + scale = calib.arm_length_scale if length_scale is None else length_scale + offset = _map_mocap_rel_to_robot_offset( + rel, + scale, + draw_scale, + yaw_delta, + operator_faces_robot, + ) + return robot_pelvis + offset + + +def _normalize_quat(quat_xyzw: np.ndarray) -> np.ndarray: + quat = np.asarray(quat_xyzw, dtype=np.float64) + norm = np.linalg.norm(quat) + if norm < 1e-8: + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + return quat / norm + + +def _point_to_array(point: Any) -> np.ndarray: + return np.array([point.x, point.y, point.z], dtype=np.float64) + + +def _quat_to_array(point: Any) -> np.ndarray: + return _normalize_quat( + np.array([point.x, point.y, point.z, point.w], dtype=np.float64) + ) + + +def _joint_pose(frame: Any, joint_index: BodyJointPN | int) -> SE3Pose | None: + if frame.joints is None: + return None + joint = frame.joints.joints(int(joint_index)) + if not joint.is_valid: + return None + pos = _point_to_array(joint.pose.position) + quat = _quat_to_array(joint.pose.orientation) + if not np.all(np.isfinite(pos)) or not np.all(np.isfinite(quat)): + return None + return SE3Pose( + noitom_position_to_isaac(pos), + noitom_quaternion_to_isaac(quat), + ) + + +def _build_torso_frame(frame: Any) -> _TorsoFrame | None: + pelvis = _joint_pose(frame, BodyJointPN.PELVIS) + spine = _joint_pose(frame, BodyJointPN.SPINE3) + left_shoulder = _joint_pose(frame, BodyJointPN.LEFT_SHOULDER) + right_shoulder = _joint_pose(frame, BodyJointPN.RIGHT_SHOULDER) + if ( + pelvis is None + or spine is None + or left_shoulder is None + or right_shoulder is None + ): + return None + + up = spine.position - pelvis.position + right = right_shoulder.position - left_shoulder.position + if np.linalg.norm(up) < 1e-5 or np.linalg.norm(right) < 1e-5: + return None + up /= np.linalg.norm(up) + right /= np.linalg.norm(right) + forward = np.cross(up, right) + if np.linalg.norm(forward) < 1e-5: + return None + forward /= np.linalg.norm(forward) + right = np.cross(forward, up) + right /= np.linalg.norm(right) + rotation = Rotation.from_matrix(np.column_stack([right, forward, up])) + return _TorsoFrame(origin=spine.position.copy(), rotation=rotation) + + +def _compute_torso_yaw(torso: _TorsoFrame) -> float: + forward = torso.rotation.as_matrix()[:, 1] + return float(np.arctan2(forward[1], forward[0])) + + +def _pose_to_torso(pose: SE3Pose, torso: _TorsoFrame) -> tuple[np.ndarray, Rotation]: + pos_torso = torso.rotation.inv().apply(pose.position - torso.origin) + rot_torso = torso.rotation.inv() * Rotation.from_quat(pose.quaternion_xyzw) + return pos_torso, rot_torso + + +def _parse_arm( + frame: Any, + torso: _TorsoFrame, + pelvis_world: np.ndarray, + shoulder_index: BodyJointPN, + elbow_index: BodyJointPN, + wrist_index: BodyJointPN, +) -> _ArmCalibration | None: + shoulder = _joint_pose(frame, shoulder_index) + elbow = _joint_pose(frame, elbow_index) + wrist = _joint_pose(frame, wrist_index) + if shoulder is None or elbow is None or wrist is None: + return None + + upper_arm_length = float(np.linalg.norm(elbow.position - shoulder.position)) + forearm_length = float(np.linalg.norm(wrist.position - elbow.position)) + if upper_arm_length < 1e-4 or forearm_length < 1e-4: + return None + + wrist_pos_torso, wrist_rot_torso = _pose_to_torso(wrist, torso) + shoulder_torso = torso.rotation.inv().apply(shoulder.position - torso.origin) + wrist_rel_pelvis = wrist.position - pelvis_world + return _ArmCalibration( + shoulder_torso=shoulder_torso, + shoulder_world=shoulder.position.copy(), + elbow_world=elbow.position.copy(), + wrist_pos_torso=wrist_pos_torso, + wrist_rot_torso=wrist_rot_torso, + wrist_rel_pelvis=wrist_rel_pelvis, + wrist_world=wrist.position.copy(), + upper_arm_length=upper_arm_length, + forearm_length=forearm_length, + ) + + +def _parse_upper_body( + frame: Any, +) -> tuple[_TorsoFrame, _ArmCalibration, _ArmCalibration, np.ndarray] | None: + pelvis_pose = _joint_pose(frame, BodyJointPN.PELVIS) + torso = _build_torso_frame(frame) + if pelvis_pose is None or torso is None: + return None + pelvis_world = pelvis_pose.position.copy() + left = _parse_arm( + frame, + torso, + pelvis_world, + BodyJointPN.LEFT_SHOULDER, + BodyJointPN.LEFT_ELBOW, + BodyJointPN.LEFT_WRIST, + ) + right = _parse_arm( + frame, + torso, + pelvis_world, + BodyJointPN.RIGHT_SHOULDER, + BodyJointPN.RIGHT_ELBOW, + BodyJointPN.RIGHT_WRIST, + ) + if left is None or right is None: + return None + return torso, left, right, pelvis_world + + +def _resolve_yaw_delta(yaw_delta: float, settings: NoitomRetargetingSettings) -> float: + """Limit torso twist fed into arm FK (prevents waist+arm IK deadlock).""" + influenced = yaw_delta * settings.torso_yaw_arm_influence + limit = settings.max_torso_yaw_delta + return float(np.clip(influenced, -limit, limit)) + + +def _map_mocap_direction_to_robot( + direction_isaac: np.ndarray, + yaw_delta: float, + operator_faces_robot: bool, +) -> np.ndarray: + """Rotate a unit bone direction from mocap into the robot frame (no length scale).""" + direction = np.asarray(direction_isaac, dtype=np.float64) + norm = float(np.linalg.norm(direction)) + if norm < 1e-6: + return np.array([1.0, 0.0, 0.0], dtype=np.float64) + direction = direction / norm + if operator_faces_robot: + direction = Rotation.from_euler("z", np.pi).apply(direction) + return Rotation.from_euler("z", yaw_delta).apply(direction) + + +def _shoulder_world_robot( + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> np.ndarray: + anchor = settings.robot_pelvis_world.astype(np.float64) + return anchor + _shoulder_offset_robot(settings, yaw_delta, is_left) + + +def _arm_fk_robot( + arm: _ArmCalibration, + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """FK along mocap bone directions using G1 upper-arm and forearm lengths.""" + shoulder_robot = _shoulder_world_robot(settings, yaw_delta, is_left) + upper_dir = _map_mocap_direction_to_robot( + arm.elbow_world - arm.shoulder_world, + yaw_delta, + settings.operator_faces_robot, + ) + forearm_dir = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + upper_len = settings.robot_upper_arm_length + forearm_len = settings.robot_forearm_length + elbow_robot = shoulder_robot + upper_dir * upper_len + wrist_robot = elbow_robot + forearm_dir * forearm_len + return shoulder_robot, elbow_robot, wrist_robot + + +def _slerp_unit_direction( + neutral_dir: np.ndarray, full_dir: np.ndarray, blend: float +) -> np.ndarray: + """Interpolate unit bone directions (keeps FK chain valid after FK).""" + t = float(np.clip(blend, 0.0, 1.0)) + mixed = (1.0 - t) * neutral_dir + t * full_dir + norm = float(np.linalg.norm(mixed)) + if norm < 1e-6: + return full_dir + return mixed / norm + + +def _arm_direction_dot(upper_dir: np.ndarray, forearm_dir: np.ndarray) -> float: + return float(np.dot(upper_dir, forearm_dir)) + + +def _unit_direction( + vector: np.ndarray, fallback: np.ndarray | None = None +) -> np.ndarray: + vec = np.asarray(vector, dtype=np.float64) + norm = float(np.linalg.norm(vec)) + if norm < 1e-6: + if fallback is not None: + return _unit_direction(fallback) + return np.array([1.0, 0.0, 0.0], dtype=np.float64) + return vec / norm + + +def _elbow_interior_angle(upper_dir: np.ndarray, forearm_dir: np.ndarray) -> float: + """Angle (rad) between upper-arm and forearm unit directions; 0 = fully extended.""" + dot = float( + np.clip( + _arm_direction_dot( + _unit_direction(upper_dir), _unit_direction(forearm_dir) + ), + -1.0, + 1.0, + ) + ) + return float(np.arccos(dot)) + + +def _forearm_dir_from_elbow_angle( + upper_dir: np.ndarray, + forearm_hint: np.ndarray, + elbow_angle: float, +) -> np.ndarray: + """Rebuild a forearm unit vector with the given interior elbow angle.""" + upper = _unit_direction(upper_dir) + hint = _unit_direction(forearm_hint, fallback=upper) + perp = hint - upper * float(np.dot(hint, upper)) + perp_norm = float(np.linalg.norm(perp)) + if perp_norm < 1e-6: + perp = np.cross(upper, np.array([0.0, 0.0, 1.0], dtype=np.float64)) + perp_norm = float(np.linalg.norm(perp)) + if perp_norm < 1e-6: + perp = np.cross(upper, np.array([0.0, 1.0, 0.0], dtype=np.float64)) + perp_norm = float(np.linalg.norm(perp)) + perp = perp / max(perp_norm, 1e-8) + angle = float(np.clip(elbow_angle, 0.0, np.pi - 1e-3)) + forearm = np.cos(angle) * upper + np.sin(angle) * perp + return _unit_direction(forearm, fallback=upper) + + +def _human_robot_reach_scale( + arm: _ArmCalibration, settings: NoitomRetargetingSettings +) -> float: + """Shrink motion when the operator arm is longer than the fixed G1 chain.""" + human_reach = arm.upper_arm_length + arm.forearm_length + robot_reach = settings.robot_upper_arm_length + settings.robot_forearm_length + if human_reach <= robot_reach + 1e-4: + return 1.0 + return float( + np.clip(robot_reach * settings.human_reach_margin / human_reach, 0.25, 1.0) + ) + + +def _effective_motion_scale( + base_scale: float, extension_dot: float, soft_limit: float +) -> float: + """Shrink motion when the target arm chain is near full extension.""" + scale = float(np.clip(base_scale, 0.0, 1.0)) + if extension_dot <= soft_limit: + return scale + penalty = (extension_dot - soft_limit) / max(1e-3, 1.0 - soft_limit) + return scale * (1.0 - 0.7 * float(np.clip(penalty, 0.0, 1.0))) + + +def _bend_forearm_direction( + upper_dir: np.ndarray, forearm_dir: np.ndarray, target_dot: float = 0.55 +) -> np.ndarray: + """Pull forearm direction off a straight line to avoid elbow singularities.""" + upper = upper_dir / (np.linalg.norm(upper_dir) + 1e-8) + forearm = forearm_dir / (np.linalg.norm(forearm_dir) + 1e-8) + if _arm_direction_dot(upper, forearm) <= target_dot: + return forearm + axis = np.cross(upper, np.array([0.0, 0.0, 1.0], dtype=np.float64)) + if float(np.linalg.norm(axis)) < 1e-4: + axis = np.cross(upper, np.array([0.0, 1.0, 0.0], dtype=np.float64)) + axis = axis / (np.linalg.norm(axis) + 1e-8) + bent = forearm.copy() + for _ in range(8): + if _arm_direction_dot(upper, bent) <= target_dot: + break + bent = Rotation.from_rotvec(axis * 0.12).apply(bent) + bent = bent / (np.linalg.norm(bent) + 1e-8) + return bent + + +def _arm_chain_from_directions( + shoulder_robot: np.ndarray, + upper_dir: np.ndarray, + forearm_dir: np.ndarray, + settings: NoitomRetargetingSettings, +) -> tuple[np.ndarray, np.ndarray]: + upper_len = settings.robot_upper_arm_length + forearm_len = settings.robot_forearm_length + elbow = shoulder_robot + upper_dir * upper_len + wrist = elbow + forearm_dir * forearm_len + wrist = _clamp_reach(shoulder_robot, wrist, upper_len, forearm_len) + fore_vec = wrist - elbow + fore_dist = float(np.linalg.norm(fore_vec)) + if fore_dist > 1e-6 and fore_dist > forearm_len: + wrist = elbow + fore_vec * (forearm_len / fore_dist) + return elbow, wrist + + +def _arm_fk_robot_blended( + arm: _ArmCalibration, + neutral_arm: _ArmCalibration, + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """Blend mocap posture (bone directions + elbow angle), then FK with G1 link lengths.""" + shoulder_robot = _shoulder_world_robot(settings, yaw_delta, is_left) + upper_n = _map_mocap_direction_to_robot( + neutral_arm.elbow_world - neutral_arm.shoulder_world, + yaw_delta, + settings.operator_faces_robot, + ) + upper_f = _map_mocap_direction_to_robot( + arm.elbow_world - arm.shoulder_world, + yaw_delta, + settings.operator_faces_robot, + ) + fore_n = _map_mocap_direction_to_robot( + neutral_arm.wrist_world - neutral_arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + fore_f = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + scale = _effective_motion_scale( + settings.motion_scale, + _arm_direction_dot(upper_f, fore_f), + settings.arm_extension_soft_limit, + ) + yaw_factor = 1.0 - 0.45 * min( + 1.0, abs(yaw_delta) / max(settings.max_torso_yaw_delta, 1e-3) + ) + scale *= max(0.25, yaw_factor) + scale *= _human_robot_reach_scale(arm, settings) + upper_d = _slerp_unit_direction(upper_n, upper_f, scale) + + theta_n = _elbow_interior_angle(upper_n, fore_n) + theta_f = _elbow_interior_angle(upper_f, fore_f) + theta_d = (1.0 - scale) * theta_n + scale * theta_f + min_theta = max( + settings.min_elbow_interior_angle, + float(np.arccos(np.clip(settings.arm_extension_soft_limit, -1.0, 1.0))), + ) + theta_d = float(np.clip(max(theta_d, min_theta), min_theta, np.pi - 1e-3)) + fore_d = _forearm_dir_from_elbow_angle(upper_d, fore_f, theta_d) + + if _arm_direction_dot(upper_d, fore_d) > settings.arm_extension_soft_limit: + fore_d = _bend_forearm_direction(upper_d, fore_d) + + elbow_neutral, wrist_neutral = _arm_chain_from_directions( + shoulder_robot, upper_n, fore_n, settings + ) + elbow_full, wrist_full = _arm_chain_from_directions( + shoulder_robot, upper_d, fore_d, settings + ) + elbow_robot = elbow_neutral + scale * (elbow_full - elbow_neutral) + wrist_robot = wrist_neutral + scale * (wrist_full - wrist_neutral) + wrist_robot = _clamp_reach( + shoulder_robot, + wrist_robot, + settings.robot_upper_arm_length, + settings.robot_forearm_length, + ) + return shoulder_robot, elbow_robot, wrist_robot + + +def _wrist_quat_from_forearm( + forearm_dir: np.ndarray, + nominal_quat: np.ndarray, + blend: float, +) -> np.ndarray: + """Derive a wrist quaternion consistent with the solved forearm direction.""" + forward = np.asarray(forearm_dir, dtype=np.float64) + norm = float(np.linalg.norm(forward)) + if norm < 1e-6: + return _normalize_quat(nominal_quat) + forward = forward / norm + world_up = np.array([0.0, 0.0, 1.0], dtype=np.float64) + if abs(float(np.dot(forward, world_up))) > 0.92: + world_up = np.array([0.0, 1.0, 0.0], dtype=np.float64) + right = np.cross(forward, world_up) + right_norm = float(np.linalg.norm(right)) + if right_norm < 1e-6: + return _normalize_quat(nominal_quat) + right = right / right_norm + up = np.cross(right, forward) + aligned = Rotation.from_matrix(np.column_stack([right, forward, up])) + blend_clamped = float(np.clip(blend, 0.0, 1.0)) + if blend_clamped <= 0.0: + return _normalize_quat(nominal_quat) + if blend_clamped >= 1.0: + return _normalize_quat(aligned.as_quat()) + nominal_rot = Rotation.from_quat(nominal_quat) + slerp = Slerp( + [0.0, 1.0], + Rotation.concatenate([nominal_rot, aligned]), + ) + return _normalize_quat(slerp([blend_clamped]).as_quat()[0]) + + +def _wrist_pose_from_posture( + arm: _ArmCalibration, + settings: NoitomRetargetingSettings, + is_left: bool, + yaw_delta: float, + nominal_quat: np.ndarray, +) -> SE3Pose: + _shoulder_robot, _elbow_robot, wrist_robot = _arm_fk_robot( + arm, settings, yaw_delta, is_left + ) + forearm_dir = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + quat = _wrist_quat_for_ik( + forearm_dir, nominal_quat, settings, track_orientation=False + ) + return SE3Pose(wrist_robot, quat) + + +def _wrist_quat_for_ik( + forearm_dir: np.ndarray, + nominal_quat: np.ndarray, + settings: NoitomRetargetingSettings, + track_orientation: bool, +) -> np.ndarray: + if track_orientation: + return _normalize_quat(nominal_quat) + return _wrist_quat_from_forearm( + forearm_dir, + nominal_quat, + settings.wrist_orientation_forearm_blend, + ) + + +def compute_robot_reference_positions( + frame: Any, + settings: NoitomRetargetingSettings, + calib: NoitomCalibrationView, + current_yaw: float, + neutral_left: _ArmCalibration, + neutral_right: _ArmCalibration, +) -> dict[int, np.ndarray]: + """Build a robot-proportioned reference skeleton (posture, not scaled joint dots).""" + parsed = _parse_upper_body(frame) + if parsed is None: + return {} + torso, left, right, _pelvis_world = parsed + yaw_delta = _resolve_yaw_delta(current_yaw - calib.body_yaw_isaac, settings) + anchor = settings.robot_pelvis_world.astype(np.float64) + positions: dict[int, np.ndarray] = {int(BodyJointPN.PELVIS): anchor.copy()} + + _fill_torso_reference_positions(frame, positions, anchor, yaw_delta, settings) + + for arm, neutral_arm, is_left, shoulder_idx, elbow_idx, wrist_idx, hand_idx in ( + ( + left, + neutral_left, + True, + BodyJointPN.LEFT_SHOULDER, + BodyJointPN.LEFT_ELBOW, + BodyJointPN.LEFT_WRIST, + BodyJointPN.LEFT_HAND, + ), + ( + right, + neutral_right, + False, + BodyJointPN.RIGHT_SHOULDER, + BodyJointPN.RIGHT_ELBOW, + BodyJointPN.RIGHT_WRIST, + BodyJointPN.RIGHT_HAND, + ), + ): + shoulder_robot, elbow_robot, wrist_robot = _arm_fk_robot_blended( + arm, neutral_arm, settings, yaw_delta, is_left + ) + positions[int(shoulder_idx)] = shoulder_robot + positions[int(elbow_idx)] = elbow_robot + positions[int(wrist_idx)] = wrist_robot + forearm = wrist_robot - elbow_robot + forearm_norm = float(np.linalg.norm(forearm)) + if forearm_norm > 1e-6: + forearm_dir = forearm / forearm_norm + else: + forearm_dir = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + positions[int(hand_idx)] = wrist_robot + forearm_dir * _ROBOT_HAND_EXTENSION + + spine3 = positions.get(int(BodyJointPN.SPINE3)) + if spine3 is not None: + for collar_idx, shoulder_idx in ( + (BodyJointPN.LEFT_COLLAR, BodyJointPN.LEFT_SHOULDER), + (BodyJointPN.RIGHT_COLLAR, BodyJointPN.RIGHT_SHOULDER), + ): + shoulder_pos = positions.get(int(shoulder_idx)) + if shoulder_pos is not None: + positions[int(collar_idx)] = 0.5 * (spine3 + shoulder_pos) + + return positions + + +def _fill_torso_reference_positions( + frame: Any, + positions: dict[int, np.ndarray], + anchor: np.ndarray, + yaw_delta: float, + settings: NoitomRetargetingSettings, +) -> None: + chain = ( + BodyJointPN.PELVIS, + BodyJointPN.SPINE1, + BodyJointPN.SPINE2, + BodyJointPN.SPINE3, + BodyJointPN.NECK, + BodyJointPN.HEAD, + ) + segment_lengths = { + BodyJointPN.SPINE1: _ROBOT_TORSO_SEGMENT_Z, + BodyJointPN.SPINE2: _ROBOT_TORSO_SEGMENT_Z, + BodyJointPN.SPINE3: _ROBOT_TORSO_SEGMENT_Z, + BodyJointPN.NECK: _ROBOT_NECK_SEGMENT, + BodyJointPN.HEAD: _ROBOT_HEAD_SEGMENT, + } + prev_robot = anchor.copy() + prev_mocap = _joint_pose(frame, BodyJointPN.PELVIS) + if prev_mocap is None: + return + prev_mocap_pos = prev_mocap.position.copy() + for joint in chain[1:]: + mocap_joint = _joint_pose(frame, joint) + if mocap_joint is None: + continue + direction = _map_mocap_direction_to_robot( + mocap_joint.position - prev_mocap_pos, + yaw_delta, + settings.operator_faces_robot, + ) + seg_len = segment_lengths.get(joint, _ROBOT_TORSO_SEGMENT_Z) + robot_pos = prev_robot + direction * seg_len + positions[int(joint)] = robot_pos + prev_robot = robot_pos + prev_mocap_pos = mocap_joint.position.copy() + + +def _wrist_pose_from_pelvis_relative( + wrist_rel_pelvis: np.ndarray, + arm_length_scale: float, + settings: NoitomRetargetingSettings, + nominal_quat_xyzw: np.ndarray, +) -> SE3Pose: + anchor = settings.robot_pelvis_world.astype(np.float64) + offset = _map_mocap_rel_to_robot_offset( + wrist_rel_pelvis, + arm_length_scale, + settings.motion_scale, + 0.0, + settings.operator_faces_robot, + ) + position = anchor + offset + quat = _normalize_quat(nominal_quat_xyzw) + return SE3Pose(position, quat) + + +def _map_mocap_rel_to_robot_offset( + rel_isaac: np.ndarray, + arm_length_scale: float, + motion_scale: float, + yaw_delta: float, + operator_faces_robot: bool, +) -> np.ndarray: + """Map a pelvis-relative mocap vector into robot pelvis-relative offset.""" + rel = np.asarray(rel_isaac, dtype=np.float64) * arm_length_scale * motion_scale + if operator_faces_robot: + rel = Rotation.from_euler("z", np.pi).apply(rel) + return Rotation.from_euler("z", yaw_delta).apply(rel) + + +def _shoulder_offset_robot( + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> np.ndarray: + offset = ( + settings.robot_left_shoulder_offset + if is_left + else settings.robot_right_shoulder_offset + ) + return Rotation.from_euler("z", yaw_delta).apply( + np.asarray(offset, dtype=np.float64) + ) + + +def _clamp_reach( + shoulder_torso: np.ndarray, + target_torso: np.ndarray, + upper_len: float, + forearm_len: float, +) -> np.ndarray: + offset = target_torso - shoulder_torso + distance = float(np.linalg.norm(offset)) + max_reach = (upper_len + forearm_len) * 0.98 + min_reach = abs(upper_len - forearm_len) * 1.02 + if distance < 1e-6: + return shoulder_torso + np.array([max_reach * 0.5, 0.0, 0.0], dtype=np.float64) + clamped = float(np.clip(distance, min_reach, max_reach)) + return shoulder_torso + offset * (clamped / distance) + + +def _calibration_view_from_state(calib: _CalibrationState) -> NoitomCalibrationView: + return NoitomCalibrationView( + pelvis_world=calib.pelvis_world.copy(), + body_yaw_isaac=calib.body_yaw_isaac, + arm_length_scale=calib.arm_length_scale, + body_height_scale=calib.body_height_scale, + ) + + +def _calibration_view_from_scales( + arm_length_scale: float, + body_height_scale: float, + pelvis_world: np.ndarray, + body_yaw_isaac: float = 0.0, +) -> NoitomCalibrationView: + return NoitomCalibrationView( + pelvis_world=pelvis_world.copy(), + body_yaw_isaac=body_yaw_isaac, + arm_length_scale=arm_length_scale, + body_height_scale=body_height_scale, + ) + + +def _aligned_skeleton_positions( + frame: Any, + settings: NoitomRetargetingSettings, + calib_view: NoitomCalibrationView, +) -> dict[int, np.ndarray]: + from noitom_reference_draw import ( + ReferenceSkeletonLengths, + aligned_reference_skeleton_from_frame, + ) + + return aligned_reference_skeleton_from_frame( + frame, + settings.robot_pelvis_world, + draw_scale=1.0, + calib_view=(None if settings.reference_use_robot_link_lengths else calib_view), + use_robot_link_lengths=settings.reference_use_robot_link_lengths, + link_lengths=ReferenceSkeletonLengths.from_retargeting_settings(settings), + length_scale=settings.reference_length_scale, + arm_length_scale=settings.reference_arm_length_scale, + shoulder_span_scale=settings.reference_shoulder_span_scale, + ) + + +def _shoulder_se3_from_aligned_positions( + positions: dict[int, np.ndarray], + is_left: bool, + nominal: SE3Pose, + settings: NoitomRetargetingSettings, +) -> SE3Pose | None: + shoulder_index = int( + BodyJointPN.LEFT_SHOULDER if is_left else BodyJointPN.RIGHT_SHOULDER + ) + elbow_index = int(BodyJointPN.LEFT_ELBOW if is_left else BodyJointPN.RIGHT_ELBOW) + shoulder = positions.get(shoulder_index) + if shoulder is None: + return None + elbow = positions.get(elbow_index) + if elbow is not None: + upper_arm = elbow - shoulder + quat = _elbow_quat_for_ik( + upper_arm, + nominal.quaternion_xyzw, + settings, + ) + else: + quat = _normalize_quat(nominal.quaternion_xyzw) + return SE3Pose(shoulder.copy(), quat) + + +def _elbow_quat_for_ik( + upper_arm_dir: np.ndarray, + nominal_quat: np.ndarray, + settings: NoitomRetargetingSettings, +) -> np.ndarray: + return _wrist_quat_from_forearm( + upper_arm_dir, + nominal_quat, + settings.wrist_orientation_forearm_blend, + ) + + +def _elbow_se3_from_aligned_positions( + positions: dict[int, np.ndarray], + is_left: bool, + nominal: SE3Pose, + settings: NoitomRetargetingSettings, +) -> SE3Pose | None: + elbow_index = int(BodyJointPN.LEFT_ELBOW if is_left else BodyJointPN.RIGHT_ELBOW) + shoulder_index = int( + BodyJointPN.LEFT_SHOULDER if is_left else BodyJointPN.RIGHT_SHOULDER + ) + elbow = positions.get(elbow_index) + if elbow is None: + return None + shoulder = positions.get(shoulder_index) + if shoulder is not None: + upper_arm = elbow - shoulder + quat = _elbow_quat_for_ik( + upper_arm, + nominal.quaternion_xyzw, + settings, + ) + else: + quat = _normalize_quat(nominal.quaternion_xyzw) + return SE3Pose(elbow.copy(), quat) + + +def _wrist_se3_from_aligned_positions( + positions: dict[int, np.ndarray], + is_left: bool, + nominal: SE3Pose, + settings: NoitomRetargetingSettings, +) -> SE3Pose | None: + wrist_index = int(BodyJointPN.LEFT_WRIST if is_left else BodyJointPN.RIGHT_WRIST) + elbow_index = int(BodyJointPN.LEFT_ELBOW if is_left else BodyJointPN.RIGHT_ELBOW) + wrist = positions.get(wrist_index) + if wrist is None: + return None + elbow = positions.get(elbow_index) + if elbow is not None: + forearm = wrist - elbow + quat = _wrist_quat_for_ik( + forearm, + nominal.quaternion_xyzw, + settings, + track_orientation=False, + ) + else: + quat = _normalize_quat(nominal.quaternion_xyzw) + return SE3Pose(wrist.copy(), quat) + + +def _nominal_shoulders_from_aligned_frame( + frame: Any, + settings: NoitomRetargetingSettings, + arm_length_scale: float, + body_height_scale: float, + pelvis_world: np.ndarray, +) -> tuple[SE3Pose, SE3Pose] | None: + calib_view = _calibration_view_from_scales( + arm_length_scale, body_height_scale, pelvis_world + ) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None + default_left = SE3Pose.from_nominal( + np.zeros(3, dtype=np.float64), + settings.nominal_left_wrist_quat_xyzw, + ) + default_right = SE3Pose.from_nominal( + np.zeros(3, dtype=np.float64), + settings.nominal_right_wrist_quat_xyzw, + ) + left = _shoulder_se3_from_aligned_positions(positions, True, default_left, settings) + right = _shoulder_se3_from_aligned_positions( + positions, False, default_right, settings + ) + if left is None or right is None: + return None + return left, right + + +def _nominal_elbows_from_aligned_frame( + frame: Any, + settings: NoitomRetargetingSettings, + arm_length_scale: float, + body_height_scale: float, + pelvis_world: np.ndarray, +) -> tuple[SE3Pose, SE3Pose] | None: + calib_view = _calibration_view_from_scales( + arm_length_scale, body_height_scale, pelvis_world + ) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None + default_left = SE3Pose.from_nominal( + np.zeros(3, dtype=np.float64), + settings.nominal_left_wrist_quat_xyzw, + ) + default_right = SE3Pose.from_nominal( + np.zeros(3, dtype=np.float64), + settings.nominal_right_wrist_quat_xyzw, + ) + left = _elbow_se3_from_aligned_positions(positions, True, default_left, settings) + right = _elbow_se3_from_aligned_positions(positions, False, default_right, settings) + if left is None or right is None: + return None + return left, right + + +def _nominal_wrists_from_aligned_frame( + frame: Any, + settings: NoitomRetargetingSettings, + arm_length_scale: float, + body_height_scale: float, + pelvis_world: np.ndarray, +) -> tuple[SE3Pose | None, SE3Pose | None]: + calib_view = _calibration_view_from_scales( + arm_length_scale, body_height_scale, pelvis_world + ) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None, None + default_left = SE3Pose.from_nominal( + settings.nominal_left_wrist_pos, settings.nominal_left_wrist_quat_xyzw + ) + default_right = SE3Pose.from_nominal( + settings.nominal_right_wrist_pos, settings.nominal_right_wrist_quat_xyzw + ) + return ( + _wrist_se3_from_aligned_positions(positions, True, default_left, settings), + _wrist_se3_from_aligned_positions(positions, False, default_right, settings), + ) + + +def _shoulder_target_from_aligned_skeleton( + frame: Any, + calib: _CalibrationState, + settings: NoitomRetargetingSettings, + is_left: bool, +) -> SE3Pose | None: + calib_view = _calibration_view_from_state(calib) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None + nominal = calib.nominal_left_shoulder if is_left else calib.nominal_right_shoulder + return _shoulder_se3_from_aligned_positions(positions, is_left, nominal, settings) + + +def _solve_shoulder_target( + arm: _ArmCalibration, + neutral: _ArmCalibration, + nominal: SE3Pose, + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> SE3Pose: + shoulder_robot, _elbow_robot, _wrist_robot = _arm_fk_robot_blended( + arm, neutral, settings, yaw_delta, is_left + ) + upper_arm = _elbow_robot - shoulder_robot + upper_norm = float(np.linalg.norm(upper_arm)) + if upper_norm > 1e-6: + upper_dir = upper_arm / upper_norm + else: + upper_dir = _map_mocap_direction_to_robot( + arm.elbow_world - arm.shoulder_world, + yaw_delta, + settings.operator_faces_robot, + ) + quat = _elbow_quat_for_ik(upper_dir, nominal.quaternion_xyzw, settings) + return SE3Pose(shoulder_robot, quat) + + +def _elbow_target_from_aligned_skeleton( + frame: Any, + calib: _CalibrationState, + settings: NoitomRetargetingSettings, + is_left: bool, +) -> SE3Pose | None: + calib_view = _calibration_view_from_state(calib) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None + nominal = calib.nominal_left_elbow if is_left else calib.nominal_right_elbow + return _elbow_se3_from_aligned_positions(positions, is_left, nominal, settings) + + +def _solve_elbow_target( + arm: _ArmCalibration, + neutral: _ArmCalibration, + nominal: SE3Pose, + settings: NoitomRetargetingSettings, + yaw_delta: float, + is_left: bool, +) -> SE3Pose: + _shoulder_robot, elbow_robot, _wrist_robot = _arm_fk_robot_blended( + arm, neutral, settings, yaw_delta, is_left + ) + upper_arm = elbow_robot - _shoulder_world_robot(settings, yaw_delta, is_left) + upper_norm = float(np.linalg.norm(upper_arm)) + if upper_norm > 1e-6: + upper_dir = upper_arm / upper_norm + else: + upper_dir = _map_mocap_direction_to_robot( + arm.elbow_world - arm.shoulder_world, + yaw_delta, + settings.operator_faces_robot, + ) + quat = _elbow_quat_for_ik(upper_dir, nominal.quaternion_xyzw, settings) + return SE3Pose(elbow_robot, quat) + + +def _wrist_target_from_aligned_skeleton( + frame: Any, + calib: _CalibrationState, + settings: NoitomRetargetingSettings, + is_left: bool, +) -> SE3Pose | None: + calib_view = _calibration_view_from_state(calib) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + if not positions: + return None + nominal = calib.nominal_left if is_left else calib.nominal_right + return _wrist_se3_from_aligned_positions(positions, is_left, nominal, settings) + + +def _solve_wrist_target( + torso: _TorsoFrame, + arm: _ArmCalibration, + pelvis_world: np.ndarray, + neutral: _ArmCalibration, + neutral_torso: _TorsoFrame, + nominal: SE3Pose, + calib_yaw: float, + arm_length_scale: float, + settings: NoitomRetargetingSettings, + is_left: bool, +) -> SE3Pose: + yaw_delta = _resolve_yaw_delta(_compute_torso_yaw(torso) - calib_yaw, settings) + + if settings.use_posture_based_arms: + shoulder_robot, elbow_robot, wrist_robot = _arm_fk_robot_blended( + arm, neutral, settings, yaw_delta, is_left + ) + forearm = wrist_robot - elbow_robot + forearm_norm = float(np.linalg.norm(forearm)) + if forearm_norm > 1e-6: + forearm_dir = forearm / forearm_norm + else: + forearm_dir = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + if settings.track_wrist_orientation: + wrist_world_rot = torso.rotation * arm.wrist_rot_torso + wrist_neutral_world_rot = neutral_torso.rotation * neutral.wrist_rot_torso + delta_rot_world = wrist_world_rot * wrist_neutral_world_rot.inv() + nominal_rot = Rotation.from_quat(nominal.quaternion_xyzw) + target_rot = delta_rot_world * nominal_rot + quat = _normalize_quat(target_rot.as_quat()) + else: + quat = _wrist_quat_for_ik( + forearm_dir, + nominal.quaternion_xyzw, + settings, + track_orientation=False, + ) + return SE3Pose(wrist_robot, quat) + + rel_now = arm.wrist_world - pelvis_world + anchor = settings.robot_pelvis_world.astype(np.float64) + off_shoulder = _shoulder_offset_robot(settings, yaw_delta, is_left) + off_wrist = _map_mocap_rel_to_robot_offset( + rel_now, + arm_length_scale, + settings.motion_scale, + yaw_delta, + settings.operator_faces_robot, + ) + clamped = _clamp_reach( + off_shoulder, + off_wrist, + settings.robot_upper_arm_length, + settings.robot_forearm_length, + ) + target_pos = anchor + clamped + + if settings.track_wrist_orientation: + wrist_world_rot = torso.rotation * arm.wrist_rot_torso + wrist_neutral_world_rot = neutral_torso.rotation * neutral.wrist_rot_torso + delta_rot_world = wrist_world_rot * wrist_neutral_world_rot.inv() + nominal_rot = Rotation.from_quat(nominal.quaternion_xyzw) + target_rot = delta_rot_world * nominal_rot + quat = _normalize_quat(target_rot.as_quat()) + else: + quat = _normalize_quat(nominal.quaternion_xyzw) + + return SE3Pose(target_pos, quat) + + +def _smooth_pose( + current: SE3Pose, + target: SE3Pose, + position_alpha: float, + rotation_alpha: float, +) -> SE3Pose: + pos_alpha = float(np.clip(position_alpha, 0.0, 1.0)) + rot_alpha = float(np.clip(rotation_alpha, 0.0, 1.0)) + position = (1.0 - pos_alpha) * current.position + pos_alpha * target.position + if rot_alpha <= 0.0: + quaternion = current.quaternion_xyzw.copy() + elif rot_alpha >= 1.0: + quaternion = target.quaternion_xyzw.copy() + else: + slerp = Slerp( + [0.0, 1.0], + Rotation.from_quat( + np.vstack([current.quaternion_xyzw, target.quaternion_xyzw]) + ), + ) + quaternion = _normalize_quat(slerp([rot_alpha]).as_quat()[0]) + return SE3Pose(position, quaternion) + + +__all__ = [ + "ArmIkTargets", + "NoitomCalibrationView", + "NoitomG1Retargeter", + "NoitomRetargetingSettings", + "SE3Pose", + "compute_robot_reference_positions", + "map_point_to_robot_frame", + "noitom_position_to_isaac", + "noitom_quaternion_to_isaac", +] diff --git a/examples/noitom/noitom_tasks.py b/examples/noitom/noitom_tasks.py new file mode 100644 index 000000000..33e6ce4c0 --- /dev/null +++ b/examples/noitom/noitom_tasks.py @@ -0,0 +1,936 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""External Isaac Lab task registration for Noitom-driven G1 teleop testing.""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import registry + +from isaaclab.utils.configclass import configclass +from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, +) +from isaacteleop.schema import BodyJointPN, FullBodyPosePNT, FullBodyPosePNTrackedT +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPosePNTracked, + IDeviceIOSource, +) +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + OutputCombiner, + RetargeterIO, + RetargeterIOType, + TensorGroup, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import DLDataType, NDArrayType + +from noitom_retargeting import ( + ArmIkTargets, + NoitomG1Retargeter, + NoitomRetargetingSettings, + noitom_position_to_isaac, +) +from noitom_reference_draw import ( + ReferenceSkeletonLengths, + aligned_reference_skeleton_from_frame, +) + +TASK_ID = "Isaac-PickPlace-Locomanipulation-G1-Noitom-Abs-v0" + +_FULL_BODY_INPUT = "deviceio_full_body" +_ACTION_OUTPUT = "action" +_G1_ACTION_DIM_WRIST_ONLY = 32 +_G1_ACTION_DIM_WITH_ARM_IK = 60 +_PINK_PELVIS_LINK = "g1_29dof_with_hand_rev_1_0_pelvis" +_PINK_LEFT_ELBOW_LINK = "g1_29dof_with_hand_rev_1_0_left_elbow_link" +_PINK_RIGHT_ELBOW_LINK = "g1_29dof_with_hand_rev_1_0_right_elbow_link" +_PINK_LEFT_SHOULDER_LINK = "g1_29dof_with_hand_rev_1_0_left_shoulder_pitch_link" +_PINK_RIGHT_SHOULDER_LINK = "g1_29dof_with_hand_rev_1_0_right_shoulder_pitch_link" +_LineList = list[list[float]] +_ColorList = list[tuple[float, float, float, float]] + +_LOCOMOTION_DEFAULT_HIP_HEIGHT = 0.72 + +_NOITOM_REFERENCE_COLOR = (0.15, 0.85, 1.0, 1.0) +_NOITOM_REFERENCE_LINE_THICKNESS = 4.0 +_NOITOM_REFERENCE_JOINT_MARKER_SIZE = 0.018 +# Approximate G1 pelvis height in the locomanipulation scene (meters, Isaac Z-up). +_ROBOT_PELVIS_ANCHOR = (0.0, 0.0, 0.72) +_NOITOM_REFERENCE_DEFAULT_OFFSET = (0.0, 0.0, 0.0) +_NOITOM_WRIST_TARGET_COLOR = (1.0, 0.65, 0.1, 1.0) +_NOITOM_WRIST_TARGET_MARKER_SIZE = 0.025 +_NOITOM_ELBOW_TARGET_COLOR = (1.0, 0.2, 0.85, 1.0) +_NOITOM_ELBOW_TARGET_MARKER_SIZE = 0.022 +_NOITOM_SHOULDER_TARGET_COLOR = (0.2, 1.0, 0.35, 1.0) +_NOITOM_SHOULDER_TARGET_MARKER_SIZE = 0.02 + + +def g1_action_dim(*, use_arm_ik_frame_tasks: bool) -> int: + """Flat teleop action size for the Noitom G1 locomanipulation task.""" + return ( + _G1_ACTION_DIM_WITH_ARM_IK + if use_arm_ik_frame_tasks + else _G1_ACTION_DIM_WRIST_ONLY + ) + + +@dataclass(frozen=True) +class NoitomG1Settings: + """Release defaults for the Noitom-driven G1 locomanipulation example.""" + + collection_id: str = "noitom_mocap" + max_flatbuffer_size: int = 16 * 1024 + teleoperation_active_default: bool = True + enable_motion: bool = True + print_period_s: float = 0.5 + draw_reference: bool = True + draw_scale: float = 1.0 + draw_offset: tuple[float, float, float] = _NOITOM_REFERENCE_DEFAULT_OFFSET + # Anchor the cyan skeleton to the robot pelvis instead of a fixed world offset. + draw_pelvis_relative: bool = True + draw_pelvis_anchor: tuple[float, float, float] = _ROBOT_PELVIS_ANCHOR + draw_wrist_targets: bool = True + draw_elbow_targets: bool = True + draw_shoulder_targets: bool = True + # Wrist + elbow + shoulder LocalFrameTasks for Pink IK (60D action). + use_arm_ik_frame_tasks: bool = True + retargeting: NoitomRetargetingSettings = field( + default_factory=lambda: NoitomRetargetingSettings( + robot_pelvis_world=np.array(_ROBOT_PELVIS_ANCHOR, dtype=np.float64), + motion_scale=0.75, + track_aligned_mocap_wrists=True, + track_elbow_ik_targets=True, + track_shoulder_ik_targets=True, + ) + ) + + +_FULL_BODY_BONES = ( + (BodyJointPN.PELVIS, BodyJointPN.SPINE1), + (BodyJointPN.SPINE1, BodyJointPN.SPINE2), + (BodyJointPN.SPINE2, BodyJointPN.SPINE3), + (BodyJointPN.SPINE3, BodyJointPN.NECK), + (BodyJointPN.NECK, BodyJointPN.HEAD), + (BodyJointPN.SPINE3, BodyJointPN.LEFT_COLLAR), + (BodyJointPN.LEFT_COLLAR, BodyJointPN.LEFT_SHOULDER), + (BodyJointPN.LEFT_SHOULDER, BodyJointPN.LEFT_ELBOW), + (BodyJointPN.LEFT_ELBOW, BodyJointPN.LEFT_WRIST), + (BodyJointPN.LEFT_WRIST, BodyJointPN.LEFT_HAND), + (BodyJointPN.SPINE3, BodyJointPN.RIGHT_COLLAR), + (BodyJointPN.RIGHT_COLLAR, BodyJointPN.RIGHT_SHOULDER), + (BodyJointPN.RIGHT_SHOULDER, BodyJointPN.RIGHT_ELBOW), + (BodyJointPN.RIGHT_ELBOW, BodyJointPN.RIGHT_WRIST), + (BodyJointPN.RIGHT_WRIST, BodyJointPN.RIGHT_HAND), + (BodyJointPN.PELVIS, BodyJointPN.LEFT_HIP), + (BodyJointPN.LEFT_HIP, BodyJointPN.LEFT_KNEE), + (BodyJointPN.LEFT_KNEE, BodyJointPN.LEFT_ANKLE), + (BodyJointPN.LEFT_ANKLE, BodyJointPN.LEFT_FOOT), + (BodyJointPN.PELVIS, BodyJointPN.RIGHT_HIP), + (BodyJointPN.RIGHT_HIP, BodyJointPN.RIGHT_KNEE), + (BodyJointPN.RIGHT_KNEE, BodyJointPN.RIGHT_ANKLE), + (BodyJointPN.RIGHT_ANKLE, BodyJointPN.RIGHT_FOOT), +) +DEFAULT_NOITOM_G1_SETTINGS = NoitomG1Settings() + +# Waist joints stay in IK; clip targets slightly inside URDF hard stops (not fixed narrow ranges). +_WAIST_JOINT_NAMES = frozenset( + {"waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"} +) +# Inset from hard joint limits (rad). Small enough to avoid IK deadlock at the stops. +_WAIST_HARD_LIMIT_MARGIN_RAD = 0.10 + + +def _build_noitom_pink_ik_action_class(): + """Lazy import so unit tests can load noitom_tasks without Isaac Lab.""" + import torch + from isaaclab.envs.mdp.actions.pink_task_space_actions import ( + PinkInverseKinematicsAction, + ) + + class NoitomPinkInverseKinematicsAction(PinkInverseKinematicsAction): + """Pink IK with waist joint targets clamped to teleop-safe ranges.""" + + def __init__(self, cfg, env): + super().__init__(cfg, env) + clip_ids: list[int] = [] + lows: list[float] = [] + highs: list[float] = [] + hard_limits = self._asset.data.joint_pos_limits.torch[0] + name_to_joint_idx = { + name: index for index, name in enumerate(self._asset.data.joint_names) + } + margin = _WAIST_HARD_LIMIT_MARGIN_RAD + for ik_index, name in enumerate(self._isaaclab_controlled_joint_names): + if name not in _WAIST_JOINT_NAMES: + continue + joint_idx = name_to_joint_idx[name] + lo_hard = float(hard_limits[joint_idx, 0]) + hi_hard = float(hard_limits[joint_idx, 1]) + lo = lo_hard + margin + hi = hi_hard - margin + if lo >= hi: + mid = 0.5 * (lo_hard + hi_hard) + half = 0.45 * (hi_hard - lo_hard) + lo, hi = mid - half, mid + half + clip_ids.append(ik_index) + lows.append(lo) + highs.append(hi) + self._waist_ik_indices = clip_ids + if clip_ids: + self._waist_low = torch.tensor(lows, device=self.device).view(1, -1) + self._waist_high = torch.tensor(highs, device=self.device).view(1, -1) + self._waist_ik_idx = torch.tensor( + clip_ids, device=self.device, dtype=torch.long + ) + + def _compute_ik_solutions(self) -> torch.Tensor: + sol = super()._compute_ik_solutions() + if self._waist_ik_indices: + waist = sol.index_select(1, self._waist_ik_idx) + sol.index_copy_( + 1, + self._waist_ik_idx, + torch.clamp(waist, self._waist_low, self._waist_high), + ) + return sol + + return NoitomPinkInverseKinematicsAction + + +def _configure_noitom_pink_ik( + env_cfg: NoitomLocomanipulationG1EnvCfg, use_arm_frames: bool +) -> None: + """Tune Pink IK for Noitom teleop; optionally add elbow/shoulder frame tasks.""" + from isaaclab.controllers.pink_ik import LocalFrameTaskCfg, NullSpacePostureTaskCfg + + env_cfg.actions.upper_body_ik.class_type = _build_noitom_pink_ik_action_class() + controller = env_cfg.actions.upper_body_ik.controller + controller.show_ik_warnings = False + + if use_arm_frames: + tasks = list(controller.variable_input_tasks) + wrist_task_count = sum( + 1 for task in tasks if isinstance(task, LocalFrameTaskCfg) + ) + arm_frame_tasks = [ + LocalFrameTaskCfg( + frame=_PINK_LEFT_ELBOW_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=9.0, + orientation_cost=0.0, + lm_damping=30.0, + gain=0.38, + ), + LocalFrameTaskCfg( + frame=_PINK_RIGHT_ELBOW_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=9.0, + orientation_cost=0.0, + lm_damping=30.0, + gain=0.38, + ), + LocalFrameTaskCfg( + frame=_PINK_LEFT_SHOULDER_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=6.0, + orientation_cost=0.0, + lm_damping=30.0, + gain=0.38, + ), + LocalFrameTaskCfg( + frame=_PINK_RIGHT_SHOULDER_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=6.0, + orientation_cost=0.0, + lm_damping=30.0, + gain=0.38, + ), + ] + controller.variable_input_tasks = ( + tasks[:wrist_task_count] + arm_frame_tasks + tasks[wrist_task_count:] + ) + + for task in controller.variable_input_tasks: + if isinstance(task, LocalFrameTaskCfg): + frame = task.frame + task.gain = 0.38 + task.lm_damping = 30.0 + task.orientation_cost = 0.0 + if "wrist" in frame: + task.position_cost = 18.0 + elif "elbow" in frame: + task.position_cost = 9.0 + elif "shoulder" in frame: + task.position_cost = 6.0 + else: + task.position_cost = 14.0 + elif isinstance(task, NullSpacePostureTaskCfg): + task.cost = 0.05 + task.gain = 0.25 + task.lm_damping = 50.0 + + +def register_tasks() -> list[str]: + """Register the Noitom G1 locomanipulation task with Gymnasium.""" + if TASK_ID not in registry: + gym.register( + id=TASK_ID, + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": ("noitom_tasks:NoitomLocomanipulationG1EnvCfg"), + }, + disable_env_checker=True, + ) + return [] + + +@configclass +class NoitomLocomanipulationG1EnvCfg(LocomanipulationG1EnvCfg): + """G1 locomanipulation config using Noitom mocap as the IsaacTeleop source.""" + + def __post_init__(self) -> None: + """Use the base scene/action config and swap in the Noitom pipeline.""" + super().__post_init__() + self.isaac_teleop.pipeline_builder = build_noitom_g1_locomanipulation_pipeline + # Pelvis fixed: agile lower-body policy otherwise crouches and breaks IK reach. + self.scene.robot.spawn.articulation_props.fix_root_link = True + # Pink IK: wrist primary, elbow/shoulder secondary frame tasks. + # Waist stays in IK (base G1_UPPER_BODY_IK_ACTION_CFG) but is soft-limited + # after solve and biased toward neutral via NullSpacePostureTask. + _configure_noitom_pink_ik( + self, + use_arm_frames=DEFAULT_NOITOM_G1_SETTINGS.use_arm_ik_frame_tasks, + ) + self.isaac_teleop.teleoperation_active_default = ( + DEFAULT_NOITOM_G1_SETTINGS.teleoperation_active_default + ) + self.isaac_teleop.control_channel_uuid = None + self.isaac_teleop.app_name = "IsaacLabNoitomG1" + + +def G1LocomanipulationAction(*, use_arm_ik_frame_tasks: bool = True) -> TensorGroupType: + """G1 locomanipulation action tensor type.""" + return TensorGroupType( + "g1_locomanipulation_action", + [ + NDArrayType( + "action", + shape=(g1_action_dim(use_arm_ik_frame_tasks=use_arm_ik_frame_tasks),), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) + + +class NoitomG1ActionSource(IDeviceIOSource): + """Convert Noitom mocap frames into G1 locomanipulation wrist actions.""" + + def __init__( + self, + name: str = "noitom_g1_action", + settings: NoitomG1Settings = DEFAULT_NOITOM_G1_SETTINGS, + ) -> None: + """Initialize the Noitom DeviceIO tracker and retargeter.""" + import isaacteleop.deviceio as deviceio + + self._tracker = deviceio.FullBodyTrackerPN( + settings.collection_id, settings.max_flatbuffer_size + ) + self._collection_id = settings.collection_id + self._enable_motion = settings.enable_motion + self._print_period_s = max(0.0, settings.print_period_s) + self._last_print_s = 0.0 + self._reference_viz = _NoitomReferenceVisualizer(settings) + self._retargeter = NoitomG1Retargeter(settings.retargeting) + self._use_arm_ik_frame_tasks = settings.use_arm_ik_frame_tasks + self._hold_targets = self._retargeter.current_arm_targets + self._frame_count = 0 + self._calibration_attempts = 0 + self._no_data_count = 0 + self._first_frame_printed = False + self._calibration_fail_count = 0 + super().__init__(name) + + def get_tracker(self): + """Return the Noitom mocap tracker used by this source.""" + return self._tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + """Poll Noitom data from the active DeviceIO session.""" + tracked = self._tracker.get_body_pose(deviceio_session) + group = TensorGroup(self.input_spec()[_FULL_BODY_INPUT]) + group[0] = tracked + return {_FULL_BODY_INPUT: group} + + def input_spec(self) -> RetargeterIOType: + """Declare the raw full-body DeviceIO input.""" + return {_FULL_BODY_INPUT: DeviceIOFullBodyPosePNTracked()} + + def output_spec(self) -> RetargeterIOType: + """Declare the flattened G1 action output.""" + return { + _ACTION_OUTPUT: G1LocomanipulationAction( + use_arm_ik_frame_tasks=self._use_arm_ik_frame_tasks + ) + } + + def _compute_fn( + self, + inputs: RetargeterIO, + outputs: RetargeterIO, + context: ComputeContext, + ) -> None: + """Convert a pushed full-body frame into the G1 action tensor.""" + self._frame_count += 1 + + if context.execution_events.reset: + self._retargeter.clear_calibration() + self._calibration_attempts = 0 + self._no_data_count = 0 + self._calibration_fail_count = 0 + print( + "NoitomG1ActionSource: cleared retargeting calibration " + f"collection={self._collection_id}" + ) + + # Read the raw tracked data from DeviceIO + tracked: FullBodyPosePNTrackedT = inputs[_FULL_BODY_INPUT][0] + frame: FullBodyPosePNT | None = tracked.data + + # --- First-frame diagnostic --- + if not self._first_frame_printed: + self._first_frame_printed = True + print( + "NoitomG1ActionSource: first frame " + f"collection={self._collection_id} " + f"has_data={frame is not None} " + f"motion_enabled={self._enable_motion}" + ) + + # --- No data warning --- + if frame is None: + self._no_data_count += 1 + if self._no_data_count == 1 or self._no_data_count % 300 == 0: + print( + f"NoitomG1ActionSource: WARNING no data from tracker " + f"collection={self._collection_id} " + f"no_data_frames={self._no_data_count}/{self._frame_count} " + f"(is the noitom_mocap plugin running with matching collection_id?)" + ) + # Still output hold pose even without data + body_yaw_delta = self._retargeter.body_yaw_delta + action = _make_action( + self._hold_targets, + use_arm_ik_frame_tasks=self._use_arm_ik_frame_tasks, + ) + outputs[_ACTION_OUTPUT][0] = np.ascontiguousarray(action, dtype=np.float32) + return + + # Reset no-data counter when we get valid frames + self._no_data_count = 0 + + if not self._enable_motion: + body_yaw_delta = self._retargeter.body_yaw_delta + action = _make_action( + self._hold_targets, + use_arm_ik_frame_tasks=self._use_arm_ik_frame_tasks, + ) + outputs[_ACTION_OUTPUT][0] = np.ascontiguousarray(action, dtype=np.float32) + self._reference_viz.update(frame, self._retargeter) + self._print_status(frame, body_yaw_delta, context) + return + + # --- Calibration / retarget phase --- + if self._retargeter.awaiting_calibration: + self._calibration_attempts += 1 + success = self._retargeter.calibrate(frame) + if success: + self._hold_targets = self._retargeter.current_arm_targets + print( + "NoitomG1ActionSource: calibrated neutral pose " + f"collection={self._collection_id} " + f"attempts={self._calibration_attempts}" + ) + else: + self._calibration_fail_count += 1 + if ( + self._calibration_fail_count == 1 + or self._calibration_fail_count % 150 == 0 + ): + # Diagnose WHY calibration is failing + diag = _calibration_diagnostics(frame) + print( + f"NoitomG1ActionSource: calibration attempt " + f"{self._calibration_attempts} failed {diag}" + ) + else: + result = self._retargeter.retarget(frame) + if result is not None: + self._hold_targets = result + + body_yaw_delta = self._retargeter.body_yaw_delta + action = _make_action( + self._hold_targets, + use_arm_ik_frame_tasks=self._use_arm_ik_frame_tasks, + ) + outputs[_ACTION_OUTPUT][0] = np.ascontiguousarray(action, dtype=np.float32) + + self._reference_viz.update(frame, self._retargeter) + self._print_status(frame, body_yaw_delta, context) + + def _print_status( + self, frame: FullBodyPosePNT, body_yaw_delta: float, context: ComputeContext + ) -> None: + if self._print_period_s <= 0.0: + return + now_s = context.graph_time.real_time_ns * 1.0e-9 + if now_s - self._last_print_s < self._print_period_s: + return + + self._last_print_s = now_s + motion = "on" if self._enable_motion else "off" + calib = "ready" if self._retargeter.is_calibrated else "awaiting_neutral" + left_pose = self._hold_targets.left_wrist.as_action_pose() + right_pose = self._hold_targets.right_wrist.as_action_pose() + frame_info = "" + if self._use_arm_ik_frame_tasks: + left_elbow_pose = self._hold_targets.left_elbow.as_action_pose() + right_elbow_pose = self._hold_targets.right_elbow.as_action_pose() + left_shoulder_pose = self._hold_targets.left_shoulder.as_action_pose() + right_shoulder_pose = self._hold_targets.right_shoulder.as_action_pose() + frame_info = ( + f" target_left_elbow={_fmt_pose(left_elbow_pose)}" + f" target_right_elbow={_fmt_pose(right_elbow_pose)}" + f" target_left_shoulder={_fmt_pose(left_shoulder_pose)}" + f" target_right_shoulder={_fmt_pose(right_shoulder_pose)}" + ) + print( + "NoitomG1ActionSource: " + f"joints={_valid_joint_count(frame)}/{int(BodyJointPN.NUM_JOINTS)} " + f"motion={motion} calibrated={calib} " + f"yaw_delta={body_yaw_delta:+.3f} " + f"motion_scale={self._retargeter.retargeting_settings.motion_scale:.2f} " + f"torso_yaw_influence={self._retargeter.retargeting_settings.torso_yaw_arm_influence:.2f} " + f"target_left={_fmt_pose(left_pose)} target_right={_fmt_pose(right_pose)}" + f"{frame_info} " + f"{_raw_full_body_status(frame)}" + ) + + +def build_noitom_g1_locomanipulation_pipeline() -> OutputCombiner: + """Build a one-source IsaacTeleop pipeline for Noitom G1 testing.""" + source = NoitomG1ActionSource(settings=DEFAULT_NOITOM_G1_SETTINGS) + return OutputCombiner({_ACTION_OUTPUT: source.output(_ACTION_OUTPUT)}) + + +def _make_action( + targets: ArmIkTargets, + *, + use_arm_ik_frame_tasks: bool, +) -> np.ndarray: + action = np.zeros( + g1_action_dim(use_arm_ik_frame_tasks=use_arm_ik_frame_tasks), + dtype=np.float32, + ) + action[0:7] = targets.left_wrist.as_action_pose() + action[7:14] = targets.right_wrist.as_action_pose() + hand_offset = 14 + if use_arm_ik_frame_tasks: + action[14:21] = targets.left_elbow.as_action_pose() + action[21:28] = targets.right_elbow.as_action_pose() + action[28:35] = targets.left_shoulder.as_action_pose() + action[35:42] = targets.right_shoulder.as_action_pose() + hand_offset = 42 + action[hand_offset : hand_offset + 14] = 0.0 + action[-4] = 0.0 + action[-3] = 0.0 + action[-2] = 0.0 + action[-1] = _LOCOMOTION_DEFAULT_HIP_HEIGHT + return action + + +class _NoitomReferenceVisualizer: + """Draw the incoming Noitom frame as a Kit debug-draw stick figure.""" + + def __init__(self, settings: NoitomG1Settings) -> None: + self._enabled = settings.draw_reference + self._draw: Any | None = None + self._warned = False + self._printed_first_draw = False + self._scale = settings.draw_scale + self._offset = np.array(settings.draw_offset, dtype=np.float32) + self._pelvis_relative = settings.draw_pelvis_relative + self._pelvis_anchor = np.array(settings.draw_pelvis_anchor, dtype=np.float32) + self._draw_wrist_targets = settings.draw_wrist_targets + self._draw_elbow_targets = ( + settings.draw_elbow_targets and settings.use_arm_ik_frame_tasks + ) + self._draw_shoulder_targets = ( + settings.draw_shoulder_targets and settings.use_arm_ik_frame_tasks + ) + self._retargeting = settings.retargeting + + def update( + self, + frame: FullBodyPosePNT, + retargeter: NoitomG1Retargeter, + ) -> None: + if not self._enabled: + return + draw = self._get_draw_interface() + if draw is None: + return + + calib_view = retargeter.calibration_view + draw_positions = self._reference_positions(frame, calib_view) + starts: list[list[float]] = [] + ends: list[list[float]] = [] + colors: list[tuple[float, float, float, float]] = [] + thicknesses: list[float] = [] + + for parent_index, child_index in _FULL_BODY_BONES: + parent = draw_positions.get(int(parent_index)) + child = draw_positions.get(int(child_index)) + if parent is None or child is None: + continue + starts.append(parent.tolist()) + ends.append(child.tolist()) + colors.append(_NOITOM_REFERENCE_COLOR) + thicknesses.append(_NOITOM_REFERENCE_LINE_THICKNESS) + + marker_starts, marker_ends, marker_colors = self._joint_markers(draw_positions) + starts.extend(marker_starts) + ends.extend(marker_ends) + colors.extend(marker_colors) + thicknesses.extend([_NOITOM_REFERENCE_LINE_THICKNESS] * len(marker_starts)) + + if self._draw_wrist_targets: + wrist_starts, wrist_ends, wrist_colors = self._frame_highlight_markers( + draw_positions, + ( + int(BodyJointPN.LEFT_WRIST), + int(BodyJointPN.RIGHT_WRIST), + ), + _NOITOM_WRIST_TARGET_COLOR, + _NOITOM_WRIST_TARGET_MARKER_SIZE, + ) + starts.extend(wrist_starts) + ends.extend(wrist_ends) + colors.extend(wrist_colors) + thicknesses.extend([_NOITOM_REFERENCE_LINE_THICKNESS] * len(wrist_starts)) + + if self._draw_elbow_targets: + elbow_starts, elbow_ends, elbow_colors = self._frame_highlight_markers( + draw_positions, + ( + int(BodyJointPN.LEFT_ELBOW), + int(BodyJointPN.RIGHT_ELBOW), + ), + _NOITOM_ELBOW_TARGET_COLOR, + _NOITOM_ELBOW_TARGET_MARKER_SIZE, + ) + starts.extend(elbow_starts) + ends.extend(elbow_ends) + colors.extend(elbow_colors) + thicknesses.extend([_NOITOM_REFERENCE_LINE_THICKNESS] * len(elbow_starts)) + + if self._draw_shoulder_targets: + shoulder_starts, shoulder_ends, shoulder_colors = ( + self._frame_highlight_markers( + draw_positions, + ( + int(BodyJointPN.LEFT_SHOULDER), + int(BodyJointPN.RIGHT_SHOULDER), + ), + _NOITOM_SHOULDER_TARGET_COLOR, + _NOITOM_SHOULDER_TARGET_MARKER_SIZE, + ) + ) + starts.extend(shoulder_starts) + ends.extend(shoulder_ends) + colors.extend(shoulder_colors) + thicknesses.extend( + [_NOITOM_REFERENCE_LINE_THICKNESS] * len(shoulder_starts) + ) + + draw.clear_lines() + if starts: + draw.draw_lines(starts, ends, colors, thicknesses) + if not self._printed_first_draw: + anchor = ( + _fmt_vec(self._pelvis_anchor) + if self._pelvis_relative + else "disabled" + ) + print( + "NoitomG1ActionSource: drawing Noitom reference skeleton " + f"segments={len(starts)} joints={len(draw_positions)} " + f"pelvis_relative={self._pelvis_relative} " + f"robot_pelvis_anchor={anchor}" + ) + self._printed_first_draw = True + elif not self._warned: + print( + "NoitomG1ActionSource: reference visualizer found no drawable " + "Noitom bones or joints" + ) + self._warned = True + + def _get_draw_interface(self) -> Any | None: + if self._draw is not None: + return self._draw + try: + from isaacsim.core.experimental.utils.app import enable_extension + + enable_extension("isaacsim.util.debug_draw") + from isaacsim.util.debug_draw import _debug_draw as omni_debug_draw + + self._draw = omni_debug_draw.acquire_debug_draw_interface() + except (ImportError, AttributeError, RuntimeError, ModuleNotFoundError): + try: + import omni.isaac.debug_draw._debug_draw as omni_debug_draw + + self._draw = omni_debug_draw.acquire_debug_draw_interface() + except ( + ImportError, + AttributeError, + RuntimeError, + ModuleNotFoundError, + ) as exc: + if not self._warned: + print( + "NoitomG1ActionSource: reference visualizer disabled " + f"({type(exc).__name__}: {exc})" + ) + self._warned = True + return None + except Exception as exc: + if not self._warned: + print( + "NoitomG1ActionSource: reference visualizer disabled " + f"({type(exc).__name__}: {exc})" + ) + self._warned = True + return None + return self._draw + + def _reference_positions( + self, + frame: FullBodyPosePNT, + calib_view: Any | None, + ) -> dict[int, np.ndarray]: + if self._pelvis_relative: + rt = self._retargeting + positions = aligned_reference_skeleton_from_frame( + frame, + self._pelvis_anchor, + draw_scale=self._scale, + calib_view=( + calib_view if not rt.reference_use_robot_link_lengths else None + ), + use_robot_link_lengths=rt.reference_use_robot_link_lengths, + link_lengths=ReferenceSkeletonLengths.from_retargeting_settings(rt), + length_scale=rt.reference_length_scale, + arm_length_scale=rt.reference_arm_length_scale, + shoulder_span_scale=rt.reference_shoulder_span_scale, + ) + return { + index: (pos + self._offset).astype(np.float32) + for index, pos in positions.items() + } + + raw_positions = _joint_position_map(frame) + return { + index: ( + self._offset + + noitom_position_to_isaac(point).astype(np.float32) * self._scale + ) + for index, point in raw_positions.items() + } + + def _joint_markers( + self, + positions: dict[int, np.ndarray], + ) -> tuple[_LineList, _LineList, _ColorList]: + starts: _LineList = [] + ends: _LineList = [] + colors: _ColorList = [] + marker_delta_x = np.array([_NOITOM_REFERENCE_JOINT_MARKER_SIZE, 0.0, 0.0]) + marker_delta_y = np.array([0.0, _NOITOM_REFERENCE_JOINT_MARKER_SIZE, 0.0]) + marker_delta_z = np.array([0.0, 0.0, _NOITOM_REFERENCE_JOINT_MARKER_SIZE]) + for position in positions.values(): + point = position.astype(np.float32) + for marker_delta in (marker_delta_x, marker_delta_y, marker_delta_z): + starts.append((point - marker_delta).tolist()) + ends.append((point + marker_delta).tolist()) + colors.append(_NOITOM_REFERENCE_COLOR) + return starts, ends, colors + + def _frame_highlight_markers( + self, + draw_positions: dict[int, np.ndarray], + joint_indices: tuple[int, ...], + color: tuple[float, float, float, float], + marker_size: float, + ) -> tuple[_LineList, _LineList, _ColorList]: + """Highlight selected joints on the cyan skeleton.""" + starts: _LineList = [] + ends: _LineList = [] + colors: _ColorList = [] + marker_delta_x = np.array([marker_size, 0.0, 0.0]) + marker_delta_y = np.array([0.0, marker_size, 0.0]) + marker_delta_z = np.array([0.0, 0.0, marker_size]) + for joint_index in joint_indices: + position = draw_positions.get(joint_index) + if position is None: + continue + point = position.astype(np.float32) + for marker_delta in (marker_delta_x, marker_delta_y, marker_delta_z): + starts.append((point - marker_delta).tolist()) + ends.append((point + marker_delta).tolist()) + colors.append(color) + return starts, ends, colors + + +def _joint_position_map(frame: FullBodyPosePNT) -> dict[int, np.ndarray]: + positions: dict[int, np.ndarray] = {} + if frame.joints is None: + return positions + for index in range(int(BodyJointPN.NUM_JOINTS)): + position = _joint_position(frame, index) + if position is not None: + positions[index] = position + return positions + + +def _joint_position(frame: FullBodyPosePNT, joint_index: int) -> np.ndarray | None: + if frame.joints is None: + return None + joint = frame.joints.joints(int(joint_index)) + if not joint.is_valid: + return None + value = _point_to_array(joint.pose.position) + if np.all(np.isfinite(value)): + return value + return None + + +def _valid_joint_count(frame: FullBodyPosePNT) -> int: + if frame.joints is None: + return 0 + count = 0 + for index in range(int(BodyJointPN.NUM_JOINTS)): + if frame.joints.joints(index).is_valid: + count += 1 + return count + + +def _raw_full_body_status(frame: FullBodyPosePNT) -> str: + left_wrist = _joint_position(frame, BodyJointPN.LEFT_WRIST) + right_wrist = _joint_position(frame, BodyJointPN.RIGHT_WRIST) + pelvis = _joint_position(frame, BodyJointPN.PELVIS) + spine3 = _joint_position(frame, BodyJointPN.SPINE3) + left_shoulder = _joint_position(frame, BodyJointPN.LEFT_SHOULDER) + right_shoulder = _joint_position(frame, BodyJointPN.RIGHT_SHOULDER) + left_elbow = _joint_position(frame, BodyJointPN.LEFT_ELBOW) + right_elbow = _joint_position(frame, BodyJointPN.RIGHT_ELBOW) + + missing = [] + if pelvis is None: + missing.append("pelvis") + if spine3 is None: + missing.append("spine3") + if left_shoulder is None: + missing.append("l_shoulder") + if right_shoulder is None: + missing.append("r_shoulder") + if left_elbow is None: + missing.append("l_elbow") + if right_elbow is None: + missing.append("r_elbow") + if left_wrist is None: + missing.append("l_wrist") + if right_wrist is None: + missing.append("r_wrist") + + if missing: + return f"upper_body_missing={','.join(missing)}" + left_isaac = noitom_position_to_isaac(left_wrist) + right_isaac = noitom_position_to_isaac(right_wrist) + pelvis_isaac = noitom_position_to_isaac(pelvis) + return ( + f"left_wrist={_fmt_vec(left_wrist)} right_wrist={_fmt_vec(right_wrist)} " + f"isaac_left={_fmt_vec(left_isaac)} isaac_right={_fmt_vec(right_isaac)} " + f"isaac_pelvis={_fmt_vec(pelvis_isaac)}" + ) + + +def _calibration_diagnostics(frame: FullBodyPosePNT) -> str: + """Detailed calibration diagnostics: which joints are valid/invalid.""" + pelvis = _joint_position(frame, BodyJointPN.PELVIS) + spine3 = _joint_position(frame, BodyJointPN.SPINE3) + left_shoulder = _joint_position(frame, BodyJointPN.LEFT_SHOULDER) + right_shoulder = _joint_position(frame, BodyJointPN.RIGHT_SHOULDER) + left_elbow = _joint_position(frame, BodyJointPN.LEFT_ELBOW) + right_elbow = _joint_position(frame, BodyJointPN.RIGHT_ELBOW) + left_wrist = _joint_position(frame, BodyJointPN.LEFT_WRIST) + right_wrist = _joint_position(frame, BodyJointPN.RIGHT_WRIST) + + required = { + "pelvis": pelvis, + "spine3": spine3, + "L_shoulder": left_shoulder, + "R_shoulder": right_shoulder, + "L_elbow": left_elbow, + "R_elbow": right_elbow, + "L_wrist": left_wrist, + "R_wrist": right_wrist, + } + valid = [k for k, v in required.items() if v is not None] + missing = [k for k, v in required.items() if v is None] + total_joints = int(BodyJointPN.NUM_JOINTS) + all_valid = sum( + 1 + for i in range(total_joints) + if frame.joints is not None and frame.joints.joints(i).is_valid + ) + return ( + f"total_valid={all_valid}/{total_joints} " + f"required_valid={len(valid)}/8 " + f"missing=[{','.join(missing)}]" + if missing + else f"all_required_ok={valid}" + ) + + +def _point_to_array(point: Any) -> np.ndarray: + return np.array([point.x, point.y, point.z], dtype=np.float32) + + +def _fmt_vec(vec: np.ndarray) -> str: + return "[" + ", ".join(f"{v:+.3f}" for v in vec) + "]" + + +def _fmt_pose(pose: np.ndarray) -> str: + return ( + f"pos=[{pose[0]:+.3f}, {pose[1]:+.3f}, {pose[2]:+.3f}] " + f"quat=[{pose[3]:+.3f}, {pose[4]:+.3f}, {pose[5]:+.3f}, {pose[6]:+.3f}]" + ) + + +__all__ = [ + "TASK_ID", + "NoitomG1ActionSource", + "NoitomLocomanipulationG1EnvCfg", + "build_noitom_g1_locomanipulation_pipeline", + "g1_action_dim", + "register_tasks", +] diff --git a/examples/noitom/record_noitom_full_body.py b/examples/noitom/record_noitom_full_body.py new file mode 100644 index 000000000..b19145e94 --- /dev/null +++ b/examples/noitom/record_noitom_full_body.py @@ -0,0 +1,122 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Record Noitom-published full-body data to an MCAP file. + +Requires ``noitom_mocap_plugin`` to be running. This records the +``full_body_pn`` channel through ``FullBodyTrackerPN``. + +Usage: + python record_noitom_full_body.py [duration_seconds] [output.mcap] + +Defaults: 10 seconds -> recordings/noitom_full_body_.mcap +""" + +import sys +import time +from datetime import datetime +from pathlib import Path +from typing import NamedTuple + +import numpy as np + +from isaacteleop.deviceio import ( + DeviceIOSession, + FullBodyTrackerPN, + McapRecordingConfig, +) +from isaacteleop.oxr import OpenXRSession +from isaacteleop.schema import BodyJointPN + +_MCAP_RECORD_REPLAY_EXAMPLE = ( + Path(__file__).resolve().parents[1] / "mcap_record_replay" / "python" +) +sys.path.insert(0, str(_MCAP_RECORD_REPLAY_EXAMPLE)) +from common import BODY_JOINT_NAMES # noqa: E402 + + +class Args(NamedTuple): + duration_s: float + mcap_path: Path + + +def _valid_array(frame: object | None) -> np.ndarray | None: + if frame is None or frame.joints is None: + return None + + valid = np.zeros(int(BodyJointPN.NUM_JOINTS), dtype=np.uint8) + for index in range(int(BodyJointPN.NUM_JOINTS)): + valid[index] = 1 if frame.joints.joints(index).is_valid else 0 + return valid + + +def parse_args(argv: list[str]) -> Args: + args = list(argv[1:]) + duration_option = None + if "--duration" in args: + duration_index = args.index("--duration") + try: + duration_option = float(args[duration_index + 1]) + except IndexError: + raise SystemExit("[record] error: --duration requires a value") from None + del args[duration_index : duration_index + 2] + + if duration_option is None: + duration_s = float(args[0]) if args else 10.0 + output_arg = args[1] if len(args) > 1 else None + else: + duration_s = duration_option + output_arg = args[0] if args else None + + if output_arg is not None: + mcap_path = Path(output_arg) + mcap_path.parent.mkdir(parents=True, exist_ok=True) + else: + out_dir = Path(__file__).resolve().parent / "recordings" + out_dir.mkdir(exist_ok=True) + mcap_path = out_dir / f"noitom_full_body_{datetime.now():%Y%m%d_%H%M%S}.mcap" + + return Args(duration_s=duration_s, mcap_path=mcap_path) + + +def main(argv: list[str]) -> int: + args = parse_args(argv) + duration_s = args.duration_s + mcap_path = args.mcap_path + + print(f"[record] writing {mcap_path} for {duration_s:.1f}s") + print("[record] start noitom_mocap_plugin before running this script") + + tracker = FullBodyTrackerPN() + required_extensions = DeviceIOSession.get_required_extensions([tracker]) + mcap_config = McapRecordingConfig(str(mcap_path), [(tracker, "full_body_pn")]) + + with OpenXRSession( + "NoitomFullBodyRecordExample", required_extensions + ) as oxr_session: + handles = oxr_session.get_handles() + with DeviceIOSession.run([tracker], handles, mcap_config) as session: + start = time.time() + frame_count = 0 + while time.time() - start < duration_s: + session.update() + frame_count += 1 + + if frame_count % 60 == 0: + tracked = tracker.get_body_pose(session) + valid = _valid_array(tracked.data) + n_valid = 0 if valid is None else int(np.count_nonzero(valid)) + print( + f"[record] t={time.time() - start:5.2f}s " + f"frame={frame_count} " + f"joints={n_valid:02d}/{len(BODY_JOINT_NAMES)}" + ) + time.sleep(1 / 60) + + print(f"[record] done - {mcap_path}") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/noitom/replay_noitom_full_body.py b/examples/noitom/replay_noitom_full_body.py new file mode 100644 index 000000000..27d7ae001 --- /dev/null +++ b/examples/noitom/replay_noitom_full_body.py @@ -0,0 +1,232 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Replay a recorded Noitom full-body MCAP file and visualize it with viser. + +``ReplaySession`` skips OpenXR initialization, so this runs headless on any +machine. Open the URL viser prints (default http://localhost:8080) in a browser +to see the body skeleton. + +Usage: + python replay_noitom_full_body.py [path/to/file.mcap] [--port 8080] [--loop] + +If no path is given, the newest file under ``recordings/`` is used. +``--loop`` keeps replaying the file end-to-end until the process is killed. +""" + +import argparse +import sys +import time +from pathlib import Path + +import numpy as np +import viser +from mcap.reader import make_reader + +from isaacteleop.deviceio import FullBodyTrackerPN, McapReplayConfig, ReplaySession +from isaacteleop.schema import BodyJointPN + +_MCAP_RECORD_REPLAY_EXAMPLE = ( + Path(__file__).resolve().parents[1] / "mcap_record_replay" / "python" +) +sys.path.insert(0, str(_MCAP_RECORD_REPLAY_EXAMPLE)) +from common import BODY_BONES, BODY_JOINT_NAMES # noqa: E402 + + +def mcap_duration_s(path: Path) -> float: + """Read MCAP summary statistics and return wall-clock duration in seconds. + + The C++ replay session does not signal end-of-file — it just logs + ``Replay*TrackerImpl: ... data not found`` and keeps spinning. We use + this duration as the stop condition so playback exits cleanly. + """ + with open(path, "rb") as f: + reader = make_reader(f) + summary = reader.get_summary() + if summary is None or summary.statistics is None: + raise RuntimeError(f"{path}: MCAP file has no summary/statistics block") + stats = summary.statistics + if stats.message_count == 0: + return 0.0 + return (stats.message_end_time - stats.message_start_time) / 1e9 + + +TRACKED_COLOR = (0.25, 0.85, 0.35) +INVALID_COLOR = (1.0, 0.0, 0.0) + + +def resolve_mcap(path_arg: str | None) -> Path: + if path_arg: + path = Path(path_arg) + if not path.exists(): + sys.exit(f"[replay] error: {path} does not exist") + return path + + recordings = Path(__file__).resolve().parent / "recordings" + candidates = list(recordings.glob("*.mcap")) + if not candidates: + sys.exit( + f"[replay] error: no .mcap files in {recordings}. " + "Run record_noitom_full_body.py first or pass a path." + ) + return max(candidates, key=lambda p: p.stat().st_mtime) + + +def _valid_bone_segments(positions: np.ndarray, valid: np.ndarray) -> np.ndarray: + """Return (N, 2, 3) segment array for bones whose endpoints are both valid.""" + segments: list[np.ndarray] = [] + for a, b in BODY_BONES: + if valid[a] and valid[b]: + segments.append(np.stack([positions[a], positions[b]], axis=0)) + if not segments: + return np.zeros((0, 2, 3), dtype=np.float32) + return np.stack(segments, axis=0).astype(np.float32) + + +def _pn_frame_arrays( + frame: object | None, +) -> tuple[np.ndarray | None, np.ndarray | None]: + if frame is None or frame.joints is None: + return None, None + + joint_count = int(BodyJointPN.NUM_JOINTS) + positions = np.zeros((joint_count, 3), dtype=np.float32) + valid = np.zeros(joint_count, dtype=np.uint8) + for index in range(joint_count): + joint = frame.joints.joints(index) + point = joint.pose.position + positions[index] = (point.x, point.y, point.z) + valid[index] = 1 if joint.is_valid else 0 + return positions, valid + + +class FullBodyViz: + """Viser handles (joint cloud + skeleton segments).""" + + def __init__(self, server: viser.ViserServer): + self.color = np.array(TRACKED_COLOR, dtype=np.float32) + zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + zero_segs = np.zeros((0, 2, 3), dtype=np.float32) + + self.points = server.scene.add_point_cloud( + name="/full_body/joints", + points=zero_pts, + colors=np.tile(self.color, (len(BODY_JOINT_NAMES), 1)), + point_size=0.01, + ) + self.bones = server.scene.add_line_segments( + name="/full_body/bones", + points=zero_segs, + colors=np.zeros((0, 2, 3), dtype=np.float32), + line_width=2.0, + ) + + def update(self, positions: np.ndarray | None, valid: np.ndarray | None) -> None: + if positions is None or valid is None: + zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + self.points.points = zero_pts + self.points.colors = np.tile(INVALID_COLOR, (len(BODY_JOINT_NAMES), 1)) + self.bones.points = np.zeros((0, 2, 3), dtype=np.float32) + self.bones.colors = np.zeros((0, 2, 3), dtype=np.float32) + return + + positions = positions.astype(np.float32) + valid_bool = valid.astype(bool) + self.points.points = positions + + point_colors = np.tile(self.color, (positions.shape[0], 1)) + point_colors[~valid_bool] = INVALID_COLOR + self.points.colors = point_colors + + segs = _valid_bone_segments(positions, valid_bool) + self.bones.points = segs + self.bones.colors = np.tile(self.color, (segs.shape[0], 2, 1)) + + +def run_once( + mcap_path: Path, + duration_s: float, + viz: FullBodyViz, + channel_name: str, +) -> int: + """Play the file once for ``duration_s`` wall-clock seconds. Returns frame count.""" + tracker = FullBodyTrackerPN() + replay_config = McapReplayConfig(str(mcap_path), [(tracker, channel_name)]) + + frames = 0 + with ReplaySession.run(replay_config) as session: + start = time.time() + while time.time() - start < duration_s: + session.update() + tracked = tracker.get_body_pose(session) + + positions, valid = _pn_frame_arrays(tracked.data) + if positions is None or valid is None: + viz.update(None, None) + n_valid = 0 + else: + viz.update(positions, valid) + n_valid = int(np.count_nonzero(valid)) + + frames += 1 + if frames % 60 == 0: + print( + f"[replay] t={time.time() - start:5.2f}s " + f"frame={frames} " + f"joints={n_valid:02d}/{len(BODY_JOINT_NAMES)}" + ) + time.sleep(1 / 60) + print(f"[replay] reached end of recording after {frames} frames") + return frames + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("mcap", nargs="?", help="Path to .mcap file") + parser.add_argument( + "--host", + default="127.0.0.1", + help="Viser HTTP bind address (default: 127.0.0.1; pass 0.0.0.0 to expose externally)", + ) + parser.add_argument("--port", type=int, default=8080, help="Viser HTTP port") + parser.add_argument( + "--loop", + action="store_true", + help="Replay the file in a loop until Ctrl+C", + ) + parser.add_argument( + "--channel-name", + default="full_body_pn", + help="MCAP channel base name used when recording.", + ) + args = parser.parse_args(argv[1:]) + + mcap_path = resolve_mcap(args.mcap) + duration_s = mcap_duration_s(mcap_path) + + server = viser.ViserServer(host=args.host, port=args.port) + server.scene.set_up_direction("+y") + server.scene.add_grid(name="/grid", width=2.0, height=2.0, cell_size=0.1) + viz = FullBodyViz(server) + + print(f"[replay] viser running at http://localhost:{args.port}") + print(f"[replay] reading {mcap_path} (duration {duration_s:.2f}s)") + + while True: + run_once(mcap_path, duration_s, viz, args.channel_name) + if not args.loop: + break + print("[replay] looping...") + + print("[replay] done - viser server still up; Ctrl+C to exit") + try: + while True: + time.sleep(1.0) + except KeyboardInterrupt: + pass + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/teleop_ros2/python/teleop_ros2_node.py b/examples/teleop_ros2/python/teleop_ros2_node.py index 8515aecd7..2fcdc3943 100755 --- a/examples/teleop_ros2/python/teleop_ros2_node.py +++ b/examples/teleop_ros2/python/teleop_ros2_node.py @@ -390,6 +390,7 @@ def run(self) -> int: accept_eula=self._params.cloudxr_params.accept_eula, setup_oob=self._params.cloudxr_params.setup_oob, usb_local=self._params.cloudxr_params.usb_local, + host_client=True, ) as launcher: self.get_logger().info( "CloudXR runtime and WSS proxy started " diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pn_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pn_base.hpp new file mode 100644 index 000000000..ad4be4b2f --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/full_body_tracker_pn_base.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct FullBodyPosePNTrackedT; + +// Abstract base interface for Perception Neuron full-body tracker implementations. +class IFullBodyTrackerPNImpl : public ITrackerImpl +{ +public: + virtual const FullBodyPosePNTrackedT& get_body_pose() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 113bda804..95a96a56b 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -13,11 +13,13 @@ add_library(deviceio_trackers STATIC joint_state_tracker.cpp frame_metadata_tracker_oak.cpp full_body_tracker_pico.cpp + full_body_tracker_pn.cpp inc/deviceio_trackers/head_tracker.hpp inc/deviceio_trackers/hand_tracker.hpp inc/deviceio_trackers/controller_tracker.hpp inc/deviceio_trackers/message_channel_tracker.hpp inc/deviceio_trackers/full_body_tracker_pico.hpp + inc/deviceio_trackers/full_body_tracker_pn.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp inc/deviceio_trackers/joint_state_tracker.hpp inc/deviceio_trackers/frame_metadata_tracker_oak.hpp diff --git a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp b/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp index aed4029d4..5ff4cd3b3 100644 --- a/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp +++ b/src/core/deviceio_trackers/cpp/full_body_tracker_pico.cpp @@ -6,10 +6,6 @@ namespace core { -// ============================================================================ -// FullBodyTrackerPico Public Interface -// ============================================================================ - const FullBodyPosePicoTrackedT& FullBodyTrackerPico::get_body_pose(const ITrackerSession& session) const { return static_cast(session.get_tracker_impl(*this)).get_body_pose(); diff --git a/src/core/deviceio_trackers/cpp/full_body_tracker_pn.cpp b/src/core/deviceio_trackers/cpp/full_body_tracker_pn.cpp new file mode 100644 index 000000000..9bdbef240 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/full_body_tracker_pn.cpp @@ -0,0 +1,19 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/full_body_tracker_pn.hpp" + +namespace core +{ + +FullBodyTrackerPN::FullBodyTrackerPN(const std::string& collection_id, size_t max_flatbuffer_size) + : collection_id_(collection_id), max_flatbuffer_size_(max_flatbuffer_size) +{ +} + +const FullBodyPosePNTrackedT& FullBodyTrackerPN::get_body_pose(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_body_pose(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pn.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pn.hpp new file mode 100644 index 000000000..c7e7c68c0 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/full_body_tracker_pn.hpp @@ -0,0 +1,50 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include +#include + +namespace core +{ + +// SchemaTracker-backed full-body tracker for Noitom Perception Neuron samples. +class FullBodyTrackerPN : public ITracker +{ +public: + static constexpr uint32_t JOINT_COUNT = 24; + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 16 * 1024; + + explicit FullBodyTrackerPN(const std::string& collection_id = "noitom_mocap", + size_t max_flatbuffer_size = DEFAULT_MAX_FLATBUFFER_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + const FullBodyPosePNTrackedT& get_body_pose(const ITrackerSession& session) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "FullBodyTrackerPN"; + + std::string collection_id_; + size_t max_flatbuffer_size_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index 323a499a3..abd89bd80 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -14,6 +14,7 @@ Generic3AxisPedalTracker, JointStateTracker, FullBodyTrackerPico, + FullBodyTrackerPN, ITrackerSession, NUM_JOINTS, JOINT_PALM, @@ -28,6 +29,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "FullBodyTrackerPico", + "FullBodyTrackerPN", "Generic3AxisPedalTracker", "JointStateTracker", "HandTracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 189cefc78..f15a5477e 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -164,13 +165,23 @@ PYBIND11_MODULE(_deviceio_trackers, m) py::class_>( m, "FullBodyTrackerPico") - .def(py::init<>()) + .def(py::init<>(), "Construct a native PICO XR_BD_body_tracking full body tracker.") .def( "get_body_pose", [](const core::FullBodyTrackerPico& self, const core::ITrackerSession& session) -> core::FullBodyPosePicoTrackedT { return self.get_body_pose(session); }, py::arg("session"), "Get full body pose tracked state (data is None if inactive)"); + py::class_>(m, "FullBodyTrackerPN") + .def(py::init(), py::arg("collection_id") = "noitom_mocap", + py::arg("max_flatbuffer_size") = core::FullBodyTrackerPN::DEFAULT_MAX_FLATBUFFER_SIZE, + "Construct a Perception Neuron full body tracker backed by pushed FullBodyPosePN schema data.") + .def( + "get_body_pose", + [](const core::FullBodyTrackerPN& self, const core::ITrackerSession& session) -> core::FullBodyPosePNTrackedT + { return self.get_body_pose(session); }, + py::arg("session"), "Get PN full body pose tracked state (data is None if inactive)"); + m.attr("NUM_JOINTS") = static_cast(core::HandJoint_NUM_JOINTS); m.attr("JOINT_PALM") = static_cast(core::HandJoint_PALM); m.attr("JOINT_WRIST") = static_cast(core::HandJoint_WRIST); diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index 9f93e58df..6be4c168f 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -11,6 +11,7 @@ add_library(live_trackers STATIC live_controller_tracker_impl.cpp live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp + live_full_body_tracker_pn_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp live_joint_state_tracker_impl.cpp live_frame_metadata_tracker_oak_impl.cpp @@ -22,6 +23,7 @@ add_library(live_trackers STATIC live_controller_tracker_impl.hpp live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp + live_full_body_tracker_pn_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp live_joint_state_tracker_impl.hpp live_frame_metadata_tracker_oak_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 24a78a475..1e76787a1 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 @@ -28,6 +28,8 @@ class MessageChannelTracker; class IMessageChannelTrackerImpl; class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; +class FullBodyTrackerPN; +class IFullBodyTrackerPNImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; class JointStateTracker; @@ -62,6 +64,7 @@ class LiveDeviceIOFactory std::unique_ptr create_controller_tracker_impl(const ControllerTracker* tracker); std::unique_ptr create_message_channel_tracker_impl(const MessageChannelTracker* tracker); std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); + std::unique_ptr create_full_body_tracker_pn_impl(const FullBodyTrackerPN* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); std::unique_ptr create_joint_state_tracker_impl(const JointStateTracker* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index c0b3471ba..4bb6610b0 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -6,6 +6,7 @@ #include "live_controller_tracker_impl.hpp" #include "live_frame_metadata_tracker_oak_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" +#include "live_full_body_tracker_pn_impl.hpp" #include "live_generic_3axis_pedal_tracker_impl.hpp" #include "live_hand_tracker_impl.hpp" #include "live_head_tracker_impl.hpp" @@ -15,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -75,6 +77,12 @@ std::unique_ptr try_create_full_body_pico_impl(LiveDeviceIOFactory return typed ? factory.create_full_body_tracker_pico_impl(typed) : nullptr; } +std::unique_ptr try_create_full_body_pn_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_pn_impl(typed) : nullptr; +} + std::unique_ptr try_create_generic_pedal_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -109,6 +117,7 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { { &try_add_extensions, &try_create_controller_impl }, { &try_add_extensions, &try_create_message_channel_impl }, { &try_add_extensions, &try_create_full_body_pico_impl }, + { &try_add_extensions, &try_create_full_body_pn_impl }, { &try_add_extensions, &try_create_generic_pedal_impl }, { &try_add_extensions, &try_create_joint_state_impl }, { &try_add_extensions, &try_create_oak_impl }, @@ -242,6 +251,16 @@ std::unique_ptr LiveDeviceIOFactory::create_full_body_ return std::make_unique(handles_, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_pn_impl(const FullBodyTrackerPN* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFullBodyTrackerPNImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker) { diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp index 181d6b803..383fb0d97 100644 --- a/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp +++ b/src/core/live_trackers/cpp/live_full_body_tracker_pico_impl.hpp @@ -4,6 +4,7 @@ #pragma once #include +#include #include #include #include diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.cpp b/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.cpp new file mode 100644 index 000000000..8dca39ffd --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.cpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_full_body_tracker_pn_impl.hpp" + +#include +#include + +namespace core +{ + +namespace +{ + +SchemaTrackerConfig make_full_body_pn_tensor_config(const FullBodyTrackerPN* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_flatbuffer_size(); + cfg.tensor_identifier = "full_body_pn"; + cfg.localized_name = "FullBodyTrackerPN"; + return cfg; +} + +} // namespace + +std::unique_ptr LiveFullBodyTrackerPNImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, FullBodyPNRecordingTraits::schema_name, + std::vector(FullBodyPNRecordingTraits::recording_channels.begin(), + FullBodyPNRecordingTraits::recording_channels.end())); +} + +LiveFullBodyTrackerPNImpl::LiveFullBodyTrackerPNImpl(const OpenXRSessionHandles& handles, + const FullBodyTrackerPN* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + schema_reader_(handles, make_full_body_pn_tensor_config(tracker), mcap_channels_.get(), /*mcap_channel_index=*/0) +{ +} + +void LiveFullBodyTrackerPNImpl::update(int64_t /*monotonic_time_ns*/) +{ + schema_reader_.update(tracked_.data); +} + +const FullBodyPosePNTrackedT& LiveFullBodyTrackerPNImpl::get_body_pose() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.hpp new file mode 100644 index 000000000..888af4b83 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_pn_impl.hpp @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. 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 + +namespace core +{ + +using FullBodyPNMcapChannels = McapTrackerChannels; +using FullBodyPNSchemaTracker = SchemaTracker; + +class LiveFullBodyTrackerPNImpl : public IFullBodyTrackerPNImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + + LiveFullBodyTrackerPNImpl(const OpenXRSessionHandles& handles, + const FullBodyTrackerPN* tracker, + std::unique_ptr mcap_channels); + + LiveFullBodyTrackerPNImpl(const LiveFullBodyTrackerPNImpl&) = delete; + LiveFullBodyTrackerPNImpl& operator=(const LiveFullBodyTrackerPNImpl&) = delete; + LiveFullBodyTrackerPNImpl(LiveFullBodyTrackerPNImpl&&) = delete; + LiveFullBodyTrackerPNImpl& operator=(LiveFullBodyTrackerPNImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const FullBodyPosePNTrackedT& get_body_pose() const override; + +private: + std::unique_ptr mcap_channels_; + FullBodyPNSchemaTracker schema_reader_; + FullBodyPosePNTrackedT tracked_; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp index cf0031e23..ce9af08e9 100644 --- a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -45,6 +45,13 @@ struct FullBodyPicoRecordingTraits static constexpr std::array replay_channels = { "full_body" }; }; +struct FullBodyPNRecordingTraits +{ + static constexpr std::string_view schema_name = "core.FullBodyPosePNRecord"; + static constexpr std::array recording_channels = { "full_body_pn" }; + static constexpr std::array replay_channels = { "full_body_pn" }; +}; + struct PedalRecordingTraits { static constexpr std::string_view schema_name = "core.Generic3AxisPedalOutputRecord"; diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index a640a10e4..18a724c3f 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -19,6 +19,7 @@ Generic3AxisPedalTracker, JointStateTracker, FullBodyTrackerPico, + FullBodyTrackerPN, NUM_JOINTS, JOINT_PALM, JOINT_WRIST, @@ -63,6 +64,7 @@ "Generic3AxisPedalTracker", "JointStateTracker", "FullBodyTrackerPico", + "FullBodyTrackerPN", "OpenXRSessionHandles", "DeviceIOSession", "McapRecordingConfig", diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index 770cc4ff3..18652f616 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(replay_trackers STATIC replay_head_tracker_impl.cpp replay_controller_tracker_impl.cpp replay_full_body_tracker_pico_impl.cpp + replay_full_body_tracker_pn_impl.cpp replay_generic_3axis_pedal_tracker_impl.cpp replay_joint_state_tracker_impl.cpp replay_message_channel_tracker_impl.cpp @@ -17,6 +18,7 @@ add_library(replay_trackers STATIC replay_head_tracker_impl.hpp replay_controller_tracker_impl.hpp replay_full_body_tracker_pico_impl.hpp + replay_full_body_tracker_pn_impl.hpp replay_generic_3axis_pedal_tracker_impl.hpp replay_joint_state_tracker_impl.hpp replay_message_channel_tracker_impl.hpp diff --git a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp index ddc04b063..67751fcff 100644 --- a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp +++ b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp @@ -19,6 +19,8 @@ class ControllerTracker; class IControllerTrackerImpl; class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; +class FullBodyTrackerPN; +class IFullBodyTrackerPNImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; class JointStateTracker; @@ -50,6 +52,7 @@ class ReplayDeviceIOFactory std::unique_ptr create_hand_tracker_impl(const HandTracker* tracker); std::unique_ptr create_controller_tracker_impl(const ControllerTracker* tracker); std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); + std::unique_ptr create_full_body_tracker_pn_impl(const FullBodyTrackerPN* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); std::unique_ptr create_joint_state_tracker_impl(const JointStateTracker* tracker); diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index 4941f3a63..5c87881f2 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -5,6 +5,7 @@ #include "replay_controller_tracker_impl.hpp" #include "replay_full_body_tracker_pico_impl.hpp" +#include "replay_full_body_tracker_pn_impl.hpp" #include "replay_generic_3axis_pedal_tracker_impl.hpp" #include "replay_hand_tracker_impl.hpp" #include "replay_head_tracker_impl.hpp" @@ -13,6 +14,7 @@ #include #include +#include #include #include #include @@ -67,6 +69,12 @@ std::unique_ptr try_create_full_body_pico_impl(ReplayDeviceIOFacto return typed ? factory.create_full_body_tracker_pico_impl(typed) : nullptr; } +std::unique_ptr try_create_full_body_pn_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_pn_impl(typed) : nullptr; +} + std::unique_ptr try_create_generic_pedal_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -88,13 +96,10 @@ std::unique_ptr try_create_message_channel_impl(ReplayDeviceIOFact using TryCreateFn = std::unique_ptr (*)(ReplayDeviceIOFactory&, const ITracker&); inline const TryCreateFn k_tracker_dispatch[] = { - &try_create_head_impl, - &try_create_hand_impl, - &try_create_controller_impl, - &try_create_full_body_pico_impl, - &try_create_generic_pedal_impl, - &try_create_joint_state_impl, - &try_create_message_channel_impl, + &try_create_head_impl, &try_create_hand_impl, + &try_create_controller_impl, &try_create_full_body_pico_impl, + &try_create_full_body_pn_impl, &try_create_generic_pedal_impl, + &try_create_joint_state_impl, &try_create_message_channel_impl, }; } // namespace @@ -155,6 +160,12 @@ std::unique_ptr ReplayDeviceIOFactory::create_full_bod return std::make_unique(open_reader(filename_), get_name(tracker)); } +std::unique_ptr ReplayDeviceIOFactory::create_full_body_tracker_pn_impl( + const FullBodyTrackerPN* tracker) +{ + return std::make_unique(open_reader(filename_), get_name(tracker)); +} + std::unique_ptr ReplayDeviceIOFactory::create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker) { diff --git a/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.cpp b/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.cpp new file mode 100644 index 000000000..ee6a5d2a0 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.cpp @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_full_body_tracker_pn_impl.hpp" + +#include +#include +#include + +#include + +namespace core +{ + +ReplayFullBodyTrackerPNImpl::ReplayFullBodyTrackerPNImpl(std::unique_ptr reader, + std::string_view base_name) + : mcap_viewers_(std::make_unique( + std::move(reader), + base_name, + std::vector( + FullBodyPNRecordingTraits::replay_channels.begin(), FullBodyPNRecordingTraits::replay_channels.end()))) +{ +} + +const FullBodyPosePNTrackedT& ReplayFullBodyTrackerPNImpl::get_body_pose() const +{ + return tracked_; +} + +void ReplayFullBodyTrackerPNImpl::update(int64_t /*monotonic_time_ns*/) +{ + auto record = mcap_viewers_->read(0); + if (record) + { + tracked_.data = std::move(record->data); + } + else + { + std::cerr << "ReplayFullBodyTrackerPNImpl: body data not found" << std::endl; + tracked_.data.reset(); + } +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.hpp b/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.hpp new file mode 100644 index 000000000..26e1bb310 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_full_body_tracker_pn_impl.hpp @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +using FullBodyPNMcapViewers = McapTrackerViewers; + +class ReplayFullBodyTrackerPNImpl : public IFullBodyTrackerPNImpl +{ +public: + ReplayFullBodyTrackerPNImpl(std::unique_ptr reader, std::string_view base_name); + + ReplayFullBodyTrackerPNImpl(const ReplayFullBodyTrackerPNImpl&) = delete; + ReplayFullBodyTrackerPNImpl& operator=(const ReplayFullBodyTrackerPNImpl&) = delete; + ReplayFullBodyTrackerPNImpl(ReplayFullBodyTrackerPNImpl&&) = delete; + ReplayFullBodyTrackerPNImpl& operator=(ReplayFullBodyTrackerPNImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const FullBodyPosePNTrackedT& get_body_pose() const override; + +private: + FullBodyPosePNTrackedT tracked_; + std::unique_ptr mcap_viewers_; +}; + +} // namespace core diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py index 7b0a0efee..6d613cf70 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py @@ -26,12 +26,14 @@ Generic3AxisPedalOutputTrackedType, JointStateOutputTrackedType, FullBodyPosePicoTrackedType, + FullBodyPosePNTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, DeviceIOGeneric3AxisPedalOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPosePicoTracked, + DeviceIOFullBodyPosePNTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -61,6 +63,7 @@ "Generic3AxisPedalOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPosePicoTrackedType", + "FullBodyPosePNTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -70,6 +73,7 @@ "DeviceIOGeneric3AxisPedalOutputTracked", "DeviceIOJointStateOutputTracked", "DeviceIOFullBodyPosePicoTracked", + "DeviceIOFullBodyPosePNTracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py index 89f92b5db..c95cb7cb3 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py @@ -20,6 +20,7 @@ Generic3AxisPedalOutputTrackedT, JointStateOutputTrackedT, FullBodyPosePicoTrackedT, + FullBodyPosePNTrackedT, MessageChannelMessagesTrackedT, ) @@ -140,6 +141,26 @@ def validate_value(self, value: Any) -> None: ) +class FullBodyPosePNTrackedType(TensorType): + """FullBodyPosePNTrackedT wrapper type from DeviceIO FullBodyTrackerPN.""" + + def __init__(self, name: str) -> None: + super().__init__(name) + + def _check_instance_compatibility(self, other: TensorType) -> bool: + if not isinstance(other, FullBodyPosePNTrackedType): + raise TypeError( + f"Expected FullBodyPosePNTrackedType, got {type(other).__name__}" + ) + return True + + def validate_value(self, value: Any) -> None: + if not isinstance(value, FullBodyPosePNTrackedT): + raise TypeError( + f"Expected FullBodyPosePNTrackedT for '{self.name}', got {type(value).__name__}" + ) + + class MessageChannelMessagesTrackedType(TensorType): """MessageChannelMessagesTrackedT wrapper type from DeviceIO MessageChannelTracker.""" @@ -256,6 +277,18 @@ def DeviceIOFullBodyPosePicoTracked() -> TensorGroupType: ) +def DeviceIOFullBodyPosePNTracked() -> TensorGroupType: + """Tracked full body pose data from DeviceIO FullBodyTrackerPN. + + Contains: + full_body_tracked: FullBodyPosePNTrackedT wrapper (always set; .data is None when inactive) + """ + return TensorGroupType( + "deviceio_full_body_pose_pn", + [FullBodyPosePNTrackedType("full_body_tracked")], + ) + + def DeviceIOMessageChannelMessagesTracked() -> TensorGroupType: """Tracked message wrapper from DeviceIO MessageChannelTracker.""" return TensorGroupType( diff --git a/src/core/schema/fbs/full_body_pn.fbs b/src/core/schema/fbs/full_body_pn.fbs new file mode 100644 index 000000000..93a192c58 --- /dev/null +++ b/src/core/schema/fbs/full_body_pn.fbs @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "full_body.fbs"; +include "timestamp.fbs"; + +namespace core; + +// Body joint indices for Noitom Perception Neuron full-body mocap samples. +// The order intentionally mirrors the 24 slots consumed by the Noitom G1 example. +enum BodyJointPN : uint8 { + PELVIS = 0, + LEFT_HIP = 1, + RIGHT_HIP = 2, + SPINE1 = 3, + LEFT_KNEE = 4, + RIGHT_KNEE = 5, + SPINE2 = 6, + LEFT_ANKLE = 7, + RIGHT_ANKLE = 8, + SPINE3 = 9, + LEFT_FOOT = 10, + RIGHT_FOOT = 11, + NECK = 12, + LEFT_COLLAR = 13, + RIGHT_COLLAR = 14, + HEAD = 15, + LEFT_SHOULDER = 16, + RIGHT_SHOULDER = 17, + LEFT_ELBOW = 18, + RIGHT_ELBOW = 19, + LEFT_WRIST = 20, + RIGHT_WRIST = 21, + LEFT_HAND = 22, + RIGHT_HAND = 23, + + NUM_JOINTS = 24, +} + +// Fixed-size array of 24 Perception Neuron body joint locations. +struct BodyJointsPN { + joints: [BodyJointPose:24]; +} + +// Full body pose data pushed by the Noitom/Perception Neuron plugin. +table FullBodyPosePN { + joints: BodyJointsPN (id: 0); + + // Quality flag only. When false, individual joint is_valid flags should be consulted. + all_joint_poses_tracked: bool (id: 1); +} + +// Tracked wrapper for the in-memory tracker API (data is null when no PN sample is available). +table FullBodyPosePNTracked { + data: FullBodyPosePN (id: 0); +} + +// MCAP recording wrapper for FullBodyPosePN. +table FullBodyPosePNRecord { + data: FullBodyPosePN (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type FullBodyPosePNRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index 4d02a4e8d..25484d24f 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -5,6 +5,7 @@ pybind11_add_module(schema_py oak_bindings.h controller_bindings.h full_body_bindings.h + full_body_pn_bindings.h hand_bindings.h head_bindings.h joint_state_bindings.h diff --git a/src/core/schema/python/full_body_pn_bindings.h b/src/core/schema/python/full_body_pn_bindings.h new file mode 100644 index 000000000..8010dccef --- /dev/null +++ b/src/core/schema/python/full_body_pn_bindings.h @@ -0,0 +1,127 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the FullBodyPosePN FlatBuffer schema. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_full_body_pn(py::module& m) +{ + py::enum_(m, "BodyJointPN") + .value("PELVIS", BodyJointPN_PELVIS) + .value("LEFT_HIP", BodyJointPN_LEFT_HIP) + .value("RIGHT_HIP", BodyJointPN_RIGHT_HIP) + .value("SPINE1", BodyJointPN_SPINE1) + .value("LEFT_KNEE", BodyJointPN_LEFT_KNEE) + .value("RIGHT_KNEE", BodyJointPN_RIGHT_KNEE) + .value("SPINE2", BodyJointPN_SPINE2) + .value("LEFT_ANKLE", BodyJointPN_LEFT_ANKLE) + .value("RIGHT_ANKLE", BodyJointPN_RIGHT_ANKLE) + .value("SPINE3", BodyJointPN_SPINE3) + .value("LEFT_FOOT", BodyJointPN_LEFT_FOOT) + .value("RIGHT_FOOT", BodyJointPN_RIGHT_FOOT) + .value("NECK", BodyJointPN_NECK) + .value("LEFT_COLLAR", BodyJointPN_LEFT_COLLAR) + .value("RIGHT_COLLAR", BodyJointPN_RIGHT_COLLAR) + .value("HEAD", BodyJointPN_HEAD) + .value("LEFT_SHOULDER", BodyJointPN_LEFT_SHOULDER) + .value("RIGHT_SHOULDER", BodyJointPN_RIGHT_SHOULDER) + .value("LEFT_ELBOW", BodyJointPN_LEFT_ELBOW) + .value("RIGHT_ELBOW", BodyJointPN_RIGHT_ELBOW) + .value("LEFT_WRIST", BodyJointPN_LEFT_WRIST) + .value("RIGHT_WRIST", BodyJointPN_RIGHT_WRIST) + .value("LEFT_HAND", BodyJointPN_LEFT_HAND) + .value("RIGHT_HAND", BodyJointPN_RIGHT_HAND) + .value("NUM_JOINTS", BodyJointPN_NUM_JOINTS); + + py::class_(m, "BodyJointsPN") + .def(py::init<>()) + .def( + "joints", + [](const BodyJointsPN& self, size_t index) -> const BodyJointPose* + { + if (index >= static_cast(BodyJointPN_NUM_JOINTS)) + { + throw py::index_error("BodyJointsPN index out of range (must be 0-23)"); + } + return (*self.joints())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal, + "Get the BodyJointPose at the specified index (0 to NUM_JOINTS-1).") + .def("__repr__", [](const BodyJointsPN&) { return "BodyJointsPN(joints=[...24 BodyJointPose entries...])"; }); + + py::class_>(m, "FullBodyPosePNT") + .def(py::init( + []() + { + auto obj = std::make_shared(); + obj->joints = std::make_shared(); + return obj; + })) + .def(py::init( + [](const BodyJointsPN& joints) + { + auto obj = std::make_shared(); + obj->joints = std::make_shared(joints); + return obj; + }), + py::arg("joints")) + .def_property_readonly( + "joints", [](const FullBodyPosePNT& self) -> const BodyJointsPN* { return self.joints.get(); }, + py::return_value_policy::reference_internal) + .def_readwrite("all_joint_poses_tracked", &FullBodyPosePNT::all_joint_poses_tracked) + .def("__repr__", + [](const FullBodyPosePNT& self) + { + std::string joints_str = self.joints ? "BodyJointsPN(joints=[...24 entries...])" : "None"; + return "FullBodyPosePNT(joints=" + joints_str + ")"; + }); + + py::class_>(m, "FullBodyPosePNRecord") + .def(py::init<>()) + .def(py::init( + [](const FullBodyPosePNT& data, const DeviceDataTimestamp& timestamp) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + obj->timestamp = std::make_shared(timestamp); + return obj; + }), + py::arg("data"), py::arg("timestamp")) + .def_property_readonly( + "data", [](const FullBodyPosePNRecordT& self) -> std::shared_ptr { return self.data; }) + .def_readonly("timestamp", &FullBodyPosePNRecordT::timestamp) + .def("__repr__", [](const FullBodyPosePNRecordT& self) + { return "FullBodyPosePNRecord(data=" + std::string(self.data ? "FullBodyPosePNT(...)" : "None") + ")"; }); + + py::class_>(m, "FullBodyPosePNTrackedT") + .def(py::init<>()) + .def(py::init( + [](const FullBodyPosePNT& data) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + return obj; + }), + py::arg("data")) + .def_property_readonly( + "data", [](const FullBodyPosePNTrackedT& self) -> std::shared_ptr { return self.data; }) + .def("__repr__", + [](const FullBodyPosePNTrackedT& self) { + return std::string("FullBodyPosePNTrackedT(data=") + (self.data ? "FullBodyPosePNT(...)" : "None") + ")"; + }); +} + +} // namespace core diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 478847dad..be5acddf3 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -51,11 +51,16 @@ FrameMetadataOakRecord, # Full body-related types. BodyJointPico, + BodyJointPN, BodyJointPose, BodyJointsPico, + BodyJointsPN, FullBodyPosePicoT, FullBodyPosePicoTrackedT, FullBodyPosePicoRecord, + FullBodyPosePNT, + FullBodyPosePNTrackedT, + FullBodyPosePNRecord, ) @@ -105,7 +110,12 @@ "BodyJointPose", "BodyJointsPico", "BodyJointPico", + "BodyJointPN", + "BodyJointsPN", "FullBodyPosePicoT", "FullBodyPosePicoTrackedT", "FullBodyPosePicoRecord", + "FullBodyPosePNT", + "FullBodyPosePNTrackedT", + "FullBodyPosePNRecord", ] diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index b08e3bfcd..7afe56365 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -8,6 +8,7 @@ // Include binding definitions. #include "controller_bindings.h" #include "full_body_bindings.h" +#include "full_body_pn_bindings.h" #include "hand_bindings.h" #include "head_bindings.h" #include "joint_state_bindings.h" @@ -52,4 +53,7 @@ PYBIND11_MODULE(_schema, m) // Bind full body types (BodyJointPose, BodyJointsPico structs, FullBodyPosePicoT table). core::bind_full_body(m); + + // Bind Perception Neuron full body types. + core::bind_full_body_pn(m); } diff --git a/src/core/schema_tests/cpp/test_full_body.cpp b/src/core/schema_tests/cpp/test_full_body.cpp index 614d94a43..dd9025318 100644 --- a/src/core/schema_tests/cpp/test_full_body.cpp +++ b/src/core/schema_tests/cpp/test_full_body.cpp @@ -9,6 +9,7 @@ // Include generated FlatBuffer headers. #include +#include #include #include @@ -51,6 +52,9 @@ static_assert(sizeof(core::BodyJointsPico) == 24 * sizeof(core::BodyJointPose), static_assert(core::BodyJointPico_PELVIS == 0, "PELVIS should be index 0"); static_assert(core::BodyJointPico_RIGHT_HAND == 23, "RIGHT_HAND should be index 23"); static_assert(core::BodyJointPico_NUM_JOINTS == 24, "NUM_JOINTS should be 24"); +static_assert(core::BodyJointPN_PELVIS == 0, "PN PELVIS should be index 0"); +static_assert(core::BodyJointPN_RIGHT_HAND == 23, "PN RIGHT_HAND should be index 23"); +static_assert(core::BodyJointPN_NUM_JOINTS == 24, "PN NUM_JOINTS should be 24"); // ============================================================================= // BodyJointPose Tests @@ -404,3 +408,23 @@ TEST_CASE("FullBodyPosePicoRecord can be unpacked with DeviceDataTimestamp", "[f CHECK(unpacked->timestamp->sample_time_raw_device_clock() == 333LL); CHECK(unpacked->data->joints->joints()->size() == static_cast(core::BodyJointPico_NUM_JOINTS)); } + +TEST_CASE("FullBodyPosePNRecord serialization with DeviceDataTimestamp", "[full_body_pn][flatbuffers]") +{ + flatbuffers::FlatBufferBuilder builder(4096); + + auto record = std::make_shared(); + record->data = std::make_shared(); + record->data->joints = std::make_unique(); + record->timestamp = std::make_shared(10LL, 20LL, 30LL); + + auto offset = core::FullBodyPosePNRecord::Pack(builder, record.get()); + builder.Finish(offset); + + auto deserialized = flatbuffers::GetRoot(builder.GetBufferPointer()); + + CHECK(deserialized->timestamp()->available_time_local_common_clock() == 10LL); + CHECK(deserialized->timestamp()->sample_time_local_common_clock() == 20LL); + CHECK(deserialized->timestamp()->sample_time_raw_device_clock() == 30LL); + CHECK(deserialized->data()->joints()->joints()->size() == static_cast(core::BodyJointPN_NUM_JOINTS)); +} diff --git a/src/core/schema_tests/python/test_full_body.py b/src/core/schema_tests/python/test_full_body.py index a1a7f32ed..43f3dcd2f 100644 --- a/src/core/schema_tests/python/test_full_body.py +++ b/src/core/schema_tests/python/test_full_body.py @@ -26,9 +26,13 @@ from isaacteleop.schema import ( FullBodyPosePicoT, FullBodyPosePicoRecord, + FullBodyPosePNT, + FullBodyPosePNRecord, BodyJointsPico, + BodyJointsPN, BodyJointPose, BodyJointPico, + BodyJointPN, Pose, Point, Quaternion, @@ -285,3 +289,35 @@ def test_timestamp_fields(self): assert record.timestamp.available_time_local_common_clock == 111 assert record.timestamp.sample_time_local_common_clock == 222 assert record.timestamp.sample_time_raw_device_clock == 333 + + +class TestFullBodyPosePNSchema: + """Tests for Perception Neuron full-body schema bindings.""" + + def test_joint_values_match_expected_slots(self): + """Test that BodyJointPN preserves the 24-slot Noitom full-body layout.""" + assert int(BodyJointPN.PELVIS) == 0 + assert int(BodyJointPN.LEFT_WRIST) == 20 + assert int(BodyJointPN.RIGHT_WRIST) == 21 + assert int(BodyJointPN.LEFT_HAND) == 22 + assert int(BodyJointPN.RIGHT_HAND) == 23 + assert int(BodyJointPN.NUM_JOINTS) == 24 + + def test_default_construction(self): + """Test default construction creates PN pose data with fixed joints.""" + body_pose = FullBodyPosePNT() + + assert body_pose is not None + assert body_pose.joints is not None + assert body_pose.joints.joints(int(BodyJointPN.PELVIS)) is not None + + def test_record_timestamp(self): + """Test FullBodyPosePNRecord carries DeviceDataTimestamp.""" + data = FullBodyPosePNT(BodyJointsPN()) + ts = DeviceDataTimestamp(10, 20, 30) + record = FullBodyPosePNRecord(data, ts) + + assert record.data is not None + assert record.timestamp.available_time_local_common_clock == 10 + assert record.timestamp.sample_time_local_common_clock == 20 + assert record.timestamp.sample_time_raw_device_clock == 30 diff --git a/src/plugins/noitom_mocap/CMakeLists.txt b/src/plugins/noitom_mocap/CMakeLists.txt new file mode 100644 index 000000000..850a9b913 --- /dev/null +++ b/src/plugins/noitom_mocap/CMakeLists.txt @@ -0,0 +1,57 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping noitom_mocap plugin (Linux only)") + add_custom_target(noitom_mocap_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping noitom_mocap: Linux only") + return() +endif() + +set(NOITOM_MOCAP_API_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/third_party/mocap_api" + CACHE PATH "Path to the bundled Noitom MocapApi SDK root") + +if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(aarch64|arm64)$") + set(_NOITOM_MOCAP_LIB_DIR "${NOITOM_MOCAP_API_ROOT}/lib/arm64") +else() + set(_NOITOM_MOCAP_LIB_DIR "${NOITOM_MOCAP_API_ROOT}/lib/x86_64") +endif() +set(_NOITOM_MOCAP_LIB "${_NOITOM_MOCAP_LIB_DIR}/libMocapApi.so") +set(_NOITOM_MOCAP_INCLUDE_DIR "${NOITOM_MOCAP_API_ROOT}/include") + +if(NOT EXISTS "${_NOITOM_MOCAP_LIB}") + message(FATAL_ERROR "Noitom Mocap SDK library not found: ${_NOITOM_MOCAP_LIB}") +endif() +if(NOT EXISTS "${_NOITOM_MOCAP_INCLUDE_DIR}/MocapApi/MocapApi.h") + message(FATAL_ERROR "Noitom Mocap SDK headers not found under: ${_NOITOM_MOCAP_INCLUDE_DIR}") +endif() + +add_executable(noitom_mocap_plugin + main.cpp + noitom_mocap_plugin.cpp +) + +target_include_directories(noitom_mocap_plugin + PRIVATE + "${_NOITOM_MOCAP_INCLUDE_DIR}" +) + +target_link_libraries(noitom_mocap_plugin PRIVATE + "${_NOITOM_MOCAP_LIB}" + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +set_target_properties(noitom_mocap_plugin PROPERTIES + INSTALL_RPATH "$ORIGIN" +) + +install(TARGETS noitom_mocap_plugin RUNTIME DESTINATION plugins/noitom_mocap) +install(FILES plugin.yaml README.md DESTINATION plugins/noitom_mocap) +install( + FILES tools/noitom_mocap_printer.py + DESTINATION plugins/noitom_mocap/tools + PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE +) +install(FILES "${_NOITOM_MOCAP_LIB}" DESTINATION plugins/noitom_mocap) diff --git a/src/plugins/noitom_mocap/README.md b/src/plugins/noitom_mocap/README.md new file mode 100644 index 000000000..8fde68902 --- /dev/null +++ b/src/plugins/noitom_mocap/README.md @@ -0,0 +1,71 @@ + + +# Noitom Mocap Plugin + +Optional plugin that reads Noitom Hybrid Data Server data through the bundled +MocapApi SDK and publishes IsaacTeleop full-body samples over OpenXR tensor +data. + +The plugin publishes the Perception Neuron schema in +`src/core/schema/fbs/full_body_pn.fbs`. Each Noitom avatar update is converted +to `FullBodyPosePN`, pushed on tensor identifier `full_body_pn`, and consumed +with `FullBodyTrackerPN("noitom_mocap")`. + +Positions are read from the Noitom SDK in centimeters and published in meters. +MCAP timestamps use the local monotonic clock for the local/common time fields +and the Noitom avatar PTP timestamp for the raw device clock when the SDK +provides it. + +Build with: + +```bash +cmake -B build -DISAAC_TELEOP_PYTHON_VERSION=3.12 -DBUILD_PLUGIN_NOITOM_MOCAP=ON +cmake --build build --target noitom_mocap_plugin --parallel +cmake --install build +``` + +## Run the Pusher + +Start CloudXR/OpenXR first, keep Axis Studio or the Noitom Data Server +running (TCP mode), then launch the plugin. + +**Before starting the plugin on Linux**, verify the Windows HDS port is open: + +```bash +nc -zv 8001 +``` + +`Connection refused` means HDS is not listening yet — fix Windows broadcast first. + +```bash +./install/plugins/noitom_mocap/noitom_mocap_plugin --host=127.0.0.1 --port=8001 +``` + +The default collection id is `noitom_mocap`. Override it only if the consumer +uses the same value: + +```bash +./install/plugins/noitom_mocap/noitom_mocap_plugin \ + --host= \ + --port= \ + --collection-id=noitom_mocap +``` + +In a second terminal, print frames received through DeviceIO: + +```bash +python src/plugins/noitom_mocap/tools/noitom_mocap_printer.py --duration=10 +``` + +## Record and Replay + +```bash +cmake --build build --target python_package noitom_mocap_plugin --parallel + +python examples/noitom/record_noitom_full_body.py --duration 10 + +python examples/noitom/replay_noitom_full_body.py +``` diff --git a/src/plugins/noitom_mocap/main.cpp b/src/plugins/noitom_mocap/main.cpp new file mode 100644 index 000000000..97449dff7 --- /dev/null +++ b/src/plugins/noitom_mocap/main.cpp @@ -0,0 +1,171 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "noitom_mocap_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::noitom_mocap; + +namespace +{ + +std::atomic g_stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + g_stop_requested.store(true, std::memory_order_relaxed); + } +} + +void print_usage(const char* program_name) +{ + std::cout << "Usage: " << program_name << " [options]\n" + << "\nConnection:\n" + << " --protocol=tcp|udp Noitom HDS connection mode (default: tcp)\n" + << " --host=ADDR TCP server address (default: 127.0.0.1)\n" + << " --port=N TCP server port (default: 8001)\n" + << " --udp-local-port=N UDP local listen port (default: 8002)\n" + << " --udp-server-host=ADDR Optional UDP server address\n" + << " --udp-server-port=N Optional UDP server port (default: 8001)\n" + << "\nDeviceIO:\n" + << " --collection-id=ID Tensor collection id (default: noitom_mocap)\n" + << " --max-flatbuffer-size=N Max serialized frame size (default: " + << DEFAULT_NOITOM_MOCAP_MAX_FLATBUFFER_SIZE << ")\n" + << " --rate=N Poll loop rate in Hz (default: 90)\n" + << "\nGeneral:\n" + << " --help Show this message\n"; +} + +uint16_t parse_u16(const std::string& value, const std::string& name) +{ + int parsed = std::stoi(value); + if (parsed < 0 || parsed > 65535) + { + throw std::runtime_error(name + " must fit uint16"); + } + return static_cast(parsed); +} + +} // namespace + +int main(int argc, char** argv) +try +{ + NoitomMocapPluginConfig config; + double rate_hz = 90.0; + + for (int i = 1; i < argc; ++i) + { + std::string arg = argv[i]; + if (arg == "--help" || arg == "-h") + { + print_usage(argv[0]); + return 0; + } + if (arg == "--plugin-root-id") + { + if (i + 1 >= argc) + { + throw std::runtime_error("--plugin-root-id requires a value"); + } + ++i; + continue; + } + if (arg.find("--protocol=") == 0) + { + std::string value = arg.substr(11); + if (value == "tcp") + config.protocol = MocapProtocol::Tcp; + else if (value == "udp") + config.protocol = MocapProtocol::Udp; + else + throw std::runtime_error("--protocol must be tcp or udp"); + } + else if (arg.find("--host=") == 0) + { + config.host = arg.substr(7); + } + else if (arg.find("--port=") == 0) + { + config.port = parse_u16(arg.substr(7), "--port"); + } + else if (arg.find("--udp-local-port=") == 0) + { + config.udp_local_port = parse_u16(arg.substr(17), "--udp-local-port"); + } + else if (arg.find("--udp-server-host=") == 0) + { + config.udp_server_host = arg.substr(18); + } + else if (arg.find("--udp-server-port=") == 0) + { + config.udp_server_port = parse_u16(arg.substr(18), "--udp-server-port"); + } + else if (arg.find("--collection-id=") == 0) + { + config.collection_id = arg.substr(16); + } + else if (arg.find("--max-flatbuffer-size=") == 0) + { + config.max_flatbuffer_size = static_cast(std::stoull(arg.substr(22))); + } + else if (arg.find("--rate=") == 0) + { + rate_hz = std::stod(arg.substr(7)); + if (rate_hz <= 0.0) + { + throw std::runtime_error("--rate must be positive"); + } + } + else if (arg.find("--plugin-root-id=") == 0) + { + continue; + } + else + { + std::cerr << "Unknown option: " << arg << std::endl; + print_usage(argv[0]); + return 1; + } + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + NoitomMocapPlugin plugin(config); + + const auto frame_duration = std::chrono::nanoseconds(static_cast(1000000000.0 / rate_hz)); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t frame_count = 0; + + while (!g_stop_requested.load(std::memory_order_relaxed)) + { + if (!plugin.update()) + { + return 1; + } + ++frame_count; + std::this_thread::sleep_until(program_start + frame_duration * frame_count); + } + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp new file mode 100644 index 000000000..851071f73 --- /dev/null +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp @@ -0,0 +1,545 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "noitom_mocap_plugin.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace noitom_mocap +{ + +namespace +{ + +constexpr float SDK_CENTIMETERS_TO_METERS = 0.01f; +constexpr const char* ANSI_ORANGE = "\033[38;5;208m"; +constexpr const char* ANSI_RESET = "\033[0m"; + +core::Point make_point(float x, float y, float z) +{ + // Noitom live position getters produce centimeter-scale values; publish schema positions in meters. + return core::Point(x * SDK_CENTIMETERS_TO_METERS, y * SDK_CENTIMETERS_TO_METERS, z * SDK_CENTIMETERS_TO_METERS); +} + +core::Quaternion make_quaternion(float x, float y, float z, float w) +{ + return core::Quaternion(x, y, z, w); +} + +std::string normalize_joint_name(std::string_view name) +{ + std::string result; + result.reserve(name.size()); + for (char c : name) + { + if (c == ' ' || c == '_' || c == '-') + { + continue; + } + result.push_back(static_cast(std::tolower(static_cast(c)))); + } + return result; +} + +std::optional map_noitom_joint_name(std::string_view name) +{ + const std::string normalized = normalize_joint_name(name); + if (normalized == "hips" || normalized == "hip" || normalized == "pelvis") + return core::BodyJointPN_PELVIS; + if (normalized == "leftupleg" || normalized == "lefthip") + return core::BodyJointPN_LEFT_HIP; + if (normalized == "rightupleg" || normalized == "righthip") + return core::BodyJointPN_RIGHT_HIP; + if (normalized == "spine") + return core::BodyJointPN_SPINE1; + if (normalized == "spine1") + return core::BodyJointPN_SPINE2; + if (normalized == "spine2" || normalized == "chest") + return core::BodyJointPN_SPINE3; + if (normalized == "leftleg" || normalized == "leftknee") + return core::BodyJointPN_LEFT_KNEE; + if (normalized == "rightleg" || normalized == "rightknee") + return core::BodyJointPN_RIGHT_KNEE; + if (normalized == "leftfoot" || normalized == "leftankle") + return core::BodyJointPN_LEFT_ANKLE; + if (normalized == "rightfoot" || normalized == "rightankle") + return core::BodyJointPN_RIGHT_ANKLE; + if (normalized == "lefttoebase" || normalized == "lefttoe") + return core::BodyJointPN_LEFT_FOOT; + if (normalized == "righttoebase" || normalized == "righttoe") + return core::BodyJointPN_RIGHT_FOOT; + if (normalized == "neck") + return core::BodyJointPN_NECK; + if (normalized == "head") + return core::BodyJointPN_HEAD; + if (normalized == "leftshoulder" || normalized == "leftcollar") + return core::BodyJointPN_LEFT_COLLAR; + if (normalized == "rightshoulder" || normalized == "rightcollar") + return core::BodyJointPN_RIGHT_COLLAR; + if (normalized == "leftarm" || normalized == "leftupperarm") + return core::BodyJointPN_LEFT_SHOULDER; + if (normalized == "rightarm" || normalized == "rightupperarm") + return core::BodyJointPN_RIGHT_SHOULDER; + if (normalized == "leftforearm" || normalized == "leftelbow") + return core::BodyJointPN_LEFT_ELBOW; + if (normalized == "rightforearm" || normalized == "rightelbow") + return core::BodyJointPN_RIGHT_ELBOW; + if (normalized == "lefthand" || normalized == "leftwrist") + return core::BodyJointPN_LEFT_WRIST; + if (normalized == "righthand" || normalized == "rightwrist") + return core::BodyJointPN_RIGHT_WRIST; + if (normalized == "lefthandend" || normalized == "lefthandtip") + return core::BodyJointPN_LEFT_HAND; + if (normalized == "righthandend" || normalized == "righthandtip") + return core::BodyJointPN_RIGHT_HAND; + return std::nullopt; +} + +core::BodyJointPose make_invalid_body_joint_pose() +{ + return core::BodyJointPose(core::Pose(core::Point(), core::Quaternion()), false); +} + +int64_t ptp_to_nanoseconds(uint32_t second, uint32_t nanosecond) +{ + if (second == 0 && nanosecond == 0) + { + return 0; + } + return static_cast(second) * 1000000000LL + static_cast(nanosecond); +} + +std::string error_name(MocapApi::EMCPError err) +{ + switch (err) + { + case MocapApi::Error_None: + return "None"; + case MocapApi::Error_MoreEvent: + return "MoreEvent"; + case MocapApi::Error_ServerNotReady: + return "ServerNotReady"; + case MocapApi::Error_ClientNotReady: + return "ClientNotReady"; + case MocapApi::Error_TCP: + return "TCP"; + case MocapApi::Error_UDP: + return "UDP"; + default: + return "code_" + std::to_string(static_cast(err)); + } +} + +std::string error_string(MocapApi::EMCPError err) +{ + return "MocapApi " + error_name(err) + " (" + std::to_string(static_cast(err)) + ")"; +} + +void check_mocap(MocapApi::EMCPError err, const std::string& context) +{ + if (err != MocapApi::Error_None) + { + throw std::runtime_error(context + ": " + error_string(err)); + } +} + +void warn_optional_ptp_missing_once() +{ + static bool warned = false; + if (warned) + { + return; + } + warned = true; + std::cerr << ANSI_ORANGE + << "NoitomMocapPlugin: warning: avatar PTP timestamp is unavailable; publishing frames with ptp=0" + << ANSI_RESET << std::endl; +} + +} // namespace + +NoitomMocapPlugin::NoitomMocapPlugin(NoitomMocapPluginConfig config) + : config_(std::move(config)), + session_(std::make_shared("NoitomMocapPlugin", core::SchemaPusher::get_required_extensions())) +{ + initialize_mocap(); +} + +NoitomMocapPlugin::~NoitomMocapPlugin() +{ + close_mocap(); +} + +template +InterfaceT* NoitomMocapPlugin::get_interface(const char* version) +{ + void* iface = nullptr; + check_mocap( + MocapApi::MCPGetGenericInterface(version, &iface), std::string("MCPGetGenericInterface(") + version + ")"); + if (!iface) + { + throw std::runtime_error(std::string("MCPGetGenericInterface(") + version + ") returned null"); + } + return static_cast(iface); +} + +void NoitomMocapPlugin::initialize_mocap() +{ + settings_api_ = get_interface(MocapApi::IMCPSettings_Version); + application_api_ = get_interface(MocapApi::IMCPApplication_Version); + avatar_api_ = get_interface(MocapApi::IMCPAvatar_Version); + joint_api_ = get_interface(MocapApi::IMCPJoint_Version); + render_settings_api_ = get_interface(MocapApi::IMCPRenderSettings_Version); + + check_mocap(settings_api_->CreateSettings(&settings_handle_), "CreateSettings"); + if (config_.protocol == MocapProtocol::Tcp) + { + check_mocap( + settings_api_->SetSettingsTCP(config_.host.c_str(), config_.port, settings_handle_), "SetSettingsTCP"); + } + else + { + check_mocap(settings_api_->SetSettingsUDP(config_.udp_local_port, settings_handle_), "SetSettingsUDP"); + if (!config_.udp_server_host.empty()) + { + check_mocap(settings_api_->SetSettingsUDPServer( + config_.udp_server_host.c_str(), config_.udp_server_port, settings_handle_), + "SetSettingsUDPServer"); + } + } + check_mocap( + settings_api_->SetSettingsBvhRotation(MocapApi::BvhRotation_XYZ, settings_handle_), "SetSettingsBvhRotation"); + check_mocap(render_settings_api_->CreateRenderSettings(&render_settings_handle_), "CreateRenderSettings"); + check_mocap( + render_settings_api_->SetUnit(MocapApi::Unit_Centimeter, render_settings_handle_), "SetUnit(Unit_Centimeter)"); + + MocapApi::EMCPUnit unit = MocapApi::Uint_Meter; + check_mocap(render_settings_api_->GetUnit(&unit, render_settings_handle_), "GetUnit"); + if (unit != MocapApi::Unit_Centimeter) + { + throw std::runtime_error("NoitomMocapPlugin: failed to configure SDK position units to centimeters"); + } + + check_mocap(application_api_->CreateApplication(&application_handle_), "CreateApplication"); + check_mocap( + application_api_->SetApplicationSettings(settings_handle_, application_handle_), "SetApplicationSettings"); + check_mocap(application_api_->SetApplicationRenderSettings(render_settings_handle_, application_handle_), + "SetApplicationRenderSettings"); + check_mocap(application_api_->OpenApplication(application_handle_), "OpenApplication"); + application_open_ = true; + + bool cache_events_enabled = false; + const auto cache_err = application_api_->EnableApplicationCacheEvents(application_handle_); + if (cache_err == MocapApi::Error_None) + { + check_mocap(application_api_->ApplicationCacheEventsIsEnabled(&cache_events_enabled, application_handle_), + "ApplicationCacheEventsIsEnabled"); + } + else if (cache_err == MocapApi::Error_NotSupported) + { + std::cerr << ANSI_ORANGE + << "NoitomMocapPlugin: warning: SDK application event cache is not supported; " + "continuing with polling" + << ANSI_RESET << std::endl; + } + else + { + check_mocap(cache_err, "EnableApplicationCacheEvents"); + } + + std::cout << "NoitomMocapPlugin: connected via " << (config_.protocol == MocapProtocol::Tcp ? "TCP" : "UDP") + << ", collection=" << config_.collection_id << ", sdk_units=centimeters, output_units=meters, event_cache=" + << (cache_events_enabled ? "enabled" : "disabled") << std::endl; +} + +void NoitomMocapPlugin::close_mocap() +{ + if (application_api_ && application_handle_) + { + if (application_open_) + { + application_api_->CloseApplication(application_handle_); + application_open_ = false; + } + application_api_->DestroyApplication(application_handle_); + application_handle_ = 0; + } + + if (settings_api_ && settings_handle_) + { + settings_api_->DestroySettings(settings_handle_); + settings_handle_ = 0; + } + + if (render_settings_api_ && render_settings_handle_) + { + render_settings_api_->DestroyRenderSettings(render_settings_handle_); + render_settings_handle_ = 0; + } +} + +std::vector NoitomMocapPlugin::poll_events() +{ + uint32_t event_count = 0; + MocapApi::EMCPError err = application_api_->PollApplicationNextEvent(nullptr, &event_count, application_handle_); + if (err != MocapApi::Error_None && err != MocapApi::Error_MoreEvent) + { + throw std::runtime_error("PollApplicationNextEvent(count): " + error_string(err)); + } + if (event_count == 0) + { + return {}; + } + + std::vector events(event_count); + for (auto& event : events) + { + event.size = sizeof(MocapApi::MCPEvent_t); + } + + err = application_api_->PollApplicationNextEvent(events.data(), &event_count, application_handle_); + if (err != MocapApi::Error_None && err != MocapApi::Error_MoreEvent) + { + throw std::runtime_error("PollApplicationNextEvent(events): " + error_string(err)); + } + events.resize(event_count); + return events; +} + +std::vector NoitomMocapPlugin::poll_avatars() +{ + uint32_t avatar_count = 0; + auto err = application_api_->GetApplicationAvatars(nullptr, &avatar_count, application_handle_); + if (err != MocapApi::Error_None) + { + throw std::runtime_error("GetApplicationAvatars(count): " + error_string(err)); + } + if (avatar_count == 0) + { + if (!warned_no_avatars_) + { + warned_no_avatars_ = true; + std::cerr << ANSI_ORANGE + << "NoitomMocapPlugin: warning: SDK reports zero avatars; waiting for HDS avatar data" + << ANSI_RESET << std::endl; + } + return {}; + } + + warned_no_avatars_ = false; + std::vector avatars(avatar_count); + check_mocap(application_api_->GetApplicationAvatars(avatars.data(), &avatar_count, application_handle_), + "GetApplicationAvatars"); + avatars.resize(avatar_count); + return avatars; +} + +bool NoitomMocapPlugin::handle_avatar(MocapApi::MCPAvatarHandle_t avatar_handle) +{ + uint32_t avatar_index = 0; + uint32_t posture_index = 0; + uint32_t ptp_second = 0; + uint32_t ptp_nanosecond = 0; + check_mocap(avatar_api_->GetAvatarIndex(&avatar_index, avatar_handle), "GetAvatarIndex"); + check_mocap(avatar_api_->GetAvatarPostureIndex(&posture_index, avatar_handle), "GetAvatarPostureIndex"); + const MocapApi::EMCPError ptp_err = avatar_api_->GetAvatarPosturePtpTime(&ptp_second, &ptp_nanosecond, avatar_handle); + if (ptp_err == MocapApi::Error_NoneMessage) + { + warn_optional_ptp_missing_once(); + } + else + { + check_mocap(ptp_err, "GetAvatarPosturePtpTime"); + } + latest_sample_time_raw_device_clock_ns_ = ptp_to_nanoseconds(ptp_second, ptp_nanosecond); + + uint32_t joint_count = 0; + auto err = avatar_api_->GetAvatarJoints(nullptr, &joint_count, avatar_handle); + if (err != MocapApi::Error_None || joint_count == 0) + { + frame_.joints.reset(); + return false; + } + + std::vector joints(joint_count); + check_mocap(avatar_api_->GetAvatarJoints(joints.data(), &joint_count, avatar_handle), "GetAvatarJoints"); + + frame_.joints = std::make_shared(); + frame_.all_joint_poses_tracked = false; + std::array seen{}; + for (uint8_t i = 0; i < static_cast(core::BodyJointPN_NUM_JOINTS); ++i) + { + frame_.joints->mutable_joints()->Mutate(i, make_invalid_body_joint_pose()); + } + + for (auto handle : joints) + { + const char* name = nullptr; + float gx = 0.0f; + float gy = 0.0f; + float gz = 0.0f; + float gqx = 0.0f; + float gqy = 0.0f; + float gqz = 0.0f; + float gqw = 1.0f; + + bool valid = true; + valid &= (joint_api_->GetJointName(&name, handle) == MocapApi::Error_None); + valid &= (joint_api_->GetJointGlobalPosition(&gx, &gy, &gz, handle) == MocapApi::Error_None); + valid &= (joint_api_->GetJointGlobalRotation(&gqx, &gqy, &gqz, &gqw, handle) == MocapApi::Error_None); + + const auto slot = map_noitom_joint_name(name ? name : ""); + if (!slot) + { + continue; + } + + const auto index = static_cast(*slot); + core::Pose pose(make_point(gx, gy, gz), make_quaternion(gqx, gqy, gqz, gqw)); + core::BodyJointPose joint_pose(pose, valid); + frame_.joints->mutable_joints()->Mutate(index, joint_pose); + seen[index] = valid; + + if (*slot == core::BodyJointPN_LEFT_WRIST) + { + frame_.joints->mutable_joints()->Mutate(core::BodyJointPN_LEFT_HAND, joint_pose); + seen[core::BodyJointPN_LEFT_HAND] = valid; + } + else if (*slot == core::BodyJointPN_RIGHT_WRIST) + { + frame_.joints->mutable_joints()->Mutate(core::BodyJointPN_RIGHT_HAND, joint_pose); + seen[core::BodyJointPN_RIGHT_HAND] = valid; + } + else if (*slot == core::BodyJointPN_LEFT_ANKLE) + { + frame_.joints->mutable_joints()->Mutate(core::BodyJointPN_LEFT_FOOT, joint_pose); + seen[core::BodyJointPN_LEFT_FOOT] = valid; + } + else if (*slot == core::BodyJointPN_RIGHT_ANKLE) + { + frame_.joints->mutable_joints()->Mutate(core::BodyJointPN_RIGHT_FOOT, joint_pose); + seen[core::BodyJointPN_RIGHT_FOOT] = valid; + } + } + + frame_.all_joint_poses_tracked = std::all_of(seen.begin(), seen.end(), [](bool value) { return value; }); + std::cout << "NoitomMocapPlugin: converted avatar=" << avatar_index << " posture=" << posture_index + << " valid_full_body_joints=" << std::count(seen.begin(), seen.end(), true) << "/" + << static_cast(core::BodyJointPN_NUM_JOINTS) << std::endl; + return true; +} + +bool NoitomMocapPlugin::update() +{ + try + { + auto events = poll_events(); + bool should_push = false; + + for (const auto& event : events) + { + switch (event.eventType) + { + case MocapApi::MCPEvent_AvatarUpdated: + should_push = handle_avatar(event.eventData.motionData.avatarHandle) || should_push; + break; + case MocapApi::MCPEvent_Error: + { + const auto sdk_err = event.eventData.systemError.error; + std::cerr << ANSI_ORANGE << "NoitomMocapPlugin: warning: SDK error event " << error_string(sdk_err); + if (sdk_err == MocapApi::Error_ServerNotReady) + { + std::cerr << " — Hybrid Data Server is not streaming avatar data yet. " + "On Windows: start Axis Studio calibration, then enable HDS TCP " + "broadcast on this port"; + } + std::cerr << ANSI_RESET << std::endl; + break; + } + default: + break; + } + } + + if (!should_push) + { + for (auto avatar_handle : poll_avatars()) + { + should_push = handle_avatar(avatar_handle) || should_push; + } + } + + if (should_push) + { + const int64_t sample_time_ns = core::os_monotonic_now_ns(); + const int64_t raw_device_time_ns = + latest_sample_time_raw_device_clock_ns_ == 0 ? sample_time_ns : latest_sample_time_raw_device_clock_ns_; + push_frame(sample_time_ns, raw_device_time_ns); + } + return true; + } + catch (const std::exception& e) + { + std::cerr << "NoitomMocapPlugin: fatal: " << e.what() << " (check HDS TCP broadcast and: nc -zv " + << config_.host << " " << config_.port << ")" << std::endl; + return false; + } + catch (...) + { + std::cerr << "NoitomMocapPlugin: fatal: Noitom SDK connection lost (software caused connection abort). " + << "Ensure Windows HDS is broadcasting TCP on " << config_.host << ":" << config_.port + << " and verify with: nc -zv " << config_.host << " " << config_.port << std::endl; + return false; + } +} + +void NoitomMocapPlugin::ensure_pusher(size_t flatbuffer_size) +{ + if (flatbuffer_size == 0 || flatbuffer_size > config_.max_flatbuffer_size) + { + throw std::runtime_error("NoitomMocapPlugin: serialized frame size " + std::to_string(flatbuffer_size) + + " exceeds max_flatbuffer_size " + std::to_string(config_.max_flatbuffer_size)); + } + + if (pusher_) + { + return; + } + + // Keep the OpenXR tensor collection stable. SchemaPusher pads smaller samples + // to max_flatbuffer_size before publishing. + pusher_ = std::make_unique( + session_->get_handles(), core::SchemaPusherConfig{ .collection_id = config_.collection_id, + .max_flatbuffer_size = config_.max_flatbuffer_size, + .tensor_identifier = "full_body_pn", + .localized_name = "Noitom Full Body PN", + .app_name = "NoitomMocapPlugin" }); + std::cout << "NoitomMocapPlugin: push tensor sample size set to " << config_.max_flatbuffer_size << " bytes" + << std::endl; +} + +void NoitomMocapPlugin::push_frame(int64_t sample_time_local_common_clock_ns, int64_t sample_time_raw_device_clock_ns) +{ + flatbuffers::FlatBufferBuilder builder(config_.max_flatbuffer_size); + auto offset = core::FullBodyPosePN::Pack(builder, &frame_); + builder.Finish(offset); + ensure_pusher(builder.GetSize()); + pusher_->push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_local_common_clock_ns, + sample_time_raw_device_clock_ns); +} + +} // namespace noitom_mocap +} // namespace plugins diff --git a/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp b/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp new file mode 100644 index 000000000..55718f804 --- /dev/null +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace noitom_mocap +{ + +static constexpr size_t DEFAULT_NOITOM_MOCAP_MAX_FLATBUFFER_SIZE = 16 * 1024; + +enum class MocapProtocol +{ + Tcp, + Udp, +}; + +struct NoitomMocapPluginConfig +{ + MocapProtocol protocol = MocapProtocol::Tcp; + std::string host = "127.0.0.1"; + uint16_t port = 8001; + uint16_t udp_local_port = 8002; + std::string udp_server_host; + uint16_t udp_server_port = 8001; + std::string collection_id = "noitom_mocap"; + size_t max_flatbuffer_size = DEFAULT_NOITOM_MOCAP_MAX_FLATBUFFER_SIZE; +}; + +class NoitomMocapPlugin +{ +public: + explicit NoitomMocapPlugin(NoitomMocapPluginConfig config); + ~NoitomMocapPlugin(); + + // Returns false when the Noitom SDK connection is lost (caller should exit). + bool update(); + +private: + template + InterfaceT* get_interface(const char* version); + + void initialize_mocap(); + void close_mocap(); + std::vector poll_events(); + std::vector poll_avatars(); + bool handle_avatar(MocapApi::MCPAvatarHandle_t avatar_handle); + void ensure_pusher(size_t flatbuffer_size); + void push_frame(int64_t sample_time_local_common_clock_ns, int64_t sample_time_raw_device_clock_ns); + + NoitomMocapPluginConfig config_; + std::shared_ptr session_; + std::unique_ptr pusher_; + + MocapApi::IMCPSettings* settings_api_ = nullptr; + MocapApi::IMCPApplication* application_api_ = nullptr; + MocapApi::IMCPAvatar* avatar_api_ = nullptr; + MocapApi::IMCPJoint* joint_api_ = nullptr; + MocapApi::IMCPRenderSettings* render_settings_api_ = nullptr; + + MocapApi::MCPSettingsHandle_t settings_handle_ = 0; + MocapApi::MCPRenderSettingsHandle_t render_settings_handle_ = 0; + MocapApi::MCPApplicationHandle_t application_handle_ = 0; + bool application_open_ = false; + bool warned_no_avatars_ = false; + + core::FullBodyPosePNT frame_; + int64_t latest_sample_time_raw_device_clock_ns_ = 0; +}; + +} // namespace noitom_mocap +} // namespace plugins diff --git a/src/plugins/noitom_mocap/plugin.yaml b/src/plugins/noitom_mocap/plugin.yaml new file mode 100644 index 000000000..d353c4333 --- /dev/null +++ b/src/plugins/noitom_mocap/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: noitom_mocap +description: "Noitom mocap stream via Hybrid Data Server and OpenXR tensor data" +command: "./noitom_mocap_plugin" +version: "0.1.0" +devices: + - path: "/mocap/noitom" + type: "noitom_mocap" + description: "Noitom avatar joints and PWR trackers" diff --git a/src/plugins/noitom_mocap/third_party/.gitignore b/src/plugins/noitom_mocap/third_party/.gitignore new file mode 100644 index 000000000..2b266c48b --- /dev/null +++ b/src/plugins/noitom_mocap/third_party/.gitignore @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The Noitom MocapApi SDK +!mocap_api/lib/arm64/libMocapApi.so +!mocap_api/lib/x86_64/libMocapApi.so diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/__init__.py b/src/plugins/noitom_mocap/third_party/mocap_api/__init__.py new file mode 100644 index 000000000..76c7344af --- /dev/null +++ b/src/plugins/noitom_mocap/third_party/mocap_api/__init__.py @@ -0,0 +1,125 @@ +# mocap/__init__.py + +# Import all objects that need to be exported from mocap_api +from .mocap_api import ( + MCPError, + MCPJointTag, + MCPRigidBodyHandle, + MCPRigidBody, + MCPSensorModuleHandle, + MCPSensorModule, + MCPBodyPartHandle, + MCPBodyPart, + MCPJointHandle, + MCPJoint, + MCPAvatarHandle, + MCPAvatar, + MCPCalibrateMotionProgressStep, + MCPCalibrateMotionProgressHandle, + MCPCalibrateMotionProgress, + MCPCommandProgress, + MCPCommandHandle, + MCPCommand, + MCPEventDataReserved, + MCPEventCommandRespond, + MCPReplay, + MCPPWRHandle, + MCPPWR, + MCPMarkerHandle, + MCPAliceBusHandle, + MCPAliceHub, + MCPSensorModuleHandle, + MCPSystemHandle, + MCPSystem, + MCPEventNotify, + MCPEventNotifyData, + MCPTrackerHandle, + MCPEventData, + MCPEventType, + MCPEvent, + MCPBvhRotation, + MCPBvhData, + MCPBvhDisplacement, + MCPTracker, + MCPMarker, + MCPSettingsHandle, + MCPSettings, + MCPUpVector, + MCPFrontVector, + MCPCoordSystem, + MCPRotatingDirection, + MCPPreDefinedRenderSettings, + MCPUnit, + MCPRenderSettingsHandle, + MCPRenderSettings, + MCPRobotJointType, + MCPRobotHandle, + MCPRobot, + EMCPCommand, + MCPApplicationHandle, + MCPTrackerHandle, + MCPEventHandleProc, + MCPApplication +) + +# Define package-level __all__ to control what's exported when using 'from mocap import *' +__all__ = [ + 'MCPError', + 'MCPJointTag', + 'MCPRigidBodyHandle', + 'MCPRigidBody', + 'MCPSensorModuleHandle', + 'MCPSensorModule', + 'MCPBodyPartHandle', + 'MCPBodyPart', + 'MCPJointHandle', + 'MCPJoint', + 'MCPAvatarHandle', + 'MCPAvatar', + 'MCPCalibrateMotionProgressStep', + 'MCPCalibrateMotionProgressHandle', + 'MCPCalibrateMotionProgress', + 'MCPCommandProgress', + 'MCPCommandHandle', + 'MCPCommand', + 'MCPEventDataReserved', + 'MCPEventCommandRespond', + 'MCPReplay', + 'MCPPWRHandle', + 'MCPPWR', + 'MCPMarkerHandle', + 'MCPAliceBusHandle', + 'MCPAliceHub', + 'MCPSensorModuleHandle', + 'MCPSystemHandle', + 'MCPSystem', + 'MCPEventNotify', + 'MCPEventNotifyData', + 'MCPTrackerHandle', + 'MCPEventData', + 'MCPEventType', + 'MCPEvent', + 'MCPBvhRotation', + 'MCPBvhData', + 'MCPBvhDisplacement', + 'MCPTracker', + 'MCPMarker', + 'MCPSettingsHandle', + 'MCPSettings', + 'MCPUpVector', + 'MCPFrontVector', + 'MCPCoordSystem', + 'MCPRotatingDirection', + 'MCPPreDefinedRenderSettings', + 'MCPUnit', + 'MCPRenderSettingsHandle', + 'MCPRenderSettings', + 'MCPRobotJointType', + 'MCPRobotHandle', + 'MCPRobot', + 'EMCPCommand', + 'MCPApplicationHandle', + 'MCPTrackerHandle', + 'MCPEventHandleProc', + 'MCPApplication' +] \ No newline at end of file diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/include/MocapApi/MocapApi.h b/src/plugins/noitom_mocap/third_party/mocap_api/include/MocapApi/MocapApi.h new file mode 100644 index 000000000..2560fae81 --- /dev/null +++ b/src/plugins/noitom_mocap/third_party/mocap_api/include/MocapApi/MocapApi.h @@ -0,0 +1,641 @@ +// clang-format off +#ifndef _NOITOM_MOCAPAPI_H +#define _NOITOM_MOCAPAPI_H + +#include +#if defined(_WIN32) +#define MCP_CALLTYPE __cdecl +#ifdef MCP_API_EXPORT +#define MCP_INTERFACE extern "C" __declspec( dllexport ) +#elif (defined MCP_API_STATIC) +#define MCP_INTERFACE extern "C" +#else +#define MCP_INTERFACE extern "C" __declspec( dllimport ) +#endif +#elif defined(__GNUC__) || defined(COMPILER_GCC) || defined(__APPLE__) +#define MCP_CALLTYPE +#ifdef MCP_API_EXPORT +#define MCP_INTERFACE extern "C" __attribute__((visibility("default"))) +#else +#define MCP_INTERFACE extern "C" +#endif +#else +#error "Unsupported Platform." +#endif + +#define MOCAP_API_VERSION_MAJOR 0 +#define MOCAP_API_VERSION_MINOR 0 +#define MOCAP_API_VERSION_BUILD 73 +#define MOCAP_API_VERSION_REVISION 19a54f85 + +namespace MocapApi +{ + enum EMCPError + { + Error_None=0, + Error_MoreEvent=1, + Error_InsufficientBuffer=2, + Error_InvalidObject=3, + Error_InvalidHandle=4, + Error_InvalidParameter=5, + Error_NotSupported=6, + Error_IgnoreUDPSetting=7, + Error_IgnoreTCPSetting=8, + Error_IgnoreBvhSetting=9, + Error_JointNotFound=10, + Error_WithoutTransformation=11, + Error_NoneMessage=12, + Error_NoneParent=13, + Error_NoneChild=14, + Error_AddressInUse=15, + Error_ServerNotReady=16, + Error_ClientNotReady=17, + Error_IncompleteCommand=18, + Error_UDP=19, + Error_TCP=20, + Error_QueuedCommandFaild=21, + Error_InterfaceIncompatible=22 + }; + enum EMCPJointTag + { + JointTag_Invalid=-1, + JointTag_Hips=0, + JointTag_RightUpLeg=1, + JointTag_RightLeg=2, + JointTag_RightFoot=3, + JointTag_LeftUpLeg=4, + JointTag_LeftLeg=5, + JointTag_LeftFoot=6, + JointTag_Spine=7, + JointTag_Spine1=8, + JointTag_Spine2=9, + JointTag_Neck=10, + JointTag_Neck1=11, + JointTag_Head=12, + JointTag_RightShoulder=13, + JointTag_RightArm=14, + JointTag_RightForeArm=15, + JointTag_RightHand=16, + JointTag_RightHandThumb1=17, + JointTag_RightHandThumb2=18, + JointTag_RightHandThumb3=19, + JointTag_RightInHandIndex=20, + JointTag_RightHandIndex1=21, + JointTag_RightHandIndex2=22, + JointTag_RightHandIndex3=23, + JointTag_RightInHandMiddle=24, + JointTag_RightHandMiddle1=25, + JointTag_RightHandMiddle2=26, + JointTag_RightHandMiddle3=27, + JointTag_RightInHandRing=28, + JointTag_RightHandRing1=29, + JointTag_RightHandRing2=30, + JointTag_RightHandRing3=31, + JointTag_RightInHandPinky=32, + JointTag_RightHandPinky1=33, + JointTag_RightHandPinky2=34, + JointTag_RightHandPinky3=35, + JointTag_LeftShoulder=36, + JointTag_LeftArm=37, + JointTag_LeftForeArm=38, + JointTag_LeftHand=39, + JointTag_LeftHandThumb1=40, + JointTag_LeftHandThumb2=41, + JointTag_LeftHandThumb3=42, + JointTag_LeftInHandIndex=43, + JointTag_LeftHandIndex1=44, + JointTag_LeftHandIndex2=45, + JointTag_LeftHandIndex3=46, + JointTag_LeftInHandMiddle=47, + JointTag_LeftHandMiddle1=48, + JointTag_LeftHandMiddle2=49, + JointTag_LeftHandMiddle3=50, + JointTag_LeftInHandRing=51, + JointTag_LeftHandRing1=52, + JointTag_LeftHandRing2=53, + JointTag_LeftHandRing3=54, + JointTag_LeftInHandPinky=55, + JointTag_LeftHandPinky1=56, + JointTag_LeftHandPinky2=57, + JointTag_LeftHandPinky3=58, + JointTag_Spine3=59, + JointTag_JointsCount=60 + }; + typedef uint64_t MCPRigidBodyHandle_t; + class IMCPRigidBody + { + public: + virtual EMCPError GetRigidBodyRotation(float * x, float * y, float * z, float * w, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyPosition(float * x, float * y, float * z, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyStatus(int * status, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyId(int * id, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyJointTag(EMCPJointTag * jointTag_, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyAxisAngle(float * x, float * y, float * z, float * angle, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyType(uint32_t * type, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + virtual EMCPError GetRigidBodyName(const char ** name, MCPRigidBodyHandle_t ulRigidBodyHandle) = 0; + }; + static const char * IMCPRigidBody_Version = "IMCPRigidBody_002"; + typedef uint64_t MCPTrackerHandle_t; + class IMCPTracker + { + public: + virtual EMCPError SendMessageData(const char * message, int len, MCPTrackerHandle_t ulTrackerHandle) = 0; + virtual EMCPError GetTrackerRotation(float * x, float * y, float * z, float * w, const char * deviceName, MCPTrackerHandle_t ulTrackerHandle) = 0; + virtual EMCPError GetTrackerPosition(float * x, float * y, float * z, const char * deviceName, MCPTrackerHandle_t ulTrackerHandle) = 0; + virtual EMCPError GetTrackerEulerAng(float * x, float * y, float * z, const char * deviceName, MCPTrackerHandle_t ulTrackerHandle) = 0; + virtual EMCPError GetDeviceCount(int * devCount, MCPTrackerHandle_t ulTrackerHandle) = 0; + virtual EMCPError GetDeviceName(int serialNum, const char ** name, MCPTrackerHandle_t ulTrackerHandle) = 0; + }; + static const char * IMCPTracker_Version = "IMCPTracker_001"; + enum EMCPMagEnv + { + kMagEnv_Unknown=-1, + kMagEnv_Worst=0, + kMagEnv_Worse=1, + kMagEnv_Bad=2, + kMagEnv_Good=3 + }; + typedef uint64_t MCPSensorModuleHandle_t; + class IMCPSensorModule + { + public: + virtual EMCPError GetSensorModulePosture(float * x, float * y, float * z, float * w, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleAngularVelocity(float * x, float * y, float * z, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleAcceleratedVelocity(float * x, float * y, float * z, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleId(uint32_t * id, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleCompassValue(float * x, float * y, float * z, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleTemperature(float * temperature, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModulePostureTimeCode(uint32_t * hour, uint32_t * minute, uint32_t * second, uint32_t * frame, uint32_t * rate, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModulePostureTime(uint32_t * hour, uint32_t * minute, uint32_t * second, uint32_t * millisecond, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleBoundBone(EMCPJointTag * bone, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleMagEnvValue(EMCPMagEnv * pMagEnvValue, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSesnorModulePacketRate(float * pRate, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleSerialNumber(const char ** serialNumber, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleFirmwareVersionString(const char ** firmwareVersion, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + virtual EMCPError GetSensorModuleFirmwareVersion(uint32_t * major, uint32_t * minor, uint32_t * revision, uint32_t * build, MCPSensorModuleHandle_t sensorModuleHandle) = 0; + }; + static const char * IMCPSensorModule_Version = "IMCPSensorModule_002"; + typedef uint64_t MCPBodyPartHandle_t; + class IMCPBodyPart + { + public: + virtual EMCPError GetJointPosition(float * x, float * y, float * z, MCPBodyPartHandle_t bodyPartHandle) = 0; + virtual EMCPError GetJointDisplacementSpeed(float * x, float * y, float * z, MCPBodyPartHandle_t bodyPartHandle) = 0; + virtual EMCPError GetBodyPartPosture(float * x, float * y, float * z, float * w, MCPBodyPartHandle_t bodyPartHandle) = 0; + }; + static const char * IMCPBodyPart_Version = "IMCPBodyPart_001"; + enum EMCPGroundingState + { + GroundingState_Grounding=0, + GroundingState_Flying=1 + }; + typedef uint64_t MCPJointHandle_t; + class IMCPJoint + { + public: + virtual EMCPError GetJointName(const char ** ppStr, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointLocalRotation(float * x, float * y, float * z, float * w, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointLocalRotationByEuler(float * x, float * y, float * z, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointLocalPosition(float * x, float * y, float * z, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointDefaultLocalPosition(float * x, float * y, float * z, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointChild(MCPJointHandle_t * pJointHandle, uint32_t * punSizeOfJointHandle, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointBodyPart(MCPBodyPartHandle_t * pBodyPartHandle, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointSensorModule(MCPSensorModuleHandle_t * pSensorModuleHandle, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointTag(EMCPJointTag * pJointTag, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointNameByTag(const char ** ppStr, EMCPJointTag jointTag) = 0; + virtual EMCPError GetJointChildJointTag(EMCPJointTag * pJointTag, uint32_t * punSizeOfJointTag, EMCPJointTag jointTag) = 0; + virtual EMCPError GetJointParentJointTag(EMCPJointTag * pJointTag, EMCPJointTag jointTag) = 0; + virtual EMCPError GetJointGroundingState(EMCPGroundingState * pGroundingState, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointGroundablePoints(float * pointsPostion, uint32_t * numberOfPoints, uint32_t * plowest_index, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointGlobalRotation(float * x, float * y, float * z, float * w, MCPJointHandle_t ulJointHandle) = 0; + virtual EMCPError GetJointGlobalPosition(float * x, float * y, float * z, MCPJointHandle_t ulJointHandle) = 0; + }; + static const char * IMCPJoint_Version = "IMCPJoint_004"; + typedef uint64_t MCPAvatarHandle_t; + class IMCPAvatar + { + public: + virtual EMCPError GetAvatarIndex(uint32_t * index, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarRootJoint(MCPJointHandle_t * pJointHandle, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarJoints(MCPJointHandle_t * pJointHandle, uint32_t * punSizeOfJointHandle, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarJointByName(const char * name, MCPJointHandle_t * pJointHandle, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarName(const char ** ppStr, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarRigidBodies(MCPRigidBodyHandle_t * vRigidBodies, uint32_t * punSizeOfRigidBodies, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarJointHierarchy(const char ** ppStr) = 0; + virtual EMCPError GetAvatarPostureIndex(uint32_t * postureIndex, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarPostureTimeCode(uint32_t * hour, uint32_t * minute, uint32_t * second, uint32_t * frame, uint32_t * rate, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarPostureTime(uint32_t * hour, uint32_t * minute, uint32_t * second, uint32_t * millisecond, MCPAvatarHandle_t ulAvatarHandle) = 0; + virtual EMCPError GetAvatarPosturePtpTime(uint32_t * second, uint32_t * nanosecond, MCPAvatarHandle_t ulAvatarHandle) = 0; + }; + static const char * IMCPAvatar_Version = "IMCPAvatar_005"; + typedef uint64_t MCPMarkerHandle_t; + class IMCPMarker + { + public: + virtual EMCPError GetMarkerPosition(float * x, float * y, float * z, MCPMarkerHandle_t handle) = 0; + }; + static const char * IMCPMarker_Version = "IMCPMarker_001"; + typedef uint64_t MCPPWRHandle_t; + class IMCPPWR + { + public: + virtual EMCPError GetPWRId(uint32_t * id, MCPPWRHandle_t handle) = 0; + virtual EMCPError GetPWRStatus(int * status, MCPPWRHandle_t handle) = 0; + virtual EMCPError GetPWRPosition(float * x, float * y, float * z, MCPPWRHandle_t handle) = 0; + virtual EMCPError GetPWRQuaternion(float * x, float * y, float * z, float * w, MCPPWRHandle_t handle) = 0; + }; + static const char * IMCPPWR_Version = "IMCPPWR_001"; + typedef uint64_t MCPAliceBusHandle_t; + class IMCPAliceHub + { + public: + virtual EMCPError GetSensorModuleList(MCPSensorModuleHandle_t * pHandles, uint32_t * nHandles) = 0; + virtual EMCPError GetSensorModuleTimestamp(uint64_t * timestamp) = 0; + virtual EMCPError GetMarkerList(MCPMarkerHandle_t * pHandles, uint32_t * nHandles) = 0; + virtual EMCPError GetMarkerTimestamp(uint64_t * timestamp) = 0; + virtual EMCPError GetRigidBodyList(MCPRigidBodyHandle_t * pHandles, uint32_t * nHandles) = 0; + virtual EMCPError GetRigidBodyTimestamp(uint64_t * timestamp) = 0; + virtual EMCPError GetPWRList(MCPPWRHandle_t * pHandles, uint32_t * nHandles) = 0; + virtual EMCPError GetPWRTimestamp(uint64_t * timestamp) = 0; + }; + static const char * IMCPAliceHub_Version = "IMCPAliceHub_001"; + enum EStartCaptureFlag + { + StartCaptureFlag_EnablePtp=1 + }; + enum EMCPCommand + { + CommandStartCapture=0, + CommandStopCapture=1, + CommandZeroPosition=2, + CommandCalibrateMotion=3, + CommandStartRecored=4, + CommandStopRecored=5, + CommandResumeOriginalPosture=6, + CommandClearZeroMotionDrift=7, + CommandResumeOriginalHandsPosture=8, + CommandPilotSimulationDriving=9 + }; + enum EMCPCalibrateMotionFlag + { + CalibrateMotionFlag_AutoNextStep=0, + CalibrateMotionFlag_ManualNextStep=1 + }; + enum EMCPCalibrateMotionOperation + { + CalibrateMotionOperation_Next=0, + CalibrateMotionOperation_Cancel=1 + }; + enum EMCPCommandStopCatpureExtraFlag + { + StopCatpureExtraFlag_SensorsModulesPowerOff=0, + StopCatpureExtraFlag_SensorsModulesHibernate=1 + }; + enum EMCPCommandExtraLong + { + CommandExtraLong_DeviceRadio=0, + CommandExtraLong_AvatarName=1, + CommandExtraLong_Extra0=0, + CommandExtraLong_Extra1=1, + CommandExtraLong_Extra2=2, + CommandExtraLong_Extra3=3 + }; + enum EMCPCommandProgress + { + CommandProgress_CalibrateMotion=0 + }; + enum EMCPRecoredFlag + { + RecoredFlag_AutoRecalculating=1, + RecoredFlag_AutoExportingBvh=2 + }; + typedef uint64_t MCPCommandHandle_t; + class IMCPCommand + { + public: + virtual EMCPError CreateCommand(uint32_t cmd, MCPCommandHandle_t * handle_) = 0; + virtual EMCPError SetCommandExtraFlags(uint32_t extraFlags, MCPCommandHandle_t handle_) = 0; + virtual EMCPError SetCommandExtraLong(uint32_t extraLongIndex, intptr_t extraLong, MCPCommandHandle_t handle_) = 0; + virtual EMCPError GetCommandResultMessage(const char ** pMsg, MCPCommandHandle_t handle_) = 0; + virtual EMCPError GetCommandResultCode(uint32_t * pResCode, MCPCommandHandle_t handle_) = 0; + virtual EMCPError GetCommandProgress(uint32_t progress, intptr_t extra, MCPCommandHandle_t handle_) = 0; + virtual EMCPError DestroyCommand(MCPCommandHandle_t handle_) = 0; + virtual EMCPError DuplicateCommand(MCPCommandHandle_t * duplicateCmd, MCPCommandHandle_t handle_) = 0; + virtual EMCPError AddCommandExtraLong(uint32_t extraLongIndex, intptr_t extraLong, MCPCommandHandle_t handle_) = 0; + virtual EMCPError GetCommandExtraLong(uint32_t extraLongIndex, intptr_t * pExtraLong, MCPCommandHandle_t handle_) = 0; + virtual EMCPError RemoveCommandExtraLong(uint32_t extraLongIndex, MCPCommandHandle_t handle_) = 0; + virtual EMCPError GetCommandTag(uint32_t * pTag, MCPCommandHandle_t handle_) = 0; + }; + static const char * IMCPCommand_Version = "IMCPCommand_003"; + enum EMCPCalibrateMotionProgressStep + { + CalibrateMotionProgressStep_Prepare=0, + CalibrateMotionProgressStep_Countdown=1, + CalibrateMotionProgressStep_Progress=2 + }; + typedef uint64_t MCPCalibrateMotionProgressHandle_t; + class IMCPCalibrateMotionProgress + { + public: + virtual EMCPError GetCalibrateMotionProgressCountOfSupportPoses(uint32_t * pCount, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressNameOfSupportPose(char * pName, uint32_t * pLenOfName, uint32_t index, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressStepOfPose(uint32_t * pStep, const char * name, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressCountdownOfPose(uint32_t * pCountdown, const char * name, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressProgressOfPose(uint32_t * pProgress, const char * name, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressStepOfCurrentPose(uint32_t * pStep, char * pName, uint32_t * pLenOfName, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressCountdownOfCurrentPose(uint32_t * pCountdown, char * pName, uint32_t * pLenOfName, MCPCalibrateMotionProgressHandle_t handle_) = 0; + virtual EMCPError GetCalibrateMotionProgressProgressOfCurrentPose(uint32_t * pProgress, char * pName, uint32_t * pLenOfName, MCPCalibrateMotionProgressHandle_t handle_) = 0; + }; + static const char * IMCPCalibrateMotionProgress_Version = "IMCPCalibrateMotionProgress_001"; + enum EMCPMasterStatus + { + MasterStatus_Online=0, + MasterStatus_Offline=1 + }; + typedef uint64_t MCPSystemHandle_t; + class IMCPSystem + { + public: + virtual EMCPError GetMasterVersion(const char ** pstr, MCPSystemHandle_t handle) = 0; + virtual EMCPError GetMasterSerialNumber(const char ** pstr, MCPSystemHandle_t handle) = 0; + virtual EMCPError GetSensorModules(MCPSensorModuleHandle_t * pHandles, uint32_t * pLenOfHandles, MCPSystemHandle_t handle) = 0; + virtual EMCPError GetMasterStatus(EMCPMasterStatus * masterStatus, MCPSystemHandle_t handle) = 0; + }; + static const char * IMCPSystem_Version = "IMCPSystem_001"; + struct MCPEvent_Reserved_t + { + uint64_t reserved0; + uint64_t reserved1; + uint64_t reserved2; + uint64_t reserved3; + uint64_t reserved4; + uint64_t reserved5; + }; + struct MCPEvent_MotionData_t + { + MCPAvatarHandle_t avatarHandle; + }; + struct MCPEvent_SystemError_t + { + EMCPError error; + uint64_t info0; + }; + struct MCPEvent_SensorModuleData_t + { + MCPSensorModuleHandle_t _sensorModuleHandle; + }; + struct MCPEvent_TrackerData_t + { + MCPTrackerHandle_t _trackerHandle; + }; + struct MCPEvent_MarkerData_t + { + MCPMarkerHandle_t _markerHandle; + }; + struct MCPEvent_PWRData_t + { + MCPPWRHandle_t _pwrHandle; + }; + enum EMCPNotify + { + Notify_RecordStarted=0, + Notify_RecordStoped=1, + Notify_RecordFinished=2, + Notify_SystemUpdated=3 + }; + struct MCPEvent_NotifyData_t + { + EMCPNotify _notify; + uint64_t _notifyHandle; + }; + enum EMCPReplay + { + MCPReplay_Response=0, + MCPReplay_Running=1, + MCPReplay_Result=2 + }; + struct MCPEvent_CommandRespond_t + { + MCPCommandHandle_t _commandHandle; + EMCPReplay _replay; + }; + union MCPEventData_t + { + MCPEvent_Reserved_t reserved; + MCPEvent_MotionData_t motionData; + MCPEvent_SystemError_t systemError; + MCPEvent_SensorModuleData_t sensorModuleData; + MCPEvent_TrackerData_t trackerData; + MCPEvent_CommandRespond_t commandRespond; + MCPEvent_MarkerData_t markerData; + MCPEvent_PWRData_t pwrData; + MCPEvent_NotifyData_t notifyData; + }; + enum EMCPEventType + { + MCPEvent_None=0, + MCPEvent_AvatarUpdated=256, + MCPEvent_RigidBodyUpdated=512, + MCPEvent_Error=768, + MCPEvent_SensorModulesUpdated=1024, + MCPEvent_TrackerUpdated=1280, + MCPEvent_CommandReply=1536, + MCPEvent_Notify=1792, + MCPEvent_AliceIMUUpdated=4096, + MCPEvent_AliceRigidbodyUpdated=4097, + MCPEvent_AliceTrackerUpdated=4098, + MCPEvent_AliceMarkerUpdated=4099 + }; + struct MCPEvent_t + { + uint32_t size; + EMCPEventType eventType; + double fTimestamp; + MCPEventData_t eventData; + }; + enum EMCPBvhRotation + { + BvhRotation_XYZ=0, + BvhRotation_XZY=1, + BvhRotation_YXZ=2, + BvhRotation_YZX=3, + BvhRotation_ZXY=4, + BvhRotation_ZYX=5 + }; + enum EMCPBvhData + { + BvhDataType_String=0, + BvhDataType_BinaryWithOldFrameHeader=1, + BvhDataType_Binary=2, + BvhDataType_Mask_LegacyHumanHierarchy=4 + }; + enum EMCPBvhTransformation + { + BvhTransformation_Disable=0, + BvhTransformation_Enable=1 + }; + typedef uint64_t MCPSettingsHandle_t; + class IMCPSettings + { + public: + virtual EMCPError CreateSettings(MCPSettingsHandle_t * pSettingsHandle) = 0; + virtual EMCPError DestroySettings(MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsUDP(uint16_t localPort, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsTCP(const char * serverIp, uint16_t serverPort, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsBvhRotation(EMCPBvhRotation bvhRotation, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsBvhTransformation(EMCPBvhTransformation bvhTransformation, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsBvhData(EMCPBvhData bvhData, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsCalcData(MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsUDPServer(const char * serverIp, uint16_t serverPort, MCPSettingsHandle_t ulSettingsHandle) = 0; + virtual EMCPError SetSettingsUDPEx(const char * localIp, uint16_t localPort, MCPSettingsHandle_t ulSettingsHandle) = 0; + }; + static const char * IMCPSettings_Version = "IMCPSettings_001"; + enum EMCPUpVector + { + UpVector_XAxis=1, + UpVector_YAxis=2, + UpVector_ZAxis=3 + }; + enum EMCPFrontVector + { + FrontVector_ParityEven=1, + FrontVector_ParityOdd=2 + }; + enum EMCPCoordSystem + { + CoordSystem_RightHanded=0, + CoordSystem_LeftHanded=1 + }; + enum EMCPRotatingDirection + { + RotatingDirection_Clockwise=0, + RotatingDirection_CounterClockwise=1 + }; + enum EMCPPreDefinedRenderSettings + { + PreDefinedRenderSettings_Default=0, + PreDefinedRenderSettings_UnrealEngine=1, + PreDefinedRenderSettings_Unity3D=2, + PreDefinedRenderSettings_Count=3 + }; + enum EMCPUnit + { + Unit_Centimeter=0, + Uint_Meter=1 + }; + typedef uint64_t MCPRenderSettingsHandle_t; + class IMCPRenderSettings + { + public: + virtual EMCPError CreateRenderSettings(MCPRenderSettingsHandle_t * pRenderSettings) = 0; + virtual EMCPError GetPreDefRenderSettings(EMCPPreDefinedRenderSettings preDefinedRenderSettings, MCPRenderSettingsHandle_t * pRenderSettings) = 0; + virtual EMCPError SetUpVector(EMCPUpVector upVector, int sign, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError GetUpVector(EMCPUpVector * pUpVector, int * sign, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError SetFrontVector(EMCPFrontVector frontVector, int sign, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError GetFrontVector(EMCPFrontVector * pFrontVector, int * sign, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError SetCoordSystem(EMCPCoordSystem coordSystem, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError GetCoordSystem(EMCPCoordSystem * pCoordSystem, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError SetRotatingDirection(EMCPRotatingDirection rotatingDirection, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError GetRotatingDirection(EMCPRotatingDirection * pRotatingDirection, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError SetUnit(EMCPUnit mcpUnit, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError GetUnit(EMCPUnit * mcpUnit, MCPRenderSettingsHandle_t renderSettings) = 0; + virtual EMCPError DestroyRenderSettings(MCPRenderSettingsHandle_t renderSettings) = 0; + }; + static const char * IMCPRenderSettings_Version = "IMCPRenderSettings_001"; + typedef void ( * MCPEventHandleProc ) ( const MCPEvent_t * ev , void * custom ); + typedef uint64_t MCPApplicationHandle_t; + class IMCPApplication + { + public: + virtual EMCPError CreateApplication(MCPApplicationHandle_t * ulApplicationHandle) = 0; + virtual EMCPError DestroyApplication(MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError SetApplicationSettings(MCPSettingsHandle_t ulSettingsHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError SetApplicationRenderSettings(MCPRenderSettingsHandle_t ulRenderSettings, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError OpenApplication(MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError EnableApplicationCacheEvents(MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError DisableApplicationCacheEvents(MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError ApplicationCacheEventsIsEnabled(bool * isEnabled, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError CloseApplication(MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError GetApplicationRigidBodies(MCPRigidBodyHandle_t * pRigidBodyHandle, uint32_t * punRigidBodyHandleSize, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError GetApplicationAvatars(MCPAvatarHandle_t * pAvatarHandle, uint32_t * punAvatarHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError PollApplicationNextEvent(MCPEvent_t * pEvent, uint32_t * punSizeOfEvent, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError GetApplicationSensorModules(MCPSensorModuleHandle_t * pSensorModuleHandle, uint32_t * punSensorModuleHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError GetApplicationTrackers(MCPTrackerHandle_t * pTrackerHandle, uint32_t * punTrackerHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError QueuedServerCommand(MCPCommandHandle_t cmdHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError RegisterEventHandler(MCPEventHandleProc eventHandleProc, intptr_t userData, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError UnregisterEventHandler(MCPEventHandleProc eventHandleProc, intptr_t * userData, MCPApplicationHandle_t ulApplicationHandle) = 0; + virtual EMCPError GetAppplicationSystem(MCPSystemHandle_t * pSystemHandle, MCPApplicationHandle_t ulApplicationHandle) = 0; + }; + static const char * IMCPApplication_Version = "IMCPApplication_004"; + typedef uint64_t MCPRecordNotifyHandle_t; + class IMCPRecordNotify + { + public: + virtual EMCPError DuplicateRecordNotify(MCPRecordNotifyHandle_t * duplicatedRecordNotifyHandle, MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + virtual EMCPError RecordNotifyGetTakeName(const char ** takeName, MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + virtual EMCPError RecordNotifyGetTakePath(const char ** takePath, MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + virtual EMCPError RecordNotifyGetTakeSaveDir(const char ** takeSaveDir, MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + virtual EMCPError RecordNotifyGetTakeFileSuffix(const char ** takeFileSuffix, MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + virtual EMCPError DestroyRecordNotify(MCPRecordNotifyHandle_t recordNotifyHandle) = 0; + }; + static const char * IMCPRecordNotify_Version = "IMCPRecordNotify_001"; + enum EMCPRobotJointType + { + JointLeftShoulder_Pitch=0, + JointLeftShoulder_Roll=1, + JointLeftShoulder_Yaw=2, + JointLeftElbow=3, + JointLeftWrist_Yaw=4, + JointLeftWrist_Roll=5, + JointLeftWrist_Pitch=6, + JointRightShoulder_Pitch=7, + JointRightShoulder_Roll=8, + JointRightShoulder_Yaw=9, + JointRightElbow=10, + JointRightWrist_Yaw=11, + JointRightWrist_Roll=12, + JointRightWrist_Pitch=13, + JointLeftHip_Yaw=14, + JointLeftHip_Pitch=15, + JointLeftHip_Roll=16, + JointLeftKnee=17, + JointLeftAnkle_Pitch=18, + JointLeftAnkle_Roll=19, + JointRightHip_Yaw=20, + JointRightHip_Pitch=21, + JointRightHip_Roll=22, + JointRightKnee=23, + JointRightAnkle_Pitch=24, + JointRightAnkle_Roll=25, + JointSpine_Yaw=26, + JointSpine_Roll=27, + JointSpine_Pitch=28, + JointHead_Yaw=29, + JointHead_Roll=30, + JointHead_Pitch=31, + JointCount=32 + }; + typedef uint64_t MCPRobotHandle_t; + class IMCPRobot + { + public: + virtual EMCPError CreateRobot(const char * jsonConfigStr, MCPRobotHandle_t * pHandle) = 0; + virtual EMCPError SetRobotFPS(int fps, MCPRobotHandle_t handle) = 0; + virtual EMCPError UpdateRobot(MCPAvatarHandle_t avatarHandle, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotRawJointAngle(EMCPRobotJointType jointType, float * value, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotRetargetJointAngle(const char * jointName, float * value, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotRosFrameJson(const char ** jsonStr, bool compress, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotRootRotation(float * x, float * y, float * z, float * w, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotRootPosition(float * x, float * y, float * z, MCPRobotHandle_t handle) = 0; + virtual EMCPError DestroyRobot(MCPRobotHandle_t handle) = 0; + virtual EMCPError RunRobotStep(MCPRobotHandle_t handle) = 0; + virtual EMCPError RunRobotStep1(float fixedDelta, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotSlideSpeed(float * value, MCPRobotHandle_t handle) = 0; + virtual EMCPError GetRobotSlideHeight(float * value, MCPRobotHandle_t handle) = 0; + }; + static const char * IMCPRobot_Version = "IMCPRobot_001"; + MCP_INTERFACE EMCPError MCP_CALLTYPE MCPGetGenericInterface(const char * pchInterfaceVersion, void ** ppInterface); + MCP_INTERFACE void MCP_CALLTYPE MCPGetMocapApiVersion(uint32_t * major, uint32_t * minor, uint32_t * build, uint32_t * revision); + MCP_INTERFACE const char * MCP_CALLTYPE MCPGetMocapApiVersionString(); +} +#endif // end _NOITOM_MOCAPAPI_H diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.dll b/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.dll new file mode 100644 index 000000000..a32ef4d17 Binary files /dev/null and b/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.dll differ diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.lib b/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.lib new file mode 100644 index 000000000..d55783e0d Binary files /dev/null and b/src/plugins/noitom_mocap/third_party/mocap_api/lib/amd64/MocapApi.lib differ diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/lib/arm64/libMocapApi.so b/src/plugins/noitom_mocap/third_party/mocap_api/lib/arm64/libMocapApi.so new file mode 100644 index 000000000..cc1880c08 Binary files /dev/null and b/src/plugins/noitom_mocap/third_party/mocap_api/lib/arm64/libMocapApi.so differ diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/lib/x86_64/libMocapApi.so b/src/plugins/noitom_mocap/third_party/mocap_api/lib/x86_64/libMocapApi.so new file mode 100644 index 000000000..b8ac5caa2 Binary files /dev/null and b/src/plugins/noitom_mocap/third_party/mocap_api/lib/x86_64/libMocapApi.so differ diff --git a/src/plugins/noitom_mocap/third_party/mocap_api/mocap_api.py b/src/plugins/noitom_mocap/third_party/mocap_api/mocap_api.py new file mode 100644 index 000000000..53654f63f --- /dev/null +++ b/src/plugins/noitom_mocap/third_party/mocap_api/mocap_api.py @@ -0,0 +1,1799 @@ +from ctypes import * +from collections import namedtuple +from platform import * +import time +import os +from pathlib import Path + +__version__ = "0.1.3" + +architecture = machine() +mocap_file_path = None +mocap_file_names = [] + +if architecture == "arm64" or architecture == "aarch64": + mocap_file_path = 'lib/arm64' + mocap_file_names = ['libMocapApi.so'] +elif architecture == "x86_64": + mocap_file_path = 'lib/x86_64' + mocap_file_names = ['libMocapApi.so'] +elif architecture == "AMD64": + mocap_file_path = "lib/amd64" + mocap_file_names = ["MocapApi.dll", "MocapApidll"] +else: + raise Exception("Unsupported architecture") + +package_dir = Path(__file__).resolve().parent +candidate_paths = [] + +for mocap_file_name in mocap_file_names: + candidate_paths.append(package_dir / mocap_file_path / mocap_file_name) + candidate_paths.append(package_dir.parent / 'build' / 'lib' / 'mocap_api' / mocap_file_path / mocap_file_name) + +mocap_lib_file = None +for candidate in candidate_paths: + if candidate.exists(): + mocap_lib_file = candidate + break + +if mocap_lib_file is None: + checked_paths = '\n'.join(str(path) for path in candidate_paths) + raise FileNotFoundError(f'Could not locate Mocap API library. Checked:\n{checked_paths}') + +if os.name == 'nt': + os.add_dll_directory(str(mocap_lib_file.parent)) + +print(f'mocap_lib_file={mocap_lib_file}') + +MocapApi = cdll.LoadLibrary(str(mocap_lib_file)) + +MCPError = namedtuple('EMCPError', [ + 'NoError', + 'MoreEvent', + 'InsufficientBuffer', + 'InvalidObject', + 'InvalidHandle', + 'InvalidParameter', + 'NotSupported', + 'IgnoreUDPSettings', + 'IgnoreTCPSettings', + 'IgnoreBvhSettings', + 'JointNotFound', + 'WithoutTransformation', + 'NoneMessage', + 'NoneParent', + 'NoneChild', + 'AddressInUse' +])._make(range(16)) + +MCPJointTag = namedtuple('EMCPJointTag', [ + 'Hips', + 'RightUpLeg', + 'RightLeg', + 'RightFoot', + 'LeftUpLeg', + 'LeftLeg', + 'LeftFoot', + 'Spine', + 'Spine1', + 'Spine2', + 'Neck', + 'Neck1', + 'Head', + 'RightShoulder', + 'RightArm', + 'RightForeArm', + 'RightHand', + 'RightHandThumb1', + 'RightHandThumb2', + 'RightHandThumb3', + 'RightInHandIndex', + 'RightHandIndex1', + 'RightHandIndex2', + 'RightHandIndex3', + 'RightInHandMiddle', + 'RightHandMiddle1', + 'RightHandMiddle2', + 'RightHandMiddle3', + 'RightInHandRing', + 'RightHandRing1', + 'RightHandRing2', + 'RightHandRing3', + 'RightInHandPinky', + 'RightHandPinky1', + 'RightHandPinky2', + 'RightHandPinky3', + 'LeftShoulder', + 'LeftArm', + 'LeftForeArm', + 'LeftHand', + 'LeftHandThumb1', + 'LeftHandThumb2', + 'LeftHandThumb3', + 'LeftInHandIndex', + 'LeftHandIndex1', + 'LeftHandIndex2', + 'LeftHandIndex3', + 'LeftInHandMiddle', + 'LeftHandMiddle1', + 'LeftHandMiddle2', + 'LeftHandMiddle3', + 'LeftInHandRing', + 'LeftHandRing1', + 'LeftHandRing2', + 'LeftHandRing3', + 'LeftInHandPinky', + 'LeftHandPinky1', + 'LeftHandPinky2', + 'LeftHandPinky3', + 'Spine3', + 'JointsCount', +])._make([x for x in range(61)]) + +MCPRigidBodyHandle = c_uint64 + +class MCPRigidBody(object): + IMCPRigidBodyApi_Version = c_char_p(b'PROC_TABLE:IMCPRigidBody_002') + + class MCPRigidBodyApi(Structure): + _fields_ = [ + ('GetRigidBodyRotation', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPRigidBodyHandle)), + ('GetRigidBodyPosition', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPRigidBodyHandle)), + ('GetRigidBodyStatus', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRigidBodyHandle)), + ('GetRigidBodyId', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRigidBodyHandle)), + ('GetRigidBodyJointTag', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRigidBodyHandle)), + ('GetRigidBodyAxisAngle', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPRigidBodyHandle)), + ('GetRigidBodyType', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPRigidBodyHandle)), + ('GetRigidBodyName', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPRigidBodyHandle)) + ] + + api = POINTER(MCPRigidBodyApi)() + + def __init__(self, rigid_body_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPRigidBodyApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPSensorModule interface: {0}'.format(MCPError._fields[err])) + self.handle = rigid_body_handle + + def get_rotation(self): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + err = self.api.contents.GetRigidBodyRotation(pointer(x), pointer(y), pointer(z), pointer(w), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body rotation: {0}'.format(MCPError._fields[err])) + return w.value, x.value, y.value, z.value + + def get_position(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetRigidBodyPosition(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body position: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_status(self): + status = c_int32() + err = self.api.contents.GetRigidBodyStatus(pointer(status), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body status: {0}'.format(MCPError._fields[err])) + return status.value + + def get_id(self): + rigid_id = c_int32() + err = self.api.contents.GetRigidBodyId(pointer(rigid_id), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body id: {0}'.format(err)) + return rigid_id.value + + def get_joint_tag(self): + joint_tag = c_int32() + err = self.api.contents.GetRigidBodyJointTag(pointer(joint_tag), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body joint tag: {0}'.format(MCPError._fields[err])) + return joint_tag.value + + def get_axis_angle(self): + x = c_float() + y = c_float() + z = c_float() + angle = c_float() + err = self.api.contents.GetRigidBodyAxisAngle(pointer(x), pointer(y), pointer(z), pointer(angle), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body axis angle: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value, angle.value + + def get_type(self): + rigid_type = c_uint32() + err = self.api.contents.GetRigidBodyType(pointer(rigid_type), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body type: {0}'.format(MCPError._fields[err])) + return rigid_type.value + + def get_name(self): + rigid_name = c_char_p() + err = self.api.contents.GetRigidBodyName(pointer(rigid_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rigid body name: {0}'.format(MCPError._fields[err])) + return rigid_name.value.decode('utf-8') if rigid_name.value else '' + +MCPSensorModuleHandle = c_uint64 + +class MCPSensorModule(object): + IMCPSensorModuleApi_Version = c_char_p(b'PROC_TABLE:IMCPSensorModule_001') + + class MCPSensorModuleApi(Structure): + _fields_ = [ + ('GetSensorModulePosture', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), + MCPSensorModuleHandle)), + ('GetSensorModuleAngularVelocity', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPSensorModuleHandle)), + ('GetSensorModuleAcceleratedVelocity', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPSensorModuleHandle)), + ('GetSensorModuleId', + CFUNCTYPE(c_int32, POINTER(c_int32), MCPSensorModuleHandle)) + ] + + api = POINTER(MCPSensorModuleApi)() + + def __init__(self, sensor_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPSensorModuleApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPSensorModule interface: {0}'.format(MCPError._fields[err])) + self.handle = sensor_handle + + def get_posture(self): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + err = self.api.contents.GetSensorModulePosture(pointer(x), pointer(y), pointer(z), pointer(w), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor module posture: {0}'.format(MCPError._fields[err])) + return w.value, x.value, y.value, z.value + + def get_angular_velocity(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetSensorModuleAngularVelocity(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor module angular velocity: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_accelerated_velocity(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetSensorModuleAcceleratedVelocity(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor module accelerated velocity: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_sensor_module_id(self): + sensor_id = c_int32() + err = self.api.contents.GetSensorModuleId(pointer(sensor_id), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor module id: {0}'.format(err)) + return sensor_id.value + +MCPBodyPartHandle = c_uint64 + +class MCPBodyPart(object): + IMCPBodyPartApi_Version = c_char_p(b'PROC_TABLE:IMCPBodyPart_001') + + class MCPBodyPartApi(Structure): + _fields_ = [ + ('GetJointPosition', CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPBodyPartHandle)), + ('GetJointDisplacementSpeed', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPBodyPartHandle)), + ('GetBodyPartPosture', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPBodyPartHandle)) + ] + + api = POINTER(MCPBodyPartApi)() + + def __init__(self, body_part_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPBodyPartApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPBodyPartApi interface: {0}'.format(MCPError._fields[err])) + self.handle = body_part_handle + + def get_position(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetJointPosition(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint position:{0}'.format(MCPError._fileds[err])) + return x.value, y.value, z.value + + def get_displacement_speed(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetJointDisplacementSpeed(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint displacement speed: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_posture(self): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + err = self.api.contents.GetBodyPartPosture(pointer(x), pointer(y), pointer(z), pointer(w), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get body part posture: {0}'.format(MCPError._fields[err])) + return w.value, x.value, y.value, z.value + +MCPJointHandle = c_uint64 + +class MCPJoint(object): + IMCPJointApi_Version = c_char_p(b"PROC_TABLE:IMCPJoint_003") + + class MCPJointApi(Structure): + _fields_ = [ + ('GetJointName', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPJointHandle)), + ('GetJointLocalRotation', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPJointHandle)), + ('GetJointLocalRotationByEuler', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPJointHandle)), + ('GetJointLocalTransformation', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPJointHandle)), + ('GetJointDefaultLocalTransformation', + CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPJointHandle)), + ('GetJointChild', CFUNCTYPE(c_int32, POINTER(MCPJointHandle), POINTER(c_uint32), MCPJointHandle)), + ('GetJointBodyPart', CFUNCTYPE(c_int32, POINTER(MCPBodyPartHandle), MCPJointHandle)), + ('GetJointSensorModule', CFUNCTYPE(c_int32, POINTER(MCPSensorModuleHandle), MCPJointHandle)), + ('GetJointTag', CFUNCTYPE(c_int32, POINTER(c_int32), MCPJointHandle)), + ('GetJointNameByTag', CFUNCTYPE(c_int32, POINTER(c_char_p), c_int32)), + ('GetJointChildJointTag', CFUNCTYPE(c_int32, POINTER(c_int32), POINTER(c_uint32), c_int32)), + ('GetJointParentJointTag', CFUNCTYPE(c_int32, POINTER(c_int32), c_int32)) + ] + + api = POINTER(MCPJointApi)() + + def __init__(self, joint_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPJointApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPJointApi interface: {0}'.format(MCPError._fields[err])) + self.handle = joint_handle + + def get_name(self): + joint_name = c_char_p() + err = self.api.contents.GetJointName(pointer(joint_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint name: {0}'.format(MCPError._fields[err])) + return str(joint_name.value, encoding='utf8') + + def get_local_rotation(self): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + err = self.api.contents.GetJointLocalRotation(pointer(x), pointer(y), pointer(z), pointer(w), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint local rotation: {0}'.format(MCPError._fields[err])) + return w.value, x.value, y.value, z.value + + def get_local_rotation_by_euler(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetJointLocalRotationByEuler(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint euler: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_local_position(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetJointLocalTransformation(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + return None,None,None + return x.value, y.value, z.value + + def get_default_local_position(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetJointDefaultLocalTransformation(pointer(x), pointer(y), pointer(z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint default local transformation: {0}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def get_children(self): + joint_count = c_uint32() + err = self.api.contents.GetJointChild(POINTER(MCPJointHandle)(), pointer(joint_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint child count: {0}'.format(MCPError._fields[err])) + joint_handles = (MCPJointHandle * joint_count.value)() + err = self.api.contents.GetJointChild(joint_handles, pointer(joint_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint child: {0}'.format(MCPError._fields[err])) + return [MCPJoint(joint_handles[i]) for i in range(joint_count.value)] + + def get_body_part(self): + body_part_handle = MCPBodyPartHandle() + err = self.api.contents.GetJointBodyPart(pointer(body_part_handle), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint body part: {0}'.format(MCPError._fields[err])) + return MCPBodyPart(body_part_handle) + + def get_sensor_module(self): + sensor_handle = MCPSensorModuleHandle() + err = self.api.contents.GetJointSensorModule(pointer(sensor_handle), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor module: {0}'.format(err)) + return MCPSensorModule(sensor_handle) + + def get_tag(self): + tag = c_int32() + err = self.api.contents.GetJointTag(pointer(tag), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint tag: {0}'.format(MCPError._fields[err])) + return tag.value + + def get_name_by_tag(self, tag): + joint_name = c_char_p() + err = self.api.contents.GetJointNameByTag(pointer(joint_name), c_int32(tag)) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint name of joint tag {0}: {1}'.format(tag, err)) + return str(joint_name.value, encoding='utf8') + + def get_child_joint_tag(self, tag): + joint_count = c_uint32() + err = self.api.contents.GetJointChildJointTag(POINTER(c_int32)(), pointer(joint_count), c_int32(tag)) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint child joint tag: {0}'.format(MCPError._fields[err])) + joints_tag = (c_int32 * joint_count.value)() + err = self.api.contents.GetJointChildJointTag(joints_tag, pointer(joint_count), c_int32(tag)) + if err != MCPError.NoError: + raise RuntimeError('Can not get joint child joint tag: {0}'.format(MCPError._fields[err])) + return [joints_tag[i].value for i in range(joint_count.value)] + + def get_parent_joint_tag(self, tag): + joint_tag = c_int32() + err = self.api.contents.GetJointParentJointTag(pointer(joint_tag), c_int32(tag)) + if err != MCPError.NoError: + raise RuntimeError('can not get joint parent tag: {0}'.format(MCPError._fields[err])) + return joint_tag.value + +MCPAvatarHandle = c_uint64 + +class MCPAvatar(object): + IMCPAvatarApi_Version = c_char_p(b'PROC_TABLE:IMCPAvatar_003') + + class MCPAvatarApi(Structure): + _fields_ = [ + ('GetAvatarIndex', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarRootJoint', CFUNCTYPE(c_int32, POINTER(MCPJointHandle), MCPAvatarHandle)), + ('GetAvatarJoints', CFUNCTYPE(c_int32, POINTER(MCPJointHandle), POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarJointByName', CFUNCTYPE(c_int32, c_char_p, POINTER(MCPJointHandle), MCPAvatarHandle)), + ('GetAvatarName', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPAvatarHandle)), + ('GetAvatarRigidBodies', CFUNCTYPE(c_int32, POINTER(MCPRigidBodyHandle), POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarJointHierarchy', CFUNCTYPE(c_int32, POINTER(c_char_p))), + ('GetAvatarPostureIndex', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarPostureTimeCode', CFUNCTYPE(c_int32, POINTER(c_uint32), POINTER(c_uint32), POINTER(c_uint32), POINTER(c_uint32), POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarPostureTime', CFUNCTYPE(c_int32, POINTER(c_uint32), POINTER(c_uint32), POINTER(c_uint32), POINTER(c_uint32), MCPAvatarHandle)), + ('GetAvatarPosturePtpTime', CFUNCTYPE(c_int32, POINTER(c_uint32), POINTER(c_uint32), MCPAvatarHandle)), + ] + + api = POINTER(MCPAvatarApi)() + + def __init__(self, avatar_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPAvatarApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPAvatar interface: {0}'.format(MCPError._fields[err])) + self.handle = avatar_handle + + def get_index(self): + index = c_uint32(0) + err = self.api.contents.GetAvatarIndex(pointer(index), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar index: {0}'.format(MCPError._fields[err])) + return index.value + + def get_root_joint(self): + joint_handle = MCPJointHandle() + err = self.api.contents.GetAvatarRootJoint(pointer(joint_handle), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar root joint: {0}'.format(err)) + return MCPJoint(joint_handle) + + def get_joints(self): + joint_count = c_uint32() + err = self.api.contents.GetAvatarJoints(POINTER(MCPJointHandle)(), pointer(joint_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar joints: {0}'.format(MCPError._fields[err])) + joints_handle = (MCPJointHandle * joint_count.value)() + err = self.api.contents.GetAvatarJoints(joints_handle, pointer(joint_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar Joints: {0}'.format(MCPError._fields[err])) + return [MCPJoint(joints_handle[i]) for i in range(joint_count.value)] + + def get_joint_by_name(self, name): + joint_name = c_char_p(bytes(name, encoding='utf8')) + joint_handle = MCPJointHandle() + err = self.api.contents.GetAvatarJointByName(joint_name, pointer(joint_handle), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar Joints: {0}'.format(MCPError._fields[err])) + return MCPJoint(joint_handle) + + def get_name(self): + avatar_name = c_char_p() + err = self.api.contents.GetAvatarName(pointer(avatar_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar name: {0}'.format(MCPError._fields[err])) + return str(avatar_name.value, encoding='utf8') + + def get_rigid_bodies(self): + rigid_body_count = c_uint32() + err = self.api.contents.GetAvatarRigidBodies(POINTER(MCPRigidBodyHandle)(), pointer(rigid_body_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar rigid bodies: {0}'.format(MCPError._fields[err])) + rigid_body_handles = (MCPRigidBodyHandle * rigid_body_count.value)() + err = self.api.contents.GetAvatarRigidBodies(rigid_body_handles, pointer(rigid_body_count), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar rigid bodies: {0}'.format(MCPError._fields[err])) + return [MCPRigidBody(rigid_body_handles[i]) for i in range(rigid_body_count.value)] + + def get_joint_hierarchy(self): + hierarchy = c_char_p() + err = self.api.contents.GetAvatarJointHierarchy(pointer(hierarchy)) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar joint hierarchy: {0}'.format(err)) + return str(hierarchy.value, encoding='utf8') + + def get_avatar_posture_index(self): + postureIndex = c_uint32(0) + err = self.api.contents.GetAvatarPostureIndex(byref(postureIndex), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar posture index: {0}'.format(MCPError._fields[err])) + return postureIndex.value + + def get_avatar_posture_time(self): + hour = c_uint32(0) + minute = c_uint32(0) + second = c_uint32(0) + millisecond = c_uint32(0) + err = self.api.contents.GetAvatarPostureTime(byref(hour), byref(minute), byref(second), byref(millisecond), self.handle) + print(f"get_avatar_posture_time err: {err}") + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar posture time: {0}'.format(MCPError._fields[err])) + return hour.value, minute.value, second.value, millisecond.value + + def get_avatar_posture_time_code(self): + hour = c_uint32(0) + minute = c_uint32(0) + second = c_uint32(0) + frame = c_uint32(0) + rate = c_uint32(0) + err = self.api.contents.GetAvatarPostureTimeCode(byref(hour), byref(minute), byref(second), byref(frame), byref(rate), self.handle) + print(f"get_avatar_posture_time_code err: {err}") + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar posture time code: {0}'.format(MCPError._fields[err])) + return hour.value, minute.value, second.value, frame.value, rate.value + + def get_avatar_posture_ptp_time(self): + second = c_uint32(0) + nanosecond = c_uint32(0) + err = self.api.contents.GetAvatarPosturePtpTime(byref(second), byref(nanosecond), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get avatar posture ptp time : {0}'.format(MCPError._fields[err])) + return second.value, nanosecond.value + + +MCPCalibrateMotionProgressStep = namedtuple('EMCPCalibrateMotionProgressStep', [ + 'CalibrateMotionProgressStep_Prepare', + 'CalibrateMotionProgressStep_Countdown', + 'CalibrateMotionProgressStep_Progress' +])(0, 1, 2) + +MCPCalibrateMotionProgressHandle = c_uint64 +class MCPCalibrateMotionProgress(object): + IMCPCalibrateMotionProgress_Version = c_char_p(b"PROC_TABLE:IMCPCalibrateMotionProgress_001") + class MCPCalibrateMotionProgressApi(Structure): + _fields_ = [ + ('GetCalibrateMotionProgressCountOfSupportPoses', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressNameOfSupportPose', CFUNCTYPE(c_int32,c_char_p, POINTER(c_uint32), c_int32, MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressStepOfPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p, MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressCountdownOfPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p, MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressProgressOfPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p, MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressStepOfCurrentPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p,POINTER(c_uint32), MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressCountdownOfCurrentPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p,POINTER(c_uint32), MCPCalibrateMotionProgressHandle)), + ('GetCalibrateMotionProgressProgressOfCurrentPose', CFUNCTYPE(c_int32,POINTER(c_uint32),c_char_p,POINTER(c_uint32), MCPCalibrateMotionProgressHandle)), + ] + api = POINTER(MCPCalibrateMotionProgressApi)() + + def __init__(self, progressHandle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPCalibrateMotionProgress_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Cannot get MCPCommandApi {0}'.format(MCPError._fields[err])) + self.handle = progressHandle + + def get_count_of_support_poses(self): + count = c_uint32() + err = self.api.contents.GetCalibrateMotionProgressCountOfSupportPoses(count, self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress Count Of Support Poses with error {0}'.format(MCPError._fields[err])) + return count.value + + def get_name_of_support_poses(self, index): + pName = None + pName_ = None + pLenOfName = c_uint32(0) + err = self.api.contents.GetCalibrateMotionProgressNameOfSupportPose(None, byref(pLenOfName), index, self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress Name Of Support Pose with error {0}'.format(MCPError._fields[err])) + + pName_ = create_string_buffer(pLenOfName.value) + err_ = self.api.contents.GetCalibrateMotionProgressNameOfSupportPose(pName_, byref(pLenOfName), index, self.handle) + if err_ != MCPError.NoError: + raise RuntimeError('GetProgress Name Of Support Pose with error {0}'.format(MCPError._fields[err_])) + + pName = pName_.value.decode('utf-8') + return pName + + def get_step_current_pose(self): + current_step = c_uint32(0) + p_len_of_name = c_uint32(0) + p_name = None + + err = self.api.contents.GetCalibrateMotionProgressStepOfCurrentPose(byref(current_step), p_name, byref(p_len_of_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err])) + + p_name = create_string_buffer(p_len_of_name.value) + + err_ = self.api.contents.GetCalibrateMotionProgressStepOfCurrentPose(byref(current_step), p_name, byref(p_len_of_name), self.handle) + if err_ != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err_])) + + p_name_str = p_name.value.decode('utf-8') + return current_step.value, p_name_str + + def get_countdown_current_pose(self): + countdown = c_uint32(0) + p_len_of_name = c_uint32(0) + p_name = None + + err = self.api.contents.GetCalibrateMotionProgressCountdownOfCurrentPose(byref(countdown), p_name, byref(p_len_of_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err])) + + p_name = create_string_buffer(p_len_of_name.value) + + err_ = self.api.contents.GetCalibrateMotionProgressCountdownOfCurrentPose(byref(countdown), p_name, byref(p_len_of_name), self.handle) + if err_ != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err_])) + + p_name_str = p_name.value.decode('utf-8') + return countdown.value, p_name_str + + + def get_progress_current_pose(self): + progress = c_uint32(0) + p_len_of_name = c_uint32(0) + p_name = None + + err = self.api.contents.GetCalibrateMotionProgressProgressOfCurrentPose(byref(progress), p_name, byref(p_len_of_name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err])) + + p_name = create_string_buffer(p_len_of_name.value) + + err_ = self.api.contents.GetCalibrateMotionProgressProgressOfCurrentPose(byref(progress), p_name, byref(p_len_of_name), self.handle) + if err_ != MCPError.NoError: + raise RuntimeError('GetProgress step Of current Pose with error {0}'.format(MCPError._fields[err_])) + + p_name_str = p_name.value.decode('utf-8') + return progress.value, p_name_str + +MCPCommandProgress = namedtuple('MCPCommandProgress', [ + 'CommandProgress_CalibrateMotion' +])(0) + +MCPCommandHandle = c_uint64 +class MCPCommand(object): + IMCPCommand_Version = c_char_p(b"PROC_TABLE:IMCPCommand_002") + class MCPCommandApi(Structure): + _fields_ = [ + ('CreateCommand', CFUNCTYPE(c_int32, c_int32, POINTER(MCPCommandHandle))), + ('SetCommandExtraFlags', CFUNCTYPE(c_int32, c_int32, POINTER(MCPCommandHandle))), + ('SetCommandExtraLong', CFUNCTYPE(c_int32, c_int, c_uint64, MCPCommandHandle)), + ('GetCommandResultMessage', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPCommandHandle)), + ('GetCommandResultCode', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPCommandHandle)), + ('GetCommandProgress', CFUNCTYPE(c_int32, c_int32, POINTER(c_ulonglong), MCPCommandHandle)), + ('DestroyCommand', CFUNCTYPE(c_int32, MCPCommandHandle)), + ] + api = POINTER(MCPCommandApi)() + def __init__(self): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPCommand_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Cannot get MCPCommandApi {0}'.format(MCPError._fields[err])) + self.handle = MCPCommandHandle() + def create_command(self, commandType): + err = self.api.contents.CreateCommand(commandType, self.handle) + if err != MCPError.NoError: + raise RuntimeError('CreateCommand failed with error {0}'.format(MCPError._fields[err])) + + def set_command_extra_long(self, data=0): + # 保存缓冲区引用，防止垃圾回收 + self._buffer = None + + if isinstance(data, str): + # Convert string to bytes + data_bytes = data.encode('utf-8') + # Create a buffer to hold the bytes + data_buffer = create_string_buffer(data_bytes) + # 保存缓冲区引用 + self._buffer = data_buffer + # Get the pointer to the buffer + # 使用cast将缓冲区转换为POINTER(c_char)，然后获取其地址 + data_ptr = c_uint64(addressof(data_buffer)) + else: + # If data is not a numeric value, convert it to integer first + data_ptr = c_uint64(int(data)) + + err = self.api.contents.SetCommandExtraLong(c_int(0), data_ptr, self.handle)# 0 is CommandExtraLong_Extra0 + if err != MCPError.NoError: + raise RuntimeError('SetCommandExtraLong failed with error {0}'.format(MCPError._fields[err])) + + def get_result_code(self, commandHandle): + pCode = c_uint32() + err = self.api.contents.GetCommandResultCode(byref(pCode), commandHandle) + if err != MCPError.NoError: + raise RuntimeError('Get command result code failed with error {0}'.format(MCPError._fields[err])) + return pCode.value + + + def get_result_message(self, commandHandle): + pMsg = c_char_p() + err = self.api.contents.GetCommandResultMessage(pointer(pMsg), commandHandle) + if err != MCPError.NoError: + raise RuntimeError('Get command result message failed with error {0}'.format(MCPError._fields[err])) + return str(pMsg.value, encoding='utf8') + + def get_progress(self, commandHandle): + pExtraLong = c_ulonglong() + err = self.api.contents.GetCommandProgress(MCPCommandProgress.CommandProgress_CalibrateMotion, byref(pExtraLong), commandHandle) + if err != MCPError.NoError: + raise RuntimeError('GetProgress failed with error {0}'.format(MCPError._fields[err])) + return pExtraLong.value + + def destroy_command(self, commandHandle): + err = self.api.contents.DestroyCommand(commandHandle) + if err != MCPError.NoError: + raise RuntimeError('Destroy command failed with error {0}'.format(MCPError._fields[err])) + +class MCPEventDataReserved(Structure): + _fields_ = [ + ('reserved0', c_uint64), + ('reserved1', c_uint64), + ('reserved2', c_uint64), + ('reserved3', c_uint64), + ('reserved4', c_uint64), + ('reserved5', c_uint64), + ] + +class MCPEventCommandRespond(Structure): + _fields_ = [ + ('_commandHandle', MCPCommandHandle), + ('_replay', c_int32) + ] + +MCPReplay = namedtuple('EMCPReplay', [ + 'MCPReplay_Response', + 'MCPReplay_Running', + 'MCPReplay_Result' +])(0, 1, 2) + + + +MCPPWRHandle = c_uint64 +class MCPPWR(object): + IMCPPWR_Version = c_char_p(b"PROC_TABLE:IMCPPWR_001") + + class MCPPWRApi(Structure): + _fields_ = [ + ('GetPWRId', CFUNCTYPE(c_int32, POINTER(c_uint32), MCPPWRHandle)), + ('GetPWRStatus', CFUNCTYPE(c_int32, POINTER(c_int32), MCPPWRHandle)), + ('GetPWRPosition', CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPPWRHandle)), + ('GetPWRQuaternion', CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPPWRHandle)) + ] + api = POINTER(MCPPWRApi)() + def __init__(self, handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPPWR_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Cannot get MCPAliceHubApi: {0}'.format(MCPError._fields[err])) + self.handle = handle + + def get_PWR_id(self): + p_id = c_uint32() + err = self.api.contents.GetPWRId(byref(p_id), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetPWRId failed with error {0}'.format(MCPError._fields[err])) + return p_id.value + + def get_PWR_status(self): + p_status = c_int32() + err = self.api.contents.GetPWRStatus(byref(p_status), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetPWRStatus failed with error {0}'.format(MCPError._fields[err])) + return p_status.value + + def get_PWR_position(self): + p_x = c_float() + p_y = c_float() + p_z = c_float() + err = self.api.contents.GetPWRPosition(byref(p_x), byref(p_y), byref(p_z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetPWRPosition failed with error {0}'.format(MCPError._fields[err])) + return (p_x.value, p_y.value, p_z.value) + + def get_PWR_quaternion(self): + p_x = c_float() + p_y = c_float() + p_z = c_float() + p_w = c_float() + err = self.api.contents.GetPWRQuaternion(byref(p_x), byref(p_y), byref(p_z), byref(p_w), self.handle) + if err != MCPError.NoError: + raise RuntimeError('GetPWRQuaternion failed with error {0}'.format(MCPError._fields[err])) + return (p_x.value, p_y.value, p_z.value, p_w.value) + +MCPMarkerHandle = c_uint64 +MCPAliceBusHandle = c_uint64 +class MCPAliceHub(object): + IMCPAliceHub_Version = c_char_p(b"PROC_TABLE:IMCPAliceHub_001") + + class MCPAliceHubApi(Structure): + _fields_ = [ + ('GetSensorModuleList', CFUNCTYPE(c_int32, POINTER(MCPSensorModuleHandle), POINTER(c_int32))), + ('GetSensorModuleTimestamp', CFUNCTYPE(c_int32, POINTER(c_uint64))), + ('GetMarkerList', CFUNCTYPE(c_int32, POINTER(MCPMarkerHandle), POINTER(c_int32))), + ('GetMarkerTimestamp', CFUNCTYPE(c_int32, POINTER(c_uint64))), + ('GetRigidBodyList', CFUNCTYPE(c_int32, POINTER(MCPRigidBodyHandle), POINTER(c_int32))), + ('GetRigidBodyTimestamp', CFUNCTYPE(c_int32, POINTER(c_uint64))), + ('GetPWRList', CFUNCTYPE(c_int32, POINTER(MCPPWRHandle), POINTER(c_int32))), + ('GetPWRTimestamp', CFUNCTYPE(c_int32, POINTER(c_uint64))) + ] + + api = POINTER(MCPAliceHubApi)() + + def __init__(self): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPAliceHub_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Cannot get MCPAliceHubApi: {0}'.format(MCPError._fields[err])) + self.handle = MCPAliceBusHandle() + + def get_sensor_module_list(self, count=None): + if count is None: + count = c_int32() + err = self.api.contents.GetSensorModuleList(None, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get sensor module list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return None, count.value + else: + if isinstance(count, int): + count = c_int32(count) + handles = (MCPSensorModuleHandle * count.value)() + err = self.api.contents.GetSensorModuleList(handles, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get sensor module list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return handles, count.value + + def get_sensor_module_timestamp(self): + pTimestamp = c_uint64() + err = self.api.contents.GetSensorModuleTimestamp(byref(pTimestamp)) + if err != MCPError.NoError: + raise RuntimeError('Get sensor module timestamp failed with error {0}'.format(MCPError._fields[err])) + return pTimestamp.value + + def get_marker_list(self, count=None): + if count is None: + count = c_int32() + err = self.api.contents.GetMarkerList(None, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get marker list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return None, count.value + else: + if isinstance(count, int): + count = c_int32(count) + handles = (MCPMarkerHandle * count.value)() + err = self.api.contents.GetMarkerList(handles, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get marker list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return handles, count.value + + def get_marker_timestamp(self): + pTimestamp = c_uint64() + err = self.api.contents.GetMarkerTimestamp(byref(pTimestamp)) + if err != MCPError.NoError: + raise RuntimeError('Get PWR timestamp failed with error {0}'.format(MCPError._fields[err])) + return pTimestamp.value + + def get_rigid_body_list(self, count=None): + if count is None: + count = c_int32() + err = self.api.contents.GetRigidBodyList(None, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get rigid body list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return None, count.value + else: + if isinstance(count, int): + count = c_int32(count) + handles = (MCPRigidBodyHandle * count.value)() + err = self.api.contents.GetRigidBodyList(handles, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get rigid body list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return handles, count.value + + def get_rigid_body_timestamp(self): + pTimestamp = c_uint64() + err = self.api.contents.GetRigidBodyTimestamp(byref(pTimestamp)) + if err != MCPError.NoError: + raise RuntimeError('Get rigid body timestamp failed with error {0}'.format(MCPError._fields[err])) + return pTimestamp.value + + def get_PWR_list(self, count=None): + if count is None: + count = c_int32() + err = self.api.contents.GetPWRList(None, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get PWR list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return None, count.value + else: + if isinstance(count, int): + count = c_int32(count) + handles = (MCPPWRHandle * count.value)() + err = self.api.contents.GetPWRList(handles, byref(count)) + if err != MCPError.NoError: + raise RuntimeError('Get PWR list failed with error {0}'.format(MCPError._fields.get(err, "Unknown Error"))) + return handles, count.value + + def get_PWR_timestamp(self): + pTimestamp = c_uint64() + err = self.api.contents.GetPWRTimestamp(byref(pTimestamp)) + if err != MCPError.NoError: + raise RuntimeError('Get PWR timestamp failed with error {0}'.format(MCPError._fields[err])) + return pTimestamp.value + + +MCPSensorModuleHandle = c_uint64 +MCPSystemHandle = c_uint64 +class MCPSystem(object): + IMCPSystem_Version = c_char_p(b"PROC_TABLE:IMCPSystem_001") + + class MCPSystemApi(Structure): + _fields_ = [ + ('GetMasterVersion', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPSystemHandle)), + ('GetMasterSerialNumber', CFUNCTYPE(c_int32, POINTER(c_char_p), MCPSystemHandle)), + ('GetSensorModules', CFUNCTYPE(c_int32, MCPSensorModuleHandle, c_int32, POINTER(MCPSystemHandle))), + ] + + api = POINTER(MCPSystemApi)() + + def __init__(self, system_handle): + if not self.api: + self.api = POINTER(self.MCPSystemApi)() + err = MocapApi.MCPGetGenericInterface(self.IMCPSystem_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Cannot get MCPSystemApi: {0}'.format(MCPError._fields[err])) + self.handle = system_handle + + def get_master_version(self): + pstr = c_char_p() + err = self.api.contents.GetMasterVersion(pointer(pstr), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get master version: {0}'.format(MCPError._fields[err])) + return str(pstr.value, encoding='utf8') + + def get_master_serial_number(self): + pstr = c_char_p() + err = self.api.contents.GetMasterSerialNumber(pointer(pstr), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get master serial number: {0}'.format(MCPError._fields[err])) + return str(pstr.value, encoding='utf8') + + def get_sensor_modules(self, sensor_module_type): + pstr = c_char_p() + err = self.api.contents.GetSensorModules(sensor_module_type, pointer(pstr), pointer(self.handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not get sensor modules: {0}'.format(MCPError._fields[err])) + return str(pstr.value, encoding='utf8') + +MCPEventNotify = namedtuple('EMCPNotify', [ + 'Notify_RecordStarted', + 'Notify_RecordStoped', + 'Notify_RecordFinished', + 'Notify_SystemUpdated', +])(0, 1, 2, 3) + +class MCPEventNotifyData(Structure): + _fields_ = [ + ('_notify', c_int32), + ('_notifyHandle', MCPSystemHandle) + ] + +MCPTrackerHandle = c_uint64 + +class MCPEventMotionData(Structure): + _fields_ = [ + ('avatarHandle', MCPAvatarHandle) + ] + +class MCPEventSystemError(Structure): + _fields_ = [ + ('error', c_int32), + ('info0', c_uint64) + ] + +class MCPEventSensorModuleData(Structure): + _fields_ = [ + ('_sensorModuleHandle', MCPSensorModuleHandle) + ] + +class MCPEventTrackerData(Structure): + _fields_ = [ + ('_trackerHandle', MCPTrackerHandle) + ] + +class MCPEventMarkerData(Structure): + _fields_ = [ + ('_markerHandle', MCPMarkerHandle) + ] + +class MCPEventPWRData(Structure): + _fields_ = [ + ('_pwrHandle', MCPPWRHandle) + ] + +class MCPEventData(Union): + _fields_ = [ + ('reserved', MCPEventDataReserved), + ('motionData', MCPEventMotionData), + ('systemError', MCPEventSystemError), + ('sensorModuleData', MCPEventSensorModuleData), + ('trackerData', MCPEventTrackerData), + ('commandRespond', MCPEventCommandRespond), + ('markerData', MCPEventMarkerData), + ('pwrData', MCPEventPWRData), + ('notifyData', MCPEventNotifyData) + ] + +MCPEventType = namedtuple('EMCPEventType', [ + 'InvalidEvent', + 'AvatarUpdated', + 'RigidBodyUpdated', + 'Error', + 'SensorModulesUpdated', + 'TrackerUpdated', + "CommandReply", + "Notify", + "AliceIMUUpdated", + "AliceRigidbodyUpdated", + "AliceTrackerUpdated", + "AliceMarkerUpdated", +])(0, 256, 512, 768, 1024, 1280,1536, 1792, 4096,4097,4098,4099) + +class MCPEvent(Structure): + _fields_ = [ + ("size", c_uint32), + ("event_type", c_int32), + ('timestamp', c_double), + ("event_data", MCPEventData) + ] + +MCPBvhRotation = namedtuple('EMCPBvhRotation', [ + 'XYZ', + 'XZY', + 'YXZ', + 'YZX', + 'ZXY', + 'ZYX' +])(0, 1, 2, 3, 4, 5) + +MCPBvhData = namedtuple('EMCPBvhData', [ + 'String', + 'BinaryWithOldFrameHeader', + 'Binary', + 'BinaryLegacyHumanHierarchy' +])(0, 1, 2, 6) + +MCPBvhDisplacement = namedtuple('EMCPBvhDisplacement', [ + 'Disable', + 'Enable' +])(0, 1) + +class MCPTracker(object): + IMCPTracker_Version = c_char_p(b"PROC_TABLE:IMCPTracker_001") + + class MCPTrackerApi(Structure): + _fields_ = [ + ('SendMessageData', CFUNCTYPE(c_int32, c_char_p, c_int32, MCPTrackerHandle)), + ('GetTrackerRotation', CFUNCTYPE(c_int32, POINTER(c_float),POINTER(c_float),POINTER(c_float),POINTER(c_float), c_char_p, MCPTrackerHandle)), + ('GetTrackerPosition', CFUNCTYPE(c_int32, POINTER(c_float),POINTER(c_float),POINTER(c_float), c_char_p, MCPTrackerHandle)), + ('GetTrackerEulerAng', CFUNCTYPE(c_int32, POINTER(c_float),POINTER(c_float),POINTER(c_float), c_char_p, MCPTrackerHandle)), + ('GetDeviceCount', CFUNCTYPE(c_int32, POINTER(c_int32), MCPTrackerHandle)), + ('GetDeviceName', CFUNCTYPE(c_int32, c_int32, POINTER(c_char_p), MCPTrackerHandle)), + ] + + api = POINTER(MCPTrackerApi)() + + def __init__(self, tracker_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPTracker_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPTracker interface: {0}'.format(MCPError._fields[err])) + self.handle = tracker_handle + + def get_device_name(self, serialNum): + name = c_char_p() + err = self.api.contents.GetDeviceName(c_int32(serialNum), byref(name), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get device name: {0}'.format(MCPError._fields[err])) + return name.value.decode('utf-8') + + def get_device_count(self): + devCount = c_int32(0) + err = self.api.contents.GetDeviceCount(byref(devCount), self.handle) + if err != MCPError.NoError: + print(f"GetDeviceCount err: {err}") + raise RuntimeError('Can not get device count: {0}'.format(MCPError._fields[err])) + return devCount.value + + + def get_tracker_rotation(self, device_name): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + device_name_arg = c_char_p(device_name.encode('utf-8')) + err = self.api.contents.GetTrackerRotation(byref(x), byref(y), byref(z), byref(w), device_name_arg, self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get tracker rotation:{}'.format(MCPError._fields[err])) + return x.value, y.value, z.value, w.value + + def get_tracker_rotataion(self, device_name): + return self.get_tracker_rotation(device_name) + + def get_tracker_position(self, device_name): + x = c_float() + y = c_float() + z = c_float() + device_name_arg = c_char_p(device_name.encode('utf-8')) + err = self.api.contents.GetTrackerPosition(byref(x), byref(y), byref(z), device_name_arg, self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get tracker position:{}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + + def send_message_data(self): + message = c_char_p() + len = c_int32() + err = self.api.contents.SendMessageData(message, len, self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not Send Message Data: {0}'.format(MCPError._fields[err])) + return message.value, len.value + + + +class MCPMarker(object): + IMCPMarker_Version = c_char_p(b"PROC_TABLE:IMCPMarker_001") + + class MCPMarkerApi(Structure): + _fields_ = [ + ('GetMarkerPosition', CFUNCTYPE(c_int32, POINTER(c_float),POINTER(c_float),POINTER(c_float), MCPMarkerHandle)), + ] + + api = POINTER(MCPMarkerApi)() + + def __init__(self, marker_handle): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPMarker_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPMarker interface: {0}'.format(MCPError._fields[err])) + self.handle = marker_handle + + def get_marker_position(self): + marker_x = c_float() + marker_y = c_float() + marker_z = c_float() + err = self.api.contents.GetMarkerPosition(pointer(marker_x), pointer(marker_y), pointer(marker_z), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get marker position: {0}'.format(MCPError._fields[err])) + return marker_x.value, marker_y.value, marker_z.value + +MCPSettingsHandle = c_uint64 + +class MCPSettings(object): + IMCPSettingsApi_Version = c_char_p(b'PROC_TABLE:IMCPSettings_001') + + class MCPSettingsApi(Structure): + _fields_ = [ + ('CreateSettings', CFUNCTYPE(c_int32, POINTER(MCPSettingsHandle))), + ('DestroySettings', CFUNCTYPE(c_int32, MCPSettingsHandle)), + ('SetSettingsUDP', CFUNCTYPE(c_int32, c_uint16, MCPSettingsHandle)), + ('SetSettingsTCP', CFUNCTYPE(c_int32, c_char_p, c_uint16, MCPSettingsHandle)), + ('SetSettingsBvhRotation', CFUNCTYPE(c_int32, c_int32, MCPSettingsHandle)), + ('SetSettingsBvhTransformation', CFUNCTYPE(c_int32, c_int32, MCPSettingsHandle)), + ('SetSettingsBvhData', CFUNCTYPE(c_int32, c_int32, MCPSettingsHandle)), + ('SetSettingsCalcData', CFUNCTYPE(c_int32, MCPSettingsHandle)), + ("SetSettingsUDPServer", CFUNCTYPE(c_int32, c_char_p, c_uint16, MCPSettingsHandle)), + ("SetSettingsUDPEx", CFUNCTYPE(c_int32, c_char_p, c_uint16, MCPSettingsHandle)) + ] + + api = POINTER(MCPSettingsApi)() + + def __init__(self): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPSettingsApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPSettings interface: {0}'.format(MCPError._fields[err])) + self.handle = MCPSettingsHandle() + err = self.api.contents.CreateSettings(pointer(self.handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not create settings: {0}'.format(MCPError._fields[err])) + + def __del__(self): + err = self.api.contents.DestroySettings(self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not destroy settings: {0}'.format(MCPError._fields[err])) + + def set_udp(self, local_port): + err = self.api.contents.SetSettingsUDP(c_uint16(local_port), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set udp port of {0}: {1}'.format(local_port, MCPError._fields[err])) + + def set_tcp(self, ip, port): + err = self.api.contents.SetSettingsTCP(c_char_p(bytes(ip, encoding='utf8')), c_uint16(port), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not settings tcp addr of {0}:{1}: {2}'.format(ip, port, MCPError._fields[err])) + + def set_bvh_rotation(self, bvh_rotation): + err = self.api.contents.SetSettingsBvhRotation(c_int32(bvh_rotation), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set bvh rotation: {0}'.format(MCPError._fields[err])) + + def set_bvh_displacement(self, bvh_transformation): + err = self.api.contents.SetSettingsBvhDisplacement(c_int32(bvh_transformation), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set bvh displacement: {0}'.format(MCPError._fields[err])) + def set_bvh_transformation(self, bvh_transformation): + err = self.api.contents.SetSettingsBvhTransformation(c_int32(bvh_transformation), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set bvh Transformation: {0}'.format(MCPError._fields[err])) + + def set_bvh_data(self, bvh_data): + err = self.api.contents.SetSettingsBvhData(c_int32(bvh_data), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set bvh data: {0}'.format(MCPError._fields[err])) + + def set_calc_data(self): + err = self.api.contents.SetSettingsCalcData(self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set calc data: {0}'.format(MCPError._fields[err])) + def SetSettingsUDPServer(self, server_ip, port) -> None: + err = self.api.contents.SetSettingsUDPServer( + c_char_p(bytes(server_ip, encoding='utf8')), + c_uint16(port), + self.handle + ) + if err != 0: + raise RuntimeError(f"UDPServer config failed with code {err}") + def SetSettingsUDPEx(self, local_ip, port) -> None: + err = self.api.contents.SetSettingsUDPEx( + c_char_p(bytes(local_ip, encoding='utf8')), + c_uint16(port), + self.handle + ) + if err != 0: + raise RuntimeError(f"UDPEx config failed with code {err}") + +MCPUpVector = namedtuple('EMCPUpVector', [ + 'XAxis', + 'YAxis', + 'ZAxis' +])(1, 2, 3) + +MCPFrontVector = namedtuple('EMCPFrontVector', [ + 'ParityEven', + 'ParityOdd' +])(1, 2) + +MCPCoordSystem = namedtuple('EMCPCoordSystem', [ + 'RightHanded', + 'LeftHanded' +])(0, 1) + +MCPRotatingDirection = namedtuple('EMCPRotatingDirection', [ + 'Clockwise', + 'CounterClockwise' +])(0, 1) + +MCPPreDefinedRenderSettings = namedtuple('EMCPPreDefinedRenderSettings', [ + 'Default', + 'UnrealEngine', + 'Unity3D', + 'Count' +])(0, 1, 2, 3) + +MCPUnit = namedtuple('EMCPUnit', [ + 'Centimeter', + 'Meter' +])(0, 1) + +MCPRenderSettingsHandle = c_uint64 + + +class MCPRenderSettings(object): + IMCPRenderSettingsApi_Version = c_char_p(b'PROC_TABLE:IMCPRenderSettings_001') + + class MCPRenderSettingsApi(Structure): + _fields_ = [ + ('CreateRenderSettings', CFUNCTYPE(c_int32, POINTER(MCPRenderSettingsHandle))), + ('GetPreDefRenderSettings', CFUNCTYPE(c_int32, c_int32, POINTER(MCPRenderSettingsHandle))), + ('SetUpVector', CFUNCTYPE(c_int32, c_int32, c_int32, MCPRenderSettingsHandle)), + ('GetUpVector', CFUNCTYPE(c_int32, POINTER(c_int32), POINTER(c_int32), MCPRenderSettingsHandle)), + ('SetFrontVector', CFUNCTYPE(c_int32, c_int32, c_int32, MCPRenderSettingsHandle)), + ('GetFrontVector', CFUNCTYPE(c_int32, POINTER(c_int32), POINTER(c_int32), MCPRenderSettingsHandle)), + ('SetCoordSystem', CFUNCTYPE(c_int32, c_int32, MCPRenderSettingsHandle)), + ('GetCoordSystem', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRenderSettingsHandle)), + ('SetRotatingDirection', CFUNCTYPE(c_int32, c_int32, MCPRenderSettingsHandle)), + ('GetRotationDirection', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRenderSettingsHandle)), + ('SetUnit', CFUNCTYPE(c_int32, c_int32, MCPRenderSettingsHandle)), + ('GetUnit', CFUNCTYPE(c_int32, POINTER(c_int32), MCPRenderSettingsHandle)), + ('DestroyRenderSettings', CFUNCTYPE(c_int32, MCPRenderSettingsHandle)) + ] + + api = POINTER(MCPRenderSettingsApi)() + + def __init__(self, pre_def=None): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPRenderSettingsApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPRenderSettings interface: {0}'.format(MCPError._fields[err])) + self.pre_def = pre_def + self.handle = MCPRenderSettingsHandle() + if self.pre_def == None: + err = self.api.contents.CreateRenderSettings(pointer(self.handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not create render settings: {0}'.format(MCPError._fields[err])) + else: + err = self.api.contents.GetPreDefRenderSettings(c_int32(pre_def), pointer(self.handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not get render settings: {0}'.format(MCPError._fields[err])) + + def __del__(self): + if self.pre_def == None: + err = self.api.contents.DestroyRenderSettings(self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not destroy render settings: {0}'.format(MCPError._fields[err])) + + def set_up_vector(self, up_vector, sign): + err = self.api.contents.SetUpVector(c_int32(up_vector), c_int32(sign), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set up vector: {0}'.format(MCPError._fields[err])) + + def get_up_vector(self): + up_vector = c_int32() + sign = c_int32() + err = self.api.contents.GetUpVector(pointer(up_vector), pointer(sign), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get up vector: {0}'.format(MCPError._fields[err])) + return up_vector.value, sign.value + + def set_front_vector(self, front_vector, sign): + err = self.api.contents.SetFrontVector(c_int32(front_vector), c_int32(sign), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set front vector: {0}'.format(MCPError._fields[err])) + + def get_front_vector(self): + front_vector = c_int32() + sign = c_int32() + err = self.api.contents.GetFrontVector(pointer(front_vector), pointer(sign), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get front vector: {0}'.format(MCPError._fields[err])) + return front_vector.value, sign.value + + def set_coord_system(self, coord_sys): + err = self.api.contents.SetCoordSystem(c_int32(coord_sys), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set coord system: {0}'.format(MCPError._fields[err])) + + def get_coord_system(self): + coord_sys = c_int32() + err = self.api.contents.GetCoordSystem(pointer(coord_sys), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get coord system: {0}'.format(MCPError._fields[err])) + return coord_sys.value + + def set_rotating_direction(self, rotating_direction): + err = self.api.contents.SetRotatingDirection(c_int32(rotating_direction), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set rotating direction: {0}'.format(MCPError._fields[err])) + + def get_rotating_direction(self): + rotating_direction = c_int32() + err = self.api.contents.GetRotatingDirection(pointer(rotating_direction), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get rotating direction: {0}'.format(MCPError._fields[err])) + return rotating_direction.value + + def set_unit(self, unit): + err = self.api.contents.SetUnit(c_int32(unit), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set unit: {0}'.format(MCPError._fields[err])) + + def get_unit(self): + unit = c_int32() + err = self.api.contents.GetUnit(pointer(unit), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get unit: {0}'.format(MCPError._fields[err])) + return unit.value + +MCPRobotJointType=namedtuple('EMCPRobotJointType', + ['JointLeftShoulder_Pitch', + 'JointLeftShoulder_Roll', + 'JointLeftShoulder_Yaw', + 'JointLeftElbow', + 'JointLeftWrist_Yaw', + 'JointLeftWrist_Roll', + 'JointLeftWrist_Pitch', + 'JointRightShoulder_Pitch', + 'JointRightShoulder_Roll', + 'JointRightShoulder_Yaw', + 'JointRightElbow', + 'JointRightWrist_Yaw', + 'JointRightWrist_Roll', + 'JointRightWrist_Pitch', + 'JointLeftHip_Yaw', + 'JointLeftHip_Pitch', + 'JointLeftHip_Roll', + 'JointLeftKnee', + 'JointLeftAnkle_Pitch', + 'JointLeftAnkle_Roll', + 'JointRightHip_Yaw', + 'JointRightHip_Pitch', + 'JointRightHip_Roll', + 'JointRightKnee', + 'JointRightAnkle_Pitch', + 'JointRightAnkle_Roll', + 'JointSpine_Yaw', + 'JointSpine_Roll', + 'JointSpine_Pitch', + 'JointHead_Yaw', + 'JointHead_Roll', + 'JointHead_Pitch', + 'JointCount'])._make([x for x in range(0,33)]) + +MCPRobotHandle = c_uint64 + +class MCPRobot(object): + IMCPRobot_Version = c_char_p(b"PROC_TABLE:IMCPRobot_001") + class MCPRobotApi(Structure): + _fields_=[ + ('CreateRobot', CFUNCTYPE(c_int32,c_char_p,POINTER(MCPRobotHandle))), + ('SetRobotFPS', CFUNCTYPE(c_int32, c_int32, MCPRobotHandle)), + ('UpdateRobot', CFUNCTYPE(c_int32, MCPAvatarHandle, MCPRobotHandle)), + ('GetRobotRawJointAngle', CFUNCTYPE(c_int32, c_int32, POINTER(c_float), MCPRobotHandle)), + ('GetRobotRetargetJointAngle', CFUNCTYPE(c_int32, c_char_p, POINTER(c_float), MCPRobotHandle)), + ('GetRobotRosFrameJson', CFUNCTYPE(c_int32, POINTER(c_char_p), c_bool, MCPRobotHandle)), + ('GetRobotRootRotation', CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPRobotHandle)), + ('GetRobotRootPosition', CFUNCTYPE(c_int32, POINTER(c_float), POINTER(c_float), POINTER(c_float), MCPRobotHandle)), + ('DestroyRobot', CFUNCTYPE(c_int32, MCPRobotHandle)), + ('RunRobotStep', CFUNCTYPE(c_int32, MCPRobotHandle)) + ] + api = POINTER(MCPRobotApi)() + def __init__(self, json_robot_desc): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPRobot_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get MCPRobot interface: {}'.format(MCPError._fields[err])) + self.handle = MCPSettingsHandle() + err = self.api.contents.CreateRobot(c_char_p(bytes(json_robot_desc, encoding='utf8')), pointer(self.handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not create robot: {0}'.format(MCPError._fields[err])) + def __del__(self): + err = self.api.contents.DestroyRobot(self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not destroy robot : {0}'.format(MCPError._fields[err])) + + def set_robot_fps(self,fps): + err = self.api.contents.SetRobotFPS(c_uint32(fps),self.handle) + if self != MCPError.NoError: + raise RuntimeError('Can not set robot fps: {0}'.format(MCPError._fields[err])) + def update_robot(self,avatar): + err = self.api.contents.UpdateRobot(avatar.handle, self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not update robot: {0}'.format(MCPError._fields[err])) + def get_robot_raw_joint_angle(self,robotJointType): + val = c_float() + err = self.api.contents.GetRobotRawJointAngle(c_int32(robotJointType),pointer(val), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get robot raw joint angle: {0}'.format(MCPError._fields[err])) + return val.value + def get_robot_retarget_joint_angle(self,jointName): + val = c_float() + err = self.api.contents.GetRobotRetargetJointAngle(c_char_p(bytes(jointName,encoding='utf8')), pointer(val),self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get robot retarget joint angle: {0}'.format(MCPError._fields[err])) + return val.value + def get_robot_ros_frame_json(self): + val = c_float() + json_str = c_char_p() + err = self.api.contents.GetRobotRosFrameJson(pointer(json_str), pointer(val), self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get robot ros frame json: {0}'.format(MCPError._fields[err])) + return str(json_str.value, encoding='utf8'),val.value + def get_robot_root_rotataion(self): + x = c_float() + y = c_float() + z = c_float() + w = c_float() + err = self.api.contents.GetRobotRootRotation(pointer(x),pointer(y),pointer(z),pointer(w),self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get robot robot rotation:{}'.format(MCPError._fields[err])) + return x.value, y.value, z.value, w.value + def get_robot_root_position(self): + x = c_float() + y = c_float() + z = c_float() + err = self.api.contents.GetRobotRootPosition(pointer(x), pointer(y), pointer(z),self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get robot root position:{}'.format(MCPError._fields[err])) + return x.value, y.value, z.value + def run_robot_step(self): + err = self.api.contents.RunRobotStep(self.handle) + if err != MCPError.NoError: + raise RuntimeError('Can not run robot step:{}'.format(MCPError._fields[err])) + +EMCPCommand = namedtuple('EMCPCommand', [ + 'CommandStartCapture', + 'CommandStopCapture', + 'CommandZeroPosition', + 'CommandCalibrateMotion', + 'CommandStartRecored', + 'CommandStopRecored', + 'CommandResumeOriginalPosture', + 'CommandClearZeroMotionDrift', + "CommandResumeOriginalHandsPosture" +])(0, 1, 2, 3,4,5,6,7,8) + +MCPApplicationHandle = c_uint64 +MCPTrackerHandle = c_uint64 +MCPEventHandleProc = c_uint64 +class MCPApplication(object): + IMCPApplicationApi_Version = c_char_p(b'PROC_TABLE:IMCPApplication_002') + + class MCPApplicationApi(Structure): + _fields_ = [ + ('CreateApplication', CFUNCTYPE(c_int32, POINTER(MCPApplicationHandle))), + ('DestroyApplication', CFUNCTYPE(c_int32, MCPApplicationHandle)), + ('SetApplicationSettings', CFUNCTYPE(c_int32, MCPSettingsHandle, MCPApplicationHandle)), + ('SetApplicationRenderSettings', CFUNCTYPE(c_int32, MCPRenderSettingsHandle, MCPApplicationHandle)), + ('OpenApplication', CFUNCTYPE(c_int32, MCPApplicationHandle)), + ('EnableApplicationCacheEvents', CFUNCTYPE(c_int32, MCPApplicationHandle)), + ('DisableApplicationCacheEvents', CFUNCTYPE(c_int32, MCPApplicationHandle)), + ('ApplicationCacheEventsIsEnabled', CFUNCTYPE(c_int32, POINTER(c_bool), MCPApplicationHandle)), + ('CloseApplication', CFUNCTYPE(c_int32, MCPApplicationHandle)), + ('GetApplicationRigidBodies', CFUNCTYPE(c_int32, POINTER(c_uint64), POINTER(c_uint32), MCPApplicationHandle)), + ('GetApplicationAvatars', CFUNCTYPE(c_int32, POINTER(c_uint64), POINTER(c_uint32), MCPApplicationHandle)), + ('PollApplicationNextEvent', CFUNCTYPE(c_int32, POINTER(MCPEvent), POINTER(c_uint32), MCPApplicationHandle)), + ('GetApplicationSensorModules', CFUNCTYPE(c_int32, POINTER(MCPSensorModuleHandle), POINTER(c_uint32), MCPApplicationHandle)), + ('GetApplicationTrackers', CFUNCTYPE(c_int32, POINTER(MCPTrackerHandle), POINTER(c_uint32), MCPApplicationHandle)), + ('QueuedServerCommand', CFUNCTYPE(c_int32, MCPCommandHandle, MCPApplicationHandle)), + ('RegisterEventHandler', CFUNCTYPE(c_int32, MCPEventHandleProc, POINTER(c_uint64), MCPApplicationHandle)), + ('UnregisterEventHandler', CFUNCTYPE(c_int32, MCPEventHandleProc, POINTER(c_uint64), MCPApplicationHandle)), + ('GetAppplicationSystem', CFUNCTYPE(c_int32, POINTER(MCPSystemHandle), MCPApplicationHandle)), + ] + + api = POINTER(MCPApplicationApi)() + + def __init__(self): + if not self.api: + err = MocapApi.MCPGetGenericInterface(self.IMCPApplicationApi_Version, pointer(self.api)) + if err != MCPError.NoError: + raise RuntimeError('Can not get IMCPApplication interface: {0}'.format(MCPError._fields[err])) + self._handle = MCPApplicationHandle() + err = self.api.contents.CreateApplication(pointer(self._handle)) + if err != MCPError.NoError: + raise RuntimeError('Can not create application: {0}'.format(MCPError._fields[err])) + self._is_opened = False + + def queue_command(self, cmd_type): + cmd = MCPCommand() + cmd.create_command(cmd_type) + err = self.api.contents.QueuedServerCommand(cmd.handle, self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not queued server command: {0}'.format(MCPError._fields[err])) + + def queue_command_with_extra_long(self, cmd_type, extra_long): + cmd = MCPCommand() + cmd.create_command(cmd_type) # 先创建命令初始化句柄 + # 根据C++实现，使用CommandExtraLong_Extra0（值为0）作为commandType + cmd.set_command_extra_long(extra_long) # 0 is CommandExtraLong_Extra0 + err = self.api.contents.QueuedServerCommand(cmd.handle, self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not queued server command: {0}'.format(MCPError._fields[err])) + + def __del__(self): + err = self.api.contents.DestroyApplication(self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not destroy application: {0}'.format(MCPError._fields[err])) + + def set_settings(self, settings): + err = self.api.contents.SetApplicationSettings(settings.handle, self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set application settings: {0}'.format(MCPError._fields[err])) + + def set_render_settings(self, settings): + err = self.api.contents.SetApplicationRenderSettings(settings.handle, self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not set application render settings: {0}'.format(MCPError._fields[err])) + + def open(self): + err = self.api.contents.OpenApplication(self._handle) + self._is_opened = (err == MCPError.NoError) + return err == MCPError.NoError, MCPError._fields[err] + + def is_opened(self): + return self._is_opened + + def enable_event_cache(self): + err = self.api.contents.EnableApplicationCacheEvents(self._handle) + return err == MCPError.NoError, MCPError._fields[err] + + def disable_event_cache(self): + err = self.api.contents.DisableApplicationCacheEvents(self._handle) + return err == MCPError.NoError, MCPError._fields[err] + + def is_event_cache_enabled(self): + enable = c_bool() + err = self.api.contents.ApplicationCacheEventsIsEnabled(pointer(enable), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application event cache settings: {0}'.format(MCPError._fields[err])) + return enable.value + + def close(self): + err = self.api.contents.CloseApplication(self._handle) + self._is_opened = False + return err == MCPError.NoError, MCPError._fields[err] + + def get_rigid_bodies(self): + rigid_body_size = c_uint32() + err = self.api.contents.GetApplicationRigidBodies(POINTER(MCPRigidBodyHandle)(), pointer(rigid_body_size), + self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application rigid bodies: {0}'.format(MCPError._fields[err])) + rigid_body_handles = (MCPRigidBodyHandle * rigid_body_size.value)() + err = self.api.GetApplicationRigidBodies(rigid_body_handles, pointer(rigid_body_size), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application rigid bodies: {0}'.format(MCPError._fields[err])) + return [MCPRigidBody(rigid_body_handles[i]) for i in range(rigid_body_size.value)] + + def get_avatars(self): + avatar_count = c_uint32() + err = self.api.contents.GetApplicationAvatars(POINTER(MCPAvatarHandle)(), pointer(avatar_count), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application avatars: {0}'.format(MCPError._fields[err])) + avatar_handles = (MCPAvatarHandle * avatar_count.value)() + err = self.api.contents.GetApplicationAvatars(avatar_handles, pointer(avatar_count), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application avatars: {0}'.format(MCPError._fields[err])) + return [MCPAvatar(avatar_handles[i]) for i in range(avatar_count.value)] + def get_trackers(self): + tracker_count = c_uint32() + err = self.api.contents.GetApplicationTrackers(POINTER(MCPTrackerHandle)(), pointer(tracker_count), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application trackers: {0}'.format(MCPError._fields[err])) + tracker_handles = (MCPTrackerHandle * tracker_count.value)() + err = self.api.contents.GetApplicationTrackers(tracker_handles, pointer(tracker_count), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application trackers: {0}'.format(MCPError._fields[err])) + return [MCPTracker(tracker_handles[i]) for i in range(tracker_count.value)] + + def poll_next_event(self): + evt_count = c_uint32(0) + err = self.api.contents.PollApplicationNextEvent(POINTER(MCPEvent)(), pointer(evt_count), self._handle) + if err not in (MCPError.NoError, MCPError.MoreEvent): + raise RuntimeError('Can not poll application next event: {0}'.format(MCPError._fields[err])) + if evt_count.value == 0: + return [] + evt_array = (MCPEvent * evt_count.value)() + for i in range(evt_count.value): + evt_array[i].size = sizeof(MCPEvent) + err = self.api.contents.PollApplicationNextEvent(evt_array, pointer(evt_count), self._handle) + if err not in (MCPError.NoError, MCPError.MoreEvent): + raise RuntimeError('Can not poll application next event: {0}'.format(MCPError._fields[err])) + if evt_count.value == 0: + return [] + return [evt_array[i] for i in range(evt_count.value)] + + def get_system(self): + systemHandle = MCPSystemHandle() + err = self.api.contents.GetAppplicationSystem(pointer(systemHandle), self._handle) + if err != MCPError.NoError: + raise RuntimeError('Can not get application system: {0}'.format(MCPError._fields[err])) + return systemHandle + +def test_mocap_api(): + def print_joint(joint): + print(joint.get_name()) + print(joint.get_local_rotation()) + print(joint.get_local_rotation_by_euler()) + print(joint.get_local_position()) + + children = joint.get_children() + for child in children: + print_joint(child) + + app = MCPApplication() + settings = MCPSettings() + settings.set_udp(7012) + app.set_settings(settings) + app.open() + # app.queue_command1(1) + + + while True: + evts = app.poll_next_event() + for evt in evts: + if evt.event_type == MCPEventType.AvatarUpdated: + avatar = MCPAvatar(evt.event_data.avatar_handle) + print(f"get_avatar_posture_ptp_time():{avatar.get_avatar_posture_ptp_time()}") + elif evt.event_type == MCPEventType.RigidBodyUpdated: + print('rigid body updated') + elif evt.event_type == MCPEventType.AliceTrackerUpdated: + _handle_tracker_data() + elif evt.event_type == MCPEventType.AliceMarkerUpdated: + _handle_marker_data() + else: + print('unknow event') + time.sleep(0.001) + +def _handle_marker_data(): + alicehub = MCPAliceHub() + recv, count = alicehub.get_marker_list() + if count > 0: + recv, count1 = alicehub.get_marker_list(count) + for i in range(count): + marker_handle = recv[i] + marker = MCPMarker(marker_handle) + position = marker.get_marker_position() + print('position:', position) + + +def _handle_tracker_data(): + alicehub = MCPAliceHub() + recv, count = alicehub.get_PWR_list() + if count > 0: + recv, count1 = alicehub.get_PWR_list(count) + timestamp = alicehub.get_PWR_timestamp() + for i in range(count): + PWRHandle = recv[i] + MCPPWRH = MCPPWR(PWRHandle) + id = MCPPWRH.get_PWR_id() + status = MCPPWRH.get_PWR_status() + position = MCPPWRH.get_PWR_position() + quaternion = MCPPWRH.get_PWR_quaternion() + print('timestamp',timestamp,'id:', id, 'status:', status, 'position:', position, 'quaternion:', quaternion) +if __name__ == '__main__': + test_mocap_api() \ No newline at end of file diff --git a/src/plugins/noitom_mocap/tools/noitom_mocap_printer.py b/src/plugins/noitom_mocap/tools/noitom_mocap_printer.py new file mode 100755 index 000000000..be8cc2522 --- /dev/null +++ b/src/plugins/noitom_mocap/tools/noitom_mocap_printer.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Print live full-body frames published by noitom_mocap_plugin.""" + +from __future__ import annotations + +import argparse +import time + +import isaacteleop.deviceio as deviceio +import isaacteleop.oxr as oxr +from isaacteleop.schema import BodyJointPN + + +_JOINT_NAMES = ( + "pelvis", + "left_hip", + "right_hip", + "spine1", + "left_knee", + "right_knee", + "spine2", + "left_ankle", + "right_ankle", + "spine3", + "left_foot", + "right_foot", + "neck", + "left_collar", + "right_collar", + "head", + "left_shoulder", + "right_shoulder", + "left_elbow", + "right_elbow", + "left_wrist", + "right_wrist", + "left_hand", + "right_hand", +) + + +def _format_point(point: object | None) -> str: + if point is None: + return "[n/a]" + return f"[{point.x:+.3f}, {point.y:+.3f}, {point.z:+.3f}]" + + +def _joint(frame: object, index: int) -> object: + return frame.joints.joints(index) + + +def _print_frame( + frame: object, + frame_index: int, + elapsed_s: float, + max_joints: int, +) -> None: + joint_count = int(BodyJointPN.NUM_JOINTS) + valid_joints = ( + 0 + if frame.joints is None + else sum(1 for index in range(joint_count) if _joint(frame, index).is_valid) + ) + + print( + f"[{elapsed_s:6.2f}s] frame={frame_index:05d} " + f"joints={valid_joints}/{joint_count} " + f"all_joint_poses_tracked={frame.all_joint_poses_tracked}" + ) + + if frame.joints is None: + return + for index in range(min(max_joints, joint_count)): + joint = _joint(frame, index) + print( + f" joint {_JOINT_NAMES[index]:16s} valid={joint.is_valid} " + f"pos={_format_point(joint.pose.position)}" + ) + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--collection-id", default="noitom_mocap", help="OpenXR tensor collection id." + ) + parser.add_argument("--max-flatbuffer-size", type=int, default=16 * 1024) + parser.add_argument( + "--duration", + type=float, + default=10.0, + help="Seconds to read. Use 0 for forever.", + ) + parser.add_argument("--rate", type=float, default=60.0, help="Polling rate in Hz.") + parser.add_argument( + "--print-period", type=float, default=0.5, help="Seconds between status prints." + ) + parser.add_argument("--max-joints", type=int, default=6) + args = parser.parse_args() + + tracker = deviceio.FullBodyTrackerPN(args.collection_id, args.max_flatbuffer_size) + required_extensions = deviceio.DeviceIOSession.get_required_extensions([tracker]) + + print("Noitom full-body printer") + print(f" collection_id={args.collection_id}") + print(" positions=meters") + print(f" required_extensions={required_extensions}") + print(" start noitom_mocap_plugin before running this script") + + sleep_s = 0.0 if args.rate <= 0.0 else 1.0 / args.rate + start_s = time.monotonic() + last_print_s = 0.0 + frame_index = 0 + seen_frame = False + + with oxr.OpenXRSession("NoitomMocapPrinter", required_extensions) as oxr_session: + handles = oxr_session.get_handles() + with deviceio.DeviceIOSession.run([tracker], handles) as session: + while args.duration <= 0.0 or time.monotonic() - start_s < args.duration: + session.update() + tracked = tracker.get_body_pose(session) + frame = tracked.data + elapsed_s = time.monotonic() - start_s + + if frame is None: + if elapsed_s - last_print_s >= args.print_period: + print(f"[{elapsed_s:6.2f}s] no Noitom frame yet") + last_print_s = elapsed_s + elif elapsed_s - last_print_s >= args.print_period: + seen_frame = True + _print_frame(frame, frame_index, elapsed_s, args.max_joints) + last_print_s = elapsed_s + + frame_index += 1 + if sleep_s > 0.0: + time.sleep(sleep_s) + + if not seen_frame: + print( + "No frames received. Check that the plugin is running and the collection id matches." + ) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())