diff --git a/CMakeLists.txt b/CMakeLists.txt index ca89af08b..38ca74cd0 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 (downloads MocapApi SDK)" OFF) option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 example (e.g. for Docker)" OFF) @@ -188,6 +189,9 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/rebot_devarm_leader) add_subdirectory(src/plugins/manus) add_subdirectory(src/plugins/haptikos) + if(BUILD_PLUGIN_NOITOM_MOCAP) + add_subdirectory(src/plugins/noitom_mocap) + endif() if(BUILD_PLUGIN_OAK_CAMERA) add_subdirectory(src/plugins/oak) endif() diff --git a/examples/noitom/README.md b/examples/noitom/README.md new file mode 100644 index 000000000..0dc48edc8 --- /dev/null +++ b/examples/noitom/README.md @@ -0,0 +1,127 @@ + + +# Noitom G1 Teleop + +This example registers an external Isaac Lab task based on +`Isaac-PickPlace-Locomanipulation-G1-Abs-v0` and drives its upper-body action +pipeline from Noitom full-body mocap. + +The `noitom_mocap` plugin reads Noitom Hybrid Data Server data through +MocapApi, converts each avatar update to the existing IsaacTeleop +`FullBodyPose` layout, and publishes it on tensor identifier `full_body` +inside the `noitom_mocap` OpenXR tensor collection. The Python task consumes +that stream through the generic `FullBodyTracker()` with the `body.noitom` +vendor selected. Plugin launch defaults, including the HDS endpoint, live in +[`plugin.yaml`](../../src/plugins/noitom_mocap/plugin.yaml). Before installing, +set its `--host` and `--port` arguments to the Hybrid Data Server TCP endpoint. + +## Build + +The Noitom SDK is not vendored in this repository. Build the optional plugin +when you need Noitom support: + +```bash +cmake -B build -DBUILD_PLUGIN_NOITOM_MOCAP=ON +cmake --build build --target python_package noitom_mocap_plugin --parallel +cmake --install build +uv pip install --find-links=install/wheels "isaacteleop[cloudxr]" +``` + +Run `cmake --install build` again whenever `plugin.yaml` changes so the updated +configuration is copied to `install/plugins/noitom_mocap/plugin.yaml`. + +By default CMake fetches MocapApi from `https://github.com/pnmocap/MocapApi`. +For offline builds, pass `-DNOITOM_MOCAP_API_ROOT=/path/to/MocapApi`. + +## Example 1: Record And Replay + +Record uses a Noitom wrapper around `TeleopSession` so the Noitom plugin can be +launched and consumed through the `noitom_mocap` tensor collection without +changing the shared MCAP examples. Replay uses the existing generic full-body +MCAP script because the recording uses the standard `full_body` channel. + +![Noitom full-body recording](assets/record.gif) + +Record the Noitom full-body stream: + +```bash +uv run python examples/noitom/record_noitom_full_body.py \ + 10 examples/noitom/recordings/noitom_full_body.mcap +``` + +The resulting MCAP uses the standard `core.FullBodyPoseRecord` schema and +the standard `full_body` channel, so the generic replay script can play it back: + +![Noitom MCAP replay](assets/replay.gif) + +```bash +cd examples/mcap_record_replay/python +uv sync +uv run python replay_full_body.py ../../noitom/recordings/noitom_full_body.mcap +``` + +## Example 2: Teleop + +![Noitom G1 live teleoperation](assets/teleop.gif) + +Run Isaac Lab with the external task registration callback. The task launches +`noitom_mocap_plugin` through IsaacTeleop's plugin manager by default. + +```bash +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 +``` + +For advanced manual plugin control, start the plugin yourself and disable +auto-launch in the Isaac Lab terminal: + +```bash +./install/plugins/noitom_mocap/noitom_mocap_plugin + +NOITOM_MOCAP_AUTO_LAUNCH=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 you run a dedicated CloudXR runtime yourself, source its environment before +starting Isaac Lab and pass Isaac Lab's flags for using the existing runtime +instead of auto-launching another one. + +## Behavior + +Retargeting lives in `noitom_retargeting.py` and is wired by +`noitom_tasks.py`. It maps Noitom shoulder, elbow, and wrist bones into G1 Pink +IK frame targets, with optional elbow and shoulder frame tasks enabled by +default. + +Pipeline: + +```text +FullBodyPose (`body.noitom` vendor) + -> torso frame (pelvis, SPINE3, shoulders) + -> posture-based arm targets (shoulder, elbow, wrist) + -> Pink IK frame-task action [wrists, elbows, shoulders, hands, locomotion] +``` + +Calibration: + +1. After teleop reset, retargeting clears its neutral reference. +2. Hold a stable upper-body pose; the next valid frame becomes the neutral + reference. +3. Motion is applied relative to that neutral pose. + +Default settings live in `NoitomG1Settings` and +`NoitomRetargetingSettings`. When Kit visualization is enabled, the incoming +Noitom pose is shown as a cyan stick figure anchored to the robot pelvis. 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..2f6be4b73 --- /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 BodyJoint + +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(BodyJoint.LEFT_COLLAR), + int(BodyJoint.LEFT_SHOULDER), + int(BodyJoint.LEFT_ELBOW), + int(BodyJoint.LEFT_WRIST), + int(BodyJoint.LEFT_HAND), + int(BodyJoint.RIGHT_COLLAR), + int(BodyJoint.RIGHT_SHOULDER), + int(BodyJoint.RIGHT_ELBOW), + int(BodyJoint.RIGHT_WRIST), + int(BodyJoint.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 FullBodyPose frame.""" + if frame.joints is None: + return {}, None + positions: dict[int, np.ndarray] = {} + for index in range(int(BodyJoint.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(BodyJoint.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(BodyJoint.PELVIS), + spine3_index: int = int(BodyJoint.SPINE3), + left_shoulder_index: int = int(BodyJoint.LEFT_SHOULDER), + right_shoulder_index: int = int(BodyJoint.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(BodyJoint.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(BodyJoint.HEAD)) + pelvis_src = positions.get(int(BodyJoint.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(BodyJoint.SPINE1), lengths.torso_segment), + (int(BodyJoint.SPINE2), lengths.torso_segment), + (int(BodyJoint.SPINE3), lengths.torso_segment), + (int(BodyJoint.NECK), lengths.neck), + (int(BodyJoint.HEAD), lengths.head), + ) + parent_robot = anchor + parent_mocap = positions.get(int(BodyJoint.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(BodyJoint.LEFT_SHOULDER)) + right_shoulder_src = positions.get(int(BodyJoint.RIGHT_SHOULDER)) + spine3_robot = out.get(int(BodyJoint.SPINE3)) + spine3_src = positions.get(int(BodyJoint.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(BodyJoint.LEFT_SHOULDER), + int(BodyJoint.LEFT_ELBOW), + int(BodyJoint.LEFT_WRIST), + int(BodyJoint.LEFT_HAND), + ), + ( + right_shoulder_robot, + int(BodyJoint.RIGHT_SHOULDER), + int(BodyJoint.RIGHT_ELBOW), + int(BodyJoint.RIGHT_WRIST), + int(BodyJoint.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(BodyJoint.SPINE3)) + if spine3 is not None: + for collar_index, shoulder_index in ( + (int(BodyJoint.LEFT_COLLAR), int(BodyJoint.LEFT_SHOULDER)), + (int(BodyJoint.RIGHT_COLLAR), int(BodyJoint.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(BodyJoint.PELVIS), + spine3_index: int = int(BodyJoint.SPINE3), + left_shoulder_index: int = int(BodyJoint.LEFT_SHOULDER), + right_shoulder_index: int = int(BodyJoint.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: FullBodyPose -> 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..2264f46ab --- /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 ( + DeviceIOFullBodyPoseTracked, +) +from isaacteleop.schema import BodyJoint + +# 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": DeviceIOFullBodyPoseTracked()} + + 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: BodyJoint | 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, BodyJoint.PELVIS) + spine = _joint_pose(frame, BodyJoint.SPINE3) + left_shoulder = _joint_pose(frame, BodyJoint.LEFT_SHOULDER) + right_shoulder = _joint_pose(frame, BodyJoint.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: BodyJoint, + elbow_index: BodyJoint, + wrist_index: BodyJoint, +) -> _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, BodyJoint.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, + BodyJoint.LEFT_SHOULDER, + BodyJoint.LEFT_ELBOW, + BodyJoint.LEFT_WRIST, + ) + right = _parse_arm( + frame, + torso, + pelvis_world, + BodyJoint.RIGHT_SHOULDER, + BodyJoint.RIGHT_ELBOW, + BodyJoint.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(BodyJoint.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, + BodyJoint.LEFT_SHOULDER, + BodyJoint.LEFT_ELBOW, + BodyJoint.LEFT_WRIST, + BodyJoint.LEFT_HAND, + ), + ( + right, + neutral_right, + False, + BodyJoint.RIGHT_SHOULDER, + BodyJoint.RIGHT_ELBOW, + BodyJoint.RIGHT_WRIST, + BodyJoint.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(BodyJoint.SPINE3)) + if spine3 is not None: + for collar_idx, shoulder_idx in ( + (BodyJoint.LEFT_COLLAR, BodyJoint.LEFT_SHOULDER), + (BodyJoint.RIGHT_COLLAR, BodyJoint.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 = ( + BodyJoint.PELVIS, + BodyJoint.SPINE1, + BodyJoint.SPINE2, + BodyJoint.SPINE3, + BodyJoint.NECK, + BodyJoint.HEAD, + ) + segment_lengths = { + BodyJoint.SPINE1: _ROBOT_TORSO_SEGMENT_Z, + BodyJoint.SPINE2: _ROBOT_TORSO_SEGMENT_Z, + BodyJoint.SPINE3: _ROBOT_TORSO_SEGMENT_Z, + BodyJoint.NECK: _ROBOT_NECK_SEGMENT, + BodyJoint.HEAD: _ROBOT_HEAD_SEGMENT, + } + prev_robot = anchor.copy() + prev_mocap = _joint_pose(frame, BodyJoint.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( + BodyJoint.LEFT_SHOULDER if is_left else BodyJoint.RIGHT_SHOULDER + ) + elbow_index = int(BodyJoint.LEFT_ELBOW if is_left else BodyJoint.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(BodyJoint.LEFT_ELBOW if is_left else BodyJoint.RIGHT_ELBOW) + shoulder_index = int( + BodyJoint.LEFT_SHOULDER if is_left else BodyJoint.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(BodyJoint.LEFT_WRIST if is_left else BodyJoint.RIGHT_WRIST) + elbow_index = int(BodyJoint.LEFT_ELBOW if is_left else BodyJoint.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..37f0bbabd --- /dev/null +++ b/examples/noitom/noitom_tasks.py @@ -0,0 +1,1004 @@ +# 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 + +import os +from dataclasses import dataclass, field, replace +from pathlib import Path +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 ( + BodyJoint, + FullBodyPoseT, + FullBodyPoseTrackedT, +) +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPoseTracked, + IDeviceIOSource, +) +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + OutputCombiner, + RetargeterIO, + RetargeterIOType, + TensorGroup, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import DLDataType, NDArrayType +from isaacteleop.teleop_session_manager import PluginConfig + +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 +_NOITOM_PLUGIN_NAME = "noitom_mocap" +_NOITOM_PLUGIN_ROOT_ID = "noitom_mocap" +_NOITOM_VENDOR_ID = "body.noitom" + + +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 + plugin_auto_launch: bool = True + 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 = ( + (BodyJoint.PELVIS, BodyJoint.SPINE1), + (BodyJoint.SPINE1, BodyJoint.SPINE2), + (BodyJoint.SPINE2, BodyJoint.SPINE3), + (BodyJoint.SPINE3, BodyJoint.NECK), + (BodyJoint.NECK, BodyJoint.HEAD), + (BodyJoint.SPINE3, BodyJoint.LEFT_COLLAR), + (BodyJoint.LEFT_COLLAR, BodyJoint.LEFT_SHOULDER), + (BodyJoint.LEFT_SHOULDER, BodyJoint.LEFT_ELBOW), + (BodyJoint.LEFT_ELBOW, BodyJoint.LEFT_WRIST), + (BodyJoint.LEFT_WRIST, BodyJoint.LEFT_HAND), + (BodyJoint.SPINE3, BodyJoint.RIGHT_COLLAR), + (BodyJoint.RIGHT_COLLAR, BodyJoint.RIGHT_SHOULDER), + (BodyJoint.RIGHT_SHOULDER, BodyJoint.RIGHT_ELBOW), + (BodyJoint.RIGHT_ELBOW, BodyJoint.RIGHT_WRIST), + (BodyJoint.RIGHT_WRIST, BodyJoint.RIGHT_HAND), + (BodyJoint.PELVIS, BodyJoint.LEFT_HIP), + (BodyJoint.LEFT_HIP, BodyJoint.LEFT_KNEE), + (BodyJoint.LEFT_KNEE, BodyJoint.LEFT_ANKLE), + (BodyJoint.LEFT_ANKLE, BodyJoint.LEFT_FOOT), + (BodyJoint.PELVIS, BodyJoint.RIGHT_HIP), + (BodyJoint.RIGHT_HIP, BodyJoint.RIGHT_KNEE), + (BodyJoint.RIGHT_KNEE, BodyJoint.RIGHT_ANKLE), + (BodyJoint.RIGHT_ANKLE, BodyJoint.RIGHT_FOOT), +) +DEFAULT_NOITOM_G1_SETTINGS = NoitomG1Settings() + + +def _env_bool(name: str, default: bool) -> bool: + value = os.environ.get(name) + if value is None: + return default + return value.lower() not in {"0", "false", "no", "off"} + + +def _noitom_settings_from_env() -> NoitomG1Settings: + return replace( + DEFAULT_NOITOM_G1_SETTINGS, + plugin_auto_launch=_env_bool( + "NOITOM_MOCAP_AUTO_LAUNCH", + DEFAULT_NOITOM_G1_SETTINGS.plugin_auto_launch, + ), + ) + + +def _plugin_search_paths() -> list[Path]: + base = Path(__file__).resolve().parents[2] + candidates = [ + base / "plugins", + base / "install" / "plugins", + ] + return [path for path in candidates if path.exists()] + + +def _noitom_plugin_configs(settings: NoitomG1Settings) -> list[PluginConfig]: + if not settings.plugin_auto_launch: + return [] + search_paths = _plugin_search_paths() + if not search_paths: + raise RuntimeError( + "Noitom plugin directory not found. Run `cmake --install build` " + "or set NOITOM_MOCAP_AUTO_LAUNCH=0 for a manually started plugin." + ) + return [ + # Noitom plugin launch arguments live in src/plugins/noitom_mocap/plugin.yaml. + PluginConfig( + plugin_name=_NOITOM_PLUGIN_NAME, + plugin_root_id=_NOITOM_PLUGIN_ROOT_ID, + search_paths=search_paths, + ) + ] + + +# 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__() + settings = _noitom_settings_from_env() + self.isaac_teleop.pipeline_builder = lambda: ( + build_noitom_g1_locomanipulation_pipeline(settings) + ) + self.isaac_teleop.plugins = _noitom_plugin_configs(settings) + # 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=settings.use_arm_ik_frame_tasks, + ) + self.isaac_teleop.teleoperation_active_default = ( + 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.FullBodyTracker() + vendor = deviceio.TrackerVendor( + _NOITOM_VENDOR_ID, + { + "collection_id": settings.collection_id, + "max_flatbuffer_size": str(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, vendor=vendor) + + 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: DeviceIOFullBodyPoseTracked()} + + 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: FullBodyPoseTrackedT = inputs[_FULL_BODY_INPUT][0] + frame: FullBodyPoseT | 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: FullBodyPoseT, 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(BodyJoint.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( + settings: NoitomG1Settings = DEFAULT_NOITOM_G1_SETTINGS, +) -> OutputCombiner: + """Build a one-source IsaacTeleop pipeline for Noitom G1 testing.""" + source = NoitomG1ActionSource(settings=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: FullBodyPoseT, + 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(BodyJoint.LEFT_WRIST), + int(BodyJoint.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(BodyJoint.LEFT_ELBOW), + int(BodyJoint.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(BodyJoint.LEFT_SHOULDER), + int(BodyJoint.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: FullBodyPoseT, + 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: FullBodyPoseT) -> dict[int, np.ndarray]: + positions: dict[int, np.ndarray] = {} + if frame.joints is None: + return positions + for index in range(int(BodyJoint.NUM_JOINTS)): + position = _joint_position(frame, index) + if position is not None: + positions[index] = position + return positions + + +def _joint_position(frame: FullBodyPoseT, 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: FullBodyPoseT) -> int: + if frame.joints is None: + return 0 + count = 0 + for index in range(int(BodyJoint.NUM_JOINTS)): + if frame.joints.joints(index).is_valid: + count += 1 + return count + + +def _raw_full_body_status(frame: FullBodyPoseT) -> str: + left_wrist = _joint_position(frame, BodyJoint.LEFT_WRIST) + right_wrist = _joint_position(frame, BodyJoint.RIGHT_WRIST) + pelvis = _joint_position(frame, BodyJoint.PELVIS) + spine3 = _joint_position(frame, BodyJoint.SPINE3) + left_shoulder = _joint_position(frame, BodyJoint.LEFT_SHOULDER) + right_shoulder = _joint_position(frame, BodyJoint.RIGHT_SHOULDER) + left_elbow = _joint_position(frame, BodyJoint.LEFT_ELBOW) + right_elbow = _joint_position(frame, BodyJoint.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: FullBodyPoseT) -> str: + """Detailed calibration diagnostics: which joints are valid/invalid.""" + pelvis = _joint_position(frame, BodyJoint.PELVIS) + spine3 = _joint_position(frame, BodyJoint.SPINE3) + left_shoulder = _joint_position(frame, BodyJoint.LEFT_SHOULDER) + right_shoulder = _joint_position(frame, BodyJoint.RIGHT_SHOULDER) + left_elbow = _joint_position(frame, BodyJoint.LEFT_ELBOW) + right_elbow = _joint_position(frame, BodyJoint.RIGHT_ELBOW) + left_wrist = _joint_position(frame, BodyJoint.LEFT_WRIST) + right_wrist = _joint_position(frame, BodyJoint.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(BodyJoint.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..e87651af3 --- /dev/null +++ b/examples/noitom/record_noitom_full_body.py @@ -0,0 +1,161 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Record Noitom full-body samples to a standard IsaacTeleop full-body MCAP.""" + +from __future__ import annotations + +import argparse +import sys +import time +from datetime import datetime +from pathlib import Path + +import numpy as np + +from isaacteleop.cloudxr import CloudXRLauncher +from isaacteleop.deviceio import McapRecordingConfig, TrackerVendor +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + FullBodySource, +) +from isaacteleop.retargeting_engine.interface import OutputCombiner +from isaacteleop.retargeting_engine.tensor_types import FullBodyInputIndex +from isaacteleop.schema import BodyJoint +from isaacteleop.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, +) + + +DEFAULT_COLLECTION_ID = "noitom_mocap" +DEFAULT_MAX_FLATBUFFER_SIZE = 16 * 1024 +PLUGIN_NAME = "noitom_mocap" +PLUGIN_ROOT_ID = "noitom_mocap" +NOITOM_VENDOR_ID = "body.noitom" + + +def _plugin_search_paths() -> list[Path]: + base = Path(__file__).resolve().parents[2] + candidates = [ + base / "plugins", + base / "install" / "plugins", + ] + return [path for path in candidates if path.exists()] + + +def _build_pipeline(collection_id: str, max_flatbuffer_size: int) -> OutputCombiner: + controllers = ControllersSource(name="controllers") + full_body = FullBodySource( + name="full_body", + vendor=TrackerVendor( + NOITOM_VENDOR_ID, + { + "collection_id": collection_id, + "max_flatbuffer_size": str(max_flatbuffer_size), + }, + ), + ) + return OutputCombiner( + { + "controller_left": controllers.output(ControllersSource.LEFT), + "controller_right": controllers.output(ControllersSource.RIGHT), + "full_body": full_body.output(FullBodySource.FULL_BODY), + } + ) + + +def _resolve_output(path_arg: str | None) -> Path: + if path_arg: + path = Path(path_arg) + else: + out_dir = Path(__file__).resolve().parent / "recordings" + path = out_dir / f"noitom_full_body_{datetime.now():%Y%m%d_%H%M%S}.mcap" + path.parent.mkdir(parents=True, exist_ok=True) + return path + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "duration", nargs="?", type=float, default=5.0, help="Recording duration (s)" + ) + parser.add_argument("output", nargs="?", help="Output .mcap path") + parser.add_argument("--collection-id", default=DEFAULT_COLLECTION_ID) + parser.add_argument( + "--max-flatbuffer-size", type=int, default=DEFAULT_MAX_FLATBUFFER_SIZE + ) + parser.add_argument( + "--no-plugin", + action="store_true", + help="Do not auto-launch noitom_mocap_plugin; use an already running plugin.", + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args(argv[1:]) + + mcap_path = _resolve_output(args.output) + plugins: list[PluginConfig] = [] + if not args.no_plugin: + search_paths = _plugin_search_paths() + if not search_paths: + sys.exit( + "[record] error: no installed plugin directory found. " + "Run `cmake --install build` or pass `--no-plugin` for a manually " + "started plugin." + ) + plugins.append( + PluginConfig( + plugin_name=PLUGIN_NAME, + plugin_root_id=PLUGIN_ROOT_ID, + search_paths=search_paths, + ) + ) + + config = TeleopSessionConfig( + app_name="NoitomFullBodyRecordExample", + pipeline=_build_pipeline(args.collection_id, args.max_flatbuffer_size), + plugins=plugins, + mcap_config=McapRecordingConfig(str(mcap_path)), + ) + + joint_count = int(BodyJoint.NUM_JOINTS) + print(f"[record] writing {mcap_path} for {args.duration:.1f}s") + print(f"[record] collection_id={args.collection_id}") + + with CloudXRLauncher.launch_context(args) as launcher: + if launcher is not None: + print( + f"[record] CloudXR runtime started (WSS log: {launcher.wss_log_path})" + ) + with TeleopSession(config) as session: + start = time.time() + while time.time() - start < args.duration: + result = session.step() + if session.frame_count % 60 == 0: + full_body = result["full_body"] + n_valid = ( + 0 + if full_body.is_none + else int( + np.count_nonzero( + np.asarray( + full_body[FullBodyInputIndex.JOINT_VALID], + dtype=np.uint8, + ) + ) + ) + ) + print( + f"[record] t={time.time() - start:5.2f}s " + f"frame={session.frame_count} " + f"joints={n_valid:02d}/{joint_count}" + ) + time.sleep(1 / 60) + + print(f"[record] done - {mcap_path}") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp index d44125a49..c20e8fcc3 100644 --- a/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/tracker_vendor.hpp @@ -19,10 +19,9 @@ namespace core // `id` is a string (rather than an enum baked into the tracker marker) so vendor // routing stays decoupled from the tracker type. The selectable vendors are the // live factory's compile-time dispatch table (e.g. "body.pico-xr"), so adding a -// vendor still means extending that table and rebuilding core. `params` is -// reserved for vendor-specific settings as free-form strings (mirroring plugin -// CLI arguments, e.g. {"max_flatbuffer_size": "16384"}); no vendor consumes it -// yet, so a non-empty map is rejected at validation until the first consumer lands. +// vendor still means extending that table and rebuilding core. `params` carries +// vendor-specific settings as free-form strings (mirroring plugin CLI arguments, +// e.g. {"max_flatbuffer_size": "16384"}); each vendor validates the keys it supports. struct TrackerVendor { std::string id; diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index db0b26078..3679c45a5 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_noitom_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp live_oglo_tactile_tracker_impl.cpp live_tensor_push_tracker_impl.cpp @@ -26,6 +27,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_noitom_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp live_oglo_tactile_tracker_impl.hpp live_tensor_push_tracker_impl.hpp diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index ce8e557b4..a6a8ffab2 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 @@ -89,6 +89,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 FullBodyTracker* tracker); + std::unique_ptr create_full_body_tracker_noitom_impl(const FullBodyTracker* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); @@ -122,7 +123,7 @@ class LiveDeviceIOFactory * @brief Validate per-tracker vendor selections against the live vendor dispatch table. * * Rejects selections on tracker types that do not support vendors, unknown vendor ids, vendor - * ids that belong to a different tracker type, non-empty vendor params, and duplicate entries. + * ids that belong to a different tracker type, unsupported vendor params, and duplicate entries. * Throws std::invalid_argument on the first violation. List-independent: the caller checks that * each selection references a tracker it owns. * diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index 028934ef1..940e7b879 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -5,6 +5,7 @@ #include "live_controller_tracker_impl.hpp" #include "live_frame_metadata_tracker_oak_impl.hpp" +#include "live_full_body_tracker_noitom_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" #include "live_generic_3axis_pedal_tracker_impl.hpp" #include "live_hand_tracker_impl.hpp" @@ -94,6 +95,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_noitom_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_full_body_tracker_noitom_impl(typed) : nullptr; +} + std::unique_ptr try_create_generic_pedal_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -172,6 +179,8 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { make_dispatch_entry(&try_create_controller_impl), make_dispatch_entry(&try_create_message_channel_impl), make_dispatch_entry(&try_create_full_body_pico_impl, "body.pico-xr"), + make_dispatch_entry( + &try_create_full_body_noitom_impl, LiveFullBodyTrackerNoitomImpl::VENDOR_ID), make_dispatch_entry(&try_create_generic_pedal_impl), make_dispatch_entry(&try_create_tensor_push_impl), make_dispatch_entry( @@ -303,10 +312,11 @@ void validate_vendor_selections(const std::vector LiveDeviceIOFactory::create_full_body_trac return std::make_unique(handles_, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_full_body_tracker_noitom_impl(const FullBodyTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveFullBodyTrackerPicoImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + + const TrackerVendor* vendor = find_vendor(tracker); + assert(vendor && vendor->id == LiveFullBodyTrackerNoitomImpl::VENDOR_ID); + return std::make_unique(handles_, *vendor, 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_noitom_impl.cpp b/src/core/live_trackers/cpp/live_full_body_tracker_noitom_impl.cpp new file mode 100644 index 000000000..2b0a636eb --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_noitom_impl.cpp @@ -0,0 +1,94 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_full_body_tracker_noitom_impl.hpp" + +#include +#include +#include +#include +#include + +namespace core +{ + +namespace +{ + +constexpr std::string_view COLLECTION_ID_PARAM = "collection_id"; +constexpr std::string_view MAX_FLATBUFFER_SIZE_PARAM = "max_flatbuffer_size"; + +SchemaTrackerConfig make_noitom_tensor_config(const TrackerVendor& vendor) +{ + if (vendor.id != LiveFullBodyTrackerNoitomImpl::VENDOR_ID) + { + throw std::invalid_argument("Noitom full-body vendor id must be '" + + std::string(LiveFullBodyTrackerNoitomImpl::VENDOR_ID) + "'"); + } + + for (const auto& [key, value] : vendor.params) + { + (void)value; + if (key != COLLECTION_ID_PARAM && key != MAX_FLATBUFFER_SIZE_PARAM) + { + throw std::invalid_argument("Noitom full-body vendor does not support parameter '" + key + "'"); + } + } + + SchemaTrackerConfig config; + config.collection_id = std::string(LiveFullBodyTrackerNoitomImpl::DEFAULT_COLLECTION_ID); + config.max_flatbuffer_size = LiveFullBodyTrackerNoitomImpl::DEFAULT_MAX_FLATBUFFER_SIZE; + config.tensor_identifier = std::string(LiveFullBodyTrackerNoitomImpl::TENSOR_IDENTIFIER); + config.localized_name = "Noitom Full Body"; + + if (auto it = vendor.params.find(std::string(COLLECTION_ID_PARAM)); it != vendor.params.end()) + { + if (it->second.empty()) + { + throw std::invalid_argument("Noitom full-body collection_id must not be empty"); + } + config.collection_id = it->second; + } + + if (auto it = vendor.params.find(std::string(MAX_FLATBUFFER_SIZE_PARAM)); it != vendor.params.end()) + { + size_t parsed = 0; + const char* begin = it->second.data(); + const char* end = begin + it->second.size(); + const auto [ptr, error] = std::from_chars(begin, end, parsed); + if (error != std::errc{} || ptr != end || parsed == 0) + { + throw std::invalid_argument("Noitom full-body max_flatbuffer_size must be a positive integer"); + } + config.max_flatbuffer_size = parsed; + } + + return config; +} + +} // namespace + +void LiveFullBodyTrackerNoitomImpl::validate_vendor(const TrackerVendor& vendor) +{ + (void)make_noitom_tensor_config(vendor); +} + +LiveFullBodyTrackerNoitomImpl::LiveFullBodyTrackerNoitomImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + schema_reader_(handles, make_noitom_tensor_config(vendor), mcap_channels_.get(), /*mcap_channel_index=*/0) +{ +} + +void LiveFullBodyTrackerNoitomImpl::update(int64_t /*monotonic_time_ns*/) +{ + schema_reader_.update(tracked_.data); +} + +const FullBodyPoseTrackedT& LiveFullBodyTrackerNoitomImpl::get_body_pose() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_full_body_tracker_noitom_impl.hpp b/src/core/live_trackers/cpp/live_full_body_tracker_noitom_impl.hpp new file mode 100644 index 000000000..696739936 --- /dev/null +++ b/src/core/live_trackers/cpp/live_full_body_tracker_noitom_impl.hpp @@ -0,0 +1,57 @@ +// 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 "live_full_body_tracker_pico_impl.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using FullBodyNoitomSchemaTracker = SchemaTracker; + +class LiveFullBodyTrackerNoitomImpl : public IFullBodyTrackerImpl +{ +public: + static constexpr std::string_view VENDOR_ID = "body.noitom"; + static constexpr std::string_view DEFAULT_COLLECTION_ID = "noitom_mocap"; + static constexpr std::string_view TENSOR_IDENTIFIER = "full_body"; + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 16 * 1024; + + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + + static void validate_vendor(const TrackerVendor& vendor); + + LiveFullBodyTrackerNoitomImpl(const OpenXRSessionHandles& handles, + const TrackerVendor& vendor, + std::unique_ptr mcap_channels); + + LiveFullBodyTrackerNoitomImpl(const LiveFullBodyTrackerNoitomImpl&) = delete; + LiveFullBodyTrackerNoitomImpl& operator=(const LiveFullBodyTrackerNoitomImpl&) = delete; + LiveFullBodyTrackerNoitomImpl(LiveFullBodyTrackerNoitomImpl&&) = delete; + LiveFullBodyTrackerNoitomImpl& operator=(LiveFullBodyTrackerNoitomImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const FullBodyPoseTrackedT& get_body_pose() const override; + +private: + std::unique_ptr mcap_channels_; + FullBodyNoitomSchemaTracker schema_reader_; + FullBodyPoseTrackedT tracked_; +}; + +} // namespace core diff --git a/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp b/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp index c843dc56b..8053fe2b2 100644 --- a/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp +++ b/src/core/live_trackers_tests/cpp/test_vendor_validation.cpp @@ -15,7 +15,7 @@ // no-selection, path) resolves and returns extensions. // 2. rejected - a vendor selection on a non-vendored tracker type. // 3. rejected - an unknown vendor id. -// 4. rejected - non-empty vendor params (no consumer reads them yet). +// 4. accepted or rejected - vendor params according to the selected vendor's contract. // 5. rejected - a duplicate selection for the same tracker. // 6. rejected - a selection referencing a tracker absent from the list. // Plus a direct unit test of the list-independent primitive @@ -87,6 +87,22 @@ TEST_CASE("vendor validation: accepted configurations resolve extensions", "[liv core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); REQUIRE(contains(extensions, "XR_BD_body_tracking")); } + + SECTION("Noitom vendor selects tensor data extensions") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.noitom" } } }; + const auto extensions = core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors }); + REQUIRE(contains(extensions, "XR_NVX1_tensor_data")); + REQUIRE_FALSE(contains(extensions, "XR_BD_body_tracking")); + } + + SECTION("Noitom vendor accepts collection and sample-size parameters") + { + VendorList vendors{ { body.get(), core::TrackerVendor{ "body.noitom", + { { "collection_id", "custom_noitom" }, + { "max_flatbuffer_size", "32768" } } } } }; + REQUIRE_NOTHROW(core::DeviceIOSession::get_required_extensions(trackers, core::VendorConfig{ vendors })); + } } TEST_CASE("vendor validation: invalid configurations are rejected", "[live_trackers][vendor]") @@ -124,6 +140,22 @@ TEST_CASE("vendor validation: invalid configurations are rejected", "[live_track REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("params are not supported")); } + SECTION("an unsupported Noitom vendor parameter is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.noitom", { { "unsupported", "value" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("does not support parameter")); + } + + SECTION("an invalid Noitom max flatbuffer size is rejected") + { + VendorList vendors{ + { body.get(), core::TrackerVendor{ "body.noitom", { { "max_flatbuffer_size", "0" } } } }, + }; + REQUIRE_THAT(vendor_validation_error(trackers, vendors), ContainsSubstring("must be a positive integer")); + } + SECTION("a duplicate selection for the same tracker is rejected") { VendorList vendors{ diff --git a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp index f9ac92515..30ee9f457 100644 --- a/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp +++ b/src/core/plugin_manager/cpp/inc/plugin_manager/plugin_manager.hpp @@ -26,6 +26,7 @@ struct PluginInfo std::string command; std::string version; std::string working_dir; + std::vector args; std::vector devices; }; @@ -55,7 +56,7 @@ class PluginManager * @brief Start a plugin and return a RAII handle. * @param plugin_name The name of the plugin to start. * @param plugin_root_id The root ID for the plugin. - * @param plugin_args Optional arguments passed to the plugin process. + * @param plugin_args Optional arguments appended after plugin.yaml args. * @return A unique pointer to the Plugin instance (stops on destruction). */ std::unique_ptr start(const std::string& plugin_name, diff --git a/src/core/plugin_manager/cpp/plugin_manager.cpp b/src/core/plugin_manager/cpp/plugin_manager.cpp index 5849ae533..107d191ff 100644 --- a/src/core/plugin_manager/cpp/plugin_manager.cpp +++ b/src/core/plugin_manager/cpp/plugin_manager.cpp @@ -33,6 +33,13 @@ static PluginInfo parse_plugin_yaml(const std::string& path) info.command = config["command"].as(); if (config["version"]) info.version = config["version"].as(); + if (config["args"] && config["args"].IsSequence()) + { + for (const auto& arg_node : config["args"]) + { + info.args.push_back(arg_node.as()); + } + } // Parse devices list if (config["devices"] && config["devices"].IsSequence()) @@ -159,7 +166,9 @@ std::unique_ptr PluginManager::start(const std::string& plugin_name, } const auto& info = it->second; - return std::make_unique(info.command, info.working_dir, plugin_root_id, plugin_args); + std::vector args = info.args; + args.insert(args.end(), plugin_args.begin(), plugin_args.end()); + return std::make_unique(info.command, info.working_dir, plugin_root_id, args); } } // namespace core diff --git a/src/core/plugin_manager/python/plugin_manager_bindings.cpp b/src/core/plugin_manager/python/plugin_manager_bindings.cpp index a05f38e23..e252fb592 100644 --- a/src/core/plugin_manager/python/plugin_manager_bindings.cpp +++ b/src/core/plugin_manager/python/plugin_manager_bindings.cpp @@ -29,5 +29,6 @@ PYBIND11_MODULE(_plugin_manager, m) .def("query_devices", &PluginManager::query_devices, py::arg("plugin_name"), "Query available devices from a plugin") .def("start", &PluginManager::start, py::arg("plugin_name"), py::arg("plugin_root_id"), - py::arg("plugin_args") = std::vector{}, "Start a plugin and return a RAII handle."); + py::arg("plugin_args") = std::vector{}, + "Start a plugin and return a RAII handle. plugin_args are appended after plugin.yaml args."); } diff --git a/src/plugins/noitom_mocap/CMakeLists.txt b/src/plugins/noitom_mocap/CMakeLists.txt new file mode 100644 index 000000000..9c9e1f403 --- /dev/null +++ b/src/plugins/noitom_mocap/CMakeLists.txt @@ -0,0 +1,121 @@ +# 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() + +include(FetchContent) + +set(NOITOM_MOCAP_API_REPOSITORY "https://github.com/pnmocap/MocapApi.git" + CACHE STRING "Noitom MocapApi SDK repository") +set(NOITOM_MOCAP_API_TAG "a62cb6dfc33fc74631c5732bd90f7a91bebe622a" + CACHE STRING "Noitom MocapApi SDK git revision") +set(NOITOM_MOCAP_API_ROOT "" CACHE PATH "Optional local MocapApi checkout; if empty, FetchContent downloads it") + +if(NOITOM_MOCAP_API_ROOT) + if(NOT IS_DIRECTORY "${NOITOM_MOCAP_API_ROOT}") + message(FATAL_ERROR + "NOITOM_MOCAP_API_ROOT does not exist: ${NOITOM_MOCAP_API_ROOT}\n" + "Clear NOITOM_MOCAP_API_ROOT to let CMake download MocapApi, or set it to a persistent MocapApi checkout." + ) + endif() + get_filename_component(_NOITOM_MOCAP_API_SOURCE_DIR "${NOITOM_MOCAP_API_ROOT}" ABSOLUTE) +else() + FetchContent_Declare(noitom_mocap_api + GIT_REPOSITORY "${NOITOM_MOCAP_API_REPOSITORY}" + GIT_TAG "${NOITOM_MOCAP_API_TAG}" + GIT_SHALLOW FALSE + ) + FetchContent_GetProperties(noitom_mocap_api) + if(NOT noitom_mocap_api_POPULATED) + FetchContent_Populate(noitom_mocap_api) + endif() + set(_NOITOM_MOCAP_API_SOURCE_DIR "${noitom_mocap_api_SOURCE_DIR}") +endif() + +set(_NOITOM_MOCAP_PYTHON_SDK_DIR "${_NOITOM_MOCAP_API_SOURCE_DIR}/demo/demo-py/MocapApi/mocap_api") +set(_NOITOM_MOCAP_INCLUDE_CANDIDATES + "${_NOITOM_MOCAP_PYTHON_SDK_DIR}/include/MocapApi" + "${_NOITOM_MOCAP_API_SOURCE_DIR}/include" +) + +if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(aarch64|arm64)$") + set(_NOITOM_MOCAP_LIB_CANDIDATES + "${_NOITOM_MOCAP_PYTHON_SDK_DIR}/lib/arm64" + "${_NOITOM_MOCAP_API_SOURCE_DIR}/bin/linux/aarch64" + ) +else() + set(_NOITOM_MOCAP_LIB_CANDIDATES + "${_NOITOM_MOCAP_PYTHON_SDK_DIR}/lib/x86_64" + "${_NOITOM_MOCAP_API_SOURCE_DIR}/bin/linux/x64" + ) +endif() + +set(_NOITOM_MOCAP_INCLUDE_DIR "") +foreach(_NOITOM_MOCAP_INCLUDE_CANDIDATE IN LISTS _NOITOM_MOCAP_INCLUDE_CANDIDATES) + if(EXISTS "${_NOITOM_MOCAP_INCLUDE_CANDIDATE}/MocapApi.h") + set(_NOITOM_MOCAP_INCLUDE_DIR "${_NOITOM_MOCAP_INCLUDE_CANDIDATE}") + break() + endif() +endforeach() + +set(_NOITOM_MOCAP_LIB "") +foreach(_NOITOM_MOCAP_LIB_CANDIDATE IN LISTS _NOITOM_MOCAP_LIB_CANDIDATES) + if(EXISTS "${_NOITOM_MOCAP_LIB_CANDIDATE}/libMocapApi.so") + set(_NOITOM_MOCAP_LIB "${_NOITOM_MOCAP_LIB_CANDIDATE}/libMocapApi.so") + break() + endif() +endforeach() + +if(NOT _NOITOM_MOCAP_LIB) + message(FATAL_ERROR + "Noitom Mocap SDK library not found under ${_NOITOM_MOCAP_API_SOURCE_DIR}. " + "Checked: ${_NOITOM_MOCAP_LIB_CANDIDATES}" + ) +endif() +if(NOT _NOITOM_MOCAP_INCLUDE_DIR) + message(FATAL_ERROR + "Noitom Mocap SDK headers not found under ${_NOITOM_MOCAP_API_SOURCE_DIR}. " + "Checked: ${_NOITOM_MOCAP_INCLUDE_CANDIDATES}" + ) +endif() + +message(STATUS "Noitom Mocap SDK include dir: ${_NOITOM_MOCAP_INCLUDE_DIR}") +message(STATUS "Noitom Mocap SDK library: ${_NOITOM_MOCAP_LIB}") + +add_library(NoitomMocapApi::MocapApi SHARED IMPORTED) +set_target_properties(NoitomMocapApi::MocapApi PROPERTIES + IMPORTED_LOCATION "${_NOITOM_MOCAP_LIB}" + IMPORTED_NO_SONAME TRUE + INTERFACE_INCLUDE_DIRECTORIES "${_NOITOM_MOCAP_INCLUDE_DIR}" +) + +add_executable(noitom_mocap_plugin + main.cpp + noitom_mocap_plugin.cpp +) + +target_link_libraries(noitom_mocap_plugin PRIVATE + NoitomMocapApi::MocapApi + deviceio::deviceio_trackers + 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..636ec5fdf --- /dev/null +++ b/src/plugins/noitom_mocap/README.md @@ -0,0 +1,69 @@ + + +# Noitom Mocap Plugin + +Optional plugin that reads Noitom Hybrid Data Server data through MocapApi and +publishes IsaacTeleop full-body samples over OpenXR tensor data. + +The plugin converts Noitom avatar joints to the existing `FullBodyPose` +layout and publishes tensor identifier `full_body` in collection +`noitom_mocap`. Consumers use `FullBodyTracker()` with the `body.noitom` +vendor selected; vendor parameters carry the collection ID and maximum sample +size from the Noitom-specific source. + +Positions are read from the Noitom SDK in centimeters and published in meters. +MCAP timestamps use the local monotonic clock for local/common time fields and +the Noitom avatar posture timestamp for the raw device clock when the SDK +provides it. + +## Build + +The SDK is fetched from `https://github.com/pnmocap/MocapApi`; it is not checked +into this repository. + +```bash +cmake -B build -DBUILD_PLUGIN_NOITOM_MOCAP=ON +cmake --build build --target noitom_mocap_plugin --parallel +cmake --install build +``` + +For offline builds: + +```bash +cmake -B build \ + -DBUILD_PLUGIN_NOITOM_MOCAP=ON \ + -DNOITOM_MOCAP_API_ROOT=/path/to/MocapApi +``` + +## Run + +Start CloudXR/OpenXR first, keep Axis Studio or the Noitom Data Server running, +then let IsaacTeleop's plugin manager launch the plugin. Before installing, +edit [`plugin.yaml`](plugin.yaml) and set `--host` and `--port` to the Hybrid +Data Server TCP endpoint. Run `cmake --install build` again after changing the +file so the installed plugin configuration is updated. + +```bash +./install/plugins/noitom_mocap/noitom_mocap_plugin +``` + +Print frames received through DeviceIO: + +```bash +python src/plugins/noitom_mocap/tools/noitom_mocap_printer.py --duration=10 +``` + +Record through the Noitom example wrapper and replay with the shared MCAP +script: + +```bash +uv run python examples/noitom/record_noitom_full_body.py \ + 10 examples/noitom/recordings/noitom_full_body.mcap + +cd examples/mcap_record_replay/python +uv sync +uv run python replay_full_body.py ../../noitom/recordings/noitom_full_body.mcap +``` 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..6423a8e71 --- /dev/null +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp @@ -0,0 +1,622 @@ +// 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 +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace noitom_mocap +{ + +namespace +{ + +constexpr float SDK_CENTIMETERS_TO_METERS = 0.01f; +constexpr std::string_view FULL_BODY_TENSOR_IDENTIFIER = "full_body"; +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); +} + +XrPosef make_identity_xr_pose() +{ + XrPosef pose{}; + pose.orientation.w = 1.0f; + return pose; +} + +XrQuaternionf normalize_xr_quaternion(float x, float y, float z, float w) +{ + const float norm = std::sqrt(x * x + y * y + z * z + w * w); + if (norm <= 0.0f) + { + XrQuaternionf identity{}; + identity.w = 1.0f; + return identity; + } + return XrQuaternionf{ x / norm, y / norm, z / norm, w / norm }; +} + +core::BodyJointPose make_body_joint_pose(const XrPosef& pose_cm, bool is_valid) +{ + return core::BodyJointPose(core::Pose(make_point(pose_cm.position.x, pose_cm.position.y, pose_cm.position.z), + make_quaternion(pose_cm.orientation.x, pose_cm.orientation.y, + pose_cm.orientation.z, pose_cm.orientation.w)), + is_valid); +} + +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::BodyJoint_PELVIS; + if (normalized == "leftupleg" || normalized == "lefthip") + return core::BodyJoint_LEFT_HIP; + if (normalized == "rightupleg" || normalized == "righthip") + return core::BodyJoint_RIGHT_HIP; + if (normalized == "spine") + return core::BodyJoint_SPINE1; + if (normalized == "spine1") + return core::BodyJoint_SPINE2; + if (normalized == "spine2" || normalized == "chest") + return core::BodyJoint_SPINE3; + if (normalized == "leftleg" || normalized == "leftknee") + return core::BodyJoint_LEFT_KNEE; + if (normalized == "rightleg" || normalized == "rightknee") + return core::BodyJoint_RIGHT_KNEE; + if (normalized == "leftfoot" || normalized == "leftankle") + return core::BodyJoint_LEFT_ANKLE; + if (normalized == "rightfoot" || normalized == "rightankle") + return core::BodyJoint_RIGHT_ANKLE; + if (normalized == "lefttoebase" || normalized == "lefttoe") + return core::BodyJoint_LEFT_FOOT; + if (normalized == "righttoebase" || normalized == "righttoe") + return core::BodyJoint_RIGHT_FOOT; + if (normalized == "neck") + return core::BodyJoint_NECK; + if (normalized == "head") + return core::BodyJoint_HEAD; + if (normalized == "leftshoulder" || normalized == "leftcollar") + return core::BodyJoint_LEFT_COLLAR; + if (normalized == "rightshoulder" || normalized == "rightcollar") + return core::BodyJoint_RIGHT_COLLAR; + if (normalized == "leftarm" || normalized == "leftupperarm") + return core::BodyJoint_LEFT_SHOULDER; + if (normalized == "rightarm" || normalized == "rightupperarm") + return core::BodyJoint_RIGHT_SHOULDER; + if (normalized == "leftforearm" || normalized == "leftelbow") + return core::BodyJoint_LEFT_ELBOW; + if (normalized == "rightforearm" || normalized == "rightelbow") + return core::BodyJoint_RIGHT_ELBOW; + if (normalized == "lefthand" || normalized == "leftwrist") + return core::BodyJoint_LEFT_WRIST; + if (normalized == "righthand" || normalized == "rightwrist") + return core::BodyJoint_RIGHT_WRIST; + if (normalized == "lefthandend" || normalized == "lefthandtip") + return core::BodyJoint_LEFT_HAND; + if (normalized == "righthandend" || normalized == "righthandtip") + return core::BodyJoint_RIGHT_HAND; + return std::nullopt; +} + +core::BodyJointPose make_invalid_body_joint_pose() +{ + return core::BodyJointPose(core::Pose(core::Point(), core::Quaternion()), false); +} + +int64_t posture_time_to_nanoseconds(uint32_t hour, uint32_t minute, uint32_t second, uint32_t millisecond) +{ + return (((static_cast(hour) * 60 + static_cast(minute)) * 60 + static_cast(second)) * 1000 + + static_cast(millisecond)) * + 1000000LL; +} + +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 posture timestamp is unavailable; using local sample time" + << 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(settings_api_->SetSettingsBvhData(MocapApi::BvhDataType_Binary, settings_handle_), "SetSettingsBvhData"); + check_mocap(settings_api_->SetSettingsBvhTransformation(MocapApi::BvhTransformation_Enable, settings_handle_), + "SetSettingsBvhTransformation"); + 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 posture_hour = 0; + uint32_t posture_minute = 0; + uint32_t posture_second = 0; + uint32_t posture_millisecond = 0; + check_mocap(avatar_api_->GetAvatarIndex(&avatar_index, avatar_handle), "GetAvatarIndex"); + check_mocap(avatar_api_->GetAvatarPostureIndex(&posture_index, avatar_handle), "GetAvatarPostureIndex"); + const MocapApi::EMCPError posture_time_err = avatar_api_->GetAvatarPostureTime( + &posture_hour, &posture_minute, &posture_second, &posture_millisecond, avatar_handle); + if (posture_time_err == MocapApi::Error_None) + { + latest_sample_time_raw_device_clock_ns_ = + posture_time_to_nanoseconds(posture_hour, posture_minute, posture_second, posture_millisecond); + } + else if (posture_time_err == MocapApi::Error_NoneMessage) + { + warn_optional_ptp_missing_once(); + } + else + { + check_mocap(posture_time_err, "GetAvatarPostureTime"); + } + + MocapApi::MCPJointHandle_t root_joint = 0; + auto err = avatar_api_->GetAvatarRootJoint(&root_joint, avatar_handle); + if (err != MocapApi::Error_None || root_joint == 0) + { + frame_.joints.reset(); + return false; + } + + frame_.joints = std::make_shared(); + frame_.all_joint_poses_tracked = false; + std::array seen{}; + for (uint8_t i = 0; i < static_cast(core::BodyJoint_NUM_JOINTS); ++i) + { + frame_.joints->mutable_joints()->Mutate(i, make_invalid_body_joint_pose()); + } + + auto publish_joint = [&](std::string_view name, const XrPosef& world_pose, bool valid) + { + const auto slot = map_noitom_joint_name(name); + if (!slot) + { + return; + } + + const auto index = static_cast(*slot); + const core::BodyJointPose joint_pose = make_body_joint_pose(world_pose, valid); + frame_.joints->mutable_joints()->Mutate(index, joint_pose); + seen[index] = valid; + + auto backfill_endpoint = [&](core::BodyJoint endpoint) + { + const auto endpoint_index = static_cast(endpoint); + if (seen[endpoint_index]) + { + return; + } + frame_.joints->mutable_joints()->Mutate(endpoint_index, joint_pose); + seen[endpoint_index] = valid; + }; + + if (*slot == core::BodyJoint_LEFT_WRIST) + { + backfill_endpoint(core::BodyJoint_LEFT_HAND); + } + else if (*slot == core::BodyJoint_RIGHT_WRIST) + { + backfill_endpoint(core::BodyJoint_RIGHT_HAND); + } + else if (*slot == core::BodyJoint_LEFT_ANKLE) + { + backfill_endpoint(core::BodyJoint_LEFT_FOOT); + } + else if (*slot == core::BodyJoint_RIGHT_ANKLE) + { + backfill_endpoint(core::BodyJoint_RIGHT_FOOT); + } + }; + + std::function visit_joint = + [&](MocapApi::MCPJointHandle_t handle, const XrPosef& parent_pose) + { + const char* name = nullptr; + float px = 0.0f; + float py = 0.0f; + float pz = 0.0f; + float qx = 0.0f; + float qy = 0.0f; + float qz = 0.0f; + float qw = 1.0f; + + bool valid = true; + valid &= (joint_api_->GetJointName(&name, handle) == MocapApi::Error_None); + valid &= (joint_api_->GetJointLocalPosition(&px, &py, &pz, handle) == MocapApi::Error_None); + valid &= (joint_api_->GetJointLocalRotation(&qx, &qy, &qz, &qw, handle) == MocapApi::Error_None); + + XrPosef local_pose{}; + local_pose.position = XrVector3f{ px, py, pz }; + local_pose.orientation = normalize_xr_quaternion(qx, qy, qz, qw); + const XrPosef world_pose = oxr_utils::multiply_poses(parent_pose, local_pose); + publish_joint(name ? name : "", world_pose, valid); + + uint32_t child_count = 0; + MocapApi::EMCPError child_err = joint_api_->GetJointChild(nullptr, &child_count, handle); + if (child_err == MocapApi::Error_NoneChild || child_count == 0) + { + return; + } + check_mocap(child_err, "GetJointChild(count)"); + + std::vector children(child_count); + child_err = joint_api_->GetJointChild(children.data(), &child_count, handle); + if (child_err == MocapApi::Error_NoneChild) + { + return; + } + check_mocap(child_err, "GetJointChild(children)"); + children.resize(child_count); + for (auto child : children) + { + visit_joint(child, world_pose); + } + }; + + visit_joint(root_joint, make_identity_xr_pose()); + + 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::BodyJoint_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 = std::string(FULL_BODY_TENSOR_IDENTIFIER), + .localized_name = "Noitom Full Body", + .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::FullBodyPose::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..f11a22368 --- /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::FullBodyPoseT 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..fbb0c7baa --- /dev/null +++ b/src/plugins/noitom_mocap/plugin.yaml @@ -0,0 +1,18 @@ +# 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" +args: + - "--protocol=tcp" + - "--host=127.0.0.1" + - "--port=8001" + - "--collection-id=noitom_mocap" + - "--max-flatbuffer-size=16384" + - "--rate=90" +devices: + - path: "/mocap/noitom" + type: "noitom_mocap" + description: "Noitom avatar joints and PWR trackers" 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..3058a57c8 --- /dev/null +++ b/src/plugins/noitom_mocap/tools/noitom_mocap_printer.py @@ -0,0 +1,170 @@ +#!/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 BodyJoint + + +_NOITOM_VENDOR_ID = "body.noitom" + + +_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(BodyJoint.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.FullBodyTracker() + vendor_config = deviceio.VendorConfig( + [ + ( + tracker, + deviceio.TrackerVendor( + _NOITOM_VENDOR_ID, + { + "collection_id": args.collection_id, + "max_flatbuffer_size": str(args.max_flatbuffer_size), + }, + ), + ) + ] + ) + required_extensions = deviceio.DeviceIOSession.get_required_extensions( + [tracker], vendor_config + ) + + 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, vendor_config=vendor_config + ) 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())