From d98aa752ee727a11aa6700f1d87964f7990db008 Mon Sep 17 00:00:00 2001 From: maji Date: Wed, 2 Sep 2026 17:39:24 +0800 Subject: [PATCH 1/2] fix(noitom): reduce mocap latency and add retargeting support Signed-off-by: maji --- docs/source/_data/devices.yaml | 2 + docs/source/device/noitom.rst | 211 ++ docs/source/index.rst | 1 + examples/noitom/README.md | 11 +- examples/noitom/ik_config/noitom_to_g1.json | 87 + examples/noitom/noitom_retargeting.py | 1788 +++++++++++++++-- examples/noitom/noitom_tasks.py | 1017 +++++++++- .../noitom_mocap/noitom_mocap_plugin.cpp | 212 +- .../noitom_mocap/noitom_mocap_plugin.hpp | 6 +- 9 files changed, 2975 insertions(+), 360 deletions(-) create mode 100644 docs/source/device/noitom.rst create mode 100644 examples/noitom/ik_config/noitom_to_g1.json diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index 5717dfb0e9..cc0093c572 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -253,6 +253,8 @@ devices: modes: Full-body motion capture details: setup: + - label: Noitom Motion Capture Guide + doc: /device/noitom - label: Noitom Mocap Plugin url: https://github.com/NVIDIA/IsaacTeleop/tree/main/src/plugins/noitom_mocap diff --git a/docs/source/device/noitom.rst b/docs/source/device/noitom.rst new file mode 100644 index 0000000000..210ff3d46f --- /dev/null +++ b/docs/source/device/noitom.rst @@ -0,0 +1,211 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +Noitom Motion Capture +===================== + +Use a Noitom full-body motion-capture suit as an Isaac Teleop +``FullBodyTracker`` source. The optional ``noitom_mocap`` plugin reads avatar +updates from Noitom Hybrid Data Server (HDS), converts them to Isaac Teleop's +standard 24-joint ``FullBodyPose`` schema, and publishes them for recording, +replay, or G1 upper-body retargeting. + +.. contents:: On this page + :local: + :depth: 2 + +Architecture +------------ + +The integration keeps the vendor SDK inside a plugin while applications consume +the same vendor-neutral tracker interface used by other full-body sources. + +.. code-block:: text + + Noitom suit -> Axis Studio / HDS -> MocapApi TCP + -> noitom_mocap_plugin + -> OpenXR tensor collection "noitom_mocap", tensor "full_body" + -> FullBodyTracker (vendor "body.noitom") + -> recording / replay or Noitom-to-G1 retargeting + +The plugin converts Noitom positions from centimeters to meters. Consumers select +it with vendor ID ``body.noitom`` and pass the collection ID and maximum FlatBuffer +size as vendor parameters. The default collection ID is ``noitom_mocap`` and the +default maximum sample size is 16 KiB. + +Prerequisites +------------- + +You need: + +- a Noitom suit configured and calibrated in Axis Studio or another application + that provides Noitom Hybrid Data Server; +- the HDS TCP endpoint reachable from the machine running Isaac Teleop; +- an Isaac Teleop source build; and +- Isaac Lab when running the G1 retargeting example. + +The Noitom MocapApi SDK is optional and is not vendored in this repository. CMake +fetches it from `pnmocap/MocapApi `_ when the +plugin is enabled. + +Build and install +----------------- + +From the Isaac Teleop repository root, enable the optional plugin and install the +Python package and plugin manifest: + +.. code-block:: 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]" + +For an offline SDK checkout, configure with +``-DNOITOM_MOCAP_API_ROOT=/path/to/MocapApi``. + +Configure Hybrid Data Server +---------------------------- + +Start Axis Studio or the Noitom data-server application and enable its TCP data +stream. Then edit +:code-file:`src/plugins/noitom_mocap/plugin.yaml` +and set ``--host`` and ``--port`` to that HDS TCP endpoint. The checked-in values +may reflect the developer's network; they are not universal defaults. + +Run the install step again after every manifest change: + +.. code-block:: bash + + cmake --install build + +This copies the updated manifest to +``install/plugins/noitom_mocap/plugin.yaml``. The source and installed manifests +must agree with the active HDS endpoint. Keep ``--collection-id=noitom_mocap`` +unless the consuming application is configured with the same replacement ID. + +Verify the stream +----------------- + +Start the CloudXR/OpenXR runtime and HDS, then let a session launch the plugin or +start it manually: + +.. code-block:: bash + + ./install/plugins/noitom_mocap/noitom_mocap_plugin + +Print the frames received through DeviceIO from another terminal: + +.. code-block:: bash + + python src/plugins/noitom_mocap/tools/noitom_mocap_printer.py --duration=10 + +If no frames appear, verify that HDS is streaming, its protocol is TCP, the host +and port are reachable, and the installed plugin manifest contains the latest +endpoint. Also verify that the producer and consumer use the same collection ID. + +Record and replay +----------------- + +The Noitom recording wrapper launches the plugin by default and records the +standard ``full_body`` channel: + +.. code-block:: bash + + uv run python examples/noitom/record_noitom_full_body.py \ + 10 examples/noitom/recordings/noitom_full_body.mcap + +The resulting MCAP uses ``core.FullBodyPoseRecord``, so the generic full-body +replay example can read it: + +.. code-block:: bash + + cd examples/mcap_record_replay/python + uv sync + uv run python replay_full_body.py \ + ../../noitom/recordings/noitom_full_body.mcap + +Pass ``--no-plugin`` to the recording wrapper when the plugin is already running +manually. + +G1 retargeting example +---------------------- + +The Noitom example registers an external Isaac Lab task based on +``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. Set ``ISAAC_TELEOP_ROOT`` and +``ISAACLAB_ROOT`` to your checkouts, then run: + +.. code-block:: bash + + cd "$ISAACLAB_ROOT" + PYTHONPATH="$ISAAC_TELEOP_ROOT/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 + +The task launches ``noitom_mocap_plugin`` through the plugin manager. For manual +plugin control, start the installed executable first and add +``NOITOM_MOCAP_AUTO_LAUNCH=0`` to the Isaac Lab command environment. + +Retargeting and calibration +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The retargeting pipeline maps the calibration-relative torso orientation and arm +bones to G1 Pink IK frame targets. The robot root remains fixed. The torso task +drives the waist joints with zero position cost, while elbow and shoulder +position tasks can be enabled or disabled independently. + +After a teleop reset: + +1. The previous neutral reference is cleared. +2. Hold a stable upper-body pose until a valid frame is received. +3. That frame becomes the neutral reference, and subsequent motion is applied + relative to it. + +With Kit visualization enabled, the incoming Noitom pose appears as a cyan stick +figure anchored to the robot pelvis. The default mapping and IK frame settings +are in :code-file:`examples/noitom/ik_config/noitom_to_g1.json`. + +Troubleshooting +--------------- + +No plugin is found +~~~~~~~~~~~~~~~~~~ + +Run ``cmake --install build`` and confirm that +``install/plugins/noitom_mocap/`` contains both the executable and +``plugin.yaml``. + +No full-body samples arrive +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Confirm that HDS is producing an avatar stream, not only displaying a local +preview. Check the TCP endpoint in both the source and installed manifests, then +use the printer command above before starting Isaac Lab. A consumer configured +for a different collection ID cannot see the stream. + +Calibration does not complete +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Stand still in a neutral upper-body pose and confirm that pelvis, torso, +shoulder, elbow, and wrist joints are valid. The task waits for a usable frame +rather than calibrating from incomplete data. + +Motion appears delayed +~~~~~~~~~~~~~~~~~~~~~~ + +Confirm that only the intended plugin instance is connected to HDS and that the +network path is stable. Use the printer to distinguish delayed input from Isaac +Lab rendering or retargeting latency. + +See also +-------- + +- :doc:`body_tracking` for the standard full-body schema and tracker concepts. +- :code-file:`examples/noitom/README.md` for the example's source-level guide. +- :code-file:`src/plugins/noitom_mocap/README.md` for plugin implementation and + standalone run details. +- :code-file:`examples/noitom/noitom_retargeting.py` and + :code-file:`examples/noitom/noitom_tasks.py` for the G1 pipeline. diff --git a/docs/source/index.rst b/docs/source/index.rst index a16af2a939..41e47a04dd 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -64,6 +64,7 @@ Table of Contents device/add_device device/joint_space device/body_tracking + device/noitom device/haptic_feedback device/manus device/oak diff --git a/examples/noitom/README.md b/examples/noitom/README.md index 0dc48edc82..41582b5ab8 100644 --- a/examples/noitom/README.md +++ b/examples/noitom/README.md @@ -102,17 +102,18 @@ 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. +`noitom_tasks.py`. It maps the Noitom torso orientation and arm bones into G1 +Pink IK frame targets. The robot root stays fixed; the torso task has zero +position cost and drives the three waist joints from calibration-relative +yaw, roll, and pitch. Elbow and shoulder position tasks remain optional. 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] + -> relative torso orientation + posture-based arm targets + -> Pink IK frame-task action [wrists, torso, optional elbows/shoulders, hands] ``` Calibration: diff --git a/examples/noitom/ik_config/noitom_to_g1.json b/examples/noitom/ik_config/noitom_to_g1.json new file mode 100644 index 0000000000..c951788449 --- /dev/null +++ b/examples/noitom/ik_config/noitom_to_g1.json @@ -0,0 +1,87 @@ +{ + "_comment": [ + "Noitom full-body to Unitree G1 upper-body IK targets for real-time locomanipulation.", + "Coordinates use Isaac Z-up after operator_faces_robot; quaternion order is xyzw.", + "Only shoulder, elbow, and wrist mappings are included; the Noitom task fixes the legs.", + "Calibration scales upper arms by 0.8 and forearms by 0.7.", + "Wrist rotation offsets are BVH-local post-rotations, not world poses or VR", + "controller offsets. The side-specific values align Noitom hand frames with G1." + ], + "human_root_name": "PELVIS", + "robot_root_name": "pelvis", + "arm_chains": { + "left": { + "shoulder": "left_shoulder_yaw_link", + "elbow": "left_elbow_link", + "wrist": "left_wrist_yaw_link" + }, + "right": { + "shoulder": "right_shoulder_yaw_link", + "elbow": "right_elbow_link", + "wrist": "right_wrist_yaw_link" + } + }, + "human_scale_table": { + "LEFT_SHOULDER": 0.8, + "LEFT_ELBOW": 0.8, + "LEFT_WRIST": 0.7, + "RIGHT_SHOULDER": 0.8, + "RIGHT_ELBOW": 0.8, + "RIGHT_WRIST": 0.7 + }, + "arm_segment_clamps_m": { + "upper_arm_min": 0.1, + "upper_arm_max": 0.5, + "forearm_min": 0.1, + "forearm_max": 0.45 + }, + "pink_task_weights": { + "torso_position": 0.0, + "torso_rotation": 5.0, + "null_space_posture": 0.05 + }, + "ik_match_table": { + "left_shoulder_yaw_link": [ + "LEFT_SHOULDER", + 0.0, + 0.0, + [0.0, 0.0, 0.0], + [-0.2706, 0.6533, 0.2706, 0.6533] + ], + "left_elbow_link": [ + "LEFT_ELBOW", + 0.5, + 0.0, + [0.0, 0.0, 0.0], + [-0.2706, 0.6533, 0.2706, 0.6533] + ], + "left_wrist_yaw_link": [ + "LEFT_WRIST", + 5.0, + 0.75, + [0.0, 0.0, 0.0], + [0.0, 0.7071, 0.7071, 0.0] + ], + "right_shoulder_yaw_link": [ + "RIGHT_SHOULDER", + 0.0, + 0.0, + [0.0, 0.0, 0.0], + [-0.7071, 0.0, 0.7071, 0.0] + ], + "right_elbow_link": [ + "RIGHT_ELBOW", + 0.5, + 0.0, + [0.0, 0.0, 0.0], + [-0.7071, 0.0, 0.7071, 0.0] + ], + "right_wrist_yaw_link": [ + "RIGHT_WRIST", + 5.0, + 0.75, + [0.0, 0.0, 0.0], + [-0.7071, 0.0, 0.0, 0.7071] + ] + } +} diff --git a/examples/noitom/noitom_retargeting.py b/examples/noitom/noitom_retargeting.py index 52973038e8..9f9ba54f27 100644 --- a/examples/noitom/noitom_retargeting.py +++ b/examples/noitom/noitom_retargeting.py @@ -1,7 +1,7 @@ # 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. +"""Noitom full-body to G1 upper-body 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. @@ -9,7 +9,10 @@ from __future__ import annotations +import json as _json +import os as _os from dataclasses import dataclass, field +from pathlib import Path from typing import Any import numpy as np @@ -32,6 +35,7 @@ ) 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( @@ -46,6 +50,9 @@ _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) +_DEFAULT_ROBOT_PELVIS_QUAT = np.array( + [0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)], 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) @@ -55,11 +62,253 @@ _ROBOT_HEAD_SEGMENT = 0.12 _ROBOT_HAND_EXTENSION = 0.05 _DELTA_LIMIT = np.array([0.65, 0.65, 0.65], dtype=np.float64) +_WRIST_ORIENTATION_MODES = frozenset({"source", "forearm", "twist", "full"}) +DEFAULT_NOITOM_IK_CONFIG_PATH = ( + Path(__file__).resolve().parent / "ik_config" / "noitom_to_g1.json" +) + + +@dataclass(frozen=True) +class NoitomIkMatch: + """One declarative human-joint to G1-link IK target mapping.""" + + robot_link: str + human_joint: str + position_weight: float + rotation_weight: float + position_offset: np.ndarray + rotation_offset_xyzw: np.ndarray + + +@dataclass(frozen=True) +class NoitomPinkTaskWeights: + """Pink task weights that are not part of an arm-link mapping.""" + + torso_position: float + torso_rotation: float + null_space_posture: float + + +@dataclass(frozen=True) +class NoitomIkConfig: + """Validated upper-body subset of the GMR-style Noitom mapping config.""" + + human_root_name: str + robot_root_name: str + arm_chains: dict[str, dict[str, str]] + human_scale_table: dict[str, float] + arm_segment_clamps_m: dict[str, float] + ik_match_table: dict[str, NoitomIkMatch] + pink_task_weights: NoitomPinkTaskWeights + + def match(self, side: str, role: str) -> NoitomIkMatch: + return self.ik_match_table[self.arm_chains[side][role]] + + +@dataclass(frozen=True) +class _ArmBoneScales: + """Per-arm robot link lengths derived from calibration and config ratios.""" + + upper_arm: float # robot upper-arm length in metres + forearm: float # robot forearm length in metres + + +def load_noitom_ik_config(path: str | _os.PathLike) -> NoitomIkConfig: + """Load and validate a declarative Noitom-to-G1 upper-body mapping.""" + config_path = Path(path) + with config_path.open(encoding="utf-8") as config_file: + raw = _json.load(config_file) + + def require_mapping(key: str) -> dict[str, Any]: + value = raw.get(key) + if not isinstance(value, dict): + raise ValueError(f"{config_path}: {key!r} must be an object") + return value + + human_root_name = raw.get("human_root_name") + robot_root_name = raw.get("robot_root_name") + if not isinstance(human_root_name, str) or not human_root_name: + raise ValueError(f"{config_path}: 'human_root_name' must be a non-empty string") + if not isinstance(robot_root_name, str) or not robot_root_name: + raise ValueError(f"{config_path}: 'robot_root_name' must be a non-empty string") + + raw_pink_task_weights = require_mapping("pink_task_weights") + pink_task_weight_keys = { + "torso_position", + "torso_rotation", + "null_space_posture", + } + if set(raw_pink_task_weights) != pink_task_weight_keys: + raise ValueError( + f"{config_path}: pink_task_weights must contain exactly " + f"{sorted(pink_task_weight_keys)}" + ) + parsed_pink_task_weights = { + key: float(raw_pink_task_weights[key]) for key in pink_task_weight_keys + } + if any( + not np.isfinite(value) or value < 0.0 + for value in parsed_pink_task_weights.values() + ): + raise ValueError( + f"{config_path}: Pink task weights must be finite and nonnegative" + ) + pink_task_weights = NoitomPinkTaskWeights(**parsed_pink_task_weights) + + raw_chains = require_mapping("arm_chains") + arm_chains: dict[str, dict[str, str]] = {} + required_roles = {"shoulder", "elbow", "wrist"} + for side in ("left", "right"): + raw_chain = raw_chains.get(side) + if not isinstance(raw_chain, dict) or set(raw_chain) != required_roles: + raise ValueError( + f"{config_path}: arm_chains.{side} must contain exactly " + f"{sorted(required_roles)}" + ) + if any(not isinstance(link, str) or not link for link in raw_chain.values()): + raise ValueError(f"{config_path}: arm_chains.{side} links must be strings") + arm_chains[side] = dict(raw_chain) + + raw_matches = require_mapping("ik_match_table") + matches: dict[str, NoitomIkMatch] = {} + for robot_link, entry in raw_matches.items(): + if not isinstance(entry, list) or len(entry) != 5: + raise ValueError( + f"{config_path}: ik_match_table.{robot_link} must be " + "[human_joint, pos_weight, rot_weight, pos_offset, rot_offset_xyzw]" + ) + human_joint, position_weight, rotation_weight, position_offset, rot_offset = ( + entry + ) + if not isinstance(human_joint, str) or not hasattr(BodyJoint, human_joint): + raise ValueError( + f"{config_path}: unknown BodyJoint joint {human_joint!r} " + f"for {robot_link}" + ) + position_offset_array = np.asarray(position_offset, dtype=np.float64) + rotation_offset_array = np.asarray(rot_offset, dtype=np.float64) + if position_offset_array.shape != (3,): + raise ValueError( + f"{config_path}: {robot_link} position offset must have 3 values" + ) + if rotation_offset_array.shape != (4,): + raise ValueError( + f"{config_path}: {robot_link} rotation offset must have 4 values" + ) + if not np.all(np.isfinite(position_offset_array)) or not np.all( + np.isfinite(rotation_offset_array) + ): + raise ValueError(f"{config_path}: {robot_link} offsets must be finite") + if float(np.linalg.norm(rotation_offset_array)) < 1.0e-8: + raise ValueError( + f"{config_path}: {robot_link} rotation offset must be nonzero" + ) + position_weight = float(position_weight) + rotation_weight = float(rotation_weight) + if position_weight < 0.0 or rotation_weight < 0.0: + raise ValueError(f"{config_path}: {robot_link} weights must be nonnegative") + matches[robot_link] = NoitomIkMatch( + robot_link=robot_link, + human_joint=human_joint, + position_weight=position_weight, + rotation_weight=rotation_weight, + position_offset=position_offset_array, + rotation_offset_xyzw=_normalize_quat(rotation_offset_array), + ) + + referenced_links = { + link for chain in arm_chains.values() for link in chain.values() + } + if set(matches) != referenced_links: + missing = sorted(referenced_links - set(matches)) + extra = sorted(set(matches) - referenced_links) + raise ValueError( + f"{config_path}: ik_match_table must match arm_chains; " + f"missing={missing}, extra={extra}" + ) + + raw_scales = require_mapping("human_scale_table") + human_scale_table = {key: float(value) for key, value in raw_scales.items()} + segment_joint_names = { + arm_chains[side][role]: matches[arm_chains[side][role]].human_joint + for side in ("left", "right") + for role in ("elbow", "wrist") + } + missing_scales = sorted( + joint_name + for joint_name in segment_joint_names.values() + if joint_name not in human_scale_table + ) + if missing_scales: + raise ValueError( + f"{config_path}: human_scale_table is missing {missing_scales}" + ) + if any( + not np.isfinite(value) or value <= 0.0 for value in human_scale_table.values() + ): + raise ValueError( + f"{config_path}: human scale values must be finite and positive" + ) + + raw_clamps = require_mapping("arm_segment_clamps_m") + clamp_keys = { + "upper_arm_min", + "upper_arm_max", + "forearm_min", + "forearm_max", + } + if set(raw_clamps) != clamp_keys: + raise ValueError( + f"{config_path}: arm_segment_clamps_m must contain exactly {sorted(clamp_keys)}" + ) + clamps = {key: float(value) for key, value in raw_clamps.items()} + if ( + clamps["upper_arm_min"] <= 0.0 + or clamps["forearm_min"] <= 0.0 + or clamps["upper_arm_min"] >= clamps["upper_arm_max"] + or clamps["forearm_min"] >= clamps["forearm_max"] + ): + raise ValueError(f"{config_path}: arm segment clamp ranges are invalid") + + return NoitomIkConfig( + human_root_name=human_root_name, + robot_root_name=robot_root_name, + arm_chains=arm_chains, + human_scale_table=human_scale_table, + arm_segment_clamps_m=clamps, + ik_match_table=matches, + pink_task_weights=pink_task_weights, + ) + + +def _arm_bone_scales_from_config( + arm: _ArmCalibration, + config: NoitomIkConfig, + side: str, +) -> _ArmBoneScales: + """Scale measured arm segments and clamp them to configured robot limits.""" + elbow_joint = config.match(side, "elbow").human_joint + wrist_joint = config.match(side, "wrist").human_joint + upper_ratio = config.human_scale_table[elbow_joint] + forearm_ratio = config.human_scale_table[wrist_joint] + clamps = config.arm_segment_clamps_m + + raw_upper = arm.upper_arm_length * upper_ratio + raw_forearm = arm.forearm_length * forearm_ratio + + return _ArmBoneScales( + upper_arm=float( + np.clip(raw_upper, clamps["upper_arm_min"], clamps["upper_arm_max"]) + ), + forearm=float( + np.clip(raw_forearm, clamps["forearm_min"], clamps["forearm_max"]) + ), + ) @dataclass class NoitomRetargetingSettings: - """Tunable retargeting parameters for Noitom-driven G1 wrists.""" + """Tunable retargeting parameters for Noitom-driven G1 upper body.""" # Fraction of mocap pose applied relative to calibrated neutral (not link length). motion_scale: float = 0.55 @@ -73,6 +322,14 @@ class NoitomRetargetingSettings: 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 + # Scale calibration-relative torso yaw/roll/pitch sent to the G1 waist. + torso_orientation_scale: float = 1.0 + # Torso orientation smoothing alpha (0=hold last, 1=instant). + torso_rotation_smoothing: float = 0.35 + # Bounds keep the torso target inside the G1 waist's useful workspace. + torso_yaw_limit_deg: float = 120.0 + torso_roll_limit_deg: float = 24.0 + torso_pitch_limit_deg: float = 24.0 position_smoothing: float = 0.85 rotation_smoothing: float = 0.75 robot_upper_arm_length: float = 0.28 @@ -82,6 +339,9 @@ class NoitomRetargetingSettings: robot_pelvis_world: np.ndarray = field( default_factory=lambda: _DEFAULT_ROBOT_PELVIS.copy() ) + robot_pelvis_quat_xyzw: np.ndarray = field( + default_factory=lambda: _DEFAULT_ROBOT_PELVIS_QUAT.copy() + ) robot_left_shoulder_offset: np.ndarray = field( default_factory=lambda: _ROBOT_LEFT_SHOULDER_OFFSET.copy() ) @@ -100,8 +360,14 @@ class NoitomRetargetingSettings: 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 + # "source" directly tracks the robot-aligned BVH wrist axes. "forearm" + # parallel-transports the calibrated pose, "twist" also adds residual source + # roll, and "full" retains the calibration-relative 3-axis mapping. + wrist_orientation_mode: str = "source" + # Clamp the calibrated Noitom forearm-axis twist before it reaches Pink IK. + wrist_twist_limit_deg: float = 60.0 + # Limit each retarget update while changing between equivalent twist branches. + wrist_twist_max_step_deg: float = 4.0 # 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). @@ -124,6 +390,8 @@ class NoitomRetargetingSettings: track_shoulder_ik_targets: bool = True delta_limit: np.ndarray = field(default_factory=lambda: _DELTA_LIMIT.copy()) sync_nominal_at_calibration: bool = True + # Without a config, FK uses the fixed arm lengths above. + ik_config_path: str | None = None @dataclass @@ -148,16 +416,26 @@ def from_nominal(position: np.ndarray, quaternion_xyzw: np.ndarray) -> SE3Pose: @dataclass class ArmIkTargets: - """Pink IK frame targets for one update (wrist, elbow, shoulder per arm).""" + """Pink IK frame targets for one upper-body update.""" left_wrist: SE3Pose right_wrist: SE3Pose + torso: SE3Pose left_elbow: SE3Pose right_elbow: SE3Pose left_shoulder: SE3Pose right_shoulder: SE3Pose +@dataclass(frozen=True) +class _DeclarativeArmTargets: + """Shoulder, elbow, and wrist targets produced by one config-driven solve.""" + + shoulder: SE3Pose + elbow: SE3Pose + wrist: SE3Pose + + @dataclass class NoitomCalibrationView: """Read-only calibration snapshot for debug visualization alignment.""" @@ -168,6 +446,45 @@ class NoitomCalibrationView: body_height_scale: float +@dataclass(frozen=True) +class WristOrientationDiagnostics: + """One wrist's source and target rotations for low-rate diagnostics.""" + + world_quaternion_xyzw: np.ndarray + reference_quaternion_xyzw: np.ndarray + torso_quaternion_xyzw: np.ndarray + world_delta_rotvec_deg: np.ndarray + torso_delta_rotvec_deg: np.ndarray + forearm_swing_deg: float + twist_deg: float + bounded_twist_deg: float + target_quaternion_xyzw: np.ndarray + source_target_error_deg: float + aligned_raw_quaternion_xyzw: np.ndarray + semantic_quaternion_xyzw: np.ndarray + local_offset_xyzw: np.ndarray + raw_x_dot_forearm: float | None + semantic_x_dot_forearm: float | None + semantic_x_world: np.ndarray + semantic_y_world: np.ndarray + semantic_z_world: np.ndarray + + +@dataclass(frozen=True) +class WristPoseDiagnostics: + """Raw, robot-aligned raw, and anatomy-normalized BVH wrist poses.""" + + bvh_raw_isaac_world: SE3Pose + bvh_aligned_raw_world: SE3Pose + bvh_semantic_world: SE3Pose + local_offset_xyzw: np.ndarray + raw_x_dot_forearm: float | None + semantic_x_dot_forearm: float | None + semantic_x_world: np.ndarray + semantic_y_world: np.ndarray + semantic_z_world: np.ndarray + + @dataclass class _TorsoFrame: origin: np.ndarray @@ -198,10 +515,174 @@ class _CalibrationState: pelvis_world: np.ndarray nominal_left: SE3Pose nominal_right: SE3Pose + neutral_left_forearm_robot: np.ndarray + neutral_right_forearm_robot: np.ndarray nominal_left_elbow: SE3Pose nominal_right_elbow: SE3Pose nominal_left_shoulder: SE3Pose nominal_right_shoulder: SE3Pose + left_arm_bone_scales: _ArmBoneScales | None = None + right_arm_bone_scales: _ArmBoneScales | None = None + + +class NoitomArmIkTargetNode: + """Own smoothed IK targets and bounded wrist-twist state for one arm.""" + + def __init__( + self, + is_left: bool, + settings: NoitomRetargetingSettings, + nominal_wrist_pos: np.ndarray, + nominal_wrist_quat_xyzw: np.ndarray, + ) -> None: + self._is_left = is_left + # Shared reference — parameter updates propagate without extra wiring. + self._settings = settings + self._nominal_pos = np.asarray(nominal_wrist_pos, dtype=np.float64).copy() + self._nominal_quat = np.asarray( + nominal_wrist_quat_xyzw, dtype=np.float64 + ).copy() + + self._smoothed_wrist = SE3Pose.from_nominal( + self._nominal_pos, self._nominal_quat + ) + self._smoothed_elbow = self._build_default_elbow() + self._smoothed_shoulder = self._build_default_shoulder() + # Bounded wrist twist — None until calibration + first retarget frame. + self._bounded_twist_rad: float | None = None + + # ------------------------------------------------------------------ + # Read-only pose accessors + # ------------------------------------------------------------------ + + @property + def current_wrist(self) -> SE3Pose: + return self._smoothed_wrist + + @property + def current_elbow(self) -> SE3Pose: + return self._smoothed_elbow + + @property + def current_shoulder(self) -> SE3Pose: + return self._smoothed_shoulder + + @property + def bounded_twist_rad(self) -> float | None: + return self._bounded_twist_rad + + # ------------------------------------------------------------------ + # State reset helpers + # ------------------------------------------------------------------ + + def reset(self) -> None: + """Restore factory-default poses (on clear_calibration / episode reset).""" + self._smoothed_wrist = SE3Pose.from_nominal( + self._nominal_pos, self._nominal_quat + ) + self._smoothed_elbow = self._build_default_elbow() + self._smoothed_shoulder = self._build_default_shoulder() + self._bounded_twist_rad = None + + def reset_to_nominal( + self, + nominal_wrist: SE3Pose, + nominal_elbow: SE3Pose, + nominal_shoulder: SE3Pose, + ) -> None: + """Sync smoothed poses to calibration-time nominal poses and clear twist. + + Called by ``NoitomG1Retargeter.calibrate()`` after a successful + ``_CalibrationState`` is built so the arm starts from neutral on the + first live retarget frame. + """ + self._smoothed_wrist = nominal_wrist + self._smoothed_elbow = nominal_elbow + self._smoothed_shoulder = nominal_shoulder + self._bounded_twist_rad = None + + def reset_bounded_twist(self) -> None: + """Reset twist accumulator (called when calibration is cleared mid-episode).""" + self._bounded_twist_rad = None + + # ------------------------------------------------------------------ + # Bounded wrist-twist accumulator + # ------------------------------------------------------------------ + + def apply_bound_twist(self, raw_twist_rad: float) -> float: + """Choose the nearest equivalent twist angle, clamp, then slew-rate-limit. + + Mirrors the logic that was in ``NoitomG1Retargeter._bound_wrist_twist``. + The accumulator enables continuous crossing of ±π without accumulating + full turns that the G1 wrist cannot represent. + """ + previous = self._bounded_twist_rad + nearest = _unwrap_angle_near(raw_twist_rad, previous) + desired = _clamp_wrist_twist(nearest, self._settings.wrist_twist_limit_deg) + # Never retain unbounded 2*pi winding for a bounded robot joint. Noitom's + # residual can loop during fast arm motion; keeping that winding made the + # old state stick at one wrist limit long after the source returned. + if previous is not None and self._settings.wrist_twist_max_step_deg > 0.0: + max_step_rad = float(np.deg2rad(self._settings.wrist_twist_max_step_deg)) + desired = previous + float( + np.clip(desired - previous, -max_step_rad, max_step_rad) + ) + bounded = _clamp_wrist_twist(desired, self._settings.wrist_twist_limit_deg) + self._bounded_twist_rad = bounded + return bounded + + # ------------------------------------------------------------------ + # Smoothing update helpers + # ------------------------------------------------------------------ + + def update_wrist(self, target: SE3Pose) -> SE3Pose: + """Exponentially smooth wrist pose toward *target*; return result.""" + self._smoothed_wrist = _smooth_pose( + self._smoothed_wrist, + target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + return self._smoothed_wrist + + def update_elbow(self, target: SE3Pose) -> SE3Pose: + """Exponentially smooth elbow pose toward *target*; return result.""" + self._smoothed_elbow = _smooth_pose( + self._smoothed_elbow, + target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + return self._smoothed_elbow + + def update_shoulder(self, target: SE3Pose) -> SE3Pose: + """Exponentially smooth shoulder pose toward *target*; return result.""" + self._smoothed_shoulder = _smooth_pose( + self._smoothed_shoulder, + target, + self._settings.position_smoothing, + self._settings.rotation_smoothing, + ) + return self._smoothed_shoulder + + # ------------------------------------------------------------------ + # Default pose builders + # ------------------------------------------------------------------ + + def _build_default_elbow(self) -> SE3Pose: + """Elbow directly below G1 shoulder — arms-down rest posture.""" + shoulder = _shoulder_world_robot(self._settings, 0.0, self._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 + quat = _elbow_quat_for_ik(upper_dir, self._nominal_quat, self._settings) + return SE3Pose(elbow, quat) + + def _build_default_shoulder(self) -> SE3Pose: + """Shoulder at G1 shoulder origin — arms-down rest posture.""" + shoulder = _shoulder_world_robot(self._settings, 0.0, self._is_left) + upper_dir = np.array([0.0, 0.0, -1.0], dtype=np.float64) + quat = _elbow_quat_for_ik(upper_dir, self._nominal_quat, self._settings) + return SE3Pose(shoulder, quat) class NoitomG1Retargeter(BaseRetargeter): @@ -213,6 +694,21 @@ def __init__( name: str = "noitom_g1_retargeter", ) -> None: self._settings = settings or NoitomRetargetingSettings() + if self._settings.wrist_orientation_mode not in _WRIST_ORIENTATION_MODES: + raise ValueError( + "wrist_orientation_mode must be one of " + f"{sorted(_WRIST_ORIENTATION_MODES)}, got " + f"{self._settings.wrist_orientation_mode!r}" + ) + if self._settings.torso_orientation_scale < 0.0: + raise ValueError("torso_orientation_scale must be nonnegative") + torso_limits = ( + self._settings.torso_yaw_limit_deg, + self._settings.torso_roll_limit_deg, + self._settings.torso_pitch_limit_deg, + ) + if any(limit <= 0.0 for limit in torso_limits): + raise ValueError("torso orientation limits must be positive") self._nominal_left = SE3Pose.from_nominal( self._settings.nominal_left_wrist_pos, self._settings.nominal_left_wrist_quat_xyzw, @@ -223,16 +719,22 @@ def __init__( ) 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._current_torso = self._neutral_torso_target() + self._ik_config: NoitomIkConfig | None = None + if self._settings.ik_config_path is not None: + self._ik_config = load_noitom_ik_config(self._settings.ik_config_path) + self._left_arm = NoitomArmIkTargetNode( + is_left=True, + settings=self._settings, + nominal_wrist_pos=self._nominal_left.position, + nominal_wrist_quat_xyzw=self._nominal_left.quaternion_xyzw, ) - self._smoothed_right = SE3Pose.from_nominal( - self._nominal_right.position, self._nominal_right.quaternion_xyzw + self._right_arm = NoitomArmIkTargetNode( + is_left=False, + settings=self._settings, + nominal_wrist_pos=self._nominal_right.position, + nominal_wrist_quat_xyzw=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, @@ -246,6 +748,17 @@ def __init__( step_size=0.05, sync_fn=lambda v: setattr(self._settings, "motion_scale", v), ), + FloatParameter( + "torso_orientation_scale", + "Calibration-relative torso orientation amplitude.", + default_value=self._settings.torso_orientation_scale, + min_value=0.0, + max_value=1.5, + step_size=0.05, + sync_fn=lambda v: setattr( + self._settings, "torso_orientation_scale", v + ), + ), FloatParameter( "position_smoothing", "Position smoothing alpha (0=hold last, 1=instant).", @@ -319,37 +832,42 @@ def awaiting_calibration(self) -> bool: @property def current_left(self) -> SE3Pose: - return self._smoothed_left + return self._left_arm.current_wrist @property def current_right(self) -> SE3Pose: - return self._smoothed_right + return self._right_arm.current_wrist + + @property + def current_torso(self) -> SE3Pose: + return self._current_torso @property def current_left_elbow(self) -> SE3Pose: - return self._smoothed_left_elbow + return self._left_arm.current_elbow @property def current_right_elbow(self) -> SE3Pose: - return self._smoothed_right_elbow + return self._right_arm.current_elbow @property def current_left_shoulder(self) -> SE3Pose: - return self._smoothed_left_shoulder + return self._left_arm.current_shoulder @property def current_right_shoulder(self) -> SE3Pose: - return self._smoothed_right_shoulder + return self._right_arm.current_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, + left_wrist=self._left_arm.current_wrist, + right_wrist=self._right_arm.current_wrist, + torso=self._current_torso, + left_elbow=self._left_arm.current_elbow, + right_elbow=self._right_arm.current_elbow, + left_shoulder=self._left_arm.current_shoulder, + right_shoulder=self._right_arm.current_shoulder, ) @property @@ -380,48 +898,314 @@ def body_yaw_delta(self) -> float: def retargeting_settings(self) -> NoitomRetargetingSettings: return self._settings + @property + def ik_config(self) -> NoitomIkConfig | None: + """Return the validated declarative mapping, when config mode is enabled.""" + return self._ik_config + + def _wrist_local_offset(self, side: str) -> np.ndarray: + if self._ik_config is None: + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + return self._ik_config.match(side, "wrist").rotation_offset_xyzw.copy() + + @property + def calibration_bone_scales( + self, + ) -> tuple[_ArmBoneScales, _ArmBoneScales] | None: + """Return calibrated left/right robot arm link lengths when available.""" + if self._calibration is None: + return None + left = self._calibration.left_arm_bone_scales + right = self._calibration.right_arm_bone_scales + if left is None or right is None: + return None + return left, right + @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 wrist_orientation_diagnostics( + self, frame: Any + ) -> dict[str, WristOrientationDiagnostics] | None: + """Return rotations needed to distinguish world/torso mapping from IK error.""" + if self._calibration is None: + return None + parsed = _parse_upper_body(frame) + if parsed is None: + return None + torso, left, right, _pelvis_world = parsed + calib = self._calibration + aligned_positions = self.reference_skeleton_positions(frame) - 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 + def target_forearm_direction(side: str) -> np.ndarray | None: + elbow_index = int( + BodyJoint.LEFT_ELBOW if side == "left" else BodyJoint.RIGHT_ELBOW + ) + wrist_index = int( + BodyJoint.LEFT_WRIST if side == "left" else BodyJoint.RIGHT_WRIST + ) + elbow_position = aligned_positions.get(elbow_index) + wrist_position = aligned_positions.get(wrist_index) + if elbow_position is None or wrist_position is None: + return None + forearm = wrist_position - elbow_position + norm = float(np.linalg.norm(forearm)) + return None if norm < 1.0e-8 else forearm / norm + + def make_diagnostics( + side: str, + arm: _ArmCalibration, + neutral: _ArmCalibration, + target: SE3Pose, + ) -> WristOrientationDiagnostics: + wrist_world = torso.rotation * arm.wrist_rot_torso + aligned_raw = _aligned_source_wrist_rotation(torso, arm) + local_offset = self._wrist_local_offset(side) + semantic = _semantic_source_wrist_rotation(aligned_raw, local_offset) + reference = ( + semantic + if self._settings.wrist_orientation_mode == "source" + else aligned_raw + ) + forearm_direction = target_forearm_direction(side) + raw_x = aligned_raw.as_matrix()[:, 0] + semantic_basis = semantic.as_matrix() + neutral_world = calib.torso.rotation * neutral.wrist_rot_torso + world_delta = wrist_world * neutral_world.inv() + torso_delta = arm.wrist_rot_torso * neutral.wrist_rot_torso.inv() + forearm_swing = _source_forearm_swing_rotation( + arm, + neutral, + calib.torso, + ) + twist_rad = _wrist_twist_delta_rad( + torso, + arm, + calib.torso, + neutral, + ) + arm_node = self._left_arm if side == "left" else self._right_arm + bounded_twist_rad = arm_node.bounded_twist_rad + if bounded_twist_rad is None: + bounded_twist_rad = _clamp_wrist_twist( + twist_rad, self._settings.wrist_twist_limit_deg + ) + return WristOrientationDiagnostics( + world_quaternion_xyzw=_normalize_quat(wrist_world.as_quat()), + reference_quaternion_xyzw=_normalize_quat(reference.as_quat()), + torso_quaternion_xyzw=_normalize_quat(arm.wrist_rot_torso.as_quat()), + world_delta_rotvec_deg=np.rad2deg(world_delta.as_rotvec()), + torso_delta_rotvec_deg=np.rad2deg(torso_delta.as_rotvec()), + forearm_swing_deg=float(np.rad2deg(forearm_swing.magnitude())), + twist_deg=float(np.rad2deg(twist_rad)), + bounded_twist_deg=float(np.rad2deg(bounded_twist_rad)), + target_quaternion_xyzw=target.quaternion_xyzw.copy(), + source_target_error_deg=float( + np.rad2deg( + ( + reference.inv() * Rotation.from_quat(target.quaternion_xyzw) + ).magnitude() + ) + ), + aligned_raw_quaternion_xyzw=_normalize_quat(aligned_raw.as_quat()), + semantic_quaternion_xyzw=_normalize_quat(semantic.as_quat()), + local_offset_xyzw=local_offset, + raw_x_dot_forearm=( + None + if forearm_direction is None + else float(np.dot(raw_x, forearm_direction)) + ), + semantic_x_dot_forearm=( + None + if forearm_direction is None + else float(np.dot(semantic_basis[:, 0], forearm_direction)) + ), + semantic_x_world=semantic_basis[:, 0].copy(), + semantic_y_world=semantic_basis[:, 1].copy(), + semantic_z_world=semantic_basis[:, 2].copy(), + ) + + return { + "left": make_diagnostics( + "left", left, calib.left, self._left_arm.current_wrist + ), + "right": make_diagnostics( + "right", right, calib.right, self._right_arm.current_wrist + ), + } + + def wrist_pose_diagnostics( + self, frame: Any + ) -> dict[str, WristPoseDiagnostics] | None: + """Return raw and semantic-normalized wrist poses for layered diagnostics.""" + raw_poses = { + "left": _joint_pose(frame, BodyJoint.LEFT_WRIST), + "right": _joint_pose(frame, BodyJoint.RIGHT_WRIST), + } + if any(pose is None for pose in raw_poses.values()): + return None + + parsed = _parse_upper_body(frame) + if parsed is None: + return None + torso, left_arm, right_arm, _pelvis_world = parsed + aligned_positions = self.reference_skeleton_positions(frame) + joint_indices = { + "left": int(BodyJoint.LEFT_WRIST), + "right": int(BodyJoint.RIGHT_WRIST), + } + required_indices = { + *joint_indices.values(), + int(BodyJoint.LEFT_ELBOW), + int(BodyJoint.RIGHT_ELBOW), + } + if any(index not in aligned_positions for index in required_indices): + return None + + diagnostics: dict[str, WristPoseDiagnostics] = {} + for side, arm, elbow_index in ( + ("left", left_arm, int(BodyJoint.LEFT_ELBOW)), + ("right", right_arm, int(BodyJoint.RIGHT_ELBOW)), + ): + joint_index = joint_indices[side] + raw_pose = raw_poses[side] + assert raw_pose is not None + aligned_raw = _aligned_source_wrist_rotation(torso, arm) + local_offset = self._wrist_local_offset(side) + semantic = _semantic_source_wrist_rotation(aligned_raw, local_offset) + forearm = aligned_positions[joint_index] - aligned_positions[elbow_index] + forearm_norm = float(np.linalg.norm(forearm)) + forearm_direction = ( + None if forearm_norm < 1.0e-8 else forearm / forearm_norm + ) + raw_basis = aligned_raw.as_matrix() + semantic_basis = semantic.as_matrix() + diagnostics[side] = WristPoseDiagnostics( + bvh_raw_isaac_world=SE3Pose( + raw_pose.position.copy(), raw_pose.quaternion_xyzw.copy() + ), + bvh_aligned_raw_world=SE3Pose( + aligned_positions[joint_index].copy(), + _normalize_quat(aligned_raw.as_quat()), + ), + bvh_semantic_world=SE3Pose( + aligned_positions[joint_index].copy(), + _normalize_quat(semantic.as_quat()), + ), + local_offset_xyzw=local_offset, + raw_x_dot_forearm=( + None + if forearm_direction is None + else float(np.dot(raw_basis[:, 0], forearm_direction)) + ), + semantic_x_dot_forearm=( + None + if forearm_direction is None + else float(np.dot(semantic_basis[:, 0], forearm_direction)) + ), + semantic_x_world=semantic_basis[:, 0].copy(), + semantic_y_world=semantic_basis[:, 1].copy(), + semantic_z_world=semantic_basis[:, 2].copy(), + ) + return diagnostics + + def reference_wrist_frames(self, frame: Any) -> dict[str, SE3Pose]: + """Return BVH wrist axes used for viewport comparison with G1 wrists.""" + parsed = _parse_upper_body(frame) + if parsed is None: + return {} + torso, left, right, _pelvis_world = parsed + if self._calibration is None: + calib_view = None + else: + calib_view = _calibration_view_from_state(self._calibration) + positions = _aligned_skeleton_positions(frame, self._settings, calib_view) + frames: dict[str, SE3Pose] = {} + for side, arm, joint_index in ( + ("left", left, BodyJoint.LEFT_WRIST), + ("right", right, BodyJoint.RIGHT_WRIST), + ): + position = positions.get(int(joint_index)) + if position is None: + continue + aligned_raw = _aligned_source_wrist_rotation(torso, arm) + orientation = ( + _semantic_source_wrist_rotation( + aligned_raw, self._wrist_local_offset(side) + ) + if self._settings.wrist_orientation_mode == "source" + else aligned_raw + ) + frames[side] = SE3Pose( + position.copy(), _normalize_quat(orientation.as_quat()) + ) + return frames + + def reference_skeleton_positions(self, frame: Any) -> dict[int, np.ndarray]: + """Return cyan-skeleton positions with config-driven arm targets overlaid.""" + calib_view = ( + None + if self._calibration is None + else _calibration_view_from_state(self._calibration) ) - quat = _elbow_quat_for_ik(upper_dir, nominal_quat, self._settings) - return SE3Pose(shoulder, quat) + positions = _aligned_skeleton_positions(frame, self._settings, calib_view) + if self._ik_config is None or self._calibration is None: + return positions + + targets = self.current_arm_targets + for is_left, shoulder, elbow, wrist in ( + ( + True, + targets.left_shoulder.position, + targets.left_elbow.position, + targets.left_wrist.position, + ), + ( + False, + targets.right_shoulder.position, + targets.right_elbow.position, + targets.right_wrist.position, + ), + ): + shoulder_joint = ( + BodyJoint.LEFT_SHOULDER if is_left else BodyJoint.RIGHT_SHOULDER + ) + elbow_joint = BodyJoint.LEFT_ELBOW if is_left else BodyJoint.RIGHT_ELBOW + wrist_joint = BodyJoint.LEFT_WRIST if is_left else BodyJoint.RIGHT_WRIST + hand_joint = BodyJoint.LEFT_HAND if is_left else BodyJoint.RIGHT_HAND + positions[int(shoulder_joint)] = shoulder.copy() + positions[int(elbow_joint)] = elbow.copy() + positions[int(wrist_joint)] = wrist.copy() + positions[int(hand_joint)] = ( + wrist + _unit_direction(wrist - elbow) * _ROBOT_HAND_EXTENSION + ) - 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 + spine3 = positions.get(int(BodyJoint.SPINE3)) + if spine3 is not None: + for collar_joint, shoulder_joint in ( + (BodyJoint.LEFT_COLLAR, BodyJoint.LEFT_SHOULDER), + (BodyJoint.RIGHT_COLLAR, BodyJoint.RIGHT_SHOULDER), + ): + shoulder = positions.get(int(shoulder_joint)) + if shoulder is not None: + positions[int(collar_joint)] = 0.5 * (spine3 + shoulder) + return positions + + def clear_calibration(self) -> None: + self._calibration = None + self._latest_torso = None + self._current_torso = self._neutral_torso_target() + self._left_arm.reset() + self._right_arm.reset() + + def _neutral_torso_target(self) -> SE3Pose: + return SE3Pose( + self._settings.robot_pelvis_world.astype(np.float64).copy(), + _normalize_quat(self._settings.robot_pelvis_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() @@ -456,32 +1240,101 @@ def calibrate(self, frame: Any) -> bool: 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, - ) + # Config mode derives a separate target length for every arm segment. The + # config-free branch below remains the exact Phase-A numerical path. + left_bone_scales: _ArmBoneScales | None = None + right_bone_scales: _ArmBoneScales | None = None + if self._ik_config is not None: + left_bone_scales = _arm_bone_scales_from_config( + left, self._ik_config, "left" + ) + right_bone_scales = _arm_bone_scales_from_config( + right, self._ik_config, "right" + ) + calibration_view = _calibration_view_from_scales( + arm_scale, body_height_scale, pelvis_world + ) + aligned_positions = _aligned_skeleton_positions( + frame, self._settings, calibration_view + ) + left_shoulder_position = aligned_positions.get(int(BodyJoint.LEFT_SHOULDER)) + right_shoulder_position = aligned_positions.get( + int(BodyJoint.RIGHT_SHOULDER) + ) + if left_shoulder_position is None or right_shoulder_position is None: + return False + left_targets = _declarative_arm_targets( + torso=torso, + arm=left, + neutral_torso=torso, + neutral=left, + settings=self._settings, + is_left=True, + bone_scales=left_bone_scales, + config=self._ik_config, + bounded_twist_rad=0.0, + shoulder_position=left_shoulder_position, + ) + right_targets = _declarative_arm_targets( + torso=torso, + arm=right, + neutral_torso=torso, + neutral=right, + settings=self._settings, + is_left=False, + bone_scales=right_bone_scales, + config=self._ik_config, + bounded_twist_rad=0.0, + shoulder_position=right_shoulder_position, + ) + nominal_left = left_targets.wrist + nominal_right = right_targets.wrist + nominal_left_elbow = left_targets.elbow + nominal_right_elbow = right_targets.elbow + nominal_left_shoulder = left_targets.shoulder + nominal_right_shoulder = right_targets.shoulder + neutral_left_forearm_robot = _unit_direction( + nominal_left.position - nominal_left_elbow.position + ) + neutral_right_forearm_robot = _unit_direction( + nominal_right.position - nominal_right_elbow.position + ) + else: + 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 @@ -489,45 +1342,61 @@ def calibrate(self, frame: Any) -> bool: 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( + if self._settings.wrist_orientation_mode == "source": + # Config-free source mode starts from the aligned BVH frames used by + # updates and visualization. + nominal_left.quaternion_xyzw = _normalize_quat( + _aligned_source_wrist_rotation(torso, left).as_quat() + ) + nominal_right.quaternion_xyzw = _normalize_quat( + _aligned_source_wrist_rotation(torso, right).as_quat() + ) + + neutral_forearms = _neutral_robot_forearm_directions( frame, + left, + right, self._settings, arm_scale, body_height_scale, pelvis_world, ) - if shoulder_pair is not None: - nominal_left_shoulder, nominal_right_shoulder = shoulder_pair + if neutral_forearms is None: + return False + neutral_left_forearm_robot, neutral_right_forearm_robot = neutral_forearms + + nominal_left_elbow = self._left_arm._build_default_elbow() + nominal_right_elbow = self._right_arm._build_default_elbow() + 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._left_arm._build_default_shoulder() + nominal_right_shoulder = self._right_arm._build_default_shoulder() + 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, @@ -539,17 +1408,22 @@ def calibrate(self, frame: Any) -> bool: pelvis_world=pelvis_world, nominal_left=nominal_left, nominal_right=nominal_right, + neutral_left_forearm_robot=neutral_left_forearm_robot, + neutral_right_forearm_robot=neutral_right_forearm_robot, nominal_left_elbow=nominal_left_elbow, nominal_right_elbow=nominal_right_elbow, nominal_left_shoulder=nominal_left_shoulder, nominal_right_shoulder=nominal_right_shoulder, + left_arm_bone_scales=left_bone_scales, + right_arm_bone_scales=right_bone_scales, + ) + self._left_arm.reset_to_nominal( + nominal_left, nominal_left_elbow, nominal_left_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 + self._right_arm.reset_to_nominal( + nominal_right, nominal_right_elbow, nominal_right_shoulder + ) + self._current_torso = self._neutral_torso_target() return True def retarget(self, frame: Any) -> ArmIkTargets | None: @@ -563,6 +1437,75 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: torso, left, right, pelvis_world = parsed self._latest_torso = torso calib = self._calibration + torso_target = _torso_target_from_relative_motion( + torso, calib.torso, self._settings + ) + self._current_torso = _smooth_pose( + self._current_torso, + torso_target, + position_alpha=0.0, + rotation_alpha=self._settings.torso_rotation_smoothing, + ) + left_bone_scales = calib.left_arm_bone_scales + right_bone_scales = calib.right_arm_bone_scales + left_twist_rad: float | None = None + right_twist_rad: float | None = None + if self._settings.wrist_orientation_mode == "twist": + # Choose the equivalent source angle nearest the previous bounded + # target, then clamp and slew it. This crosses +/-pi continuously + # without accumulating turns that G1 cannot represent. + left_twist_rad = self._left_arm.apply_bound_twist( + _wrist_twist_delta_rad(torso, left, calib.torso, calib.left), + ) + right_twist_rad = self._right_arm.apply_bound_twist( + _wrist_twist_delta_rad(torso, right, calib.torso, calib.right), + ) + if self._ik_config is not None: + if left_bone_scales is None or right_bone_scales is None: + raise RuntimeError("declarative IK config is missing calibrated scales") + aligned_positions = _aligned_skeleton_positions( + frame, self._settings, _calibration_view_from_state(calib) + ) + left_shoulder_position = aligned_positions.get(int(BodyJoint.LEFT_SHOULDER)) + right_shoulder_position = aligned_positions.get( + int(BodyJoint.RIGHT_SHOULDER) + ) + if left_shoulder_position is None or right_shoulder_position is None: + return None + left_targets = _declarative_arm_targets( + torso=torso, + arm=left, + neutral_torso=calib.torso, + neutral=calib.left, + settings=self._settings, + is_left=True, + bone_scales=left_bone_scales, + config=self._ik_config, + bounded_twist_rad=left_twist_rad, + shoulder_position=left_shoulder_position, + ) + right_targets = _declarative_arm_targets( + torso=torso, + arm=right, + neutral_torso=calib.torso, + neutral=calib.right, + settings=self._settings, + is_left=False, + bone_scales=right_bone_scales, + config=self._ik_config, + bounded_twist_rad=right_twist_rad, + shoulder_position=right_shoulder_position, + ) + self._left_arm.update_wrist(left_targets.wrist) + self._right_arm.update_wrist(right_targets.wrist) + if self._settings.track_elbow_ik_targets: + self._left_arm.update_elbow(left_targets.elbow) + self._right_arm.update_elbow(right_targets.elbow) + if self._settings.track_shoulder_ik_targets: + self._left_arm.update_shoulder(left_targets.shoulder) + self._right_arm.update_shoulder(right_targets.shoulder) + return self.current_arm_targets + if self._settings.track_aligned_mocap_wrists: left_target = _wrist_target_from_aligned_skeleton( frame, @@ -578,6 +1521,34 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: ) if left_target is None or right_target is None: return None + left_forearm = _aligned_forearm_direction( + frame, calib, self._settings, is_left=True + ) + right_forearm = _aligned_forearm_direction( + frame, calib, self._settings, is_left=False + ) + left_target.quaternion_xyzw = _tracked_wrist_quaternion( + torso, + left, + calib.torso, + calib.left, + calib.nominal_left.quaternion_xyzw, + calib.neutral_left_forearm_robot, + left_forearm, + self._settings, + bounded_twist_rad=left_twist_rad, + ) + right_target.quaternion_xyzw = _tracked_wrist_quaternion( + torso, + right, + calib.torso, + calib.right, + calib.nominal_right.quaternion_xyzw, + calib.neutral_right_forearm_robot, + right_forearm, + self._settings, + bounded_twist_rad=right_twist_rad, + ) else: left_target = _solve_wrist_target( torso=torso, @@ -586,10 +1557,13 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: neutral=calib.left, neutral_torso=calib.torso, nominal=calib.nominal_left, + neutral_forearm_robot=calib.neutral_left_forearm_robot, calib_yaw=calib.body_yaw_isaac, arm_length_scale=calib.arm_length_scale, settings=self._settings, is_left=True, + bounded_twist_rad=left_twist_rad, + bone_scales=left_bone_scales, ) right_target = _solve_wrist_target( torso=torso, @@ -598,25 +1572,17 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: neutral=calib.right, neutral_torso=calib.torso, nominal=calib.nominal_right, + neutral_forearm_robot=calib.neutral_right_forearm_robot, calib_yaw=calib.body_yaw_isaac, arm_length_scale=calib.arm_length_scale, settings=self._settings, is_left=False, + bounded_twist_rad=right_twist_rad, + bone_scales=right_bone_scales, ) - 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 + self._left_arm.update_wrist(left_target) + self._right_arm.update_wrist(right_target) + if self._settings.track_elbow_ik_targets: if self._settings.track_aligned_mocap_wrists: left_elbow_target = _elbow_target_from_aligned_skeleton( @@ -636,6 +1602,7 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: settings=self._settings, yaw_delta=yaw_delta, is_left=True, + bone_scales=left_bone_scales, ) right_elbow_target = _solve_elbow_target( arm=right, @@ -644,23 +1611,13 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: settings=self._settings, yaw_delta=yaw_delta, is_left=False, + bone_scales=right_bone_scales, ) 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 + self._left_arm.update_elbow(left_elbow_target) + self._right_arm.update_elbow(right_elbow_target) + if self._settings.track_shoulder_ik_targets: if self._settings.track_aligned_mocap_wrists: left_shoulder_target = _shoulder_target_from_aligned_skeleton( @@ -680,6 +1637,7 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: settings=self._settings, yaw_delta=yaw_delta, is_left=True, + bone_scales=left_bone_scales, ) right_shoulder_target = _solve_shoulder_target( arm=right, @@ -688,21 +1646,13 @@ def retarget(self, frame: Any) -> ArmIkTargets | None: settings=self._settings, yaw_delta=yaw_delta, is_left=False, + bone_scales=right_bone_scales, ) 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, - ) + self._left_arm.update_shoulder(left_shoulder_target) + self._right_arm.update_shoulder(right_shoulder_target) + return self.current_arm_targets def input_spec(self) -> RetargeterIOType: @@ -738,8 +1688,8 @@ def _compute_fn( 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["left_wrist"][0] = self._left_arm.current_wrist.as_action_pose() + outputs["right_wrist"][0] = self._right_arm.current_wrist.as_action_pose() outputs["body_yaw_delta"][0] = np.float32(self.body_yaw_delta) @@ -856,6 +1806,52 @@ def _compute_torso_yaw(torso: _TorsoFrame) -> float: return float(np.arctan2(forward[1], forward[0])) +def _reference_alignment_rotation(torso: _TorsoFrame) -> Rotation: + """Yaw-align raw source frames with the cyan skeleton facing G1 world +Y.""" + return Rotation.from_euler("z", np.pi * 0.5 - _compute_torso_yaw(torso)) + + +def _torso_target_from_relative_motion( + torso: _TorsoFrame, + neutral_torso: _TorsoFrame, + settings: NoitomRetargetingSettings, +) -> SE3Pose: + """Map calibration-relative torso orientation into the fixed G1 pelvis frame.""" + relative = neutral_torso.rotation.inv() * torso.rotation + + yaw_roll_pitch = relative.as_euler("ZXY") * settings.torso_orientation_scale + limits = np.deg2rad( + [ + settings.torso_yaw_limit_deg, + settings.torso_roll_limit_deg, + settings.torso_pitch_limit_deg, + ] + ) + bounded = np.clip(yaw_roll_pitch, -limits, limits) + pelvis_world_rotation = Rotation.from_quat( + _normalize_quat(settings.robot_pelvis_quat_xyzw) + ) + quaternion = _normalize_quat( + (pelvis_world_rotation * Rotation.from_euler("ZXY", bounded)).as_quat() + ) + return SE3Pose(settings.robot_pelvis_world.astype(np.float64).copy(), quaternion) + + +def _aligned_source_wrist_rotation( + torso: _TorsoFrame, arm: _ArmCalibration +) -> Rotation: + """Place one BVH wrist world frame in the cyan skeleton's robot alignment.""" + wrist_world = torso.rotation * arm.wrist_rot_torso + return _reference_alignment_rotation(torso) * wrist_world + + +def _semantic_source_wrist_rotation( + aligned_raw: Rotation, local_offset_xyzw: np.ndarray +) -> Rotation: + """Apply one normalized BVH-local post-rotation to an aligned raw wrist.""" + return aligned_raw * Rotation.from_quat(_normalize_quat(local_offset_xyzw)) + + 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) @@ -1011,6 +2007,123 @@ def _unit_direction( return vec / norm +def _interpolate_shortest_arc_direction( + neutral_direction: np.ndarray, + current_direction: np.ndarray, + amount: float, +) -> np.ndarray: + """Apply a fraction of the shortest neutral-to-current bone rotation.""" + neutral = _unit_direction(neutral_direction) + current = _unit_direction(current_direction, fallback=neutral) + delta = _shortest_arc_rotation(neutral, current) + fraction = float(np.clip(amount, 0.0, 1.0)) + return _unit_direction( + Rotation.from_rotvec(delta.as_rotvec() * fraction).apply(neutral), + fallback=neutral, + ) + + +def _pose_with_mapping_offset( + position: np.ndarray, + quaternion_xyzw: np.ndarray, + mapping: NoitomIkMatch, +) -> SE3Pose: + """Apply a GMR-style local position offset to one configured target pose.""" + rotation = Rotation.from_quat(_normalize_quat(quaternion_xyzw)) + target_position = np.asarray(position, dtype=np.float64) + rotation.apply( + mapping.position_offset + ) + return SE3Pose(target_position, _normalize_quat(rotation.as_quat())) + + +def _declarative_arm_targets( + *, + torso: _TorsoFrame, + arm: _ArmCalibration, + neutral_torso: _TorsoFrame, + neutral: _ArmCalibration, + settings: NoitomRetargetingSettings, + is_left: bool, + bone_scales: _ArmBoneScales, + config: NoitomIkConfig, + bounded_twist_rad: float | None, + shoulder_position: np.ndarray, +) -> _DeclarativeArmTargets: + """Solve one arm from the validated mapping table and calibrated bone vectors. + + The position solve is deliberately generic: rotate each neutral human bone + direction along its shortest arc toward the current direction, apply the + configured motion fraction, then rebuild the chain with the per-segment scale + table. Pink consumes the configured target weights downstream. + """ + side = "left" if is_left else "right" + yaw_delta = _resolve_yaw_delta( + _compute_torso_yaw(torso) - _compute_torso_yaw(neutral_torso), settings + ) + neutral_alignment = _reference_alignment_rotation(neutral_torso) + current_alignment = _reference_alignment_rotation(torso) + yaw_rotation = Rotation.from_euler("z", yaw_delta) + neutral_upper = neutral_alignment.apply( + neutral.elbow_world - neutral.shoulder_world + ) + current_upper = yaw_rotation.apply( + current_alignment.apply(arm.elbow_world - arm.shoulder_world) + ) + neutral_forearm = neutral_alignment.apply(neutral.wrist_world - neutral.elbow_world) + current_forearm = yaw_rotation.apply( + current_alignment.apply(arm.wrist_world - arm.elbow_world) + ) + upper_direction = _interpolate_shortest_arc_direction( + neutral_upper, current_upper, settings.motion_scale + ) + forearm_direction = _interpolate_shortest_arc_direction( + neutral_forearm, current_forearm, settings.motion_scale + ) + + shoulder_mapping = config.match(side, "shoulder") + elbow_mapping = config.match(side, "elbow") + wrist_mapping = config.match(side, "wrist") + shoulder_position = np.asarray(shoulder_position, dtype=np.float64) + elbow_position = shoulder_position + upper_direction * bone_scales.upper_arm + wrist_position = elbow_position + forearm_direction * bone_scales.forearm + + shoulder_pose = _pose_with_mapping_offset( + shoulder_position, + shoulder_mapping.rotation_offset_xyzw, + shoulder_mapping, + ) + elbow_pose = _pose_with_mapping_offset( + elbow_position, + elbow_mapping.rotation_offset_xyzw, + elbow_mapping, + ) + # The mapping offset normalizes BVH anatomy only for direct source tracking. + # Compatibility modes used identity here before the semantic-frame correction. + compatibility_nominal = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + wrist_quaternion = _tracked_wrist_quaternion( + torso, + arm, + neutral_torso, + neutral, + compatibility_nominal, + neutral_forearm, + forearm_direction, + settings, + bounded_twist_rad=bounded_twist_rad, + source_local_offset_xyzw=wrist_mapping.rotation_offset_xyzw, + ) + wrist_pose = _pose_with_mapping_offset( + wrist_position, + wrist_quaternion, + wrist_mapping, + ) + return _DeclarativeArmTargets( + shoulder=shoulder_pose, + elbow=elbow_pose, + wrist=wrist_pose, + ) + + 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( @@ -1048,11 +2161,16 @@ def _forearm_dir_from_elbow_angle( def _human_robot_reach_scale( - arm: _ArmCalibration, settings: NoitomRetargetingSettings + arm: _ArmCalibration, + settings: NoitomRetargetingSettings, + bone_scales: _ArmBoneScales | None = None, ) -> float: - """Shrink motion when the operator arm is longer than the fixed G1 chain.""" + """Shrink motion when the operator arm is longer than the active robot chain.""" human_reach = arm.upper_arm_length + arm.forearm_length - robot_reach = settings.robot_upper_arm_length + settings.robot_forearm_length + if bone_scales is not None: + robot_reach = bone_scales.upper_arm + bone_scales.forearm + else: + robot_reach = settings.robot_upper_arm_length + settings.robot_forearm_length if human_reach <= robot_reach + 1e-4: return 1.0 return float( @@ -1097,9 +2215,15 @@ def _arm_chain_from_directions( upper_dir: np.ndarray, forearm_dir: np.ndarray, settings: NoitomRetargetingSettings, + bone_scales: _ArmBoneScales | None = None, ) -> tuple[np.ndarray, np.ndarray]: - upper_len = settings.robot_upper_arm_length - forearm_len = settings.robot_forearm_length + """Run arm FK along unit bone directions using active robot link lengths.""" + if bone_scales is not None: + upper_len = bone_scales.upper_arm + forearm_len = bone_scales.forearm + else: + 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) @@ -1116,8 +2240,9 @@ def _arm_fk_robot_blended( settings: NoitomRetargetingSettings, yaw_delta: float, is_left: bool, + bone_scales: _ArmBoneScales | None = None, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Blend mocap posture (bone directions + elbow angle), then FK with G1 link lengths.""" + """Blend mocap posture, then run FK with the active arm 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, @@ -1148,7 +2273,7 @@ def _arm_fk_robot_blended( 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) + scale *= _human_robot_reach_scale(arm, settings, bone_scales=bone_scales) upper_d = _slerp_unit_direction(upper_n, upper_f, scale) theta_n = _elbow_interior_angle(upper_n, fore_n) @@ -1165,19 +2290,27 @@ def _arm_fk_robot_blended( fore_d = _bend_forearm_direction(upper_d, fore_d) elbow_neutral, wrist_neutral = _arm_chain_from_directions( - shoulder_robot, upper_n, fore_n, settings + shoulder_robot, upper_n, fore_n, settings, bone_scales=bone_scales ) elbow_full, wrist_full = _arm_chain_from_directions( - shoulder_robot, upper_d, fore_d, settings + shoulder_robot, upper_d, fore_d, settings, bone_scales=bone_scales ) 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, - ) + if bone_scales is not None: + wrist_robot = _clamp_reach( + shoulder_robot, + wrist_robot, + bone_scales.upper_arm, + bone_scales.forearm, + ) + else: + wrist_robot = _clamp_reach( + shoulder_robot, + wrist_robot, + settings.robot_upper_arm_length, + settings.robot_forearm_length, + ) return shoulder_robot, elbow_robot, wrist_robot @@ -1251,6 +2384,141 @@ def _wrist_quat_for_ik( ) +def _tracked_wrist_quaternion( + torso: _TorsoFrame, + arm: _ArmCalibration, + neutral_torso: _TorsoFrame, + neutral: _ArmCalibration, + nominal_quat: np.ndarray, + neutral_target_forearm_dir: np.ndarray, + target_forearm_dir: np.ndarray, + settings: NoitomRetargetingSettings, + bounded_twist_rad: float | None = None, + source_local_offset_xyzw: np.ndarray | None = None, +) -> np.ndarray: + """Parallel-transport neutral orientation, then add residual source roll.""" + if settings.wrist_orientation_mode == "source": + local_offset = ( + np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + if source_local_offset_xyzw is None + else _normalize_quat(source_local_offset_xyzw) + ) + target_rot = _semantic_source_wrist_rotation( + _aligned_source_wrist_rotation(torso, arm), local_offset + ) + return _normalize_quat(target_rot.as_quat()) + + wrist_world_rot = torso.rotation * arm.wrist_rot_torso + wrist_neutral_world_rot = neutral_torso.rotation * neutral.wrist_rot_torso + nominal_rot = Rotation.from_quat(nominal_quat) + if settings.wrist_orientation_mode == "full": + delta_rot_world = wrist_world_rot * wrist_neutral_world_rot.inv() + target_rot = delta_rot_world * nominal_rot + return _normalize_quat(target_rot.as_quat()) + + # Do not rebuild a forearm frame from world-up or blend the calibrated + # nominal again: arms-down makes that frame singular and repeated blending + # creates a target jump even when the source pose is unchanged. + target_swing = _shortest_arc_rotation( + neutral_target_forearm_dir, + target_forearm_dir, + fallback_axis=nominal_rot.apply(np.array([1.0, 0.0, 0.0])), + ) + target_rot = target_swing * nominal_rot + if settings.wrist_orientation_mode == "twist": + twist_rad = bounded_twist_rad + if twist_rad is None: + twist_rad = _wrist_twist_delta_rad(torso, arm, neutral_torso, neutral) + twist_rad = _clamp_wrist_twist(twist_rad, settings.wrist_twist_limit_deg) + target_axis = _unit_direction(target_forearm_dir) + target_rot = Rotation.from_rotvec(target_axis * twist_rad) * target_rot + return _normalize_quat(target_rot.as_quat()) + + +def _shortest_arc_rotation( + from_direction: np.ndarray, + to_direction: np.ndarray, + fallback_axis: np.ndarray | None = None, +) -> Rotation: + """Return the minimum rotation mapping one direction onto another.""" + source = _unit_direction(from_direction) + target = _unit_direction(to_direction) + dot = float(np.clip(np.dot(source, target), -1.0, 1.0)) + if dot > 1.0 - 1e-10: + return Rotation.identity() + if dot < -1.0 + 1e-8: + axis_hint = ( + np.asarray(fallback_axis, dtype=np.float64) + if fallback_axis is not None + else np.array([1.0, 0.0, 0.0], dtype=np.float64) + ) + axis = axis_hint - source * float(np.dot(axis_hint, source)) + if float(np.linalg.norm(axis)) < 1e-6: + alternate = np.array([0.0, 1.0, 0.0], dtype=np.float64) + axis = alternate - source * float(np.dot(alternate, source)) + return Rotation.from_rotvec(_unit_direction(axis) * np.pi) + quaternion_xyzw = np.concatenate([np.cross(source, target), [1.0 + dot]]) + return Rotation.from_quat(_normalize_quat(quaternion_xyzw)) + + +def _signed_twist_rad(rotation: Rotation, axis: np.ndarray) -> float: + """Extract the shortest signed quaternion twist about one unit axis.""" + quat = _normalize_quat(rotation.as_quat()) + if quat[3] < 0.0: + quat = -quat + unit_axis = _unit_direction(axis) + projected = float(np.dot(quat[:3], unit_axis)) + angle = 2.0 * float(np.arctan2(projected, quat[3])) + return float((angle + np.pi) % (2.0 * np.pi) - np.pi) + + +def _unwrap_angle_near(angle_rad: float, reference_rad: float | None) -> float: + """Choose the angle's 2*pi-equivalent value nearest the previous sample.""" + if reference_rad is None: + return float(angle_rad) + delta = (angle_rad - reference_rad + np.pi) % (2.0 * np.pi) - np.pi + return float(reference_rad + delta) + + +def _wrist_twist_delta_rad( + torso: _TorsoFrame, + arm: _ArmCalibration, + neutral_torso: _TorsoFrame, + neutral: _ArmCalibration, +) -> float: + """Remove calibrated forearm swing, leaving pronation/supination residual.""" + wrist_world = torso.rotation * arm.wrist_rot_torso + neutral_wrist_world = neutral_torso.rotation * neutral.wrist_rot_torso + current_forearm = _unit_direction(arm.wrist_world - arm.elbow_world) + forearm_swing = _source_forearm_swing_rotation( + arm, + neutral, + neutral_torso, + ) + no_twist_prediction = forearm_swing * neutral_wrist_world + residual = wrist_world * no_twist_prediction.inv() + return _signed_twist_rad(residual, current_forearm) + + +def _source_forearm_swing_rotation( + arm: _ArmCalibration, + neutral: _ArmCalibration, + neutral_torso: _TorsoFrame, +) -> Rotation: + neutral_forearm = neutral.wrist_world - neutral.elbow_world + current_forearm = arm.wrist_world - arm.elbow_world + return _shortest_arc_rotation( + neutral_forearm, + current_forearm, + fallback_axis=neutral_torso.rotation.as_matrix()[:, 0], + ) + + +def _clamp_wrist_twist(twist_rad: float, limit_deg: float) -> float: + limit_rad = float(np.deg2rad(max(0.0, limit_deg))) + return float(np.clip(twist_rad, -limit_rad, limit_rad)) + + def compute_robot_reference_positions( frame: Any, settings: NoitomRetargetingSettings, @@ -1454,7 +2722,7 @@ def _calibration_view_from_scales( def _aligned_skeleton_positions( frame: Any, settings: NoitomRetargetingSettings, - calib_view: NoitomCalibrationView, + calib_view: NoitomCalibrationView | None, ) -> dict[int, np.ndarray]: from noitom_reference_draw import ( ReferenceSkeletonLengths, @@ -1474,6 +2742,57 @@ def _aligned_skeleton_positions( ) +def _forearm_direction_from_positions( + positions: dict[int, np.ndarray], is_left: bool +) -> np.ndarray | None: + elbow_index = int(BodyJoint.LEFT_ELBOW if is_left else BodyJoint.RIGHT_ELBOW) + wrist_index = int(BodyJoint.LEFT_WRIST if is_left else BodyJoint.RIGHT_WRIST) + elbow = positions.get(elbow_index) + wrist = positions.get(wrist_index) + if elbow is None or wrist is None: + return None + forearm = wrist - elbow + if float(np.linalg.norm(forearm)) < 1e-6: + return None + return _unit_direction(forearm) + + +def _neutral_robot_forearm_directions( + frame: Any, + left: _ArmCalibration, + right: _ArmCalibration, + settings: NoitomRetargetingSettings, + arm_length_scale: float, + body_height_scale: float, + pelvis_world: np.ndarray, +) -> tuple[np.ndarray, np.ndarray] | None: + if settings.track_aligned_mocap_wrists: + calib_view = _calibration_view_from_scales( + arm_length_scale, + body_height_scale, + pelvis_world, + ) + positions = _aligned_skeleton_positions(frame, settings, calib_view) + left_direction = _forearm_direction_from_positions(positions, is_left=True) + right_direction = _forearm_direction_from_positions(positions, is_left=False) + if left_direction is None or right_direction is None: + return None + return left_direction, right_direction + + return ( + _map_mocap_direction_to_robot( + left.wrist_world - left.elbow_world, + 0.0, + settings.operator_faces_robot, + ), + _map_mocap_direction_to_robot( + right.wrist_world - right.elbow_world, + 0.0, + settings.operator_faces_robot, + ), + ) + + def _shoulder_se3_from_aligned_positions( positions: dict[int, np.ndarray], is_left: bool, @@ -1543,6 +2862,8 @@ def _wrist_se3_from_aligned_positions( is_left: bool, nominal: SE3Pose, settings: NoitomRetargetingSettings, + *, + derive_orientation: bool = True, ) -> 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) @@ -1550,7 +2871,7 @@ def _wrist_se3_from_aligned_positions( if wrist is None: return None elbow = positions.get(elbow_index) - if elbow is not None: + if derive_orientation and elbow is not None: forearm = wrist - elbow quat = _wrist_quat_for_ik( forearm, @@ -1667,9 +2988,10 @@ def _solve_shoulder_target( settings: NoitomRetargetingSettings, yaw_delta: float, is_left: bool, + bone_scales: _ArmBoneScales | None = None, ) -> SE3Pose: shoulder_robot, _elbow_robot, _wrist_robot = _arm_fk_robot_blended( - arm, neutral, settings, yaw_delta, is_left + arm, neutral, settings, yaw_delta, is_left, bone_scales=bone_scales ) upper_arm = _elbow_robot - shoulder_robot upper_norm = float(np.linalg.norm(upper_arm)) @@ -1706,9 +3028,10 @@ def _solve_elbow_target( settings: NoitomRetargetingSettings, yaw_delta: float, is_left: bool, + bone_scales: _ArmBoneScales | None = None, ) -> SE3Pose: _shoulder_robot, elbow_robot, _wrist_robot = _arm_fk_robot_blended( - arm, neutral, settings, yaw_delta, is_left + arm, neutral, settings, yaw_delta, is_left, bone_scales=bone_scales ) upper_arm = elbow_robot - _shoulder_world_robot(settings, yaw_delta, is_left) upper_norm = float(np.linalg.norm(upper_arm)) @@ -1735,7 +3058,28 @@ def _wrist_target_from_aligned_skeleton( 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) + return _wrist_se3_from_aligned_positions( + positions, + is_left, + nominal, + settings, + derive_orientation=False, + ) + + +def _aligned_forearm_direction( + frame: Any, + calib: _CalibrationState, + settings: NoitomRetargetingSettings, + is_left: bool, +) -> np.ndarray: + positions = _aligned_skeleton_positions( + frame, settings, _calibration_view_from_state(calib) + ) + direction = _forearm_direction_from_positions(positions, is_left) + if direction is None: + return np.array([0.0, 1.0, 0.0], dtype=np.float64) + return direction def _solve_wrist_target( @@ -1745,16 +3089,20 @@ def _solve_wrist_target( neutral: _ArmCalibration, neutral_torso: _TorsoFrame, nominal: SE3Pose, + neutral_forearm_robot: np.ndarray, calib_yaw: float, arm_length_scale: float, settings: NoitomRetargetingSettings, is_left: bool, + bounded_twist_rad: float | None = None, + bone_scales: _ArmBoneScales | None = None, ) -> SE3Pose: + """Compute one wrist IK target from a mocap frame.""" 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 + arm, neutral, settings, yaw_delta, is_left, bone_scales=bone_scales ) forearm = wrist_robot - elbow_robot forearm_norm = float(np.linalg.norm(forearm)) @@ -1766,22 +3114,20 @@ def _solve_wrist_target( 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, - ) + quat = _tracked_wrist_quaternion( + torso, + arm, + neutral_torso, + neutral, + nominal.quaternion_xyzw, + neutral_forearm_robot, + forearm_dir, + settings, + bounded_twist_rad=bounded_twist_rad, + ) return SE3Pose(wrist_robot, quat) + # Non-posture path: scale raw mocap position into robot frame. 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) @@ -1792,23 +3138,31 @@ def _solve_wrist_target( yaw_delta, settings.operator_faces_robot, ) - clamped = _clamp_reach( - off_shoulder, - off_wrist, - settings.robot_upper_arm_length, - settings.robot_forearm_length, - ) + if bone_scales is not None: + upper_len = bone_scales.upper_arm + forearm_len = bone_scales.forearm + else: + upper_len = settings.robot_upper_arm_length + forearm_len = settings.robot_forearm_length + clamped = _clamp_reach(off_shoulder, off_wrist, upper_len, forearm_len) 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) + forearm_dir = _map_mocap_direction_to_robot( + arm.wrist_world - arm.elbow_world, + yaw_delta, + settings.operator_faces_robot, + ) + quat = _tracked_wrist_quaternion( + torso, + arm, + neutral_torso, + neutral, + nominal.quaternion_xyzw, + neutral_forearm_robot, + forearm_dir, + settings, + bounded_twist_rad=bounded_twist_rad, + ) return SE3Pose(target_pos, quat) @@ -1839,11 +3193,19 @@ def _smooth_pose( __all__ = [ "ArmIkTargets", + "DEFAULT_NOITOM_IK_CONFIG_PATH", + "NoitomArmIkTargetNode", "NoitomCalibrationView", "NoitomG1Retargeter", + "NoitomIkConfig", + "NoitomIkMatch", + "NoitomPinkTaskWeights", "NoitomRetargetingSettings", + "WristOrientationDiagnostics", + "WristPoseDiagnostics", "SE3Pose", "compute_robot_reference_positions", + "load_noitom_ik_config", "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 index 7820f90c0f..0fe4c6935e 100644 --- a/examples/noitom/noitom_tasks.py +++ b/examples/noitom/noitom_tasks.py @@ -6,6 +6,7 @@ from __future__ import annotations import os +import time from dataclasses import dataclass, field, replace from pathlib import Path from typing import Any @@ -13,6 +14,7 @@ import gymnasium as gym import numpy as np from gymnasium.envs.registration import registry +from scipy.spatial.transform import Rotation from isaaclab.utils.configclass import configclass from isaaclab_tasks.manager_based.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( @@ -39,8 +41,10 @@ from noitom_retargeting import ( ArmIkTargets, + DEFAULT_NOITOM_IK_CONFIG_PATH, NoitomG1Retargeter, NoitomRetargetingSettings, + load_noitom_ik_config, noitom_position_to_isaac, ) from noitom_reference_draw import ( @@ -52,9 +56,10 @@ _FULL_BODY_INPUT = "deviceio_full_body" _ACTION_OUTPUT = "action" -_G1_ACTION_DIM_WRIST_ONLY = 32 -_G1_ACTION_DIM_WITH_ARM_IK = 60 +_G1_ACTION_DIM_WRIST_TORSO = 35 +_G1_ACTION_DIM_WITH_ARM_IK = 63 _PINK_PELVIS_LINK = "g1_29dof_with_hand_rev_1_0_pelvis" +_PINK_TORSO_LINK = "torso_link" _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" @@ -62,8 +67,6 @@ _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 @@ -76,6 +79,11 @@ _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 +_FRAME_AXIS_COLORS = ( + (1.0, 0.1, 0.1, 1.0), + (0.1, 1.0, 0.1, 1.0), + (0.1, 0.35, 1.0, 1.0), +) _NOITOM_PLUGIN_NAME = "noitom_mocap" _NOITOM_PLUGIN_ROOT_ID = "noitom_mocap" _NOITOM_VENDOR_ID = "body.noitom" @@ -86,7 +94,7 @@ def g1_action_dim(*, use_arm_ik_frame_tasks: bool) -> int: return ( _G1_ACTION_DIM_WITH_ARM_IK if use_arm_ik_frame_tasks - else _G1_ACTION_DIM_WRIST_ONLY + else _G1_ACTION_DIM_WRIST_TORSO ) @@ -100,6 +108,9 @@ class NoitomG1Settings: teleoperation_active_default: bool = True enable_motion: bool = True print_period_s: float = 0.5 + orientation_debug: bool = False + clear_workspace: bool = False + robot_world_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) draw_reference: bool = True draw_scale: float = 1.0 draw_offset: tuple[float, float, float] = _NOITOM_REFERENCE_DEFAULT_OFFSET @@ -107,17 +118,28 @@ class NoitomG1Settings: draw_pelvis_relative: bool = True draw_pelvis_anchor: tuple[float, float, float] = _ROBOT_PELVIS_ANCHOR draw_wrist_targets: bool = True + draw_wrist_frames: bool = False + wrist_frame_axis_length: float = 0.10 draw_elbow_targets: bool = True draw_shoulder_targets: bool = True - # Wrist + elbow + shoulder LocalFrameTasks for Pink IK (60D action). + # Wrist + torso + elbow + shoulder LocalFrameTasks for Pink IK (63D action). use_arm_ik_frame_tasks: bool = True + # Noitom-only Pink tuning for fast recorded motion. + pink_task_gain: float = 0.20 + pink_lm_damping: float = 50.0 + # Symmetric safety bounds applied inside Noitom Pink and to its output only. + wrist_pitch_limit_deg: float = 80.0 + wrist_yaw_limit_deg: float = 80.0 retargeting: NoitomRetargetingSettings = field( default_factory=lambda: NoitomRetargetingSettings( robot_pelvis_world=np.array(_ROBOT_PELVIS_ANCHOR, dtype=np.float64), - motion_scale=0.75, + motion_scale=1.0, track_aligned_mocap_wrists=True, + wrist_orientation_mode="source", + wrist_twist_limit_deg=60.0, track_elbow_ik_targets=True, track_shoulder_ik_targets=True, + ik_config_path=str(DEFAULT_NOITOM_IK_CONFIG_PATH), ) ) @@ -157,13 +179,98 @@ def _env_bool(name: str, default: bool) -> bool: return value.lower() not in {"0", "false", "no", "off"} +def _env_float(name: str, default: float) -> float: + value = os.environ.get(name) + return default if value is None else float(value) + + +def _env_str(name: str, default: str) -> str: + value = os.environ.get(name) + return default if value is None else value.strip().lower() + + +def _env_vec3( + name: str, default: tuple[float, float, float] +) -> tuple[float, float, float]: + value = os.environ.get(name) + if value is None: + return default + parts = value.split(",") + if len(parts) != 3: + raise ValueError(f"{name} must contain three comma-separated numbers") + return tuple(float(part.strip()) for part in parts) + + def _noitom_settings_from_env() -> NoitomG1Settings: + defaults = DEFAULT_NOITOM_G1_SETTINGS + robot_world_offset = _env_vec3("NOITOM_ROBOT_OFFSET", defaults.robot_world_offset) + offset = np.asarray(robot_world_offset, dtype=np.float64) + draw_pelvis_anchor = tuple( + np.asarray(defaults.draw_pelvis_anchor, dtype=np.float64) + offset + ) + retargeting = replace( + defaults.retargeting, + robot_pelvis_world=defaults.retargeting.robot_pelvis_world + offset, + wrist_orientation_mode=_env_str( + "NOITOM_WRIST_ORIENTATION_MODE", + defaults.retargeting.wrist_orientation_mode, + ), + wrist_twist_limit_deg=_env_float( + "NOITOM_WRIST_TWIST_LIMIT_DEG", + defaults.retargeting.wrist_twist_limit_deg, + ), + wrist_twist_max_step_deg=_env_float( + "NOITOM_WRIST_TWIST_MAX_STEP_DEG", + defaults.retargeting.wrist_twist_max_step_deg, + ), + torso_orientation_scale=_env_float( + "NOITOM_TORSO_ORIENTATION_SCALE", + defaults.retargeting.torso_orientation_scale, + ), + torso_rotation_smoothing=_env_float( + "NOITOM_TORSO_ROTATION_SMOOTHING", + defaults.retargeting.torso_rotation_smoothing, + ), + torso_yaw_limit_deg=_env_float( + "NOITOM_TORSO_YAW_LIMIT_DEG", + defaults.retargeting.torso_yaw_limit_deg, + ), + torso_roll_limit_deg=_env_float( + "NOITOM_TORSO_ROLL_LIMIT_DEG", + defaults.retargeting.torso_roll_limit_deg, + ), + torso_pitch_limit_deg=_env_float( + "NOITOM_TORSO_PITCH_LIMIT_DEG", + defaults.retargeting.torso_pitch_limit_deg, + ), + ik_config_path=os.environ.get( + "NOITOM_IK_CONFIG", defaults.retargeting.ik_config_path + ), + ) return replace( - DEFAULT_NOITOM_G1_SETTINGS, + defaults, plugin_auto_launch=_env_bool( "NOITOM_MOCAP_AUTO_LAUNCH", - DEFAULT_NOITOM_G1_SETTINGS.plugin_auto_launch, + defaults.plugin_auto_launch, + ), + orientation_debug=_env_bool( + "NOITOM_ORIENTATION_DEBUG", defaults.orientation_debug + ), + clear_workspace=_env_bool("NOITOM_CLEAR_WORKSPACE", defaults.clear_workspace), + draw_wrist_frames=_env_bool( + "NOITOM_DRAW_WRIST_FRAMES", defaults.draw_wrist_frames + ), + pink_task_gain=_env_float("NOITOM_PINK_TASK_GAIN", defaults.pink_task_gain), + pink_lm_damping=_env_float("NOITOM_PINK_LM_DAMPING", defaults.pink_lm_damping), + wrist_pitch_limit_deg=_env_float( + "NOITOM_WRIST_PITCH_LIMIT_DEG", defaults.wrist_pitch_limit_deg + ), + wrist_yaw_limit_deg=_env_float( + "NOITOM_WRIST_YAW_LIMIT_DEG", defaults.wrist_yaw_limit_deg ), + robot_world_offset=robot_world_offset, + draw_pelvis_anchor=draw_pelvis_anchor, + retargeting=retargeting, ) @@ -201,136 +308,594 @@ def _noitom_plugin_configs(settings: NoitomG1Settings) -> list[PluginConfig]: ) # Inset from hard joint limits (rad). Small enough to avoid IK deadlock at the stops. _WAIST_HARD_LIMIT_MARGIN_RAD = 0.10 +_WRIST_HARD_LIMIT_MARGIN_RAD = 0.05 -def _build_noitom_pink_ik_action_class(): +def _normalized_pose_xyzw(pose: np.ndarray) -> np.ndarray: + """Return a copied 7DoF pose with a normalized xyzw quaternion.""" + result = np.asarray(pose, dtype=np.float64).copy() + if result.shape != (7,): + raise ValueError(f"expected a 7DoF pose, got shape {result.shape}") + norm = float(np.linalg.norm(result[3:7])) + if norm < 1.0e-8: + raise ValueError("pose quaternion must be nonzero") + result[3:7] /= norm + return result + + +def _pose_error(left: np.ndarray, right: np.ndarray) -> tuple[float, float]: + """Return position meters and sign-invariant quaternion distance degrees.""" + left_pose = _normalized_pose_xyzw(left) + right_pose = _normalized_pose_xyzw(right) + return ( + float(np.linalg.norm(left_pose[:3] - right_pose[:3])), + _quaternion_distance_deg(left_pose[3:7], right_pose[3:7]), + ) + + +def _pose_to_matrix(pose: np.ndarray) -> np.ndarray: + normalized = _normalized_pose_xyzw(pose) + transform = np.eye(4, dtype=np.float64) + transform[:3, :3] = _quaternion_matrix(normalized[3:7]) + transform[:3, 3] = normalized[:3] + return transform + + +def _matrix_to_pose(matrix: np.ndarray) -> np.ndarray: + transform = np.asarray(matrix, dtype=np.float64) + if transform.shape != (4, 4): + raise ValueError(f"expected a 4x4 transform, got shape {transform.shape}") + quaternion = Rotation.from_matrix(transform[:3, :3]).as_quat() + return _normalized_pose_xyzw(np.concatenate([transform[:3, 3], quaternion])) + + +def _pose_in_local_frame( + world_pose: np.ndarray, local_frame_world: np.ndarray +) -> np.ndarray: + """Express one environment-world pose in a supplied local-frame transform.""" + local_from_world = np.linalg.inv(np.asarray(local_frame_world, dtype=np.float64)) + return _matrix_to_pose(local_from_world @ _pose_to_matrix(world_pose)) + + +def _pose_from_local_frame( + local_pose: np.ndarray, local_frame_world: np.ndarray +) -> np.ndarray: + """Compose one local pose into environment world using the supplied transform.""" + return _matrix_to_pose( + np.asarray(local_frame_world, dtype=np.float64) @ _pose_to_matrix(local_pose) + ) + + +def _environment_world_pose( + simulator_world_pose: np.ndarray, env_origin: np.ndarray +) -> np.ndarray: + """Subtract an Isaac environment origin while preserving link orientation.""" + pose = _normalized_pose_xyzw(simulator_world_pose) + pose[:3] -= np.asarray(env_origin, dtype=np.float64) + return pose + + +def _pin_se3_pose(transform: Any) -> np.ndarray: + """Convert a Pinocchio-style SE3 into a normalized xyzw 7DoF pose.""" + return _normalized_pose_xyzw( + np.concatenate( + [ + np.asarray(transform.translation, dtype=np.float64), + Rotation.from_matrix( + np.asarray(transform.rotation, dtype=np.float64) + ).as_quat(), + ] + ) + ) + + +def _pink_solution_fk_pelvis_poses( + ik_controller: Any, + current_joint_positions_isaac: np.ndarray, + controlled_joint_ids: list[int], + final_solution: np.ndarray, + wrist_frame_names: dict[str, str], +) -> dict[str, np.ndarray]: + """Evaluate final clipped joint targets without mutating Pink's solve state.""" + configuration = ik_controller.pink_configuration + saved_configuration = np.asarray(configuration.full_q, dtype=np.float64).copy() + solved_configuration = np.asarray( + current_joint_positions_isaac, dtype=np.float64 + ).copy() + solved_configuration[np.asarray(controlled_joint_ids, dtype=np.int64)] = np.asarray( + final_solution, dtype=np.float64 + ) + pink_ordered = solved_configuration[ik_controller.isaac_lab_to_pink_ordering] + try: + configuration.update(pink_ordered) + return { + side: _pin_se3_pose( + configuration.get_transform(frame_name, _PINK_PELVIS_LINK) + ) + for side, frame_name in wrist_frame_names.items() + } + finally: + # Diagnostics must never leave FK state behind for the next control solve. + configuration.update(saved_configuration) + + +def _build_noitom_pink_ik_action_class( + orientation_debug: bool, + print_period_s: float, + draw_wrist_frames: bool, + wrist_frame_axis_length: float, + wrist_pitch_limit_deg: float, + wrist_yaw_limit_deg: float, +): """Lazy import so unit tests can load noitom_tasks without Isaac Lab.""" import torch + from isaaclab.controllers.pink_ik.pink_tasks import LocalFrameTask 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.""" + """Pink IK with Noitom-only waist and wrist safety limits.""" def __init__(self, cfg, env): super().__init__(cfg, env) + self._orientation_debug = orientation_debug + self._orientation_debug_period_s = max(0.0, print_period_s) + self._last_orientation_debug_s = 0.0 + self._wrist_body_ids: dict[str, int] = {} + self._wrist_joint_debug: dict[ + str, list[tuple[str, int, int, float, float]] + ] = {"left": [], "right": []} + self._last_ik_current = None + self._last_ik_solution = None + self._solve_cycle = 0 + self._pink_input_world = None + self._pink_target_pelvis: list[dict[str, np.ndarray]] | None = None + self._wrist_pink_frame_names: dict[str, str] = {} + self._wrist_pink_tasks: list[dict[str, Any]] = [] + self._g1_wrist_frame_viz = None + for side in ("left", "right"): + suffix = f"_{side}_wrist_yaw_link" + matches = [ + task.frame + for task in self._ik_controllers[0].cfg.variable_input_tasks + if isinstance(task, LocalFrameTask) and task.frame.endswith(suffix) + ] + if len(matches) != 1: + raise RuntimeError( + f"Expected one Pink {side} wrist frame, found {matches}" + ) + self._wrist_pink_frame_names[side] = matches[0] + for ik_controller in self._ik_controllers: + tasks_by_frame = { + task.frame: task + for task in ik_controller.cfg.variable_input_tasks + if isinstance(task, LocalFrameTask) + } + self._wrist_pink_tasks.append( + { + side: tasks_by_frame[frame_name] + for side, frame_name in self._wrist_pink_frame_names.items() + } + ) + if self._orientation_debug or draw_wrist_frames: + for side in ("left", "right"): + body_name = self.cfg.target_eef_link_names[f"{side}_wrist"] + body_ids, _body_names = self._asset.find_bodies([body_name]) + if len(body_ids) != 1: + raise RuntimeError( + f"Expected one G1 {side} wrist body for {body_name}, " + f"found {len(body_ids)}" + ) + self._wrist_body_ids[side] = body_ids[0] + if draw_wrist_frames: + from isaaclab.markers import VisualizationMarkers + from isaaclab.markers.config import FRAME_MARKER_CFG + + marker_cfg = FRAME_MARKER_CFG.copy() + marker_cfg.prim_path = "/Visuals/Noitom/G1WristFrames" + axis_length = max(0.01, wrist_frame_axis_length) + marker_cfg.markers["frame"].scale = ( + axis_length, + axis_length, + axis_length, + ) + self._g1_wrist_frame_viz = VisualizationMarkers(marker_cfg) + print( + "NoitomG1Env: drawing actual G1 wrist frames " + "(X=red, Y=green, Z=blue)" + ) + if wrist_pitch_limit_deg <= 0.0 or wrist_yaw_limit_deg <= 0.0: + raise ValueError("Noitom wrist pitch/yaw limits must be positive") + clip_ids: list[int] = [] lows: list[float] = [] highs: list[float] = [] + safe_bounds: dict[str, tuple[float, 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 + wrist_limits_rad = { + "pitch": float(np.deg2rad(wrist_pitch_limit_deg)), + "yaw": float(np.deg2rad(wrist_yaw_limit_deg)), + } 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 name in _WAIST_JOINT_NAMES: + lo = lo_hard + _WAIST_HARD_LIMIT_MARGIN_RAD + hi = hi_hard - _WAIST_HARD_LIMIT_MARGIN_RAD + elif name.endswith("_wrist_pitch_joint"): + limit = wrist_limits_rad["pitch"] + lo = max(lo_hard + _WRIST_HARD_LIMIT_MARGIN_RAD, -limit) + hi = min(hi_hard - _WRIST_HARD_LIMIT_MARGIN_RAD, limit) + elif name.endswith("_wrist_yaw_joint"): + limit = wrist_limits_rad["yaw"] + lo = max(lo_hard + _WRIST_HARD_LIMIT_MARGIN_RAD, -limit) + hi = min(hi_hard - _WRIST_HARD_LIMIT_MARGIN_RAD, limit) + else: + continue if lo >= hi: mid = 0.5 * (lo_hard + hi_hard) half = 0.45 * (hi_hard - lo_hard) lo, hi = mid - half, mid + half + safe_bounds[name] = (lo, hi) clip_ids.append(ik_index) lows.append(lo) highs.append(hi) - self._waist_ik_indices = clip_ids + self._safe_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( + self._safe_low = torch.tensor(lows, device=self.device).view(1, -1) + self._safe_high = torch.tensor(highs, device=self.device).view(1, -1) + self._safe_ik_idx = torch.tensor( clip_ids, device=self.device, dtype=torch.long ) + # Pink reads these model limits when it builds each QP. Restrict only + # this Noitom action's controller models; generic Isaac Lab tasks keep + # their original URDF limits. + wrist_bounds = { + name: bounds + for name, bounds in safe_bounds.items() + if "_wrist_pitch_joint" in name or "_wrist_yaw_joint" in name + } + for ik_controller in self._ik_controllers: + model = ik_controller.pink_configuration.model + for name, (lo, hi) in wrist_bounds.items(): + joint_id = model.getJointId(name) + if joint_id <= 0 or joint_id >= model.njoints: + raise RuntimeError(f"Pink model does not contain {name}") + joint = model.joints[joint_id] + if joint.nq != 1: + raise RuntimeError(f"Expected one-DoF wrist joint {name}") + model.lowerPositionLimit[joint.idx_q] = lo + model.upperPositionLimit[joint.idx_q] = hi + + for side in ("left", "right"): + for axis in ("roll", "pitch", "yaw"): + name = f"{side}_wrist_{axis}_joint" + if name not in self._isaaclab_controlled_joint_names: + continue + ik_index = self._isaaclab_controlled_joint_names.index(name) + joint_idx = name_to_joint_idx[name] + lo_hard = float(hard_limits[joint_idx, 0]) + hi_hard = float(hard_limits[joint_idx, 1]) + lo, hi = safe_bounds.get(name, (lo_hard, hi_hard)) + self._wrist_joint_debug[side].append( + (axis, ik_index, joint_idx, lo, hi) + ) + print( + "NoitomG1Env: Noitom-only wrist IK limits " + f"pitch=+/-{wrist_pitch_limit_deg:.1f} deg " + f"yaw=+/-{wrist_yaw_limit_deg:.1f} deg" + ) + def _compute_ik_solutions(self) -> torch.Tensor: + current_all = self._asset.data.joint_pos.torch.clone() + current = self._asset.data.joint_pos.torch[ + :, self._isaaclab_controlled_joint_ids + ].clone() sol = super()._compute_ik_solutions() - if self._waist_ik_indices: - waist = sol.index_select(1, self._waist_ik_idx) + if self._safe_ik_indices: + safe_joints = sol.index_select(1, self._safe_ik_idx) sol.index_copy_( 1, - self._waist_ik_idx, - torch.clamp(waist, self._waist_low, self._waist_high), + self._safe_ik_idx, + torch.clamp(safe_joints, self._safe_low, self._safe_high), ) + self._last_ik_current = current + self._last_ik_solution = sol.detach().clone() + self._maybe_print_pink_pose_diagnostics(current_all, current, sol) return sol + def process_actions(self, actions: torch.Tensor) -> None: + super().process_actions(actions) + if self._orientation_debug: + self._solve_cycle += 1 + self._pink_input_world = actions[:, :14].detach().clone() + self._pink_target_pelvis = [] + for wrist_tasks in self._wrist_pink_tasks: + self._pink_target_pelvis.append( + { + side: _pin_se3_pose(task.transform_target_to_base) + for side, task in wrist_tasks.items() + } + ) + body_poses = self._asset.data.body_link_pose_w.torch + if self._g1_wrist_frame_viz is not None: + actual_poses = torch.stack( + [ + body_poses[0, self._wrist_body_ids["left"]], + body_poses[0, self._wrist_body_ids["right"]], + ] + ) + self._g1_wrist_frame_viz.visualize( + actual_poses[:, :3], actual_poses[:, 3:7] + ) + + def _maybe_print_pink_pose_diagnostics( + self, + current_all: torch.Tensor, + current_controlled: torch.Tensor, + solution: torch.Tensor, + ) -> None: + if not self._orientation_debug: + return + if self._pink_input_world is None or self._pink_target_pelvis is None: + return + now_s = time.monotonic() + if ( + self._orientation_debug_period_s > 0.0 + and now_s - self._last_orientation_debug_s + < self._orientation_debug_period_s + ): + return + self._last_orientation_debug_s = now_s + body_poses = self._asset.data.body_link_pose_w.torch + env_origins = self._env.scene.env_origins + for env_index, ik_controller in enumerate(self._ik_controllers): + base_world = ( + self.base_link_frame_in_world_rf[env_index].detach().cpu().numpy() + ) + solution_fk_pelvis = _pink_solution_fk_pelvis_poses( + ik_controller, + current_all[env_index].detach().cpu().numpy(), + self._isaaclab_controlled_joint_ids, + solution[env_index].detach().cpu().numpy(), + self._wrist_pink_frame_names, + ) + env_label = f" env={env_index}" if self.num_envs > 1 else "" + for side, action_start in (("left", 0), ("right", 7)): + input_world = _normalized_pose_xyzw( + self._pink_input_world[ + env_index, action_start : action_start + 7 + ] + .detach() + .cpu() + .numpy() + ) + target_pelvis = self._pink_target_pelvis[env_index][side] + solution_pelvis = solution_fk_pelvis[side] + solution_world = _pose_from_local_frame(solution_pelvis, base_world) + actual_world = _environment_world_pose( + body_poses[env_index, self._wrist_body_ids[side]] + .detach() + .cpu() + .numpy(), + env_origins[env_index].detach().cpu().numpy(), + ) + actual_pelvis = _pose_in_local_frame(actual_world, base_world) + input_solution = _pose_error(input_world, solution_world) + solution_actual = _pose_error(solution_world, actual_world) + input_actual = _pose_error(input_world, actual_world) + + for stage, pose in ( + ("pink_input_world", input_world), + ("pink_target_pelvis", target_pelvis), + ("pink_solution_fk_world", solution_world), + ("pink_solution_fk_pelvis", solution_pelvis), + ("robot_actual_pre_apply_world", actual_world), + ("robot_actual_pelvis", actual_pelvis), + ): + print( + f"NoitomWristPoseDebug solve_cycle={self._solve_cycle}" + f"{env_label} stage={stage} side={side} " + f"{_fmt_pose_debug(pose)}" + ) + print( + f"NoitomWristPoseError solve_cycle={self._solve_cycle}" + f"{env_label} side={side} " + f"input_to_solution_pos_m={input_solution[0]:.6f} " + f"input_to_solution_rot_deg={input_solution[1]:.6f} " + f"solution_to_actual_pos_m={solution_actual[0]:.6f} " + f"solution_to_actual_rot_deg={solution_actual[1]:.6f} " + f"input_to_actual_pos_m={input_actual[0]:.6f} " + f"input_to_actual_rot_deg={input_actual[1]:.6f}" + ) + print( + f"NoitomPinkOrientationDebug side={side} " + f"target_q={_fmt_array(input_world[3:7], precision=6)} " + f"actual_q={_fmt_array(actual_world[3:7], precision=6)} " + f"error_deg={input_actual[1]:.2f}" + ) + joint_info = self._wrist_joint_debug[side] + if not joint_info: + continue + actual_joints = torch.stack( + [ + self._asset.data.joint_pos.torch[env_index, joint_idx] + for _axis, _ik_idx, joint_idx, _lo, _hi in joint_info + ] + ) + ik_targets = torch.stack( + [ + solution[env_index, ik_idx] + for _axis, ik_idx, _joint_idx, _lo, _hi in joint_info + ] + ) + previous = torch.stack( + [ + current_controlled[env_index, ik_idx] + for _axis, ik_idx, _joint_idx, _lo, _hi in joint_info + ] + ) + lows = torch.tensor( + [lo for _axis, _ik_idx, _joint_idx, lo, _hi in joint_info], + device=self.device, + ) + highs = torch.tensor( + [hi for _axis, _ik_idx, _joint_idx, _lo, hi in joint_info], + device=self.device, + ) + margins = torch.minimum(actual_joints - lows, highs - actual_joints) + ik_step_max_rad = float( + torch.max(torch.abs(ik_targets - previous)).item() + ) + solver_held = ik_step_max_rad < 1.0e-7 + axes = ",".join(item[0] for item in joint_info) + print( + f"NoitomPinkJointDebug side={side} axes={axes} " + f"actual_rad={_fmt_tensor(actual_joints)} " + f"ik_target_rad={_fmt_tensor(ik_targets)} " + f"safe_margin_rad={_fmt_tensor(margins)} " + f"ik_step_max_rad={ik_step_max_rad:.6f} " + f"solver_held={int(solver_held)}" + ) + return NoitomPinkInverseKinematicsAction def _configure_noitom_pink_ik( - env_cfg: NoitomLocomanipulationG1EnvCfg, use_arm_frames: bool + env_cfg: NoitomLocomanipulationG1EnvCfg, + use_arm_frames: bool, + pink_task_gain: float, + pink_lm_damping: float, + orientation_debug: bool, + print_period_s: float, + draw_wrist_frames: bool, + wrist_frame_axis_length: float, + wrist_pitch_limit_deg: float, + wrist_yaw_limit_deg: float, + ik_config_path: str | None, ) -> None: - """Tune Pink IK for Noitom teleop; optionally add elbow/shoulder frame tasks.""" + """Tune Pink IK and add torso plus optional 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() + if not 0.0 < pink_task_gain <= 1.0: + raise ValueError("NOITOM_PINK_TASK_GAIN must be in (0, 1]") + if pink_lm_damping < 0.0: + raise ValueError("NOITOM_PINK_LM_DAMPING must be nonnegative") + if ik_config_path is None: + raise ValueError("Noitom Pink task weights require an IK config") + + env_cfg.actions.upper_body_ik.class_type = _build_noitom_pink_ik_action_class( + orientation_debug, + print_period_s, + draw_wrist_frames, + wrist_frame_axis_length, + wrist_pitch_limit_deg, + wrist_yaw_limit_deg, + ) 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) + controller.show_ik_warnings = orientation_debug + ik_config = load_noitom_ik_config(ik_config_path) + task_weights = ik_config.pink_task_weights + + def configured_cost(frame: str, role: str, *, rotation: bool = False) -> float: + side = "left" if "left" in frame else "right" + mapping = ik_config.match(side, role) + return mapping.rotation_weight if rotation else mapping.position_weight + + tasks = list(controller.variable_input_tasks) + wrist_task_count = sum(1 for task in tasks if isinstance(task, LocalFrameTaskCfg)) + extra_frame_tasks = [ + LocalFrameTaskCfg( + frame=_PINK_TORSO_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=task_weights.torso_position, + orientation_cost=task_weights.torso_rotation, + lm_damping=pink_lm_damping, + gain=pink_task_gain, ) - 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:] + ] + if use_arm_frames: + extra_frame_tasks.extend( + [ + LocalFrameTaskCfg( + frame=_PINK_LEFT_ELBOW_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=configured_cost(_PINK_LEFT_ELBOW_LINK, "elbow"), + orientation_cost=configured_cost( + _PINK_LEFT_ELBOW_LINK, "elbow", rotation=True + ), + lm_damping=pink_lm_damping, + gain=pink_task_gain, + ), + LocalFrameTaskCfg( + frame=_PINK_RIGHT_ELBOW_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=configured_cost(_PINK_RIGHT_ELBOW_LINK, "elbow"), + orientation_cost=configured_cost( + _PINK_RIGHT_ELBOW_LINK, "elbow", rotation=True + ), + lm_damping=pink_lm_damping, + gain=pink_task_gain, + ), + LocalFrameTaskCfg( + frame=_PINK_LEFT_SHOULDER_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=configured_cost(_PINK_LEFT_SHOULDER_LINK, "shoulder"), + orientation_cost=configured_cost( + _PINK_LEFT_SHOULDER_LINK, "shoulder", rotation=True + ), + lm_damping=pink_lm_damping, + gain=pink_task_gain, + ), + LocalFrameTaskCfg( + frame=_PINK_RIGHT_SHOULDER_LINK, + base_link_frame_name=_PINK_PELVIS_LINK, + position_cost=configured_cost( + _PINK_RIGHT_SHOULDER_LINK, "shoulder" + ), + orientation_cost=configured_cost( + _PINK_RIGHT_SHOULDER_LINK, "shoulder", rotation=True + ), + lm_damping=pink_lm_damping, + gain=pink_task_gain, + ), + ] ) + controller.variable_input_tasks = ( + tasks[:wrist_task_count] + extra_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 + task.gain = pink_task_gain + task.lm_damping = pink_lm_damping if "wrist" in frame: - task.position_cost = 18.0 + task.position_cost = configured_cost(frame, "wrist") + task.orientation_cost = configured_cost(frame, "wrist", rotation=True) + elif frame == _PINK_TORSO_LINK: + task.position_cost = task_weights.torso_position + task.orientation_cost = task_weights.torso_rotation elif "elbow" in frame: - task.position_cost = 9.0 + task.position_cost = configured_cost(frame, "elbow") + task.orientation_cost = configured_cost(frame, "elbow", rotation=True) elif "shoulder" in frame: - task.position_cost = 6.0 + task.position_cost = configured_cost(frame, "shoulder") + task.orientation_cost = configured_cost( + frame, "shoulder", rotation=True + ) else: - task.position_cost = 14.0 + raise ValueError(f"No Pink task weights configured for frame {frame!r}") elif isinstance(task, NullSpacePostureTaskCfg): - task.cost = 0.05 - task.gain = 0.25 - task.lm_damping = 50.0 + task.cost = task_weights.null_space_posture + task.gain = pink_task_gain + task.lm_damping = pink_lm_damping def register_tasks() -> list[str]: @@ -355,24 +920,69 @@ 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() + settings = replace( + settings, + retargeting=replace( + settings.retargeting, + robot_pelvis_quat_xyzw=np.asarray( + self.scene.robot.init_state.rot, dtype=np.float64 + ), + ), + ) 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. + if settings.clear_workspace: + del self.scene.packing_table + self.scene.object.init_state.pos = (3.0, 3.0, 1.0) + self.scene.object.spawn.rigid_props.disable_gravity = True + self.terminations.object_dropping = None + self.terminations.object_too_far = None + self.terminations.success = None + print("NoitomG1Env: removed packing_table and parked object at [3, 3, 1]") + robot_world_offset = np.asarray(settings.robot_world_offset, dtype=np.float64) + if np.any(robot_world_offset != 0.0): + initial_position = np.asarray( + self.scene.robot.init_state.pos, dtype=np.float64 + ) + self.scene.robot.init_state.pos = tuple( + initial_position + robot_world_offset + ) + print( + "NoitomG1Env: shifted robot/reference/IK anchors " + f"offset={_fmt_vec(robot_world_offset)}" + ) + # Fixed-root upper-body teleoperation: the inherited Agile term must be + # removed before managers are instantiated, while waist remains in Pink. 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. + self.actions.lower_body_joint_pos = None + self.observations.lower_body_policy = None + # Pink IK: wrists plus an orientation-only torso task, with optional + # elbow/shoulder position tasks. Root stays fixed while waist remains in IK. _configure_noitom_pink_ik( self, use_arm_frames=settings.use_arm_ik_frame_tasks, + pink_task_gain=settings.pink_task_gain, + pink_lm_damping=settings.pink_lm_damping, + orientation_debug=settings.orientation_debug, + print_period_s=settings.print_period_s, + draw_wrist_frames=settings.draw_wrist_frames, + wrist_frame_axis_length=settings.wrist_frame_axis_length, + wrist_pitch_limit_deg=settings.wrist_pitch_limit_deg, + wrist_yaw_limit_deg=settings.wrist_yaw_limit_deg, + ik_config_path=settings.retargeting.ik_config_path, ) self.isaac_teleop.teleoperation_active_default = ( settings.teleoperation_active_default ) self.isaac_teleop.control_channel_uuid = None self.isaac_teleop.app_name = "IsaacLabNoitomG1" + print( + "NoitomG1Env: lower_body=fixed root_fixed=1 " + "leg_action=disabled " + f"action_dim={g1_action_dim(use_arm_ik_frame_tasks=settings.use_arm_ik_frame_tasks)}" + ) def G1LocomanipulationAction(*, use_arm_ik_frame_tasks: bool = True) -> TensorGroupType: @@ -391,7 +1001,7 @@ def G1LocomanipulationAction(*, use_arm_ik_frame_tasks: bool = True) -> TensorGr class NoitomG1ActionSource(IDeviceIOSource): - """Convert Noitom mocap frames into G1 locomanipulation wrist actions.""" + """Convert Noitom mocap frames into G1 upper-body task-space actions.""" def __init__( self, @@ -421,7 +1031,13 @@ def __init__( self._calibration_attempts = 0 self._no_data_count = 0 self._first_frame_printed = False + self._first_valid_wrist_pose_printed = False self._calibration_fail_count = 0 + self._orientation_debug = settings.orientation_debug + self._previous_orientation_debug_time_s: float | None = None + self._previous_orientation_debug_quaternions: dict[ + str, tuple[np.ndarray, np.ndarray] + ] = {} super().__init__(name, vendor=vendor) def get_tracker(self): @@ -461,6 +1077,9 @@ def _compute_fn( self._calibration_attempts = 0 self._no_data_count = 0 self._calibration_fail_count = 0 + self._previous_orientation_debug_time_s = None + self._previous_orientation_debug_quaternions.clear() + self._reset_wrist_pose_debug_state() print( "NoitomG1ActionSource: cleared retargeting calibration " f"collection={self._collection_id}" @@ -547,9 +1166,73 @@ def _compute_fn( ) outputs[_ACTION_OUTPUT][0] = np.ascontiguousarray(action, dtype=np.float32) + self._maybe_print_first_valid_wrist_pose(frame, action) self._reference_viz.update(frame, self._retargeter) self._print_status(frame, body_yaw_delta, context) + def _reset_wrist_pose_debug_state(self) -> None: + self._first_valid_wrist_pose_printed = False + + def _maybe_print_first_valid_wrist_pose( + self, frame: FullBodyPose, action: np.ndarray + ) -> None: + if ( + not self._orientation_debug + or self._first_valid_wrist_pose_printed + or not self._retargeter.is_calibrated + ): + return + diagnostics = self._retargeter.wrist_pose_diagnostics(frame) + if diagnostics is None: + return + + for side, action_start in (("left", 0), ("right", 7)): + raw_pose = diagnostics[side].bvh_raw_isaac_world.as_action_pose() + aligned_raw_pose = diagnostics[side].bvh_aligned_raw_world.as_action_pose() + semantic_pose = diagnostics[side].bvh_semantic_world.as_action_pose() + target_pose = np.asarray( + action[action_start : action_start + 7], dtype=np.float64 + ) + _raw_semantic_pos_error, raw_semantic_rot_error = _pose_error( + aligned_raw_pose, semantic_pose + ) + pos_error, rot_error = _pose_error(semantic_pose, target_pose) + for stage, pose in ( + ("bvh_raw_isaac_world", raw_pose), + ("bvh_aligned_raw_world", aligned_raw_pose), + ("bvh_semantic_world", semantic_pose), + ("retarget_target_world", target_pose), + ): + print( + f"NoitomWristPoseDebug sample=first_valid " + f"source_frame={self._frame_count} stage={stage} side={side} " + f"{_fmt_pose_debug(pose)}" + ) + print( + f"NoitomWristPoseError sample=first_valid " + f"source_frame={self._frame_count} side={side} " + f"raw_to_semantic_rot_deg={raw_semantic_rot_error:.6f} " + f"semantic_to_retarget_pos_m={pos_error:.6f} " + f"semantic_to_retarget_rot_deg={rot_error:.6f}" + ) + print( + f"NoitomWristAxisDebug sample=first_valid " + f"source_frame={self._frame_count} side={side} " + f"raw_x_dot_forearm=" + f"{_fmt_optional_float(diagnostics[side].raw_x_dot_forearm)} " + f"semantic_x_dot_forearm=" + f"{_fmt_optional_float(diagnostics[side].semantic_x_dot_forearm)} " + f"semantic_x_world=" + f"{_fmt_array(diagnostics[side].semantic_x_world, precision=6)} " + f"semantic_y_world=" + f"{_fmt_array(diagnostics[side].semantic_y_world, precision=6)} " + f"semantic_z_world=" + f"{_fmt_array(diagnostics[side].semantic_z_world, precision=6)} " + f"local_offset_xyzw=" + f"{_fmt_array(diagnostics[side].local_offset_xyzw, precision=6)}" + ) + self._first_valid_wrist_pose_printed = True + def _print_status( self, frame: FullBodyPose, body_yaw_delta: float, context: ComputeContext ) -> None: @@ -564,6 +1247,7 @@ def _print_status( 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() + torso_pose = self._hold_targets.torso.as_action_pose() frame_info = "" if self._use_arm_ik_frame_tasks: left_elbow_pose = self._hold_targets.left_elbow.as_action_pose() @@ -582,11 +1266,82 @@ def _print_status( f"motion={motion} calibrated={calib} " f"yaw_delta={body_yaw_delta:+.3f} " f"motion_scale={self._retargeter.retargeting_settings.motion_scale:.2f} " + f"wrist_orientation_mode=" + f"{self._retargeter.retargeting_settings.wrist_orientation_mode} " f"torso_yaw_influence={self._retargeter.retargeting_settings.torso_yaw_arm_influence:.2f} " + f"target_torso_quat={_fmt_quat(torso_pose[3:7])} " f"target_left={_fmt_pose(left_pose)} target_right={_fmt_pose(right_pose)}" f"{frame_info} " f"{_raw_full_body_status(frame)}" ) + if self._orientation_debug: + self._print_orientation_debug(frame, now_s) + + def _print_orientation_debug(self, frame: FullBodyPose, now_s: float) -> None: + diagnostics = self._retargeter.wrist_orientation_diagnostics(frame) + if diagnostics is None: + return + elapsed_s = ( + None + if self._previous_orientation_debug_time_s is None + else now_s - self._previous_orientation_debug_time_s + ) + for side, diagnostic in diagnostics.items(): + world_speed = 0.0 + target_speed = 0.0 + previous = self._previous_orientation_debug_quaternions.get(side) + if previous is not None and elapsed_s is not None and elapsed_s > 1.0e-6: + world_speed = ( + _quaternion_distance_deg( + previous[0], diagnostic.world_quaternion_xyzw + ) + / elapsed_s + ) + target_speed = ( + _quaternion_distance_deg( + previous[1], diagnostic.target_quaternion_xyzw + ) + / elapsed_s + ) + world_delta_angle = float(np.linalg.norm(diagnostic.world_delta_rotvec_deg)) + torso_delta_angle = float(np.linalg.norm(diagnostic.torso_delta_rotvec_deg)) + print( + f"NoitomOrientationDebug side={side} " + f"world_q={_fmt_quat(diagnostic.world_quaternion_xyzw)} " + f"reference_q={_fmt_quat(diagnostic.reference_quaternion_xyzw)} " + f"torso_q={_fmt_quat(diagnostic.torso_quaternion_xyzw)} " + f"world_delta_rotvec_deg={_fmt_vec(diagnostic.world_delta_rotvec_deg)} " + f"world_delta_deg={world_delta_angle:.2f} " + f"torso_delta_rotvec_deg={_fmt_vec(diagnostic.torso_delta_rotvec_deg)} " + f"torso_delta_deg={torso_delta_angle:.2f} " + f"forearm_swing_deg={diagnostic.forearm_swing_deg:.2f} " + f"twist_deg={diagnostic.twist_deg:+.2f} " + f"bounded_twist_deg={diagnostic.bounded_twist_deg:+.2f} " + f"source_target_error_deg={diagnostic.source_target_error_deg:.2f} " + f"target_q={_fmt_quat(diagnostic.target_quaternion_xyzw)} " + f"world_speed_deg_s={world_speed:.2f} " + f"target_speed_deg_s={target_speed:.2f}" + ) + print( + f"NoitomWristAxisDebug side={side} " + f"raw_x_dot_forearm=" + f"{_fmt_optional_float(diagnostic.raw_x_dot_forearm)} " + f"semantic_x_dot_forearm=" + f"{_fmt_optional_float(diagnostic.semantic_x_dot_forearm)} " + f"semantic_x_world=" + f"{_fmt_array(diagnostic.semantic_x_world, precision=6)} " + f"semantic_y_world=" + f"{_fmt_array(diagnostic.semantic_y_world, precision=6)} " + f"semantic_z_world=" + f"{_fmt_array(diagnostic.semantic_z_world, precision=6)} " + f"local_offset_xyzw=" + f"{_fmt_array(diagnostic.local_offset_xyzw, precision=6)}" + ) + self._previous_orientation_debug_quaternions[side] = ( + diagnostic.world_quaternion_xyzw.copy(), + diagnostic.target_quaternion_xyzw.copy(), + ) + self._previous_orientation_debug_time_s = now_s def build_noitom_g1_locomanipulation_pipeline( @@ -608,18 +1363,15 @@ def _make_action( ) action[0:7] = targets.left_wrist.as_action_pose() action[7:14] = targets.right_wrist.as_action_pose() - hand_offset = 14 + action[14:21] = targets.torso.as_action_pose() + hand_offset = 21 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[21:28] = targets.left_elbow.as_action_pose() + action[28:35] = targets.right_elbow.as_action_pose() + action[35:42] = targets.left_shoulder.as_action_pose() + action[42:49] = targets.right_shoulder.as_action_pose() + hand_offset = 49 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 @@ -636,6 +1388,8 @@ def __init__(self, settings: NoitomG1Settings) -> None: 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_wrist_frames = settings.draw_wrist_frames + self._wrist_frame_axis_length = max(0.01, settings.wrist_frame_axis_length) self._draw_elbow_targets = ( settings.draw_elbow_targets and settings.use_arm_ik_frame_tasks ) @@ -656,7 +1410,7 @@ def update( return calib_view = retargeter.calibration_view - draw_positions = self._reference_positions(frame, calib_view) + draw_positions = self._reference_positions(frame, calib_view, retargeter) starts: list[list[float]] = [] ends: list[list[float]] = [] colors: list[tuple[float, float, float, float]] = [] @@ -693,6 +1447,23 @@ def update( colors.extend(wrist_colors) thicknesses.extend([_NOITOM_REFERENCE_LINE_THICKNESS] * len(wrist_starts)) + if self._draw_wrist_frames: + for side, wrist_frame in retargeter.reference_wrist_frames(frame).items(): + wrist_index = int( + BodyJoint.LEFT_WRIST if side == "left" else BodyJoint.RIGHT_WRIST + ) + position = draw_positions.get(wrist_index) + if position is None: + continue + axis_starts, axis_ends, axis_colors = self._coordinate_axes( + position.astype(np.float32), + wrist_frame.quaternion_xyzw, + ) + starts.extend(axis_starts) + ends.extend(axis_ends) + colors.extend(axis_colors) + thicknesses.extend([5.0] * len(axis_starts)) + if self._draw_elbow_targets: elbow_starts, elbow_ends, elbow_colors = self._frame_highlight_markers( draw_positions, @@ -740,7 +1511,8 @@ def update( "NoitomG1ActionSource: drawing Noitom reference skeleton " f"segments={len(starts)} joints={len(draw_positions)} " f"pelvis_relative={self._pelvis_relative} " - f"robot_pelvis_anchor={anchor}" + f"robot_pelvis_anchor={anchor}; BVH wrist axes " + "X=red Y=green Z=blue" ) self._printed_first_draw = True elif not self._warned: @@ -792,9 +1564,16 @@ def _reference_positions( self, frame: FullBodyPose, calib_view: Any | None, + retargeter: NoitomG1Retargeter, ) -> dict[int, np.ndarray]: if self._pelvis_relative: rt = self._retargeting + if rt.ik_config_path is not None: + positions = retargeter.reference_skeleton_positions(frame) + return { + index: (pos + self._offset).astype(np.float32) + for index, pos in positions.items() + } positions = aligned_reference_skeleton_from_frame( frame, self._pelvis_anchor, @@ -985,6 +1764,52 @@ def _fmt_vec(vec: np.ndarray) -> str: return "[" + ", ".join(f"{v:+.3f}" for v in vec) + "]" +def _fmt_quat(quat: np.ndarray) -> str: + return "[" + ", ".join(f"{v:+.4f}" for v in quat) + "]" + + +def _fmt_array(values: np.ndarray, *, precision: int) -> str: + return "[" + ", ".join(f"{value:+.{precision}f}" for value in values) + "]" + + +def _fmt_optional_float(value: float | None) -> str: + return "skipped" if value is None else f"{value:+.6f}" + + +def _fmt_pose_debug(pose: np.ndarray) -> str: + normalized = _normalized_pose_xyzw(pose) + return ( + f"pos_m={_fmt_array(normalized[:3], precision=6)} " + f"quat_xyzw={_fmt_array(normalized[3:7], precision=6)}" + ) + + +def _fmt_tensor(tensor: Any) -> str: + values = tensor.detach().cpu().tolist() + return "[" + ", ".join(f"{value:+.4f}" for value in values) + "]" + + +def _quaternion_matrix(quaternion_xyzw: np.ndarray) -> np.ndarray: + quat = np.asarray(quaternion_xyzw, dtype=np.float64) + quat /= max(float(np.linalg.norm(quat)), 1.0e-8) + x, y, z, w = quat + return np.array( + [ + [1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)], + [2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)], + [2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)], + ], + dtype=np.float64, + ) + + +def _quaternion_distance_deg(left: np.ndarray, right: np.ndarray) -> float: + left_normalized = left / max(float(np.linalg.norm(left)), 1.0e-8) + right_normalized = right / max(float(np.linalg.norm(right)), 1.0e-8) + dot = float(np.clip(abs(np.dot(left_normalized, right_normalized)), 0.0, 1.0)) + return float(np.rad2deg(2.0 * np.arccos(dot))) + + def _fmt_pose(pose: np.ndarray) -> str: return ( f"pos=[{pose[0]:+.3f}, {pose[1]:+.3f}, {pose[2]:+.3f}] " diff --git a/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp index bc1bbfbaf0..dc1bd45bb9 100644 --- a/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp @@ -30,6 +30,10 @@ namespace { constexpr float SDK_CENTIMETERS_TO_METERS = 0.01f; +constexpr size_t MAX_EVENT_BATCHES_PER_UPDATE = 64; +constexpr size_t MAX_EVENT_BYTES_PER_UPDATE = 4 * 1024 * 1024; +constexpr size_t MAX_EVENTS_PER_UPDATE = MAX_EVENT_BYTES_PER_UPDATE / sizeof(MocapApi::MCPEvent_t); +constexpr size_t MAX_EVENT_POLL_ATTEMPTS_PER_UPDATE = 128; 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"; @@ -161,6 +165,8 @@ std::string error_name(MocapApi::EMCPError err) return "None"; case MocapApi::Error_MoreEvent: return "MoreEvent"; + case MocapApi::Error_InsufficientBuffer: + return "InsufficientBuffer"; case MocapApi::Error_ServerNotReady: return "ServerNotReady"; case MocapApi::Error_ClientNotReady: @@ -267,6 +273,15 @@ void NoitomMocapPlugin::initialize_mocap() throw std::runtime_error("NoitomMocapPlugin: failed to configure SDK position units to centimeters"); } + const bool cache_events_enabled = open_mocap_application(); + + 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; +} + +bool NoitomMocapPlugin::open_mocap_application() +{ check_mocap(application_api_->CreateApplication(&application_handle_), "CreateApplication"); check_mocap( application_api_->SetApplicationSettings(settings_handle_, application_handle_), "SetApplicationSettings"); @@ -293,10 +308,26 @@ void NoitomMocapPlugin::initialize_mocap() { check_mocap(cache_err, "EnableApplicationCacheEvents"); } + return cache_events_enabled; +} - 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::reset_oversized_event_backlog(uint32_t pending_event_count) +{ + std::cerr << ANSI_ORANGE + << "NoitomMocapPlugin: warning: discarding oversized SDK event backlog; pending=" << pending_event_count + << " limit=" << MAX_EVENTS_PER_UPDATE << ANSI_RESET << std::endl; + + if (application_open_) + { + check_mocap(application_api_->CloseApplication(application_handle_), "CloseApplication(backlog reset)"); + application_open_ = false; + } + check_mocap(application_api_->DestroyApplication(application_handle_), "DestroyApplication(backlog reset)"); + application_handle_ = 0; + latest_sample_time_raw_device_clock_ns_ = 0; + warned_no_avatars_ = false; + open_mocap_application(); + application_reset_this_update_ = true; } void NoitomMocapPlugin::close_mocap() @@ -325,32 +356,137 @@ void NoitomMocapPlugin::close_mocap() } } -std::vector NoitomMocapPlugin::poll_events() +std::vector NoitomMocapPlugin::poll_updated_avatars() { - 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 updated_avatars; + size_t batch_count = 0; + size_t allocation_bytes_attempted = 0; + size_t event_count_processed = 0; + size_t poll_attempt_count = 0; + uint32_t last_pending_event_count = 0; + bool drained = false; + + while (poll_attempt_count < MAX_EVENT_POLL_ATTEMPTS_PER_UPDATE && batch_count < MAX_EVENT_BATCHES_PER_UPDATE && + event_count_processed < MAX_EVENTS_PER_UPDATE) + { + ++poll_attempt_count; + uint32_t event_count = 0; + MocapApi::EMCPError err = application_api_->PollApplicationNextEvent(nullptr, &event_count, application_handle_); + if (err == MocapApi::Error_MoreEvent || err == MocapApi::Error_InsufficientBuffer) + { + continue; + } + if (err != MocapApi::Error_None) + { + throw std::runtime_error("PollApplicationNextEvent(count): " + error_string(err)); + } + if (event_count == 0) + { + drained = true; + break; + } + const uint32_t pending_event_count = event_count; + last_pending_event_count = pending_event_count; + if (event_count > MAX_EVENTS_PER_UPDATE) + { + reset_oversized_event_backlog(event_count); + return {}; + } + if (event_count > MAX_EVENTS_PER_UPDATE - event_count_processed) + { + break; + } + const size_t batch_bytes = static_cast(event_count) * sizeof(MocapApi::MCPEvent_t); + if (batch_bytes > MAX_EVENT_BYTES_PER_UPDATE - allocation_bytes_attempted) + { + break; + } + allocation_bytes_attempted += batch_bytes; + + std::vector batch(event_count); + for (auto& event : batch) + { + event.size = sizeof(MocapApi::MCPEvent_t); + } + + err = application_api_->PollApplicationNextEvent(batch.data(), &event_count, application_handle_); + if (err == MocapApi::Error_MoreEvent || err == MocapApi::Error_InsufficientBuffer) + { + continue; + } + if (err != MocapApi::Error_None) + { + throw std::runtime_error("PollApplicationNextEvent(events): " + error_string(err)); + } + + batch.resize(event_count); + if (batch.empty()) + { + drained = true; + break; + } + + ++batch_count; + event_count_processed += batch.size(); + for (const auto& event : batch) + { + switch (event.eventType) + { + case MocapApi::MCPEvent_AvatarUpdated: + { + const auto avatar_handle = event.eventData.motionData.avatarHandle; + const auto existing = std::find(updated_avatars.begin(), updated_avatars.end(), avatar_handle); + if (existing != updated_avatars.end()) + { + updated_avatars.erase(existing); + } + updated_avatars.push_back(avatar_handle); + 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 (pending_event_count == 1) + { + drained = true; + break; + } } - std::vector events(event_count); - for (auto& event : events) + if (!drained) { - event.size = sizeof(MocapApi::MCPEvent_t); + if (!warned_event_drain_limit_) + { + std::cerr + << ANSI_ORANGE + << "NoitomMocapPlugin: warning: event drain budget reached; attempts=" << poll_attempt_count + << " batches=" << batch_count << " events=" << event_count_processed + << " allocated_bytes=" << allocation_bytes_attempted << " last_pending=" << last_pending_event_count + << "; publishing the latest avatar state without waiting for the backlog" << ANSI_RESET << std::endl; + warned_event_drain_limit_ = true; + } } - - err = application_api_->PollApplicationNextEvent(events.data(), &event_count, application_handle_); - if (err != MocapApi::Error_None && err != MocapApi::Error_MoreEvent) + else { - throw std::runtime_error("PollApplicationNextEvent(events): " + error_string(err)); + warned_event_drain_limit_ = false; } - events.resize(event_count); - return events; + + return updated_avatars; } std::vector NoitomMocapPlugin::poll_avatars() @@ -400,6 +536,7 @@ bool NoitomMocapPlugin::handle_avatar(MocapApi::MCPAvatarHandle_t avatar_handle) } else if (posture_time_err == MocapApi::Error_NoneMessage) { + latest_sample_time_raw_device_clock_ns_ = 0; warn_optional_ptp_missing_once(); } else @@ -527,32 +664,17 @@ bool NoitomMocapPlugin::update() { try { - auto events = poll_events(); + auto updated_avatars = poll_updated_avatars(); + if (application_reset_this_update_) + { + application_reset_this_update_ = false; + return true; + } bool should_push = false; - for (const auto& event : events) + for (auto avatar_handle : updated_avatars) { - 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; - } + should_push = handle_avatar(avatar_handle) || should_push; } if (!should_push) diff --git a/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp b/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp index 19ab6d89e1..907ecfbc88 100644 --- a/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.hpp @@ -57,8 +57,10 @@ class NoitomMocapPlugin InterfaceT* get_interface(const char* version); void initialize_mocap(); + bool open_mocap_application(); + void reset_oversized_event_backlog(uint32_t pending_event_count); void close_mocap(); - std::vector poll_events(); + std::vector poll_updated_avatars(); std::vector poll_avatars(); bool handle_avatar(MocapApi::MCPAvatarHandle_t avatar_handle); void ensure_pusher(size_t flatbuffer_size); @@ -78,7 +80,9 @@ class NoitomMocapPlugin MocapApi::MCPRenderSettingsHandle_t render_settings_handle_ = 0; MocapApi::MCPApplicationHandle_t application_handle_ = 0; bool application_open_ = false; + bool application_reset_this_update_ = false; bool warned_no_avatars_ = false; + bool warned_event_drain_limit_ = false; bool logged_first_avatar_frame_ = false; core::FullBodyPoseT frame_; From 464212c0dd120504cce107f74c59f1d9a3c7bf98 Mon Sep 17 00:00:00 2001 From: maji Date: Wed, 2 Sep 2026 18:19:11 +0800 Subject: [PATCH 2/2] fix(noitom): process partial event batches Signed-off-by: maji --- .../noitom_mocap/noitom_mocap_plugin.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp index dc1bd45bb9..91e907c1c0 100644 --- a/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp +++ b/src/plugins/noitom_mocap/noitom_mocap_plugin.cpp @@ -372,18 +372,18 @@ std::vector NoitomMocapPlugin::poll_updated_avatars ++poll_attempt_count; uint32_t event_count = 0; MocapApi::EMCPError err = application_api_->PollApplicationNextEvent(nullptr, &event_count, application_handle_); - if (err == MocapApi::Error_MoreEvent || err == MocapApi::Error_InsufficientBuffer) - { - continue; - } - if (err != MocapApi::Error_None) + if (err != MocapApi::Error_None && err != MocapApi::Error_MoreEvent && err != MocapApi::Error_InsufficientBuffer) { throw std::runtime_error("PollApplicationNextEvent(count): " + error_string(err)); } if (event_count == 0) { - drained = true; - break; + if (err == MocapApi::Error_None) + { + drained = true; + break; + } + continue; } const uint32_t pending_event_count = event_count; last_pending_event_count = pending_event_count; @@ -410,11 +410,11 @@ std::vector NoitomMocapPlugin::poll_updated_avatars } err = application_api_->PollApplicationNextEvent(batch.data(), &event_count, application_handle_); - if (err == MocapApi::Error_MoreEvent || err == MocapApi::Error_InsufficientBuffer) + if (err == MocapApi::Error_InsufficientBuffer) { continue; } - if (err != MocapApi::Error_None) + if (err != MocapApi::Error_None && err != MocapApi::Error_MoreEvent) { throw std::runtime_error("PollApplicationNextEvent(events): " + error_string(err)); } @@ -461,7 +461,7 @@ std::vector NoitomMocapPlugin::poll_updated_avatars } } - if (pending_event_count == 1) + if (err == MocapApi::Error_None && pending_event_count == 1) { drained = true; break;