diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 70173033..03c3d183 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -56,7 +56,7 @@ jobs: timeout-minutes: 20 # sphinx-multiversion runs `git worktree add` for each version, which # materializes the full repo at that SHA and triggers Git LFS smudging - # on sibling-package assets (parquet / pkl / npz under robotic_grounding/). + # on large sibling-package LFS assets (parquet / pkl / npz). # We never need those file contents to build docs -- skipping smudging # keeps them as LFS pointer files in the worktree. env: diff --git a/.github/workflows/robotic_grounding_ci.yml b/.github/workflows/robotic_grounding_ci.yml deleted file mode 100644 index 737a4f7e..00000000 --- a/.github/workflows/robotic_grounding_ci.yml +++ /dev/null @@ -1,111 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. -name: Robotic Grounding CI - -on: - push: - branches: [main] - paths: - - 'robotic_grounding/**' - - '.github/workflows/robotic_grounding_ci.yml' - pull_request: - branches: [main] - paths: - - 'robotic_grounding/**' - - '.github/workflows/robotic_grounding_ci.yml' - workflow_dispatch: - -# Cancel in-progress runs for the same PR -concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} - cancel-in-progress: true - -jobs: - ## ------------------------------------------ - ## Linting and formatting (pre-commit) - ## ------------------------------------------ - lint: - name: pre-commit - runs-on: ubuntu-latest - steps: - - name: Checkout Code - uses: actions/checkout@v4 - - name: Set up Python - uses: actions/setup-python@v5 - with: - python-version: "3.11" - - name: Install pre-commit - run: | - python -m pip install --upgrade pip - pip install pre-commit - - name: Cache pre-commit environments - uses: actions/cache@v4 - with: - path: ~/.cache/pre-commit - key: pre-commit-${{ hashFiles('robotic_grounding/.pre-commit-config.yaml') }} - restore-keys: | - pre-commit- - - name: Run pre-commit - run: | - cd robotic_grounding - pre-commit run --show-diff-on-failure --files $(git ls-files .) - - ## ------------------------------------------ - ## Build & Test (GPU required) - ## ------------------------------------------ - build_and_test: - name: Build & Test - needs: [lint] - runs-on: [self-hosted, gpu] - timeout-minutes: 180 - env: - DOCKER_IMAGE: robotic-grounding-ci-${{ github.run_id }} - NGC_API_KEY: ${{ secrets.NGC_API_KEY }} - steps: - - name: Checkout Code - uses: actions/checkout@v4 - with: - lfs: true - - name: NGC Login - run: | - if [ -n "$NGC_API_KEY" ]; then - echo "Logging into NGC registry..." - docker login -u \$oauthtoken -p $NGC_API_KEY nvcr.io - echo "Successfully logged into NGC registry" - else - echo "NGC_API_KEY not available - skipping NGC login" - fi - - name: Build Docker image - run: | - echo "::group::Build Docker image" - time docker build -f robotic_grounding/workflow/Dockerfile -t $DOCKER_IMAGE ./robotic_grounding - echo "::endgroup::" - - name: Run E2E tests - run: | - echo "::group::E2E Tests" - docker run --name robotic-grounding-e2e-${{ github.run_id }} \ - --entrypoint bash --gpus all --network=host \ - -v ${{ github.workspace }}/robotic_grounding/tests:/workspace/video_to_data/robotic_grounding/tests \ - -e ACCEPT_EULA=Y \ - -e OMNI_KIT_ACCEPT_EULA=yes \ - -e OMNI_KIT_DISABLE_CUP=1 \ - -e ISAAC_SIM_HEADLESS=1 \ - -e ISAAC_SIM_LOW_MEMORY=1 \ - -e WANDB_MODE=disabled \ - -e CI=true \ - -e PYTHONUNBUFFERED=1 \ - $DOCKER_IMAGE \ - -c "\${ISAACLAB_PATH}/isaaclab.sh -p -m pytest \ - /workspace/video_to_data/robotic_grounding/tests/test_train_e2e.py \ - /workspace/video_to_data/robotic_grounding/tests/test_retarget_pipeline_e2e.py -v" - echo "::endgroup::" - - name: Cleanup - if: always() - run: | - docker rm robotic-grounding-e2e-${{ github.run_id }} 2>/dev/null || true - docker rmi $DOCKER_IMAGE 2>/dev/null || true diff --git a/README.md b/README.md index b1d46215..bb867089 100644 --- a/README.md +++ b/README.md @@ -8,8 +8,8 @@ Monorepo for **Video to Data (V2D)** — an end-to-end pipeline that converts hu ┌───────────────┐ ┌──────────────────────┐ ┌──────────────────────┐ ┌────────────────────────────┐ │ Human demo │ → │ 1. Video Ingestion │ → │ 2. Reconstruction │ → │ 3. Robotic Grounding │ │ video / rosbag│ │ Agent │ │ depth · masks · │ │ retargeting → Isaac Lab │ - │ │ │ action segments · │ │ meshes · 6D pose · │ │ RL training (RSL-RL PPO) │ - │ │ │ entity graph · │ │ SMPL body │ │ │ + │ │ │ action segments · │ │ meshes · 6D pose · │ │ RL training │ + │ │ │ entity graph · │ │ body pose │ │ Coming soon │ │ │ │ visual embeddings │ │ │ │ │ └───────────────┘ └──────────────────────┘ └──────────────────────┘ └────────────────────────────┘ video_ingestion_agent/ reconstruction/ robotic_grounding/ @@ -17,7 +17,7 @@ Monorepo for **Video to Data (V2D)** — an end-to-end pipeline that converts hu 1. **Video Ingestion Agent** — a LangGraph-driven agentic workflow that segments demonstration videos into temporally-bounded action clips, extracts an entity-relation scene graph, and stores per-frame SigLIP-2 embeddings. The result is a queryable action database (`graph.db` + `vector.db`) that lets downstream stages select which clips to process via natural-language retrieval, instead of brute-forcing the full video. 2. **Reconstruction** — containerized vision modules turn the selected RGB (or stereo) clips into per-frame depth, object masks, textured meshes, 6-DoF object poses, and SMPL human body parameters. Multi-view pipelines (`run_mv_hoi_reconstruction`, `run_mv_calibration`) orchestrate the full reconstruction from a rosbag. -3. **Robotic Grounding** — human motion (e.g. Arctic) is retargeted onto the target robot embodiment (Sharpa), then the reconstructed scene and retargeted motion drive Isaac Lab environments trained with RSL-RL PPO to produce deployable policies. +3. **Robotic Grounding** — human motion is retargeted onto the target robot embodiment (Sharpa or G1), then the reconstructed scene and retargeted motion drive Isaac Lab environments trained with RL to produce deployable policies. ## Packages @@ -25,14 +25,13 @@ Monorepo for **Video to Data (V2D)** — an end-to-end pipeline that converts hu |---|---|---| | [`video_ingestion_agent/`](video_ingestion_agent/) | Video → action segments + entity scene graph + frame embeddings. LangGraph pipeline (segment → verify/refine → entity graph → embeddings) plus an EGAgent-style natural-language retrieval agent and an optional Gradio UI. | Python venv + vLLM server | | [`reconstruction/`](reconstruction/) | Video → depth, masks, meshes, 6D poses, human body. 18 containerized modules + multi-view pipelines. | Docker (per-module images) | -| [`robotic_grounding/`](robotic_grounding/) | RL training on NVIDIA Isaac Lab 2.3.1 with RSL-RL (PPO); motion retargeting utilities. | Docker (`nvcr.io/nvstaging/isaac-amr`) | +| [`robotic_grounding/`](robotic_grounding/) | RL training on NVIDIA Isaac Lab with PPO; motion retargeting utilities. **Code will be published in a later release.** | Coming soon | ## Prerequisites - Docker with GPU support ([install](https://docs.docker.com/engine/install/ubuntu/)) - [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) - Python 3.10+ -- NVIDIA driver 580.126.09 / CUDA 13.0 recommended (for `robotic_grounding`) ## Quickstart @@ -95,27 +94,9 @@ See [reconstruction/README.md](reconstruction/README.md) for the complete module ### Robotic Grounding (data → RL policy) -```bash -cd robotic_grounding - -# One-time host setup (git-lfs, pre-commit) -bash workflow/setup_deps.sh - -# Build + enter the Isaac Lab container -./workflow/run.sh build [version] -./workflow/run.sh start [version] [gpu_id] - -# Inside the container — train a policy -python scripts/rsl_rl/train.py --task Sharpa-V2P-v0 -``` - -See [robotic_grounding/README.md](robotic_grounding/README.md) for retargeting, debug environments, and task definitions. - -### Visualizer (retargeting gallery) - -Browse retargeted sequences as 3D animations at **http://10.111.83.14:8080/** +The Robotic Grounding stage (motion retargeting + Isaac Lab RL training) will be publically available in a later release. See [robotic_grounding/README.md](robotic_grounding/README.md) for an overview. -See [robotic_grounding/README.md#visualizer](robotic_grounding/README.md#visualizer) for setup instructions. +The technical report and project website are available on the project page: [Webpage](https://nvidia-isaac.github.io/video_to_data/chord/). ## Design Philosophy diff --git a/reconstruction/modules/v2d_task_library_loader/.gitignore b/reconstruction/modules/v2d_task_library_loader/.gitignore deleted file mode 100644 index bb868b37..00000000 --- a/reconstruction/modules/v2d_task_library_loader/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# robotic_grounding python package staged into the build context by docker/build.py -# (assets excluded). Regenerated each build; never committed. -_rg_pkg/ diff --git a/reconstruction/modules/v2d_task_library_loader/docker/Dockerfile b/reconstruction/modules/v2d_task_library_loader/docker/Dockerfile deleted file mode 100644 index 2fd92755..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/Dockerfile +++ /dev/null @@ -1,36 +0,0 @@ -FROM pytorch/pytorch:2.5.1-cuda12.4-cudnn9-devel -ENV DEBIAN_FRONTEND=noninteractive -WORKDIR /app - -RUN apt-get update && apt-get install -y \ - libgl1 \ - libegl1-mesa \ - libglib2.0-0 \ - ffmpeg \ - git \ - && rm -rf /var/lib/apt/lists/* - -COPY . /workspace - -# MANO model layers (GPL-3.0) — contained in this image only; never installed -# into the robotic_grounding training image. The MANO .pkl models themselves are -# NOT vendored; they are bind-mounted at runtime via --mano_model_dir. -RUN pip install opencv-python-headless \ - deprecation \ - git+https://github.com/mattloper/chumpy \ - git+https://github.com/lixiny/manotorch - -# pytorch3d (used by distance_utils for faster mesh surface sampling) — built -# against the environment torch, so --no-build-isolation. Best-effort: distance_utils -# falls back to trimesh sampling if it's unavailable, so a build-time failure here -# must not fail the image. -RUN pip install --no-build-isolation "git+https://github.com/facebookresearch/pytorch3d.git" \ - || echo "WARNING: pytorch3d install failed; distance_utils will use the trimesh fallback" - -RUN pip install -e /workspace/v2d_common -e /workspace/v2d_task_library_loader/lib - -# robotic_grounding python package (assets excluded) — staged into this module's -# build context by build.py (monorepo: RG lives outside modules/). Provides the -# GPL-clean ManoSharpaData/ManoDex3Data schema + params + quat_from_matrix + naming. -# PYTHONPATH (not pip install) avoids bundling RG's large package_data (assets) into a wheel. -ENV PYTHONPATH=/workspace/v2d_task_library_loader/_rg_pkg diff --git a/reconstruction/modules/v2d_task_library_loader/docker/__init__.py b/reconstruction/modules/v2d_task_library_loader/docker/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/reconstruction/modules/v2d_task_library_loader/docker/_config.py b/reconstruction/modules/v2d_task_library_loader/docker/_config.py deleted file mode 100644 index f2361740..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/_config.py +++ /dev/null @@ -1,6 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import os - -IMAGE_NAME = "v2d_task_library_loader" -MODULES_DIR = os.path.abspath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "..")) diff --git a/reconstruction/modules/v2d_task_library_loader/docker/build.py b/reconstruction/modules/v2d_task_library_loader/docker/build.py deleted file mode 100644 index 75e2b25b..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/build.py +++ /dev/null @@ -1,57 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import os -import shutil -import subprocess - -IMAGE_NAME = "v2d_task_library_loader" - -current_dir = os.path.dirname(os.path.abspath(__file__)) -module_dir = os.path.join(current_dir, "..") # v2d_task_library_loader/ -root_dir = os.path.join(module_dir, "..") # reconstruction/modules/ (build context) -dockerfile_path = os.path.join(current_dir, "Dockerfile") - -# robotic_grounding's python package lives outside the modules/ build context -# (monorepo). Stage a copy WITHOUT its large assets dir into the build context so -# the image can import the schema/params/utils; only the python package is -# needed. Staged dir is git-ignored; the Dockerfile puts it on PYTHONPATH. -_RG_PKG_SRC = os.path.abspath( - os.path.join( - root_dir, "..", "..", "robotic_grounding", - "source", "robotic_grounding", "robotic_grounding", - ) -) -_RG_STAGE = os.path.join(module_dir, "_rg_pkg", "robotic_grounding") - - -def stage_robotic_grounding() -> None: - """Copy RG's python package (NO assets) into the build context. - - Object assets (urdfs/meshes) are NOT baked — the load workflow fetches the - per-dataset object assets from swift at runtime (kept lean; see the OSMO load - workflow). human_motion_data (raw) and body_models (MANO, licensed) likewise - come in at runtime, never in the image. - """ - stage_root = os.path.join(module_dir, "_rg_pkg") - if os.path.exists(stage_root): - shutil.rmtree(stage_root) - if not os.path.isdir(_RG_PKG_SRC): - raise FileNotFoundError( - f"robotic_grounding package not found at {_RG_PKG_SRC}; " - "expected the monorepo layout video_to_data/{robotic_grounding,reconstruction}." - ) - shutil.copytree( - _RG_PKG_SRC, - _RG_STAGE, - ignore=shutil.ignore_patterns("assets", "*.egg-info", "__pycache__", "*.pyc"), - symlinks=True, - ) - - -def build_docker_image() -> None: - stage_robotic_grounding() - subprocess.run(["docker", "build", "-t", IMAGE_NAME, "-f", dockerfile_path, root_dir], check=True) - - -if __name__ == "__main__": - build_docker_image() diff --git a/reconstruction/modules/v2d_task_library_loader/docker/pyproject.toml b/reconstruction/modules/v2d_task_library_loader/docker/pyproject.toml deleted file mode 100644 index b32a22b4..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/pyproject.toml +++ /dev/null @@ -1,18 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -[build-system] -requires = ["setuptools>=68.0", "wheel"] -build-backend = "setuptools.build_meta" - -[project] -name = "v2d-task-library-loader-docker" -version = "0.1.0" -description = "Docker orchestration for the task-library dataset loaders" -requires-python = ">=3.10" -dependencies = ["v2d-docker"] - -[tool.setuptools] -packages = ["v2d.task_library_loader.docker"] - -[tool.setuptools.package-dir] -"v2d.task_library_loader.docker" = "." diff --git a/reconstruction/modules/v2d_task_library_loader/docker/run_loader.py b/reconstruction/modules/v2d_task_library_loader/docker/run_loader.py deleted file mode 100644 index 167bdd57..00000000 --- a/reconstruction/modules/v2d_task_library_loader/docker/run_loader.py +++ /dev/null @@ -1,108 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Host wrapper: run a hand-object dataset loader (Stage 1 / MANO FK) in-container. - -Mounts the (separately-licensed, never-vendored) MANO model dir, the raw -human-motion-data dir, and the output dir, then runs the module-dispatch loader. -The container produces the ``${dataset}_loaded`` Parquet (ManoSharpaData with -MANO + object only) consumed downstream by robotic_grounding's IK retarget. -""" -import os - -from v2d.docker.container import run_in_container -from v2d.task_library_loader.docker._config import IMAGE_NAME, MODULES_DIR - - -def run_loader( - dataset: str, - output_dir: str, - mano_model_dir: str, - human_motion_data_dir: str, - object_assets_dir: str | None = None, - device: str = "cuda:0", - save: bool = True, - sequence_pattern: str | None = None, - sequence_id: str | None = None, - max_sequences: int | None = None, - shard_id: int | None = None, - num_shards: int | None = None, - dev: bool = False, -) -> None: - extra_volumes = [ - f"{os.path.abspath(human_motion_data_dir)}:/data/human_motion_data" - ] - extra_args: dict[str, object] = { - "dataset": dataset, - "device": device, - "save": save, - "sequence_pattern": sequence_pattern, - "sequence_id": sequence_id, - "max_sequences": max_sequences, - "shard_id": shard_id, - "num_shards": num_shards, - } - - # Object assets (rigid URDFs + meshes). Mount the root containing - # `urdfs//` and `meshes//` as ONE volume so the URDFs' - # relative `../../meshes//…` refs resolve (MuJoCo loads arctic's - # articulated URDF by path and follows those refs). - if object_assets_dir is not None: - extra_volumes.append( - f"{os.path.abspath(object_assets_dir)}:/data/object_assets" - ) - extra_args["object_model_root"] = f"/data/object_assets/urdfs/{dataset}" - extra_args["mesh_dir"] = f"/data/object_assets/meshes/{dataset}" - - run_in_container( - image=IMAGE_NAME, - module="v2d.task_library_loader.lib.run_loader", - inputs={"mano_model_dir": mano_model_dir}, - outputs={"output_dir": output_dir}, - extra_args=extra_args, - # The loaders read raw inputs from $HUMAN_MOTION_DATA_DIR//... - env={"HUMAN_MOTION_DATA_DIR": "/data/human_motion_data"}, - extra_volumes=extra_volumes, - dev=dev, - modules_dir=MODULES_DIR, - gpus=True, - ) - - -if __name__ == "__main__": - import argparse - - parser = argparse.ArgumentParser() - parser.add_argument("--dataset", required=True) - parser.add_argument("--output_dir", required=True) - parser.add_argument("--mano_model_dir", required=True) - parser.add_argument("--human_motion_data_dir", required=True) - parser.add_argument( - "--object_assets_dir", - default=None, - help="Root holding urdfs// + meshes// (mounted as one " - "volume so URDF '../../meshes/...' refs resolve). Omit for h2o/grab/dexycb.", - ) - parser.add_argument("--device", default="cuda:0") - parser.add_argument("--save", action="store_true", default=True) - parser.add_argument("--sequence_pattern", default=None) - parser.add_argument("--sequence_id", default=None) - parser.add_argument("--max_sequences", type=int, default=None) - parser.add_argument("--shard_id", type=int, default=None) - parser.add_argument("--num_shards", type=int, default=None) - parser.add_argument("--dev", action="store_true") - args = parser.parse_args() - run_loader( - dataset=args.dataset, - output_dir=args.output_dir, - mano_model_dir=args.mano_model_dir, - human_motion_data_dir=args.human_motion_data_dir, - object_assets_dir=args.object_assets_dir, - device=args.device, - save=args.save, - sequence_pattern=args.sequence_pattern, - sequence_id=args.sequence_id, - max_sequences=args.max_sequences, - shard_id=args.shard_id, - num_shards=args.num_shards, - dev=args.dev, - ) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/__init__.py b/reconstruction/modules/v2d_task_library_loader/lib/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/reconstruction/modules/v2d_task_library_loader/lib/arctic_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/arctic_loader.py deleted file mode 100644 index bbafd474..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/arctic_loader.py +++ /dev/null @@ -1,282 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load ARCTIC dataset into ManoSharpaData schema (MANO + object only, no robot). - -Saves Parquet under human_motion_data/arctic_loaded/mano_object_only. Use -arctic_to_sharpa.py to retarget the saved data to Sharpa robot and write -arctic_processed. -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import mujoco -import numpy as np -import torch -from robotic_grounding.retarget import ( - ASSETS_DIR, - HUMAN_MOTION_DATA_DIR, - MESHES_DIR, -) -from v2d.task_library_loader.lib.dataset_loader_base import ( - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, -) -from scipy.spatial.transform import Rotation as R - -# Suppress warnings about joint limits being slightly out of bounds -logging.getLogger().setLevel(logging.ERROR) - -# ARCTIC paths (defaults; overridable via CLI args) -ARCTIC_MOTION_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "dataset" -ARCTIC_URDF_DIR = ASSETS_DIR / "urdfs" / "arctic" -ARCTIC_MESH_DIR = MESHES_DIR / "arctic" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_loaded" - -OBJECT_BODY_NAMES = ["bottom", "top"] -FRAME_START = 0 -FRAME_END_OFFSET = 0 -ARCTIC_FPS = 30.0 - -ARCTIC_MANO_KWARGS = {"flat_hand_mean": False, "center_idx": None} - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Load ARCTIC sequences into ManoSharpaData schema (MANO + object only)." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=ARCTIC_MOTION_DIR, - object_model_root=ARCTIC_URDF_DIR, - mesh_dir=ARCTIC_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - return parser.parse_args() - - -def load_arctic_mano_data(mano_data_file: Path, device: torch.device) -> dict[str, Any]: - """Load MANO parameters from an ARCTIC .mano.npy file.""" - mano_data = np.load(mano_data_file, allow_pickle=True).item() - return { - "right_global_orient": torch.from_numpy(mano_data["right"]["rot"]).to(device), - "right_finger_pose": torch.from_numpy(mano_data["right"]["pose"]).to(device), - "right_trans": torch.from_numpy(mano_data["right"]["trans"]).to(device), - "right_betas": torch.from_numpy(mano_data["right"]["shape"]).to(device), - "right_fitting_err": torch.tensor(mano_data["right"]["fitting_err"]).to(device), - "left_global_orient": torch.from_numpy(mano_data["left"]["rot"]).to(device), - "left_finger_pose": torch.from_numpy(mano_data["left"]["pose"]).to(device), - "left_trans": torch.from_numpy(mano_data["left"]["trans"]).to(device), - "left_betas": torch.from_numpy(mano_data["left"]["shape"]).to(device), - "left_fitting_err": torch.tensor(mano_data["left"]["fitting_err"]).to(device), - "H": len(mano_data["right"]["rot"]), - } - - -def load_arctic_object_data( - mano_data_path: str, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Load object pose and articulation from ARCTIC .object.npy.""" - object_path = str(mano_data_path).replace(".mano.", ".object.") - object_data = np.load(object_path, allow_pickle=True) - object_articulation = object_data[:, 0] - object_axis_angle = object_data[:, 1:4] - object_translation = object_data[:, 4:] / 1000.0 - return object_articulation, object_axis_angle, object_translation - - -def setup_arctic_mujoco_object( - object_name: str, - urdf_dir: Path | None = None, -) -> tuple[mujoco.MjModel, mujoco.MjData, int, int]: - """Load MuJoCo model for ARCTIC object; return body/joint ids for FK.""" - object_urdf_path = (urdf_dir or ARCTIC_URDF_DIR) / f"{object_name}.urdf" - model = mujoco.MjModel.from_xml_path(str(object_urdf_path)) - data = mujoco.MjData(model) - top_body_id = model.body("top").id - rotation_joint_id = model.joint("rotation").id - return model, data, top_body_id, rotation_joint_id - - -def get_arctic_object_world_transforms( - frame_id: int, - object_articulation: np.ndarray, - object_axis_angle: np.ndarray, - object_translation: np.ndarray, - mj_model: mujoco.MjModel, - mj_data: mujoco.MjData, - top_body_id: int, - rotation_joint_id: int, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Compute world position and quat (wxyz) for bottom and top at one frame.""" - world_p_bottom = object_translation[frame_id] - world_q_bottom = R.from_rotvec(object_axis_angle[frame_id]).as_quat( - scalar_first=True - ) - world_t_bottom = np.eye(4) - world_t_bottom[:3, :3] = R.from_quat(world_q_bottom, scalar_first=True).as_matrix() - world_t_bottom[:3, 3] = world_p_bottom - - mj_data.qpos[rotation_joint_id] = object_articulation[frame_id] - mujoco.mj_forward(mj_model, mj_data) - bottom_t_top = np.eye(4) - bottom_t_top[:3, :3] = np.array(mj_data.xmat[top_body_id]).reshape(3, 3) - bottom_t_top[:3, 3] = mj_data.xpos[top_body_id] - world_t_top = world_t_bottom @ bottom_t_top - world_p_top = world_t_top[:3, 3] - world_q_top = R.from_matrix(world_t_top[:3, :3]).as_quat(scalar_first=True) - - world_p_objects = np.vstack([world_p_bottom, world_p_top]) - world_q_objects = np.vstack([world_q_bottom, world_q_top]) - return ( - world_p_bottom, - world_q_bottom, - world_p_top, - world_q_top, - world_p_objects, - world_q_objects, - ) - - -def _arctic_world_poses_one_frame( - frame_id: int, - object_articulation: np.ndarray, - object_axis_angle: np.ndarray, - object_translation: np.ndarray, - mj_model: mujoco.MjModel, - mj_data: mujoco.MjData, - top_body_id: int, - rotation_joint_id: int, -) -> tuple[np.ndarray, np.ndarray]: - """Return (world_t_bottom, world_t_top) 4x4 for one frame.""" - world_t_bottom = np.eye(4) - world_q_bottom = R.from_rotvec(object_axis_angle[frame_id]).as_quat( - scalar_first=True - ) - world_t_bottom[:3, :3] = R.from_quat(world_q_bottom, scalar_first=True).as_matrix() - world_t_bottom[:3, 3] = object_translation[frame_id] - mj_data.qpos[rotation_joint_id] = object_articulation[frame_id] - mujoco.mj_forward(mj_model, mj_data) - bottom_t_top = np.eye(4) - bottom_t_top[:3, :3] = np.array(mj_data.xmat[top_body_id]).reshape(3, 3) - bottom_t_top[:3, 3] = mj_data.xpos[top_body_id] - world_t_top = world_t_bottom @ bottom_t_top - return world_t_bottom, world_t_top - - -class ArcticDatasetLoader(DatasetLoaderBase): - """ARCTIC dataset loader.""" - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """List ARCTIC sequences (discover *.mano.npy, exclude scissor).""" - dataset_root = Path(getattr(args, "dataset_root", ARCTIC_MOTION_DIR)) - mano_files = sorted( - [f for f in dataset_root.glob("*/*.mano.npy") if "scissor" not in f.name] - ) - out = [] - for mano_data_file in mano_files: - object_name = mano_data_file.name.split("_")[0] - raw_motion_file = str(Path(*mano_data_file.parts[-3:]))[:-9] - sequence_id = raw_motion_file.replace("/", "_") - out.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=raw_motion_file, - object_name=object_name, - object_body_names=OBJECT_BODY_NAMES, - source=mano_data_file, - ) - ) - return out - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO parameters from ARCTIC .mano.npy for the sequence.""" - mano_data_file = sequence_info.source - if mano_data_file is None: - raise FileNotFoundError("ARCTIC sequence has no source path") - return load_arctic_mano_data(Path(mano_data_file), device) - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object data: name -> (pose, root_position, root_axis_angle, articulation).""" - mano_data_file = sequence_info.source - if mano_data_file is None: - raise FileNotFoundError("ARCTIC sequence has no source path") - art, axis, trans = load_arctic_object_data(str(mano_data_file)) - urdf_dir = Path(getattr(self._args, "object_model_root", ARCTIC_URDF_DIR)) - mj_model, mj_data, top_id, rot_id = setup_arctic_mujoco_object( - sequence_info.object_name, urdf_dir=urdf_dir - ) - n_frames = len(art) - bottom_poses = np.zeros((n_frames, 4, 4), dtype=np.float64) - top_poses = np.zeros((n_frames, 4, 4), dtype=np.float64) - for i in range(n_frames): - world_t_bottom, world_t_top = _arctic_world_poses_one_frame( - i, art, axis, trans, mj_model, mj_data, top_id, rot_id - ) - bottom_poses[i] = world_t_bottom - top_poses[i] = world_t_top - # Root (bottom) has articulation; top uses same root_position/root_axis_angle - return { - OBJECT_BODY_NAMES[0]: (bottom_poses, trans, axis, art), - OBJECT_BODY_NAMES[1]: (top_poses, trans, axis, None), - } - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple[ - dict[str, Any], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - bool, - ]: - """Load ARCTIC object part meshes (bottom/top) for the sequence object.""" - mesh_dir = Path(getattr(self._args, "mesh_dir", ARCTIC_MESH_DIR)) - mesh_paths = { - part: str( - mesh_dir / sequence_info.object_name / f"{part}_watertight_tiny.obj" - ) - for part in sequence_info.object_body_names - } - return load_meshes_to_device(mesh_paths, device) - - def get_mano_kwargs(self) -> dict[str, Any]: - """Return MANO model kwargs for ARCTIC (flat_hand_mean=False).""" - return ARCTIC_MANO_KWARGS - - def get_fps(self) -> float: - """Return ARCTIC sequence FPS.""" - return ARCTIC_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to ARCTIC object part meshes (bottom/top watertight_tiny.obj).""" - mesh_dir = Path(getattr(self._args, "mesh_dir", ARCTIC_MESH_DIR)) - return [ - str(mesh_dir / sequence_info.object_name / f"{part}_watertight_tiny.obj") - for part in sequence_info.object_body_names - ] - - def get_frame_range(self, num_frames: int) -> tuple[int, int]: - """Return (start, end) frame indices; ARCTIC trims first/last frames.""" - return FRAME_START, num_frames - FRAME_END_OFFSET - - -def main(args: argparse.Namespace) -> None: - """Load ARCTIC sequences and save as ManoSharpaData (MANO + object only).""" - loader = ArcticDatasetLoader() - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/contact_utils.py b/reconstruction/modules/v2d_task_library_loader/lib/contact_utils.py deleted file mode 100644 index ba631b0a..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/contact_utils.py +++ /dev/null @@ -1,200 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Contact computation utilities for MANO hand–object contact (dexmachina format).""" - -import torch - -from robotic_grounding.retarget.params import MANO_HAND_LINKS, NUM_MANO_LINKS - - -def approximate_contact_with_id( - object_surface_points_world: torch.Tensor, - object_surface_normals_world: torch.Tensor, - object_surface_points_part_ids: torch.Tensor, - hand_verts: torch.Tensor, - hand_normals: torch.Tensor, - threshold: float = 0.01, - dist_min: float = 0.0, - dist_max: float = 100.0, -) -> tuple[ - torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor -]: - """Find contact points between object and hand with object part IDs. - - For each object vertex, find the closest hand vertex. - - Args: - object_surface_points_world: Object surface points in world frame (N, 3). - object_surface_normals_world: Object surface normals in world frame (N, 3), pointing inward. - object_surface_points_part_ids: Part ID per object surface point (N,). - hand_verts: Hand mesh vertices in world frame (M, 3). - hand_normals: Hand mesh normals in world frame (M, 3), pointing outward. - threshold: Max distance for a pair to count as contact. - dist_min: Lower clip for distances. - dist_max: Upper clip for distances. - - Returns: - object_contact_points_world: (num_contact, 3) — world_xyz. - object_contact_normals_world: (num_contact, 3) — world_normal. - object_contact_part_ids: (num_contact,) — part_id. - hand_contact_points_world: (num_contact, 3) — world_xyz. - hand_contact_normals_world: (num_contact, 3) — world_normal. - contact_dists: (num_contact,) — distance from object to hand. - """ - dists = torch.clamp( - torch.cdist(object_surface_points_world, hand_verts), min=dist_min, max=dist_max - ) # (N, M) - - object_to_hand_closest_dist = dists.amin(dim=-1) # (N,) - contact_mask = object_to_hand_closest_dist < threshold # (N,) - - closest_hand_idx = dists.argmin(dim=-1) # (N,) - closet_hand_verts = hand_verts[closest_hand_idx] # (N, 3) - closet_hand_normals = hand_normals[closest_hand_idx] # (N, 3) - - if contact_mask.sum() == 0: - return ( - torch.zeros((0, 3), device=object_surface_points_world.device), - torch.zeros((0, 3), device=object_surface_points_world.device), - torch.zeros((0, 1), device=object_surface_points_world.device), - torch.zeros((0, 3), device=object_surface_points_world.device), - torch.zeros((0, 3), device=object_surface_points_world.device), - torch.zeros((0, 1), device=object_surface_points_world.device), - ) - - object_contact_points_world = object_surface_points_world[contact_mask] - object_contact_normals_world = object_surface_normals_world[contact_mask] - object_contact_part_ids = object_surface_points_part_ids[contact_mask] - - hand_contact_points_world = closet_hand_verts[contact_mask] - hand_contact_normals_world = closet_hand_normals[contact_mask] - contact_dists = object_to_hand_closest_dist[contact_mask] - - return ( - object_contact_points_world, - object_contact_normals_world, - object_contact_part_ids, - hand_contact_points_world, - hand_contact_normals_world, - contact_dists, - ) - - -def compute_hand_link_contact_positions( - joint_points: torch.Tensor, - object_contact_part_ids: torch.Tensor, - hand_contact_points_world: torch.Tensor, - hand_contact_normals_world: torch.Tensor, - contact_dists: torch.Tensor, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - """Assign contact points to MANO links and compute one contact position and part_id per link. - - For each link, average distance from each contact to the link's joints is - computed; each contact is assigned to the closest link. For each link with - at least one contact, returns weighted average position (by inverse - distance) and voted part_id. Links with no contact get zeros (4,). - - Args: - joint_points: (21, 3) MANO joint positions in world frame. - object_contact_part_ids: (N,) object part IDs with values 1 or 2. - hand_contact_points_world: (N, 3) hand contact points in world frame. - hand_contact_normals_world: (N, 3) hand contact normals in world frame. - contact_dists: (N,) distances from object to hand. - - Returns: - hand_link_contact_positions: (NUM_MANO_LINKS, 3) contact positions in world frame. - hand_link_contact_normals: (NUM_MANO_LINKS, 3) contact normals in world frame. - hand_link_contact_part_ids: (NUM_MANO_LINKS,) contact part IDs. - """ - # Compute MANO link position by averaging the joint positions. - hand_link_positions = torch.zeros( - (NUM_MANO_LINKS, 3), device=joint_points.device - ) # (NUM_MANO_LINKS, 3) - for link_idx, joint_idxs in enumerate(MANO_HAND_LINKS.values()): - hand_link_positions[link_idx] = joint_points[joint_idxs].mean(dim=0) # (3,) - - # Compute distance from contact points to link - dists = torch.cdist( - hand_contact_points_world, hand_link_positions - ) # (N, NUM_MANO_LINKS) - closest_link_idx = dists.argmin(dim=-1) # (N,) - - # Average contact positions and vote part id - hand_link_contact_positions = torch.zeros( - (NUM_MANO_LINKS, 3), device=joint_points.device - ) # (NUM_MANO_LINKS, 3) - hand_link_contact_normals = torch.zeros( - (NUM_MANO_LINKS, 3), device=joint_points.device - ) # (NUM_MANO_LINKS, 3) - hand_link_contact_part_ids = torch.zeros( - (NUM_MANO_LINKS,), device=joint_points.device - ) # (NUM_MANO_LINKS,) - - for link_idx in range(NUM_MANO_LINKS): - mask = closest_link_idx == link_idx - if not torch.any(mask): - continue - # Average contact positions by inverse distance - weights = torch.nn.functional.softmax(-contact_dists[mask], dim=-1) - hand_link_contact_positions[link_idx] = ( - hand_contact_points_world[mask] * weights[:, None] - ).sum(dim=0) - hand_link_contact_normals[link_idx] = ( - hand_contact_normals_world[mask] * weights[:, None] - ).sum(dim=0) - hand_link_contact_normals[link_idx] /= hand_link_contact_normals[ - link_idx - ].norm() - - # Vote part id - part_ids = object_contact_part_ids[mask] - voted_part_id = part_ids.mode().values.item() - hand_link_contact_part_ids[link_idx] = voted_part_id - - return ( - hand_link_contact_positions, - hand_link_contact_normals, - hand_link_contact_part_ids, - ) - - -def find_object_contact_positions( - hand_link_contact_positions: torch.Tensor, - object_surface_points: torch.Tensor, - object_surface_normals: torch.Tensor, -) -> tuple[torch.Tensor, torch.Tensor]: - """Assign contact points to object vertices and normals per contact. - - The object contact positions are the closest object vertices to the hand link contact positions that - has surface normal pointing towards the hand link contact positions. - - Args: - hand_link_contact_positions: (NUM_MANO_LINKS, 3) link contact positions in world frame. - object_surface_points: (N, 3) object vertices in world frame. - object_surface_normals: (N, 3) object normals in world frame. - - Returns: - object_contact_positions: (NUM_MANO_LINKS, 3) object contact positions in world frame. - object_contact_normals: (NUM_MANO_LINKS, 3) object contact normals in world frame. - """ - dists = torch.cdist( - hand_link_contact_positions, object_surface_points - ) # (NUM_MANO_LINKS, N) - - closest_object_idx = dists.argmin(dim=-1) # (NUM_MANO_LINKS,) - object_contact_positions = object_surface_points[ - closest_object_idx - ] # (NUM_MANO_LINKS, 3) - object_contact_normals = object_surface_normals[ - closest_object_idx - ] # (NUM_MANO_LINKS, 3) - - # Set invalid contacts to zero. - invalid_contact_mask = ( - hand_link_contact_positions.norm(dim=-1) < 1e-3 - ) # (NUM_MANO_LINKS,) - object_contact_positions[invalid_contact_mask] = 0.0 - object_contact_normals[invalid_contact_mask] = 0.0 - - return object_contact_positions, object_contact_normals diff --git a/reconstruction/modules/v2d_task_library_loader/lib/dataset_loader_base.py b/reconstruction/modules/v2d_task_library_loader/lib/dataset_loader_base.py deleted file mode 100644 index ac0ca0ca..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/dataset_loader_base.py +++ /dev/null @@ -1,870 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Base class for loading hand-object datasets into ManoSharpaData schema. - -Subclass and implement the abstract methods to add a new dataset (e.g. ARCTIC, TACO). -""" - -import argparse -import pickle -import re -from abc import ABC, abstractmethod -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from scipy.spatial.transform import Rotation as R -from tqdm import tqdm - -# FK / contact code ported into this module (manotorch-tainted or FK-output utils). -from v2d.task_library_loader.lib.contact_utils import ( - approximate_contact_with_id, - compute_hand_link_contact_positions, - find_object_contact_positions, -) -from v2d.task_library_loader.lib.distance_utils import ( - compute_tip_to_object_surface_distance, -) -from v2d.task_library_loader.lib.read_mano import MANO - -# Clean shared code imported from robotic_grounding (installed dependency; GPL-free). -from robotic_grounding.retarget.data_logger import ManoSharpaData, shard_matches -from robotic_grounding.retarget.naming import make_usd_safe -from robotic_grounding.retarget.params import MANO_HAND_LINKS, NUM_MANO_LINKS - -# Partition columns for the output Parquet. Copied from robotic_grounding's -# retarget_utils to avoid importing it here (it would pull pink/pinocchio IK deps). -DEFAULT_PARTITION_COLS = ["sequence_id", "robot_name"] -# ``make_usd_safe`` (imported above from naming.py) is re-exported here so the -# ported loaders' ``from ...dataset_loader_base import make_usd_safe`` keep working. - - -@dataclass -class SequenceInfo: - """Metadata for one sequence to process.""" - - sequence_id: str - raw_motion_file: str - object_name: str - object_body_names: list[str] - source: Any = None # Subclass-specific (e.g. Path to .mano.npy, or (hand_dir, N)) - - -@dataclass -class FrameObjectPoses: - """Per-frame object poses and optional combined mesh for tips-distance.""" - - object_body_position: list[list[float]] - object_body_wxyz: list[list[float]] - object_root_axis_angle: list[float] - object_root_position: list[float] - object_articulation: float - object_surface_points_world: torch.Tensor - object_surface_normals_world: torch.Tensor - object_surface_points_part_ids: torch.Tensor - - -def poses_to_root_position_and_axis_angle( - poses: np.ndarray, -) -> tuple[np.ndarray, np.ndarray]: - """Extract root position and axis-angle from (N, 4, 4) world transform matrices. - - Returns: - root_position: (N, 3) translation column. - root_axis_angle: (N, 3) axis-angle from rotation matrices. - """ - root_position = poses[:, :3, 3] - root_axis_angle = np.array( - [R.from_matrix(poses[i, :3, :3]).as_rotvec() for i in range(len(poses))] - ) - return root_position, root_axis_angle - - -def load_meshes_to_device( - mesh_paths: dict[str, str], - device: torch.device, - vertex_scale: float = 1.0, - num_surface_points: int = 4096, -) -> tuple[ - dict[str, Any], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - bool, -]: - """Load trimesh objects by part name, convert verts/faces to device tensors. - - Args: - mesh_paths: Mapping of part name to mesh file path. - device: Target device for tensors. - vertex_scale: Factor to scale vertex positions (e.g. 0.01 for cm->m). - num_surface_points: Number of surface points to sample per part. - - Returns: - (meshes, verts, faces, compute_tips_dist) -- meshes is the raw trimesh - per part; verts/faces are on device; compute_tips_dist is False if any - part mesh was not found. - """ - meshes: dict[str, Any] = {} - verts: dict[str, torch.Tensor] = {} - faces: dict[str, torch.Tensor] = {} - surface_points: dict[str, torch.Tensor] = {} - surface_normals: dict[str, torch.Tensor] = {} - compute_tips_dist = True - - for part, path_str in mesh_paths.items(): - path = Path(path_str) - if not path.exists(): - print(f"Warning: Mesh not found at {path}, skipping {part}") - compute_tips_dist = False - continue - mesh = trimesh.load(str(path)) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - if vertex_scale != 1.0: - mesh.vertices *= vertex_scale - meshes[part] = mesh - verts[part] = torch.from_numpy(mesh.vertices).float().to(device) - faces[part] = torch.from_numpy(mesh.faces).long().to(device) - - pts, face_idx = trimesh.sample.sample_surface_even(mesh, num_surface_points) - surface_points[part] = torch.from_numpy(pts).float().to(device) - part_normals = ( - torch.from_numpy(mesh.face_normals[face_idx]).float().to(device) - ) # point outward - part_normals = part_normals / torch.norm( - part_normals, dim=-1, keepdim=True - ).clamp(min=1e-6) - surface_normals[part] = -part_normals # point inward - - return meshes, verts, faces, surface_points, surface_normals, compute_tips_dist - - -def build_combined_object_surface( - object_surface_points: dict[str, torch.Tensor], - object_surface_normals: dict[str, torch.Tensor], - body_names: list[str], - world_transforms: list[np.ndarray], - device: torch.device, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - """Combine per-body surface points and normals transformed to world frame. - - Args: - object_surface_points: Surface points per body name (on device). - object_surface_normals: Surface normals per body name (on device). - body_names: Order of bodies (must match world_transforms). - world_transforms: List of 4x4 world matrices, one per body in body_names order. - device: Target device for output tensors. - - Returns: - combined_verts: (V, 3) all vertices in world frame. - combined_faces: (F, 3) face indices with offset for concatenated verts. - """ - all_verts: list[torch.Tensor] = [] - all_normals: list[torch.Tensor] = [] - all_part_ids: list[torch.Tensor] = [] - for body_idx, (name, T) in enumerate( - zip(body_names, world_transforms, strict=True) - ): - if name not in object_surface_points or name not in object_surface_normals: - continue - R_mat = torch.from_numpy(np.asarray(T)[:3, :3]).float().to(device) - t_vec = torch.from_numpy(np.asarray(T)[:3, 3]).float().to(device) - verts = (R_mat @ object_surface_points[name].T).T + t_vec - normals = (R_mat @ object_surface_normals[name].T).T - part_ids = torch.full((verts.shape[0],), body_idx + 1, device=device) - all_verts.append(verts) - all_normals.append(normals) - all_part_ids.append(part_ids) - if not all_verts: - raise ValueError("No mesh parts found to combine") - return ( - torch.cat(all_verts, dim=0), - torch.cat(all_normals, dim=0), - torch.cat(all_part_ids, dim=0), - ) - - -class DatasetLoaderBase(ABC): - """Base class for loading a dataset into ManoSharpaData (MANO + object only). - - Subclasses implement: list_sequences, load_mano_data, load_object_data, - load_object_meshes, get_mano_kwargs, get_fps. Optionally override - get_frame_object_poses, get_object_mesh_paths, get_frame_range. - """ - - @abstractmethod - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """List sequences to process. Uses args for dataset paths/filters.""" - ... - - @abstractmethod - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO parameters for the sequence. - - Returns dict with: H, right_global_orient, right_finger_pose, right_trans, - right_betas, right_fitting_err, left_global_orient, left_finger_pose, - left_trans, left_betas, left_fitting_err. All tensors on device. - """ - ... - - @abstractmethod - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object pose data (passed to get_frame_object_poses). - - Return a dictionary mapping each object/body name to a tuple - (pose, root_position, root_axis_angle, articulation): - - pose: (N, 4, 4) world transform per frame - - root_position: (N, 3) object root position per frame - - root_axis_angle: (N, 3) object root axis-angle per frame - - articulation: (N,) array or None if no articulation - """ - ... - - @abstractmethod - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple[ - dict[str, Any], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - bool, - ]: - """Load object part meshes. - - Returns: - object_mesh_meshes: trimesh (or Scene) per part, for visualization. - object_mesh_verts: vertices per part on device. - object_mesh_faces: faces per part on device. - object_surface_points: surface points per part. - object_surface_normals: surface normals per part. - compute_tips_dist: whether tips-distance can be computed. - """ - ... - - def get_frame_object_poses( - self, - frame_id: int, - object_data: dict[str, Any], - object_surface_points: dict[str, torch.Tensor], - object_surface_normals: dict[str, torch.Tensor], - device: torch.device, - ) -> FrameObjectPoses: - """Compute per-body world poses and optional combined mesh for one frame. - - Uses the (pose, root_position, root_axis_angle, articulation) tuple - convention from load_object_data(). Override if custom logic is needed. - """ - body_names = list(object_data.keys()) - world_transforms = [object_data[name][0][frame_id] for name in body_names] - - object_body_position = [t[:3, 3].tolist() for t in world_transforms] - object_body_wxyz = [ - R.from_matrix(t[:3, :3]).as_quat(scalar_first=True).tolist() - for t in world_transforms - ] - - _pose, root_position, root_axis_angle, articulation = object_data[body_names[0]] - - ( - object_surface_points_world, - object_surface_normals_world, - object_surface_points_part_ids, - ) = build_combined_object_surface( - object_surface_points, - object_surface_normals, - body_names, - world_transforms, - device, - ) - - return FrameObjectPoses( - object_body_position=object_body_position, - object_body_wxyz=object_body_wxyz, - object_root_axis_angle=root_axis_angle[frame_id].tolist(), - object_root_position=root_position[frame_id].tolist(), - object_articulation=( - float(articulation[frame_id]) if articulation is not None else 0.0 - ), - object_surface_points_world=object_surface_points_world, - object_surface_normals_world=object_surface_normals_world, - object_surface_points_part_ids=object_surface_points_part_ids, - ) - - @abstractmethod - def get_mano_kwargs(self) -> dict[str, Any]: - """Keyword arguments for MANO(...), e.g. flat_hand_mean, center_idx.""" - ... - - @abstractmethod - def get_fps(self) -> float: - """Frames per second for this dataset.""" - ... - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to object mesh files (one per object body, same order as object_body_names). Override in subclass.""" - return [] - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to object URDF files (one per object body, same order as object_body_names). Override in subclass.""" - return [] - - def get_frame_range(self, num_frames: int) -> tuple[int, int]: - """Return (start, end) frame indices to process. Override to trim frames.""" - return 0, num_frames - - @staticmethod - def add_common_args( - parser: argparse.ArgumentParser, - *, - dataset_root: Any, - object_model_root: Any, - mesh_dir: Any, - output_dir: Any, - ) -> None: - """Register the args-first contract shared by every loader. - - Every loader resolves its raw data (``--dataset_root``), object URDFs - (``--object_model_root``), object meshes (``--mesh_dir``) and MANO models - (``--mano_model_dir``) from these flags. The OSMO load workflow passes them - as swift-fetched runtime paths; the per-dataset values passed here are - local-dev fallbacks. Also registers ``--output_dir``/``--device``/ - ``--visualize``/``--save`` and the sequence-filter args (via - :meth:`add_filter_args`). - """ - parser.add_argument( - "--dataset_root", - type=Path, - default=dataset_root, - help="Raw dataset root for this dataset.", - ) - parser.add_argument( - "--object_model_root", - type=Path, - default=object_model_root, - help="Directory with this dataset's per-object rigid URDFs.", - ) - parser.add_argument( - "--mesh_dir", - type=Path, - default=mesh_dir, - help="Directory with this dataset's object meshes.", - ) - parser.add_argument( - "--output_dir", - type=Path, - default=output_dir, - help="Parent directory for Parquet output.", - ) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - DatasetLoaderBase.add_filter_args(parser) - - @staticmethod - def add_filter_args(parser: argparse.ArgumentParser) -> None: - """Add common sequence filtering args. Call from each loader's parse_args().""" - parser.add_argument( - "--mano_model_dir", - type=str, - default=None, - help=( - "Directory containing the MANO models (a models/ subdir with " - "MANO_RIGHT.pkl / MANO_LEFT.pkl). Required to run MANO FK; " - "supplied at runtime and never vendored." - ), - ) - group = parser.add_argument_group("sequence filtering") - group.add_argument( - "--sequence_id", - type=str, - default=None, - help="Process a single sequence by exact ID.", - ) - group.add_argument( - "--sequence_pattern", - type=str, - default=None, - help="Regex pattern to filter sequence IDs (e.g., '.*box.*').", - ) - group.add_argument( - "--sequence_file", - type=str, - default=None, - help="Text file with sequence IDs to process (one per line).", - ) - group.add_argument( - "--max_sequences", - type=int, - default=None, - help="Limit to first N sequences after filtering.", - ) - group.add_argument( - "--list_only", - action="store_true", - default=False, - help="List matching sequence IDs and exit without processing.", - ) - group.add_argument( - "--shard_id", - type=int, - default=0, - help="Shard index (0-based) for parallel processing.", - ) - group.add_argument( - "--num_shards", - type=int, - default=1, - help="Total number of shards. 1 = no sharding (default).", - ) - - @staticmethod - def _apply_sequence_filters( - sequences: list["SequenceInfo"], args: Any - ) -> list["SequenceInfo"]: - """Apply common sequence filters (incl. shard partitioning) to the list.""" - if getattr(args, "sequence_id", None): - sequences = [s for s in sequences if s.sequence_id == args.sequence_id] - if getattr(args, "sequence_pattern", None): - pat = re.compile(args.sequence_pattern) - sequences = [s for s in sequences if pat.search(s.sequence_id)] - if getattr(args, "sequence_file", None): - with open(args.sequence_file) as f: - ids = {line.strip() for line in f if line.strip()} - sequences = [s for s in sequences if s.sequence_id in ids] - if getattr(args, "max_sequences", None): - sequences = sequences[: args.max_sequences] - - num_shards = getattr(args, "num_shards", 1) or 1 - shard_id = getattr(args, "shard_id", 0) or 0 - if num_shards > 1: - sequences = [ - s - for s in sequences - if shard_matches(s.sequence_id, shard_id, num_shards) - ] - return sequences - - def run(self, args: Any) -> None: - """Common pipeline: list sequences, load MANO/object, log timesteps, save.""" - self._args = args - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - else: - viser_server = None - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - sequences = self.list_sequences(args) - sequences = self._apply_sequence_filters(sequences, args) - - if getattr(args, "list_only", False): - for s in sequences: - print(s.sequence_id) - return - - print(f"Found {len(sequences)} sequences") - - if getattr(args, "mano_model_dir", None) is None: - raise ValueError( - "--mano_model_dir is required: pass the directory containing the " - "MANO models (models/MANO_RIGHT.pkl, MANO_LEFT.pkl)." - ) - mano_kwargs = self.get_mano_kwargs() - mano = MANO( - mano_assets_root=args.mano_model_dir, - gender="neutral", - device=device, - **mano_kwargs, - ) - viser_object_handles: dict[str, Any] = {} - viser_contact_handles: list[Any] = [] - - for sequence_info in tqdm(sequences): - try: - raw_data = self.load_mano_data(sequence_info, device) - except ( - FileNotFoundError, - ValueError, - KeyError, - pickle.UnpicklingError, - ) as e: - print(f"Skipping {sequence_info.sequence_id}: {e}") - continue - - # Number of frames - H = raw_data["H"] - try: - object_data = self.load_object_data(sequence_info) - except (FileNotFoundError, ValueError) as e: - print(f"Skipping {sequence_info.sequence_id}: {e}") - continue - - try: - ( - object_mesh_meshes, - _object_mesh_verts, - _object_mesh_faces, - object_surface_points, - object_surface_normals, - compute_tips_dist, - ) = self.load_object_meshes(sequence_info, device) - except (FileNotFoundError, ValueError) as e: - print(f"Skipping {sequence_info.sequence_id}: {e}") - continue - - if args.visualize and viser_server is not None: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - for part in sequence_info.object_body_names: - if part not in object_mesh_meshes: - continue - mesh = object_mesh_meshes[part] - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - viser_object_handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - - if args.save: - object_mesh_radius = [ - ( - object_surface_points[body_name] - - object_surface_points[body_name].mean(dim=0) - ) - .norm(dim=1) - .max() - .item() - for body_name in sequence_info.object_body_names - ] - logger_data = ManoSharpaData( - sequence_id=sequence_info.sequence_id, - raw_motion_file=sequence_info.raw_motion_file, - object_name=sequence_info.object_name, - safe_object_name=make_usd_safe(sequence_info.object_name), - robot_name="sharpa_wave", - fps=self.get_fps(), - mano_flat_hand_mean=mano_kwargs.get("flat_hand_mean", True), - mano_center_idx=mano_kwargs.get("center_idx", None), - mano_right_betas=raw_data["right_betas"].tolist(), - mano_left_betas=raw_data["left_betas"].tolist(), - mano_to_robot_scale=None, - right_robot_finger_joint_names=[], - right_robot_frame_names=[], - right_robot_frame_task_names=[], - left_robot_finger_joint_names=[], - left_robot_frame_names=[], - left_robot_frame_task_names=[], - object_body_names=sequence_info.object_body_names, - safe_object_body_names=[ - make_usd_safe(name) for name in sequence_info.object_body_names - ], - object_mesh_paths=self.get_object_mesh_paths(sequence_info), - object_urdf_paths=self.get_object_urdf_paths(sequence_info), - object_mesh_radius=object_mesh_radius, - mano_link_names=list(MANO_HAND_LINKS.keys()), - ) - - mano_results: dict[str, Any] = {} - for side in ("right", "left"): - mano_results[side] = mano.forward( - side=side, - global_orient=raw_data[f"{side}_global_orient"], - finger_pose=raw_data[f"{side}_finger_pose"], - transl=raw_data[f"{side}_trans"], - betas=raw_data[f"{side}_betas"], - ) - - frame_start, frame_end = self.get_frame_range(H) - for frame_id in range(frame_start, frame_end): - joints: dict[str, torch.Tensor] = {} - joints_wxyz: dict[str, torch.Tensor] = {} - for side in ("right", "left"): - joints[side] = mano_results[side]["joints"][frame_id] - joints_wxyz[side] = mano_results[side]["joints_wxyz"][frame_id] - - frame_poses = self.get_frame_object_poses( - frame_id, - object_data, - object_surface_points, - object_surface_normals, - device, - ) - - if args.visualize and viser_server is not None: - for side in ("right", "left"): - mano.visualize( - viser_server, - side, - vertices=mano_results[side]["vertices"][frame_id], - faces=mano_results[side]["faces"], - joints=joints[side], - joints_wxyz=joints_wxyz[side], - ) - for idx, part in enumerate(sequence_info.object_body_names): - if part in viser_object_handles and idx < len( - frame_poses.object_body_position - ): - pos = frame_poses.object_body_position[idx] - wxyz = frame_poses.object_body_wxyz[idx] - viser_object_handles[part].position = ( - np.asarray(pos) - if not isinstance(pos, np.ndarray) - else pos - ) - viser_object_handles[part].wxyz = ( - np.asarray(wxyz) - if not isinstance(wxyz, np.ndarray) - else wxyz - ) - - tips_dist: dict[str, Any] = {} - if ( - compute_tips_dist - and frame_poses.object_surface_points_world is not None - ): - for side in ("right", "left"): - tips_dist[side] = compute_tip_to_object_surface_distance( - joints[side], - frame_poses.object_surface_points_world, - ) - - # Compute contact positions on hand links, object surface, contact normals, and part ids. - contact_data: dict[str, dict[str, torch.Tensor]] = {} - if frame_poses.object_surface_points_world is not None: - for handle in viser_contact_handles: - handle.remove() - viser_contact_handles.clear() - for side in ("right", "left"): - hand_verts = mano_results[side]["vertices"][frame_id] - hand_faces = mano_results[side]["faces"] - hand_normals = trimesh.Trimesh( - vertices=hand_verts.cpu().numpy(), - faces=hand_faces.cpu().numpy(), - ).vertex_normals - hand_normals = ( - torch.from_numpy(hand_normals).float().to(device) - ) # point outward - ( - _, - _, - object_contact_part_ids, - hand_contact_points_world, - hand_contact_normals_world, - contact_dists, - ) = approximate_contact_with_id( - frame_poses.object_surface_points_world, - frame_poses.object_surface_normals_world, - frame_poses.object_surface_points_part_ids, - hand_verts, - hand_normals, - threshold=0.01, - ) - # Reduce the number of contacts to NUM_MANO_LINKS by averaging the contacts on the same link. - hand_link_contact_positions = torch.zeros( - (NUM_MANO_LINKS, 3), device=device - ) - hand_link_contact_normals = torch.zeros( - (NUM_MANO_LINKS, 3), device=device - ) - hand_link_contact_part_ids = torch.zeros( - (NUM_MANO_LINKS,), device=device - ) - object_contact_positions = torch.zeros( - (NUM_MANO_LINKS, 3), device=device - ) - object_contact_normals = torch.zeros( - (NUM_MANO_LINKS, 3), device=device - ) - if len(contact_dists) > 0: - ( - hand_link_contact_positions, - hand_link_contact_normals, - hand_link_contact_part_ids, - ) = compute_hand_link_contact_positions( - joints[side], - object_contact_part_ids, - hand_contact_points_world, - hand_contact_normals_world, - contact_dists, - ) - object_contact_positions, object_contact_normals = ( - find_object_contact_positions( - hand_link_contact_positions, - frame_poses.object_surface_points_world, - frame_poses.object_surface_normals_world, - ) - ) - - if args.visualize and viser_server is not None: - for link_idx in range(NUM_MANO_LINKS): - if object_contact_positions[link_idx].norm() < 1e-3: - continue - hand_link_name = list(MANO_HAND_LINKS.keys())[ - link_idx - ] - object_contact_handle = viser_server.scene.add_icosphere( - name=f"/object/contacts/{side}/{hand_link_name}", - position=object_contact_positions[link_idx] - .cpu() - .numpy(), - radius=0.003, - color=np.array([0, 0, 255]), - ) - viser_contact_handles.append(object_contact_handle) - hand_link_contact_handle = viser_server.scene.add_icosphere( - name=f"/mano/{side}/contacts/{hand_link_name}", - position=hand_link_contact_positions[link_idx] - .cpu() - .numpy(), - radius=0.003, - color=np.array([0, 255, 0]), - ) - viser_contact_handles.append( - hand_link_contact_handle - ) - normal_lines = torch.cat( - [ - hand_link_contact_positions.unsqueeze(1), - ( - hand_link_contact_positions - + hand_link_contact_normals * 0.01 - ).unsqueeze(1), - ], - dim=1, - ) - hand_link_contact_normal_handle = ( - viser_server.scene.add_line_segments( - name=f"/mano/{side}/contacts/normals", - points=normal_lines.cpu().numpy(), - colors=np.zeros_like( - normal_lines.cpu().numpy() - ), - line_width=2.0, - ) - ) - viser_contact_handles.append( - hand_link_contact_normal_handle - ) - - contact_data[side] = { - "hand_link_contact_positions": hand_link_contact_positions, - "hand_link_contact_normals": hand_link_contact_normals, - "object_contact_positions": object_contact_positions, - "object_contact_normals": object_contact_normals, - "hand_link_contact_part_ids": hand_link_contact_part_ids, - } - - if args.save: - logger_data.log_timestep( - mano_right_trans=raw_data["right_trans"][frame_id] - .cpu() - .tolist(), - mano_right_global_orient=raw_data["right_global_orient"][ - frame_id - ] - .cpu() - .tolist(), - mano_right_finger_pose=raw_data["right_finger_pose"][frame_id] - .cpu() - .tolist(), - mano_right_joints=joints["right"].cpu().tolist(), - mano_right_joints_wxyz=joints_wxyz["right"].cpu().tolist(), - mano_right_fitting_err=raw_data["right_fitting_err"][frame_id] - .cpu() - .item(), - mano_right_tips_distance=tips_dist.get("right"), - mano_right_link_contact_positions=contact_data["right"][ - "hand_link_contact_positions" - ] - .cpu() - .tolist(), - mano_right_link_contact_normals=contact_data["right"][ - "hand_link_contact_normals" - ] - .cpu() - .tolist(), - mano_right_object_contact_positions=contact_data["right"][ - "object_contact_positions" - ] - .cpu() - .tolist(), - mano_right_object_contact_normals=contact_data["right"][ - "object_contact_normals" - ] - .cpu() - .tolist(), - mano_right_object_contact_part_ids=contact_data["right"][ - "hand_link_contact_part_ids" - ] - .cpu() - .tolist(), - mano_left_trans=raw_data["left_trans"][frame_id].cpu().tolist(), - mano_left_global_orient=raw_data["left_global_orient"][frame_id] - .cpu() - .tolist(), - mano_left_finger_pose=raw_data["left_finger_pose"][frame_id] - .cpu() - .tolist(), - mano_left_joints=joints["left"].cpu().tolist(), - mano_left_joints_wxyz=joints_wxyz["left"].cpu().tolist(), - mano_left_fitting_err=raw_data["left_fitting_err"][frame_id] - .cpu() - .item(), - mano_left_tips_distance=tips_dist.get("left"), - mano_left_link_contact_positions=contact_data["left"][ - "hand_link_contact_positions" - ] - .cpu() - .tolist(), - mano_left_link_contact_normals=contact_data["left"][ - "hand_link_contact_normals" - ] - .cpu() - .tolist(), - mano_left_object_contact_positions=contact_data["left"][ - "object_contact_positions" - ] - .cpu() - .tolist(), - mano_left_object_contact_normals=contact_data["left"][ - "object_contact_normals" - ] - .cpu() - .tolist(), - mano_left_object_contact_part_ids=contact_data["left"][ - "hand_link_contact_part_ids" - ] - .cpu() - .tolist(), - object_articulation=frame_poses.object_articulation, - object_root_axis_angle=frame_poses.object_root_axis_angle, - object_root_position=frame_poses.object_root_position, - object_body_position=frame_poses.object_body_position, - object_body_wxyz=frame_poses.object_body_wxyz, - ) - - if args.save: - logger_data.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/dexycb_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/dexycb_loader.py deleted file mode 100644 index 0b8baf71..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/dexycb_loader.py +++ /dev/null @@ -1,870 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load DexYCB dataset (NVLabs, CVPR 2021) into ManoSharpaData schema. - -DexYCB layout (after extraction, ``$DEX_YCB_DIR``): - - dexycb_dir/ - 20200709-subject-01/ - 20200709_141841/ # session id - meta.yml # ycb_ids, ycb_grasp_ind, mano_side, mano_calib - {camera_serial}/ # 8 cameras per session - color_{N:06d}.jpg (ignored here) - aligned_depth_to_color_{N:06d}.png (ignored) - labels_{N:06d}.npz # per-frame pose_y / pose_m / joint_3d / seg - calibration/ - mano_{calib_id}/mano.yml # per-subject MANO betas - intrinsics/{camera_serial}_*.yml - extrinsics_{ext_id}/extrinsics.yml - models/ - {ycb_name}/textured_simple.obj # 21 YCB object meshes - -Per-frame ``labels_*.npz`` keys we consume: - pose_y : (num_obj, 3, 4) — ``[R | t]`` for each YCB object in CAMERA frame. - pose_m : (1, 51) — 3 global_orient + 45 PCA finger pose + 3 trans. - -MANO format: - ``pose_m[:, 0:48]`` is MANO in PCA representation (ncomps=45, flat_hand_mean=False). - We expand the 45 PCA coefs to 45-DOF axis-angle before handing to MANO FK, - matching the pattern in ``hot3d_loader.py``. - -Single-hand sequences: - Each DexYCB session uses exactly one hand (``meta['mano_side']`` in - {right, left}). The other hand is filled with a zero pose at the origin - so the retargeter can still produce a two-hand robot trajectory (the - idle arm just sits at the world origin). - -Coordinate frame: - ``pose_y`` / ``pose_m`` live in the chosen camera's frame. DexYCB's - per-session ``calibration/extrinsics_{ext_id}/extrinsics.yml`` stores a - 3x4 world-to-camera matrix per serial; "world" there is the master - camera's frame. The master is the overhead camera in DexYCB's rig, so - its +Y (image-down) is approximately gravity. We compose - ``R_master_to_zup @ inv(W2C_serial)`` per sequence: first undo the - serial's extrinsic to land in master frame, then rotate so +Y_master - (down) -> -Z_world and +Z_master (forward) -> +Y_world. This is not - exact if the master isn't perfectly vertical, but eliminates the - fixed-rotation approximation that caused "through the floor" artifacts - for sequences whose master camera differed from the first-fitted one. -""" - -from __future__ import annotations - -import argparse -import fcntl -import logging -import tarfile -import warnings -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import yaml -from scipy.spatial.transform import Rotation - -warnings.filterwarnings("ignore", category=DeprecationWarning, module="mano") - -from manotorch.manolayer import ManoLayer # noqa: E402 -from robotic_grounding.retarget import ( # noqa: E402 - ASSETS_DIR, - HUMAN_MOTION_DATA_DIR, - MESHES_DIR, -) -from v2d.task_library_loader.lib.dataset_loader_base import ( # noqa: E402 - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - make_usd_safe, -) - -logging.getLogger().setLevel(logging.ERROR) - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -DEFAULT_DEXYCB_DIR = HUMAN_MOTION_DATA_DIR / "dexycb" / "dataset" -DEXYCB_URDF_DIR = ASSETS_DIR / "urdfs" / "dexycb" -DEXYCB_MESH_DIR = MESHES_DIR / "dexycb" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "dexycb" / "dexycb_loaded" -DEXYCB_FPS = 30.0 - -# Camera serials (from dex-ycb-toolkit _SERIALS). -DEXYCB_SERIALS: tuple[str, ...] = ( - "836212060125", - "839512060362", - "840412060917", - "841412060263", - "932122060857", - "932122060861", - "932122062010", - "932122062940", -) -# Default to the last serial (master in the toolkit). -DEFAULT_CAMERA_SERIAL = "932122062010" - -# YCB object ID -> folder name (matches ``dex_ycb_toolkit.dex_ycb._YCB_CLASSES``). -DEXYCB_OBJECTS: dict[int, str] = { - 1: "002_master_chef_can", - 2: "003_cracker_box", - 3: "004_sugar_box", - 4: "005_tomato_soup_can", - 5: "006_mustard_bottle", - 6: "007_tuna_fish_can", - 7: "008_pudding_box", - 8: "009_gelatin_box", - 9: "010_potted_meat_can", - 10: "011_banana", - 11: "019_pitcher_base", - 12: "021_bleach_cleanser", - 13: "024_bowl", - 14: "025_mug", - 15: "035_power_drill", - 16: "036_wood_block", - 17: "037_scissors", - 18: "040_large_marker", - 19: "051_large_clamp", - 20: "052_extra_large_clamp", - 21: "061_foam_brick", -} - -# Fallback fixed rotation from the master camera's OpenCV frame (+X right, -# +Y down, +Z forward) to a gravity-aligned Z-up world. Used only if the -# data-driven gravity estimate below fails (e.g. a labels_*.npz with no -# valid object poses). Empirically off by ~49° for real DexYCB sessions, -# which is what the data-driven path fixes — keep it as a last-resort -# default so the loader doesn't crash mid-stream. -_MASTER_TO_ZUP_FALLBACK = np.array( - [[1, 0, 0], [0, 0, 1], [0, -1, 0]], - dtype=np.float32, -) - -# Per-extrinsics-id cache for the data-driven master→Z-up rotation. All -# sessions sharing the same ``extrinsics_`` calibration file share the -# same rig geometry and therefore the same gravity direction in master, -# so we memoize on the id and re-use across sequences. -_GRAVITY_ALIGN_CACHE: dict[str, np.ndarray] = {} - - -def _rotation_align_to_zup(gravity_up_in_master: np.ndarray) -> np.ndarray: - """Return a 3×3 rotation R such that ``R @ gravity_up_in_master == +Z``. - - Leaves the perpendicular-to-gravity directions well-defined (Rodrigues - rotation around the cross product), so repeated calls on the same - gravity vector give the same result. - """ - target = np.array([0.0, 0.0, 1.0], dtype=np.float64) - up = np.asarray(gravity_up_in_master, dtype=np.float64) - up = up / max(np.linalg.norm(up), 1e-12) - axis = np.cross(up, target) - axis_norm = np.linalg.norm(axis) - if axis_norm < 1e-9: - # Already aligned (or antipodal — flip via 180° around +X). - if np.dot(up, target) > 0: - return np.eye(3, dtype=np.float32) - return np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]], dtype=np.float32) - angle = np.arccos(np.clip(np.dot(up, target), -1.0, 1.0)) - return Rotation.from_rotvec(axis / axis_norm * angle).as_matrix().astype(np.float32) - - -def _estimate_gravity_up_in_master( - dexycb_dir: Path, - extrinsics_id: str, - camera_serial: str, - camera_dir: Path, - max_frames: int = 20, -) -> np.ndarray: - """Derive gravity-up (in master frame) from object rest poses. - - YCB's ``textured_simple.obj`` meshes all share the same canonical - frame (+Z = up, verified empirically — four different-shape objects in - one test sequence had local +Z within 2° of each other). When an - object sits on a horizontal surface, its local +Z in world frame is - exactly gravity-up. We read up to ``max_frames`` ``labels_*.npz`` - files from ``camera_dir``, pull every non-zero ``pose_y[k, :3, 2]`` - (object-local +Z in cam frame), transform to master via the - pre-computed ``cam_to_master``, and average. Any motion during the - sequence averages out; the mean vector is robust across typical - DexYCB sessions. - - Falls back to the ``_MASTER_TO_ZUP_FALLBACK`` convention (OpenCV Y-down - = gravity) if no valid pose is found — see the docstring comment. - """ - ext_path = ( - dexycb_dir / "calibration" / f"extrinsics_{extrinsics_id}" / "extrinsics.yml" - ) - with open(ext_path, "r") as f: - calib = yaml.load(f, Loader=yaml.Loader) - flat = np.asarray(calib["extrinsics"][camera_serial], dtype=np.float64) - world_to_cam = np.eye(4, dtype=np.float64) - world_to_cam[:3, :] = flat.reshape(3, 4) - R_cam_to_master = np.linalg.inv(world_to_cam)[:3, :3] - - ups_in_master: list[np.ndarray] = [] - for fpath in sorted(camera_dir.glob("labels_*.npz"))[:max_frames]: - try: - arr = np.load(fpath) - pose_y = arr["pose_y"] # (num_obj, 3, 4) - except Exception: # noqa: BLE001 - continue - for k in range(pose_y.shape[0]): - R_cam = pose_y[k, :3, :3] - t_cam = pose_y[k, :3, 3] - if np.allclose(R_cam, 0) and np.allclose(t_cam, 0): - continue # un-labelled object in this frame - obj_up_cam = R_cam[:, 2].astype(np.float64) # object local +Z - ups_in_master.append(R_cam_to_master @ obj_up_cam) - - if not ups_in_master: - warnings.warn( - f"No valid pose_y rotations under {camera_dir} for extrinsics " - f"{extrinsics_id!r}; falling back to the fixed master->Zup " - "approximation (likely ~45° off gravity).", - stacklevel=2, - ) - # The fallback matrix was built assuming master +Y is gravity; - # gravity-up in master is therefore master -Y. - return np.array([0.0, -1.0, 0.0], dtype=np.float64) - - mean_up = np.mean(np.stack(ups_in_master, axis=0), axis=0) - return mean_up / np.linalg.norm(mean_up) - - -# --------------------------------------------------------------------------- -# Data structures -# --------------------------------------------------------------------------- -@dataclass -class DexYCBSequenceSource: - """Source info for one DexYCB sequence (subject/session/camera triple).""" - - session_dir: Path # .../{subject}/{session}/ - camera_dir: Path # .../{subject}/{session}/{serial}/ - subject: str # e.g. "20200709-subject-01" - session: str # e.g. "20200709_141841" - camera_serial: str - ycb_ids: list[int] - ycb_grasp_ind: int - ycb_names: list[str] # derived from DEXYCB_OBJECTS - mano_side: str # "right" | "left" - mano_calib: str - extrinsics_id: str # e.g. "20200702_151821"; indexes calibration/extrinsics_/ - - -# --------------------------------------------------------------------------- -# File helpers -# --------------------------------------------------------------------------- -def _read_meta(session_dir: Path) -> dict[str, Any]: - with open(session_dir / "meta.yml", "r") as f: - return yaml.safe_load(f) - - -def _read_mano_betas(dexycb_dir: Path, mano_calib: str) -> np.ndarray: - """Load per-subject MANO betas from calibration/mano_{id}/mano.yml.""" - path = dexycb_dir / "calibration" / f"mano_{mano_calib}" / "mano.yml" - with open(path, "r") as f: - calib = yaml.safe_load(f) - return np.asarray(calib["betas"], dtype=np.float32) - - -def _load_cam_to_world( - dexycb_dir: Path, - extrinsics_id: str, - camera_serial: str, - camera_dir: Path | None = None, -) -> np.ndarray: - """Return the camera-to-world 4x4 transform for one session. - - DexYCB's ``calibration/extrinsics_/extrinsics.yml`` stores, per - serial, a 3x4 ``[R | t]`` world-to-camera matrix where "world" is the - master camera's frame. We invert to get camera-to-master, then apply - a per-``extrinsics_id`` rotation that takes master frame into Isaac - Sim's Z-up world. - - The master→Z-up rotation is **data-driven** per calibration batch: - :func:`_estimate_gravity_up_in_master` infers gravity from object - rest poses (YCB canonical +Z = gravity-up), and - :func:`_rotation_align_to_zup` turns that direction into a 3×3 - rotation matrix. Result is cached in ``_GRAVITY_ALIGN_CACHE`` so the - estimate only runs once per batch. - - ``camera_dir`` is required to compute the gravity estimate; it's - optional only for legacy callers that already have a warmed cache. - """ - path = dexycb_dir / "calibration" / f"extrinsics_{extrinsics_id}" / "extrinsics.yml" - with open(path, "r") as f: - calib = yaml.load(f, Loader=yaml.Loader) # tagged python/tuple - extrinsics = calib["extrinsics"] - if camera_serial not in extrinsics: - raise KeyError( - f"Camera serial {camera_serial!r} not found in {path}. " - f"Available: {sorted(extrinsics)}" - ) - flat = np.asarray(extrinsics[camera_serial], dtype=np.float64) - if flat.size != 12: - raise ValueError(f"{path}: expected 12 floats per serial, got {flat.size}") - world_to_cam = np.eye(4, dtype=np.float64) - world_to_cam[:3, :] = flat.reshape(3, 4) - cam_to_master = np.linalg.inv(world_to_cam) - - if extrinsics_id not in _GRAVITY_ALIGN_CACHE: - if camera_dir is None: - # No data available to estimate gravity — fall back to the - # fixed rotation. Should only happen if a legacy caller - # invokes this without camera_dir on a cold cache. - warnings.warn( - f"_load_cam_to_world called for extrinsics {extrinsics_id!r} " - "without camera_dir; using fixed master->Zup fallback.", - stacklevel=2, - ) - _GRAVITY_ALIGN_CACHE[extrinsics_id] = _MASTER_TO_ZUP_FALLBACK - else: - gravity_up = _estimate_gravity_up_in_master( - dexycb_dir, extrinsics_id, camera_serial, camera_dir - ) - _GRAVITY_ALIGN_CACHE[extrinsics_id] = _rotation_align_to_zup(gravity_up) - - master_to_zup = np.eye(4, dtype=np.float64) - master_to_zup[:3, :3] = _GRAVITY_ALIGN_CACHE[extrinsics_id].astype(np.float64) - return (master_to_zup @ cam_to_master).astype(np.float32) - - -def _collect_frame_ids(camera_dir: Path) -> list[str]: - """Return sorted frame ids (stems like '000042') present in camera_dir.""" - return sorted(p.stem.split("_", 1)[1] for p in camera_dir.glob("labels_*.npz")) - - -def _expand_pca_to_aa( - pca_coeffs: np.ndarray, components: np.ndarray, hands_mean: np.ndarray -) -> np.ndarray: - """Expand (N, 45) PCA coefficients to (N, 45) axis-angle finger pose. - - ``hands_mean`` is added because DexYCB was fitted with - ``flat_hand_mean=False`` so the mean pose is baked into the reconstruction. - """ - return pca_coeffs @ components + hands_mean[None, :] - - -def _extract_archives_if_needed(dexycb_dir: Path) -> Path: - """Extract ``{name}.tar.gz`` bundles in ``dexycb_dir`` if not yet expanded. - - The CSS upload ships per-subject tarballs (plus ``calibration.tar.gz`` - and ``models.tar.gz``) instead of the 600k+ raw label/mesh files, to - keep S3 round-trips down. Each tarball was created with entries rooted - at its namesake directory (e.g. ``20200709-subject-01/...``) so - extracting with ``-C dexycb_dir`` restores the canonical layout. - - Idempotent: skips any tarball whose extraction target directory already - exists. If the input dir is read-only (e.g. the CSS bind mount), the - extraction is redirected to a ``/tmp`` cache. - """ - archives = sorted(dexycb_dir.glob("*.tar.gz")) - if not archives: - return dexycb_dir - - def _already_extracted(archive: Path) -> bool: - stem = archive.name[: -len(".tar.gz")] - return (dexycb_dir / stem).is_dir() - - pending = [a for a in archives if not _already_extracted(a)] - if not pending: - return dexycb_dir - - # Serialize extraction across shards via an advisory file lock — the - # first shard extracts while the others block, then re-check and find - # `_already_extracted` True. - lock_path = Path("/tmp") / f"dexycb_extract_{dexycb_dir.name}.lock" - with open(lock_path, "w") as lock: - fcntl.flock(lock.fileno(), fcntl.LOCK_EX) - pending = [a for a in archives if not _already_extracted(a)] - if not pending: - return dexycb_dir - - target = dexycb_dir - try: - probe = dexycb_dir / ".extract_test" - probe.touch() - probe.unlink() - except OSError: - target = Path("/tmp") / f"dexycb_extracted_{dexycb_dir.name}" - target.mkdir(parents=True, exist_ok=True) - print(f"[dexycb] input dir read-only, extracting to cache: {target}") - - for archive in pending: - print(f"[dexycb] extracting {archive.name} -> {target}") - with tarfile.open(archive, "r:gz") as t: - t.extractall(target) - - return target - - -# --------------------------------------------------------------------------- -# Loader -# --------------------------------------------------------------------------- -class DexYCBDatasetLoader(DatasetLoaderBase): - """Load DexYCB sequences into ManoSharpaData (MANO + object only).""" - - def __init__(self) -> None: - """Defer ManoLayer build until args (mano_model_dir) are known.""" - self._args: argparse.Namespace | None = None - # Per-sequence camera-to-world extrinsic cache, shared between - # load_mano_data and load_object_data so the yml is read once. - self._cam_to_world_cache: dict[str, np.ndarray] = {} - self._right_layer: ManoLayer | None = None - self._left_layer: ManoLayer | None = None - - def _ensure_mano_layers(self) -> None: - """Lazily build right/left PCA ManoLayers from --mano_model_dir.""" - if self._right_layer is not None and self._left_layer is not None: - return - mano_assets_root = str(getattr(self._args, "mano_model_dir", None)) - self._right_layer = ManoLayer( - use_pca=True, - ncomps=45, - flat_hand_mean=False, - side="right", - mano_assets_root=mano_assets_root, - ) - self._left_layer = ManoLayer( - use_pca=True, - ncomps=45, - flat_hand_mean=False, - side="left", - mano_assets_root=mano_assets_root, - ) - self._right_components = ( - self._right_layer.th_selected_comps.detach().cpu().numpy() - ) - self._left_components = ( - self._left_layer.th_selected_comps.detach().cpu().numpy() - ) - self._right_hands_mean = ( - self._right_layer.th_hands_mean.detach().cpu().numpy().squeeze() - ) - self._left_hands_mean = ( - self._left_layer.th_hands_mean.detach().cpu().numpy().squeeze() - ) - - # ------------------------------------------------------------------ - # Sequence discovery - # ------------------------------------------------------------------ - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """Discover DexYCB sequences (one per session for the chosen camera).""" - self._args = args - dexycb_dir = Path(getattr(args, "dataset_root", DEFAULT_DEXYCB_DIR)) - serial = getattr(args, "camera_serial", DEFAULT_CAMERA_SERIAL) - - if not dexycb_dir.exists(): - raise FileNotFoundError(f"DexYCB dataset not found at {dexycb_dir}") - - # If the dataset ships as per-subject tarballs (e.g. on CSS), expand - # them once before walking the tree. - dexycb_dir = _extract_archives_if_needed(dexycb_dir) - - sequences: list[SequenceInfo] = [] - for subject_dir in sorted(dexycb_dir.iterdir()): - if not subject_dir.is_dir() or "-subject-" not in subject_dir.name: - continue - subject = subject_dir.name - for session_dir in sorted(subject_dir.iterdir()): - if not session_dir.is_dir(): - continue - camera_dir = session_dir / serial - if not camera_dir.exists(): - continue - try: - meta = _read_meta(session_dir) - except FileNotFoundError: - continue - - ycb_ids = list(meta.get("ycb_ids", [])) - ycb_names = [ - DEXYCB_OBJECTS.get(cid, f"object_{cid}") for cid in ycb_ids - ] - if not ycb_ids: - continue - sequence_id = f"dexycb_{subject}_{session_dir.name}_{serial}" - - mano_sides = meta.get("mano_sides", meta.get("mano_side", "right")) - if isinstance(mano_sides, list): - mano_side = str(mano_sides[0]) if mano_sides else "right" - else: - mano_side = str(mano_sides) - source = DexYCBSequenceSource( - session_dir=session_dir, - camera_dir=camera_dir, - subject=subject, - session=session_dir.name, - camera_serial=serial, - ycb_ids=ycb_ids, - ycb_grasp_ind=int(meta.get("ycb_grasp_ind", 0)), - ycb_names=ycb_names, - mano_side=mano_side, - mano_calib=( - str(meta.get("mano_calib", [""])[0]) - if isinstance(meta.get("mano_calib"), list) - else str(meta.get("mano_calib", "")) - ), - extrinsics_id=str(meta.get("extrinsics", "")), - ) - sequences.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=str(camera_dir), - object_name="+".join(ycb_names), - object_body_names=ycb_names, - source=source, - ) - ) - - sequences = self._apply_sequence_filters(sequences, args) - print(f"Found {len(sequences)} DexYCB sequences") - return sequences - - # ------------------------------------------------------------------ - # MANO - # ------------------------------------------------------------------ - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Parse pose_m from every labels_*.npz frame in the sequence.""" - self._ensure_mano_layers() - src: DexYCBSequenceSource = sequence_info.source - dexycb_dir = Path(getattr(self._args, "dataset_root", DEFAULT_DEXYCB_DIR)) - frame_ids = _collect_frame_ids(src.camera_dir) - if not frame_ids: - raise FileNotFoundError(f"No labels_*.npz in {src.camera_dir}") - - active_side = src.mano_side - assert active_side in {"right", "left"}, f"Bad mano_side {active_side}" - - g_list: list[np.ndarray] = [] # (3,) - p_list: list[np.ndarray] = [] # (45,) PCA - t_list: list[np.ndarray] = [] # (3,) - for fid in frame_ids: - arr = np.load(src.camera_dir / f"labels_{fid}.npz") - pose_m = arr["pose_m"] # (1, 51) - if pose_m.shape != (1, 51): - raise ValueError( - f"{src.camera_dir}/labels_{fid}.npz: pose_m shape {pose_m.shape}" - ) - g_list.append(pose_m[0, 0:3].astype(np.float32)) - p_list.append(pose_m[0, 3:48].astype(np.float32)) - t_list.append(pose_m[0, 48:51].astype(np.float32)) - - H = len(frame_ids) - global_orient = np.stack(g_list) # (H, 3) - pca_pose = np.stack(p_list) # (H, 45) - trans = np.stack(t_list) # (H, 3) - - # Expand PCA -> axis-angle in the active hand's basis. - if active_side == "right": - finger_pose = _expand_pca_to_aa( - pca_pose, self._right_components, self._right_hands_mean - ) - else: - finger_pose = _expand_pca_to_aa( - pca_pose, self._left_components, self._left_hands_mean - ) - finger_pose = finger_pose.astype(np.float32) - betas = _read_mano_betas(dexycb_dir, src.mano_calib) - active_layer = self._right_layer if active_side == "right" else self._left_layer - # manotorch (center_idx=None) uses wrist_world = J0 + transl, so when we - # rotate the world frame we must rotate that wrist anchor too. If we - # only do R @ transl, the hand drifts away from the objects after the - # gravity-alignment rotation. - with torch.no_grad(): - zero_pose = torch.zeros(1, 3 + 45, dtype=torch.float32) - active_joint0 = ( - active_layer( - pose_coeffs=zero_pose, - betas=torch.from_numpy(betas).float().unsqueeze(0), - ) - .joints[0, 0] - .detach() - .cpu() - .numpy() - .astype(np.float32) - ) - - # Camera frame -> gravity-aligned Z-up world via per-session - # extrinsic composed with master-cam->Z-up rotation. We also fold a - # per-sequence Z lift into c2w so the lowest point in the scene - # (object or active wrist) lands just above Isaac Sim's floor - # plane at z = 0; without it, sequences where the object rests at - # tabletop below the master-frame origin clip into the ground and - # drive continuous physics oscillation (aka hand shake on grasp). - c2w = _load_cam_to_world( - dexycb_dir, src.extrinsics_id, src.camera_serial, src.camera_dir - ) - R = c2w[:3, :3] - t = c2w[:3, 3] - # Provisional active-hand trans in world frame. The J0 correction is - # required for consistency with the rotated MANO global_orient. - trans_world = (R @ (trans + active_joint0).T).T + t - active_joint0 - # Peek at every frame's object origins (pose_y[k, :3, 3]) and - # transform them to world; combined with the hand trans this gives - # the scene's world-frame Z extent. - obj_z_world: list[float] = [] - for fid in frame_ids: - arr = np.load(src.camera_dir / f"labels_{fid}.npz") - pose_y = arr["pose_y"] # (num_obj, 3, 4) - for k in range(pose_y.shape[0]): - tcam = pose_y[k, :3, 3] - Rcam = pose_y[k, :3, :3] - if np.allclose(tcam, 0) and np.allclose(Rcam, 0): - continue - obj_z_world.append(float((R @ tcam + t)[2])) - scene_min_z = min( - (*obj_z_world, float(trans_world[:, 2].min())) - if obj_z_world - else (float(trans_world[:, 2].min()),) - ) - # Match h2o_loader's generous margin so the scene floats comfortably - # above Isaac Sim's floor regardless of which mesh half-extent - # happens to straddle the reconstruct ground-filter (0.05 m). YCB - # objects are smaller than H2O (tallest ~20 cm), but the same - # "centroid vs vertex bottom" mismatch still applies for thin/tall - # items (e.g. 019_pitcher_base, 011_banana) — 1 m is empirically - # safe across both datasets and dummy_agent's auto-framed camera - # hides the absolute offset anyway. - _GROUND_MARGIN = 1.0 # m above Isaac Sim's floor plane - z_lift = max(0.0, _GROUND_MARGIN - scene_min_z) - c2w[2, 3] += z_lift - t = c2w[:3, 3] - self._cam_to_world_cache[sequence_info.sequence_id] = c2w - for i in range(H): - Rm = Rotation.from_rotvec(global_orient[i]).as_matrix() - global_orient[i] = Rotation.from_matrix(R @ Rm).as_rotvec() - trans = (R @ (trans + active_joint0).T).T + t - active_joint0 - - # Fill the idle hand with a zero pose parked below the ground plane. - # DexYCB is single-handed (``mano_side`` selects which); if the idle - # hand's MANO trans is zero, downstream IK lands its robot at the - # zero-pose J0 offset (~the world origin), which spawns a second - # PD-controlled Sharpa in full camera view whose tiny tracking-error - # oscillation looks like shaking. Parking at z = -0.5 m hides the - # idle robot under Isaac Sim's floor plane without dragging the - # auto-framed viewer off the active hand's work area. - zero_g = np.zeros((H, 3), dtype=np.float32) - zero_p = np.zeros((H, 45), dtype=np.float32) - zero_t = np.tile(np.array([0.0, 0.0, -0.5], dtype=np.float32), (H, 1)) - zero_b = np.zeros(10, dtype=np.float32) - - active = { - "global_orient": global_orient.astype(np.float32), - "finger_pose": finger_pose, - "trans": trans.astype(np.float32), - "betas": betas.astype(np.float32), - } - idle = { - "global_orient": zero_g, - "finger_pose": zero_p, - "trans": zero_t, - "betas": zero_b, - } - right = active if active_side == "right" else idle - left = active if active_side == "left" else idle - - return { - "H": H, - "right_global_orient": torch.from_numpy(right["global_orient"]).to(device), - "right_finger_pose": torch.from_numpy(right["finger_pose"]).to(device), - "right_trans": torch.from_numpy(right["trans"]).to(device), - "right_betas": torch.from_numpy(right["betas"]).to(device), - "right_fitting_err": torch.zeros(H, device=device), - "left_global_orient": torch.from_numpy(left["global_orient"]).to(device), - "left_finger_pose": torch.from_numpy(left["finger_pose"]).to(device), - "left_trans": torch.from_numpy(left["trans"]).to(device), - "left_betas": torch.from_numpy(left["betas"]).to(device), - "left_fitting_err": torch.zeros(H, device=device), - } - - # ------------------------------------------------------------------ - # Object poses - # ------------------------------------------------------------------ - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Parse pose_y 3x4 matrices across all frames, rotate to Z-up.""" - src: DexYCBSequenceSource = sequence_info.source - frame_ids = _collect_frame_ids(src.camera_dir) - N = len(frame_ids) - M = len(src.ycb_ids) - - poses = np.tile(np.eye(4, dtype=np.float32), (M, N, 1, 1)) # (M, N, 4, 4) - last = [np.eye(4, dtype=np.float32) for _ in range(M)] - c2w = self._cam_to_world_cache.get(sequence_info.sequence_id) - if c2w is None: - # Cold-cache fallback. Reloads extrinsic + re-estimates gravity - # via the per-batch cache inside ``_load_cam_to_world``, so the - # result includes the correct master->Z-up rotation — but NOT - # the ``z_lift`` applied in ``load_mano_data``, which depends on - # hand trans and is only computed when MANO data is loaded - # first. Intended as a safety net, not the common path. - dexycb_dir = Path(getattr(self._args, "dataset_root", DEFAULT_DEXYCB_DIR)) - c2w = _load_cam_to_world( - dexycb_dir, src.extrinsics_id, src.camera_serial, src.camera_dir - ) - - for f_idx, fid in enumerate(frame_ids): - arr = np.load(src.camera_dir / f"labels_{fid}.npz") - pose_y = arr["pose_y"] # (num_obj_in_file, 3, 4) - if pose_y.shape[0] != M: - # Shouldn't happen if meta.ycb_ids is consistent, but be safe. - pose_y = pose_y[:M] - for k in range(M): - Rcam = pose_y[k, :3, :3] - tcam = pose_y[k, :3, 3] - if np.allclose(Rcam, 0) and np.allclose(tcam, 0): - # Missing / un-labelled frame for this object: hold last. - poses[k, f_idx] = last[k] - continue - # Compose cam-frame object pose with camera-to-world extrinsic. - T_cam = np.eye(4, dtype=np.float32) - T_cam[:3, :3] = Rcam - T_cam[:3, 3] = tcam - Twc = (c2w @ T_cam).astype(np.float32) - poses[k, f_idx] = Twc - last[k] = Twc - - result: dict[str, Any] = {} - for k, name in enumerate(src.ycb_names): - pose_seq = poses[k] - root_pos = pose_seq[:, :3, 3].copy() - root_aa = np.stack( - [Rotation.from_matrix(T[:3, :3]).as_rotvec() for T in pose_seq] - ).astype(np.float32) - result[name] = (pose_seq, root_pos, root_aa, None) - return result - - # ------------------------------------------------------------------ - # Meshes - # ------------------------------------------------------------------ - def load_object_meshes( - self, sequence_info: SequenceInfo, device: torch.device - ) -> tuple: - """Load the YCB textured meshes for each object in the scene. - - Searches in order: - 1. ``{mesh_dir}/{name}/textured_simple.obj`` (committed copy) - 2. ``{dataset_root}/models/{name}/textured_simple.obj`` (canonical) - """ - src: DexYCBSequenceSource = sequence_info.source - dexycb_dir = Path(getattr(self._args, "dataset_root", DEFAULT_DEXYCB_DIR)) - canonical_dir = Path(getattr(self._args, "mesh_dir", DEXYCB_MESH_DIR)) - models_dir = dexycb_dir / "models" - - mesh_paths: dict[str, str] = {} - missing: list[str] = [] - for name in src.ycb_names: - chosen: Path | None = None - for base in (canonical_dir, models_dir): - preferred = base / name / "textured_simple.obj" - if preferred.exists(): - chosen = preferred - break - objs = ( - sorted((base / name).glob("*.obj")) - if (base / name).is_dir() - else [] - ) - if objs: - chosen = objs[0] - break - if chosen is None: - missing.append(name) - continue - mesh_paths[name] = str(chosen) - - if missing: - raise FileNotFoundError( - f"DexYCB meshes missing for {sequence_info.sequence_id}: " - f"{missing}. Searched {canonical_dir} and {models_dir}." - ) - return load_meshes_to_device(mesh_paths, device, vertex_scale=1.0) - - def get_mano_kwargs(self) -> dict[str, Any]: - """MANO kwargs paired with the pose we store. - - :func:`_expand_pca_to_aa` already bakes ``hands_mean`` into the - 45-dim axis-angle finger pose we hand off. MANO's internal layer - with ``flat_hand_mean=False`` would add ``hands_mean`` on top - *again*, double-counting the mean pose and pushing fingertips up - to ~9 cm off ground truth. ``flat_hand_mean=True`` tells the - layer to take our finger pose as absolute — verified joint-for- - joint against DexYCB's ``joint_3d`` (0 mm error, vs 33 mm mean - with ``flat_hand_mean=False``). - """ - return {"flat_hand_mean": True, "center_idx": None} - - def get_fps(self) -> float: - """Return DexYCB capture rate (30 Hz).""" - return DEXYCB_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return per-object mesh paths for a sequence, preferring committed copies.""" - src: DexYCBSequenceSource = sequence_info.source - mesh_dir = Path(getattr(self._args, "mesh_dir", DEXYCB_MESH_DIR)) - paths: list[str] = [] - for name in src.ycb_names: - preferred = mesh_dir / name / "textured_simple.obj" - if preferred.exists(): - paths.append(str(preferred)) - continue - dexycb_dir = Path(getattr(self._args, "dataset_root", DEFAULT_DEXYCB_DIR)) - fallback = dexycb_dir / "models" / name / "textured_simple.obj" - paths.append(str(fallback)) - return paths - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return per-object URDF paths under ``--object_model_root``.""" - src: DexYCBSequenceSource = sequence_info.source - urdf_dir = Path(getattr(self._args, "object_model_root", DEXYCB_URDF_DIR)) - return [ - str(urdf_dir / f"{make_usd_safe(name)}_rigid.urdf") - for name in src.ycb_names - ] - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- -def parse_args() -> argparse.Namespace: - """Parse CLI args for the DexYCB loader.""" - parser = argparse.ArgumentParser( - description="Load DexYCB sequences into ManoSharpaData schema." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=DEFAULT_DEXYCB_DIR, - object_model_root=DEXYCB_URDF_DIR, - mesh_dir=DEXYCB_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - parser.add_argument( - "--camera_serial", - type=str, - default=DEFAULT_CAMERA_SERIAL, - help="Which of the 8 DexYCB camera serials to ingest (default: toolkit master).", - ) - parser.add_argument( - "--list_sequences", - action="store_true", - help="List sequences and exit.", - ) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Run the DexYCB loader or list sequences per CLI args.""" - loader = DexYCBDatasetLoader() - if args.list_sequences: - for s in loader.list_sequences(args): - print(s.sequence_id) - return - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/distance_utils.py b/reconstruction/modules/v2d_task_library_loader/lib/distance_utils.py deleted file mode 100644 index 76cda13a..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/distance_utils.py +++ /dev/null @@ -1,196 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Utilities for computing fingertip-to-object surface distances. - -This module implements the ManipTrans approach for pre-computing reference distances -from MANO fingertips to object mesh surfaces during data processing. -""" - -import logging - -import torch -import trimesh - -# Try to import PyTorch3D, fall back to trimesh-based sampling if unavailable -try: - from pytorch3d.ops import sample_points_from_meshes - from pytorch3d.structures import Meshes - - PYTORCH3D_AVAILABLE = True -except ImportError: - PYTORCH3D_AVAILABLE = False - sample_points_from_meshes = None # type: ignore[misc, assignment] - Meshes = None # type: ignore[misc, assignment] - -# MANO fingertip joint indices (5 per hand) -# From params.py: wrist(0), thumb1-4(1-4), index1-4(5-8), middle1-4(9-12), ring1-4(13-16), pinky1-4(17-20) -# Tips are: thumb4(4), index4(8), middle4(12), ring4(16), pinky4(20) -MANO_FINGERTIP_INDICES = [4, 8, 12, 16, 20] - - -def _sample_points_from_mesh_trimesh( - vertices: torch.Tensor, faces: torch.Tensor, num_samples: int -) -> torch.Tensor: - """Sample points from mesh surface using trimesh (CPU fallback). - - Args: - vertices: Mesh vertices (V, 3). - faces: Mesh faces (F, 3). - num_samples: Number of points to sample. - - Returns: - Sampled surface points (num_samples, 3). - """ - # Convert to numpy for trimesh - verts_np = vertices.cpu().numpy() - faces_np = faces.cpu().numpy() - - # Create trimesh and sample - mesh = trimesh.Trimesh(vertices=verts_np, faces=faces_np) - points, _ = trimesh.sample.sample_surface(mesh, num_samples) - - return torch.from_numpy(points).float().to(vertices.device) - - -def _sample_points_from_mesh_pytorch3d( - vertices: torch.Tensor, faces: torch.Tensor, num_samples: int -) -> torch.Tensor: - """Sample points from mesh surface using PyTorch3D (GPU when available). - - Tries the input device first (GPU if vertices are on CUDA). If PyTorch3D - was not compiled with GPU support, falls back to CPU and moves the result - back. For maximum speed, install PyTorch3D with CUDA (see PyTorch3D docs). - - Args: - vertices: Mesh vertices (V, 3). - faces: Mesh faces (F, 3). - num_samples: Number of points to sample. - - Returns: - Sampled surface points (num_samples, 3) on the same device as vertices. - """ - device = vertices.device - mesh = Meshes(verts=[vertices], faces=[faces]) - try: - return sample_points_from_meshes(mesh, num_samples)[0] - except RuntimeError as e: - err_msg = str(e) - if "Not compiled with GPU support" in err_msg or "CUDA" in err_msg: - logging.getLogger(__name__).warning( - "PyTorch3D GPU path failed (%s). Using CPU for mesh sampling. " - "For faster runs, install PyTorch3D with CUDA support.", - err_msg[:80], - ) - mesh_cpu = Meshes(verts=[vertices.cpu()], faces=[faces.cpu()]) - out = sample_points_from_meshes(mesh_cpu, num_samples)[0] - return out.to(device) - raise - - -def compute_tips_distance( - mano_joints: torch.Tensor, - obj_mesh_verts: torch.Tensor, - obj_mesh_faces: torch.Tensor, - obj_rotation: torch.Tensor, - obj_translation: torch.Tensor, - num_samples: int = 1000, -) -> torch.Tensor: - """Compute distance from MANO fingertips to object surface. - - This follows the ManipTrans approach of pre-computing chamfer distances - from the reference (MANO) fingertips to the object mesh surface. - - Args: - mano_joints: MANO joint positions (21, 3). - obj_mesh_verts: Object mesh vertices in local frame (V, 3). - obj_mesh_faces: Object mesh faces (F, 3). - obj_rotation: Object rotation matrix (3, 3). - obj_translation: Object translation (3,). - num_samples: Number of points to sample from mesh surface. Default: 1000. - - Returns: - Distances from each fingertip to nearest point on object surface (5,). - Order: thumb, index, middle, ring, pinky. - """ - device = mano_joints.device - - # Ensure tensors are on same device and correct dtype - obj_mesh_verts = obj_mesh_verts.to(device=device, dtype=torch.float32) - obj_mesh_faces = obj_mesh_faces.to(device=device, dtype=torch.int64) - obj_rotation = obj_rotation.to(device=device, dtype=torch.float32) - obj_translation = obj_translation.to(device=device, dtype=torch.float32) - - # Sample points on object surface - if PYTORCH3D_AVAILABLE: - surface_points = _sample_points_from_mesh_pytorch3d( - obj_mesh_verts, obj_mesh_faces, num_samples - ) - else: - surface_points = _sample_points_from_mesh_trimesh( - obj_mesh_verts, obj_mesh_faces, num_samples - ) - - # Transform surface points to world frame - # world_points = R @ local_points^T + t - surface_points_world = ( - obj_rotation @ surface_points.T - ).T + obj_translation # (num_samples, 3) - - # Get fingertip positions from MANO joints - fingertips = mano_joints[MANO_FINGERTIP_INDICES] # (5, 3) - - # Compute pairwise distances: (5, num_samples) - dists = torch.cdist( - fingertips.unsqueeze(0), surface_points_world.unsqueeze(0) - ).squeeze(0) - - # Get minimum distance from each fingertip to surface - min_dists = dists.min(dim=-1).values # (5,) - - return min_dists - - -def compute_tip_to_object_surface_distance( - mano_joints: torch.Tensor, - object_surface_points_world: torch.Tensor, -) -> list[list[float]] | None: - """Compute MANO fingertip-to-surface distances for one hand. - - Object surface points are assumed to be in world frame. - - Args: - mano_joints: MANO joints (21, 3) on same device as object_surface_points_world. - object_surface_points_world: Object surface points in world frame (V, 3). - - Returns: - List of 5 lists (one per fingertip) of float distances, or None if - computation is not possible. - """ - fingertips = mano_joints[MANO_FINGERTIP_INDICES] - - dists = torch.cdist( - fingertips.unsqueeze(0), object_surface_points_world.unsqueeze(0) - ).squeeze(0) - - # Get minimum distance from each fingertip to surface - min_dists = dists.amin(dim=-1) # (5,) - - return min_dists.cpu().tolist() - - -def load_object_mesh(mesh_path: str) -> tuple[torch.Tensor, torch.Tensor]: - """Load object mesh and return vertices and faces as tensors. - - Args: - mesh_path: Path to the mesh file (OBJ, PLY, etc.). - - Returns: - Tuple of (vertices, faces) tensors. - - vertices: (V, 3) float tensor - - faces: (F, 3) int tensor - """ - mesh = trimesh.load(mesh_path) - vertices = torch.from_numpy(mesh.vertices).float() - faces = torch.from_numpy(mesh.faces).long() - return vertices, faces diff --git a/reconstruction/modules/v2d_task_library_loader/lib/grab_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/grab_loader.py deleted file mode 100644 index a1ada281..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/grab_loader.py +++ /dev/null @@ -1,450 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load GRAB dataset (ECCV 2020) into ManoSharpaData schema. - -Canonical GRAB layout produced by ``grab/unzip_grab.py`` from -https://github.com/otaheri/GRAB: - - grab_dir/ - grab/ - s1/ ... s10/ - {object}_{action}_{take}.npz e.g. mug_pass_1.npz - tools/ - object_meshes/ - contact_meshes/ - {object}.ply full-res GRAB object meshes (meters) - {object}.stl (optional) ContactDB STLs, if the - ``contactdb_scaled_stl_files_public`` - archive was unpacked too - -Each sequence .npz contains (accessed via .item() dict): - framerate (float, 120.0) - n_frames (int) - gender (str), sbj_id (str), obj_name (str) - body.params: {transl(N,3), global_orient(N,3), body_pose(N,63), - left_hand_pose(N,24), right_hand_pose(N,24), fullpose(N,165), - jaw_pose(N,3), leye_pose(N,3), reye_pose(N,3), expression(N,10)} - body.vtemp: relative path to subject rest-pose mesh - lhand.params, rhand.params: { - global_orient(N,3), hand_pose(N,24) # PCA coefs, - transl(N,3), fullpose(N,45) # 45-DOF axis-angle finger pose - } - lhand.vtemp, rhand.vtemp: relative paths to per-subject hand meshes - object.params: {global_orient(N,3), transl(N,3)} - object.object_mesh: relative path, e.g. "tools/object_meshes/contact_meshes/airplane.ply" - -GRAB does not store MANO ``betas`` or per-frame ``fitting_err`` — subject -identity is in ``vtemp`` (a rest-pose hand mesh per subject). We use zeros -for both; the IK retarget only needs the joint positions produced by MANO -forward, and the mean-shape hand is close enough for that. - -Coordinate frame: - GRAB uses SMPL-X Y-up world. We rotate to Z-up before storing so the - retargeting pipeline is consistent with Arctic/OakInk2/Hot3D. - -Runs stage 1 of the two-stage pipeline: - 1. python scripts/retarget/grab_loader.py --save -> grab_loaded/ - 2. python scripts/retarget/grab_to_sharpa.py --save -> grab_processed/ -""" - -from __future__ import annotations - -import argparse -import logging -import warnings -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from scipy.spatial.transform import Rotation - -warnings.filterwarnings("ignore", category=DeprecationWarning, module="mano") - -from robotic_grounding.retarget import ( # noqa: E402 - ASSETS_DIR, - HUMAN_MOTION_DATA_DIR, - MESHES_DIR, -) -from v2d.task_library_loader.lib.dataset_loader_base import ( # noqa: E402 - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - make_usd_safe, -) - -logging.getLogger().setLevel(logging.ERROR) - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -DEFAULT_GRAB_DIR = HUMAN_MOTION_DATA_DIR / "grab" / "dataset" -GRAB_URDF_DIR = ASSETS_DIR / "urdfs" / "grab" -GRAB_MESH_DIR = MESHES_DIR / "grab" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "grab" / "grab_loaded" -GRAB_FPS = 120.0 - -# GRAB's SMPL-X world frame is Y-up. Rotate to Z-up (same target as our other -# loaders) via a 90-degree rotation around X: (x, y, z) -> (x, z, -y). -# This is also the convention Arctic's loader standardises on. -Y_UP_TO_Z_UP = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]], dtype=np.float32) - - -# --------------------------------------------------------------------------- -# Data structures -# --------------------------------------------------------------------------- -@dataclass -class GRABSequenceSource: - """Source info for one GRAB sequence.""" - - npz_path: Path - subject: str # e.g. "s1" - object_name: str # e.g. "mug" - action: str # e.g. "pass" - take: str # e.g. "1" - - -# --------------------------------------------------------------------------- -# File parser -# --------------------------------------------------------------------------- -def _parse_sequence_name(npz_path: Path) -> tuple[str, str, str]: - """Parse '{object}_{action}[_{take}][_Retake].npz' -> (object, action, take). - - GRAB's 51 object names are all single-word (no underscores), but take - fields are irregular: they can be just a digit (``mug_pass_1``) or a - digit plus a ``_Retake`` suffix (``camera_takepicture_3_Retake``), so - splitting from the *right* misidentifies the object. Split from the - *left* once to separate object from the rest, then again to pull off - the action; everything remaining is the take. - """ - stem = npz_path.stem - head, _, rest = stem.partition("_") - obj = head - action, _, take = rest.partition("_") - if not action: - action = "unknown" - if not take: - take = "0" - return obj, action, take - - -def _load_npz_dict(path: Path) -> dict[str, Any]: - """Load a GRAB .npz file into a pure-Python dict. - - GRAB .npz files use numpy's object-array trick to store nested dicts. - Each top-level key is usually a 0-d array whose .item() is a dict, so - we resolve it here once to keep downstream code clean. - """ - data = np.load(str(path), allow_pickle=True) - out: dict[str, Any] = {} - for key in data.files: - val = data[key] - if val.shape == () and val.dtype == object: - out[key] = val.item() - else: - out[key] = val - return out - - -# --------------------------------------------------------------------------- -# Loader -# --------------------------------------------------------------------------- -class GRABDatasetLoader(DatasetLoaderBase): - """Load GRAB sequences into ManoSharpaData (MANO + object only).""" - - def __init__(self) -> None: - """Initialize per-instance args cache populated by list_sequences.""" - self._args: argparse.Namespace | None = None - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """Discover GRAB sequences across all subjects.""" - self._args = args - grab_dir = Path(getattr(args, "dataset_root", DEFAULT_GRAB_DIR)) - - if not grab_dir.exists(): - raise FileNotFoundError(f"GRAB dataset not found at {grab_dir}") - - # The ``unzip_grab.py`` helper places per-subject data under - # ``grab_dir/grab/sN/``. Support both that canonical layout and the - # shorter ``grab_dir/sN/`` in case someone flattens it by hand. - subjects_root = grab_dir / "grab" if (grab_dir / "grab").is_dir() else grab_dir - - sequences: list[SequenceInfo] = [] - for subject_dir in sorted(subjects_root.iterdir()): - if not subject_dir.is_dir() or not subject_dir.name.startswith("s"): - continue - if not subject_dir.name[1:].isdigit(): - continue - subject = subject_dir.name - for npz_path in sorted(subject_dir.glob("*.npz")): - obj, action, take = _parse_sequence_name(npz_path) - sequence_id = f"grab_{subject}_{obj}_{action}_{take}" - source = GRABSequenceSource( - npz_path=npz_path, - subject=subject, - object_name=obj, - action=action, - take=take, - ) - sequences.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=str(npz_path), - object_name=obj, - object_body_names=[obj], - source=source, - ) - ) - - sequences = self._apply_sequence_filters(sequences, args) - print(f"Found {len(sequences)} GRAB sequences") - return sequences - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO hand parameters from a GRAB .npz file. - - Applies Y-up -> Z-up rotation so downstream code sees a consistent - world frame. With ``center_idx=None``, the MANO wrist world position - is ``R_global @ J_shaped[0](betas) + transl``. Rotating the world - requires the betas-aware ``new_transl`` transform (same pattern as - hot3d_loader.py). - """ - src: GRABSequenceSource = sequence_info.source - data = _load_npz_dict(src.npz_path) - - n_frames = int(data.get("n_frames", 0)) - fps = int(data.get("framerate", GRAB_FPS)) - if fps != GRAB_FPS: - print( - f"Warning: {src.npz_path.name} framerate={fps}, expected {int(GRAB_FPS)}" - ) - - rhand = data["rhand"]["params"] - lhand = data["lhand"]["params"] - - # GRAB stores finger pose two ways: ``hand_pose`` as 24-dim PCA coefs - # and ``fullpose`` as the already-expanded 45-dim axis-angle. We use - # ``fullpose`` to avoid an extra PCA->axis-angle expansion step. - right_global = np.asarray(rhand["global_orient"], dtype=np.float32) - right_pose = np.asarray(rhand["fullpose"], dtype=np.float32) - right_trans = np.asarray(rhand["transl"], dtype=np.float32) - - left_global = np.asarray(lhand["global_orient"], dtype=np.float32) - left_pose = np.asarray(lhand["fullpose"], dtype=np.float32) - left_trans = np.asarray(lhand["transl"], dtype=np.float32) - - # GRAB has no per-hand betas (subject identity is in vtemp). Use the - # mean-shape MANO (zeros) — the IK retarget tracks joint positions, so - # small shape mismatch is acceptable. - right_betas = np.zeros(10, dtype=np.float32) - left_betas = np.zeros(10, dtype=np.float32) - - # No fitting error is stored. - right_fit = np.zeros(n_frames, dtype=np.float32) - left_fit = np.zeros(n_frames, dtype=np.float32) - - # Apply Y-up -> Z-up rotation to global orient + translation. - # manotorch (center_idx=None) computes wrist_world = R_g @ J0 + trans, - # so rotating the world requires: new_trans = R @ (J0 + trans) - J0. - # We don't have J0 here without running MANO FK; use the zero-pose - # wrist offset computed from betas (same as hot3d_loader.py). - from manotorch.manolayer import ManoLayer # noqa: E402, PLC0415 - - R_world = Y_UP_TO_Z_UP - - def _zero_pose_joint0(side: str, betas: np.ndarray) -> np.ndarray: - layer = ManoLayer( - mano_assets_root=str(self._args.mano_model_dir), - side=side, - flat_hand_mean=False, - use_pca=True, - ncomps=15, - center_idx=None, - ) - with torch.no_grad(): - out = layer( - pose_coeffs=torch.zeros(1, 3 + 15), - betas=torch.from_numpy(betas).float().unsqueeze(0), - ) - return out.joints[0, 0].cpu().numpy() - - try: - J_right = _zero_pose_joint0("right", right_betas) - J_left = _zero_pose_joint0("left", left_betas) - except Exception: - # If MANO assets aren't available (local dev), fall back to no-offset. - # This slightly mis-aligns the wrist after rotation but keeps iteration going. - J_right = np.zeros(3, dtype=np.float32) - J_left = np.zeros(3, dtype=np.float32) - - for i in range(n_frames): - M = Rotation.from_rotvec(right_global[i]).as_matrix() - right_global[i] = Rotation.from_matrix(R_world @ M).as_rotvec() - M = Rotation.from_rotvec(left_global[i]).as_matrix() - left_global[i] = Rotation.from_matrix(R_world @ M).as_rotvec() - - right_trans = (R_world @ (right_trans + J_right).T).T - J_right - left_trans = (R_world @ (left_trans + J_left).T).T - J_left - - return { - "H": n_frames, - "right_global_orient": torch.from_numpy(right_global).to(device), - "right_finger_pose": torch.from_numpy(right_pose).to(device), - "right_trans": torch.from_numpy(right_trans).to(device), - "right_betas": torch.from_numpy(right_betas).to(device), - "right_fitting_err": torch.from_numpy(right_fit).to(device), - "left_global_orient": torch.from_numpy(left_global).to(device), - "left_finger_pose": torch.from_numpy(left_pose).to(device), - "left_trans": torch.from_numpy(left_trans).to(device), - "left_betas": torch.from_numpy(left_betas).to(device), - "left_fitting_err": torch.from_numpy(left_fit).to(device), - } - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object 4x4 poses from a GRAB .npz file (single rigid object).""" - src: GRABSequenceSource = sequence_info.source - data = _load_npz_dict(src.npz_path) - - obj_params = data["object"]["params"] - obj_rotvec = np.asarray(obj_params["global_orient"], dtype=np.float32) - obj_trans = np.asarray(obj_params["transl"], dtype=np.float32) - n_frames = obj_rotvec.shape[0] - - poses = np.tile(np.eye(4, dtype=np.float32), (n_frames, 1, 1)) - for i in range(n_frames): - R_local = Rotation.from_rotvec(obj_rotvec[i]).as_matrix() - # Rotate to Z-up: new_R = R_world @ R_local, new_t = R_world @ t - poses[i, :3, :3] = Y_UP_TO_Z_UP @ R_local - poses[i, :3, 3] = Y_UP_TO_Z_UP @ obj_trans[i] - - root_position = poses[:, :3, 3].copy() - root_axis_angle = np.stack( - [Rotation.from_matrix(T[:3, :3]).as_rotvec() for T in poses] - ).astype(np.float32) - - return {src.object_name: (poses, root_position, root_axis_angle, None)} - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple: - """Resolve the GRAB object mesh for this sequence. - - GRAB's canonical object meshes live at - ``{grab_dir}/tools/object_meshes/contact_meshes/{name}.ply``. We also - look at a committed copy under ``assets/meshes/grab/`` and (as a last - resort) at the flat ContactDB STL files at ``{grab_dir}/{name}.stl`` - — note the ContactDB names use underscores (``alarm_clock.stl``) while - GRAB npz/ply names don't (``alarmclock.ply``), so this fallback only - matches for the no-underscore objects. - """ - src: GRABSequenceSource = sequence_info.source - grab_dir = Path(getattr(self._args, "dataset_root", DEFAULT_GRAB_DIR)) - canonical_dir = Path(getattr(self._args, "mesh_dir", GRAB_MESH_DIR)) - contact_meshes_dir = grab_dir / "tools" / "object_meshes" / "contact_meshes" - - mesh_paths: dict[str, str] = {} - missing: list[str] = [] - for name in [src.object_name]: - chosen: Path | None = None - candidates = [ - canonical_dir / name / "mesh_tex.obj", - canonical_dir / f"{name}.ply", - canonical_dir / f"{name}.stl", - contact_meshes_dir / f"{name}.ply", - grab_dir / f"{name}.stl", - ] - for c in candidates: - if c.exists(): - chosen = c - break - if chosen is None: - missing.append(name) - continue - mesh_paths[name] = str(chosen) - - if missing: - raise FileNotFoundError( - f"GRAB object meshes missing for {sequence_info.sequence_id}: " - f"{missing}. Searched {canonical_dir} and {contact_meshes_dir}." - ) - return load_meshes_to_device(mesh_paths, device, vertex_scale=1.0) - - def get_mano_kwargs(self) -> dict[str, Any]: - """GRAB uses standard MANO axis-angle (no PCA) without joint centering.""" - return {"flat_hand_mean": False, "center_idx": None} - - def get_fps(self) -> float: - """GRAB native frame rate.""" - return GRAB_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return the mesh path that load_object_meshes would pick. - - Kept in sync with the candidate list in ``load_object_meshes``. - """ - src: GRABSequenceSource = sequence_info.source - grab_dir = Path(getattr(self._args, "dataset_root", DEFAULT_GRAB_DIR)) - canonical_dir = Path(getattr(self._args, "mesh_dir", GRAB_MESH_DIR)) - contact_meshes_dir = grab_dir / "tools" / "object_meshes" / "contact_meshes" - name = src.object_name - candidates = [ - canonical_dir / name / "mesh_tex.obj", - canonical_dir / f"{name}.ply", - canonical_dir / f"{name}.stl", - contact_meshes_dir / f"{name}.ply", - grab_dir / f"{name}.stl", - ] - for c in candidates: - if c.exists(): - return [str(c)] - return [str(candidates[0])] # path stub for downstream error reporting - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return rigid URDF path for the sequence's single object.""" - src: GRABSequenceSource = sequence_info.source - urdf_dir = Path(getattr(self._args, "object_model_root", GRAB_URDF_DIR)) - return [str(urdf_dir / f"{make_usd_safe(src.object_name)}_rigid.urdf")] - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the GRAB loader script.""" - parser = argparse.ArgumentParser( - description="Load GRAB sequences into ManoSharpaData schema." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=DEFAULT_GRAB_DIR, - object_model_root=GRAB_URDF_DIR, - mesh_dir=GRAB_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - parser.add_argument( - "--list_sequences", action="store_true", help="List sequences and exit." - ) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Run the GRAB loader.""" - loader = GRABDatasetLoader() - if args.list_sequences: - for s in loader.list_sequences(args): - print(s.sequence_id) - return - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/h2o_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/h2o_loader.py deleted file mode 100644 index b107c38c..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/h2o_loader.py +++ /dev/null @@ -1,737 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load H2O dataset (Hand-Two-Object, ICCV 2021) into ManoSharpaData schema. - -H2O layout (after extraction): - h2o_dir/ - subject1/ through subject4/ - {action_category}/ e.g. h1/, k2/ - {take_id}/ e.g. 0/, 1/ - cam{0-4}/ 5 cameras (cam4 is egocentric) - hand_pose_mano/ per-frame MANO params (plain text) - 000001.txt - 000002.txt - ... - obj_pose_rt/ per-frame object 4x4 poses (plain text) - 000001.txt - ... - cam_pose/ per-frame camera poses - rgb/ depth/ (only with --mode all; not needed for retargeting) - object/ object meshes - -Hand pose file format (per frame, 118 floats = 2 hands * 59 floats): - [left_hand(59), right_hand(59)] -- ordering per official repo - Each hand is: [flag(1), trans(3), pose(48), shape(10)] - - flag: 1=annotated, 0=not annotated - - trans: (tx, ty, tz) translation in camera frame - - pose: 48 axis-angle floats = 3 global rotation + 45 finger pose - - shape: 10 MANO betas - -Object pose file format (obj_pose_rt): - [class_id, 16 floats of 4x4 row-major transform matrix] in camera frame - -Coordinate frame: - Poses are stored in each (moving) camera's frame. H2O ships a per-frame - ``cam_pose/{fid}.txt`` camera-to-world extrinsic; the loader composes it - with each frame's hand and object poses so the output is in the - gravity-aligned world frame defined by H2O's external rig calibration. - -Runs stage 1 of the two-stage pipeline: - 1. python scripts/retarget/h2o_loader.py --save -> h2o_loaded/ - 2. python scripts/retarget/h2o_to_sharpa.py --save -> h2o_processed/ -""" - -from __future__ import annotations - -import argparse -import fcntl -import tarfile -import warnings -import zipfile -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from scipy.spatial.transform import Rotation - -warnings.filterwarnings("ignore", category=DeprecationWarning, module="mano") - -from robotic_grounding.retarget import ( # noqa: E402 - ASSETS_DIR, - HUMAN_MOTION_DATA_DIR, - MESHES_DIR, -) -from v2d.task_library_loader.lib.dataset_loader_base import ( # noqa: E402 - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - make_usd_safe, -) - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -DEFAULT_H2O_DIR = HUMAN_MOTION_DATA_DIR / "h2o" / "dataset" -H2O_URDF_DIR = ASSETS_DIR / "urdfs" / "h2o" -H2O_MESH_DIR = MESHES_DIR / "h2o" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "h2o" / "h2o_loaded" -H2O_FPS = 30.0 - -# H2O's world frame is the initial head-mounted camera pose in OpenCV -# convention: +X right, +Y **down** (gravity), +Z forward. Isaac Sim -# (and the rest of the pipeline) expects +Z up. Rotate −90° around X so -# H2O +Y (gravity) → Isaac Sim −Z (gravity) without flipping handedness: -# -# (x, y, z)_H2O → (x, z, −y)_ZUP -# -# Same shape as DexYCB's ``_MASTER_TO_ZUP`` — the rigs are both OpenCV. -# Without this rotation, hands end up ~60 cm *above* the head in the -# output parquet, with palms pointing along +Y (a horizontal direction) -# instead of down — which is what the Isaac Sim renders look like when -# compared against the paper's reference figures on the H2O project page. -H2O_WORLD_TO_ZUP = np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]], dtype=np.float32) -_H2O_WORLD_TO_ZUP_T4 = np.eye(4, dtype=np.float32) -_H2O_WORLD_TO_ZUP_T4[:3, :3] = H2O_WORLD_TO_ZUP - -# H2O object class IDs (from official dataset documentation). -# Index 0 ("background") is used when no object is active in a frame. -# TODO: verify this mapping against the actual data — in particular whether -# class IDs 1-8 are 1-indexed or 0-indexed in practice. -H2O_OBJECTS: dict[int, str] = { - 1: "book", - 2: "espresso", - 3: "lotion", - 4: "spray", - 5: "milk", - 6: "cocoa", - 7: "chips", - 8: "cappuccino", -} - -# Number of floats per hand in hand_pose_mano files. -# Layout: flag(1) + trans(3) + pose(48) + shape(10) = 62 per hand, 124 total. -H2O_PER_HAND_FLOATS = 62 - - -# --------------------------------------------------------------------------- -# Data structures -# --------------------------------------------------------------------------- -@dataclass -class H2OSequenceSource: - """Source info for one H2O sequence (a single camera view of a take).""" - - take_dir: Path # points at cam{N}/ directory - subject: str - action_category: str - take_id: str - camera: str - object_class_ids: list[int] - object_names: list[str] - - -# --------------------------------------------------------------------------- -# File parsers -# --------------------------------------------------------------------------- -def _parse_hand_pose_file( - path: Path, -) -> tuple[np.ndarray, np.ndarray, bool, bool]: - """Parse one hand_pose_mano text file. - - Returns: - left_data: (59,) array for left hand - right_data: (59,) array for right hand - left_valid: whether left hand is annotated - right_valid: whether right hand is annotated - """ - raw = np.loadtxt(path, dtype=np.float32) - if raw.size != 2 * H2O_PER_HAND_FLOATS: - raise ValueError( - f"{path}: expected {2 * H2O_PER_HAND_FLOATS} floats, got {raw.size}" - ) - left = raw[:H2O_PER_HAND_FLOATS] - right = raw[H2O_PER_HAND_FLOATS:] - left_valid = bool(left[0] > 0.5) - right_valid = bool(right[0] > 0.5) - return left, right, left_valid, right_valid - - -def _parse_object_pose_file(path: Path) -> tuple[int, np.ndarray]: - """Parse one obj_pose_rt text file. - - Returns: - class_id: integer object class ID (0 = no object). - T: (4, 4) homogeneous transform matrix in camera frame. - """ - raw = np.loadtxt(path, dtype=np.float32).ravel() - if raw.size < 17: - raise ValueError(f"{path}: expected 17 floats, got {raw.size}") - class_id = int(raw[0]) - T = raw[1:17].reshape(4, 4) - return class_id, T - - -def _parse_cam_pose_file(path: Path) -> np.ndarray: - """Parse one cam_pose text file into a (4, 4) camera-to-world matrix.""" - raw = np.loadtxt(path, dtype=np.float32).ravel() - if raw.size != 16: - raise ValueError(f"{path}: expected 16 floats, got {raw.size}") - return raw.reshape(4, 4) - - -def _load_cam_pose_series(cam_pose_dir: Path, frame_ids: list[str]) -> np.ndarray: - """Return (N, 4, 4) cam → Isaac-Z-up world series, one matrix per frame. - - H2O ships a per-frame ``cam_pose/{fid}.txt`` camera→world extrinsic, - where "world" is H2O's own frame: initial head-mounted camera in - OpenCV convention (+Y down = gravity). Isaac Sim and the rest of - the pipeline expect +Z up, so we pre-multiply each matrix by - :data:`_H2O_WORLD_TO_ZUP_T4`. Downstream call sites then compose - ``cam_pose[i] @ `` and land in Isaac-world - directly — no per-site rotation bookkeeping. - - Missing files emit a warning and fall back to just the Y-down→Z-up - rotation (cam-frame output, still Z-up); this keeps the loader - runnable against H2O variants that omit ``cam_pose/``. - """ - series = np.tile(_H2O_WORLD_TO_ZUP_T4, (len(frame_ids), 1, 1)).copy() - if not cam_pose_dir.is_dir(): - warnings.warn( - f"cam_pose directory missing at {cam_pose_dir}; " - "falling back to Y-down→Z-up only (no per-frame extrinsic).", - stacklevel=2, - ) - return series - missing = 0 - for i, fid in enumerate(frame_ids): - pose_file = cam_pose_dir / f"{fid}.txt" - if not pose_file.exists(): - missing += 1 - continue - series[i] = _H2O_WORLD_TO_ZUP_T4 @ _parse_cam_pose_file(pose_file) - if missing: - warnings.warn( - f"cam_pose missing for {missing}/{len(frame_ids)} frames under " - f"{cam_pose_dir}; using identity (pre-rotated) for those frames.", - stacklevel=2, - ) - return series - - -def _collect_frame_ids(cam_dir: Path) -> list[str]: - """Return sorted frame IDs (stems like '000001') present in the cam dir.""" - hand_dir = cam_dir / "hand_pose_mano" - if not hand_dir.exists(): - return [] - return sorted(p.stem for p in hand_dir.glob("*.txt")) - - -def _extract_archives_if_needed(h2o_dir: Path) -> Path: - """Extract H2O tarballs/zips if the dir hasn't been extracted yet. - - H2O ships as: - subject{1,2,3,4}_pose_v1_1.tar.gz (per-subject pose data) - object.zip (object meshes) - label_split.zip (annotations) - - Extraction is idempotent: if the target subject*/ directories already - exist, we skip. Returns the directory containing the extracted data - (same as input if already extracted, or a local cache if input is - read-only CSS mount). - """ - archives = sorted(h2o_dir.glob("*.tar.gz")) + sorted(h2o_dir.glob("*.zip")) - if not archives: - return h2o_dir # nothing to extract; assume already-extracted data - - # Already extracted? Check if each archive has a corresponding extracted - # directory (e.g. subject1_pose_v1_1.tar.gz -> subject1/). - def _already_extracted(archive: Path) -> bool: - name = archive.name - if name.startswith("subject") and "_pose_" in name: - return (h2o_dir / name.split("_pose_")[0]).is_dir() - # Generic check: any dir derived from the archive name - stem = ( - archive.stem - if archive.suffix == ".zip" - else archive.stem.replace(".tar", "") - ) - return (h2o_dir / stem).is_dir() - - archives = [a for a in archives if not _already_extracted(a)] - if not archives: - return h2o_dir - - # Serialize extraction across shards via an advisory file lock — the - # first shard extracts while the others block, then re-check and find - # `_already_extracted` True. - lock_path = Path("/tmp") / f"h2o_extract_{h2o_dir.name}.lock" - with open(lock_path, "w") as lock: - fcntl.flock(lock.fileno(), fcntl.LOCK_EX) - # Re-check under the lock — a sibling shard may have finished while we waited. - archives = [a for a in archives if not _already_extracted(a)] - if not archives: - return h2o_dir - - # If h2o_dir is writable, extract in place. Otherwise use a cache dir. - target = h2o_dir - try: - (h2o_dir / ".extract_test").touch() - (h2o_dir / ".extract_test").unlink() - except OSError: - target = Path("/tmp") / f"h2o_extracted_{h2o_dir.name}" - target.mkdir(parents=True, exist_ok=True) - print(f"[h2o] input dir read-only, extracting to cache: {target}") - - for archive in archives: - print(f"[h2o] extracting {archive.name} -> {target}") - if archive.suffix == ".zip": - with zipfile.ZipFile(archive) as z: - z.extractall(target) - elif archive.name.endswith(".tar.gz") or archive.name.endswith(".tgz"): - with tarfile.open(archive, "r:gz") as t: - t.extractall(target) - else: - print(f"[h2o] skipping unknown archive type: {archive}") - - return target - - -# --------------------------------------------------------------------------- -# Loader -# --------------------------------------------------------------------------- -class H2ODatasetLoader(DatasetLoaderBase): - """Load H2O sequences into ManoSharpaData (MANO + object only).""" - - def __init__(self) -> None: - """Initialize cached per-sequence object-id and cam-pose lookups.""" - self._args: argparse.Namespace | None = None - # Cache object class IDs discovered per sequence so load_object_data - # and get_object_mesh_paths can reuse the list built during - # list_sequences / load_mano_data. - self._object_ids_cache: dict[str, list[int]] = {} - # Cache per-frame cam-to-world extrinsics so load_mano_data and - # load_object_data both transform into the same world frame without - # re-reading cam_pose/*.txt twice. - self._cam_pose_cache: dict[str, np.ndarray] = {} - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """Discover H2O sequences (subject / action / take / camera).""" - self._args = args - h2o_dir = Path(getattr(args, "dataset_root", DEFAULT_H2O_DIR)) - camera = getattr(args, "camera", "cam4") # cam4 = egocentric - - if not h2o_dir.exists(): - raise FileNotFoundError(f"H2O dataset not found at {h2o_dir}") - - # H2O ships as tarballs; extract them the first time we see them. - h2o_dir = _extract_archives_if_needed(h2o_dir) - - sequences: list[SequenceInfo] = [] - for subject_dir in sorted(h2o_dir.iterdir()): - if not subject_dir.is_dir() or not subject_dir.name.startswith("subject"): - continue - subject = subject_dir.name - for action_dir in sorted(subject_dir.iterdir()): - if not action_dir.is_dir(): - continue - for take_dir in sorted(action_dir.iterdir()): - if not take_dir.is_dir(): - continue - cam_dir = take_dir / camera - if not cam_dir.exists(): - continue - - sequence_id = ( - f"h2o_{subject}_{action_dir.name}_{take_dir.name}_{camera}" - ) - # Object IDs are discovered lazily in load_mano_data. - source = H2OSequenceSource( - take_dir=cam_dir, - subject=subject, - action_category=action_dir.name, - take_id=take_dir.name, - camera=camera, - object_class_ids=[], - object_names=[], - ) - sequences.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=str(cam_dir), - object_name="", - object_body_names=[], - source=source, - ) - ) - - sequences = self._apply_sequence_filters(sequences, args) - print(f"Found {len(sequences)} H2O sequences") - return sequences - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO hand parameters from H2O text files, one per frame.""" - src: H2OSequenceSource = sequence_info.source - frame_ids = _collect_frame_ids(src.take_dir) - if not frame_ids: - raise FileNotFoundError(f"No hand pose frames in {src.take_dir}") - - right_g_list: list[np.ndarray] = [] - right_p_list: list[np.ndarray] = [] - right_t_list: list[np.ndarray] = [] - left_g_list: list[np.ndarray] = [] - left_p_list: list[np.ndarray] = [] - left_t_list: list[np.ndarray] = [] - right_betas = None - left_betas = None - - # Track object class IDs that appear in the obj_pose_rt files so we - # can reuse them in load_object_data and get_object_mesh_paths. - obj_ids_seen: list[int] = [] - # Per-frame object cam-frame translations (only frames with a valid - # class_id). Used below to compute z_lift so Isaac Sim's floor plane - # ends up below the scene. - obj_t_cams_per_frame: list[np.ndarray] = [] - - for fid in frame_ids: - left_raw, right_raw, left_valid, right_valid = _parse_hand_pose_file( - src.take_dir / "hand_pose_mano" / f"{fid}.txt" - ) - - # H2O stores a validity flag at index 0 of each per-hand vector. - # When a hand is not annotated the remaining floats are zero/junk, - # which fed into MANO FK + IK produces degenerate targets. Skip - # the entire sequence rather than letting garbage frames through. - if not (left_valid and right_valid): - raise ValueError( - f"Frame {fid}: hand annotation missing " - f"(left_valid={left_valid}, right_valid={right_valid})" - ) - - # Split each 62-float vector: flag(1) trans(3) pose(48) shape(10) - right_g_list.append(right_raw[4:7]) - right_p_list.append(right_raw[7:52]) - right_t_list.append(right_raw[1:4]) - left_g_list.append(left_raw[4:7]) - left_p_list.append(left_raw[7:52]) - left_t_list.append(left_raw[1:4]) - if right_betas is None: - right_betas = right_raw[52:62].copy() - if left_betas is None: - left_betas = left_raw[52:62].copy() - - # Object: read the class_id and cam-frame translation from - # obj_pose_rt for this frame. The translation feeds z_lift below. - obj_file = src.take_dir / "obj_pose_rt" / f"{fid}.txt" - if obj_file.exists(): - class_id, T_obj_cam = _parse_object_pose_file(obj_file) - if class_id > 0: - if class_id not in obj_ids_seen: - obj_ids_seen.append(class_id) - obj_t_cams_per_frame.append(T_obj_cam[:3, 3]) - - H = len(frame_ids) - right_global_orient = np.stack(right_g_list).astype(np.float32) - right_finger_pose = np.stack(right_p_list).astype(np.float32) - right_trans = np.stack(right_t_list).astype(np.float32) - left_global_orient = np.stack(left_g_list).astype(np.float32) - left_finger_pose = np.stack(left_p_list).astype(np.float32) - left_trans = np.stack(left_t_list).astype(np.float32) - - # H2O stores per-frame hand/object poses in the (moving) egocentric - # camera frame. Applying a fixed cam->world rotation leaves gravity - # tilting with the subject's head; instead, transform each frame by - # the per-frame cam_pose/*.txt extrinsic (camera-to-world). The - # resulting world frame is gravity-aligned by the H2O rig calibration. - cam_pose = _load_cam_pose_series(src.take_dir / "cam_pose", frame_ids) - - # Fold a per-sequence z_lift into cam_pose translation so the lowest - # point in the scene (hand trans or object origin) lands at least - # _GROUND_MARGIN above Isaac Sim's floor plane at z = 0. Without it, - # H2O's world origin (= initial head pose) puts tabletops and held - # objects at negative z — they clip through the floor and - # reconstruct_support_surfaces.py's 0.05 m ground filter drops every - # point, so Stage 3 produces no support surface and Stage 5 (URDF - # generation + video) has nothing to spawn. 1 m is generous enough - # to clear the tallest H2O object half-extent (chips: 0.131 m) - # regardless of its orientation. Same idiom as the other loaders. - _GROUND_MARGIN = 1.0 - min_z = np.inf - for i in range(H): - R_cw = cam_pose[i, :3, :3] - t_cw = cam_pose[i, :3, 3] - min_z = min( - min_z, - float((R_cw @ right_trans[i] + t_cw)[2]), - float((R_cw @ left_trans[i] + t_cw)[2]), - ) - # Objects: cam_pose[0] is good enough for the bound since the head - # moves ≤ a few cm in typical H2O sequences. - if obj_t_cams_per_frame: - R0, t0 = cam_pose[0, :3, :3], cam_pose[0, :3, 3] - for t_obj_cam in obj_t_cams_per_frame: - min_z = min(min_z, float((R0 @ t_obj_cam + t0)[2])) - z_lift = max(0.0, _GROUND_MARGIN - (min_z if np.isfinite(min_z) else 0.0)) - if z_lift > 0.0: - cam_pose[:, 2, 3] += z_lift - - self._cam_pose_cache[sequence_info.sequence_id] = cam_pose - for i in range(H): - R_cw = cam_pose[i, :3, :3] - t_cw = cam_pose[i, :3, 3] - R_r = Rotation.from_rotvec(right_global_orient[i]).as_matrix() - right_global_orient[i] = Rotation.from_matrix(R_cw @ R_r).as_rotvec() - R_l = Rotation.from_rotvec(left_global_orient[i]).as_matrix() - left_global_orient[i] = Rotation.from_matrix(R_cw @ R_l).as_rotvec() - right_trans[i] = R_cw @ right_trans[i] + t_cw - left_trans[i] = R_cw @ left_trans[i] + t_cw - - # Update the source with discovered objects so downstream methods can - # fill object_body_names / mesh paths consistently. - src.object_class_ids = obj_ids_seen - src.object_names = [ - H2O_OBJECTS.get(cid, f"object_{cid}") for cid in obj_ids_seen - ] - sequence_info.object_body_names = src.object_names - sequence_info.object_name = ( - "+".join(src.object_names) if src.object_names else "object" - ) - self._object_ids_cache[sequence_info.sequence_id] = obj_ids_seen - - if right_betas is None or left_betas is None: - raise ValueError(f"No MANO betas for {sequence_info.sequence_id}") - - return { - "H": H, - "right_global_orient": torch.from_numpy(right_global_orient).to(device), - "right_finger_pose": torch.from_numpy(right_finger_pose).to(device), - "right_trans": torch.from_numpy(right_trans).to(device), - "right_betas": torch.from_numpy(right_betas).to(device), - "right_fitting_err": torch.zeros(H, device=device), - "left_global_orient": torch.from_numpy(left_global_orient).to(device), - "left_finger_pose": torch.from_numpy(left_finger_pose).to(device), - "left_trans": torch.from_numpy(left_trans).to(device), - "left_betas": torch.from_numpy(left_betas).to(device), - "left_fitting_err": torch.zeros(H, device=device), - } - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load per-frame object 4x4 poses from obj_pose_rt files.""" - src: H2OSequenceSource = sequence_info.source - frame_ids = _collect_frame_ids(src.take_dir) - obj_ids = self._object_ids_cache.get( - sequence_info.sequence_id, src.object_class_ids - ) - if not obj_ids: - raise ValueError( - f"No object class IDs for {sequence_info.sequence_id}. " - "Did load_mano_data run first?" - ) - - # For each frame, the H2O file stores a single active object (the one - # being manipulated). For frames where a given object is not active, - # we fall back to the previous known pose (and zero if never seen). - N = len(frame_ids) - - # Use the per-frame cam-to-world extrinsic populated by load_mano_data; - # fall back to loading directly if the cache is cold (e.g. object-only - # invocation path). - cam_pose = self._cam_pose_cache.get(sequence_info.sequence_id) - if cam_pose is None: - cam_pose = _load_cam_pose_series(src.take_dir / "cam_pose", frame_ids) - - poses = { - cid: np.tile(np.eye(4, dtype=np.float32), (N, 1, 1)) for cid in obj_ids - } - last_T: dict[int, np.ndarray] = { - cid: np.eye(4, dtype=np.float32) for cid in obj_ids - } - for f_idx, fid in enumerate(frame_ids): - obj_file = src.take_dir / "obj_pose_rt" / f"{fid}.txt" - if not obj_file.exists(): - for cid in obj_ids: - poses[cid][f_idx] = last_T[cid] - continue - class_id, T = _parse_object_pose_file(obj_file) - # Transform cam-frame object pose to gravity-aligned world frame - # via this frame's cam-to-world extrinsic. - T_world = cam_pose[f_idx] @ T - for cid in obj_ids: - if cid == class_id: - poses[cid][f_idx] = T_world - last_T[cid] = T_world - else: - poses[cid][f_idx] = last_T[cid] - - result: dict[str, Any] = {} - for cid in obj_ids: - name = H2O_OBJECTS.get(cid, f"object_{cid}") - pose_seq = poses[cid] - root_pos = pose_seq[:, :3, 3].copy() - root_aa = np.stack( - [Rotation.from_matrix(T[:3, :3]).as_rotvec() for T in pose_seq] - ).astype(np.float32) - result[name] = (pose_seq, root_pos, root_aa, None) - return result - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple: - """Load OBJ meshes for each object present in this sequence. - - Searches, per object name: - 1. ``assets/meshes/h2o/{name}/*.obj`` (canonical committed location) - 2. ``{h2o_dir}/object/{name}/*.obj`` (extracted fallback from object.zip) - - Uses ``*.obj`` (glob) rather than a fixed name because H2O is - inconsistent: e.g. the ``spray`` folder contains ``lotion_spray.obj``. - - Raises FileNotFoundError if any mesh is missing so the base class's - sequence-level try/except can skip the sequence cleanly. - """ - src: H2OSequenceSource = sequence_info.source - canonical_dir = Path(getattr(self._args, "mesh_dir", H2O_MESH_DIR)) - h2o_dir = Path(getattr(self._args, "dataset_root", DEFAULT_H2O_DIR)) - fallback_dir = h2o_dir / "object" - - mesh_paths: dict[str, str] = {} - missing: list[str] = [] - for name in src.object_names: - candidate = None - for base in (canonical_dir, fallback_dir): - objs = ( - sorted((base / name).glob("*.obj")) - if (base / name).is_dir() - else [] - ) - if objs: - candidate = objs[0] - break - if candidate is None: - missing.append(name) - continue - mesh_paths[name] = str(candidate) - - if missing: - raise FileNotFoundError( - f"H2O object meshes missing for {sequence_info.sequence_id}: " - f"{missing}. Searched {canonical_dir} and {fallback_dir}." - ) - return load_meshes_to_device(mesh_paths, device, vertex_scale=1.0) - - def get_mano_kwargs(self) -> dict[str, Any]: - """Return MANO layer kwargs for H2O. - - H2O stores absolute MANO axis-angle (not deltas from the MANO - dataset-mean finger pose), so ``flat_hand_mean=True`` is correct — - it tells the MANO layer to treat our 45-D finger_pose as the final - pose rather than adding ``hands_mean`` on top. This matches the - official ``taeinkwon/h2oplayer`` viewer, which constructs - ``ManoLayer(use_pca=False, flat_hand_mean=True, side=…)``. - - Using ``flat_hand_mean=False`` (the previous setting) silently - adds the MANO mean finger curl to the already-curled H2O pose; - the shifted joint-0 position then cascades through the IK and - shows up as a ~45° wrist pitch offset in the Isaac Sim render. - """ - return {"flat_hand_mean": True, "center_idx": None} - - def get_fps(self) -> float: - """H2O frame rate.""" - return H2O_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return OBJ mesh paths for all objects in the sequence. - - Mirrors the search order of :meth:`load_object_meshes` so the paths - stored in the Parquet actually resolve at later stages - (reconstruct / vis / video). Searches, per object name: - - 1. ``assets/meshes/h2o/{name}/*.obj`` (canonical committed copy) - 2. ``{h2o_dir}/object/{name}/*.obj`` (raw dataset fallback) - - H2O mesh filenames don't always match the folder (e.g. ``spray/`` - contains ``lotion_spray.obj``) so we glob for ``*.obj``. If nothing - is found, we still emit a path under the canonical dir so the - Parquet column is well-typed — downstream consumers will then skip - the object with a clear "no mesh loaded" warning instead of - segfaulting. - """ - src: H2OSequenceSource = sequence_info.source - canonical_dir = Path(getattr(self._args, "mesh_dir", H2O_MESH_DIR)) - h2o_dir = Path(getattr(self._args, "dataset_root", DEFAULT_H2O_DIR)) - fallback_dir = h2o_dir / "object" - paths: list[str] = [] - for name in src.object_names: - chosen: Path | None = None - for base in (canonical_dir, fallback_dir): - objs = ( - sorted((base / name).glob("*.obj")) - if (base / name).is_dir() - else [] - ) - if objs: - chosen = objs[0] - break - paths.append( - str(chosen) if chosen else str(canonical_dir / name / f"{name}.obj") - ) - return paths - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return rigid URDF paths for all objects in the sequence.""" - src: H2OSequenceSource = sequence_info.source - urdf_dir = Path(getattr(self._args, "object_model_root", H2O_URDF_DIR)) - return [ - str(urdf_dir / f"{make_usd_safe(name)}_rigid.urdf") - for name in src.object_names - ] - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the H2O loader script.""" - parser = argparse.ArgumentParser( - description="Load H2O sequences into ManoSharpaData schema." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=DEFAULT_H2O_DIR, - object_model_root=H2O_URDF_DIR, - mesh_dir=H2O_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - parser.add_argument( - "--camera", - type=str, - default="cam4", - help="Which camera view to use (default: cam4, egocentric).", - ) - parser.add_argument( - "--list_sequences", action="store_true", help="List sequences and exit." - ) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Run the H2O loader.""" - loader = H2ODatasetLoader() - if args.list_sequences: - for s in loader.list_sequences(args): - print(s.sequence_id) - return - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/hot3d_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/hot3d_loader.py deleted file mode 100644 index b4c47b57..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/hot3d_loader.py +++ /dev/null @@ -1,739 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load Hot3D dataset into ManoSharpaData schema (MANO + object only, no robot). - -Hot3D layout: - hot3d_dir/ - {seq_name}/ e.g. P0001_8d136980/ - mano_hand_pose_trajectory.jsonl per-timestamp MANO poses (PCA format) - dynamic_objects.csv per-timestamp object world poses - metadata.json object_uids, object_names, participant_id - headset_trajectory.csv headset world pose per timestamp - masks/ quality masks (optional) - assets/meshes/hot3d/ - {object_uid}.glb object mesh files (meters) - -Coordinate frame: - Aria world frames are already Z-UP (gravity_z = -9.81). - Quest3 world frames are Y-UP and are rotated to Z-UP via QUEST3_WORLD_TO_ZUP. - - After gravity alignment, the yaw (rotation around Z) is arbitrary per session. - This loader also applies a per-sequence yaw normalization so that all scenes - start with the headset's initial forward direction pointing toward +Y. The - normalization rotation is read from the first frame of headset_trajectory.csv. - -MANO format: - Poses are stored as 15 PCA coefficients (use_pca=True, num_pca_comps=15). - This loader expands them to full 45-DOF rotation vectors before passing - to our MANO wrapper (which expects use_pca=False, 45-DOF finger pose). - Hand index 0 = left, hand index 1 = right. - -Timestamp handling: - Each "real" frame at ~30 Hz has 2-3 camera-stream timestamps within ~0.1 ms. - This loader deduplicates to keep one timestamp per ≥10 ms window (~30 Hz). - -Runs stage 1 of the two-stage pipeline: - 1. python scripts/retarget/hot3d_loader.py --save -> hot3d_loaded/ - 2. python scripts/retarget/hot3d_to_sharpa.py --save -> hot3d_processed/ -""" - -import argparse -import csv -import json -import logging -import warnings -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch - -warnings.filterwarnings("ignore", category=DeprecationWarning, module="mano") - -from manotorch.manolayer import ManoLayer # noqa: E402 -from robotic_grounding.retarget import ( # noqa: E402 - HUMAN_MOTION_DATA_DIR, - MESHES_DIR, -) -from v2d.task_library_loader.lib.dataset_loader_base import ( # noqa: E402 - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - poses_to_root_position_and_axis_angle, -) -from scipy.spatial.transform import Rotation # noqa: E402 - -logging.getLogger().setLevel(logging.ERROR) -warnings.filterwarnings("ignore", category=UserWarning, module="manotorch") - -DEFAULT_HOT3D_DIR = HUMAN_MOTION_DATA_DIR / "hot3d" / "dataset" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "hot3d" / "hot3d_loaded" -HOT3D_MESH_DIR = MESHES_DIR / "hot3d" -HOT3D_URDF_DIR = MESHES_DIR.parent / "urdfs" / "hot3d" -HOT3D_FPS = 30.0 - -# Quest3 world frame is Y-UP; Aria world frame is Z-UP (our pipeline convention). -# Rotate 90° around X to bring Quest3 data into Z-UP: -# X → X, Y → Z, Z → -Y -# Gravity in Quest3: (0, -9.81, 0) → after R: (0, 0, -9.81) ✓ -QUEST3_WORLD_TO_ZUP: np.ndarray = np.array( - [[1, 0, 0], [0, 0, -1], [0, 1, 0]], dtype=np.float32 -) - -# Minimum nanosecond gap between consecutive frames to keep (10 ms → ~30 Hz). -_MIN_FRAME_GAP_NS = 10_000_000 # 10 ms - - -def _load_headset_initial_position( - seq_dir: Path, R_coord: np.ndarray -) -> np.ndarray | None: - """Read the first frame of headset_trajectory.csv and return the headset position in Z-UP frame. - - Returns None if the file is absent or empty. - """ - traj_path = seq_dir / "headset_trajectory.csv" - if not traj_path.exists(): - return None - with traj_path.open("r") as f: - reader = csv.DictReader(f) - first_row = next(reader, None) - if first_row is None: - return None - tx = float(first_row["t_wo_x[m]"]) - ty = float(first_row["t_wo_y[m]"]) - tz = float(first_row["t_wo_z[m]"]) - return (R_coord @ np.array([tx, ty, tz], dtype=np.float32)).astype(np.float32) - - -def _yaw_cancel_headset_to_scene( - headset_pos_zup: np.ndarray, scene_center_xy: np.ndarray -) -> np.ndarray: - """Compute a Z-rotation so the direction from headset to workspace points to +Y. - - The key insight: in each recording session the SLAM/tracking world frame yaw - is arbitrary. The headset is at the person's head; the workspace (table) is - at some horizontal direction from the person. After normalising, all sessions - have the workspace in the +Y direction from the headset's starting position, - giving a consistent orientation for all sequences. - - Args: - headset_pos_zup: (3,) headset position in Z-UP world frame. - scene_center_xy: (2,) mean XY of all object + hand positions (scene centre - in the same Z-UP frame, before scene-offset subtraction). - - Returns: - 3×3 float32 rotation matrix R_yaw such that - R_yaw @ (scene_center_xy - headset_pos_zup[:2]) ∝ [0, 1]. - """ - delta_xy = scene_center_xy - headset_pos_zup[:2] # direction toward workspace - if np.linalg.norm(delta_xy) < 1e-3: - return np.eye(3, dtype=np.float32) # degenerate case - yaw_dir = np.arctan2(delta_xy[1], delta_xy[0]) - # Rotate so that delta_xy points to +Y (yaw = 90°), then add 90° extra. - cancel_angle = np.pi / 2.0 - yaw_dir + np.pi / 2.0 # = π - yaw_dir - return Rotation.from_euler("z", cancel_angle).as_matrix().astype(np.float32) - - -# --------------------------------------------------------------------------- -# Dataset-specific metadata -# --------------------------------------------------------------------------- - - -@dataclass -class Hot3DSequenceSource: - """Dataset-specific metadata stored in SequenceInfo.source.""" - - seq_dir: Path - hot3d_dir: Path - object_uids: list[str] # from metadata.json "object_uids" - object_names: list[str] # from metadata.json "object_names" - headset: str # "Aria" or "Quest3" from metadata.json - - -# --------------------------------------------------------------------------- -# PCA expansion helpers -# --------------------------------------------------------------------------- - - -def _load_pca_components( - mano_assets_root: str, ncomps: int = 15 -) -> tuple[np.ndarray, np.ndarray]: - """Load MANO PCA components for right and left hands using manotorch. - - Returns: - right_components: (ncomps, 45) PCA basis for right hand. - left_components: (ncomps, 45) PCA basis for left hand. - (The mean pose is NOT returned here — it is handled by the MANO wrapper - internally when flat_hand_mean=False.) - """ - right_layer = ManoLayer( - use_pca=True, - ncomps=ncomps, - flat_hand_mean=False, - side="right", - mano_assets_root=mano_assets_root, - ) - left_layer = ManoLayer( - use_pca=True, - ncomps=ncomps, - flat_hand_mean=False, - side="left", - mano_assets_root=mano_assets_root, - ) - right_components = right_layer.th_selected_comps.detach().cpu().numpy() # (15, 45) - left_components = left_layer.th_selected_comps.detach().cpu().numpy() # (15, 45) - return right_components, left_components - - -def _expand_pca(pca_coeffs: np.ndarray, components: np.ndarray) -> np.ndarray: - """Expand PCA coefficients to full 45-DOF finger pose (WITHOUT mean). - - Args: - pca_coeffs: (N, 15) PCA coefficients. - components: (15, 45) PCA basis matrix from _load_pca_components. - - Returns: - (N, 45) full finger pose (deviation from mean). The MANO wrapper with - flat_hand_mean=False will add the actual mean pose internally. - """ - return (pca_coeffs @ components).astype(np.float32) - - -# --------------------------------------------------------------------------- -# Data loading helpers -# --------------------------------------------------------------------------- - - -def _deduplicate_timestamps( - timestamps: list[int], min_gap_ns: int = _MIN_FRAME_GAP_NS -) -> list[int]: - """Keep at most one timestamp per min_gap_ns window (deduplicates camera streams). - - Hot3D records two camera streams per physical frame (~0.1 ms apart at ~30 Hz). - This keeps only the first timestamp in each cluster. - """ - if not timestamps: - return [] - result = [timestamps[0]] - for ts in timestamps[1:]: - if ts - result[-1] >= min_gap_ns: - result.append(ts) - return result - - -def _load_mano_jsonl(jsonl_path: Path) -> dict[int, dict]: - """Load mano_hand_pose_trajectory.jsonl. - - Returns dict mapping timestamp_ns -> hand_poses dict (keys "0"=left, "1"=right). - """ - data: dict[int, dict] = {} - with jsonl_path.open("r") as f: - for line in f: - entry = json.loads(line) - ts = int(entry["timestamp_ns"]) - if ts not in data: - data[ts] = entry["hand_poses"] - return data - - -def _load_object_csv(csv_path: Path) -> dict[int, dict[str, np.ndarray]]: - """Load dynamic_objects.csv. - - Returns dict mapping timestamp_ns -> {object_uid: (4,4) world transform}. - The transform is T_world_object (world frame pose of the object). - """ - result: dict[int, dict[str, np.ndarray]] = {} - with csv_path.open("r") as f: - reader = csv.DictReader(f) - for row in reader: - ts = int(row["timestamp[ns]"]) - uid = row["object_uid"] - tx = float(row["t_wo_x[m]"]) - ty = float(row["t_wo_y[m]"]) - tz = float(row["t_wo_z[m]"]) - qw = float(row["q_wo_w"]) - qx = float(row["q_wo_x"]) - qy = float(row["q_wo_y"]) - qz = float(row["q_wo_z"]) - R_mat = Rotation.from_quat([qx, qy, qz, qw]).as_matrix() - T = np.eye(4, dtype=np.float64) - T[:3, :3] = R_mat - T[:3, 3] = [tx, ty, tz] - if ts not in result: - result[ts] = {} - result[ts][uid] = T - return result - - -def _extract_hand_frame( - hand_entry: dict, - right_components: np.ndarray, - left_components: np.ndarray, -) -> tuple[ - np.ndarray, - np.ndarray, - np.ndarray, - np.ndarray, - np.ndarray, - np.ndarray, - np.ndarray, - np.ndarray, -]: - """Extract (global_orient_rv, finger_pose_45, trans, betas) for both hands. - - hand_entry: {"0": {...left...}, "1": {...right...}} from mano_hand_pose_trajectory.jsonl - Returns: (rg, rp, rt, rb, lg, lp, lt, lb) — all np.float32. - """ - - def _parse_hand( - h: dict, components: np.ndarray - ) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - pose_pca = np.array(h["pose"], dtype=np.float32) # (15,) - w = h["wrist_xform"] - q_wxyz = np.array(w["q_wxyz"], dtype=np.float32) # [w,x,y,z] - t_xyz = np.array(w["t_xyz"], dtype=np.float32) # (3,) - betas = np.array(h["betas"], dtype=np.float32)[:10] # (10,) - global_orient = ( - Rotation.from_quat(q_wxyz, scalar_first=True).as_rotvec().astype(np.float32) - ) # (3,) - finger_pose = _expand_pca(pose_pca[None], components)[0] # (45,) - return global_orient, finger_pose, t_xyz, betas - - rg, rp, rt, rb = _parse_hand(hand_entry["1"], right_components) # "1" = right - lg, lp, lt, lb = _parse_hand(hand_entry["0"], left_components) # "0" = left - return rg, rp, rt, rb, lg, lp, lt, lb - - -# --------------------------------------------------------------------------- -# Dataset loader -# --------------------------------------------------------------------------- - - -class Hot3DDatasetLoader(DatasetLoaderBase): - """Hot3D dataset loader.""" - - def __init__(self) -> None: - """Initialize caches; MANO layers are built lazily from --mano_model_dir.""" - super().__init__() - # The PCA MANO layers need the assets dir from --mano_model_dir, which is - # only available once run() sets self._args. Build them lazily on first - # use (see _ensure_mano_layers) instead of hardcoding a baked-in path — - # the OSMO load fetches MANO from swift at runtime. - self._right_mano_layer: ManoLayer | None = None - self._left_mano_layer: ManoLayer | None = None - self._right_components: np.ndarray | None = None - self._left_components: np.ndarray | None = None - self._valid_frame_ids_cache: dict[str, list[int]] = {} - self._scene_offset_cache: dict[str, np.ndarray] = {} - self._r_total_cache: dict[str, np.ndarray] = ( - {} - ) # combined coord+yaw rotation per sequence - - def _ensure_mano_layers(self) -> None: - """Build the PCA MANO layers from --mano_model_dir (once). - - Hot3D stores hand poses as 15-component PCA, so we need ManoLayer both - for the PCA component matrix (to expand to 45-DOF axis-angle) and for the - Quest3 J_shaped[0] zero-pose forward. The assets dir comes from - ``self._args.mano_model_dir`` (set by the base ``run()``), matching the - args-first contract the rest of the loaders use. - """ - if self._right_mano_layer is not None: - return - mano_assets_root = str(self._args.mano_model_dir) - self._right_mano_layer = ManoLayer( - use_pca=True, - ncomps=15, - flat_hand_mean=False, - side="right", - mano_assets_root=mano_assets_root, - ) - self._left_mano_layer = ManoLayer( - use_pca=True, - ncomps=15, - flat_hand_mean=False, - side="left", - mano_assets_root=mano_assets_root, - ) - self._right_components = ( - self._right_mano_layer.th_selected_comps.detach().cpu().numpy() - ) - self._left_components = ( - self._left_mano_layer.th_selected_comps.detach().cpu().numpy() - ) - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """Discover all sequences in hot3d_dir (subdirectories with MANO JSONL).""" - hot3d_dir = Path(args.dataset_root) - seq_name_filter = getattr(args, "seq_name", None) - - seq_dirs = sorted( - p - for p in hot3d_dir.iterdir() - if p.is_dir() and (p / "mano_hand_pose_trajectory.jsonl").exists() - ) - if seq_name_filter: - seq_dirs = [p for p in seq_dirs if seq_name_filter in p.name] - - out: list[SequenceInfo] = [] - for seq_dir in seq_dirs: - meta_path = seq_dir / "metadata.json" - if not meta_path.exists(): - continue - meta = json.loads(meta_path.read_text()) - if not meta.get("have_hand_object_pose_gt", False): - continue - - object_uids: list[str] = [str(uid) for uid in meta.get("object_uids", [])] - object_names: list[str] = list(meta.get("object_names", object_uids)) - if not object_uids: - continue - - headset: str = meta.get("headset", "Aria") # "Aria" or "Quest3" - sequence_id = seq_dir.name - object_name = "+".join(object_names[:3]) # summary name (first 3 objects) - out.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=sequence_id, - object_name=object_name, - object_body_names=object_uids, - source=Hot3DSequenceSource( - seq_dir=seq_dir, - hot3d_dir=hot3d_dir, - object_uids=object_uids, - object_names=object_names, - headset=headset, - ), - ) - ) - return out - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO parameters from Hot3D JSONL; expand PCA to 45-DOF.""" - self._ensure_mano_layers() - src: Hot3DSequenceSource = sequence_info.source - mano_data = _load_mano_jsonl(src.seq_dir / "mano_hand_pose_trajectory.jsonl") - obj_data_all = _load_object_csv(src.seq_dir / "dynamic_objects.csv") - - # Deduplicate timestamps (multiple camera streams per real frame) - all_ts = sorted(mano_data.keys()) - dedup_ts = _deduplicate_timestamps(all_ts) - - # Filter to timestamps where ALL object UIDs have pose data - valid_ts: list[int] = [] - for ts in dedup_ts: - if ts not in mano_data: - continue - if not all( - ts in obj_data_all and uid in obj_data_all[ts] - for uid in src.object_uids - ): - continue - # Both hands must be present - hp = mano_data[ts] - if "0" not in hp or "1" not in hp: - continue - valid_ts.append(ts) - - if not valid_ts: - raise ValueError(f"No valid frames in {src.seq_dir.name}") - - # Step 1: Gravity alignment only (Quest3 Y-UP → Z-UP; Aria already Z-UP). - R_coord = ( - QUEST3_WORLD_TO_ZUP - if src.headset == "Quest3" - else np.eye(3, dtype=np.float32) - ) - - # Step 2: Compute scene centre and table z in the gravity-aligned frame. - # We need the scene centre BEFORE applying yaw so we can use it to determine - # the per-sequence yaw normalisation (direction from headset to workspace). - # - # Z: min object z → table surface at z=0 after scene-offset subtraction. - # XY: mean of all object + wrist positions → interaction near xy origin. - all_positions_Rc: list[np.ndarray] = [] - all_obj_z_Rc: list[float] = [] - for ts in valid_ts: - for uid in src.object_uids: - pos = (R_coord @ obj_data_all[ts][uid][:3, 3]).astype(np.float32) - all_positions_Rc.append(pos) - all_obj_z_Rc.append(float(pos[2])) - for ts in valid_ts: - hp = mano_data[ts] - for hand_key in ("0", "1"): - if hand_key in hp: - t = R_coord @ np.array( - hp[hand_key]["wrist_xform"]["t_xyz"], dtype=np.float32 - ) - all_positions_Rc.append(t.astype(np.float32)) - - if all_positions_Rc: - scene_center_xy = np.mean(all_positions_Rc, axis=0)[:2].astype(np.float32) - z_table = float(np.min(all_obj_z_Rc)) - else: - scene_center_xy = np.zeros(2, dtype=np.float32) - z_table = 0.0 - - # Step 3: Compute per-sequence yaw normalisation. - # Strategy: make the direction from the headset's starting position to the - # workspace centroid point to +Y. This is more robust than using the - # headset's "forward" direction because: - # - The person looks steeply DOWN at the table, so the horizontal projection - # of the forward direction is small and unstable. - # - The headset→workspace direction directly encodes where the table is - # relative to the person, regardless of headset-specific frame conventions. - headset_pos_Rc = _load_headset_initial_position(src.seq_dir, R_coord) - if headset_pos_Rc is not None: - R_yaw = _yaw_cancel_headset_to_scene(headset_pos_Rc, scene_center_xy) - else: - R_yaw = np.eye(3, dtype=np.float32) - - # Step 4: Combined transform (gravity alignment + yaw normalisation). - R_total = (R_yaw @ R_coord).astype(np.float32) - self._r_total_cache[sequence_info.sequence_id] = R_total - - # Step 5: Compute scene_offset in R_total frame. - # Since R_yaw is a pure Z-rotation, z is unchanged: z_table stays the same. - # Only the xy centroid rotates: scene_center_xy_Rt = R_yaw[:2,:2] @ scene_center_xy. - scene_center_xy_Rt = (R_yaw[:2, :2] @ scene_center_xy).astype(np.float32) - scene_offset = np.array( - [scene_center_xy_Rt[0], scene_center_xy_Rt[1], z_table], dtype=np.float32 - ) - - # Cache for load_object_data - self._valid_frame_ids_cache[sequence_info.sequence_id] = valid_ts - self._scene_offset_cache[sequence_info.sequence_id] = scene_offset - - rg_list, rp_list, rt_list = [], [], [] - lg_list, lp_list, lt_list = [], [], [] - right_betas: np.ndarray | None = None - left_betas: np.ndarray | None = None - - for ts in valid_ts: - hp = mano_data[ts] - try: - rg, rp, rt, rb, lg, lp, lt, lb = _extract_hand_frame( - hp, self._right_components, self._left_components - ) - except (KeyError, ValueError) as e: - raise ValueError(f"Bad frame at ts={ts}: {e}") from e - rg_list.append(rg) - rp_list.append(rp) - rt_list.append(rt) - lg_list.append(lg) - lp_list.append(lp) - lt_list.append(lt) - if right_betas is None: - right_betas = rb - if left_betas is None: - left_betas = lb - - H = len(valid_ts) - right_global_orient = np.stack(rg_list).astype(np.float32) - right_finger_pose = np.stack(rp_list).astype(np.float32) - right_trans = np.stack(rt_list).astype(np.float32) - left_global_orient = np.stack(lg_list).astype(np.float32) - left_finger_pose = np.stack(lp_list).astype(np.float32) - left_trans = np.stack(lt_list).astype(np.float32) - - # Apply the combined world-frame rotation R_total to all MANO quantities. - # This covers both Quest3 Y-UP→Z-UP and the per-sequence yaw normalization. - # - # global_orient: world-frame change = simple left-multiply R @ R_old. - # NOT the similarity transform R @ R_old @ R^T (local-frame change). - # - # transl: with center_idx=None, manotorch uses wrist_world = J_shaped[0] + transl. - # Rotating the world frame requires: - # new_transl = R @ (J_shaped[0] + transl) - J_shaped[0] - # Simply doing R @ transl would leave a betas-dependent position error each frame. - with torch.no_grad(): - rb_t = torch.from_numpy(right_betas).float().unsqueeze(0) - lb_t = torch.from_numpy(left_betas).float().unsqueeze(0) - zero_pose = torch.zeros(1, 3 + 15) # global_orient(3) + PCA comps(15) - J_right = ( - self._right_mano_layer(pose_coeffs=zero_pose, betas=rb_t) - .joints[0, 0] - .numpy() - ) # (3,) wrist template position - J_left = ( - self._left_mano_layer(pose_coeffs=zero_pose, betas=lb_t) - .joints[0, 0] - .numpy() - ) - - R = R_total - for i in range(H): - R_r = Rotation.from_rotvec(right_global_orient[i]).as_matrix() - right_global_orient[i] = Rotation.from_matrix(R @ R_r).as_rotvec() - R_l = Rotation.from_rotvec(left_global_orient[i]).as_matrix() - left_global_orient[i] = Rotation.from_matrix(R @ R_l).as_rotvec() - - right_trans = (R @ (right_trans + J_right).T).T - J_right - left_trans = (R @ (left_trans + J_left).T).T - J_left - - right_trans -= scene_offset - left_trans -= scene_offset - - return { - "right_global_orient": torch.from_numpy(right_global_orient).to(device), - "right_finger_pose": torch.from_numpy(right_finger_pose).to(device), - "right_trans": torch.from_numpy(right_trans).to(device), - "right_betas": torch.from_numpy(right_betas).to(device), - "right_fitting_err": torch.zeros(H, device=device), - "left_global_orient": torch.from_numpy(left_global_orient).to(device), - "left_finger_pose": torch.from_numpy(left_finger_pose).to(device), - "left_trans": torch.from_numpy(left_trans).to(device), - "left_betas": torch.from_numpy(left_betas).to(device), - "left_fitting_err": torch.zeros(H, device=device), - "H": H, - } - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object poses aligned to the same valid timestamps as MANO data.""" - src: Hot3DSequenceSource = sequence_info.source - valid_ts = self._valid_frame_ids_cache.get(sequence_info.sequence_id) - if valid_ts is None: - raise RuntimeError( - "load_mano_data must be called before load_object_data " - f"(no cached timestamps for '{sequence_info.sequence_id}')" - ) - - obj_data_all = _load_object_csv(src.seq_dir / "dynamic_objects.csv") - scene_offset = self._scene_offset_cache.get( - sequence_info.sequence_id, np.zeros(3, dtype=np.float32) - ) - - # Use the same combined rotation (gravity alignment + yaw normalisation) that - # was applied to the MANO data. Both rotation and translation use the simple - # left-multiply R @ old (world-frame change), NOT the similarity transform. - R_total = self._r_total_cache.get(sequence_info.sequence_id) - if R_total is None: - raise RuntimeError( - "load_mano_data must be called before load_object_data " - f"(no cached R_total for '{sequence_info.sequence_id}')" - ) - - result: dict[str, Any] = {} - for uid in src.object_uids: - poses_raw = np.array( - [obj_data_all[ts][uid] for ts in valid_ts], dtype=np.float64 - ) # (N, 4, 4) - R = R_total.astype(np.float64) - poses_raw[:, :3, :3] = R @ poses_raw[:, :3, :3] - poses_raw[:, :3, 3] = (R @ poses_raw[:, :3, 3].T).T - poses_raw[:, :3, 3] -= scene_offset # apply same centering as hands - root_pos, root_aa = poses_to_root_position_and_axis_angle(poses_raw) - result[uid] = (poses_raw, root_pos, root_aa, None) # None = no articulation - - return result - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple[ - dict[str, Any], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - bool, - ]: - """Load Hot3D object meshes from assets/meshes/hot3d/ directory (.glb, meters).""" - src: Hot3DSequenceSource = sequence_info.source - assets_dir = Path(self._args.mesh_dir) - mesh_paths: dict[str, str] = {} - for uid in src.object_uids: - glb_path = assets_dir / f"{uid}.glb" - if glb_path.exists(): - mesh_paths[uid] = str(glb_path) - else: - print(f"Warning: No mesh found for object {uid} at {glb_path}") - return load_meshes_to_device(mesh_paths, device, vertex_scale=1.0) - - def get_mano_kwargs(self) -> dict[str, Any]: - """Hot3D uses standard MANO (flat_hand_mean=False) without joint centering. - - Hot3D stores t_xyz as the smplx `transl` parameter, where the true wrist - world position = R_global @ J_template[0](betas) + t_xyz. Using - center_idx=None lets manotorch compute this correctly. With center_idx=0 - the FK_joint0 term is dropped, causing the hand to float away from objects. - """ - return {"flat_hand_mean": False, "center_idx": None} - - def get_fps(self) -> float: - """Return Hot3D effective frame rate after deduplication.""" - return HOT3D_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return GLB mesh paths for all objects in the sequence.""" - src: Hot3DSequenceSource = sequence_info.source - assets_dir = Path(self._args.mesh_dir) - return [str(assets_dir / f"{uid}.glb") for uid in src.object_uids] - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return rigid URDF paths for all objects in the sequence. - - URDFs are generated by scripts/generate_rigid_urdfs.py --dataset hot3d. - """ - src: Hot3DSequenceSource = sequence_info.source - urdf_dir = Path(self._args.object_model_root) - return [str(urdf_dir / f"{uid}_rigid.urdf") for uid in src.object_uids] - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the Hot3D loader script.""" - parser = argparse.ArgumentParser( - description="Load Hot3D sequences into ManoSharpaData schema (MANO + object only)." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=DEFAULT_HOT3D_DIR, - object_model_root=HOT3D_URDF_DIR, - mesh_dir=HOT3D_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - # Hot3D-specific extras. - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - parser.add_argument( - "--seq_name", - type=str, - default=None, - help="Process only sequences whose folder name contains this substring.", - ) - parser.add_argument( - "--list_sequences", - action="store_true", - default=False, - help="List available sequences and exit.", - ) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Run the Hot3D loader: list sequences or process and save ManoSharpaData Parquet files.""" - if args.list_sequences: - loader = Hot3DDatasetLoader() - sequences = loader.list_sequences(args) - print(f"Found {len(sequences)} sequences in {args.dataset_root}") - for i, s in enumerate(sequences, start=1): - print(f"{i:3d}. {s.sequence_id} objects={s.object_name}") - return - - loader = Hot3DDatasetLoader() - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/loader_registry.py b/reconstruction/modules/v2d_task_library_loader/lib/loader_registry.py deleted file mode 100644 index 9ceebd89..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/loader_registry.py +++ /dev/null @@ -1,19 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Loader-side dataset registry: dataset name -> loader module. - -This is the loader half of robotic_grounding's ``retarget/dataset_registry.py`` -(which keeps the retarget/asset config). Each loader module exposes module-level -``parse_args()`` and ``main(args)``; ``run_loader`` dispatches by importing the -mapped module. -""" - -LOADER_MODULES: dict[str, str] = { - "taco": "v2d.task_library_loader.lib.taco_loader", - "arctic": "v2d.task_library_loader.lib.arctic_loader", - "oakink2": "v2d.task_library_loader.lib.oakink2_loader", - "hot3d": "v2d.task_library_loader.lib.hot3d_loader", - "h2o": "v2d.task_library_loader.lib.h2o_loader", - "grab": "v2d.task_library_loader.lib.grab_loader", - "dexycb": "v2d.task_library_loader.lib.dexycb_loader", -} diff --git a/reconstruction/modules/v2d_task_library_loader/lib/oakink2_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/oakink2_loader.py deleted file mode 100644 index 0ad87e94..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/oakink2_loader.py +++ /dev/null @@ -1,463 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load OakInk2 dataset into ManoSharpaData schema (MANO + object only, no robot). - -OakInk2 layout: - oakink_dir/ - anno_preview/ -> {sequence}.pkl (keys: raw_mano, obj_transf, obj_list, cam_extr) - meshes/oakink2/ - object_repair/align_ds/{object_id}/model.obj - object_raw/align_ds/{object_id}/model.obj - -Poses are stored in OakInk2's y-up OptiTrack world frame. This loader applies a -world -> z-up coordinate transform (x=x, y=-z, z=y) so that saved Parquet data -uses the same z-up convention as ARCTIC and TACO. - -Runs stage 1 of the two-stage pipeline: - 1. python scripts/retarget/oakink2_loader.py --save -> oakink2_loaded/ - 2. python scripts/retarget/oakink2_to_sharpa.py --save -> oakink2_processed/ -""" - -import argparse -import logging -import pickle -import warnings -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from robotic_grounding.retarget import ASSETS_DIR, HUMAN_MOTION_DATA_DIR -from v2d.task_library_loader.lib.dataset_loader_base import ( - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - poses_to_root_position_and_axis_angle, -) -from scipy.spatial.transform import Rotation - -logging.getLogger().setLevel(logging.ERROR) -# manotorch uses torch.cross without dim arg (deprecated in newer PyTorch); suppress. -warnings.filterwarnings("ignore", category=UserWarning, module="manotorch") - -DEFAULT_OAKINK_DIR = HUMAN_MOTION_DATA_DIR / "oakink2" / "dataset" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "oakink2" / "oakink2_loaded" -OAKINK2_FPS = 120.0 -OAKINK2_OBJECT_URDF_DIR = ASSETS_DIR / "urdfs" / "oakink2" -OAKINK2_MESH_DIR = ASSETS_DIR / "meshes" / "oakink2" - -# Rotation: OakInk2/OptiTrack y-up world -> z-up convention used by ARCTIC/TACO -# x_new = x_old -# y_new = -z_old -# z_new = y_old -_WORLD_TO_ZUP = np.array( - [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]], dtype=np.float64 -) -_WORLD_TO_ZUP_T4 = np.eye(4, dtype=np.float64) -_WORLD_TO_ZUP_T4[:3, :3] = _WORLD_TO_ZUP - - -@dataclass -class OakInk2SequenceSource: - """Dataset-specific metadata stored in SequenceInfo.source.""" - - seq_pkl: Path - oakink_dir: Path - use_object_raw: bool - object_ids: list[str] # from seq_data["obj_list"] - - -# --------------------------------------------------------------------------- -# Coordinate-frame helpers -# --------------------------------------------------------------------------- - - -def _quat_wxyz_to_rotvec(quat_wxyz: np.ndarray) -> np.ndarray: - """Convert OakInk2 quaternions [w,x,y,z] (...,4) to rotation vectors (...,3).""" - return ( - Rotation.from_quat(np.asarray(quat_wxyz, dtype=np.float32), scalar_first=True) - .as_rotvec() - .astype(np.float32) - ) - - -def _apply_zup_to_trans(trans: np.ndarray) -> np.ndarray: - """Rotate (N,3) or (3,) translations from y-up world to z-up.""" - single = trans.ndim == 1 - t = trans.reshape(-1, 3).astype(np.float64) - t_new = (_WORLD_TO_ZUP @ t.T).T.astype(np.float32) - return t_new[0] if single else t_new - - -def _apply_zup_to_rotvec(rotvec: np.ndarray) -> np.ndarray: - """Transform (N,3) rotation vectors: R_new = R_zup @ R_old. - - finger_pose (relative joint rotations) is NOT passed here — it is - frame-independent and does not change when the world frame changes. - """ - R_old = Rotation.from_rotvec(rotvec).as_matrix() # (N,3,3) - R_new = _WORLD_TO_ZUP @ R_old # (3,3) @ (N,3,3) -> (N,3,3) - return Rotation.from_matrix(R_new).as_rotvec().astype(np.float32) - - -def _apply_zup_to_poses(poses: np.ndarray) -> np.ndarray: - """Apply world->z-up transform to (N,4,4) pose matrices.""" - # _WORLD_TO_ZUP_T4 @ poses[i]: rotation applied to both rotation and translation - return np.einsum("ij,njk->nik", _WORLD_TO_ZUP_T4, poses.astype(np.float64)) - - -# --------------------------------------------------------------------------- -# OakInk2 data helpers -# --------------------------------------------------------------------------- - - -def _load_seq_pkl(seq_pkl: Path) -> dict: - with seq_pkl.open("rb") as f: - return pickle.load(f) - - -def _extract_hand_from_entry( - entry: dict, prefix: str -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Extract (global_orient_rotvec, finger_pose_rotvec, trans, betas) for one hand frame. - - prefix is "rh" (right) or "lh" (left). - pose_coeffs: (16,4) quaternions [w,x,y,z] — wrist + 15 finger joints. - """ - pose_key = f"{prefix}__pose_coeffs" - tsl_key = f"{prefix}__tsl" - betas_key = f"{prefix}__betas" - - if pose_key not in entry: - raise KeyError(f"Missing key '{pose_key}'") - - pose_quat = np.asarray(entry[pose_key], dtype=np.float32).squeeze() - if pose_quat.shape != (16, 4): - raise ValueError(f"{pose_key} expected shape (16,4), got {pose_quat.shape}") - - pose_rotvec = _quat_wxyz_to_rotvec(pose_quat) # (16,3) - global_orient = pose_rotvec[0] # (3,) wrist orientation - finger_pose = pose_rotvec[1:].reshape(45) # (45,) 15 joints × 3 - - trans = np.asarray(entry[tsl_key], dtype=np.float32).reshape(3) - betas = np.asarray(entry[betas_key], dtype=np.float32).reshape(-1)[:10] - return global_orient, finger_pose, trans, betas - - -def _resolve_mesh_path(object_id: str, use_object_raw: bool, mesh_dir: Path) -> Path: - """Resolve object mesh path; prefers object_repair, falls back to object_raw.""" - primary = "object_raw" if use_object_raw else "object_repair" - path = mesh_dir / primary / "align_ds" / object_id / "model.obj" - if path.exists(): - return path - fallback = "object_repair" if use_object_raw else "object_raw" - path2 = mesh_dir / fallback / "align_ds" / object_id / "model.obj" - if path2.exists(): - return path2 - raise FileNotFoundError( - f"No mesh for '{object_id}' in {mesh_dir} (checked {primary} and {fallback})" - ) - - -def _extract_valid_frames( - seq_data: dict, object_ids: list[str] -) -> tuple[ - list, list, list, list, list, list, list, np.ndarray | None, np.ndarray | None -]: - """Iterate raw_mano, convert quaternions, filter to frames with object poses. - - Returns lists for right and left: (global_orient, finger_pose, trans), - plus right_betas and left_betas (taken from first valid frame). - """ - raw_mano = seq_data["raw_mano"] - obj_transf = seq_data["obj_transf"] - - rg_list, rp_list, rt_list = [], [], [] - lg_list, lp_list, lt_list = [], [], [] - right_betas: np.ndarray | None = None - left_betas: np.ndarray | None = None - valid_frame_ids = [] - - for fid in sorted(raw_mano.keys()): - entry = raw_mano[fid] - try: - rg, rp, rt, rb = _extract_hand_from_entry(entry, "rh") - lg, lp, lt, lb = _extract_hand_from_entry(entry, "lh") - except (KeyError, ValueError): - continue - - # Require object pose data for every object in this sequence - if not all(oid in obj_transf and fid in obj_transf[oid] for oid in object_ids): - continue - - valid_frame_ids.append(fid) - rg_list.append(rg) - rp_list.append(rp) - rt_list.append(rt) - lg_list.append(lg) - lp_list.append(lp) - lt_list.append(lt) - if right_betas is None: - right_betas = rb - if left_betas is None: - left_betas = lb - - return ( - valid_frame_ids, - rg_list, - rp_list, - rt_list, - lg_list, - lp_list, - lt_list, - right_betas, - left_betas, - ) - - -# --------------------------------------------------------------------------- -# Dataset loader -# --------------------------------------------------------------------------- - - -class OakInk2DatasetLoader(DatasetLoaderBase): - """OakInk2 dataset loader.""" - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """Discover all anno_preview .pkl sequences. - - Only scans filenames — pkl contents are loaded lazily in load_mano_data - to avoid deserializing every file during discovery. - """ - oakink_dir = Path(args.dataset_root) - use_object_raw = getattr(args, "use_object_raw", False) - seq_name_filter = getattr(args, "seq_name", None) - - anno_dir = oakink_dir / "anno_preview" - if not anno_dir.is_dir(): - raise FileNotFoundError(f"anno_preview not found: {anno_dir}") - - pkls = sorted(anno_dir.glob("*.pkl")) - if seq_name_filter: - pkls = [p for p in pkls if seq_name_filter in p.stem] - - out = [] - for pkl_path in pkls: - sequence_id = pkl_path.stem - out.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=pkl_path.stem, - object_name="", - object_body_names=[], - source=OakInk2SequenceSource( - seq_pkl=pkl_path, - oakink_dir=oakink_dir, - use_object_raw=use_object_raw, - object_ids=[], - ), - ) - ) - return out - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO parameters from OakInk2 pkl; apply world->z-up transform.""" - src: OakInk2SequenceSource = sequence_info.source - seq_data = _load_seq_pkl(src.seq_pkl) - - if not {"raw_mano", "obj_transf", "obj_list"}.issubset(seq_data.keys()): - raise ValueError(f"Missing required keys in {src.seq_pkl.name}") - - object_ids = list(seq_data["obj_list"]) - if not object_ids or not seq_data["raw_mano"]: - raise ValueError(f"Empty object list or mano data in {src.seq_pkl.name}") - - # Populate object info deferred from list_sequences - src.object_ids = object_ids - sequence_info.object_name = "+".join(object_ids) - sequence_info.object_body_names = list(object_ids) - - ( - valid_frame_ids, - rg_list, - rp_list, - rt_list, - lg_list, - lp_list, - lt_list, - right_betas, - left_betas, - ) = _extract_valid_frames(seq_data, object_ids) - - if not valid_frame_ids: - raise ValueError(f"No valid frames in {src.seq_pkl.name}") - - # Cache valid_frame_ids so load_object_data uses the same filtered set - if not hasattr(self, "_valid_frame_ids_cache"): - self._valid_frame_ids_cache: dict[str, list] = {} - self._valid_frame_ids_cache[sequence_info.sequence_id] = valid_frame_ids - - # Apply world->z-up transform to translations and global orientations. - # finger_pose is relative (joint-to-joint) so it is unaffected. - right_global_orient = _apply_zup_to_rotvec(np.stack(rg_list)) # (N,3) - right_finger_pose = np.stack(rp_list).astype(np.float32) # (N,45) - right_trans = _apply_zup_to_trans(np.stack(rt_list)) # (N,3) - - left_global_orient = _apply_zup_to_rotvec(np.stack(lg_list)) - left_finger_pose = np.stack(lp_list).astype(np.float32) - left_trans = _apply_zup_to_trans(np.stack(lt_list)) - - H = len(valid_frame_ids) - return { - "right_global_orient": torch.from_numpy(right_global_orient).to(device), - "right_finger_pose": torch.from_numpy(right_finger_pose).to(device), - "right_trans": torch.from_numpy(right_trans).to(device), - "right_betas": torch.from_numpy(right_betas).to(device), - "right_fitting_err": torch.zeros(H, device=device), - "left_global_orient": torch.from_numpy(left_global_orient).to(device), - "left_finger_pose": torch.from_numpy(left_finger_pose).to(device), - "left_trans": torch.from_numpy(left_trans).to(device), - "left_betas": torch.from_numpy(left_betas).to(device), - "left_fitting_err": torch.zeros(H, device=device), - "H": H, - } - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object pose data; apply world->z-up transform. Returns rigid objects (no articulation).""" - src: OakInk2SequenceSource = sequence_info.source - seq_data = _load_seq_pkl(src.seq_pkl) - obj_transf = seq_data["obj_transf"] - - valid_frame_ids = self._valid_frame_ids_cache.get(sequence_info.sequence_id) - if valid_frame_ids is None: - raise RuntimeError( - "load_mano_data must be called before load_object_data " - f"(no cached frame ids for '{sequence_info.sequence_id}')" - ) - - result: dict[str, Any] = {} - for oid in src.object_ids: - poses_raw = np.array( - [ - np.asarray(obj_transf[oid][fid], dtype=np.float64) - for fid in valid_frame_ids - ] - ) # (N,4,4) - poses_zup = _apply_zup_to_poses(poses_raw) # (N,4,4) - root_pos, root_aa = poses_to_root_position_and_axis_angle(poses_zup) - result[oid] = (poses_zup, root_pos, root_aa, None) # None = no articulation - - return result - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple[dict[str, Any], dict[str, torch.Tensor], dict[str, torch.Tensor], bool]: - """Load OakInk2 object meshes (model.obj, already in meters).""" - src: OakInk2SequenceSource = sequence_info.source - mesh_dir = Path(getattr(self._args, "mesh_dir", OAKINK2_MESH_DIR)) - mesh_paths: dict[str, str] = {} - for oid in src.object_ids: - try: - mesh_paths[oid] = str( - _resolve_mesh_path(oid, src.use_object_raw, mesh_dir) - ) - except FileNotFoundError as e: - print(f"Warning: {e}") - return load_meshes_to_device(mesh_paths, device) - - def get_mano_kwargs(self) -> dict[str, Any]: - """OakInk2 uses flat_hand_mean=True and centers at wrist (center_idx=0).""" - return {"flat_hand_mean": True, "center_idx": 0} - - def get_fps(self) -> float: - """Return the frames-per-second for OakInk2 sequences.""" - return OAKINK2_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return mesh paths for all objects in the sequence (one per object_id).""" - src: OakInk2SequenceSource = sequence_info.source - mesh_dir = Path(getattr(self._args, "mesh_dir", OAKINK2_MESH_DIR)) - paths = [] - for oid in src.object_ids: - try: - paths.append(str(_resolve_mesh_path(oid, src.use_object_raw, mesh_dir))) - except FileNotFoundError: - paths.append("") - return paths - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to OakInk2 object URDF files (one per object_id).""" - src: OakInk2SequenceSource = sequence_info.source - object_urdf_root = Path( - getattr(self._args, "object_model_root", OAKINK2_OBJECT_URDF_DIR) - ) - paths = [] - for oid in src.object_ids: - paths.append(str(object_urdf_root / f"{oid}_rigid.urdf")) - return paths - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the OakInk2 loader script.""" - parser = argparse.ArgumentParser( - description="Load OakInk2 sequences into ManoSharpaData schema (MANO + object only)." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=DEFAULT_OAKINK_DIR, - object_model_root=OAKINK2_OBJECT_URDF_DIR, - mesh_dir=OAKINK2_MESH_DIR, - output_dir=LOADED_SAVE_DIR, - ) - parser.add_argument( - "--use_object_raw", - action="store_true", - default=False, - help="Use object_raw instead of object_repair meshes.", - ) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - parser.add_argument( - "--seq_name", - type=str, - default=None, - help="Process only sequences whose filename contains this substring.", - ) - parser.add_argument( - "--list_sequences", - action="store_true", - default=False, - help="List available sequences and exit.", - ) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Run the OakInk2 loader: list sequences or process and save ManoSharpaData Parquet files.""" - if args.list_sequences: - loader = OakInk2DatasetLoader() - sequences = loader.list_sequences(args) - print( - f"Found {len(sequences)} sequences in {args.dataset_root / 'anno_preview'}" - ) - for i, s in enumerate(sequences, start=1): - print(f"{i:3d}. {s.sequence_id}") - return - - loader = OakInk2DatasetLoader() - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/lib/pyproject.toml b/reconstruction/modules/v2d_task_library_loader/lib/pyproject.toml deleted file mode 100644 index 04793f29..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/pyproject.toml +++ /dev/null @@ -1,32 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -[build-system] -requires = ["setuptools>=68.0", "wheel"] -build-backend = "setuptools.build_meta" - -[project] -name = "v2d-task-library-loader-lib" -version = "0.1.0" -description = "Hand-object dataset loaders: MANO FK + contact/distance to ManoSharpaData Parquet" -requires-python = ">=3.10" -dependencies = [ - "numpy", - "scipy", - "trimesh", - "tqdm", - "viser", - "pyarrow", # data_logger (ManoSharpaData) Parquet I/O - "mujoco<3.7", # arctic_loader object FK; 3.7+ broke meshdir+path-filename resolution for the arctic URDFs - "PyYAML", # dexycb_loader - "v2d-common", - # manotorch + chumpy (GPL-3.0) are installed via the Dockerfile (git), not here. - # `deprecation` (undeclared manotorch runtime dep) is installed via the Dockerfile. - # pytorch3d is installed via the Dockerfile with --no-build-isolation. - # robotic_grounding (schema/params/utils) is installed via the Dockerfile (see note there). -] - -[tool.setuptools] -packages = ["v2d.task_library_loader.lib"] - -[tool.setuptools.package-dir] -"v2d.task_library_loader.lib" = "." diff --git a/reconstruction/modules/v2d_task_library_loader/lib/read_mano.py b/reconstruction/modules/v2d_task_library_loader/lib/read_mano.py deleted file mode 100644 index e1b50843..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/read_mano.py +++ /dev/null @@ -1,247 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MANO model wrapper: forward kinematics + optional viser visualization. - -Ported from robotic_grounding's ``retarget/read_mano.py``. The only change vs. -the original is that the MANO asset root is passed in (``mano_assets_root``) -rather than read from a hardcoded ``BODY_MODELS_DIR`` — the caller supplies a -directory holding the (separately-licensed) MANO ``.pkl`` models, matching the -``v2d_hand_alignment`` / ``v2d_wilor`` convention. The MANO ``.pkl`` files are -never vendored. - -``manotorch`` (GPL-3.0) is imported here and stays contained in this module's -Docker image; it is never installed into the ``robotic_grounding`` image. - -Reference: -https://github.com/lixiny/manotorch/blob/2f6a701e76ee544bbeac1d3e72c628061f585a6c/scripts/simple_app.py -""" - -from typing import Any, Dict - -import numpy as np -import torch -from manotorch.axislayer import AxisLayerFK -from manotorch.manolayer import ManoLayer - -# Clean, manotorch-free helpers imported from robotic_grounding (installed as a -# dependency; see the module Dockerfile). MANO_JOINTS_ORDER also defines the -# parquet joint-column ordering — the producer/consumer contract. -from robotic_grounding.retarget.params import ( - MANO_JOINTS_ORDER, - TRANSFORMS_TO_JOINTS, -) -from robotic_grounding.retarget.utils import quat_from_matrix - - -class MANO: - """MANO model class to process MANO model and visualize.""" - - def __init__( - self, - mano_assets_root: str, - gender: str = "neutral", - device: torch.device | None = None, - flat_hand_mean: bool = True, - center_idx: int | None = None, - ) -> None: - """ - Initialize the MANO model. - - Args: - mano_assets_root: directory containing the MANO models (a ``models/`` - subdir with ``MANO_RIGHT.pkl`` / ``MANO_LEFT.pkl``), supplied at - runtime by the caller. Not vendored. - gender: str, "neutral" or "male" or "female" - device: torch.device | None, the device to use for the model - flat_hand_mean: bool, whether to use the flat hand mean for MANO - center_idx: int | None, index of center joint for MANO (e.g. 0 for wrist). - If None, no joint centering is applied. - """ - self.device = device if device is not None else torch.device("cpu") - - # Right hand - self.right_mano_layer = ManoLayer( - use_pca=False, - side="right", - gender=gender, - center_idx=center_idx, - mano_assets_root=mano_assets_root, - flat_hand_mean=flat_hand_mean, - ).to(self.device) - self.right_axis_layer = AxisLayerFK( - side=self.right_mano_layer.side, - mano_assets_root=mano_assets_root, - ).to(self.device) - - # Left hand - self.left_mano_layer = ManoLayer( - use_pca=False, - side="left", - gender=gender, - center_idx=center_idx, - mano_assets_root=mano_assets_root, - flat_hand_mean=flat_hand_mean, - ).to(self.device) - self.left_axis_layer = AxisLayerFK( - side=self.left_mano_layer.side, - mano_assets_root=mano_assets_root, - ).to(self.device) - - # Faces - self.right_faces: ( - torch.Tensor - ) = self.right_mano_layer.get_mano_closed_faces().to( - self.device - ) # (1552, 3) - self.left_faces: torch.Tensor = self.left_mano_layer.get_mano_closed_faces().to( - self.device - ) # (1552, 3) - - def forward( - self, - side: str, - betas: torch.Tensor, - global_orient: torch.Tensor, - transl: torch.Tensor, - finger_pose: torch.Tensor, - ) -> Dict[str, torch.Tensor]: - """ - Forward pass of the MANO model. - - Args: - side: str, "right" or "left" - betas: torch.Tensor, (B, 10) or (10) - global_orient: torch.Tensor, (B, 3) - transl: torch.Tensor, (B, 3) - finger_pose: torch.Tensor, (B, 45) or (B, 15, 3) - """ - # 1. Check input shapes - assert betas.ndim in {1, 2}, "Betas must be of shape (B, 10) or (10)" - assert ( - global_orient.ndim == 2 and global_orient.shape[1] == 3 - ), "Global orient must be of shape (B, 3)" - assert ( - transl.ndim == 2 and transl.shape[1] == 3 - ), "Transl must be of shape (B, 3)" - assert finger_pose.ndim in { - 2, - 3, - }, "Hand pose must be of shape (B, 45) or (B, 15, 3)" - - betas = betas.to(self.device) - global_orient = global_orient.to(self.device) - transl = transl.to(self.device) - finger_pose = finger_pose.to(self.device) - - # 2. Expand betas and reshape hand pose - B = len(global_orient) - if betas.ndim == 1: - betas = betas.unsqueeze(0).expand(B, -1) - finger_pose = finger_pose.reshape(B, 45) - hand_pose = torch.cat((global_orient, finger_pose), dim=-1) - transl = transl.reshape(B, 1, 3) - - # 3. Forward pass of the MANO model - mano_layer = self.right_mano_layer if side == "right" else self.left_mano_layer - axis_layer = self.right_axis_layer if side == "right" else self.left_axis_layer - - mano_results = mano_layer( - pose_coeffs=hand_pose, # (B, 48) - betas=betas, # (B, 10) - ) - - # 4. Apply translation - joints = mano_results.joints + transl - vertices = mano_results.verts + transl - T_g_p = mano_results.transforms_abs - T_g_p[:, :, :3, 3] += transl - - # 5. Compute joint rotations - T_g_a, _, _ = axis_layer(T_g_p) # (B, 16, 4, 4) - axes_loc = ( - torch.eye(3).reshape(1, 1, 3, 3).repeat(B, 16, 1, 1).to(vertices.device) - ) # (B, 16, 3, 3) - transforms_rotation_wo_tips = torch.matmul( - T_g_a[:, :, :3, :3], axes_loc - ) # (B, 16, 3, 3) - - # 6. Add rotations to joints and convert to quaternions - joints_rotation_matrices = transforms_rotation_wo_tips[ - :, TRANSFORMS_TO_JOINTS - ] # (B, 21, 3, 3) - joints_wxyz = quat_from_matrix(joints_rotation_matrices) # (B, 21, 4) - - faces = self.right_faces if side == "right" else self.left_faces # (1538, 3) - return { - "joints": joints, # (B, 21, 3) - "joints_wxyz": joints_wxyz, # (B, 16, 3) - "vertices": vertices, # (B, 778, 3) - "faces": faces, # (1538, 3) - } - - def visualize( - self, - viser_server: Any, - side: str, - vertices: torch.Tensor | np.ndarray | None = None, - faces: torch.Tensor | np.ndarray | None = None, - joints: torch.Tensor | np.ndarray | None = None, - joints_wxyz: torch.Tensor | np.ndarray | None = None, - ) -> None: - """ - Visualize the MANO model. - - Args: - viser_server: Any - side: str, "right" or "left" - vertices: torch.Tensor | np.ndarray, (778, 3) - faces: torch.Tensor | np.ndarray, (1538, 3) - joints: torch.Tensor | np.ndarray, (21, 3) - joints_wxyz: torch.Tensor | np.ndarray, (21, 4) - """ - # Imported lazily so the FK path does not require the ``judo`` viz dep. - from judo.visualizers.model import add_mesh - - if vertices is not None and faces is not None: - vertices = ( - vertices.cpu().numpy() - if isinstance(vertices, torch.Tensor) - else vertices - ) - faces = faces.cpu().numpy() if isinstance(faces, torch.Tensor) else faces - - add_mesh( - viser_server, - f"/mano/{side}/hand", - vertices=vertices, - faces=faces, - pos=np.array([0, 0, 0]), - quat=np.array([1, 0, 0, 0]), - rgba=np.array([255, 219, 172, 180]), - ) - - if joints is not None and joints_wxyz is not None: - joints = ( - joints.cpu().numpy() if isinstance(joints, torch.Tensor) else joints - ) - joints_wxyz = ( - joints_wxyz.cpu().numpy() - if isinstance(joints_wxyz, torch.Tensor) - else joints_wxyz - ) - if not hasattr(self, f"viser_mano_{side}_joints_handles"): - setattr(self, f"viser_mano_{side}_joints_handles", {}) - handles = getattr(self, f"viser_mano_{side}_joints_handles") - for joint_idx, joint_name in enumerate(MANO_JOINTS_ORDER): - handles[joint_name] = viser_server.scene.add_frame( - f"/mano/{side}/joints/{joint_name}", - position=joints[joint_idx], - wxyz=joints_wxyz[joint_idx], - axes_length=0.015, - axes_radius=0.0005, - ) - else: - handles = getattr(self, f"viser_mano_{side}_joints_handles") - for joint_idx, joint_name in enumerate(MANO_JOINTS_ORDER): - handles[joint_name].position = joints[joint_idx] - handles[joint_name].wxyz = joints_wxyz[joint_idx] diff --git a/reconstruction/modules/v2d_task_library_loader/lib/run_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/run_loader.py deleted file mode 100644 index 660d812f..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/run_loader.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Generic dataset loader entry point (module-based dispatch). - -Extracts ``--dataset`` from argv and delegates the remaining args to the mapped -loader module's ``parse_args()`` / ``main(args)``. Ported from robotic_grounding's -``scripts/retarget/run_loader.py``; the only change is dispatching by module name -(``importlib.import_module``) instead of by file path, since the loaders are now -package modules. - -Usage:: - - python -m v2d.task_library_loader.lib.run_loader \\ - --dataset taco --output_dir /data/out --mano_model_dir /data/mano \\ - --device cuda:0 --save -""" - -from __future__ import annotations - -import importlib -import sys - -from v2d.task_library_loader.lib.loader_registry import LOADER_MODULES - - -def main() -> None: - """Parse --dataset, then delegate to the dataset's loader module.""" - if "--dataset" not in sys.argv: - print("Error: --dataset is required", file=sys.stderr) - print( - "Usage: python -m v2d.task_library_loader.lib.run_loader " - "--dataset [loader args...]", - file=sys.stderr, - ) - sys.exit(1) - - idx = sys.argv.index("--dataset") - if idx + 1 >= len(sys.argv): - print("Error: --dataset requires a value", file=sys.stderr) - sys.exit(1) - - dataset_name = sys.argv[idx + 1] - - # Remove --dataset from argv so the loader's parse_args() doesn't see it. - remaining_argv = sys.argv[:idx] + sys.argv[idx + 2 :] - - module_name = LOADER_MODULES.get(dataset_name) - if module_name is None: - print( - f"Error: unknown dataset '{dataset_name}'. " - f"Known: {sorted(LOADER_MODULES)}", - file=sys.stderr, - ) - sys.exit(1) - - module = importlib.import_module(module_name) - - # Replace sys.argv so the loader's argparse sees the right args. - sys.argv = remaining_argv - args = module.parse_args() - module.main(args) - - -if __name__ == "__main__": - main() diff --git a/reconstruction/modules/v2d_task_library_loader/lib/taco_loader.py b/reconstruction/modules/v2d_task_library_loader/lib/taco_loader.py deleted file mode 100644 index 522ea5c2..00000000 --- a/reconstruction/modules/v2d_task_library_loader/lib/taco_loader.py +++ /dev/null @@ -1,392 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""Load TACO dataset into ManoSharpaData schema (MANO + object only, no robot). - -Saves Parquet under human_motion_data/taco_loaded/mano_object_only. Use -taco_to_sharpa.py (or a shared retarget script) to retarget to Sharpa. - -TACO layout (see vis_taco_data.py): - dataset_root/ - Object_Poses/{triplet}/{sequence_name}/ -> tool_{name}.npy, target_{name}.npy (4x4) - Hand_Poses/{triplet}/{sequence_name}/ -> right_hand.pkl, left_hand.pkl, *_hand_shape.pkl - object_model_root/ -> {name}_cm.obj (mesh in cm; scale 0.01 to m) -""" - -import argparse -import logging -import pickle -import random -import re -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from robotic_grounding.retarget import ASSETS_DIR, HUMAN_MOTION_DATA_DIR, MESHES_DIR -from v2d.task_library_loader.lib.dataset_loader_base import ( - DatasetLoaderBase, - SequenceInfo, - load_meshes_to_device, - poses_to_root_position_and_axis_angle, -) - -logger = logging.getLogger(__name__) - - -@dataclass -class TacoSequenceSource: - """Typed source data for a TACO sequence (stored in SequenceInfo.source).""" - - hand_pose_dir: Path - object_pose_dir: Path - num_frames: int - tool_name: str - target_name: str - tool_poses: np.ndarray - target_poses: np.ndarray - - -# TACO paths -TACO_DATA_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "dataset" -TACO_OBJECT_MODEL_DIR = MESHES_DIR / "taco" -TACO_OBJECT_URDF_DIR = ASSETS_DIR / "urdfs" / "taco" -LOADED_SAVE_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "taco_loaded" - -TACO_OBJECT_BODY_NAMES = ["tool", "target"] -TACO_FPS = 30.0 - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Load TACO sequences into ManoSharpaData schema (MANO + object only)." - ) - DatasetLoaderBase.add_common_args( - parser, - dataset_root=TACO_DATA_DIR, - object_model_root=TACO_OBJECT_URDF_DIR, - mesh_dir=TACO_OBJECT_MODEL_DIR, - output_dir=LOADED_SAVE_DIR, - ) - parser.add_argument( - "--triplet", - type=str, - default=None, - help='Tool-action-object triplet, e.g. "(brush, brush, bowl)". If not set, process all triplets.', - ) - parser.add_argument( - "--sample_triplets", - type=int, - default=None, - help="Randomly sample N triplets from the dataset. Only used when --triplet is not set.", - ) - return parser.parse_args() - - -def _list_taco_triplets(dataset_root: Path) -> list[str]: - """List all triplet directory names under Hand_Poses/.""" - hand_dir = dataset_root / "Hand_Poses" - if not hand_dir.is_dir(): - return [] - return sorted([d.name for d in hand_dir.iterdir() if d.is_dir()]) - - -def _list_taco_sequences(dataset_root: Path, triplet: str) -> list[str]: - """List sequence names under Hand_Poses/{triplet}/.""" - hand_dir = dataset_root / "Hand_Poses" / triplet - if not hand_dir.is_dir(): - return [] - return sorted([d.name for d in hand_dir.iterdir() if d.is_dir()]) - - -def _discover_tool_and_target(object_pose_dir: Path) -> tuple[str, str]: - """Get tool_name and target_name from Object_Poses dir.""" - tool_name, target_name = None, None - for f in object_pose_dir.iterdir(): - if not f.is_file(): - continue - name = f.stem - if name.startswith("tool_"): - tool_name = name.replace("tool_", "", 1) - elif name.startswith("target_"): - target_name = name.replace("target_", "", 1) - if tool_name is None or target_name is None: - raise FileNotFoundError( - f"Could not find tool_*.npy and target_*.npy in {object_pose_dir}" - ) - return tool_name, target_name - - -def _load_taco_object_poses( - object_pose_dir: Path, - tool_name: str, - target_name: str, -) -> tuple[np.ndarray, np.ndarray]: - """Load tool and target poses; each (N, 4, 4) in world/meter frame.""" - tool_path = object_pose_dir / f"tool_{tool_name}.npy" - target_path = object_pose_dir / f"target_{target_name}.npy" - if not tool_path.exists(): - raise FileNotFoundError(f"Tool poses not found: {tool_path}") - if not target_path.exists(): - raise FileNotFoundError(f"Target poses not found: {target_path}") - tool_poses = np.load(tool_path) - target_poses = np.load(target_path) - if tool_poses.shape[-2:] != (4, 4): - raise ValueError(f"Expected (N, 4, 4), got {tool_poses.shape}") - N = tool_poses.shape[0] - if target_poses.shape[0] != N: - raise ValueError(f"Tool has {N} frames, target has {target_poses.shape[0]}") - return tool_poses, target_poses - - -def load_taco_hand_shape(hand_pose_dir: Path, side: str) -> np.ndarray: - """Load hand shape beta (10,) from *_hand_shape.pkl.""" - path = hand_pose_dir / f"{side}_hand_shape.pkl" - if not path.exists(): - raise FileNotFoundError(f"Hand shape not found: {path}") - with open(path, "rb") as f: - try: - data = pickle.load(f) - except (pickle.UnpicklingError, EOFError, ValueError, ModuleNotFoundError) as e: - raise RuntimeError(f"Failed to unpickle {path}: {e}") from e - beta = data["hand_shape"] - if hasattr(beta, "detach"): - beta = beta.detach().cpu().numpy() - return np.asarray(beta, dtype=np.float32).reshape(10) - - -def _mano_params_from_pkl( - hand_pose_dir: Path, - side: str, - N: int, - device: torch.device, -) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor] | None: - """Load integrated hand pkl: dict of frames with 'hand_pose' (48) and 'hand_trans' (3).""" - path = hand_pose_dir / f"{side}_hand.pkl" - if not path.exists(): - return None - with open(path, "rb") as f: - try: - data = pickle.load(f) - except (pickle.UnpicklingError, EOFError, ValueError, ModuleNotFoundError) as e: - logger.warning("Failed to unpickle %s: %s", path, e) - return None - keys = sorted(data.keys()) - theta_list = [] - trans_list = [] - for key in keys: - frame_data = data[key] - hp = frame_data["hand_pose"] - ht = frame_data["hand_trans"] - if hasattr(hp, "detach"): - hp = hp.detach().cpu().numpy() - if hasattr(ht, "detach"): - ht = ht.detach().cpu().numpy() - theta_list.append(np.asarray(hp, dtype=np.float32).reshape(48)) - trans_list.append(np.asarray(ht, dtype=np.float32).reshape(3)) - if not theta_list: - return None - batch_theta = np.stack(theta_list, axis=0) - batch_trans = np.stack(trans_list, axis=0) - global_orient = batch_theta[:, :3] - finger_pose = batch_theta[:, 3:48] - if batch_theta.shape[0] != N: - raise ValueError( - f"Hand pkl has {batch_theta.shape[0]} frames, object poses have {N}" - ) - return ( - torch.from_numpy(batch_trans).float().to(device), - torch.from_numpy(global_orient).float().to(device), - torch.from_numpy(finger_pose).float().to(device), - ) - - -def _triplet_to_safe_id(triplet: str) -> str: - """Normalize triplet string for use in sequence_id.""" - return re.sub(r"[\s(),]", "_", triplet).strip("_") - - -class TacoDatasetLoader(DatasetLoaderBase): - """TACO dataset loader.""" - - def _list_sequences_for_triplet( - self, dataset_root: Path, triplet: str - ) -> list[SequenceInfo]: - """List TACO sequences for a single triplet.""" - sequences = _list_taco_sequences(dataset_root, triplet) - if not sequences: - return [] - out: list[SequenceInfo] = [] - for sequence in sequences: - object_pose_dir = dataset_root / "Object_Poses" / triplet / sequence - hand_pose_dir = dataset_root / "Hand_Poses" / triplet / sequence - if not object_pose_dir.is_dir() or not hand_pose_dir.is_dir(): - continue - try: - tool_name, target_name = _discover_tool_and_target(object_pose_dir) - tool_poses, target_poses = _load_taco_object_poses( - object_pose_dir, tool_name, target_name - ) - except (FileNotFoundError, ValueError): - continue - N = tool_poses.shape[0] - sequence_id = f"taco_{_triplet_to_safe_id(triplet)}_{sequence}" - raw_motion_file = f"{triplet}/{sequence}" - object_name = f"{tool_name}_{target_name}" - out.append( - SequenceInfo( - sequence_id=sequence_id, - raw_motion_file=raw_motion_file, - object_name=object_name, - object_body_names=TACO_OBJECT_BODY_NAMES, - source=TacoSequenceSource( - hand_pose_dir=hand_pose_dir, - object_pose_dir=object_pose_dir, - num_frames=N, - tool_name=tool_name, - target_name=target_name, - tool_poses=tool_poses, - target_poses=target_poses, - ), - ) - ) - return out - - def list_sequences(self, args: Any) -> list[SequenceInfo]: - """List TACO sequences for the given dataset root and triplet(s). - - If args.triplet is None, all triplets under Hand_Poses/ are processed. - """ - dataset_root = Path(args.dataset_root) - if args.triplet is not None: - return self._list_sequences_for_triplet(dataset_root, args.triplet) - triplets = _list_taco_triplets(dataset_root) - sample_n = getattr(args, "sample_triplets", None) - if sample_n is not None and sample_n < len(triplets): - triplets = sorted(random.sample(triplets, sample_n)) - print(f"Sampled {sample_n} triplets: {triplets}") - out: list[SequenceInfo] = [] - for triplet in triplets: - out.extend(self._list_sequences_for_triplet(dataset_root, triplet)) - return out - - def load_mano_data( - self, sequence_info: SequenceInfo, device: torch.device - ) -> dict[str, Any]: - """Load MANO parameters from TACO hand pkl files for the sequence.""" - src: TacoSequenceSource = sequence_info.source - N = src.num_frames - right_params = _mano_params_from_pkl(src.hand_pose_dir, "right", N, device) - left_params = _mano_params_from_pkl(src.hand_pose_dir, "left", N, device) - if right_params is None or left_params is None: - raise FileNotFoundError("Missing hand pkl data") - right_transl, right_global_orient, right_finger_pose = right_params - left_transl, left_global_orient, left_finger_pose = left_params - right_betas = ( - torch.from_numpy(load_taco_hand_shape(src.hand_pose_dir, "right")) - .float() - .to(device) - ) - left_betas = ( - torch.from_numpy(load_taco_hand_shape(src.hand_pose_dir, "left")) - .float() - .to(device) - ) - H = N - return { - "right_global_orient": right_global_orient, - "right_finger_pose": right_finger_pose, - "right_trans": right_transl, - "right_betas": right_betas, - "right_fitting_err": torch.zeros(H, device=device), - "left_global_orient": left_global_orient, - "left_finger_pose": left_finger_pose, - "left_trans": left_transl, - "left_betas": left_betas, - "left_fitting_err": torch.zeros(H, device=device), - "H": H, - } - - def load_object_data(self, sequence_info: SequenceInfo) -> dict[str, Any]: - """Load object data: name -> (pose, root_position, root_axis_angle, articulation).""" - src: TacoSequenceSource = sequence_info.source - tool_root_pos, tool_root_aa = poses_to_root_position_and_axis_angle( - src.tool_poses - ) - target_root_pos, target_root_aa = poses_to_root_position_and_axis_angle( - src.target_poses - ) - return { - TACO_OBJECT_BODY_NAMES[0]: ( - src.tool_poses, - tool_root_pos, - tool_root_aa, - None, - ), - TACO_OBJECT_BODY_NAMES[1]: ( - src.target_poses, - target_root_pos, - target_root_aa, - None, - ), - } - - def load_object_meshes( - self, - sequence_info: SequenceInfo, - device: torch.device, - ) -> tuple[ - dict[str, Any], - dict[str, torch.Tensor], - dict[str, torch.Tensor], - bool, - ]: - """Load TACO object meshes (tool/target _cm.obj) for the sequence.""" - src: TacoSequenceSource = sequence_info.source - mesh_dir = Path(getattr(self._args, "mesh_dir", TACO_OBJECT_MODEL_DIR)) - mesh_paths = { - "tool": str(mesh_dir / f"{src.tool_name}_cm.obj"), - "target": str(mesh_dir / f"{src.target_name}_cm.obj"), - } - return load_meshes_to_device(mesh_paths, device, vertex_scale=0.01) - - def get_mano_kwargs(self) -> dict[str, Any]: - """Return MANO model kwargs for TACO (flat_hand_mean=True, center_idx=0).""" - return {"flat_hand_mean": True, "center_idx": 0} - - def get_fps(self) -> float: - """Return TACO sequence FPS.""" - return TACO_FPS - - def get_object_mesh_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to TACO object meshes (tool and target _cm.obj).""" - src: TacoSequenceSource = sequence_info.source - mesh_dir = Path(getattr(self._args, "mesh_dir", TACO_OBJECT_MODEL_DIR)) - return [ - str(mesh_dir / f"{src.tool_name}_cm.obj"), - str(mesh_dir / f"{src.target_name}_cm.obj"), - ] - - def get_object_urdf_paths(self, sequence_info: SequenceInfo) -> list[str]: - """Return paths to TACO object URDF files (tool and target _rigid.urdf).""" - src: TacoSequenceSource = sequence_info.source - object_model_root = Path( - getattr(self._args, "object_model_root", TACO_OBJECT_URDF_DIR) - ) - return [ - str(object_model_root / f"{src.tool_name}_rigid.urdf"), - str(object_model_root / f"{src.target_name}_rigid.urdf"), - ] - - -def main(args: argparse.Namespace) -> None: - """Load TACO sequences and save as ManoSharpaData (MANO + object only).""" - loader = TacoDatasetLoader() - loader.run(args) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/reconstruction/modules/v2d_task_library_loader/tests/__init__.py b/reconstruction/modules/v2d_task_library_loader/tests/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/reconstruction/modules/v2d_task_library_loader/tests/test_distance_utils.py b/reconstruction/modules/v2d_task_library_loader/tests/test_distance_utils.py deleted file mode 100644 index 12db602e..00000000 --- a/reconstruction/modules/v2d_task_library_loader/tests/test_distance_utils.py +++ /dev/null @@ -1,253 +0,0 @@ -#!/usr/bin/env python3 -"""Test script for the v2d_task_library_loader distance_utils module. - -Run this script to verify the compute_tips_distance function works correctly. - -Usage (inside the v2d_task_library_loader image): - python -m v2d.task_library_loader.tests.test_distance_utils -""" - -import torch -from robotic_grounding.retarget import ASSETS_DIR, HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ManoSharpaData -from v2d.task_library_loader.lib.distance_utils import ( - MANO_FINGERTIP_INDICES, - PYTORCH3D_AVAILABLE, - compute_tips_distance, - load_object_mesh, -) - - -def test_compute_tips_distance_basic() -> None: - """Test compute_tips_distance with synthetic data.""" - device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - print(f"Testing on device: {device}") - print(f"PyTorch3D available: {PYTORCH3D_AVAILABLE}") - - # Create synthetic MANO joints (21 joints, 3D positions) - # Place joints at known positions - mano_joints = torch.zeros(21, 3, device=device) - # Place fingertips at specific locations (indices 4, 8, 12, 16, 20) - mano_joints[4] = torch.tensor([0.1, 0.0, 0.0], device=device) # thumb tip - mano_joints[8] = torch.tensor([0.0, 0.1, 0.0], device=device) # index tip - mano_joints[12] = torch.tensor([0.0, 0.0, 0.1], device=device) # middle tip - mano_joints[16] = torch.tensor([-0.1, 0.0, 0.0], device=device) # ring tip - mano_joints[20] = torch.tensor([0.0, -0.1, 0.0], device=device) # pinky tip - - # Create a simple cube mesh centered at origin - # Vertices of a unit cube centered at origin - obj_mesh_verts = torch.tensor( - [ - [-0.5, -0.5, -0.5], - [0.5, -0.5, -0.5], - [0.5, 0.5, -0.5], - [-0.5, 0.5, -0.5], - [-0.5, -0.5, 0.5], - [0.5, -0.5, 0.5], - [0.5, 0.5, 0.5], - [-0.5, 0.5, 0.5], - ], - dtype=torch.float32, - device=device, - ) - - # Simple cube faces (2 triangles per face, 6 faces = 12 triangles) - obj_mesh_faces = torch.tensor( - [ - [0, 1, 2], - [0, 2, 3], # front - [4, 6, 5], - [4, 7, 6], # back - [0, 4, 5], - [0, 5, 1], # bottom - [2, 6, 7], - [2, 7, 3], # top - [0, 3, 7], - [0, 7, 4], # left - [1, 5, 6], - [1, 6, 2], # right - ], - dtype=torch.int64, - device=device, - ) - - # Identity rotation (no rotation) - obj_rotation = torch.eye(3, device=device) - # No translation - obj_translation = torch.zeros(3, device=device) - - # Compute distances - distances = compute_tips_distance( - mano_joints, - obj_mesh_verts, - obj_mesh_faces, - obj_rotation, - obj_translation, - num_samples=1000, - ) - - print(f"Fingertip indices: {MANO_FINGERTIP_INDICES}") - print(f"Computed distances shape: {distances.shape}") - print(f"Distances: {distances.cpu().numpy()}") - - # Verify shape - assert distances.shape == (5,), f"Expected shape (5,), got {distances.shape}" - - # Verify distances are positive - assert (distances >= 0).all(), "Distances should be non-negative" - - # The cube surface is at 0.5 from origin, fingertips are at 0.1 from origin - # So distance should be approximately 0.5 - 0.1 = 0.4 for most fingertips - # (This is approximate since we sample points on the surface) - print( - "Expected approximate distance: ~0.4 (cube surface at 0.5, fingertips at 0.1)" - ) - - print("test_compute_tips_distance_basic PASSED") - - -def test_compute_tips_distance_with_transform() -> None: - """Test compute_tips_distance with object transformation.""" - device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - - # MANO joints - fingertip at origin - mano_joints = torch.zeros(21, 3, device=device) - mano_joints[4] = torch.tensor([0.0, 0.0, 0.0], device=device) # thumb at origin - - # Small sphere-like mesh (icosahedron approximation) - # For simplicity, use a tetrahedron - obj_mesh_verts = ( - torch.tensor( - [ - [1.0, 1.0, 1.0], - [1.0, -1.0, -1.0], - [-1.0, 1.0, -1.0], - [-1.0, -1.0, 1.0], - ], - dtype=torch.float32, - device=device, - ) - * 0.1 - ) # Scale down - - obj_mesh_faces = torch.tensor( - [ - [0, 1, 2], - [0, 1, 3], - [0, 2, 3], - [1, 2, 3], - ], - dtype=torch.int64, - device=device, - ) - - # Place object at (1, 0, 0) - obj_rotation = torch.eye(3, device=device) - obj_translation = torch.tensor([1.0, 0.0, 0.0], device=device) - - distances = compute_tips_distance( - mano_joints, - obj_mesh_verts, - obj_mesh_faces, - obj_rotation, - obj_translation, - num_samples=500, - ) - - print("\nWith object translated to (1, 0, 0):") - print(f"Thumb tip distance: {distances[0].item():.4f}") - - # Distance should be approximately 1.0 - 0.1 = 0.9 (object center at 1.0, radius ~0.1) - assert distances[0] > 0.5, f"Expected distance > 0.5, got {distances[0]}" - - print("test_compute_tips_distance_with_transform PASSED") - - -def test_load_object_mesh() -> None: - """Test load_object_mesh function (requires actual mesh file).""" - # Try to find a mesh file - mesh_dir = ASSETS_DIR / "meshes" / "arctic" - if not mesh_dir.exists(): - print( - f"\nSkipping test_load_object_mesh: mesh directory not found at {mesh_dir}" - ) - return - - # Look for any .obj file - obj_files = list(mesh_dir.glob("**/*.obj")) - if not obj_files: - print(f"\nSkipping test_load_object_mesh: no .obj files found in {mesh_dir}") - return - - mesh_path = obj_files[0] - print(f"\nTesting load_object_mesh with: {mesh_path}") - - verts, faces = load_object_mesh(str(mesh_path)) - - print(f"Vertices shape: {verts.shape}") - print(f"Faces shape: {faces.shape}") - - assert ( - verts.ndim == 2 and verts.shape[1] == 3 - ), f"Expected vertices (V, 3), got {verts.shape}" - assert ( - faces.ndim == 2 and faces.shape[1] == 3 - ), f"Expected faces (F, 3), got {faces.shape}" - assert verts.dtype == torch.float32, f"Expected float32 vertices, got {verts.dtype}" - assert faces.dtype == torch.int64, f"Expected int64 faces, got {faces.dtype}" - - print("test_load_object_mesh PASSED") - - -def test_integration_with_real_data() -> None: - """Integration test with actual ARCTIC data (if available).""" - # Check for processed parquet data - processed_dir = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_processed" - if not processed_dir.exists(): - print( - f"\nSkipping integration test: processed data not found at {processed_dir}" - ) - return - - try: - # Load a sample trajectory - data = ManoSharpaData.from_parquet(str(processed_dir)) - print(f"\nLoaded trajectory: {data.sequence_id}") - print(f"Object: {data.object_name}") - print(f"Number of frames: {len(data.mano_right_joints)}") - - # Check if tips_distance was computed - if data.mano_right_tips_distance: - print( - f"Right tips_distance available: {len(data.mano_right_tips_distance)} frames" - ) - print(f"Sample distances (frame 0): {data.mano_right_tips_distance[0]}") - else: - print("Right tips_distance not yet computed for this trajectory") - - if data.mano_left_tips_distance: - print( - f"Left tips_distance available: {len(data.mano_left_tips_distance)} frames" - ) - else: - print("Left tips_distance not yet computed for this trajectory") - - print("test_integration_with_real_data PASSED") - - except Exception as e: - print(f"Integration test skipped due to: {e}") - - -if __name__ == "__main__": - print("=" * 60) - print("Testing distance_utils module") - print("=" * 60) - - test_compute_tips_distance_basic() - test_compute_tips_distance_with_transform() - test_load_object_mesh() - test_integration_with_real_data() - - print("\n" + "=" * 60) - print("All tests completed!") - print("=" * 60) diff --git a/reconstruction/workflows/task_library_load/osmo/load.yaml b/reconstruction/workflows/task_library_load/osmo/load.yaml deleted file mode 100644 index 64eff096..00000000 --- a/reconstruction/workflows/task_library_load/osmo/load.yaml +++ /dev/null @@ -1,87 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# Task-library LOAD workflow: raw hand-object dataset -# -> MANO FK + object FK + contacts -> {dataset}_loaded Parquet (ManoSharpaData, -# MANO + object only). This is the stage that uses manotorch (GPL-3.0), contained -# to the v2d_task_library_loader image; the loaded Parquet is consumed downstream -# by robotic_grounding retarget.yaml -# -# Inputs are fetched from CSS at runtime (image stays lean): raw dataset, the -# per-dataset object assets (urdfs+meshes), and the MANO models (licensed). -# -# Prereqs: -# - image built with `mujoco<3.7` (3.7+ breaks the arctic object URDFs) and pushed. -# - object_assets on CSS for {{dataset}} (committed-mesh datasets: upload via -# scripts/upload_object_assets.py; h2o/grab/dexycb resolve meshes from raw). -# - MANO models: a clean swift copy of v2d_ego_data's hand dir at -# .../datasets/v2d/reconstruction/data/weights/hand/ (has models/MANO_*.pkl). -# -# Usage: -# osmo workflow submit reconstruction/workflows/task_library_load/osmo/load.yaml \ -# --set dataset=arctic --pool - -workflow: - name: {{workflow_name}} - resources: - default: - cpu: 8 # <=1/8 of an 8-GPU node's CPUs when requesting 1 GPU - gpu: 1 - memory: 64Gi - storage: 100Gi - - tasks: - - name: load - # TODO(public-release): internal image registry (nvcr.io/nvstaging) — replace before open-sourcing. - image: nvcr.io/nvstaging/isaac-amr/v2d_task_library_loader:{{image_tag}} - command: - - /bin/bash - args: - - /tmp/entry.sh - inputs: - - url: {{raw_url}} # {{input:0}} raw dataset (e.g. .../arctic/dataset/) - - url: {{object_assets_url}} # {{input:1}} urdfs/ + meshes/ (sibling layout) - - url: {{mano_url}} # {{input:2}} MANO hand dir (has models/MANO_*.pkl) - outputs: - - url: {{loaded_output_url}} # {{output}} -> {dataset}_loaded Parquet - files: - - path: /tmp/entry.sh - contents: |- - set -ex - # The loader derives sequence_id from the dataset_root path tail - # (.../dataset//.mano.npy -> "dataset__"). OSMO - # mounts the raw input at /osmo/data/input/0, which would yield "0__..". - # Symlink it to a "dataset"-named dir so the ids match the convention - # (dataset_s01_box_grab_01), consistent with existing data. - ln -sfn {{input:0}} /tmp/dataset - FILTER_ARGS="" - [ -n "{{sequence_pattern}}" ] && FILTER_ARGS="${FILTER_ARGS} --sequence_pattern '{{sequence_pattern}}'" - [ -n "{{sequence_id}}" ] && FILTER_ARGS="${FILTER_ARGS} --sequence_id {{sequence_id}}" - [ -n "{{max_sequences}}" ] && FILTER_ARGS="${FILTER_ARGS} --max_sequences {{max_sequences}}" - eval python -m v2d.task_library_loader.lib.run_loader \ - --dataset {{dataset}} \ - --dataset_root /tmp/dataset \ - --object_model_root {{input:1}}/urdfs/{{dataset}} \ - --mesh_dir {{input:1}}/meshes/{{dataset}} \ - --mano_model_dir {{input:2}} \ - --output_dir {{output}} \ - --device cuda:0 --save ${FILTER_ARGS} - -default-values: - workflow_name: v2d_arctic_load - image_tag: latest - dataset: arctic - # TODO(public-release): the swift:// *_url defaults below point to internal NVIDIA - # storage (pdx.s8k.io / AUTH_team-isaac). Replace the swift host + bucket (or - # override all via --set) before open-sourcing. - raw_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/arctic/dataset/ - object_assets_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/arctic/object_assets/ - # MANO hand dir (clean swift copy of v2d_ego_data's data/weights/hand; - # contains models/MANO_{LEFT,RIGHT}.pkl — manotorch's mano_assets_root layout). - mano_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/reconstruction/data/weights/hand/ - # Default writes the canonical _loaded prefix (regenerates it). Override to a - # scratch prefix for tests so you don't clobber an existing _loaded. - loaded_output_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/arctic/arctic_loaded/ - # Optional sequence filters (mirror the loader CLI). - sequence_pattern: "" - sequence_id: "" - max_sequences: "" diff --git a/robotic_grounding/.claude/commands/eval-record.md b/robotic_grounding/.claude/commands/eval-record.md deleted file mode 100644 index c58d767d..00000000 --- a/robotic_grounding/.claude/commands/eval-record.md +++ /dev/null @@ -1,309 +0,0 @@ ---- -name: eval-record -description: Download wandb run model(s) (if not already present) then run one full-trajectory recording eval in Docker for each, saving videos to experiments/eval_recordings//. Also supports --stats N mode for aggregate episode statistics. -user-invocable: true -allowed-tools: - - Bash ---- - -# /eval-record — Get Run(s) + Record One Full Trajectory in Docker - -Given one or more run names, for each: ensure the model checkpoint is downloaded, compute the -trajectory length from the motion file, then launch eval.py with `--video --video_length ---num_envs 7` inside the Docker container. Videos auto-copy to `experiments/eval_recordings//` -named `_model.mp4`. When multiple runs are given, they execute **sequentially**. - -**Stats mode** (`--stats N`): instead of recording video, runs N episodes across 32 envs (headless) -and prints aggregate stats (mean episode length, completion ratio, full completion rate). No video -is saved in this mode. - -Arguments: `$ARGUMENTS` - -**Single run:** -``` -/eval-record [--project ] [--model ] [voc=] [debug_vis | no debug_vis] [--stats N] -``` - -**Multiple runs (shared options apply to all):** -``` -/eval-record ... [shared options] -``` - -Examples: -- `/eval-record 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01` -- `/eval-record 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 voc=1.0` -- `/eval-record 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 debug_viz` -- `/eval-record 2026-04-15_12-02-40_exp67_stage2_espressomachine_grab_01 model_9000.pt loose termination` -- `/eval-record 2026-04-15_12-02-40_exp67_stage2_espressomachine_grab_01 --stats 100` -- `/eval-record 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 2026-04-15_12-02-40_exp67_stage2_espressomachine_grab_01` -- `/eval-record 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 2026-04-15_12-02-40_exp67_stage2_espressomachine_grab_01 2026-04-15_14-06-44_exp65_stage2_microwave_grab_01` - ---- - -## Step 1 — Parse arguments - -Parse from: `$ARGUMENTS` - -**Identify run names** by matching tokens against the pattern `\d{4}-\d{2}-\d{2}_\d{2}-\d{2}-\d{2}_\S+`. -All matching tokens are run names; everything else is a shared option. - -**Shared options** (applied to every run in the batch): -- `project`: from `--project `, default `v2p_hands` -- `model`: from `--model `, default `None` (most recent). Also accept bare `model ` or `model_` → `model_.pt`. Applies to all runs if specified (unusual for batches — warn if used with multiple runs). -- `voc`: from `voc=`, default `0.0`. Hydra key: `env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale=` -- `debug_vis`: `debug_vis` → `true`; `no debug_vis` → `false`; default `false`. Always emit `env.commands.dual_hands_object_tracking_command.debug_vis=`. -- `loose_termination`: any of `loose`, `loose termination`, `loose_termination`, `loose term` → sets all early-termination thresholds to 100.0 (effectively disabling them, only timeout terminates). Default: `false` (use training defaults). When enabled, adds these three Hydra overrides: - - `env.terminations.hand_wrist_away_from_trajectory.params.threshold=100.0` - - `env.terminations.object_away_from_trajectory.params.position_threshold=100.0` - - `env.terminations.object_away_from_trajectory.params.orientation_threshold=100.0` -- `stats`: from `--stats `, default `None`. When set, runs N episodes with `--eval_episodes N --num_envs 32 --headless` (no video). Reports the printed summary from eval.py. Mutually exclusive with video mode. - -After parsing, print the job list before starting: -``` -[eval-record] Batch: N run(s) to process - [1/N] - [2/N] - ... -Shared options: project=v2p_hands, voc=0.0, debug_vis=false, model=most recent, loose_termination=false, stats=N or off -``` - ---- - -## Steps 2–8 loop — For each run in the list - -Execute steps 2 through 8 for each run in order. Print a header before each: -``` -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -[eval-record] Run [i/N]: -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -``` - -If a run fails (download error, eval crash, missing video), log the error, mark it as FAILED in the -results table, and continue with the next run. Do not abort the batch. - ---- - -## Step 2 — Determine checkpoint path - -The local checkpoint folder is: -``` -logs/rsl_rl/sharpa_v2p// -``` - -**If `--model` was specified**: check if that exact file exists locally. If yes, skip to Step 3.5. If no, proceed to Step 3 to download it. - -**If no model specified**: always query W&B first (Step 3) to find the latest checkpoint iteration, then check if that specific file is already local. This ensures we use the newest W&B checkpoint even if a (older) local file exists. - -## Step 3 — Resolve latest checkpoint from W&B - -```python -import wandb, os, re -api = wandb.Api() -runs = api.runs(f"nvidia-isaac/{project}", filters={"display_name": {"$regex": run_name}}) -runs = sorted(runs, key=lambda r: r.created_at, reverse=True) -run = runs[0] - -files = [f for f in run.files() if f.name.endswith(".pt")] -def iteration(f): - m = re.search(r'model_(\d+)\.pt', f.name) - return int(m.group(1)) if m else -1 -target = max(files, key=iteration) -model_filename = os.path.basename(target.name) - -dest = f"logs/rsl_rl/sharpa_v2p/{run_name}" -local_path = os.path.join(dest, model_filename) - -if os.path.exists(local_path): - print(f"Already have latest: {model_filename} — skipping download") -else: - os.makedirs(dest, exist_ok=True) - run.file(target.name).download(root=dest, replace=True) - print(f"Downloaded: {model_filename}") -``` - -## Step 3.5 — Read network architecture from checkpoint - -```bash -docker exec robotic-grounding-v2d-gpu1 bash -c "cat > /tmp/get_arch.py << 'PYEOF' -import torch -model_path = 'logs/rsl_rl/sharpa_v2p//' -checkpoint = torch.load(model_path, map_location='cpu') -sd = checkpoint.get('model_state_dict', checkpoint) -def dims(sd, prefix): - keys = sorted([k for k in sd if k.startswith(prefix+'.') and k.endswith('.weight')], key=lambda k: int(k.split('.')[1])) - return [sd[k].shape[0] for k in keys[:-1]] if len(keys)>1 else None -print('actor='+str(dims(sd,'actor'))) -print('critic='+str(dims(sd,'critic'))) -PYEOF -cd /workspace/video_to_data/robotic_grounding && /workspace/isaaclab/_isaac_sim/python.sh /tmp/get_arch.py" -``` - -Parse `actor=[...]` and `critic=[...]`. Format as `[512,256,128]` (no spaces). Always add to eval command: -``` -agent.policy.actor_hidden_dims= -agent.policy.critic_hidden_dims= -``` -If extraction fails, omit and warn. - -## Step 4 — Derive the motion_file from the run name - -Look for `(\w+)_grab_(\d+)` at the end of the run name. Examples: -- `..._mixer_grab_01` → `arctic_s01_mixer_grab_01` -- `..._espressomachine_grab_01` → `arctic_s01_espressomachine_grab_01` -- `..._microwave_grab_01` → `arctic_s01_microwave_grab_01` - -If not found via that pattern, use the last `_`-separated token as the object name and construct -`arctic_s01__grab_01`. If the last token is `grab`, use the second-to-last as the object. - -Motion file path: `arctic/arctic_processed//sharpa_wave` - -## Step 5 — Compute trajectory length from motion file - -**Important**: Use pyarrow directly (not SceneConfig — it can't be imported without the full omni.* stack): - -```bash -docker exec robotic-grounding-v2d-gpu1 bash -c "cat > /tmp/get_traj_len.py << 'PYEOF' -import pyarrow.parquet as pq, glob, os -HUMAN_MOTION_DATA_DIR = '/workspace/video_to_data/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data' -seq_id = '' -parquet_dir = os.path.join(HUMAN_MOTION_DATA_DIR, 'arctic', 'arctic_processed', f'sequence_id={seq_id}', 'robot_name=sharpa_wave') -files = glob.glob(os.path.join(parquet_dir, '*.parquet')) -data = pq.read_table(files[0]).to_pydict() -timesteps = len(data['object_articulation'][0]) -fps = float(data.get('fps', [[30.0]])[0]) -episode_length_s = timesteps / fps -video_length = int(round(episode_length_s * 20)) -print(f'episode_length_s={episode_length_s:.3f}') -print(f'video_length={video_length}') -PYEOF -/workspace/isaaclab/_isaac_sim/python.sh /tmp/get_traj_len.py" -``` - -Fallback: `video_length=200` if script fails. - -## Step 6 — Ensure the Docker container is running - -Container: `robotic-grounding-v2d-gpu1`. Check once before the first run; skip check for subsequent runs. - -```bash -docker ps --format '{{.Names}}' | grep -q "^robotic-grounding-v2d-gpu1$" -``` - -If NOT running, start it: -```bash -WANDB_API_KEY_VALUE="${WANDB_API_KEY}" docker run --rm --runtime=nvidia --gpus device=1 --network host --name robotic-grounding-v2d-gpu1 -v $(pwd):/workspace/video_to_data/robotic_grounding -v ~/.ssh:/root/.ssh:ro -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY="${DISPLAY}" -e WANDB_API_KEY="${WANDB_API_KEY_VALUE}" -e "ACCEPT_EULA=Y" -d --entrypoint /bin/bash robotic-grounding:v2d -``` - -## Step 7 — Write and launch the eval script - -**IMPORTANT**: Command must be a single line (no `\` continuation). - -### 7a — Stats mode (`--stats N` was specified) - -Use `--eval_episodes N --num_envs 32 --headless` (no `--video`). No video is saved. - -```bash -cat > /tmp/run_eval_record.sh << 'EOF' -#!/bin/bash -set -e -cd /workspace/video_to_data/robotic_grounding -python scripts/rsl_rl/eval.py --task Sharpa-V2P-v0-Play --num_envs 32 --headless --eval_episodes --checkpoint logs/rsl_rl/sharpa_v2p// --motion_file arctic/arctic_processed//sharpa_wave 'env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale=' 'env.commands.dual_hands_object_tracking_command.debug_vis=false' 'env.commands.dual_hands_object_tracking_command.always_reset_to_first_frame=true' 'agent.policy.actor_hidden_dims=' 'agent.policy.critic_hidden_dims=' [IF loose_termination: 'env.terminations.hand_wrist_away_from_trajectory.params.threshold=100.0' 'env.terminations.object_away_from_trajectory.params.position_threshold=100.0' 'env.terminations.object_away_from_trajectory.params.orientation_threshold=100.0'] -EOF -docker cp /tmp/run_eval_record.sh robotic-grounding-v2d-gpu1:/tmp/run_eval_record.sh -docker exec robotic-grounding-v2d-gpu1 bash -c "nohup bash /tmp/run_eval_record.sh > /tmp/eval_record_output.log 2>&1 &" -``` - -After completion, grep the log for the summary block and print it verbatim: -```bash -docker exec robotic-grounding-v2d-gpu1 grep -A6 'Eval Summary' /tmp/eval_record_output.log || docker exec robotic-grounding-v2d-gpu1 tail -20 /tmp/eval_record_output.log -``` - -### 7b — Video mode (default, no `--stats`) - -**IMPORTANT**: Use `--num_envs 7` — Play config has `viewer.env_index=6` (not Hydra-overridable); needs ≥7 envs. - -Output video name: `_model.mp4` - -Note: gymnasium names the recorded file `rl-video-step-0.mp4` (uses `step_trigger`). The copy step renames it. - -```bash -cat > /tmp/run_eval_record.sh << 'EOF' -#!/bin/bash -set -e -cd /workspace/video_to_data/robotic_grounding -python scripts/rsl_rl/eval.py --task Sharpa-V2P-v0-Play --num_envs 7 --video --video_length --checkpoint logs/rsl_rl/sharpa_v2p// --motion_file arctic/arctic_processed//sharpa_wave 'env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale=' 'env.commands.dual_hands_object_tracking_command.debug_vis=' 'agent.policy.actor_hidden_dims=' 'agent.policy.critic_hidden_dims=' [IF loose_termination: 'env.terminations.hand_wrist_away_from_trajectory.params.threshold=100.0' 'env.terminations.object_away_from_trajectory.params.position_threshold=100.0' 'env.terminations.object_away_from_trajectory.params.orientation_threshold=100.0'] -VIDEO_DIR="logs/rsl_rl/sharpa_v2p//videos/play" -OUT_DIR="experiments/eval_recordings/" -OUT_NAME="_model.mp4" -mkdir -p "${OUT_DIR}" -VIDEO_FILE=$(ls "${VIDEO_DIR}"/*.mp4 2>/dev/null | head -1) -if [ -n "${VIDEO_FILE}" ]; then - cp "${VIDEO_FILE}" "${OUT_DIR}/${OUT_NAME}" - echo "[eval-record] Video saved: ${OUT_DIR}/${OUT_NAME}" -else - echo "[eval-record] WARNING: no .mp4 found in ${VIDEO_DIR}" - ls "${VIDEO_DIR}" 2>/dev/null || true -fi -EOF -docker cp /tmp/run_eval_record.sh robotic-grounding-v2d-gpu1:/tmp/run_eval_record.sh -docker exec robotic-grounding-v2d-gpu1 bash -c "nohup bash /tmp/run_eval_record.sh > /tmp/eval_record_output.log 2>&1 &" -``` - -Confirm process started: -```bash -docker exec robotic-grounding-v2d-gpu1 pgrep -a python | grep eval.py -``` - -## Step 8 — Monitor to completion - -Poll every 60 seconds until eval exits. Stats mode with 32 envs typically takes 3–8 minutes; video mode 1–3 minutes. - -```bash -while docker exec robotic-grounding-v2d-gpu1 pgrep -f "eval.py" > /dev/null 2>&1; do - echo " [i/N] Still running... ($(date '+%H:%M:%S'))" - sleep 60 -done -``` - -Once exited: - -**Stats mode**: extract and print the summary block from the log: -```bash -docker exec robotic-grounding-v2d-gpu1 grep -A6 'Eval Summary' /tmp/eval_record_output.log || docker exec robotic-grounding-v2d-gpu1 tail -20 /tmp/eval_record_output.log -``` - -**Video mode**: tail the log and verify file: -```bash -docker exec robotic-grounding-v2d-gpu1 tail -5 /tmp/eval_record_output.log -ls -lh experiments/eval_recordings// -``` - -Record the outcome, then proceed to the next run. - ---- - -## Step 9 — Final summary table - -After all runs complete, print a summary table: - -**Video mode:** -``` -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -[eval-record] Batch complete — N run(s) -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ - ✓ model_XXXXX.pt s experiments/eval_recordings/... - ✓ model_XXXXX.pt s experiments/eval_recordings/... - ✗ FAILED: -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -``` - -**Stats mode:** -``` -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -[eval-record] Batch complete — N run(s) [stats mode: episodes each] -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ - ✓ model_XXXXX.pt ratio=0.XXX±0.XXX completions=XX/M (XX%) - ✓ model_XXXXX.pt ratio=0.XXX±0.XXX completions=XX/M (XX%) - ✗ FAILED: -━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━ -``` diff --git a/robotic_grounding/.claude/commands/getneval.md b/robotic_grounding/.claude/commands/getneval.md deleted file mode 100644 index 299fccc9..00000000 --- a/robotic_grounding/.claude/commands/getneval.md +++ /dev/null @@ -1,198 +0,0 @@ ---- -name: getneval -description: Download a wandb run model (if not already present) then run eval.py inside Docker on it. -user-invocable: true -allowed-tools: - - Bash ---- - -# /getneval — Get Run + Evaluate in Docker - -Given a run name, ensure the model checkpoint is downloaded, then launch eval.py inside the Docker container. - -Arguments: `$ARGUMENTS` -Expected format: ` [--project ] [--model ] [voc=]` -Examples: -- `/getneval 2026-03-31_11-33-13_exp37_cont5p0_cum0p5_with_contact_tracking_capsulemachine` -- `/getneval 2026-03-31_11-33-13_exp37_cont5p0_cum0p5_with_contact_tracking_capsulemachine --model model_2000.pt` -- `/getneval 2026-04-11_16-25-54_exp58_v3_stage2_microwave_grab_01 model 2000 voc=1.0` - ---- - -## Step 1 — Parse arguments - -Parse from: `$ARGUMENTS` - -- `run_name`: first positional argument (full run name string) -- `project`: from `--project `, default `v2p_hands` -- `model`: from `--model `, default `None` (means use most recent). Also accept bare `model ` or `model_` positional shorthand → `model_.pt` -- `voc`: from `voc=` anywhere in the args, default `0.0`. Expands to the full Hydra key `env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale=` - -## Step 2 — Determine checkpoint path - -The local checkpoint folder is: -``` -logs/rsl_rl/sharpa_v2p// -``` - -Check if any `model_*.pt` file already exists in that folder. - -- If `--model` was specified: check if that exact file exists. -- Otherwise: look for all `model_*.pt` files, extract iteration numbers, and identify the highest one as the "most recent". - -If the required model is already present, skip to Step 4 (do not re-download). -If it is missing (or the folder doesn't exist), proceed to Step 3. - -## Step 3 — Download the model via /getrun logic - -Use the wandb Python API to find the run and download the model exactly as `/getrun` does: - -```python -import wandb, os, re -api = wandb.Api() -runs = api.runs(f"nvidia-isaac/{project}", filters={"display_name": {"$regex": run_name}}) -runs = sorted(runs, key=lambda r: r.created_at, reverse=True) -run = runs[0] - -files = [f for f in run.files() if f.name.endswith(".pt")] -# find most recent by extracting N from model_N.pt -def iteration(f): - m = re.search(r'model_(\d+)\.pt', f.name) - return int(m.group(1)) if m else -1 -target = max(files, key=iteration) - -dest = f"logs/rsl_rl/sharpa_v2p/{run_name}" -os.makedirs(dest, exist_ok=True) -target_file = run.file(target.name) -target_file.download(root=dest, replace=True) -model_filename = target.name -``` - -Report which model was downloaded. - -## Step 3.5 — Read network architecture from checkpoint - -After downloading (or if the model was already cached), read the actor/critic hidden dims directly from the `.pt` checkpoint's state dict. This avoids `size mismatch` errors. Run this inside the container using `python.sh` (torch is only available via that wrapper): - -```bash -# Write script to extract architecture -docker exec robotic-grounding-v2d-gpu1 bash -c "cat > /tmp/get_arch.py << 'PYEOF' -import torch -model_path = 'logs/rsl_rl/sharpa_v2p//' -checkpoint = torch.load(model_path, map_location='cpu') -sd = checkpoint.get('model_state_dict', checkpoint) -def dims(sd, prefix): - keys = sorted([k for k in sd if k.startswith(prefix+'.') and k.endswith('.weight')], key=lambda k: int(k.split('.')[1])) - return [sd[k].shape[0] for k in keys[:-1]] if len(keys)>1 else None -print('actor='+str(dims(sd,'actor'))) -print('critic='+str(dims(sd,'critic'))) -PYEOF -cd /workspace/video_to_data/robotic_grounding && /workspace/isaaclab/_isaac_sim/python.sh /tmp/get_arch.py" -``` - -Parse `actor=[...]` and `critic=[...]` from the output. Linear layers are at even indices (0, 2, 4, ...); weight shape is `[out, in]`. All but the last are hidden layers. - -Always add the architecture overrides to the eval command in Step 6: -``` -agent.policy.actor_hidden_dims= -agent.policy.critic_hidden_dims= -``` -Format lists as `[512,256,128]` (no spaces). If extraction fails, omit and warn the user. - -## Step 4 — Derive the motion_file from the run name - -`eval.py` uses `--motion_file`, not `--sequence_filter`. - -**First**, check if the run name already contains a full `arctic_s01_..._grab_01` pattern. -If so, extract the sequence id directly — e.g.: -- `..._stage2_microwave_grab_01` → sequence id `arctic_s01_microwave_grab_01` - -Use a regex like `(arctic_s01_\w+_grab_\d+)` to detect this case. - -**Otherwise**, the run name ends with just the object name as the last `_`-separated token. -Construct the sequence id as `arctic_s01__grab_01`: -- `..._capsulemachine` → `arctic_s01_capsulemachine_grab_01` -- `..._espressomachine` → `arctic_s01_espressomachine_grab_01` -- `..._waffleiron` → `arctic_s01_waffleiron_grab_01` -- `..._microwave` → `arctic_s01_microwave_grab_01` -- `..._box` → `arctic_s01_box_grab_01` - -If the last token is literally `grab`, use the second-to-last token as the object name. - -Then form the full motion_file path: `arctic/arctic_processed//sharpa_wave` - -## Step 5 — Ensure the Docker container is running - -Container name: `robotic-grounding-v2d-gpu1` - -Check if it is already running: -```bash -docker ps --format '{{.Names}}' | grep -q "^robotic-grounding-v2d-gpu1$" -``` - -If NOT running, start it (replicating what `./workflow/run.sh start v2d 1` does, but detached without entering an interactive shell): -```bash -cd /path/to/repo && \ -WANDB_API_KEY_VALUE="${WANDB_API_KEY}" && \ -docker run --rm \ - --runtime=nvidia \ - --gpus device=1 \ - --network host \ - --name robotic-grounding-v2d-gpu1 \ - -v $(pwd):/workspace/video_to_data/robotic_grounding \ - -v ~/.ssh:/root/.ssh:ro \ - -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ - -e DISPLAY="${DISPLAY}" \ - -e WANDB_API_KEY="${WANDB_API_KEY_VALUE}" \ - -e "ACCEPT_EULA=Y" \ - -d \ - --entrypoint /bin/bash \ - robotic-grounding:v2d -``` - -Wait briefly and confirm the container started successfully. - -## Step 6 — Run eval.py inside the container - -The checkpoint path inside the container mirrors the host mount: -``` -/workspace/video_to_data/robotic_grounding/logs/rsl_rl/sharpa_v2p// -``` - -**IMPORTANT**: The eval command MUST be issued as a single line — multiline `\` continuation -commands break when pasted into a terminal and cause Isaac Sim to launch with no arguments -(`task_name=None`). Always build the command as one line. - -Write the command to a script file on the host, then launch it detached inside the container -so Isaac Sim opens without blocking Claude: - -```bash -# 1. Write script (on host via Bash tool) -cat > /tmp/run_eval.sh << 'EOF' -#!/bin/bash -cd /workspace/video_to_data/robotic_grounding -python scripts/rsl_rl/eval.py --task Sharpa-V2P-v0-Play --checkpoint logs/rsl_rl/sharpa_v2p// --motion_file arctic/arctic_processed//sharpa_wave 'env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale=' <'agent.policy.actor_hidden_dims=[...]' if found> <'agent.policy.critic_hidden_dims=[...]' if found> -EOF - -# 2. Copy script into container (/tmp is not shared between host and container) -docker cp /tmp/run_eval.sh robotic-grounding-v2d-gpu1:/tmp/run_eval.sh - -# 3. Launch detached inside container (output goes to /tmp/eval_output.log) -docker exec robotic-grounding-v2d-gpu1 bash -c "nohup bash /tmp/run_eval.sh > /tmp/eval_output.log 2>&1 &" -``` - -After launching, confirm the process started: -```bash -docker exec robotic-grounding-v2d-gpu1 pgrep -a python | grep eval.py -``` - -If architecture overrides are not found in W&B, omit those args and warn the user that the eval task's default architecture will be used. - -## Step 7 — Report - -Print a summary: -- Run name and project -- Model checkpoint used (and whether it was freshly downloaded or already cached) -- Motion file derived -- Container used -- Full eval command executed diff --git a/robotic_grounding/.claude/commands/grid-video.md b/robotic_grounding/.claude/commands/grid-video.md deleted file mode 100644 index 1b92bede..00000000 --- a/robotic_grounding/.claude/commands/grid-video.md +++ /dev/null @@ -1,188 +0,0 @@ ---- -name: grid-video -description: Combine multiple video files into a grid layout where each loops, total length = max video duration. -user-invocable: true -allowed-tools: - - Bash ---- - -# /grid-video — Combine Videos into a Looping Grid - -Arrange N videos in a grid (each looping), with total duration equal to the longest individual video. Empty grid slots (when N doesn't fill the grid) are filled with black. - -Inputs can be **W&B run names** (resolved from `experiments/eval_recordings/`) or **direct `.mp4` file paths**. - -Arguments: `$ARGUMENTS` - -**Usage:** -``` -/grid-video ... [--output ] [--cell-size ] -``` - -Examples: -- `/grid-video 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 2026-04-15_12-02-40_exp67_stage2_espressomachine_grab_01` -- `/grid-video 2026-04-13_10-14-06_exp54_v3_stage2_20k_mixer_grab_01 a.mp4 b.mp4 --output my_grid.mp4` -- `/grid-video a.mp4 b.mp4 c.mp4 d.mp4 --cell-size 640x360` - ---- - -## Step 1 — Parse arguments - -From `$ARGUMENTS`: -- **Run names**: tokens matching `\d{4}-\d{2}-\d{2}_\d{2}-\d{2}-\d{2}_\S+` — resolve to video files (see Step 1.5) -- **Video files**: tokens that are existing file paths ending in `.mp4`, `.mov`, `.avi`, `.mkv` -- `--output `: output file, default `grid_output.mp4` -- `--cell-size `: override cell dimensions (e.g. `640x360`), default: use first video's native size - -## Step 1.5 — Resolve run names to video files - -For each run name token, find its video in `experiments/eval_recordings//`: - -```bash -ls experiments/eval_recordings//*.mp4 2>/dev/null | sort -t_ -k1 -V | tail -1 -``` - -This picks the highest-numbered model file (e.g. `_model50000.pt` > `_model10000`). If multiple `.mp4` files exist, pick the one with the largest model number by sorting. If none found, abort with an error. - -Print resolved paths: -``` -[grid-video] Resolved: → experiments/eval_recordings//.mp4 -``` - -After resolving all run names, combine with any directly-specified file paths into a final ordered video list. - -Print parsed summary: -``` -[grid-video] N videos, output=, cell_size= -``` - -## Step 2 — Get video durations and dimensions - -`ffprobe`/`ffmpeg` are not available on the host — use cv2 via the Isaac Sim Python inside Docker. Video paths inside the container are `/workspace/video_to_data/robotic_grounding/`. - -```bash -docker exec robotic-grounding-v2d-gpu1 /workspace/isaaclab/_isaac_sim/python.sh -c " -import cv2 -videos = ['/workspace/video_to_data/robotic_grounding/', ...] -for path in videos: - cap = cv2.VideoCapture(path) - w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)) - h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)) - fps = cap.get(cv2.CAP_PROP_FPS) - frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) - dur = frames / fps if fps > 0 else 0 - cap.release() - print(f'{path}: {w}x{h} fps={fps} dur={dur:.2f}s') -" -``` - -Collect width, height, and duration for each video. - -Report: -``` -[grid-video] Durations: v1=Xs v2=Xs ... max=Xs -[grid-video] First video dimensions: WxH -``` - -## Step 3 — Compute grid layout - -``` -N = number of input videos -cols = ceil(sqrt(N)) -rows = ceil(N / cols) -empty = cols * rows - N -``` - -Cell size: use `--cell-size` if given, otherwise use the first video's width x height. - -Print: -``` -[grid-video] Grid: x ( videos + empty slots), cell=x -``` - -## Step 4 — Build and run via Python script - -`ffmpeg` is not on the host PATH. Use the bundled binary from `imageio_ffmpeg` inside Docker via Isaac Sim Python. - -Write the script to `/tmp/build_grid_video.py` on the **host** (using the Write tool), then copy it into the container and run it. Use the container-side paths (`/workspace/video_to_data/robotic_grounding/...`) for all video and output paths. - -**IMPORTANT**: Do not use f-strings with nested quotes in the script — use string concatenation to avoid shell quoting issues. - -```python -import subprocess, math, os, sys -import imageio_ffmpeg - -FFMPEG = imageio_ffmpeg.get_ffmpeg_exe() - -videos = ["/workspace/video_to_data/robotic_grounding/", "/workspace/video_to_data/robotic_grounding/", ...] -output = "/workspace/video_to_data/robotic_grounding/" -W, H = , -max_duration = - -N = len(videos) -cols = math.ceil(math.sqrt(N)) -rows = math.ceil(N / cols) -total = cols * rows -empty = total - N - -# Build input args: loop each real video; fill empty slots with black -inputs = [] -for v in videos: - inputs += ["-stream_loop", "-1", "-i", v] -for _ in range(empty): - inputs += ["-f", "lavfi", "-i", f"color=black:size={W}x{H}:rate=30"] - -# Scale each input to uniform cell size (letterbox to preserve aspect ratio) -scale_parts = [] -for i in range(total): - scale_parts.append( - f"[{i}:v]scale={W}:{H}:force_original_aspect_ratio=decrease," - f"pad={W}:{H}:(ow-iw)/2:(oh-ih)/2[v{i}]" - ) - -# xstack layout: pixel positions for each cell -layout_parts = [] -for i in range(total): - col = i % cols - row = i // cols - layout_parts.append(f"{col * W}_{row * H}") - -xstack = "".join(f"[v{i}]" for i in range(total)) -xstack += f"xstack=inputs={total}:layout={'|'.join(layout_parts)}[out]" - -filter_complex = ";".join(scale_parts + [xstack]) - -cmd = ["ffmpeg", "-y"] + inputs + [ - "-filter_complex", filter_complex, - "-map", "[out]", - "-t", str(max_duration), - "-c:v", "libx264", "-crf", "23", "-preset", "fast", - output, -] - -print("[grid-video] Running ffmpeg ...") -result = subprocess.run(cmd) -if result.returncode != 0: - sys.exit(1) -size = os.path.getsize(output) -print(f"[grid-video] Done: {output} ({size/1024/1024:.1f} MB)") -``` - -Copy and run inside Docker: -```bash -docker cp /tmp/build_grid_video.py robotic-grounding-v2d-gpu1:/tmp/build_grid_video.py -docker exec robotic-grounding-v2d-gpu1 /workspace/isaaclab/_isaac_sim/python.sh /tmp/build_grid_video.py -``` - -## Step 5 — Verify and report - -```bash -ls -lh "" -``` - -Print final result: -``` -[grid-video] ✓ Grid video saved: - Grid: x Videos: Duration: - File: -``` diff --git a/robotic_grounding/.dockerignore b/robotic_grounding/.dockerignore deleted file mode 100644 index 304796e5..00000000 --- a/robotic_grounding/.dockerignore +++ /dev/null @@ -1,43 +0,0 @@ -logs/ -run.py -wandb/ -outputs/ - -# Heavy, runtime-only raw dataset downloads — mounted from CSS into -# /tmp/human_motion_data//dataset/ at OSMO workflow runtime, so -# there's no reason to bake the multi-GB raw dumps into the image. -# We intentionally DO NOT exclude the rest of `human_motion_data/` — the -# repo commits small CI test fixtures under -# `arctic/arctic_processed/`, `taco/taco_processed/`, -# `arctic/reconstructed_stage/`, etc., and the e2e training smoke test -# in CI reads from those paths. -# -# Scoping to `**/human_motion_data/*/dataset/` (rather than a blanket -# `**/assets/human_motion_data/`) preserves those fixtures while still -# skipping the raw-download bulk. -**/human_motion_data/*/dataset/ - -# Common development files and directories -.git/ -# .git_info/ is included (contains lightweight git info for reproducibility) -.gitignore -.env -__pycache__/ -*.pyc -*.pyo -*.pyd -.Python -*.so -.mypy_cache/ -.ruff_cache/ -.pytest_cache/ -.pixi/ -build/ -dist/ -*.egg-info/ -.vscode/ -.idea/ - -# Core dumps -core -core.* diff --git a/robotic_grounding/.envrc b/robotic_grounding/.envrc deleted file mode 100644 index 865006ec..00000000 --- a/robotic_grounding/.envrc +++ /dev/null @@ -1,12 +0,0 @@ -# Source CSS/PDX credentials. Sets CSS_ENDPOINT_URL, CSS_ACCESS_KEY, -# CSS_SECRET_KEY, CSS_REGION. Does NOT set HTTPS_PROXY/HTTP_PROXY — those -# break botocore and osmo. Use proxychains4 for routing instead. -source_up_if_exists - -# Local overrides (gitignored) — put per-machine or secret values here. -# e.g. export CSS_SECRET_KEY="your-real-key" -if [ -f "$(pwd)/.envrc.local" ]; then - source "$(pwd)/.envrc.local" -fi - -source "$(pwd)/scripts/setup_css_env.sh" diff --git a/robotic_grounding/.gitattributes b/robotic_grounding/.gitattributes deleted file mode 100644 index 2c2470b3..00000000 --- a/robotic_grounding/.gitattributes +++ /dev/null @@ -1,49 +0,0 @@ -# SCM syntax highlighting & preventing 3-way merges -pixi.lock merge=binary linguist-language=YAML linguist-generated=true -# 3D mesh formats -*.obj filter=lfs diff=lfs merge=lfs -text -*.stl filter=lfs diff=lfs merge=lfs -text -*.STL filter=lfs diff=lfs merge=lfs -text -*.mtl filter=lfs diff=lfs merge=lfs -text -*.dae filter=lfs diff=lfs merge=lfs -text -# Texture images -*.png filter=lfs diff=lfs merge=lfs -text -# Universal Scene Description (USD) -*.usd filter=lfs diff=lfs merge=lfs -text -*.usda filter=lfs diff=lfs merge=lfs -text -*.usdc filter=lfs diff=lfs merge=lfs -text -*.usdz filter=lfs diff=lfs merge=lfs -text -# Motion data -*.h5 filter=lfs diff=lfs merge=lfs -text -*.hdf5 filter=lfs diff=lfs merge=lfs -text -*.parquet filter=lfs diff=lfs merge=lfs -text -# NumPy/Pickle data -*.npy filter=lfs diff=lfs merge=lfs -text -*.npz filter=lfs diff=lfs merge=lfs -text -*.pkl filter=lfs diff=lfs merge=lfs -text -*.pickle filter=lfs diff=lfs merge=lfs -text -# Model weights -*.onnx filter=lfs diff=lfs merge=lfs -text -*.pt filter=lfs diff=lfs merge=lfs -text -*.safetensors filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic_processed filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic_loaded filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco_loaded filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco_processed filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/xr filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/reset_trajectory.pt filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2_loaded/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2_processed/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2/object_raw/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2/object_repair/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/assets/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d_loaded/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d_processed/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/meshes/hot3d/** filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/meshes/hot3d/instance.json !filter !diff !merge text -# Migrated from root .gitattributes (taco support + Isaac Lab .pkg) -source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/**/*.parquet filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/**/*.parquet filter=lfs diff=lfs merge=lfs -text -source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/reconstructed_stage/taco_empty__kettle__plate_20231031_060_support.usda filter=lfs diff=lfs merge=lfs -text -*.pkg filter=lfs diff=lfs merge=lfs -text diff --git a/robotic_grounding/.gitignore b/robotic_grounding/.gitignore deleted file mode 100644 index f72a4c3f..00000000 --- a/robotic_grounding/.gitignore +++ /dev/null @@ -1,287 +0,0 @@ -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[codz] -*$py.class - -.devcontainer/ - -# C extensions -*.so - -# Distribution / packaging -.Python -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -wheels/ -share/python-wheels/ -*.egg-info/ -.installed.cfg -*.egg -MANIFEST - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.nox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -*.py.cover -.hypothesis/ -.pytest_cache/ -cover/ - -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py -db.sqlite3 -db.sqlite3-journal - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ - -# PyBuilder -.pybuilder/ -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# IPython -profile_default/ -ipython_config.py - -# pyenv -# For a library or package, you might want to ignore these files since the code is -# intended to run in multiple environments; otherwise, check them in: -# .python-version - -# pipenv -# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. -# However, in case of collaboration, if having platform-specific dependencies or dependencies -# having no cross-platform support, pipenv may install dependencies that don't work, or not -# install all needed dependencies. -#Pipfile.lock - -# UV -# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control. -# This is especially recommended for binary packages to ensure reproducibility, and is more -# commonly ignored for libraries. -#uv.lock - -# poetry -# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. -# This is especially recommended for binary packages to ensure reproducibility, and is more -# commonly ignored for libraries. -# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control -#poetry.lock -#poetry.toml - -# pdm -# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. -# pdm recommends including project-wide configuration in pdm.toml, but excluding .pdm-python. -# https://pdm-project.org/en/latest/usage/project/#working-with-version-control -#pdm.lock -#pdm.toml -.pdm-python -.pdm-build/ - -# pixi -# Similar to Pipfile.lock, it is generally recommended to include pixi.lock in version control. -#pixi.lock -# Pixi creates a virtual environment in the .pixi directory, just like venv module creates one -# in the .venv directory. It is recommended not to include this directory in version control. -.pixi - -# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm -__pypackages__/ - -# Celery stuff -celerybeat-schedule -celerybeat.pid - -# SageMath parsed files -*.sage.py - -# Environments -.env -.envrc.local -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ -.dmypy.json -dmypy.json - -# Pyre type checker -.pyre/ - -# pytype static type analyzer -.pytype/ - -# Cython debug symbols -cython_debug/ - -# PyCharm -# JetBrains specific template is maintained in a separate JetBrains.gitignore that can -# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore -# and can be added to the global gitignore or merged into this file. For a more nuclear -# option (not recommended) you can uncomment the following to ignore the entire idea folder. -#.idea/ - -# Abstra -# Abstra is an AI-powered process automation framework. -# Ignore directories containing user credentials, local state, and settings. -# Learn more at https://abstra.io/docs -.abstra/ - -# Visual Studio Code -# Visual Studio Code specific template is maintained in a separate VisualStudioCode.gitignore -# that can be found at https://github.com/github/gitignore/blob/main/Global/VisualStudioCode.gitignore -# and can be added to the global gitignore or merged into this file. However, if you prefer, -# you could uncomment the following to ignore the entire vscode folder -# .vscode/ - -# Ruff stuff: -.ruff_cache/ - -# PyPI configuration file -.pypirc - -# Cursor -# Cursor is an AI-powered code editor. `.cursorignore` specifies files/directories to -# exclude from AI features like autocomplete and code analysis. Recommended for sensitive data -# refer to https://docs.cursor.com/context/ignore-files -.cursorignore -.cursorindexingignore - -# Marimo -marimo/_static/ -marimo/_lsp/ -__marimo__/ - -# pixi environments -.pixi/* -!.pixi/config.toml - -# logs -logs/ -outputs/ -wandb/ -.devcontainer/ - -# RL training checkpoints (distinct from wandb_checkpoints/ below) -/checkpoints/ - -# Reconstructed data from the V2D upstream pipeline -/reconstructed_data/ - -.DS_Store - -.vscode/ - -core - -# Claude related files -.claude/ -!.claude/commands/getneval.md -!.claude/commands/eval-record.md -!.claude/commands/grid-video.md -!.claude/commands/launch-two-stage.md -*.code-workspace - -# Temporary usd files -*.tmp.usd - -# Dataset download bookkeeping -.download_status.json - -# videos -videos/ - -# wandb -wandb/ - -# Monitor state -scripts/monitor_state.json -scripts/monitor_state_exp*.json - -experiments/exp*/ -experiments/registry.local.yaml -experiments/*.mp4 -experiments/*/*.mp4 -experiments/eval_recordings/*/*.mp4 -experiments/EXPERIMENT_LOG.md -experiments/*/*.md - -# wandb checkpoints -wandb_checkpoints/ - -exps/ - -# Temporary generated YAML files (e.g. OSMO workflow submissions) -tmp_*.yaml - -# Visualizer datasets (large binary recordings, downloaded via sync_visualizer_data.py) -visualizer/datasets/ - -# Per-dataset object assets (URDFs + meshes) — large/regenerable, not committed. -# Hosted on CSS under /object_assets/; fetch with scripts/fetch_object_assets.py -# (upload with scripts/upload_object_assets.py). Robot meshes (g1, sharpa_wave, -# vega_sharpa, unitree_description) and the scene objects/ stay tracked. -source/robotic_grounding/robotic_grounding/assets/urdfs/arctic/ -source/robotic_grounding/robotic_grounding/assets/urdfs/taco/ -source/robotic_grounding/robotic_grounding/assets/urdfs/oakink2/ -source/robotic_grounding/robotic_grounding/assets/urdfs/hot3d/ -source/robotic_grounding/robotic_grounding/assets/meshes/arctic/ -source/robotic_grounding/robotic_grounding/assets/meshes/taco/ -source/robotic_grounding/robotic_grounding/assets/meshes/oakink2/ -source/robotic_grounding/robotic_grounding/assets/meshes/hot3d/ diff --git a/robotic_grounding/.pre-commit-config.yaml b/robotic_grounding/.pre-commit-config.yaml deleted file mode 100644 index 2ef9f0b7..00000000 --- a/robotic_grounding/.pre-commit-config.yaml +++ /dev/null @@ -1,85 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - -default_stages: [pre-commit] -default_install_hook_types: [pre-commit, pre-push] - -# Hook paths are evaluated from the git root, not from this file's location. -# Pre-commit always runs hooks with cwd = git root and passes git-root-relative -# paths to them, so excludes and `--config` flags need the `robotic_grounding/` -# prefix even though the file lives inside that package. -exclude: | - (?x)^( - robotic_grounding/workflow/.*\.yaml - | robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/.* - )$ - -repos: -# Normalize NVIDIA Python files to the concise 2-line Apache-2.0 SPDX header used -# in reconstruction/. Replaces legacy proprietary + verbose-Apache headers and -# inserts one where missing; third-party (e.g. Isaac Lab) headers are left alone. -- repo: local - hooks: - - id: license-header - name: enforce SPDX Apache-2.0 license header - entry: python robotic_grounding/scripts/license_header_hook.py - language: python - files: ^robotic_grounding/.*\.py$ - exclude: | - (?x)^( - robotic_grounding/scripts/rsl_rl/.* - )$ - -- repo: https://github.com/charliermarsh/ruff-pre-commit - rev: 'v0.14.11' - hooks: - - id: ruff - args: ['--fix', '--config', 'robotic_grounding/pyproject.toml'] - exclude: | - (?x)^( - robotic_grounding/scripts/rsl_rl/.* - )$ - -- repo: https://github.com/psf/black - rev: 25.12.0 - hooks: - - id: black - args: ['--config', 'robotic_grounding/pyproject.toml'] - verbose: true - -- repo: https://github.com/pre-commit/mirrors-mypy - rev: v1.19.1 - hooks: - - id: mypy - additional_dependencies: - - types-pyyaml - - types-requests - - types-setuptools - - types-toml - args: [ - "--ignore-missing-imports", - "--explicit-package-bases", - "--follow-imports=skip", - "--disable-error-code=has-type", - "--config-file=robotic_grounding/pyproject.toml", - ] - -- repo: https://github.com/pre-commit/pre-commit-hooks - rev: v6.0.0 - hooks: - - id: check-yaml - - id: check-json - exclude: ^robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/ - - id: check-xml - - id: check-ast - - id: end-of-file-fixer - - id: pretty-format-json - args: ['--autofix'] - exclude: ^robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/ - - id: detect-private-key - - id: trailing-whitespace diff --git a/robotic_grounding/README.md b/robotic_grounding/README.md index 9be7724e..d8672ef7 100644 --- a/robotic_grounding/README.md +++ b/robotic_grounding/README.md @@ -1,288 +1,16 @@ # Robotic Grounding -## Prerequisites +This is a stage of the **Video to Data (V2D)** pipeline. It turns +reconstructed human demonstrations into deployable robot policies: -- Install [Docker](https://docs.docker.com/engine/install/ubuntu/) and [post-installation](https://docs.docker.com/engine/install/linux-postinstall/#manage-docker-as-a-non-root-user) steps. +- **Motion retargeting** — maps human hand and whole-body motion + onto a target robot embodiment (e.g. Sharpa, G1), across the supported + hand-object and whole-body schemas. +- **RL training** — drives [NVIDIA Isaac Lab](https://isaac-sim.github.io/IsaacLab/) + environments with the retargeted motion and the reconstructed scene, training + control policies with RL. -- Install [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html). +The source for this stage is not included in this release and will be published +in a later release. -- Make sure you have access to `nvcr.io/nvstaging/isaac-amr`. You can request it by asking in the `#swngc-help` Slack channel. - -- Install Git LFS and `pre-commit` dependencies. - ```bash - bash workflow/setup_deps.sh - ``` - This script installs `git-lfs` and `pre-commit` and ensures `workflow/run.sh` is executable. You may need to restart your shell for pipx PATH changes. - -- NVIDIA Driver Version 580.126.09, CUDA Version: 13.0 Recommended. In case of visualization errors, check NVIDIA driver version. - -## Environment & Credentials - -Retrieve your CSS/PDX keys from `~/.config/osmo/config.yaml` under `swift://pdx.s8k.io/AUTH_team-isaac`. - -**Option A — Edit the credentials file directly (simple)** - -Fill in your keys in `scripts/setup_css_env.sh`: - -```bash -export CSS_ACCESS_KEY="" -export CSS_SECRET_KEY="" -``` - -Then source it manually when needed: -```bash -source scripts/setup_css_env.sh -``` - -**Option B — direnv (automatic, recommended)** - -[direnv](https://direnv.net/) auto-loads credentials whenever you `cd` into -`robotic_grounding/`. - -Install: -```bash -sudo apt-get install direnv # Ubuntu/Debian -# then add to ~/.bashrc: -eval "$(direnv hook bash)" -``` - -Create a **gitignored** `.envrc.local` inside this package -(`robotic_grounding/.envrc.local`): -```bash -# robotic_grounding/.envrc.local -export CSS_ACCESS_KEY="" -export CSS_SECRET_KEY="" -``` - -Allow direnv to load it (one-time, per clone): -```bash -cd robotic_grounding && direnv allow -``` - -Credentials are then injected automatically on every `cd` into -`robotic_grounding/`. - -## Docker Usage - -Development should be **inside** the Container, and Git operations should be done **outside** the Container on the host machine. - -```bash -./workflow/run.sh build [version] # Build Docker image and tag it with [version] -./workflow/run.sh push [version] # Push Docker image to NVIDIA registry -./workflow/run.sh pull [version] # Pull Docker image from NVIDIA registry -./workflow/run.sh start [version] [gpu] # Run and enter the Container with specific version and GPU -./workflow/run.sh shell [version] [gpu] # Enter Container from new shell with specific version and GPU -./workflow/run.sh stop [version] [gpu] # Stop the Container with specific version and GPU -``` - -## Development - -You can launch the container with commands in the Docker Usage section. - -If using VSCode or Cursor, you can use the `Attach to Running Container` feature in Dev Containers extension by `command/ctrl + shift + p`. Inside the container, you can use Python interpreter `/workspace/isaaclab/_isaac_sim/python.sh` for debugging. The working directory is `/workspace/video_to_data/robotic_grounding`. - -Currently, due to Isaac Lab's image requiring root for Omniverse, we are using the root user for the container. There can be some permission issues, but they can be bypassed with `sudo chown -R $(whoami) .` in the host machine. - -For agent-oriented checks, use this quick path before opening a merge request. Commands assume the `robotic_grounding/` package root, and Isaac commands should run inside the container. OSMO and W&B are not required for the local smoke tests below. - -## Agent Smoke Tests - -### Assets and dummy agent - -Motion data resolves under `source/robotic_grounding/robotic_grounding/assets/human_motion_data/`. The safest local shorthand is `/_processed//sharpa_wave`, for example `arctic/arctic_processed/arctic_s01_box_grab_01/sharpa_wave`. - -Pull retargeted outputs when they are missing: - -```bash -osmo dataset info v2d_arctic_retarget_exp_200 --order desc -osmo dataset download v2d_arctic_retarget_exp_200: \ - source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/ \ - --regex '(arctic_processed|arctic_urdfs|reconstructed_stage)/.*' -``` - -If OSMO is not configured, do not attempt the dataset download. Use any already -present local motion partition, or ask for the dataset version/path needed for -the task. OSMO setup lives in [workflow/README.md](workflow/README.md). - -Stages that load real object geometry — retargeting, kinematic replay, support-surface -reconstruction, scene view, and training — need them under -`assets/{meshes,urdfs}//` first: download the dataset's object models -from its original distribution, copy the meshes into `assets/meshes//`, -and run `python scripts/generate_rigid_urdfs.py --dataset `. See -[workflow/data_pipeline.md](workflow/data_pipeline.md#object-assets-urdfs--meshes). - - -NVIDIA-internal: pull the prebuilt assets from CSS instead — -`python scripts/fetch_object_assets.py --dataset arctic` (or `--dataset all`). - - -(The `--use_primitive_urdfs` dummy-agent smoke test above does not need them.) - -Run a GUI dummy-agent smoke test inside the container: - -```bash -python scripts/rsl_rl/dummy_agent.py \ - --task Sharpa-V2P-v0-Play \ - --motion_file arctic/arctic_processed/arctic_s01_box_grab_01/sharpa_wave \ - --num_envs 1 \ - --use_primitive_urdfs -``` - -Run the same check headless with a short MP4: - -```bash -python scripts/rsl_rl/dummy_agent.py \ - --headless \ - --task Sharpa-V2P-v0-Play \ - --motion_file arctic/arctic_processed/arctic_s01_box_grab_01/sharpa_wave \ - --num_envs 1 \ - --use_primitive_urdfs \ - --record_video \ - --output_dir /tmp/rg_dummy_agent_video \ - --video_length 300 -``` - -Success means Isaac starts, the task registers, `SceneConfig.from_motion_file` loads the parquet partition, no missing-asset exception is raised, and the simulation advances. - -### Training and OSMO - -Use the `RL training` section below for a local `train.py` one-iteration smoke test. If W&B is not configured, keep local smoke tests on TensorBoard by passing `--logger tensorboard`. Use [workflow/README.md](workflow/README.md) for OSMO image setup and cloud submission prerequisites. - -### Running Debugging Env - -The debug environment (`Sharpa-V2P-Debug-v0`) provides interactive GUI controls for testing contact sensors and MDP components: - -- **Joint GUI Control**: Adjust all robot joints (wrist 6DoF + fingers) with P/D gain sliders -- **Object Pose GUI**: Move and rotate the object in 6DoF via floating base controls -- **Reward Visualizer**: Monitor reward terms in real-time with history plots - -To run the debug environment: -```bash -python scripts/rsl_rl/dummy_agent.py --task Sharpa-V2P-Debug-v0 -``` - -**Note**: Do not use `--headless` flag since the GUI controls require a display. The environment is configured with extended episode length (1 hour) and disabled randomization events for uninterrupted manual testing. - -To create a debug environment for other tasks, use `sharpa_debug_env_cfg.py` as a template—inherit from your base environment config and override the actions with GUI-controlled versions (`JointGUIActionCfg`, `ObjectPoseGUIActionCfg`, `RewardVisualizerCfg`). Remember to disable action related MDP terms since the debug env does not have the original actions. - -## Retargeting - -### Hand-only (Sharpa / Arctic) -```bash -# Stage 1 (Load) lives in the reconstruction repo (GPL MANO FK); it produces the -# {dataset}_loaded Parquet that the retarget step below consumes: -# python -m v2d.task_library_loader.lib.run_loader --dataset arctic --dataset_root --mano_model_dir --output_dir --save -python scripts/retarget/arctic_to_sharpa.py --save # Check the file for arguments -python scripts/retarget/vis_retargeted.py # Need to run retargeting and save results first -``` - -For Arctic motion files, download them from [here](https://drive.google.com/file/d/1rL4T9N4AwQoWRqS5pOuB6a0D0B9Y8dsP/view?usp=sharing) and extract to `source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic`. (TODO: We need to figure out an appropriate way for data storage) - -### Whole-body (SOMA → G1) -```bash -# Retarget and save Parquet (data_folder must contain soma_params.npz, object/textured_mesh.obj) -python scripts/retarget/soma_to_g1.py --save - -# Visualize retargeting in Viser (port 8080) -python scripts/retarget/soma_to_g1.py --visualize -``` - -### Kinematic replay (all schemas) -Replay retargeted motion in Isaac Lab. Supports both whole-body (G1) and dual floating-hand (Sharpa/Dex3) data. -Robot and object are teleported kinematically — no physics forces act on them. -```bash -# Replay G1 retargeted data (loops by default) -python scripts/replay_motion.py \ - --motion_file source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=/robot_name=g1 - -# Replay hand-only data -python scripts/replay_motion.py \ - --motion_file source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic_processed/sequence_id=/robot_name=sharpa_wave - -# Options -python scripts/replay_motion.py --motion_file --speed 0.5 # Slow motion -python scripts/replay_motion.py --motion_file --no-loop # Stop at last frame -python scripts/replay_motion.py --motion_file --headless # No GUI -``` - -### Support surface reconstruction -Detect where objects rest on surfaces above the ground plane and generate collision geometry for RL training. -```bash -# For hand-only datasets (auto-detects schema) -python scripts/reconstruct_support_surfaces.py --input_dir --sequence_id - -# For G1 whole-body retargeted data -python scripts/reconstruct_support_surfaces.py --input_dir source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma --sequence_id - -# Or use the dataset shortcut -python scripts/reconstruct_support_surfaces.py --dataset soma_g1 --sequence_id -``` - -Objects resting on the ground are automatically filtered out (threshold configurable via `--ground_threshold`). - -### Scene viewer (static spawn verification) -```bash -python scripts/view_scene.py --motion_file -``` - -## RL training - -Commands in this section assume you are inside the container from the -`robotic_grounding/` package root. These commands do not require W&B or OSMO -when the motion data is already present locally. - -```bash -# Run a real one-iteration train smoke test. -python scripts/rsl_rl/train.py \ - --headless \ - --task Sharpa-V2P-v0 \ - --motion_file arctic/arctic_processed/arctic_s01_box_grab_01/sharpa_wave \ - --num_envs 1 \ - --max_iterations 1 \ - --logger tensorboard \ - --run_name smoke_train \ - --use_primitive_urdfs \ - agent.num_steps_per_env=8 \ - agent.save_interval=1 - -# Evaluate the checkpoint produced by the smoke train. -CHECKPOINT=$(find logs/rsl_rl -path '*smoke_train*/model_*.pt' | sort -V | tail -1) -python scripts/rsl_rl/eval.py \ - --headless \ - --task Sharpa-V2P-v0 \ - --motion_file arctic/arctic_processed/arctic_s01_box_grab_01/sharpa_wave \ - --num_envs 1 \ - --checkpoint "$CHECKPOINT" \ - --eval_episodes 1 \ - --use_primitive_urdfs - -# Other entry points. -python scripts/rsl_rl/dummy_agent.py # Run an environment with zero actions. -python scripts/rsl_rl/eval.py # Evaluate a trained checkpoint and export policy. -python scripts/rsl_rl/play.py # Play without a checkpoint. -``` - -See the `Agent Smoke Tests` section above for the required asset layout, dummy-agent commands, and OSMO setup guidance. - -## RL Tasks -- `Sharpa-V2P-v0-Play` -- `Sharpa-V2P-v0` - -## Visualizer - -Browse retargeted sequences as 3D animations at **http://10.111.83.14:8080/** - -To run the server yourself or generate new recordings: - -```bash -# Download datasets from OSMO -python visualizer/sync_visualizer_data.py - -# Start the gallery server -python visualizer/serve.py # → http://0.0.0.0:8080 - -# Serve vis_retargeted.py output directly (no copy needed) -python visualizer/serve.py --html-dir /path/to/v2d_arctic_retarget_exp_200 -``` - -See [visualizer/README.md](visualizer/README.md) for the full reference: parallel downloads, generating `.viser` files inside Docker, and running as a systemd service. +The technical report and project website are available on the project page: [Webpage](https://nvidia-isaac.github.io/video_to_data/chord/). \ No newline at end of file diff --git a/robotic_grounding/ci/assets/taco/meshes/005_cm.obj b/robotic_grounding/ci/assets/taco/meshes/005_cm.obj deleted file mode 100644 index 19bc7dbb..00000000 --- a/robotic_grounding/ci/assets/taco/meshes/005_cm.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b807e58e5ed593638c52928bf657baa7e908181ebd2c31c4610c161e714a5347 -size 7288822 diff --git a/robotic_grounding/ci/assets/taco/meshes/030_cm.obj b/robotic_grounding/ci/assets/taco/meshes/030_cm.obj deleted file mode 100644 index d9028586..00000000 --- a/robotic_grounding/ci/assets/taco/meshes/030_cm.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cb5ac367c19678cdf14a2fe70f8df32cadb3a80baa357725c39b8b7da1b8e8ce -size 7282285 diff --git a/robotic_grounding/ci/assets/taco/meshes/LICENSE b/robotic_grounding/ci/assets/taco/meshes/LICENSE deleted file mode 100644 index 4ea99c21..00000000 --- a/robotic_grounding/ci/assets/taco/meshes/LICENSE +++ /dev/null @@ -1,395 +0,0 @@ -Attribution 4.0 International - -======================================================================= - -Creative Commons Corporation ("Creative Commons") is not a law firm and -does not provide legal services or legal advice. Distribution of -Creative Commons public licenses does not create a lawyer-client or -other relationship. Creative Commons makes its licenses and related -information available on an "as-is" basis. Creative Commons gives no -warranties regarding its licenses, any material licensed under their -terms and conditions, or any related information. Creative Commons -disclaims all liability for damages resulting from their use to the -fullest extent possible. - -Using Creative Commons Public Licenses - -Creative Commons public licenses provide a standard set of terms and -conditions that creators and other rights holders may use to share -original works of authorship and other material subject to copyright -and certain other rights specified in the public license below. The -following considerations are for informational purposes only, are not -exhaustive, and do not form part of our licenses. - - Considerations for licensors: Our public licenses are - intended for use by those authorized to give the public - permission to use material in ways otherwise restricted by - copyright and certain other rights. Our licenses are - irrevocable. Licensors should read and understand the terms - and conditions of the license they choose before applying it. - Licensors should also secure all rights necessary before - applying our licenses so that the public can reuse the - material as expected. Licensors should clearly mark any - material not subject to the license. This includes other CC- - licensed material, or material used under an exception or - limitation to copyright. More considerations for licensors: - wiki.creativecommons.org/Considerations_for_licensors - - Considerations for the public: By using one of our public - licenses, a licensor grants the public permission to use the - licensed material under specified terms and conditions. If - the licensor's permission is not necessary for any reason--for - example, because of any applicable exception or limitation to - copyright--then that use is not regulated by the license. Our - licenses grant only permissions under copyright and certain - other rights that a licensor has authority to grant. Use of - the licensed material may still be restricted for other - reasons, including because others have copyright or other - rights in the material. A licensor may make special requests, - such as asking that all changes be marked or described. - Although not required by our licenses, you are encouraged to - respect those requests where reasonable. More considerations - for the public: - wiki.creativecommons.org/Considerations_for_licensees - -======================================================================= - -Creative Commons Attribution 4.0 International Public License - -By exercising the Licensed Rights (defined below), You accept and agree -to be bound by the terms and conditions of this Creative Commons -Attribution 4.0 International Public License ("Public License"). To the -extent this Public License may be interpreted as a contract, You are -granted the Licensed Rights in consideration of Your acceptance of -these terms and conditions, and the Licensor grants You such rights in -consideration of benefits the Licensor receives from making the -Licensed Material available under these terms and conditions. - - -Section 1 -- Definitions. - - a. Adapted Material means material subject to Copyright and Similar - Rights that is derived from or based upon the Licensed Material - and in which the Licensed Material is translated, altered, - arranged, transformed, or otherwise modified in a manner requiring - permission under the Copyright and Similar Rights held by the - Licensor. For purposes of this Public License, where the Licensed - Material is a musical work, performance, or sound recording, - Adapted Material is always produced where the Licensed Material is - synched in timed relation with a moving image. - - b. Adapter's License means the license You apply to Your Copyright - and Similar Rights in Your contributions to Adapted Material in - accordance with the terms and conditions of this Public License. - - c. Copyright and Similar Rights means copyright and/or similar rights - closely related to copyright including, without limitation, - performance, broadcast, sound recording, and Sui Generis Database - Rights, without regard to how the rights are labeled or - categorized. For purposes of this Public License, the rights - specified in Section 2(b)(1)-(2) are not Copyright and Similar - Rights. - - d. Effective Technological Measures means those measures that, in the - absence of proper authority, may not be circumvented under laws - fulfilling obligations under Article 11 of the WIPO Copyright - Treaty adopted on December 20, 1996, and/or similar international - agreements. - - e. Exceptions and Limitations means fair use, fair dealing, and/or - any other exception or limitation to Copyright and Similar Rights - that applies to Your use of the Licensed Material. - - f. Licensed Material means the artistic or literary work, database, - or other material to which the Licensor applied this Public - License. - - g. Licensed Rights means the rights granted to You subject to the - terms and conditions of this Public License, which are limited to - all Copyright and Similar Rights that apply to Your use of the - Licensed Material and that the Licensor has authority to license. - - h. Licensor means the individual(s) or entity(ies) granting rights - under this Public License. - - i. Share means to provide material to the public by any means or - process that requires permission under the Licensed Rights, such - as reproduction, public display, public performance, distribution, - dissemination, communication, or importation, and to make material - available to the public including in ways that members of the - public may access the material from a place and at a time - individually chosen by them. - - j. Sui Generis Database Rights means rights other than copyright - resulting from Directive 96/9/EC of the European Parliament and of - the Council of 11 March 1996 on the legal protection of databases, - as amended and/or succeeded, as well as other essentially - equivalent rights anywhere in the world. - - k. You means the individual or entity exercising the Licensed Rights - under this Public License. Your has a corresponding meaning. - - -Section 2 -- Scope. - - a. License grant. - - 1. Subject to the terms and conditions of this Public License, - the Licensor hereby grants You a worldwide, royalty-free, - non-sublicensable, non-exclusive, irrevocable license to - exercise the Licensed Rights in the Licensed Material to: - - a. reproduce and Share the Licensed Material, in whole or - in part; and - - b. produce, reproduce, and Share Adapted Material. - - 2. Exceptions and Limitations. For the avoidance of doubt, where - Exceptions and Limitations apply to Your use, this Public - License does not apply, and You do not need to comply with - its terms and conditions. - - 3. Term. The term of this Public License is specified in Section - 6(a). - - 4. Media and formats; technical modifications allowed. The - Licensor authorizes You to exercise the Licensed Rights in - all media and formats whether now known or hereafter created, - and to make technical modifications necessary to do so. The - Licensor waives and/or agrees not to assert any right or - authority to forbid You from making technical modifications - necessary to exercise the Licensed Rights, including - technical modifications necessary to circumvent Effective - Technological Measures. For purposes of this Public License, - simply making modifications authorized by this Section 2(a) - (4) never produces Adapted Material. - - 5. Downstream recipients. - - a. Offer from the Licensor -- Licensed Material. Every - recipient of the Licensed Material automatically - receives an offer from the Licensor to exercise the - Licensed Rights under the terms and conditions of this - Public License. - - b. No downstream restrictions. You may not offer or impose - any additional or different terms or conditions on, or - apply any Effective Technological Measures to, the - Licensed Material if doing so restricts exercise of the - Licensed Rights by any recipient of the Licensed - Material. - - 6. No endorsement. Nothing in this Public License constitutes or - may be construed as permission to assert or imply that You - are, or that Your use of the Licensed Material is, connected - with, or sponsored, endorsed, or granted official status by, - the Licensor or others designated to receive attribution as - provided in Section 3(a)(1)(A)(i). - - b. Other rights. - - 1. Moral rights, such as the right of integrity, are not - licensed under this Public License, nor are publicity, - privacy, and/or other similar personality rights; however, to - the extent possible, the Licensor waives and/or agrees not to - assert any such rights held by the Licensor to the limited - extent necessary to allow You to exercise the Licensed - Rights, but not otherwise. - - 2. Patent and trademark rights are not licensed under this - Public License. - - 3. To the extent possible, the Licensor waives any right to - collect royalties from You for the exercise of the Licensed - Rights, whether directly or through a collecting society - under any voluntary or waivable statutory or compulsory - licensing scheme. In all other cases the Licensor expressly - reserves any right to collect such royalties. - - -Section 3 -- License Conditions. - -Your exercise of the Licensed Rights is expressly made subject to the -following conditions. - - a. Attribution. - - 1. If You Share the Licensed Material (including in modified - form), You must: - - a. retain the following if it is supplied by the Licensor - with the Licensed Material: - - i. identification of the creator(s) of the Licensed - Material and any others designated to receive - attribution, in any reasonable manner requested by - the Licensor (including by pseudonym if - designated); - - ii. a copyright notice; - - iii. a notice that refers to this Public License; - - iv. a notice that refers to the disclaimer of - warranties; - - v. a URI or hyperlink to the Licensed Material to the - extent reasonably practicable; - - b. indicate if You modified the Licensed Material and - retain an indication of any previous modifications; and - - c. indicate the Licensed Material is licensed under this - Public License, and include the text of, or the URI or - hyperlink to, this Public License. - - 2. You may satisfy the conditions in Section 3(a)(1) in any - reasonable manner based on the medium, means, and context in - which You Share the Licensed Material. For example, it may be - reasonable to satisfy the conditions by providing a URI or - hyperlink to a resource that includes the required - information. - - 3. If requested by the Licensor, You must remove any of the - information required by Section 3(a)(1)(A) to the extent - reasonably practicable. - - 4. If You Share Adapted Material You produce, the Adapter's - License You apply must not prevent recipients of the Adapted - Material from complying with this Public License. - - -Section 4 -- Sui Generis Database Rights. - -Where the Licensed Rights include Sui Generis Database Rights that -apply to Your use of the Licensed Material: - - a. for the avoidance of doubt, Section 2(a)(1) grants You the right - to extract, reuse, reproduce, and Share all or a substantial - portion of the contents of the database; - - b. if You include all or a substantial portion of the database - contents in a database in which You have Sui Generis Database - Rights, then the database in which You have Sui Generis Database - Rights (but not its individual contents) is Adapted Material; and - - c. You must comply with the conditions in Section 3(a) if You Share - all or a substantial portion of the contents of the database. - -For the avoidance of doubt, this Section 4 supplements and does not -replace Your obligations under this Public License where the Licensed -Rights include other Copyright and Similar Rights. - - -Section 5 -- Disclaimer of Warranties and Limitation of Liability. - - a. UNLESS OTHERWISE SEPARATELY UNDERTAKEN BY THE LICENSOR, TO THE - EXTENT POSSIBLE, THE LICENSOR OFFERS THE LICENSED MATERIAL AS-IS - AND AS-AVAILABLE, AND MAKES NO REPRESENTATIONS OR WARRANTIES OF - ANY KIND CONCERNING THE LICENSED MATERIAL, WHETHER EXPRESS, - IMPLIED, STATUTORY, OR OTHER. THIS INCLUDES, WITHOUT LIMITATION, - WARRANTIES OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR - PURPOSE, NON-INFRINGEMENT, ABSENCE OF LATENT OR OTHER DEFECTS, - ACCURACY, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT - KNOWN OR DISCOVERABLE. WHERE DISCLAIMERS OF WARRANTIES ARE NOT - ALLOWED IN FULL OR IN PART, THIS DISCLAIMER MAY NOT APPLY TO YOU. - - b. TO THE EXTENT POSSIBLE, IN NO EVENT WILL THE LICENSOR BE LIABLE - TO YOU ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION, - NEGLIGENCE) OR OTHERWISE FOR ANY DIRECT, SPECIAL, INDIRECT, - INCIDENTAL, CONSEQUENTIAL, PUNITIVE, EXEMPLARY, OR OTHER LOSSES, - COSTS, EXPENSES, OR DAMAGES ARISING OUT OF THIS PUBLIC LICENSE OR - USE OF THE LICENSED MATERIAL, EVEN IF THE LICENSOR HAS BEEN - ADVISED OF THE POSSIBILITY OF SUCH LOSSES, COSTS, EXPENSES, OR - DAMAGES. WHERE A LIMITATION OF LIABILITY IS NOT ALLOWED IN FULL OR - IN PART, THIS LIMITATION MAY NOT APPLY TO YOU. - - c. The disclaimer of warranties and limitation of liability provided - above shall be interpreted in a manner that, to the extent - possible, most closely approximates an absolute disclaimer and - waiver of all liability. - - -Section 6 -- Term and Termination. - - a. This Public License applies for the term of the Copyright and - Similar Rights licensed here. However, if You fail to comply with - this Public License, then Your rights under this Public License - terminate automatically. - - b. Where Your right to use the Licensed Material has terminated under - Section 6(a), it reinstates: - - 1. automatically as of the date the violation is cured, provided - it is cured within 30 days of Your discovery of the - violation; or - - 2. upon express reinstatement by the Licensor. - - For the avoidance of doubt, this Section 6(b) does not affect any - right the Licensor may have to seek remedies for Your violations - of this Public License. - - c. For the avoidance of doubt, the Licensor may also offer the - Licensed Material under separate terms or conditions or stop - distributing the Licensed Material at any time; however, doing so - will not terminate this Public License. - - d. Sections 1, 5, 6, 7, and 8 survive termination of this Public - License. - - -Section 7 -- Other Terms and Conditions. - - a. The Licensor shall not be bound by any additional or different - terms or conditions communicated by You unless expressly agreed. - - b. Any arrangements, understandings, or agreements regarding the - Licensed Material not stated herein are separate from and - independent of the terms and conditions of this Public License. - - -Section 8 -- Interpretation. - - a. For the avoidance of doubt, this Public License does not, and - shall not be interpreted to, reduce, limit, restrict, or impose - conditions on any use of the Licensed Material that could lawfully - be made without permission under this Public License. - - b. To the extent possible, if any provision of this Public License is - deemed unenforceable, it shall be automatically reformed to the - minimum extent necessary to make it enforceable. If the provision - cannot be reformed, it shall be severed from this Public License - without affecting the enforceability of the remaining terms and - conditions. - - c. No term or condition of this Public License will be waived and no - failure to comply consented to unless expressly agreed to by the - Licensor. - - d. Nothing in this Public License constitutes or may be interpreted - as a limitation upon, or waiver of, any privileges and immunities - that apply to the Licensor or You, including from the legal - processes of any jurisdiction or authority. - - -======================================================================= - -Creative Commons is not a party to its public -licenses. Notwithstanding, Creative Commons may elect to apply one of -its public licenses to material it publishes and in those instances -will be considered the “Licensor.” The text of the Creative Commons -public licenses is dedicated to the public domain under the CC0 Public -Domain Dedication. Except for the limited purpose of indicating that -material is shared under a Creative Commons public license or as -otherwise permitted by the Creative Commons policies published at -creativecommons.org/policies, Creative Commons does not authorize the -use of the trademark "Creative Commons" or any other trademark or logo -of Creative Commons without its prior written consent including, -without limitation, in connection with any unauthorized modifications -to any of its public licenses or any other arrangements, -understandings, or agreements concerning use of licensed material. For -the avoidance of doubt, this paragraph does not form part of the -public licenses. - -Creative Commons may be contacted at creativecommons.org. diff --git a/robotic_grounding/ci/assets/taco/meshes/NOTICE b/robotic_grounding/ci/assets/taco/meshes/NOTICE deleted file mode 100644 index 59a772c5..00000000 --- a/robotic_grounding/ci/assets/taco/meshes/NOTICE +++ /dev/null @@ -1,14 +0,0 @@ -This directory contains third-party object models, redistributed unmodified -(other than OBJ export and centimetre scaling — the `_cm` filename suffix) as -fixtures for the retarget end-to-end test (tests/test_retarget_pipeline_e2e.py). -They are embedded into the Docker image (workflow/Dockerfile) which is used by the -CI pipeline. - -TACO object models © Yun Liu et al., licensed under CC BY 4.0 -(https://creativecommons.org/licenses/by/4.0/). -Source: https://github.com/leolyliu/TACO-Instructions -Liu et al., "TACO: Benchmarking Generalizable Bimanual Tool-Action-Object -Understanding", CVPR 2024. - -The full text of the CC BY 4.0 license is in the LICENSE file alongside this -notice. diff --git a/robotic_grounding/experiments/README.md b/robotic_grounding/experiments/README.md deleted file mode 100644 index d89120b1..00000000 --- a/robotic_grounding/experiments/README.md +++ /dev/null @@ -1,58 +0,0 @@ -# Experiments - -This directory keeps the committed experiment runner, shared helpers, and a small -set of checked-in configs. Private experiments and run outputs should stay local. - -Run commands from the repository root: - -```bash -python robotic_grounding/experiments/run_experiment.py --list -python robotic_grounding/experiments/run_experiment.py example_single --local --dry-run -python robotic_grounding/experiments/run_experiment.py example_single --osmo -``` - -## Tracked Files - -| Path | Purpose | -|------|---------| -| `run_experiment.py` | Main entry point for local training and OSMO submission. | -| `utils.py` | Shared command and workflow-generation helpers. | -| `registry.yaml` | Committed experiment id to directory mapping. | -| `example_single_run/config.yaml` | Minimal single-sequence smoke example. | -| `recon_body_*/config.yaml` | Whole-body reconstruction example configs currently kept in the repo. | - -## Registered Configs - -`registry.yaml` currently exposes these committed ids: - -| Id | Directory | -|----|-----------| -| `example_single` | `example_single_run` | -| `recon_body_apple` | `recon_body_apple_pick` | -| `recon_body_blue_trash_can_drag_007` | `recon_body_blue_trash_can_drag_007` | -| `recon_body_bottle_pick_transfer` | `recon_body_bottle_pick_transfer` | -| `recon_body_corn_can_right_left_handover_01` | `recon_body_corn_can_right_left_handover_01` | -| `recon_body_snack_box_pick_and_place_01` | `recon_body_snack_box_pick_and_place_01` | - -The old committed multi-run example directories are intentionally not present on this branch. - -## Adding Local Experiments - -Create local experiment directories under `experiments/`, then register them in -`experiments/registry.local.yaml`. That file is gitignored and is merged with -`registry.yaml` at runtime, so local entries do not require tracked changes. - -```yaml -# robotic_grounding/experiments/registry.local.yaml -my_experiment: my_experiment_dir -``` - -Then run: - -```bash -python robotic_grounding/experiments/run_experiment.py my_experiment --local --dry-run -``` - -Use `example_single_run/config.yaml` as the starting template for a new config. -For private sweeps or multi-task experiments, add a local `workflow.py` next to -the config and keep the directory untracked. diff --git a/robotic_grounding/experiments/__init__.py b/robotic_grounding/experiments/__init__.py deleted file mode 100644 index 6045fc0f..00000000 --- a/robotic_grounding/experiments/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# Experiments package diff --git a/robotic_grounding/experiments/example_single_run/config.yaml b/robotic_grounding/experiments/example_single_run/config.yaml deleted file mode 100644 index f3c81b50..00000000 --- a/robotic_grounding/experiments/example_single_run/config.yaml +++ /dev/null @@ -1,29 +0,0 @@ -# Example: Single-sequence training run -# -# A minimal experiment that trains on one sequence, locally or on OSMO. -# -# Usage: -# python robotic_grounding/scripts/run_experiment.py example_single --local -# python robotic_grounding/scripts/run_experiment.py example_single --osmo -id: example_single_run -description: "Example: single-sequence training run" -run_name: example_single_run -video: true -motion_file: arctic_processed/arctic_s01_capsulemachine_grab_01/sharpa_wave - -train_overrides: - env.rewards.action_rate_l2.weight: "-5e-4" - env.rewards.action_l1.weight: "-5e-3" - env.rewards.object_keypoints_tracking_exp.weight: "2.0" - env.rewards.hand_keypoints_tracking_exp.weight: "1.0" - env.rewards.contact_force_reward.weight: "1.0" - # No virtual object curriculum — train from scratch - env.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale: "0.0" - env.commands.dual_hands_object_tracking_command.virtual_object_control_decay_mode: "linear" - env.curriculum.virtual_object_control_curriculum.params.decay_mode: "linear" - env.curriculum.virtual_object_control_curriculum.params.linear_decay_step: "0.1" - env.curriculum.virtual_object_control_curriculum.params.wait_env_steps_since_last_decay: "6000" - env.curriculum.virtual_object_control_curriculum.params.zero_scale_factor_threshold: "0.01" - -osmo: - build_image: false diff --git a/robotic_grounding/experiments/experiment_config.yaml b/robotic_grounding/experiments/experiment_config.yaml deleted file mode 100644 index d7c4c7fa..00000000 --- a/robotic_grounding/experiments/experiment_config.yaml +++ /dev/null @@ -1,30 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# TODO(public-release): Remove this file prior to release. - -wandb_entity: nvidia-isaac - -osmo: - default_pool: isaac-dev-l40s-04 - runner_default_pool: isaac-dev-l40-03 - image_repo: nvcr.io/nvstaging/isaac-amr/robotic-grounding - image_latest: nvcr.io/nvstaging/isaac-amr/robotic-grounding:latest - omni_server: omniverse://isaac-dev.ov.nvidia.com - -experiments: - recon_body_apple: - osmo: - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:recon_body_apple - recon_body_bottle_pick_transfer: - osmo: - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:recon_body_apple - recon_body_snack_box_pick_and_place_01: - osmo: - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:recon_body_apple - recon_body_blue_trash_can_drag_007: - osmo: - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:recon_body_apple - recon_body_corn_can_right_left_handover_01: - osmo: - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:recon_body_apple diff --git a/robotic_grounding/experiments/recon_body_apple_pick/config.yaml b/robotic_grounding/experiments/recon_body_apple_pick/config.yaml deleted file mode 100644 index 8958d373..00000000 --- a/robotic_grounding/experiments/recon_body_apple_pick/config.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# ReconBody: Apple pick-and-place with whole-body tracking -# -# Usage: -# python robotic_grounding/scripts/run_experiment.py recon_body_apple --local -id: recon_body_apple_pick -description: "ReconBody apple pick-and-place with SONIC JOINT_RESIDUAL" -run_name: recon_body_apple_pick -task: SonicG1-ReconBody-v0 -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/robot_name=g1 -video: true -num_envs: 4096 -max_iterations: 20000 -zero_actor: true -logger: wandb -log_project_name: v2d-whole-body-tpv - -train_overrides: - env.commands.motion.reset_freeze_steps: 50 - env.commands.motion.voc_decay_steps: 10 - env.commands.motion.voc_reset_scale: 1.0 - env.commands.motion.initial_virtual_object_control_curriculum_scale: 0.0 - env.episode_length_s: 5.0 - -osmo: - build_image: false diff --git a/robotic_grounding/experiments/recon_body_blue_trash_can_drag_007/config.yaml b/robotic_grounding/experiments/recon_body_blue_trash_can_drag_007/config.yaml deleted file mode 100644 index 0a620f44..00000000 --- a/robotic_grounding/experiments/recon_body_blue_trash_can_drag_007/config.yaml +++ /dev/null @@ -1,35 +0,0 @@ -# ReconBody: Blue trash can dragging with whole-body tracking -# -# Usage: -# python robotic_grounding/experiments/run_experiment.py recon_body_blue_trash_can_drag_007 --local -id: recon_body_blue_trash_can_drag_007 -description: "ReconBody blue trash can dragging with SONIC JOINT_RESIDUAL" -run_name: recon_body_blue_trash_can_drag_007 -task: SonicG1-ReconBody-v0 -# Use the explicit repo-relative partition path because whole_body/soma does not -# follow the 4-part dataset/dataset_retargeted/sequence_id/robot shorthand. -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/robot_name=g1 -video: true -num_envs: 4096 -max_iterations: 20000 -zero_actor: true -logger: wandb -log_project_name: v2d-whole-body-tpv - -train_overrides: - env.commands.motion.reset_freeze_steps: 50 - env.commands.motion.voc_decay_steps: 10 - env.commands.motion.voc_reset_scale: 1.0 - env.commands.motion.initial_virtual_object_control_curriculum_scale: 1.0 - env.episode_length_s: 5.0 - - # Frame range of the source motion to train on. -1 for motion_end_frame - # means run to the end of the sequence. - # Use replay_motion_viser.py to visualize the motion and get the frame range. - env.commands.motion.motion_start_frame: 50 - env.commands.motion.motion_end_frame: 400 - # Reset the shoulder spread to 0.2 - env.commands.motion.reset_shoulder_spread: 0.2 - -osmo: - build_image: false diff --git a/robotic_grounding/experiments/recon_body_bottle_pick_transfer/config.yaml b/robotic_grounding/experiments/recon_body_bottle_pick_transfer/config.yaml deleted file mode 100644 index cec92000..00000000 --- a/robotic_grounding/experiments/recon_body_bottle_pick_transfer/config.yaml +++ /dev/null @@ -1,25 +0,0 @@ -# ReconBody: Bottle pick-and-transfer with whole-body tracking -# -# Usage: -# python robotic_grounding/experiments/run_experiment.py recon_body_bottle_pick_transfer --local -id: recon_body_bottle_pick_transfer -description: "ReconBody bottle pick-and-transfer with SONIC JOINT_RESIDUAL" -run_name: recon_body_bottle_pick_transfer -task: SonicG1-ReconBody-v0 -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/robot_name=g1 -video: true -num_envs: 4096 -max_iterations: 20000 -zero_actor: true -logger: wandb -log_project_name: v2d-whole-body-tpv - -train_overrides: - env.commands.motion.reset_freeze_steps: 50 - env.commands.motion.voc_decay_steps: 10 - env.commands.motion.voc_reset_scale: 1.0 - env.commands.motion.initial_virtual_object_control_curriculum_scale: 1.0 - env.episode_length_s: 5.0 - -osmo: - build_image: false diff --git a/robotic_grounding/experiments/recon_body_corn_can_right_left_handover_01/config.yaml b/robotic_grounding/experiments/recon_body_corn_can_right_left_handover_01/config.yaml deleted file mode 100644 index 2009459f..00000000 --- a/robotic_grounding/experiments/recon_body_corn_can_right_left_handover_01/config.yaml +++ /dev/null @@ -1,26 +0,0 @@ -# ReconBody: recon_body_corn_can_right_left_handover_01 (TPV whole-body) -# -# Usage: -# python robotic_grounding/experiments/run_experiment.py recon_body_corn_can_right_left_handover_01 --local -id: recon_body_corn_can_right_left_handover_01 -description: ReconBody recon_body_corn_can_right_left_handover_01 with SONIC JOINT_RESIDUAL -run_name: recon_body_corn_can_right_left_handover_01 -task: SonicG1-ReconBody-v0 -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/robot_name=g1 -video: true -num_envs: 4096 -max_iterations: 20000 -zero_actor: true -logger: wandb -log_project_name: v2d-whole-body-tpv -train_overrides: - env.commands.motion.reset_freeze_steps: 50 - env.commands.motion.voc_decay_steps: 10 - env.commands.motion.voc_reset_scale: 1.0 - env.commands.motion.initial_virtual_object_control_curriculum_scale: 1.0 - env.commands.motion.motion_start_frame: 630 - env.commands.motion.motion_end_frame: -1 - env.episode_length_s: 5.0 - env.commands.motion.reset_shoulder_spread: 0.2 -osmo: - build_image: false diff --git a/robotic_grounding/experiments/recon_body_snack_box_pick_and_place_01/config.yaml b/robotic_grounding/experiments/recon_body_snack_box_pick_and_place_01/config.yaml deleted file mode 100644 index 7ba0e4f8..00000000 --- a/robotic_grounding/experiments/recon_body_snack_box_pick_and_place_01/config.yaml +++ /dev/null @@ -1,34 +0,0 @@ -# ReconBody: Snack box pick-and-place with whole-body tracking -# -# Usage: -# python robotic_grounding/experiments/run_experiment.py recon_body_snack_box_pick_and_place_01 --local -id: recon_body_snack_box_pick_and_place_01 -description: "ReconBody snack box pick-and-place with SONIC JOINT_RESIDUAL" -run_name: recon_body_snack_box_pick_and_place_01 -task: SonicG1-ReconBody-v0 -# Use the explicit repo-relative partition path because whole_body/soma does not -# follow the 4-part dataset/dataset_retargeted/sequence_id/robot shorthand. -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/robot_name=g1 -video: true -num_envs: 4096 -max_iterations: 20000 -zero_actor: true -logger: wandb -log_project_name: v2d-whole-body-tpv - -train_overrides: - env.commands.motion.reset_freeze_steps: 50 - env.commands.motion.voc_decay_steps: 10 - env.commands.motion.voc_reset_scale: 1.0 - env.commands.motion.initial_virtual_object_control_curriculum_scale: 1.0 - # Frame range of the source motion to train on. -1 for motion_end_frame - # means run to the end of the sequence. - # Use replay_motion_viser.py to visualize the motion and get the frame range. - env.commands.motion.motion_start_frame: 300 - env.commands.motion.motion_end_frame: -1 - env.episode_length_s: 5.0 - # Reset the shoulder spread to 0.2 - env.commands.motion.reset_shoulder_spread: 0.2 - -osmo: - build_image: false diff --git a/robotic_grounding/experiments/registry.yaml b/robotic_grounding/experiments/registry.yaml deleted file mode 100644 index 13f2bae3..00000000 --- a/robotic_grounding/experiments/registry.yaml +++ /dev/null @@ -1,16 +0,0 @@ -# Experiment registry - maps short ids to experiment directories -# Used by: python robotic_grounding/scripts/run_experiment.py [--local|--osmo] -# -# Example experiments (committed): -example_single: example_single_run - -# Whole-body experiments: -recon_body_corn_can_right_left_handover_01: recon_body_corn_can_right_left_handover_01 -recon_body_apple: recon_body_apple_pick -recon_body_bottle_pick_transfer: recon_body_bottle_pick_transfer -recon_body_snack_box_pick_and_place_01: recon_body_snack_box_pick_and_place_01 -recon_body_blue_trash_can_drag_007: recon_body_blue_trash_can_drag_007 - -# Private experiments: create experiments/registry.local.yaml (gitignored) with your entries: -# exp41: exp41_my_experiment -# exp42: exp42_another_experiment diff --git a/robotic_grounding/experiments/run_experiment.py b/robotic_grounding/experiments/run_experiment.py deleted file mode 100644 index 93233c5b..00000000 --- a/robotic_grounding/experiments/run_experiment.py +++ /dev/null @@ -1,995 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unified experiment runner - run experiments locally or submit to OSMO. - -SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -SPDX-License-Identifier: Apache-2.0 - -Usage: - python scripts/run_experiment.py exp5 # defaults to --osmo - python scripts/run_experiment.py exp5 --local - python scripts/run_experiment.py exp5 --osmo --pool --build-image - python scripts/run_experiment.py exp6 --wandb-sweep-create # Create wandb sweep, print agent command - python scripts/run_experiment.py exp7 --osmo # Submit OSMO curriculum sweep - python scripts/run_experiment.py --list # List all experiments -""" - -from __future__ import annotations - -import argparse -import importlib.util -import os -import subprocess -import sys -import tempfile -from collections.abc import Callable -from pathlib import Path - -import yaml - -REPO_ROOT = Path(__file__).resolve().parent.parent.parent -RG_ROOT = Path(__file__).resolve().parent.parent -EXPERIMENTS_DIR = RG_ROOT / "experiments" -WORKFLOW_DIR = REPO_ROOT / "workflow" - -if str(RG_ROOT) not in sys.path: - sys.path.insert(0, str(RG_ROOT)) -from experiments.utils import ( # noqa: E402 - DEFAULT_OSMO_IMAGE_LATEST, - DEFAULT_OSMO_IMAGE_REPO, - DEFAULT_WANDB_ENTITY, - build_eval_command, - build_train_command, - get_internal_config_value, - make_entry_script, - overrides_to_cli, - require_internal_config_value, -) - - -def load_registry() -> dict[str, str]: - """Load experiment id -> directory mapping. - - Merges registry.yaml (committed) with registry.local.yaml (gitignored). - Local entries take precedence, allowing private experiments to coexist with - the committed examples without modifying tracked files. - """ - reg_path = EXPERIMENTS_DIR / "registry.yaml" - local_path = EXPERIMENTS_DIR / "registry.local.yaml" - registry: dict[str, str] = {} - if reg_path.exists(): - with open(reg_path, encoding="utf-8") as f: - registry.update(yaml.safe_load(f) or {}) - if local_path.exists(): - with open(local_path, encoding="utf-8") as f: - registry.update(yaml.safe_load(f) or {}) - return registry - - -def _deep_merge_dict(base: dict, overrides: dict) -> dict: - """Return base recursively overlaid with overrides.""" - merged = dict(base) - for key, value in overrides.items(): - if isinstance(value, dict) and isinstance(merged.get(key), dict): - merged[key] = _deep_merge_dict(merged[key], value) - else: - merged[key] = value - return merged - - -def _merge_internal_experiment_config(exp_id: str, config: dict) -> dict: - """Overlay optional internal per-experiment settings by id.""" - internal_experiments = get_internal_config_value("experiments", default={}) - if not isinstance(internal_experiments, dict): - return config - - merged = config - for key in dict.fromkeys((exp_id, config.get("id"))): - if not key: - continue - internal_config = internal_experiments.get(key) - if isinstance(internal_config, dict): - merged = _deep_merge_dict(merged, internal_config) - return merged - - -def load_experiment_config(exp_id: str) -> tuple[Path, dict]: - """Load config for experiment. Returns (config_path, config dict).""" - registry = load_registry() - if exp_id not in registry: - raise SystemExit( - f"Unknown experiment: {exp_id}. Use --list to see available experiments." - ) - exp_dir = EXPERIMENTS_DIR / registry[exp_id] - config_path = exp_dir / "config.yaml" - if not config_path.exists(): - raise SystemExit(f"Config not found: {config_path}") - with open(config_path, encoding="utf-8") as f: - config = yaml.safe_load(f) - if not config: - raise SystemExit(f"Empty or invalid config: {config_path}") - config = _merge_internal_experiment_config(exp_id, config) - return config_path, config - - -def get_effective_overrides( - config: dict, osmo: bool = False -) -> tuple[str, dict[str, str]]: - """Get run_name and overrides for the experiment. Handles osmo overrides.""" - run_name = config.get("run_name", "experiment") - overrides = dict(config.get("train_overrides", {})) - if osmo and "osmo" in config: - osmo_cfg = config["osmo"] - if "run_name_overrides" in osmo_cfg: - overrides.update(osmo_cfg["run_name_overrides"]) - if "run_name_suffix" in osmo_cfg: - run_name = "${RUN_TS}" + osmo_cfg["run_name_suffix"] - return run_name, overrides - - -def run_local( - exp_id: str, - config: dict, - variant: str | None = None, - dry_run: bool = False, - *, - num_envs_override: int | None = None, - max_iterations_override: int | None = None, - logger_override: str | None = None, - extra_overrides: dict[str, str] | None = None, -) -> int: - """Run experiment locally via train.py.""" - run_name = config.get("run_name", f"{exp_id}_run") - overrides = dict(config.get("train_overrides", {})) - if extra_overrides: - overrides.update(extra_overrides) - resume_from = config.get("resume_from") - seed = config.get("seed") - motion_file = config.get("motion_file") - max_iterations = ( - max_iterations_override - if max_iterations_override is not None - else config.get("max_iterations") - ) - # Stage 1 (osmo_multi_task): pick first sequence, derive motion_file. - if "osmo_multi_task" in config: - seq_ids = config["osmo_multi_task"].get("sequence_ids", []) - if seq_ids: - motion_file = ( - motion_file or f"arctic/arctic_processed/{seq_ids[0]}/sharpa_wave" - ) - - # Stage 2 (sequences): pick first sequence, derive motion_file. - elif "sequences" in config: - seqs = config["sequences"] - if seqs: - motion_file = motion_file or f"arctic_processed/{seqs[0]}/sharpa_wave" - - # Apply variant-specific overrides from workflow.py if --variant is given. - if variant is not None: - workflow_path = EXPERIMENTS_DIR / exp_id / "workflow.py" - if not workflow_path.exists(): - # Resolve experiment dir via registry (exp_id may differ from folder name) - config_path, _ = load_experiment_config(exp_id) - workflow_path = config_path.parent / "workflow.py" - if workflow_path.exists(): - spec = importlib.util.spec_from_file_location("_workflow", workflow_path) - if spec is None or spec.loader is None: - print( - f"[WARNING] Could not load workflow module from {workflow_path}; " - "variant flag ignored." - ) - else: - mod = importlib.util.module_from_spec(spec) - spec.loader.exec_module(mod) - if hasattr(mod, "get_variant_overrides"): - overrides.update(mod.get_variant_overrides(variant, config)) - else: - print( - f"[WARNING] workflow.py for {exp_id} has no get_variant_overrides(); " - "variant flag ignored." - ) - else: - print(f"[WARNING] No workflow.py found for {exp_id}; variant flag ignored.") - - video = config.get("video", True) - num_envs = ( - num_envs_override if num_envs_override is not None else config.get("num_envs") - ) - logger = logger_override if logger_override is not None else config.get("logger") - cmd = build_train_command( - run_name, - overrides, - resume_from=resume_from, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - max_iterations=max_iterations, - video=video, - task=config.get("task", "Sharpa-V2P-v0"), - logger=logger, - log_project_name=config.get("log_project_name"), - zero_actor=config.get("zero_actor", False), - ) - cmd_str = " ".join(cmd) - print(f"Running: {cmd_str}") - if dry_run: - return 0 - result = subprocess.run(cmd_str, shell=True, check=False) - return result.returncode - - -def run_eval( - exp_id: str, - config: dict, - *, - checkpoint: str | None = None, - num_envs: int | None = None, - video: bool = False, - video_length: int | None = None, - eval_episodes: int | None = None, - real_time: bool = False, - extra_overrides: dict[str, str] | None = None, - dry_run: bool = False, -) -> int: - """Run eval.py for the given experiment using its config.yaml as source of truth. - - Pulls motion_file, task, and the same train_overrides block used by training - so train + eval stay in sync (frame range, freeze steps, etc.). CLI flags - layered on top via run_experiment.py override config values. - """ - motion_file = config.get("motion_file") - overrides = dict(config.get("train_overrides", {})) - if extra_overrides: - overrides.update(extra_overrides) - - if num_envs is None: - num_envs = config.get("num_envs") - seed = config.get("seed") - - cmd = build_eval_command( - overrides, - checkpoint=checkpoint, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - video=video, - video_length=video_length, - eval_episodes=eval_episodes, - task=config.get("task", "Sharpa-V2P-v0"), - logger=config.get("logger"), - log_project_name=config.get("log_project_name"), - use_primitive_urdfs=config.get("use_primitive_urdfs", False), - real_time=real_time, - ) - cmd_str = " ".join(cmd) - print(f"Running: {cmd_str}") - if dry_run: - return 0 - result = subprocess.run(cmd_str, shell=True, check=False) - return result.returncode - - -def _is_local_path(path: str) -> bool: - """True if path looks like a local file (not s3/http/gs).""" - return not ( - path.startswith("s3://") - or path.startswith("http://") - or path.startswith("https://") - or path.startswith("gs://") - ) - - -def generate_single_task_workflow( - exp_id: str, config: dict, run_name: str, overrides: dict[str, str] -) -> str: - """Generate OSMO workflow YAML for a single training task.""" - osmo_cfg = config.get("osmo", {}) - storage_gi = osmo_cfg.get("storage_gi", 200) - # OSMO runs in a fresh container; local checkpoint paths don't exist. - # - osmo.resume_from: false -> train from scratch - # - osmo.resume_from: "" -> upload via dataset input (localpath) at submit time - # - osmo.resume_from: "" -> use as-is (must be accessible from container) - motion_data_url = osmo_cfg.get("motion_data_url") - resume_from = osmo_cfg.get("resume_from") - if resume_from is None: - resume_from = config.get("resume_from") - if resume_from is False or resume_from == "false": - resume_from = None - # Track if we're using localpath upload (add inputs block) - checkpoint_localpath = None - checkpoint_in_container = None - if resume_from and _is_local_path(resume_from): - checkpoint_localpath = resume_from - # OSMO mounts dataset at {{input:0}}/dataset_name/filename for a single file - dataset_name = "resume_checkpoint" - filename = Path(resume_from).name - checkpoint_in_container = f"{{{{input:0}}}}/{dataset_name}/{filename}" - if "run_name_suffix" in osmo_cfg: - run_name = osmo_cfg["run_name_suffix"].lstrip("_") - if "run_name_overrides" in osmo_cfg: - overrides = {**overrides, **osmo_cfg["run_name_overrides"]} - seed = config.get("seed") - video = config.get("video", True) - eval_video_only = config.get("eval_video_only", False) - video_length = config.get("video_length") - video_interval = config.get("video_interval") - eval_episodes_per_save = config.get("eval_episodes_per_save", 0) - motion_file = config.get("motion_file") - num_envs = config.get("num_envs") - max_iterations = config.get("max_iterations") - task = config.get("task", "Sharpa-V2P-v0") - zero_actor = config.get("zero_actor", False) - use_primitive_urdfs = config.get("use_primitive_urdfs", False) - logger = config.get("logger", "wandb") - log_project_name = config.get("wandb_project") or config.get( - "log_project_name", "v2p_hands" - ) - # Derive motion_file from OSMO dataset path when motion_data_url is set, matching - # the behaviour of generate_multi_sequence_workflow so single-sequence relaunch - # batches use the same path format as the original multi-sequence workflow. - urdfs_src_path = None - if ( - motion_file is None - and motion_data_url - and "sequences" in config - and config["sequences"] - ): - seq_id = config["sequences"][0] - dataset_name = motion_data_url.rstrip("/").split("/")[-1] - sequences_subfolder = osmo_cfg.get("sequences_subfolder", "arctic_processed") - if sequences_subfolder == "arctic_processed": - dataset_seq_id = seq_id.replace("arctic_", "dataset_", 1) - else: - dataset_seq_id = seq_id - motion_file = f"{{{{input:0}}}}/{dataset_name}/{sequences_subfolder}/sequence_id={dataset_seq_id}/robot_name=sharpa_wave" - urdfs_subfolder = osmo_cfg.get("urdfs_subfolder") - if urdfs_subfolder: - urdfs_src_path = f"{{{{input:0}}}}/{dataset_name}/{urdfs_subfolder}" - - entry = make_entry_script( - run_name, - overrides, - resume_from=checkpoint_in_container if checkpoint_in_container else resume_from, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - max_iterations=max_iterations, - video=video, - eval_video_only=eval_video_only, - video_length=video_length, - video_interval=video_interval, - eval_episodes_per_save=eval_episodes_per_save, - task=task, - logger=logger, - log_project_name=log_project_name, - zero_actor=zero_actor, - use_primitive_urdfs=use_primitive_urdfs, - use_timestamp=True, - urdfs_src_path=urdfs_src_path, - ) - # Escape for YAML literal block - entry_indent = "\n".join(" " + line for line in entry.split("\n")) - inputs_entries = [] - if checkpoint_localpath: - inputs_entries.append( - f" - dataset:\n name: resume_checkpoint\n localpath: {checkpoint_localpath!r}" - ) - if motion_data_url: - dataset_name = motion_data_url.rstrip("/").split("/")[-1] - inputs_entries.append(f" - dataset:\n name: {dataset_name}") - inputs_block = ( - ("\n inputs:\n" + "\n".join(inputs_entries) + "\n") if inputs_entries else "" - ) - wandb_api_key = os.environ.get("WANDB_API_KEY", "") - if not wandb_api_key: - print( - "[WARNING] WANDB_API_KEY not set in local environment — wandb will fail in the container" - ) - wandb_entity = config.get("wandb_entity") or DEFAULT_WANDB_ENTITY - if wandb_entity is None: - wandb_entity = require_internal_config_value("wandb_entity") - omni_server = require_internal_config_value("osmo", "omni_server") - image_latest = DEFAULT_OSMO_IMAGE_LATEST or require_internal_config_value( - "osmo", "image_latest" - ) - return f"""# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# Generated from experiments/ for {exp_id} -# When resume_from is a local path, OSMO uploads it at submit time via dataset localpath. - -workflow: - name: {{{{workflow_name}}}} - resources: - default: - cpu: 6 - gpu: 1 - memory: 120Gi - storage: {storage_gi}Gi - - tasks: - - name: train - image: {{{{image}}}} - command: [/bin/bash] - args: [/tmp/entry.sh] - environment: - ACCEPT_EULA: Y - OMNI_SERVER: {omni_server} - WANDB_API_KEY: {wandb_api_key} - WANDB_ENTITY: {wandb_entity} -{inputs_block} files: - - path: /tmp/entry.sh - contents: |- -{entry_indent} - -default-values: - workflow_name: robotic_grounding_{exp_id} - image: {image_latest} -""" - - -def _seq_to_key(seq_id: str) -> str: - """Strip the 'arctic_' dataset prefix, keeping subject + action for uniqueness. - - E.g. 'arctic_s01_capsulemachine_grab_01' -> 's01_capsulemachine_grab_01'. - Including the subject avoids collisions when the same object/action appears - under multiple subjects (e.g. s02_waffleiron_use_01 vs s07_waffleiron_use_01). - """ - prefix = "arctic_" - return seq_id[len(prefix) :] if seq_id.startswith(prefix) else seq_id - - -def generate_multi_sequence_workflow( - exp_id: str, config: dict, overrides: dict[str, str], workflow_label: str = "" -) -> str: - """Generate a multi-task OSMO workflow for single-stage configs with multiple sequences. - - Creates one task per sequence — the same pattern stage2 uses, but without checkpoint inputs. - """ - sequences = config.get("sequences", []) - osmo_cfg = config.get("osmo", {}) - storage_gi = osmo_cfg.get("storage_gi", 200) - motion_data_url = osmo_cfg.get("motion_data_url") - run_name_suffix = config.get("run_name_suffix", exp_id) - project = config.get("wandb_project", "v2p_hands") - eval_video_only = config.get("eval_video_only", False) - video = config.get("video", True) - video_length = config.get("video_length") - video_interval = config.get("video_interval") - eval_episodes_per_save = config.get("eval_episodes_per_save", 0) - seed = config.get("seed") - num_envs = config.get("num_envs") - task = config.get("task", "Sharpa-V2P-v0") - use_primitive_urdfs = config.get("use_primitive_urdfs", False) - wandb_api_key = os.environ.get("WANDB_API_KEY", "") - if not wandb_api_key: - print( - "[WARNING] WANDB_API_KEY not set in local environment — wandb will fail in the container" - ) - wandb_entity = config.get("wandb_entity") or DEFAULT_WANDB_ENTITY - if wandb_entity is None: - wandb_entity = require_internal_config_value("wandb_entity") - omni_server = require_internal_config_value("osmo", "omni_server") - image_latest = DEFAULT_OSMO_IMAGE_LATEST or require_internal_config_value( - "osmo", "image_latest" - ) - - tasks = [] - for seq_id in sequences: - seq_key = _seq_to_key(seq_id) - run_name = f"{run_name_suffix}_{seq_key}" - - if motion_data_url: - dataset_name = motion_data_url.rstrip("/").split("/")[-1] - sequences_subfolder = osmo_cfg.get( - "sequences_subfolder", "arctic_processed" - ) - if sequences_subfolder == "arctic_processed": - dataset_seq_id = seq_id.replace("arctic_", "dataset_", 1) - else: - dataset_seq_id = seq_id - motion_file = f"{{{{input:0}}}}/{dataset_name}/{sequences_subfolder}/sequence_id={dataset_seq_id}/robot_name=sharpa_wave" - inputs_block = ( - "\n inputs:\n" " - dataset:\n" f" name: {dataset_name}\n" - ) - urdfs_subfolder = osmo_cfg.get("urdfs_subfolder") - urdfs_src_path = ( - f"{{{{input:0}}}}/{dataset_name}/{urdfs_subfolder}" - if urdfs_subfolder - else None - ) - else: - motion_file = f"arctic/arctic_processed/{seq_id}/sharpa_wave" - inputs_block = "" - urdfs_src_path = None - - entry = make_entry_script( - run_name, - overrides, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - video=video, - eval_video_only=eval_video_only, - video_length=video_length, - video_interval=video_interval, - eval_episodes_per_save=eval_episodes_per_save, - task=task, - logger="wandb", - log_project_name=project, - use_primitive_urdfs=use_primitive_urdfs, - use_timestamp=True, - urdfs_src_path=urdfs_src_path, - ) - entry_indent = "\n".join(" " + line for line in entry.split("\n")) - task_name = f"train-{seq_key.replace('_', '-')}" - tasks.append( - f" - name: {task_name}\n" - f" image: {{{{image}}}}\n" - f" command: [/bin/bash]\n" - f" args: [/tmp/entry.sh]\n" - f" environment:\n" - f" ACCEPT_EULA: Y\n" - f" OMNI_SERVER: {omni_server}\n" - f" WANDB_API_KEY: {wandb_api_key}\n" - f" WANDB_ENTITY: {wandb_entity}\n" - f"{inputs_block}" - f" files:\n" - f" - path: /tmp/entry.sh\n" - f" contents: |-\n" - f"{entry_indent}" - ) - - tasks_str = "\n".join(tasks) - return ( - f"# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n" - f"# SPDX-License-Identifier: Apache-2.0\n" - f"# Generated from experiments/ for {exp_id}\n" - f"\n" - f"workflow:\n" - f" name: {{{{workflow_name}}}}\n" - f" resources:\n" - f" default:\n" - f" cpu: 6\n" - f" gpu: 1\n" - f" memory: 120Gi\n" - f" storage: {storage_gi}Gi\n" - f"\n" - f" tasks:\n" - f"{tasks_str}\n" - f"\n" - f"default-values:\n" - f" workflow_name: robotic_grounding_{exp_id}{'_' + workflow_label if workflow_label else ''}\n" - f" image: {image_latest}\n" - ) - - -def _load_workflow_generator(exp_dir: Path) -> Callable[[str, dict], str] | None: - """Load generate_workflow from experiment's workflow.py if it exists.""" - workflow_py = exp_dir / "workflow.py" - if not workflow_py.exists(): - return None - spec = importlib.util.spec_from_file_location("workflow", workflow_py) - if spec is None or spec.loader is None: - return None - mod = importlib.util.module_from_spec(spec) - spec.loader.exec_module(mod) - return getattr(mod, "generate_workflow", None) - - -def _print_workflow(exp_id: str, config: dict) -> None: - """Print the OSMO entry script (train command) for verification.""" - run_name, overrides = get_effective_overrides(config, osmo=True) - if "osmo" in config and "run_name_suffix" in config["osmo"]: - run_name = config["osmo"]["run_name_suffix"].lstrip("_") - else: - run_name = config.get("run_name", exp_id) - osmo_cfg = config.get("osmo", {}) - resume_from = osmo_cfg.get("resume_from") - if resume_from is None: - resume_from = config.get("resume_from") - if resume_from is False or resume_from == "false": - resume_from = None - seed = config.get("seed") - video = config.get("video", True) - eval_video_only = config.get("eval_video_only", False) - video_length = config.get("video_length") - video_interval = config.get("video_interval") - eval_episodes_per_save = config.get("eval_episodes_per_save", 0) - motion_file = config.get("motion_file") - num_envs = config.get("num_envs") - max_iterations = config.get("max_iterations") - task = config.get("task", "Sharpa-V2P-v0") - zero_actor = config.get("zero_actor", False) - use_primitive_urdfs = config.get("use_primitive_urdfs", False) - logger = config.get("logger", "wandb") - log_project_name = config.get("log_project_name", "v2p_hands") - if "run_name_overrides" in osmo_cfg: - overrides = {**overrides, **osmo_cfg["run_name_overrides"]} - entry = make_entry_script( - run_name, - overrides, - resume_from=resume_from, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - max_iterations=max_iterations, - video=video, - eval_video_only=eval_video_only, - video_length=video_length, - video_interval=video_interval, - eval_episodes_per_save=eval_episodes_per_save, - task=task, - logger=logger, - log_project_name=log_project_name, - zero_actor=zero_actor, - use_primitive_urdfs=use_primitive_urdfs, - use_timestamp=True, - ) - print("# OSMO entry script (train command) for", exp_id) - print("# Run: python scripts/run_experiment.py", exp_id, "--osmo") - print() - print(entry) - - -def _check_osmo_login() -> None: - """Abort early if the OSMO CLI is not authenticated.""" - result = subprocess.run( - ["osmo", "profile", "list"], - capture_output=True, - text=True, - check=False, - ) - if result.returncode != 0: - print("[ERROR] OSMO is not logged in. Run: osmo login") - print(result.stderr.strip()) - sys.exit(1) - - -def run_osmo( - exp_id: str, - config: dict, - pool: str | None = None, - build_image: bool = False, - image: str | None = None, - priority: str = "NORMAL", - dry_run: bool = False, - workflow_label: str = "", -) -> None: - """Generate workflow YAML and submit to OSMO via run_osmo.py.""" - if not dry_run: - _check_osmo_login() - if pool is None: - pool = require_internal_config_value("osmo", "default_pool") - - # When the user asks to build but doesn't pin an explicit image tag, derive one from - # exp_id so the tag produced by run_osmo.py matches the tag baked into the workflow - # YAML below. Without this, --build-image silently builds and pushes a new tag while - # the submitted workflow still references :latest, which causes OSMO to run a stale - # image and e.g. miss newly-registered gym task IDs (see SonicG1-ReconBody-v0). - if build_image and image is None: - image_repo = DEFAULT_OSMO_IMAGE_REPO or require_internal_config_value( - "osmo", "image_repo" - ) - image = f"{image_repo}:{exp_id}" - print(f"[INFO] --build-image without --image: pinning to {image}") - - if "osmo_multi_task" in config: - exp_dir = EXPERIMENTS_DIR / load_registry()[exp_id] - generator = _load_workflow_generator(exp_dir) - if generator is None: - raise SystemExit( - f"Experiment {exp_id} has osmo_multi_task but no workflow.py found in {exp_dir}" - ) - workflow_content = generator(exp_id, config) - elif "sequences" in config and len(config["sequences"]) > 1: - _, overrides = get_effective_overrides(config, osmo=True) - workflow_content = generate_multi_sequence_workflow( - exp_id, config, overrides, workflow_label - ) - else: - run_name, overrides = get_effective_overrides(config, osmo=True) - if "osmo" in config and "run_name_suffix" in config["osmo"]: - run_name = config["osmo"]["run_name_suffix"].lstrip("_") - else: - run_name = config.get("run_name", exp_id) - workflow_content = generate_single_task_workflow( - exp_id, config, run_name, overrides - ) - - with tempfile.NamedTemporaryFile( - mode="w", - suffix=".yaml", - delete=False, - dir=RG_ROOT, - ) as f: - f.write(workflow_content) - workflow_path = Path(f.name) - - try: - run_osmo_py = RG_ROOT / "scripts" / "run_osmo.py" - exp_name = exp_id - if exp_id == "exp6": - exp_name = "exp6_contact_sweep" - elif exp_id == "exp7": - exp_name = "exp7_curriculum_sweep" - elif exp_id == "exp10": - exp_name = "exp10_sequence_parallel" - elif exp_id == "exp11": - exp_name = "exp11_zeroinit" - if workflow_label: - exp_name = f"{exp_name}_{workflow_label}" - cmd = [ - sys.executable, - str(run_osmo_py), - "--experiment-name", - exp_name, - "--workflow-yaml", - str(workflow_path), - "--pool", - pool, - ] - if build_image: - cmd.append("--build-image") - if image: - cmd.extend(["--image", image]) - cmd.extend(["--priority", priority]) - if dry_run: - cmd.append("--dry-run") - result = subprocess.run(cmd, cwd=RG_ROOT, check=False) - sys.exit(result.returncode) - finally: - workflow_path.unlink(missing_ok=True) - - -def create_wandb_sweep(exp_id: str, config: dict) -> None: - """Create wandb sweep for exp6 and print agent command.""" - if "wandb_sweep" not in config: - raise SystemExit(f"Experiment {exp_id} does not support wandb sweep") - ws = config["wandb_sweep"] - param = ws["sweep_param"] - values = ws["values"] - project = ws.get("project", "v2p_hands") - base = dict(config["train_overrides"]) - sweep_config = { - "program": "scripts/rsl_rl/train.py", - "method": "grid", - "project": project, - "command": [ - "${env}", - "${interpreter}", - "${program}", - "--headless", - "--video", - "--task", - "Sharpa-V2P-v0", - "--run_name", - config.get("run_name", "experiment_6_contactTrackingSweep"), - *overrides_to_cli(base), - ], - "parameters": {param: {"values": values}}, - } - exp_dir = EXPERIMENTS_DIR / load_registry()[exp_id] - sweep_path = exp_dir / "sweep_wandb.yaml" - sweep_path.parent.mkdir(parents=True, exist_ok=True) - with open(sweep_path, "w", encoding="utf-8") as f: - yaml.dump(sweep_config, f, default_flow_style=False, sort_keys=False) - print(f"Wrote {sweep_path}") - print("Run: wandb sweep", sweep_path) - print("Then: wandb agent /" + project + "/") - - -def main() -> None: - """Parse arguments and run experiment locally or on OSMO.""" - parser = argparse.ArgumentParser(description="Run experiments locally or on OSMO") - parser.add_argument("exp_id", nargs="?", help="Experiment id (e.g. exp0, exp5)") - parser.add_argument("--list", action="store_true", help="List all experiments") - parser.add_argument("--local", action="store_true", help="Run locally via train.py") - parser.add_argument("--osmo", action="store_true", help="Submit to OSMO") - parser.add_argument( - "--eval", - action="store_true", - help="Run eval.py locally with the experiment's motion_file + train_overrides.", - ) - parser.add_argument( - "--checkpoint", - default=None, - help="(--eval) Path to the policy checkpoint to evaluate.", - ) - parser.add_argument( - "--num-envs", - type=int, - default=None, - help="(--eval) Override num_envs from config.", - ) - parser.add_argument( - "--video", - action="store_true", - help="(--eval) Record an evaluation video.", - ) - parser.add_argument( - "--video-length", - type=int, - default=None, - help="(--eval) Number of steps in the recorded video.", - ) - parser.add_argument( - "--eval-episodes", - type=int, - default=None, - help="(--eval) Stop after this many completed episodes and print stats.", - ) - parser.add_argument( - "--real-time", - action="store_true", - help="(--eval) Run eval at real time, sleeping between sim steps.", - ) - parser.add_argument( - "--wandb-sweep-create", - action="store_true", - help="Create wandb sweep config (exp6 only)", - ) - parser.add_argument( - "--pool", - default=None, - help="OSMO pool (default: config osmo.pool, then internal experiment_config.yaml)", - ) - parser.add_argument( - "--build-image", action="store_true", help="Build and push image before OSMO" - ) - parser.add_argument("--image", help="Use specific Docker image for OSMO") - parser.add_argument( - "--priority", - default="NORMAL", - help="OSMO job priority (default: NORMAL)", - ) - parser.add_argument( - "--dry-run", action="store_true", help="Print without executing" - ) - parser.add_argument( - "--variant", - default=None, - help="For multi-task experiments: name of workflow variant to run locally (e.g. hand_kp_w2)", - ) - parser.add_argument( - "--print-workflow", - action="store_true", - help="Print generated OSMO entry script (train command) without submitting", - ) - parser.add_argument( - "--run-name", - default=None, - help="Override the config's run_name (the W&B run name will be {timestamp}_{run_name}).", - ) - parser.add_argument( - "--run-name-prefix", - default=None, - help="Prefix to prepend to W&B run names (e.g. 'exp48_')", - ) - parser.add_argument( - "--workflow-label", - default="", - help="Label appended to the OSMO workflow name for descriptive identification (e.g. 'init', 'rerun_2')", - ) - parser.add_argument( - "--max-iterations", - type=int, - default=None, - help="(--local) Override max_iterations from config (useful for smoke tests).", - ) - parser.add_argument( - "--logger", - default=None, - help="(--local) Override logger from config, e.g. tensorboard for offline smoke tests.", - ) - parser.add_argument( - "-O", - "--override", - action="append", - default=[], - metavar="KEY=VAL", - help="Hydra-style override appended to train_overrides / eval overrides. Repeatable. " - "Example: -O env.episode_length_s=2.0 -O env.commands.motion.voc_decay_steps=2", - ) - args = parser.parse_args() - - if args.list: - registry = load_registry() - for eid, dirname in sorted(registry.items()): - config_path = EXPERIMENTS_DIR / dirname / "config.yaml" - if config_path.exists(): - with open(config_path, encoding="utf-8") as f: - cfg = yaml.safe_load(f) or {} - desc = cfg.get("description", "") - print(f" {eid}: {desc} ({dirname})") - else: - print(f" {eid}: missing config ({dirname})") - return - - if not args.exp_id: - parser.error("exp_id required unless --list") - if ( - not args.local - and not args.osmo - and not args.eval - and not args.wandb_sweep_create - and not args.print_workflow - ): - args.osmo = True - - _, config = load_experiment_config(args.exp_id) - - if args.run_name is not None: - config["run_name"] = args.run_name - if args.run_name_prefix is not None: - config["run_name_prefix"] = args.run_name_prefix - - extra_overrides: dict[str, str] = {} - for item in args.override: - if "=" not in item: - parser.error(f"--override expects KEY=VAL, got: {item!r}") - k, v = item.split("=", 1) - extra_overrides[k.strip()] = v.strip() - - if args.print_workflow: - _print_workflow(args.exp_id, config) - return - - if args.eval: - sys.exit( - run_eval( - args.exp_id, - config, - checkpoint=args.checkpoint, - num_envs=args.num_envs, - video=args.video, - video_length=args.video_length, - eval_episodes=args.eval_episodes, - real_time=args.real_time, - extra_overrides=extra_overrides or None, - dry_run=args.dry_run, - ) - ) - if args.local: - sys.exit( - run_local( - args.exp_id, - config, - variant=args.variant, - dry_run=args.dry_run, - num_envs_override=args.num_envs, - max_iterations_override=args.max_iterations, - logger_override=args.logger, - extra_overrides=extra_overrides or None, - ) - ) - elif args.osmo: - # Use --build-image from CLI, or osmo.build_image from config (e.g. exp9 needs it for contact_force) - osmo_cfg = config.get("osmo", {}) - build_image = args.build_image or osmo_cfg.get("build_image", False) - # Image precedence: CLI --image > config osmo.image > (auto-derived from exp_id - # when --build-image and nothing else is set, see run_osmo) > workflow YAML default. - image = args.image or osmo_cfg.get("image") - pool = ( - args.pool - or osmo_cfg.get("pool") - or get_internal_config_value("osmo", "default_pool") - ) - priority = args.priority or osmo_cfg.get("priority", "NORMAL") - run_osmo( - args.exp_id, - config, - pool=pool, - build_image=build_image, - image=image, - priority=priority, - dry_run=args.dry_run, - workflow_label=args.workflow_label, - ) - elif args.wandb_sweep_create: - create_wandb_sweep(args.exp_id, config) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/experiments/utils.py b/robotic_grounding/experiments/utils.py deleted file mode 100644 index 7401d60c..00000000 --- a/robotic_grounding/experiments/utils.py +++ /dev/null @@ -1,263 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Shared utilities for experiment workflow generation. - -Used by run_experiment.py and experiment-specific workflow.py modules. -""" - -from __future__ import annotations - -from functools import lru_cache -from pathlib import Path -from typing import Any - -import yaml - -_REPO_ROOT = Path(__file__).resolve().parent.parent -EXPERIMENT_CONFIG_PATH = Path(__file__).resolve().parent / "experiment_config.yaml" - - -@lru_cache(maxsize=1) -def load_internal_experiment_config() -> dict[str, Any]: - """Load optional internal launch settings removed from release builds.""" - if not EXPERIMENT_CONFIG_PATH.exists(): - return {} - with open(EXPERIMENT_CONFIG_PATH, encoding="utf-8") as f: - data = yaml.safe_load(f) or {} - if not isinstance(data, dict): - raise ValueError( - f"Invalid internal experiment config: {EXPERIMENT_CONFIG_PATH}" - ) - return data - - -def get_internal_config_value(*keys: str, default: Any = None) -> Any: - """Return a nested value from experiment_config.yaml when present.""" - value: Any = load_internal_experiment_config() - for key in keys: - if not isinstance(value, dict) or key not in value: - return default - value = value[key] - return value - - -def require_internal_config_value(*keys: str) -> Any: - """Return a required internal value or fail with release-safe guidance.""" - value = get_internal_config_value(*keys) - if value is None: - dotted = ".".join(keys) - raise RuntimeError( - f"Missing internal experiment config value: {dotted}. " - f"Create {EXPERIMENT_CONFIG_PATH} or pass an explicit override." - ) - return value - - -DEFAULT_WANDB_ENTITY = get_internal_config_value("wandb_entity") -DEFAULT_OSMO_IMAGE_REPO = get_internal_config_value("osmo", "image_repo") -DEFAULT_OSMO_IMAGE_LATEST = get_internal_config_value("osmo", "image_latest") - - -def sequence_to_object(sequence_id: str) -> str: - """e.g. arctic_s01_capsulemachine_grab_01 -> capsulemachine.""" - parts = sequence_id.split("_") - return parts[2] if len(parts) >= 3 else sequence_id - - -def is_local_path(path: str) -> bool: - """True if path looks like a local file (not s3/http/gs).""" - return not ( - path.startswith("s3://") - or path.startswith("http://") - or path.startswith("https://") - or path.startswith("gs://") - ) - - -def _format_train_override_value(key: str, value: object) -> str: - """Format a single train override for Hydra key=value syntax.""" - if isinstance(value, bool): - return "true" if value else "false" - # YAML often parses 1 / 0 as int; Hydra then infers int, but ConfigClass expects float. - if isinstance(value, int) and not isinstance(value, bool): - if "initial_virtual_object_control_curriculum_scale" in key: - return str(float(value)) - return str(value) - - -def overrides_to_cli(overrides: dict[str, Any]) -> list[str]: - """Convert overrides dict to train.py CLI args (key=value format).""" - return [f"{k}={_format_train_override_value(k, v)}" for k, v in overrides.items()] - - -def build_train_command( - run_name: str, - overrides: dict[str, Any], - *, - resume_from: str | None = None, - seed: int | None = None, - motion_file: str | None = None, - num_envs: int | None = None, - max_iterations: int | None = None, - headless: bool = True, - video: bool = True, - eval_video_only: bool = False, - video_length: int | None = None, - video_interval: int | None = None, - eval_episodes_per_save: int = 0, - task: str = "Sharpa-V2P-v0", - logger: str | None = None, - log_project_name: str | None = None, - zero_actor: bool = False, - use_primitive_urdfs: bool = False, -) -> list[str]: - """Build train.py command as list of args.""" - cmd = [ - "python", - "scripts/rsl_rl/train.py", - "--headless" if headless else "", - "--video" if video else "", - "--zero-actor" if zero_actor else "", - "--use_primitive_urdfs" if use_primitive_urdfs else "", - "--task", - task, - "--run_name", - run_name, - ] - cmd = [c for c in cmd if c] # drop empty - if eval_video_only: - cmd.append("--eval_video_only") - if video_length is not None: - cmd.extend(["--video_length", str(video_length)]) - if video_interval is not None: - cmd.extend(["--video_interval", str(video_interval)]) - if eval_episodes_per_save > 0: - cmd.extend(["--eval_episodes_per_save", str(eval_episodes_per_save)]) - if resume_from: - cmd.extend(["--resume", "--checkpoint", resume_from]) - if seed is not None: - cmd.extend(["--seed", str(seed)]) - if motion_file is not None: - cmd.extend(["--motion_file", motion_file]) - if num_envs is not None: - cmd.extend(["--num_envs", str(num_envs)]) - if max_iterations is not None: - cmd.extend(["--max_iterations", str(max_iterations)]) - if logger: - cmd.extend(["--logger", logger]) - if log_project_name: - cmd.extend(["--log_project_name", log_project_name]) - cmd.extend(overrides_to_cli(overrides)) - return cmd - - -def build_eval_command( - overrides: dict[str, Any], - *, - checkpoint: str | None = None, - seed: int | None = None, - motion_file: str | None = None, - num_envs: int | None = None, - video: bool = False, - video_length: int | None = None, - eval_episodes: int | None = None, - task: str = "Sharpa-V2P-v0", - logger: str | None = None, - log_project_name: str | None = None, - use_primitive_urdfs: bool = False, - real_time: bool = False, -) -> list[str]: - """Build eval.py command as list of args. - - Mirrors build_train_command but only forwards flags eval.py understands. - Hydra-style train_overrides are forwarded verbatim so configs can scope - e.g. motion_start_frame / motion_end_frame uniformly across train + eval. - """ - cmd = [ - "python", - "scripts/rsl_rl/eval.py", - "--video" if video else "", - "--use_primitive_urdfs" if use_primitive_urdfs else "", - "--real-time" if real_time else "", - "--task", - task, - ] - cmd = [c for c in cmd if c] - if checkpoint: - cmd.extend(["--checkpoint", checkpoint]) - if seed is not None: - cmd.extend(["--seed", str(seed)]) - if motion_file is not None: - cmd.extend(["--motion_file", motion_file]) - if num_envs is not None: - cmd.extend(["--num_envs", str(num_envs)]) - if video_length is not None: - cmd.extend(["--video_length", str(video_length)]) - if eval_episodes is not None: - cmd.extend(["--eval_episodes", str(eval_episodes)]) - if logger: - cmd.extend(["--logger", logger]) - if log_project_name: - cmd.extend(["--log_project_name", log_project_name]) - cmd.extend(overrides_to_cli(overrides)) - return cmd - - -def make_entry_script( - run_name: str, - overrides: dict[str, Any], - *, - resume_from: str | None = None, - seed: int | None = None, - motion_file: str | None = None, - num_envs: int | None = None, - max_iterations: int | None = None, - use_timestamp: bool = True, - video: bool = True, - eval_video_only: bool = False, - video_length: int | None = None, - video_interval: int | None = None, - eval_episodes_per_save: int = 0, - task: str = "Sharpa-V2P-v0", - logger: str = "wandb", - log_project_name: str = "v2p_hands", - zero_actor: bool = False, - use_primitive_urdfs: bool = False, - urdfs_src_path: str | None = None, -) -> str: - """Generate /tmp/entry.sh content for OSMO.""" - # Pass run_name (suffix only) to train; train.py adds its own timestamp to avoid duplication. - lines = [ - "set -ex", - "", - ] - if urdfs_src_path: - lines += [ - # Generate all TACO rigid URDFs + visual STL meshes from *_cm.obj files in - # the image. Produces workspace assets/urdfs/taco/ + assets/meshes/taco/ with - # correct relative mesh paths — no OSMO cp needed. - "python scripts/generate_rigid_urdfs.py --dataset taco", - "", - ] - cmd = build_train_command( - run_name, - overrides, - resume_from=resume_from, - seed=seed, - motion_file=motion_file, - num_envs=num_envs, - max_iterations=max_iterations, - video=video, - eval_video_only=eval_video_only, - video_length=video_length, - video_interval=video_interval, - eval_episodes_per_save=eval_episodes_per_save, - task=task, - logger=logger, - log_project_name=log_project_name, - zero_actor=zero_actor, - use_primitive_urdfs=use_primitive_urdfs, - ) - cmd_str = " \\\n ".join(cmd) - lines.append(cmd_str) - return "\n".join(lines) diff --git a/robotic_grounding/pyproject.toml b/robotic_grounding/pyproject.toml deleted file mode 100644 index 149908c6..00000000 --- a/robotic_grounding/pyproject.toml +++ /dev/null @@ -1,200 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - -[build-system] -requires = ["setuptools", "toml"] -build-backend = "setuptools.build_meta" - -[tool.isort] - -atomic = true -profile = "black" -line_length = 120 -py_version = 311 -skip_glob = ["docs/*", "logs/*"] -group_by_package = true - -sections = [ - "FUTURE", - "STDLIB", - "THIRDPARTY", - "ISAACLABPARTY", - "FIRSTPARTY", - "LOCALFOLDER", -] -extra_standard_library = [ - "numpy", - "h5py", - "open3d", - "torch", - "tensordict", - "matplotlib", - "gymnasium", - "gym", - "scipy", - "hid", - "yaml", - "prettytable", - "toml", - "trimesh", - "tqdm", - "psutil", -] -known_thirdparty = [ - "isaacsim.core.api", - "omni.replicator.core", - "pxr", - "omni.kit.*", - "warp", - "carb", - "Semantics", -] -known_isaaclabparty = [ - "isaaclab", - "isaaclab_tasks", - "isaaclab_assets", - "isaaclab_mimic", - "isaaclab_rl" -] - -# Modify the following to include the package names of your first-party code -known_firstparty = "robotic_grounding" -known_local_folder = "config" - -[tool.pyright] - -exclude = [ - "**/__pycache__", - "**/docs", - "**/logs", - ".git", - ".vscode", -] - -typeCheckingMode = "basic" -pythonPlatform = "Linux" -enableTypeIgnoreComments = true - -# This is required as the CI pre-commit does not download the module (i.e. numpy, torch, prettytable) -# Therefore, we have to ignore missing imports -reportMissingImports = "none" -# This is required to ignore for type checks of modules with stubs missing. -reportMissingModuleSource = "none" # -> most common: prettytable in mdp managers - -reportGeneralTypeIssues = "none" # -> raises 218 errors (usage of literal MISSING in dataclasses) -reportOptionalMemberAccess = "warning" # -> raises 8 errors -reportPrivateUsage = "warning" - -[tool.ruff] - -line-length = 120 - -target-version = "py311" - -[tool.ruff.lint] - -dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" - -select = [ - "ANN", # annotations - "N", # naming conventions - "D", # docstrings - "B", # flake8 bugbear - "E", # pycodestyle errors - "F", # Pyflakes rules - "I", # isort formatting - "PLC", # Pylint convention warnings - "PLE", # Pylint errors - "PLR", # Pylint refactor recommendations - "PLW", # Pylint warnings -] -ignore = [ - "ANN401", # Dynamically typed expressions (typing.Any) are disallowed - "D100", # missing docstring in public module - "D104", # missing docstring in public package - "D203", # blank line before class docstring - "D211", # no blank line before class - "D212", # multi-line docstring summary at first line - "D213", # multi-line docstring summary at second line - "E741", # Ambiguous variable name. (l, O, or I) - "E501", # Line too long. - "E721", # Do not compare types, use `isinstance()`. - "F722", # Forward annotation false positive from jaxtyping. Should be caught by pyright. - "F821", # Forward annotation false positive from jaxtyping. Should be caught by pyright. - "N803", # Argument name should be lowercase - "N806", # Variable in function should be lowercase - "N812", # lowercase imported as non-lowercase - "N817", # CamelCase imported as acronym - "PLR0911", # Too many return statements - "PLR0912", # Too many branches (conditional statements) - "PLR0913", # Too many arguments in function definition - "PLR0915", # Too many statements in function - "PLR2004", # magic numbers -] - -[tool.ruff.lint.isort] -known-third-party = ["wandb"] - -[tool.ruff.lint.mccabe] -# Unlike Flake8, default to a complexity level of 10. -max-complexity = 10 - -[tool.ruff.lint.pydocstyle] -convention = "google" - -[tool.ruff.lint.flake8-bugbear] -extend-immutable-calls = ["gtsam.Pose3"] - -##################################################################################### -# Pixi (You should use Docker, this is not maintained. For reference only.) -##################################################################################### - -[project] -name = "robotic_grounding" -version = "0.1.0" -description = "IsaacLab for robotic grounding training code." -authors = [ - {name = "Xinghao Zhu", email = "xinghaoz@nvidia.com"}, -] -requires-python = ">=3.11,<3.12" - -[tool.pixi.workspace] -name = "robotic_grounding" -channels = ["conda-forge"] -platforms = ["linux-64"] - -[tool.pixi.system-requirements] -cuda = "12" -libc = { family = "glibc", version = "2.35" } - -[tool.setuptools] -packages = ["robotic_grounding"] - -[tool.setuptools.package-dir] -"" = "source/robotic_grounding" - -[tool.pixi.pypi-options] -index-url = "https://pypi.org/simple" -extra-index-urls = ["https://pypi.nvidia.com/simple"] - -[tool.pixi.pypi-dependencies] -robotic_grounding = { path = "source/robotic_grounding", editable = true } -torch = { version = ">=2.7.0", index = "https://download.pytorch.org/whl/cu128" } -torchvision = { version = ">=0.20.1", index = "https://download.pytorch.org/whl/cu128" } -isaaclab = { version = "==2.2.0", extras = ["isaacsim", "all"] } -nvidia-cudnn-cu12 = { version = "*", index = "https://pypi.org/simple" } -viser = { version = ">=0.2.11,<0.3" } -pre-commit = { version = ">=4.3.0,<5" } -py-soma-x = "*" - -[tool.pixi.tasks] -install-hooks = "pre-commit install" -lint = "pre-commit run --all-files" - -[tool.pixi.activation.env] -PYTHONNOUSERSITE = "1" diff --git a/robotic_grounding/scripts/data_assessor.py b/robotic_grounding/scripts/data_assessor.py deleted file mode 100644 index 33a80672..00000000 --- a/robotic_grounding/scripts/data_assessor.py +++ /dev/null @@ -1,334 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Evaluate retargeted sequence quality using auto-discovered checks. - -Quality checks are Python files in ``scripts/data_quality_checks/`` that -define a ``check(data, **kwargs)`` function. Adding a new check is as -simple as dropping a ``.py`` file there — no other files need to change. - -Usage:: - - # Report metrics for all sequences - python scripts/data_assessor.py --input_dir - - # Output CSV - python scripts/data_assessor.py --input_dir --output_csv metrics.csv - - # Reject mode: write failed sequence IDs to file - python scripts/data_assessor.py --input_dir --reject --output_reject rejected.txt - - # Override a check threshold - python scripts/data_assessor.py --input_dir --set fitting_error.threshold=0.08 - - # Disable a check - python scripts/data_assessor.py --input_dir --disable fitting_error - - # Run only specific checks - python scripts/data_assessor.py --input_dir --checks fitting_error,ik_task_error -""" - -from __future__ import annotations - -import argparse -import csv -import inspect -import json -import re -import sys -from pathlib import Path -from typing import Callable - -import pyarrow.parquet as pq - -# Add the scripts directory to the path so data_quality_checks can be imported -SCRIPTS_DIR = Path(__file__).resolve().parent -sys.path.insert(0, str(SCRIPTS_DIR)) - -from data_quality_checks import discover_checks # noqa: E402 - - -def _parse_overrides(set_args: list[str]) -> dict[str, dict[str, float | str]]: - """Parse ``--set check_name.param=value`` args into nested dict. - - Returns: - ``{check_name: {param: value}}`` - """ - overrides: dict[str, dict[str, float | str]] = {} - for arg in set_args or []: - if "=" not in arg: - print(f"Warning: ignoring malformed --set arg: {arg}", file=sys.stderr) - continue - key, val = arg.split("=", 1) - parts = key.split(".", 1) - if len(parts) != 2: - print( - f"Warning: --set must be check_name.param=value, got: {arg}", - file=sys.stderr, - ) - continue - check_name, param = parts - try: - overrides.setdefault(check_name, {})[param] = float(val) - except ValueError: - overrides.setdefault(check_name, {})[param] = val - return overrides - - -def _find_parquet_dirs(input_dir: Path) -> list[tuple[str, Path]]: - """Find all sequence partition directories under input_dir. - - Returns: - List of ``(sequence_id, partition_dir)`` tuples. - """ - results: list[tuple[str, Path]] = [] - for seq_dir in sorted(input_dir.glob("sequence_id=*")): - if not seq_dir.is_dir(): - continue - seq_id = seq_dir.name.replace("sequence_id=", "") - # Find first robot_name partition - robot_dirs = list(seq_dir.glob("robot_name=*")) - if robot_dirs: - results.append((seq_id, robot_dirs[0])) - else: - # Parquet files directly in sequence_id dir - results.append((seq_id, seq_dir)) - return results - - -def _load_parquet_data(parquet_dir: Path) -> dict | None: - """Read a single partitioned Parquet directory into a dict.""" - parquet_files = list(parquet_dir.glob("*.parquet")) - if not parquet_files: - return None - try: - return pq.read_table(str(parquet_files[0])).to_pydict() - except Exception as e: - print(f"Warning: failed to read {parquet_files[0]}: {e}", file=sys.stderr) - return None - - -def _run_check( - check_fn: Callable[..., dict], - data: dict, - overrides: dict[str, float | str], - seq_dir: Path | None = None, -) -> dict: - """Run a single check with optional parameter overrides. - - Checks that declare a ``seq_dir`` parameter receive the sequence's - partition directory — the ``robot_name=`` dir — so filesystem-aware - checks (e.g. sentinel-based ones) can read sibling files. - """ - sig = inspect.signature(check_fn) - kwargs = {} - for param_name, param in sig.parameters.items(): - if param_name == "data": - continue - if param_name == "seq_dir": - kwargs[param_name] = seq_dir - continue - if param_name in overrides: - kwargs[param_name] = type(param.default)(overrides[param_name]) - elif param.default is not inspect.Parameter.empty: - kwargs[param_name] = param.default - return check_fn(data, **kwargs) - - -def main() -> None: - """CLI entry point.""" - parser = argparse.ArgumentParser( - description="Evaluate retargeted sequence quality.", - formatter_class=argparse.RawDescriptionHelpFormatter, - ) - parser.add_argument( - "--input_dir", - type=Path, - required=True, - help="Directory containing processed Parquet partitions.", - ) - parser.add_argument( - "--output_csv", - type=Path, - default=None, - help="Write per-sequence metrics to CSV.", - ) - parser.add_argument( - "--output_json", - type=Path, - default=None, - help="Write per-sequence metrics to JSON.", - ) - parser.add_argument( - "--reject", - action="store_true", - help="Enable rejection mode.", - ) - parser.add_argument( - "--output_reject", - type=Path, - default=None, - help="Write rejected sequence IDs to file (one per line).", - ) - parser.add_argument( - "--checks", - type=str, - default=None, - help="Comma-separated list of checks to run (default: all).", - ) - parser.add_argument( - "--disable", - type=str, - default=None, - help="Comma-separated list of checks to disable.", - ) - parser.add_argument( - "--set", - action="append", - dest="set_args", - metavar="CHECK.PARAM=VALUE", - help="Override a check parameter (e.g., fitting_error.threshold=0.08).", - ) - parser.add_argument( - "--sequence_id", - type=str, - default=None, - help="Evaluate a single sequence.", - ) - parser.add_argument( - "--sequence_pattern", - type=str, - default=None, - help="Regex pattern to filter sequences.", - ) - args = parser.parse_args() - - # Discover checks - all_checks = discover_checks() - if not all_checks: - print( - "No quality checks found in scripts/data_quality_checks/", file=sys.stderr - ) - sys.exit(1) - - # Filter checks - if args.checks: - selected = set(args.checks.split(",")) - all_checks = {k: v for k, v in all_checks.items() if k in selected} - if args.disable: - disabled = set(args.disable.split(",")) - all_checks = {k: v for k, v in all_checks.items() if k not in disabled} - - check_names = sorted(all_checks) - overrides = _parse_overrides(args.set_args) - - print(f"Running {len(check_names)} checks: {', '.join(check_names)}") - print(f"Input: {args.input_dir}\n") - - # Find sequences - sequences = _find_parquet_dirs(args.input_dir) - if not sequences: - print(f"No sequences found in {args.input_dir}", file=sys.stderr) - sys.exit(1) - - # Apply sequence filters - if args.sequence_id: - sequences = [(sid, p) for sid, p in sequences if sid == args.sequence_id] - if args.sequence_pattern: - pat = re.compile(args.sequence_pattern) - sequences = [(sid, p) for sid, p in sequences if pat.match(sid)] - - # Run checks - results: list[dict] = [] - rejected: list[str] = [] - pass_counts = {name: 0 for name in check_names} - total = len(sequences) - - for seq_id, parquet_dir in sequences: - data = _load_parquet_data(parquet_dir) - if data is None: - print(f" SKIP {seq_id}: could not read parquet") - continue - - row: dict = {"sequence_id": seq_id} - all_pass = True - - for check_name in check_names: - check_fn = all_checks[check_name] - check_overrides = overrides.get(check_name, {}) - result = _run_check(check_fn, data, check_overrides, seq_dir=parquet_dir) - - row[f"{check_name}_score"] = result["score"] - row[f"{check_name}_pass"] = result["pass"] - row[f"{check_name}_reason"] = result["reason"] - - if result["pass"]: - pass_counts[check_name] += 1 - else: - all_pass = False - - row["pass_all"] = all_pass - results.append(row) - - if not all_pass: - rejected.append(seq_id) - - # Console summary - print(f"{'='*60}") - print(f"Results: {total} sequences evaluated\n") - - print(f"{'Check':<25} {'Pass Rate':>12} {'Mean Score':>12}") - print(f"{'-'*25} {'-'*12} {'-'*12}") - for name in check_names: - pass_rate = pass_counts[name] / total if total > 0 else 0 - scores = [r[f"{name}_score"] for r in results] - mean_score = sum(scores) / len(scores) if scores else 0 - print(f"{name:<25} {pass_rate:>11.1%} {mean_score:>12.4f}") - - passed = sum(1 for r in results if r["pass_all"]) - print(f"\nOverall: {passed}/{total} sequences pass all checks") - - if rejected: - print(f"\nRejected ({len(rejected)}):") - for seq_id in rejected[:20]: - row = next(r for r in results if r["sequence_id"] == seq_id) - fails = [n for n in check_names if not row[f"{n}_pass"]] - print(f" {seq_id}: failed {', '.join(fails)}") - if len(rejected) > 20: - print(f" ... and {len(rejected) - 20} more") - - # Write CSV - if args.output_csv: - fieldnames = ( - ["sequence_id"] - + [f"{n}_{s}" for n in check_names for s in ("score", "pass", "reason")] - + ["pass_all"] - ) - with open(args.output_csv, "w", newline="") as f: - writer = csv.DictWriter(f, fieldnames=fieldnames) - writer.writeheader() - writer.writerows(results) - print(f"\nCSV written to {args.output_csv}") - - # Write JSON - if args.output_json: - with open(args.output_json, "w") as f: - json.dump(results, f, indent=2) - print(f"JSON written to {args.output_json}") - - # Write reject list - if args.reject and args.output_reject: - with open(args.output_reject, "w") as f: - for seq_id in rejected: - f.write(f"{seq_id}\n") - print( - f"Rejected sequences written to {args.output_reject} ({len(rejected)} IDs)" - ) - - # Exit with non-zero if any rejected (useful in CI) - if args.reject and rejected: - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/data_quality_checks/__init__.py b/robotic_grounding/scripts/data_quality_checks/__init__.py deleted file mode 100644 index 2283f624..00000000 --- a/robotic_grounding/scripts/data_quality_checks/__init__.py +++ /dev/null @@ -1,71 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Auto-discovery for data quality checks. - -Each ``.py`` file in this directory that defines a ``check()`` function is -automatically registered as a quality check. The file name (without extension) -becomes the check name. - -Check protocol:: - - def check(data: dict, **kwargs) -> dict: - '''Evaluate one sequence. - -Args: - data: Parquet row as a dict (from ``pq.read_table().to_pydict()``). - Each value is a list with one element per row. - **kwargs: Overridable parameters (e.g. ``threshold``). - -Returns: - {"pass": bool, "score": float, "reason": str} - ''' -""" - -from __future__ import annotations - -import importlib -import importlib.util -from pathlib import Path -from typing import Any, Callable - -CheckFn = Callable[..., dict[str, Any]] - -_CHECKS_DIR = Path(__file__).resolve().parent - - -def discover_checks() -> dict[str, CheckFn]: - """Scan this directory for modules that expose a ``check()`` function. - - Returns: - Dict mapping check name to the callable ``check`` function. - """ - checks: dict[str, CheckFn] = {} - for py_file in sorted(_CHECKS_DIR.glob("*.py")): - if py_file.name.startswith("_"): - continue - module_name = py_file.stem - spec = importlib.util.spec_from_file_location( - f"data_quality_checks.{module_name}", str(py_file) - ) - if spec is None or spec.loader is None: - continue - module = importlib.util.module_from_spec(spec) - try: - spec.loader.exec_module(module) - except Exception: - continue - fn = getattr(module, "check", None) - if callable(fn): - checks[module_name] = fn - return checks - - -def get_check_description(check_fn: CheckFn) -> str: - """Return the first line of the check module's docstring, or ''.""" - doc = getattr(check_fn, "__doc__", "") or "" - # Prefer the module-level docstring if available - if hasattr(check_fn, "__globals__") and "__doc__" in check_fn.__globals__: - module_doc = check_fn.__globals__["__doc__"] - if module_doc: - doc = module_doc - return doc.strip().split("\n")[0] if doc else "" diff --git a/robotic_grounding/scripts/data_quality_checks/arctic_support_disks.py b/robotic_grounding/scripts/data_quality_checks/arctic_support_disks.py deleted file mode 100644 index 61c931fe..00000000 --- a/robotic_grounding/scripts/data_quality_checks/arctic_support_disks.py +++ /dev/null @@ -1,70 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reject arctic sequences with more than one reconstructed support surface. - -Arctic objects are articulated (root + lid/lever) but should only produce a -single support disk — the root body's footprint. Multiple disks indicate a -regression in ``scripts/reconstruct_support_surfaces.py`` (the ``top`` body -leaking through, or cross-body merge failing to collapse stacked footprints). - -Non-arctic sequences skip this check vacuously. -""" - -from __future__ import annotations - -from pathlib import Path - -MAX_SUPPORT_DISKS = 1 -CYLINDER_TOKEN = "def Cylinder " - - -def _dataset_from_seq_dir(seq_dir: Path) -> str | None: - """Infer dataset from the ``_processed`` grandparent of ``seq_dir``. - - Expected layout: ``/_processed/sequence_id=/robot_name=``. - """ - try: - processed = seq_dir.parent.parent - except AttributeError: - return None - suffix = "_processed" - return processed.name[: -len(suffix)] if processed.name.endswith(suffix) else None - - -def _support_usda(seq_dir: Path) -> Path | None: - """Return the path to ``reconstructed_stage/_support.usda`` or None.""" - try: - seq_id = seq_dir.parent.name.split("=", 1)[1] - root = seq_dir.parent.parent.parent - except IndexError: - return None - return root / "reconstructed_stage" / f"{seq_id}_support.usda" - - -def check(data: dict, seq_dir: Path | None = None) -> dict: - """Pass unless an arctic sequence has ≥ 2 ``Cylinder`` prims in its support USD.""" - if seq_dir is None: - return {"pass": True, "score": 0.0, "reason": "no seq_dir provided"} - seq_dir = Path(seq_dir) - dataset = _dataset_from_seq_dir(seq_dir) - if dataset != "arctic": - return {"pass": True, "score": 0.0, "reason": f"skipped (dataset={dataset})"} - - usda = _support_usda(seq_dir) - if usda is None or not usda.exists(): - return {"pass": True, "score": 0.0, "reason": "no support USD"} - - try: - n = usda.read_text().count(CYLINDER_TOKEN) - except OSError as exc: - return { - "pass": True, - "score": 0.0, - "reason": f"could not read {usda.name}: {exc}", - } - - return { - "pass": n <= MAX_SUPPORT_DISKS, - "score": float(n), - "reason": f"support_disks={n} (max={MAX_SUPPORT_DISKS})", - } diff --git a/robotic_grounding/scripts/data_quality_checks/dummy_agent_success.py b/robotic_grounding/scripts/data_quality_checks/dummy_agent_success.py deleted file mode 100644 index 05375fa9..00000000 --- a/robotic_grounding/scripts/data_quality_checks/dummy_agent_success.py +++ /dev/null @@ -1,40 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reject sequences where dummy_agent did not complete a clean render. - -The sentinel file (`.dummy_agent_ok`) is written by -``scripts/rsl_rl/dummy_agent.py`` only after (a) the recording loop -finishes end-to-end, (b) ``env.close()`` returns — which is where -``gymnasium.wrappers.RecordVideo`` actually flushes the MP4 via moviepy, -because its in-loop check is a strict ``len(recorded_frames) > -video_length`` that never fires when the caller runs exactly -``video_length`` steps — and (c) the rendered MP4 is on disk with -non-zero size. CUDA assert, uncaught exception, Omniverse-shutdown -deadlock, silent moviepy failure, or ``timeout --signal=KILL`` all -prevent the touch, so sentinel *presence* is a strict proof the sim -rendered the whole sequence AND the MP4 was persisted. -""" - -from __future__ import annotations - -from pathlib import Path - -MARKER = ".dummy_agent_ok" - - -def check(data: dict, seq_dir: Path | None = None) -> dict: - """Pass iff ``/.dummy_agent_ok`` exists. - - ``seq_dir`` is the ``robot_name=`` partition dir — exactly where - the workflow writes the sentinel (see ``workflow/retarget.yaml`` Stage 5). - When running the assessor standalone without ``seq_dir`` wired in, this - check passes vacuously so it doesn't block non-workflow invocations. - """ - if seq_dir is None: - return {"pass": True, "score": 1.0, "reason": "no seq_dir provided"} - found = (Path(seq_dir) / MARKER).exists() - return { - "pass": found, - "score": float(found), - "reason": "sentinel present" if found else f"missing {MARKER}", - } diff --git a/robotic_grounding/scripts/data_quality_checks/hand_penetration.py b/robotic_grounding/scripts/data_quality_checks/hand_penetration.py deleted file mode 100644 index a255534c..00000000 --- a/robotic_grounding/scripts/data_quality_checks/hand_penetration.py +++ /dev/null @@ -1,21 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reject sequences where the retargeted robot hand penetrates an object or itself by > 2 cm. - -Wraps the capsule-based penetration check from ``scripts/filter_penetrations.py``. -Checks both hand-object penetration (convex hull signed-distance) and hand-hand -penetration (analytic capsule-capsule distance). Hollow objects (AR glasses, -mugs, bowls) are skipped via hull_volume_ratio filtering to avoid false positives. - -score = max penetration depth in centimetres across all sampled frames. -""" - -from __future__ import annotations - -import sys -from pathlib import Path - -# filter_penetrations.py lives one directory up (scripts/) -sys.path.insert(0, str(Path(__file__).resolve().parent.parent)) - -from filter_penetrations import check # noqa: F401, E402 diff --git a/robotic_grounding/scripts/download_sources/dexycb.sh b/robotic_grounding/scripts/download_sources/dexycb.sh deleted file mode 100644 index 6d18c25e..00000000 --- a/robotic_grounding/scripts/download_sources/dexycb.sh +++ /dev/null @@ -1,68 +0,0 @@ -#!/bin/bash -# Download DexYCB dataset (NVLabs, CVPR 2021) and upload to CSS. -# -# DexYCB is distributed as a single ~119 GB archive on Google Drive -# (dex-ycb-20210415.tar.gz) that contains all 10 subjects + calibration + -# models + BOP annotations. The URL is gated behind a click-through license -# at https://dex-ycb.github.io/, so there is no programmatic auth flow — -# supply the Google Drive file ID via the DEXYCB_GDRIVE_ID env var after -# you've accepted the license on the provider page in your browser. -# -# Run inside an OSMO dev_env container (2 TB /tmp storage recommended). -# -# Required env vars: -# CSS_ENDPOINT_URL, CSS_ACCESS_KEY, CSS_SECRET_KEY -# DEXYCB_GDRIVE_ID — Google Drive file ID of dex-ycb-20210415.tar.gz. - -set -ex - -: "${CSS_ENDPOINT_URL:?CSS_ENDPOINT_URL must be set}" -: "${CSS_ACCESS_KEY:?CSS_ACCESS_KEY must be set}" -: "${CSS_SECRET_KEY:?CSS_SECRET_KEY must be set}" -: "${DEXYCB_GDRIVE_ID:?DEXYCB_GDRIVE_ID must be set (Google Drive file ID of dex-ycb-20210415.tar.gz)}" - -DATASET=dexycb -STAGING=/tmp/${DATASET} -CSS_DEST=s3://datasets/v2d/human_motion_data/${DATASET}/dataset/ -ARCHIVE=${STAGING}/dex-ycb-20210415.tar.gz - -mkdir -p "${STAGING}/extracted" -cd "${STAGING}" - -# 1. Tools -apt-get update -qq && apt-get install -y -qq curl tar -if ! command -v gdown &>/dev/null; then - pip install -q gdown -fi -if ! command -v aws &>/dev/null; then - if command -v pip &>/dev/null; then pip install -q awscli - else python3 -m pip install -q awscli; fi -fi - -# 2. Download the single 119 GB archive (gdown resolves the Drive confirm token). -if [ -s "${ARCHIVE}" ]; then - echo "[dexycb] archive already present at ${ARCHIVE}" -else - echo "[dexycb] downloading dex-ycb-20210415.tar.gz via gdown (this will take a while)" - gdown --fuzzy --id "${DEXYCB_GDRIVE_ID}" -O "${ARCHIVE}" -fi - -# 3. Extract into extracted/. -echo "[dexycb] extracting ${ARCHIVE}" -tar -xzf "${ARCHIVE}" -C extracted - -echo "[dexycb] extracted layout:" -ls -la extracted/ - -# 4. Upload to CSS, excluding the RGB (*.jpg) + aligned depth (*.png) frames -# we don't use for retargeting. Drop the excludes if you need them later. -aws configure set default.s3.max_concurrent_requests 100 -aws configure set default.s3.max_queue_size 10000 -export AWS_ACCESS_KEY_ID=${CSS_ACCESS_KEY} -export AWS_SECRET_ACCESS_KEY=${CSS_SECRET_KEY} -aws s3 sync extracted/ "${CSS_DEST}" \ - --endpoint-url "${CSS_ENDPOINT_URL}" --region us-east-1 \ - --exclude "*.jpg" --exclude "*.png" - -echo "[dexycb] upload complete." -echo " verify from host: python scripts/list_css_sequences.py --dataset dexycb --stage raw" diff --git a/robotic_grounding/scripts/download_sources/grab.sh b/robotic_grounding/scripts/download_sources/grab.sh deleted file mode 100644 index 8e398d87..00000000 --- a/robotic_grounding/scripts/download_sources/grab.sh +++ /dev/null @@ -1,114 +0,0 @@ -#!/bin/bash -# Download GRAB dataset (ECCV 2020) and upload to CSS. -# -# GRAB distributes as 10 per-subject ZIPs + an object_meshes ZIP, gated behind -# an account on https://grab.is.tue.mpg.de/. Downloads are authenticated via a -# session cookie — the "URLs" on the downloads page only work while logged in, -# so a plain `curl ` from a new shell gets redirected to the login page. -# -# Flow: -# 1. POST email+password to https://grab.is.tue.mpg.de/login.php to mint a -# session cookie. -# 2. curl each download.php URL with that cookie jar. -# -# This script then: -# - Fetches the ZIPs into /tmp/grab/zips -# - Extracts them into the conventional layout (s1/, s2/, ..., s10/, object_meshes/) -# - Uploads the extracted data to CSS at -# s3://datasets/v2d/human_motion_data/grab/dataset/ -# -# Run inside an OSMO dev_env container with CSS + MPG credentials exported. -# -# Required env vars: -# CSS_ENDPOINT_URL, CSS_ACCESS_KEY, CSS_SECRET_KEY -# GRAB_USERNAME — email you registered on grab.is.tue.mpg.de -# GRAB_PASSWORD — password for that account -# -# Optional env vars: -# GRAB_SUBJECTS — space-separated list of subjects to fetch (default: "s1 s2 s3 s4 s5 s6 s7 s8 s9 s10") -# SKIP_UPLOAD=1 — only download + extract; don't push to CSS - -set -ex - -: "${CSS_ENDPOINT_URL:?CSS_ENDPOINT_URL must be set}" -: "${CSS_ACCESS_KEY:?CSS_ACCESS_KEY must be set}" -: "${CSS_SECRET_KEY:?CSS_SECRET_KEY must be set}" -: "${GRAB_USERNAME:?GRAB_USERNAME must be set (registered email on grab.is.tue.mpg.de)}" -: "${GRAB_PASSWORD:?GRAB_PASSWORD must be set}" - -DATASET=grab -STAGING=/tmp/${DATASET} -CSS_DEST=s3://datasets/v2d/human_motion_data/${DATASET}/dataset/ -GRAB_SUBJECTS="${GRAB_SUBJECTS:-s1 s2 s3 s4 s5 s6 s7 s8 s9 s10}" - -mkdir -p "${STAGING}/zips" "${STAGING}/extracted" -cd "${STAGING}" - -# 1. Install tools -apt-get update -qq && apt-get install -y -qq unzip curl -if ! command -v aws &>/dev/null; then - if command -v pip &>/dev/null; then pip install -q awscli - else python3 -m pip install -q awscli; fi -fi - -# 2. Login to grab.is.tue.mpg.de to mint a session cookie -COOKIES="${STAGING}/mpg_cookies.txt" -rm -f "${COOKIES}" -echo "[grab] logging in as ${GRAB_USERNAME}" -curl -s -c "${COOKIES}" -b "${COOKIES}" \ - -d "username=${GRAB_USERNAME}&password=${GRAB_PASSWORD}" \ - "https://grab.is.tue.mpg.de/login.php" >/dev/null - -# Verify login succeeded — the session cookie is named PHPSESSID and should -# appear in the cookie jar. -if ! grep -q PHPSESSID "${COOKIES}"; then - echo "[grab] ERROR: login did not return a session cookie. Check credentials." >&2 - exit 1 -fi - -# Build the list of ZIPs to fetch. The download endpoint is the same for all -# files on the downloads page. -ZIP_FILES=() -for s in ${GRAB_SUBJECTS}; do - ZIP_FILES+=("grab__${s}.zip") -done -ZIP_FILES+=("object_meshes.zip") - -# 3. Download each ZIP through the authenticated session -DOWNLOAD_BASE="https://download.is.tue.mpg.de/download.php?domain=grab&resume=1&sfile=" -for fname in "${ZIP_FILES[@]}"; do - out="zips/${fname}" - if [ -s "${out}" ]; then - echo "[grab] skipping ${fname} (already downloaded)" - continue - fi - url="${DOWNLOAD_BASE}${fname}" - echo "[grab] downloading ${fname}" - curl -L --fail -b "${COOKIES}" -c "${COOKIES}" -o "${out}" "${url}" -done - -# 4. Extract each ZIP into extracted/ -cd extracted -for z in ../zips/*.zip; do - echo "[grab] extracting $(basename "$z")" - unzip -q -o "$z" -done - -echo "[grab] extracted layout:" -ls -la - -# 5. Upload to CSS. GRAB is thousands of small .npz files — use high -# concurrency so per-file API overhead doesn't dominate. -if [ "${SKIP_UPLOAD:-0}" != "1" ]; then - aws configure set default.s3.max_concurrent_requests 100 - aws configure set default.s3.max_queue_size 10000 - export AWS_ACCESS_KEY_ID=${CSS_ACCESS_KEY} - export AWS_SECRET_ACCESS_KEY=${CSS_SECRET_KEY} - aws s3 sync ./ "${CSS_DEST}" \ - --endpoint-url "${CSS_ENDPOINT_URL}" --region us-east-1 - - echo "[grab] upload complete." - echo " verify from host: python scripts/list_css_sequences.py --dataset grab --stage raw" -else - echo "[grab] SKIP_UPLOAD=1 set — skipping CSS upload. Data staged at ${STAGING}/extracted/" -fi diff --git a/robotic_grounding/scripts/fetch_object_assets.py b/robotic_grounding/scripts/fetch_object_assets.py deleted file mode 100644 index 1153e528..00000000 --- a/robotic_grounding/scripts/fetch_object_assets.py +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Fetch a dataset's object assets (rigid URDFs + meshes) from CSS/swift. - -The inverse of ``upload_object_assets.py``. Per-dataset object URDFs + meshes are -no longer committed to the repo (they are large and regenerable); they live on -CSS under:: - - .../human_motion_data//object_assets/urdfs// - .../human_motion_data//object_assets/meshes// - -This downloads them back into the committed layout the pipeline expects:: - - source/robotic_grounding/robotic_grounding/assets/urdfs// - source/robotic_grounding/robotic_grounding/assets/meshes// - -so the URDFs' ``../../meshes/...`` sibling refs resolve, generate_rigid_urdfs -skips (idempotent) and Isaac Sim can load the meshes for retarget / training / -replay. Run it once after a fresh clone (or in the image build) before using a -dataset locally. - -Usage:: - - source scripts/setup_css_env.sh # or have aws creds for pdx.s8k.io - python scripts/fetch_object_assets.py --dataset taco - python scripts/fetch_object_assets.py --dataset all - python scripts/fetch_object_assets.py --dataset taco --dry-run -""" - -from __future__ import annotations - -import argparse -import subprocess -import sys -from pathlib import Path - -ASSET_DIR = ( - Path(__file__).resolve().parents[1] - / "source/robotic_grounding/robotic_grounding/assets" -) -# TODO(public-release): internal NVIDIA CSS endpoint + bucket — replace before open-sourcing. -ENDPOINT = "https://pdx.s8k.io" -BUCKET = "s3://datasets/v2d/human_motion_data" - -# Datasets whose object assets are hosted as object_assets/ on CSS. h2o/grab/ -# dexycb are intentionally absent — their meshes live inside the raw dataset and -# the loaders read them from there, so there is nothing to fetch here. -DATASETS = ("arctic", "taco", "oakink2", "hot3d") - - -def _sync(src: str, dst: Path, dry_run: bool, includes: list[str] | None = None) -> int: - dst.mkdir(parents=True, exist_ok=True) - cmd = [ - "aws", - "s3", - "sync", - src, - str(dst), - "--endpoint-url", - ENDPOINT, - "--region", - "us-east-1", - ] - # Restrict to specific filenames: exclude everything, then re-include each - # pattern (used by CI to grab only the handful of objects a test needs). - if includes: - cmd += ["--exclude", "*"] - for pat in includes: - cmd += ["--include", pat] - print("+", " ".join(cmd)) - if dry_run: - return 0 - return subprocess.run(cmd, check=False).returncode - - -def _fetch_dataset( - ds: str, - dry_run: bool, - kinds: tuple[str, ...] = ("urdfs", "meshes"), - includes: list[str] | None = None, -) -> None: - for kind in kinds: - src = f"{BUCKET}/{ds}/object_assets/{kind}/{ds}/" - dst = ASSET_DIR / kind / ds - rc = _sync(src, dst, dry_run, includes) - if rc != 0: - print( - f"ERROR: failed to fetch {kind} for '{ds}' from {src} " - f"(is it uploaded? see upload_object_assets.py)", - file=sys.stderr, - ) - sys.exit(rc) - - -def main() -> None: - """Download the given dataset's object_assets (urdfs + meshes) from swift.""" - ap = argparse.ArgumentParser(description=__doc__) - ap.add_argument( - "--dataset", - required=True, - help=f"Dataset name, or 'all' for: {', '.join(DATASETS)}", - ) - ap.add_argument("--dry-run", action="store_true") - ap.add_argument( - "--meshes-only", - action="store_true", - help="Fetch only meshes/, not urdfs/ (URDFs can be regenerated locally).", - ) - ap.add_argument( - "--include", - action="append", - metavar="PATTERN", - help="Restrict to filenames matching PATTERN (repeatable); fetch the rest " - "by omitting. E.g. --include '030_cm.obj' --include '005_cm.obj'.", - ) - args = ap.parse_args() - - targets = DATASETS if args.dataset == "all" else (args.dataset,) - if args.dataset != "all" and args.dataset not in DATASETS: - print( - f"WARNING: '{args.dataset}' is not a known object_assets dataset " - f"({', '.join(DATASETS)}); trying anyway.", - file=sys.stderr, - ) - kinds = ("meshes",) if args.meshes_only else ("urdfs", "meshes") - for ds in targets: - _fetch_dataset(ds, args.dry_run, kinds=kinds, includes=args.include) - print( - f"done: fetched object_assets for {', '.join(targets)}" - f"{' (dry-run)' if args.dry_run else ''}" - ) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/filter_penetrations.py b/robotic_grounding/scripts/filter_penetrations.py deleted file mode 100644 index 70cb07c2..00000000 --- a/robotic_grounding/scripts/filter_penetrations.py +++ /dev/null @@ -1,788 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -r"""Filter HOT3D processed sequences with excessive hand penetration. - -Reads from hot3d_processed/ (partitioned Parquet) and copies sequences -with max penetration ≤ threshold to an output directory. - -Two penetration types are checked per frame: - hand-object — robot finger capsule/sphere into the object mesh - hand-hand — right hand capsule/sphere into left hand capsule/sphere - -Penetration depth is measured as the depth of surface overlap in metres. -Sequences where the per-frame maximum exceeds ``--max_penetration`` (default -0.02 m = 2 cm) in ANY frame are rejected. - -Usage ------ - python scripts/filter_penetrations.py \\ - --input_dir ~/datasets/.../hot3d_processed \\ - --output_dir ~/datasets/.../hot3d_processed_valid - - # Dry-run: report stats without copying - python scripts/filter_penetrations.py \\ - --input_dir ~/datasets/.../hot3d_processed \\ - --output_dir ~/datasets/.../hot3d_processed_valid \\ - --dry_run - - # Faster (sample every 5th frame, 8 workers) - python scripts/filter_penetrations.py \\ - --input_dir ~/datasets/.../hot3d_processed \\ - --output_dir ~/datasets/.../hot3d_processed_valid \\ - --stride 5 --num_workers 8 -""" - -from __future__ import annotations - -import argparse -import csv -import logging -import os -import shutil -import sys -import xml.etree.ElementTree as ET -from dataclasses import dataclass -from pathlib import Path - -import numpy as np -import pyarrow.parquet as pq -import trimesh -from scipy.spatial.transform import Rotation - -log = logging.getLogger(__name__) - -# --------------------------------------------------------------------------- -# URDF paths for sharpa_wave primitive collision geometry -# --------------------------------------------------------------------------- -_REPO_ROOT = Path(__file__).resolve().parents[2] # .../video_to_data -_URDF_DIR = ( - _REPO_ROOT - / "robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave" -) -_URDF_RIGHT = _URDF_DIR / "right_sharpa_wave_primitive.urdf" -_URDF_LEFT = _URDF_DIR / "left_sharpa_wave_primitive.urdf" - -# Mesh paths in the parquet use the Docker container prefix; remap to local. -_MESH_PATH_REMAPS = { - "/workspace/video_to_data": str(_REPO_ROOT), -} - - -# --------------------------------------------------------------------------- -# Collision geometry data structures -# --------------------------------------------------------------------------- - - -@dataclass -class CollisionShape: - """A primitive collision shape (sphere or capsule) attached to a robot link.""" - - link_name: str - radius: float - # For capsules: the shaft endpoints in the link's local frame. - # For spheres: both endpoints equal the sphere centre. - ep1_local: np.ndarray # (3,) - ep2_local: np.ndarray # (3,) - - -def _rpy_to_matrix(rpy: np.ndarray | list[float] | tuple[float, ...]) -> np.ndarray: - """Roll-pitch-yaw (XYZ extrinsic) → 3×3 rotation matrix.""" - return Rotation.from_euler("xyz", rpy).as_matrix() - - -def _parse_collision_shapes(urdf_path: Path) -> dict[str, CollisionShape]: - """Return {link_name: CollisionShape} from a primitive URDF.""" - tree = ET.parse(str(urdf_path)) - shapes: dict[str, CollisionShape] = {} - for link in tree.getroot().findall("link"): - name = link.get("name", "") - col = link.find("collision") - if col is None: - continue - geom = col.find("geometry") - origin = col.find("origin") - if geom is None: - continue - - xyz = ( - np.array(list(map(float, origin.get("xyz", "0 0 0").split()))) - if origin is not None - else np.zeros(3) - ) - rpy = ( - np.array(list(map(float, origin.get("rpy", "0 0 0").split()))) - if origin is not None - else np.zeros(3) - ) - R_col = _rpy_to_matrix(rpy) - - sphere = geom.find("sphere") - capsule = geom.find("capsule") - - if sphere is not None: - r = float(sphere.get("radius") or 0.0) - shapes[name] = CollisionShape(name, r, xyz.copy(), xyz.copy()) - - elif capsule is not None: - r = float(capsule.get("radius") or 0.0) - half = float(capsule.get("length") or 0.0) / 2.0 - # Capsule axis is along local Z in the collision frame; rotate by R_col, - # then add the origin offset. - axis_col = R_col @ np.array([0.0, 0.0, half]) - shapes[name] = CollisionShape( - name, - r, - xyz + axis_col, - xyz - axis_col, - ) - - return shapes - - -# --------------------------------------------------------------------------- -# Geometry helpers -# --------------------------------------------------------------------------- - - -def _quat_wxyz_to_matrix(qwxyz: list | np.ndarray) -> np.ndarray: - """[qw, qx, qy, qz] → 3×3 rotation matrix.""" - qw, qx, qy, qz = qwxyz - # scipy from_quat expects [x, y, z, w] - return Rotation.from_quat([qx, qy, qz, qw]).as_matrix() - - -def _segment_segment_distance( - p1: np.ndarray, p2: np.ndarray, q1: np.ndarray, q2: np.ndarray -) -> float: - """Minimum distance between two line segments (p1-p2) and (q1-q2).""" - d1 = p2 - p1 - d2 = q2 - q1 - r = p1 - q1 - a = np.dot(d1, d1) - e = np.dot(d2, d2) - f = np.dot(d2, r) - - # Degenerate cases - if a < 1e-10 and e < 1e-10: - return float(np.linalg.norm(r)) - if a < 1e-10: - s = 0.0 - t = np.clip(f / e, 0.0, 1.0) - else: - c = np.dot(d1, r) - if e < 1e-10: - t = 0.0 - s = np.clip(-c / a, 0.0, 1.0) - else: - b = np.dot(d1, d2) - denom = a * e - b * b - if denom != 0.0: - s = np.clip((b * f - c * e) / denom, 0.0, 1.0) - else: - s = 0.0 - t = (b * s + f) / e - if t < 0.0: - t = 0.0 - s = np.clip(-c / a, 0.0, 1.0) - elif t > 1.0: - t = 1.0 - s = np.clip((b - c) / a, 0.0, 1.0) - - cp = p1 + s * d1 - (q1 + t * d2) - return float(np.linalg.norm(cp)) - - -# --------------------------------------------------------------------------- -# Per-sequence penetration computation -# --------------------------------------------------------------------------- - - -def _remap_mesh_path(path: str) -> str: - for prefix, replacement in _MESH_PATH_REMAPS.items(): - if path.startswith(prefix): - return replacement + path[len(prefix) :] - return path - - -def _load_hull(mesh_path: str, cache: dict) -> tuple[trimesh.Trimesh | None, float]: - """Load mesh and return (convex_hull, hull_volume_ratio). - - hull_volume_ratio = hull.volume / mesh.volume. A ratio >> 1 means the - object is highly concave/open (e.g. a cup, vase, or open glasses frame) and - the convex hull is a poor proxy for the actual solid. Callers should skip - the hand-object check when this ratio exceeds a threshold. - """ - key = os.path.basename(mesh_path) - if key in cache: - return cache[key] - local_path = _remap_mesh_path(mesh_path) - if not os.path.exists(local_path): - log.warning("Mesh not found: %s", local_path) - cache[key] = (None, 0.0) - return cache[key] - try: - mesh = trimesh.load(local_path, force="mesh") - hull = mesh.convex_hull - ratio = ( - hull.volume / mesh.volume if mesh.volume and mesh.volume > 1e-10 else 999.0 - ) - cache[key] = (hull, ratio) - return cache[key] - except Exception as e: - log.warning("Failed to load mesh %s: %s", local_path, e) - cache[key] = (None, 0.0) - return cache[key] - - -class _HandShapeCache: - """Pre-computes per-link local-frame capsule endpoints for one hand.""" - - def __init__( - self, shapes: dict[str, CollisionShape], frame_names: list[str] - ) -> None: - # Ordered list of (frame_index, CollisionShape) for links that appear - # in both the URDF and the parquet frame list. - self.entries: list[tuple[int, CollisionShape]] = [] - name_to_idx = {n: i for i, n in enumerate(frame_names)} - for link_name, shape in shapes.items(): - idx = name_to_idx.get(link_name) - if idx is not None: - self.entries.append((idx, shape)) - - def world_spheres( - self, frames_t: list - ) -> list[tuple[np.ndarray, np.ndarray, float]]: - """Return [(ep1_world, ep2_world, radius), ...] for frame t's link poses.""" - result: list[tuple[np.ndarray, np.ndarray, float]] = [] - for idx, shape in self.entries: - pose = frames_t[idx] # [px, py, pz, qw, qx, qy, qz] - pos = np.array(pose[:3], dtype=float) - R = _quat_wxyz_to_matrix(pose[3:7]) - # Transform both capsule endpoints to world space - ep1_w = pos + R @ shape.ep1_local - ep2_w = pos + R @ shape.ep2_local - result.append((ep1_w, ep2_w, shape.radius)) - return result - - -def _max_hand_object_penetration( - capsules: list[tuple[np.ndarray, np.ndarray, float]], - obj_hull: trimesh.Trimesh, - obj_pos: np.ndarray, - obj_R: np.ndarray, -) -> float: - """Max penetration depth (m) of any capsule/sphere into the object hull. - - Points are transformed into the object's local frame so we can query a - single static hull instead of re-transforming the mesh every frame. - - trimesh signed_distance convention: positive = inside, negative = outside. - Penetration depth of a sphere of radius r at distance sd from surface: - depth = max(0, r + sd) - For a capsule, the representative points are both endpoints. - """ - # Collect query points and matching radii - pts = [] - radii = [] - for ep1_w, ep2_w, r in capsules: - pts.append(ep1_w) - pts.append(ep2_w) - radii.extend([r, r]) - - pts_arr = np.array(pts) - radii_arr = np.array(radii) - - # Transform to object-local frame (avoids re-transforming hull every frame) - pts_local = (obj_R.T @ (pts_arr - obj_pos).T).T - - sd = trimesh.proximity.signed_distance(obj_hull, pts_local) - # depth = max(0, r + sd) [sd positive = inside] - depths = np.maximum(0.0, radii_arr + sd) - return float(depths.max()) if len(depths) > 0 else 0.0 - - -def _max_hand_hand_penetration( - right_caps: list[tuple[np.ndarray, np.ndarray, float]], - left_caps: list[tuple[np.ndarray, np.ndarray, float]], -) -> float: - """Max capsule-capsule penetration depth between right and left hand.""" - max_pen = 0.0 - for ep1_r, ep2_r, r_r in right_caps: - for ep1_l, ep2_l, r_l in left_caps: - dist = _segment_segment_distance(ep1_r, ep2_r, ep1_l, ep2_l) - pen = max(0.0, r_r + r_l - dist) - max_pen = max(max_pen, pen) - return max_pen - - -# --------------------------------------------------------------------------- -# Per-sequence entry point (used by multiprocessing) -# --------------------------------------------------------------------------- - - -def _check_sequence( - args_tuple: tuple, -) -> dict: - """Evaluate one sequence; return result dict. - - Args: - args_tuple: (seq_dir, right_shapes, left_shapes, max_pen, hull_ratio_max, stride, hull_cache) - """ - seq_dir, right_shapes, left_shapes, max_pen, hull_ratio_max, stride, hull_cache = ( - args_tuple - ) - - seq_id = seq_dir.parent.name.replace("sequence_id=", "") - parquet_files = list(seq_dir.glob("*.parquet")) - if not parquet_files: - return { - "seq_id": seq_id, - "rejected": True, - "reason": "no_parquet", - "max_ho_pen": 0.0, - "max_hh_pen": 0.0, - "object_name": "", - "seq_dir": seq_dir, - } - - try: - data = pq.read_table(str(parquet_files[0])).to_pydict() - except Exception as e: - return { - "seq_id": seq_id, - "rejected": True, - "reason": f"read_error:{e}", - "max_ho_pen": 0.0, - "max_hh_pen": 0.0, - "object_name": "", - "seq_dir": seq_dir, - } - - right_frames_seq = data.get("robot_right_frames", [[]])[0] - left_frames_seq = data.get("robot_left_frames", [[]])[0] - right_frame_names = data.get("right_robot_frame_names", [[]])[0] - left_frame_names = data.get("left_robot_frame_names", [[]])[0] - obj_positions = data.get("object_body_position", [[]])[0] - obj_wxyz = data.get("object_body_wxyz", [[]])[0] - obj_mesh_paths = data.get("object_mesh_paths", [[]])[0] - object_name = data.get("object_name", [""])[0] or "" - - n_frames = len(right_frames_seq) - if n_frames == 0: - return { - "seq_id": seq_id, - "rejected": False, - "reason": "ok", - "max_ho_pen": 0.0, - "max_hh_pen": 0.0, - "object_name": object_name, - "seq_dir": seq_dir, - } - - right_cache = _HandShapeCache(right_shapes, right_frame_names) - left_cache = _HandShapeCache(left_shapes, left_frame_names) - - # Load object hulls (one per body; most sequences have 1 body). - # Pairs of (hull_or_None, hull_ratio). Skip bodies where hull_ratio - # exceeds hull_ratio_max: highly concave objects (AR glasses, open vases) - # produce massive false positives with the convex hull signed_distance check. - hull_entries: list[tuple[trimesh.Trimesh | None, float]] = [] - for mp in obj_mesh_paths: - hull, ratio = _load_hull(mp, hull_cache) - if ratio > hull_ratio_max: - hull_entries.append((None, ratio)) # skip this body - else: - hull_entries.append((hull, ratio)) - - max_ho_pen = 0.0 - max_hh_pen = 0.0 - - for t in range(0, n_frames, max(1, stride)): - right_caps = right_cache.world_spheres(right_frames_seq[t]) - left_caps = left_cache.world_spheres(left_frames_seq[t]) - - # Hand-object - if obj_positions and len(obj_positions) > t: - for body_idx, (hull, _) in enumerate(hull_entries): - if hull is None: - continue - if body_idx >= len(obj_positions[t]): - continue - obj_pos = np.array(obj_positions[t][body_idx], dtype=float) - obj_qwxyz = obj_wxyz[t][body_idx] - obj_R = _quat_wxyz_to_matrix(obj_qwxyz) - - ho = _max_hand_object_penetration( - right_caps + left_caps, hull, obj_pos, obj_R - ) - max_ho_pen = max(max_ho_pen, ho) - - # Hand-hand - hh = _max_hand_hand_penetration(right_caps, left_caps) - max_hh_pen = max(max_hh_pen, hh) - - # Early-exit once both thresholds exceeded - if max_ho_pen > max_pen and max_hh_pen > max_pen: - break - - overall_max = max(max_ho_pen, max_hh_pen) - rejected = overall_max > max_pen - reason = "ok" - if rejected: - parts = [] - if max_ho_pen > max_pen: - parts.append(f"hand_object:{max_ho_pen*100:.1f}cm") - if max_hh_pen > max_pen: - parts.append(f"hand_hand:{max_hh_pen*100:.1f}cm") - reason = ",".join(parts) - - return { - "seq_id": seq_id, - "rejected": rejected, - "reason": reason, - "max_ho_pen": max_ho_pen, - "max_hh_pen": max_hh_pen, - "object_name": object_name, - "seq_dir": seq_dir, - } - - -# --------------------------------------------------------------------------- -# Main -# --------------------------------------------------------------------------- - - -def _find_parquet_dirs(input_dir: Path) -> list[Path]: - """Return all robot_name= subdirs under sequence_id= dirs.""" - dirs = [] - for seq_dir in sorted(input_dir.glob("sequence_id=*")): - if not seq_dir.is_dir(): - continue - for robot_dir in sorted(seq_dir.glob("robot_name=*")): - if robot_dir.is_dir(): - dirs.append(robot_dir) - return dirs - - -def main() -> None: - """CLI entry point — parse args and run the penetration filter.""" - parser = argparse.ArgumentParser( - description="Filter hot3d_processed sequences by penetration depth.", - formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=__doc__, - ) - parser.add_argument( - "--input_dir", - type=Path, - required=True, - help="Processed parquet directory (hot3d_processed/).", - ) - parser.add_argument( - "--output_dir", - type=Path, - required=True, - help="Output directory for valid sequences.", - ) - parser.add_argument( - "--max_penetration", - type=float, - default=0.02, - help="Maximum allowed penetration in metres (default 0.02 = 2 cm).", - ) - parser.add_argument( - "--stride", - type=int, - default=3, - help="Check every N-th frame (default 3; use 1 for every frame).", - ) - parser.add_argument( - "--num_workers", - type=int, - default=8, - help="Parallel worker processes (default 8).", - ) - parser.add_argument( - "--dry_run", action="store_true", help="Report stats without writing output." - ) - parser.add_argument( - "--hull_ratio_max", - type=float, - default=3.0, - help="Skip hand-object check for objects whose convex hull volume is " - "more than this multiple of the mesh volume (default 3.0). " - "Hollow / concave objects (AR glasses=26x, mugs=4.7x, bowls=4.9x) " - "produce large false positives when the hand reaches inside them; " - "3.0 keeps solid objects (cans=1.3x, food=1.0x) while skipping " - "open containers. Set to 0 to disable all hand-object checks.", - ) - parser.add_argument( - "--sequence_id", type=str, default=None, help="Evaluate a single sequence." - ) - parser.add_argument( - "--sequence_pattern", type=str, default=None, help="Regex to filter sequences." - ) - args = parser.parse_args() - - logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s") - - if not args.input_dir.exists(): - log.error("input_dir does not exist: %s", args.input_dir) - sys.exit(1) - - log.info("Parsing collision geometry from URDFs ...") - right_shapes = _parse_collision_shapes(_URDF_RIGHT) - left_shapes = _parse_collision_shapes(_URDF_LEFT) - log.info(" right: %d shapes, left: %d shapes", len(right_shapes), len(left_shapes)) - - parquet_dirs = _find_parquet_dirs(args.input_dir) - log.info("Found %d sequences", len(parquet_dirs)) - - # Apply filters - if args.sequence_id: - parquet_dirs = [ - d - for d in parquet_dirs - if d.parent.name == f"sequence_id={args.sequence_id}" - ] - if args.sequence_pattern: - import re # noqa: PLC0415 - - pat = re.compile(args.sequence_pattern) - parquet_dirs = [ - d - for d in parquet_dirs - if pat.search(d.parent.name.replace("sequence_id=", "")) - ] - - log.info( - "Processing %d sequences (stride=%d, max_pen=%.3fm, hull_ratio_max=%.1f) ...", - len(parquet_dirs), - args.stride, - args.max_penetration, - args.hull_ratio_max, - ) - - hull_cache: dict = {} - - work_items = [ - ( - d, - right_shapes, - left_shapes, - args.max_penetration, - args.hull_ratio_max, - args.stride, - hull_cache, - ) - for d in parquet_dirs - ] - - results = [] - if args.num_workers > 1: - from multiprocessing import Pool # noqa: PLC0415 - - work_items_mp: list[tuple] = [ - ( - d, - right_shapes, - left_shapes, - args.max_penetration, - args.hull_ratio_max, - args.stride, - {}, - ) - for d in parquet_dirs - ] - with Pool(processes=args.num_workers) as pool: - for i, res in enumerate( - pool.imap_unordered(_check_sequence, work_items_mp, chunksize=4) - ): - results.append(res) - if (i + 1) % 100 == 0: - n_rej = sum(1 for r in results if r["rejected"]) - log.info(" %d/%d rejected=%d", i + 1, len(parquet_dirs), n_rej) - else: - for i, item in enumerate(work_items): - res = _check_sequence(item) - results.append(res) - if (i + 1) % 100 == 0: - n_rej = sum(1 for r in results if r["rejected"]) - log.info(" %d/%d rejected=%d", i + 1, len(parquet_dirs), n_rej) - - # Summary - n_total = len(results) - n_rejected = sum(1 for r in results if r["rejected"]) - n_valid = n_total - n_rejected - log.info("=" * 60) - log.info("Total: %d", n_total) - log.info("Valid: %d (%.1f%%)", n_valid, 100 * n_valid / max(n_total, 1)) - log.info("Rejected: %d (%.1f%%)", n_rejected, 100 * n_rejected / max(n_total, 1)) - - ho_pens = [r["max_ho_pen"] for r in results] - hh_pens = [r["max_hh_pen"] for r in results] - log.info( - "Max hand-object pen: %.4f m (mean %.4f m)", - max(ho_pens), - sum(ho_pens) / len(ho_pens), - ) - log.info( - "Max hand-hand pen: %.4f m (mean %.4f m)", - max(hh_pens), - sum(hh_pens) / len(hh_pens), - ) - - if args.dry_run: - log.info("Dry-run: no output written.") - args.output_dir.mkdir(parents=True, exist_ok=True) - _write_report(args.output_dir / "penetration_report.csv", results) - return - - # Copy valid sequences - args.output_dir.mkdir(parents=True, exist_ok=True) - log.info("Copying %d valid sequences to %s ...", n_valid, args.output_dir) - - copied = 0 - for res in results: - if res["rejected"]: - continue - seq_dir: Path = res["seq_dir"] # robot_name= dir - seq_id_dir = seq_dir.parent # sequence_id= dir - dest_seq = args.output_dir / seq_id_dir.name - dest_robot = dest_seq / seq_dir.name - dest_robot.mkdir(parents=True, exist_ok=True) - for f in seq_dir.iterdir(): - dest_f = dest_robot / f.name - if not dest_f.exists(): - shutil.copy2(str(f), str(dest_f)) - copied += 1 - if copied % 200 == 0: - log.info(" copied %d/%d", copied, n_valid) - - log.info("Done. %d sequences written to %s", copied, args.output_dir) - - # Write report CSV - report_path = args.output_dir / "penetration_report.csv" - _write_report(report_path, results) - log.info("Report: %s", report_path) - - -def _write_report(path: Path, results: list[dict]) -> None: - with open(path, "w", newline="") as f: - writer = csv.DictWriter( - f, - fieldnames=[ - "sequence_id", - "object_name", - "rejected", - "reason", - "max_ho_pen_cm", - "max_hh_pen_cm", - ], - ) - writer.writeheader() - for r in sorted(results, key=lambda x: x["seq_id"]): - writer.writerow( - { - "sequence_id": r["seq_id"], - "object_name": r.get("object_name", ""), - "rejected": r["rejected"], - "reason": r["reason"], - "max_ho_pen_cm": f"{r['max_ho_pen']*100:.3f}", - "max_hh_pen_cm": f"{r['max_hh_pen']*100:.3f}", - } - ) - - -# --------------------------------------------------------------------------- -# Quality-check protocol (auto-discovered by data_quality_checks/__init__.py) -# --------------------------------------------------------------------------- - -# Module-level cache so URDFs are parsed once per process. -_RIGHT_SHAPES: dict[str, CollisionShape] | None = None -_LEFT_SHAPES: dict[str, CollisionShape] | None = None - - -def _get_shapes() -> tuple[dict[str, CollisionShape], dict[str, CollisionShape]]: - """Lazy-load and cache the right/left collision shape dicts (process-local).""" - global _RIGHT_SHAPES, _LEFT_SHAPES # noqa: PLW0603 — intentional module-level cache - if _RIGHT_SHAPES is None: - _RIGHT_SHAPES = _parse_collision_shapes(_URDF_RIGHT) - _LEFT_SHAPES = _parse_collision_shapes(_URDF_LEFT) - assert _LEFT_SHAPES is not None # narrowed by _RIGHT_SHAPES check above - return _RIGHT_SHAPES, _LEFT_SHAPES - - -def check( - data: dict, - seq_dir: Path | None = None, - max_penetration: float = 0.02, - stride: int = 3, - hull_ratio_max: float = 3.0, -) -> dict: - """Pass iff max hand-object and hand-hand penetration ≤ max_penetration (default 2 cm). - - score = max penetration depth in centimetres across all sampled frames. - """ - right_shapes, left_shapes = _get_shapes() - - right_frames_seq = data.get("robot_right_frames", [[]])[0] - left_frames_seq = data.get("robot_left_frames", [[]])[0] - right_frame_names = data.get("right_robot_frame_names", [[]])[0] - left_frame_names = data.get("left_robot_frame_names", [[]])[0] - obj_positions = data.get("object_body_position", [[]])[0] - obj_wxyz = data.get("object_body_wxyz", [[]])[0] - obj_mesh_paths = data.get("object_mesh_paths", [[]])[0] - - n_frames = len(right_frames_seq) - if n_frames == 0: - return {"pass": True, "score": 0.0, "reason": "no frames"} - - right_hsc = _HandShapeCache(right_shapes, right_frame_names) - left_hsc = _HandShapeCache(left_shapes, left_frame_names) - - hull_cache: dict = {} - hull_entries: list[tuple] = [] - for mp in obj_mesh_paths: - hull, ratio = _load_hull(mp, hull_cache) - hull_entries.append((None, ratio) if ratio > hull_ratio_max else (hull, ratio)) - - max_ho_pen = 0.0 - max_hh_pen = 0.0 - - for t in range(0, n_frames, max(1, stride)): - right_caps = right_hsc.world_spheres(right_frames_seq[t]) - left_caps = left_hsc.world_spheres(left_frames_seq[t]) - - if obj_positions and len(obj_positions) > t: - for body_idx, (hull, _) in enumerate(hull_entries): - if hull is None or body_idx >= len(obj_positions[t]): - continue - obj_pos = np.array(obj_positions[t][body_idx], dtype=float) - obj_R = _quat_wxyz_to_matrix(obj_wxyz[t][body_idx]) - ho = _max_hand_object_penetration( - right_caps + left_caps, hull, obj_pos, obj_R - ) - max_ho_pen = max(max_ho_pen, ho) - - hh = _max_hand_hand_penetration(right_caps, left_caps) - max_hh_pen = max(max_hh_pen, hh) - - if max_ho_pen > max_penetration and max_hh_pen > max_penetration: - break - - overall_max = max(max_ho_pen, max_hh_pen) - passed = overall_max <= max_penetration - - parts = [] - if max_ho_pen > max_penetration: - parts.append(f"hand_object:{max_ho_pen*100:.1f}cm") - if max_hh_pen > max_penetration: - parts.append(f"hand_hand:{max_hh_pen*100:.1f}cm") - reason = ",".join(parts) if parts else f"ok (max={overall_max*100:.2f}cm)" - - return {"pass": passed, "score": round(overall_max * 100, 4), "reason": reason} - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/generate_rigid_urdfs.py b/robotic_grounding/scripts/generate_rigid_urdfs.py deleted file mode 100644 index 035843c2..00000000 --- a/robotic_grounding/scripts/generate_rigid_urdfs.py +++ /dev/null @@ -1,416 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Generate rigid URDFs for dataset objects. - -Reads parquet data or mesh directories to discover unique objects, then -generates a single-link ``_rigid.urdf`` for each one. Dataset properties -(mesh scale, format, etc.) are read from the dataset registry. - -Usage: - python scripts/generate_rigid_urdfs.py --dataset taco - python scripts/generate_rigid_urdfs.py --dataset hot3d - python scripts/generate_rigid_urdfs.py --dataset all -""" - -from __future__ import annotations - -import argparse -import textwrap -from pathlib import Path - -import trimesh -from robotic_grounding.retarget import ( - ASSETS_DIR as ASSET_DIR, -) -from robotic_grounding.retarget import ( - HUMAN_MOTION_DATA_DIR, -) -from robotic_grounding.retarget.dataset_registry import ( - get_all_dataset_names, - get_dataset_config, -) -from robotic_grounding.retarget.naming import make_usd_safe - -# Repository root (two levels up from scripts/) -REPO_ROOT = Path(__file__).resolve().parent.parent -URDF_DIR = ASSET_DIR / "urdfs" - - -def _relative_meshdir(urdf_path: Path, mesh_file: Path) -> str: - """Compute the relative meshdir from URDF location to the mesh's parent dir.""" - urdf_dir = urdf_path.parent - mesh_dir = mesh_file.parent - try: - rel = mesh_dir.relative_to(urdf_dir) - except ValueError: - rel = Path( - *([".."] * len(urdf_dir.relative_to(ASSET_DIR).parts)) - ) / mesh_dir.relative_to(ASSET_DIR) - return str(rel) + "/" - - -def _stl_output_dir(dataset: str, safe_object_name: str, source_mesh_dir: Path) -> Path: - """Where the generated visual STL should land. - - Committed datasets (source mesh already under ASSET_DIR) keep the STL - next to the source. Runtime-only datasets (dexycb, grab — meshes live - under HUMAN_MOTION_DATA_DIR which is outside ASSET_DIR on OSMO) get a - stable home under ASSET_DIR/meshes/{dataset}/{safe_name}/ so the URDF's - meshdir resolves inside the asset tree. - """ - try: - source_mesh_dir.relative_to(ASSET_DIR) - return source_mesh_dir - except ValueError: - return ASSET_DIR / "meshes" / dataset / safe_object_name - - -def _create_visual_mesh(mesh_path: Path, out_path: Path, scale: float) -> bool: - """Create a clean visual mesh (STL) from the source OBJ. - - Isaac Sim's OBJ parser can't handle Meshlab's interleaved vn/v format. - STL is universally supported. Follows ketchup_rigid.urdf pattern. - """ - try: - mesh = trimesh.load(mesh_path, force="mesh", process=False) - except Exception as e: - print(f" Warning: failed to load mesh {mesh_path}: {e}") - return False - - mesh.vertices *= scale - - try: - mesh.export(str(out_path)) - except Exception as e: - print(f" Warning: failed to export visual mesh to {out_path}: {e}") - return False - return True - - -def _generate_urdf( - safe_object_name: str, - meshdir: str, - mesh_filename: str, -) -> str: - """Generate a rigid URDF. Uses the same mesh for visual and collision. - - Mesh is pre-scaled to meters (scale=1). - """ - return textwrap.dedent( - f"""\ - - - - - - - - - - - - - - - - - - - - - - - - - - - - """ - ) - - -def _discover_h2o_objects() -> dict[str, tuple[Path, Path]]: - """Discover unique H2O objects from OBJ mesh files. - - Searches in order (mirrors :func:`_discover_dexycb_objects`): - - 1. ``assets/meshes/h2o/{name}/*.obj`` (committed copy, if any). - 2. ``{HUMAN_MOTION_DATA_DIR}/h2o/dataset/object/{name}/*.obj`` (raw - dataset staged at OSMO runtime via the CSS mount). - - H2O mesh filenames don't always match the folder name (e.g. - ``spray/lotion_spray.obj``), so we glob for ``*.obj`` and keep the - first match. URDFs go to ``assets/urdfs/h2o/{name}_rigid.urdf``. - """ - canonical_dir = ASSET_DIR / "meshes" / "h2o" - runtime_dir = HUMAN_MOTION_DATA_DIR / "h2o" / "dataset" / "object" - urdf_out_dir = URDF_DIR / "h2o" - - sources = [d for d in (canonical_dir, runtime_dir) if d.is_dir()] - if not sources: - print(f"No H2O mesh directory at {canonical_dir} or {runtime_dir}") - return {} - - objects: dict[str, tuple[Path, Path]] = {} - for base in sources: - for sub in sorted(base.iterdir()): - if not sub.is_dir() or sub.name in objects: - continue - objs = sorted(sub.glob("*.obj")) - if not objs: - continue - objects[sub.name] = (objs[0], urdf_out_dir / f"{sub.name}_rigid.urdf") - return objects - - -def _discover_dexycb_objects() -> dict[str, tuple[Path, Path]]: - """Discover the 21 YCB objects shipped with DexYCB. - - Meshes live at ``{dataset}/models/{name}/textured_simple.obj``. Also - accepts a committed copy under ``assets/meshes/dexycb/``. - URDFs go to ``assets/urdfs/dexycb/{name}_rigid.urdf``. - """ - canonical_dir = ASSET_DIR / "meshes" / "dexycb" - runtime_dir = HUMAN_MOTION_DATA_DIR / "dexycb" / "dataset" / "models" - urdf_out_dir = URDF_DIR / "dexycb" - - sources = [d for d in (canonical_dir, runtime_dir) if d.is_dir()] - if not sources: - print(f"No DexYCB mesh directory at {canonical_dir} or {runtime_dir}") - return {} - - objects: dict[str, tuple[Path, Path]] = {} - for base in sources: - for sub in sorted(base.iterdir()): - if not sub.is_dir() or sub.name in objects: - continue - # Loader writes ``make_usd_safe(name)_rigid.urdf`` in the parquet; - # names starting with a digit (YCB ids like "002_…") get an ``obj_`` prefix. - safe = make_usd_safe(sub.name) - preferred = sub / "textured_simple.obj" - if preferred.exists(): - objects[sub.name] = (preferred, urdf_out_dir / f"{safe}_rigid.urdf") - continue - objs = sorted(sub.glob("*.obj")) - if objs: - objects[sub.name] = (objs[0], urdf_out_dir / f"{safe}_rigid.urdf") - return objects - - -def _discover_grab_objects() -> dict[str, tuple[Path, Path]]: - """Discover unique GRAB objects from the canonical contact meshes. - - GRAB ships its rigid object meshes at - ``{dataset}/tools/object_meshes/contact_meshes/{name}.ply`` (where - ``{dataset}`` is the extracted ``grab_dir`` root on OSMO / locally). - URDFs go to ``assets/urdfs/grab/{name}_rigid.urdf``. - """ - meshes_dir = ( - HUMAN_MOTION_DATA_DIR - / "grab" - / "dataset" - / "tools" - / "object_meshes" - / "contact_meshes" - ) - urdf_out_dir = URDF_DIR / "grab" - if not meshes_dir.is_dir(): - print(f"No GRAB contact_meshes directory at {meshes_dir}") - return {} - - objects: dict[str, tuple[Path, Path]] = {} - for ply_path in sorted(meshes_dir.glob("*.ply")): - name = ply_path.stem - # Skip the body/hand meshes that live in the same directory. - if name in {"body", "lhand", "rhand"}: - continue - objects[name] = (ply_path, urdf_out_dir / f"{name}_rigid.urdf") - return objects - - -def _discover_taco_objects() -> dict[str, tuple[Path, Path]]: - """Discover TACO objects from committed OBJ meshes. - - TACO meshes live at ``assets/meshes/taco/{id}_cm.obj`` (one per object, - in centimetres — the ``_cm`` suffix is stripped for the URDF name). - URDFs go to ``assets/urdfs/taco/{id}_rigid.urdf``. - """ - meshes_dir = ASSET_DIR / "meshes" / "taco" - urdf_out_dir = URDF_DIR / "taco" - if not meshes_dir.is_dir(): - print(f"No TACO mesh directory at {meshes_dir}") - return {} - objects: dict[str, tuple[Path, Path]] = {} - for obj_path in sorted(meshes_dir.glob("*_cm.obj")): - obj_id = obj_path.stem[: -len("_cm")] - objects[obj_id] = (obj_path, urdf_out_dir / f"{obj_id}_rigid.urdf") - return objects - - -def _discover_oakink2_objects() -> dict[str, tuple[Path, Path]]: - """Discover OakInk2 objects from the canonical ``object_repair`` meshes. - - OakInk2's raw meshes come in two trees — ``object_raw/align_ds/{id}/*`` - uses per-object mesh filenames (``mug.ply``, ``cup.ply``, ``scan.ply``, - ``model_align.obj``, …) which makes discovery brittle; only a subset - of IDs have ``scan.ply``. ``object_repair/align_ds/{id}/model.obj`` is - the cleaned-up version and is present for every ID the loader uses - (see ``oakink2_loader._resolve_mesh_path`` — the loader also prefers - ``object_repair``). Matching that path keeps the generator and the - loader aligned: every object the loader records in ``object_urdf_paths`` - gets a URDF here. - """ - meshes_dir = ASSET_DIR / "meshes" / "oakink2" / "object_repair" / "align_ds" - urdf_out_dir = URDF_DIR / "oakink2" - if not meshes_dir.is_dir(): - print(f"No OakInk2 mesh directory at {meshes_dir}") - return {} - objects: dict[str, tuple[Path, Path]] = {} - for sub in sorted(meshes_dir.iterdir()): - if not sub.is_dir(): - continue - mesh = sub / "model.obj" - if not mesh.exists(): - continue - # Loader stores URDF paths as ``{object_id}_rigid.urdf`` (no - # make_usd_safe); keep the same convention here. - objects[sub.name] = (mesh, urdf_out_dir / f"{sub.name}_rigid.urdf") - return objects - - -def _discover_hot3d_objects() -> dict[str, tuple[Path, Path]]: - """Discover unique Hot3D objects from GLB mesh files. - - Hot3D meshes are stored as {uid}.glb in assets/meshes/hot3d/. - URDFs go to assets/urdfs/hot3d/{uid}_rigid.urdf. - """ - meshes_dir = ASSET_DIR / "meshes" / "hot3d" - urdf_out_dir = URDF_DIR / "hot3d" - files = sorted(meshes_dir.glob("*.glb")) - if not files: - print(f"No GLB meshes found in {meshes_dir}") - return {} - - objects: dict[str, tuple[Path, Path]] = {} - for glb_path in files: - uid = glb_path.stem - urdf_path = urdf_out_dir / f"{uid}_rigid.urdf" - objects[uid] = (glb_path, urdf_path) - - return objects - - -def _discover_objects(dataset: str) -> dict[str, tuple[Path, Path]]: - """Discover unique (object_id -> mesh_path) from committed/raw mesh files. - - Each rigid dataset has its own layout (TACO flat, OakInk2 nested, etc.). - Arctic is intentionally unsupported — its URDFs are hand-crafted - articulated models, not regenerable from meshes alone. - """ - discovery = { - "taco": _discover_taco_objects, - "oakink2": _discover_oakink2_objects, - "hot3d": _discover_hot3d_objects, - "h2o": _discover_h2o_objects, - "grab": _discover_grab_objects, - "dexycb": _discover_dexycb_objects, - } - if dataset not in discovery: - raise ValueError( - f"Rigid URDF generation not supported for dataset '{dataset}' " - f"(articulated datasets must ship hand-crafted URDFs)." - ) - return discovery[dataset]() - - -def generate_for_dataset(dataset: str, dry_run: bool = False) -> None: - """Generate rigid URDFs for all objects in a dataset.""" - print(f"\n{'='*60}") - print(f"Generating rigid URDFs for {dataset}") - print(f"{'='*60}") - - objects = _discover_objects(dataset) - if not objects: - print("No objects found.") - return - - out_dir = URDF_DIR / dataset - out_dir.mkdir(parents=True, exist_ok=True) - - # All generated meshes (visual STL + collision OBJ) go next to source meshes - config = get_dataset_config(dataset) - scale = config.mesh_vertex_scale - generated = 0 - skipped = 0 - - for obj_id, (mesh_path, urdf_path) in sorted(objects.items()): - # Clean STL mesh (avoids Meshlab interleaved OBJ that crashes Isaac Sim) - safe_object_name = make_usd_safe(obj_id) - mesh_name = f"{safe_object_name}_visual.stl" - stl_dir = _stl_output_dir(dataset, safe_object_name, mesh_path.parent) - mesh_out_path = stl_dir / mesh_name - meshdir = _relative_meshdir(urdf_path, mesh_out_path) - - if dry_run: - print(f" DRY-RUN: would write {urdf_path}") - continue - - stl_dir.mkdir(parents=True, exist_ok=True) - if not _create_visual_mesh(mesh_path, mesh_out_path, scale): - print(f" SKIP {obj_id}: mesh conversion failed") - skipped += 1 - continue - - urdf_content = _generate_urdf( - safe_object_name=safe_object_name, - meshdir=meshdir, - mesh_filename=mesh_name, - ) - urdf_path.write_text(urdf_content) - generated += 1 - - print(f"\nGenerated: {generated}, Skipped: {skipped}") - print(f"Output directory: {out_dir}") - - -def main() -> None: - """Generate rigid URDFs for dataset objects.""" - all_names = list(get_all_dataset_names()) - parser = argparse.ArgumentParser( - description="Generate rigid URDFs for dataset objects." - ) - parser.add_argument( - "--dataset", - type=str, - choices=all_names + ["all"], - default="all", - help="Which dataset to generate URDFs for.", - ) - parser.add_argument( - "--dry-run", - action="store_true", - help="Print what would be generated without writing files.", - ) - args = parser.parse_args() - - if args.dataset == "all": - # Skip datasets whose objects are articulated (e.g. arctic) — their - # URDFs are hand-crafted and shipped in the repo, not regenerated here. - datasets = [ - n for n in all_names if not get_dataset_config(n).has_articulated_objects - ] - elif get_dataset_config(args.dataset).has_articulated_objects: - print( - f"[INFO] {args.dataset} uses articulated URDFs — keeping the " - "hand-crafted files committed under assets/urdfs/ and exiting." - ) - return - else: - datasets = [args.dataset] - for ds in datasets: - generate_for_dataset(ds, dry_run=args.dry_run) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/license_header_hook.py b/robotic_grounding/scripts/license_header_hook.py deleted file mode 100644 index 0f57a66c..00000000 --- a/robotic_grounding/scripts/license_header_hook.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Pre-commit hook: enforce the NVIDIA Apache-2.0 SPDX header on Python files. - -Recognizes NVIDIA's own license headers in any of their historical forms (the -legacy proprietary "all rights reserved / strictly prohibited" block, the -verbose Apache-2.0 boilerplate, or the canonical 2-line SPDX header) and -normalizes them to the concise 2-line SPDX header used across the -``reconstruction/`` tree:: - - # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. - # SPDX-License-Identifier: Apache-2.0 - -Files that carry no header get one inserted. Files whose leading comment block -carries a *non-NVIDIA* copyright (e.g. third-party Isaac Lab code) are left -completely untouched so we never relicense or strip upstream attribution. - -Run by pre-commit; modifies files in place and exits non-zero when it changed -anything (so the commit aborts and the change gets staged). -""" -from __future__ import annotations - -import sys - -HEADER_LINES = [ - "# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.\n", - "# SPDX-License-Identifier: Apache-2.0\n", -] - -# A comment is part of a license header if it is an empty `#` spacer or mentions -# one of these tokens. Any other comment (a tool directive, a real code comment) -# ends the header run so it is never consumed. -LICENSE_TOKENS = ( - "copyright", - "spdx-", - "nvidia", - "all rights reserved", - "licensed under", - "license at", - "license for", - "license is", - "the license", - "apache license", - "version 2.0", - "warranties", - "obtain a copy", - "compliance with", - "intellectual property", - "proprietary rights", - "strictly prohibited", - "related documentation", - "modifications thereto", - "applicable law", - "apache.org/licenses", - "express or implied", - "as is", - "permissions and", - "limitations under", -) - -# If the header run carries any of these, it is third-party — leave it alone. -THIRD_PARTY_TOKENS = ("isaac lab project developers",) - - -def _is_license_comment(line: str) -> bool: - stripped = line.strip() - if not stripped.startswith("#"): - return False - body = stripped[1:].strip() - if body == "": # bare `#` spacer line inside a header block - return True - low = body.lower() - return any(tok in low for tok in LICENSE_TOKENS) - - -def normalize(text: str) -> str: - """Return ``text`` with the canonical SPDX header, or unchanged if third-party.""" - lines = text.splitlines(keepends=True) - - head: list[str] = [] - idx = 0 - if lines and lines[idx].startswith("#!"): - head.append(lines[idx]) - idx += 1 - - # Skip blank lines between an optional shebang and the header/body. - while idx < len(lines) and lines[idx].strip() == "": - idx += 1 - - # Consume the contiguous run of license-like comment lines. - run_end = idx - while run_end < len(lines) and _is_license_comment(lines[run_end]): - run_end += 1 - run = "".join(lines[idx:run_end]).lower() - - if any(tok in run for tok in THIRD_PARTY_TOKENS): - return text # third-party header — never touch - if "copyright" in run and "nvidia" not in run: - return text # some other party's copyright — never touch - - is_nvidia_header = "nvidia" in run and ("copyright" in run or "spdx-" in run) - body_start = run_end if is_nvidia_header else idx - - body = lines[body_start:] - while body and body[0].strip() == "": # no blank line between header and body - body.pop(0) - - return "".join(head + HEADER_LINES + body) - - -def main(argv: list[str]) -> int: - """Normalize each path in ``argv``; exit non-zero if any file changed.""" - changed = False - for path in argv: - try: - original = open(path, encoding="utf-8").read() - except (OSError, UnicodeDecodeError): - continue - updated = normalize(original) - if updated != original: - with open(path, "w", encoding="utf-8") as fh: - fh.write(updated) - print(f"license header fixed: {path}") - changed = True - return 1 if changed else 0 - - -if __name__ == "__main__": - raise SystemExit(main(sys.argv[1:])) diff --git a/robotic_grounding/scripts/list_css_sequences.py b/robotic_grounding/scripts/list_css_sequences.py deleted file mode 100644 index 6e46f5ea..00000000 --- a/robotic_grounding/scripts/list_css_sequences.py +++ /dev/null @@ -1,151 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""List available sequences in CSS (NVIDIA PDX storage). - -Connects to the PDX object store via the S3-compatible API and lists -sequence-level prefixes for each dataset stage (raw, loaded, processed). - -Prerequisites: - - boto3: pip install boto3 - - CSS credentials configured via environment variables: - source scripts/setup_css_env.sh - -Usage: - python scripts/list_css_sequences.py - python scripts/list_css_sequences.py --dataset taco - python scripts/list_css_sequences.py --dataset arctic --stage loaded - python scripts/list_css_sequences.py --pattern '.*screw.*' -""" - -import argparse -import os -import re -import sys -from pathlib import Path - -import boto3 -from botocore.config import Config - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -ENDPOINT_URL = os.environ.get("CSS_ENDPOINT_URL", "https://pdx.s8k.io") -ACCESS_KEY = os.environ.get("CSS_ACCESS_KEY", "") -SECRET_KEY = os.environ.get("CSS_SECRET_KEY", "") -REGION = os.environ.get("CSS_REGION", "us-east-1") - -BUCKET = "datasets" - -# Dataset registry — single source of truth for dataset names and CSS paths. -# Add new datasets in robotic_grounding.retarget.dataset_registry, not here. -sys.path.insert( - 0, - str(Path(__file__).resolve().parent.parent / "source" / "robotic_grounding"), -) -from robotic_grounding.retarget.dataset_registry import ( # noqa: E402 - get_all_dataset_names, - get_css_stage_prefixes, -) - -DATASETS = get_all_dataset_names() -STAGE_PREFIXES: dict[str, dict[str, str]] = { - name: get_css_stage_prefixes(name) for name in DATASETS -} - - -# --------------------------------------------------------------------------- -# S3 helpers -# --------------------------------------------------------------------------- -def get_s3_client() -> "boto3.client": - """Create an S3 client for CSS (PDX).""" - if not ACCESS_KEY or not SECRET_KEY: - print( - "Error: Set CSS_ACCESS_KEY and CSS_SECRET_KEY environment variables.\n" - " source scripts/setup_css_env.sh", - file=sys.stderr, - ) - sys.exit(1) - return boto3.client( - "s3", - endpoint_url=ENDPOINT_URL, - aws_access_key_id=ACCESS_KEY, - aws_secret_access_key=SECRET_KEY, - region_name=REGION, - config=Config(connect_timeout=5), - ) - - -def list_prefixes(client: "boto3.client", prefix: str) -> list[str]: - """List immediate sub-prefixes (directories) under *prefix*. - - Uses the S3 delimiter to get only one level of hierarchy. - """ - names: list[str] = [] - paginator = client.get_paginator("list_objects_v2") - for page in paginator.paginate(Bucket=BUCKET, Prefix=prefix, Delimiter="/"): - for cp in page.get("CommonPrefixes", []): - name = cp["Prefix"][len(prefix) :].rstrip("/") - if name: - names.append(name) - return sorted(names) - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- -def _parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser( - description="List available sequences on CSS (PDX storage).", - ) - parser.add_argument( - "--dataset", - choices=DATASETS, - default=None, - help="Limit to a single dataset (default: all).", - ) - parser.add_argument( - "--stage", - choices=("raw", "loaded", "processed"), - default=None, - help="Limit to a single pipeline stage (default: all).", - ) - parser.add_argument( - "--pattern", - type=str, - default=None, - help="Regex pattern to filter sequence names.", - ) - return parser.parse_args() - - -def main() -> None: - """Entry point.""" - args = _parse_args() - client = get_s3_client() - - datasets = [args.dataset] if args.dataset else list(DATASETS) - stages = [args.stage] if args.stage else ["raw", "loaded", "processed"] - - for dataset in datasets: - for stage in stages: - prefix = STAGE_PREFIXES[dataset][stage] - names = list_prefixes(client, prefix) - - if args.pattern: - regex = re.compile(args.pattern) - names = [n for n in names if regex.search(n)] - - header = f"{dataset}/{stage}" - if not names: - print(f"{header}: (empty)") - continue - - print(f"{header}: {len(names)} sequences") - for name in names: - print(f" {name}") - print() - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/process_arctic_grab.py b/robotic_grounding/scripts/process_arctic_grab.py deleted file mode 100644 index c1135e4a..00000000 --- a/robotic_grounding/scripts/process_arctic_grab.py +++ /dev/null @@ -1,67 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from pathlib import Path - -import numpy as np -import trimesh -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ManoSharpaData, list_sequence_ids -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, -) - -DATASET_DIRS: dict[str, str] = { - "arctic": "arctic_processed", -} - -if __name__ == "__main__": - input_dir = HUMAN_MOTION_DATA_DIR / DATASET_DIRS["arctic"] - available = list_sequence_ids(str(input_dir)) - grab_sequences = [sequence_id for sequence_id in available if "grab" in sequence_id] - print(f"Found {len(grab_sequences)} sequences in {input_dir}") - for sequence_id in grab_sequences: - print(sequence_id) - - data = ManoSharpaData.from_parquet( - str(input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - - data.sequence_id = data.sequence_id.replace( - data.object_name, f"rigid_{data.object_name}" - ) - - data.mano_right_object_contact_part_ids = ( - np.asarray(data.mano_right_object_contact_part_ids).clip(max=1).tolist() - ) - data.mano_left_object_contact_part_ids = ( - np.asarray(data.mano_left_object_contact_part_ids).clip(max=1).tolist() - ) - - data.object_body_names = ["object"] - data.safe_object_body_names = ["object"] - - object_mesh_path = Path(data.object_mesh_paths[0]).parent / "mesh_tex.obj" - object_mesh = trimesh.load(str(object_mesh_path)).apply_scale(0.001) - object_mesh.export(str(object_mesh_path.parent / "mesh_tex_tiny.obj")) - data.object_mesh_paths = [ - str(Path(data.object_mesh_paths[0]).parent / "mesh_tex_tiny.obj") - ] - - vertices = np.asarray(object_mesh.vertices) - data.object_mesh_radius = [ - np.linalg.norm(vertices - vertices.mean(axis=0), axis=1).max() - ] - - data.object_articulation = [0.0] * len(data.object_articulation) - - data.object_body_position = np.asarray(data.object_body_position)[ - :, :1 - ].tolist() - - data.object_body_wxyz = np.asarray(data.object_body_wxyz)[:, :1].tolist() - - data.save_to_parquet( - str(input_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) diff --git a/robotic_grounding/scripts/pull_arctic_subset.py b/robotic_grounding/scripts/pull_arctic_subset.py deleted file mode 100644 index 4f5f484d..00000000 --- a/robotic_grounding/scripts/pull_arctic_subset.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Pull a SUBSET of raw arctic sequences from CSS (PDX) for local testing. - -Unlike sync_css_data.py (which pulls pipeline *outputs*), this downloads the raw -`dataset/{subject}/{seq}.mano.npy` + `.object.npy` files the arctic loader reads, -for a handful of sequences — enough to run a loader / equivalence test locally. - -Prereqs: - pip install boto3 - source scripts/setup_css_env.sh # set CSS_ACCESS_KEY / CSS_SECRET_KEY - -Usage: - python scripts/pull_arctic_subset.py # 2 seqs, any - python scripts/pull_arctic_subset.py --pattern 's01_box' --max 2 - python scripts/pull_arctic_subset.py --list # just list, no download -""" -import argparse -import os -import re -import sys -from pathlib import Path - -import boto3 -from botocore.config import Config - -BUCKET = "datasets" -RAW_PREFIX = "v2d/human_motion_data/arctic/dataset/" -DEST = ( - Path(__file__).resolve().parent.parent - / "source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/dataset" -) - - -def _client() -> "boto3.client": - ak, sk = os.environ.get("CSS_ACCESS_KEY", ""), os.environ.get("CSS_SECRET_KEY", "") - if not ak or not sk: - sys.exit("Set CSS_ACCESS_KEY / CSS_SECRET_KEY: source scripts/setup_css_env.sh") - return boto3.client( - "s3", - endpoint_url=os.environ.get("CSS_ENDPOINT_URL", "https://pdx.s8k.io"), - aws_access_key_id=ak, - aws_secret_access_key=sk, - region_name=os.environ.get("CSS_REGION", "us-east-1"), - config=Config(signature_version="s3v4"), - ) - - -def main() -> None: - """Download (or list) a small subset of arctic raw .mano.npy sequences from CSS.""" - p = argparse.ArgumentParser() - p.add_argument("--pattern", default=".*", help="regex over the .mano.npy key") - p.add_argument("--max", type=int, default=2, help="max sequences to pull") - p.add_argument("--list", action="store_true", help="list matches, don't download") - args = p.parse_args() - - s3 = _client() - paginator = s3.get_paginator("list_objects_v2") - mano_keys = [ - obj["Key"] - for page in paginator.paginate(Bucket=BUCKET, Prefix=RAW_PREFIX) - for obj in page.get("Contents", []) - if obj["Key"].endswith(".mano.npy") - and "scissor" not in obj["Key"] - and re.search(args.pattern, obj["Key"]) - ] - mano_keys.sort() - print(f"{len(mano_keys)} sequence(s) match; taking first {args.max}") - for mk in mano_keys[: args.max]: - seq = mk[len(RAW_PREFIX) : -len(".mano.npy")] - print(f" {seq}") - if args.list: - continue - for key in (mk, mk[: -len(".mano.npy")] + ".object.npy"): - dst = DEST / key[len(RAW_PREFIX) :] - dst.parent.mkdir(parents=True, exist_ok=True) - s3.download_file(BUCKET, key, str(dst)) - print(f" -> {dst}") - if not args.list: - print(f"Done. Raw arctic under: {DEST}") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/reconstruct_support_surfaces.py b/robotic_grounding/scripts/reconstruct_support_surfaces.py deleted file mode 100644 index 629e14e5..00000000 --- a/robotic_grounding/scripts/reconstruct_support_surfaces.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reconstruct support surfaces from object still-poses (CLI driver). - -Library code lives in ``robotic_grounding.retarget.support_recon``. This -script just parses CLI args and dispatches to that module so the same -logic can be reused from the planner orchestrator. - -Usage: - 1. Produce *_loaded data first: the reconstruction v2d_task_library_loader - load workflow (hand+object datasets), or soma_to_g1.py --save (whole-body) - 2. python scripts/reconstruct_support_surfaces.py --input_dir ... [--sequence_id ID] -""" - -import argparse -from pathlib import Path - -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.dataset_registry import ( - get_all_dataset_names, - get_dataset_config, -) -from robotic_grounding.retarget.support_recon import ( - GROUND_Z_THRESHOLD, - _detect_parquet_schema, - reconstruct_support_for_sequence, -) - -# soma_g1 is a *processed* whole-body schema, not a registered source -# dataset, so it's kept as a separate default outside the registry. -# ``PROCESSED_DATASET_NAMES`` is the canonical set of --dataset values -# that select ``DEFAULT_INPUT_DIR_G1`` instead of going through -# ``get_dataset_config``; keep argparse choices and the dispatch in -# ``main`` in sync by referencing this constant. -PROCESSED_DATASET_NAMES: tuple[str, ...] = ("soma_g1", "motion_v1") -DEFAULT_INPUT_DIR_G1 = HUMAN_MOTION_DATA_DIR / "whole_body" / "soma" - - -def _parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser( - description="Reconstruct support surfaces from object still-poses.", - ) - parser.add_argument( - "--input_dir", - type=Path, - default=None, - help="Parquet root (e.g. taco_loaded/mano_object_only or arctic_loaded/mano_object_only).", - ) - parser.add_argument( - "--dataset", - choices=get_all_dataset_names() + PROCESSED_DATASET_NAMES, - default="oakink2", - help="Dataset for default input_dir when --input_dir not set.", - ) - add_sequence_filter_args(parser) - parser.add_argument( - "--list", - action="store_true", - help="Only list sequence IDs and exit.", - ) - parser.add_argument( - "--output", - type=str, - default=None, - help="Output .usda path for support surfaces (default: _support.usda).", - ) - parser.add_argument( - "--ground_threshold", - type=float, - default=GROUND_Z_THRESHOLD, - help=( - f"Disks with z <= this are on the ground plane and skipped " - f"(default: {GROUND_Z_THRESHOLD}m)." - ), - ) - return parser.parse_args() - - -def main() -> None: - """Parse CLI args and reconstruct support surfaces for the dataset.""" - args = _parse_args() - if args.input_dir: - input_dir = args.input_dir - elif args.dataset in PROCESSED_DATASET_NAMES: - input_dir = DEFAULT_INPUT_DIR_G1 - else: - config = get_dataset_config(args.dataset) - input_dir = ( - HUMAN_MOTION_DATA_DIR / config.name / f"{config.name}{config.loaded_suffix}" - ) - if not input_dir.is_dir(): - print( - f"Input dir not found: {input_dir}. Run the loader first " - "(e.g. via the reconstruction load workflow)." - ) - return - - # HOT3D and OakInk2 objects often rest at floor level (z ≤ 0.05 m). Don't - # filter those disks out — the sim ground plane can coexist with a support - # surface there. Only applies when the user hasn't explicitly overridden - # --ground_threshold. - if args.ground_threshold == GROUND_Z_THRESHOLD: - is_hot3d = args.dataset == "hot3d" or "hot3d" in str(input_dir).lower() - is_oakink2 = args.dataset == "oakink2" or "oakink2" in str(input_dir).lower() - if is_hot3d or is_oakink2: - args.ground_threshold = -0.02 - detected = "HOT3D" if is_hot3d else "OakInk2" - print( - f"{detected} detected: ground_threshold set to -0.01 (keeping floor-level disks)" - ) - - sequence_ids = list_sequence_ids(str(input_dir)) - if not sequence_ids: - print(f"No sequences in {input_dir}") - return - - if args.list: - for sid in sequence_ids: - print(sid) - return - - ids_to_process = filter_sequence_ids(sequence_ids, args) - print(f"Processing {len(ids_to_process)} of {len(sequence_ids)} sequence(s).") - - schema = _detect_parquet_schema(input_dir) - print(f"Detected schema: {schema}") - - for sequence_id in ids_to_process: - reconstruct_support_for_sequence( - input_dir, - sequence_id, - args.output, - schema=schema, - ground_z_threshold=args.ground_threshold, - ) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/replay_motion.py b/robotic_grounding/scripts/replay_motion.py deleted file mode 100644 index 5520299a..00000000 --- a/robotic_grounding/scripts/replay_motion.py +++ /dev/null @@ -1,863 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Kinematic playback of retargeted motion data in Isaac Lab. - -Supports both whole-body (G1) and dual floating-hand (Sharpa/Dex3) schemas. -Robot and object are teleported each sim step — no physics forces act on them. -Object collision geometry is disabled to avoid interpenetration artifacts from -the retargeted IK solution. Playback loops by default; use ``--no-loop`` to -stop at the last frame. - -If the motion_v1 parquet carries per-side wrist positions + binary -``{left,right}_hand_contact_active`` masks, a colored sphere (red=left, -green=right) is drawn at the corresponding wrist while the mask is on. - -Usage: - python scripts/replay_motion.py \ - --motion_file source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=/robot_name=g1 - - python scripts/replay_motion.py --motion_file --speed 0.5 - python scripts/replay_motion.py --motion_file --no-loop - python scripts/replay_motion.py --motion_file --headless -""" - -import argparse -from typing import Any - -from isaaclab.app import AppLauncher - -parser = argparse.ArgumentParser(description="Replay retargeted motion from Parquet.") -parser.add_argument( - "--motion_file", type=str, required=True, help="Parquet partition dir." -) -parser.add_argument( - "--speed", type=float, default=1.0, help="Playback speed multiplier." -) -parser.add_argument( - "--no-loop", - action="store_true", - help="Stop at the last frame instead of looping (default: loop).", -) -parser.add_argument("--num_envs", type=int, default=1, help="Number of environments.") -parser.add_argument( - "--start_frame", - type=int, - default=0, - help="First frame of the source motion to play (inclusive).", -) -parser.add_argument( - "--end_frame", - type=int, - default=None, - help="One past the last frame to play. Default: end of sequence.", -) -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -# --- Isaac / torch imports after AppLauncher --------------------------------- -import isaaclab.sim as sim_utils # noqa: E402 -import torch # noqa: E402 -from isaaclab.envs import ManagerBasedEnv # noqa: E402 -from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg # noqa: E402 -from isaaclab.markers.config import FRAME_MARKER_CFG # noqa: E402 -from isaaclab.utils import configclass # noqa: E402 -from robotic_grounding.motion_schema import load_motion_data_parquet # noqa: E402 -from robotic_grounding.tasks.scene_utils.replay_data import ( # noqa: E402 - DualHandTrajectory, - SingleRobotTrajectory, - load_replay_trajectory, -) -from robotic_grounding.tasks.scene_utils.scene_viewer_env_cfg import ( # noqa: E402 - SceneViewerEnvCfg, -) -from scipy.spatial.transform import Rotation as R # noqa: E402 - -# Foot-contact proxy: in kinematic replay there's no physics contact force, -# so "in contact" is a simple height check on the ankle-roll link. When the -# G1 foot is flat, left/right_ankle_roll_link sits ≈ 0.037 m above the -# ground (the LL_FOOT/LR_FOOT frames are defined at ``0.04 0 -0.037`` below -# their parent ankle-roll). 0.06 m gives a small tolerance for retarget noise -# without falsely triggering during swing. -FOOT_CONTACT_Z_THRESHOLD = 0.06 - - -# ============================================================================= -# Env configuration — kinematic replay variant -# ============================================================================= - - -def _disable_gravity_in_articulation_cfg(cfg: Any) -> Any: - """Return articulation cfg with gravity disabled in rigid properties.""" - spawn = getattr(cfg, "spawn", None) - rigid_props = getattr(spawn, "rigid_props", None) - if spawn is None or rigid_props is None: - return cfg - return cfg.replace( - spawn=spawn.replace( - rigid_props=rigid_props.replace(disable_gravity=True), - ) - ) - - -@configclass -class ReplayEnvCfg(SceneViewerEnvCfg): - """Env cfg for kinematic trajectory replay. - - Note: This config disables collisions and gravity on all replayed bodies - because the robot and object are teleported each step from the parquet - trajectory (no physics forces act on them). Any contact-based rewards or - termination signals would not be meaningful in this env; use a physics - env (e.g. ManagerBasedRLEnv) for those. - """ - - def __post_init__(self) -> None: - """Post-init: build viewer scene, then apply replay-only overrides.""" - super().__post_init__() - - # Replay: never resolve robot-object or robot-fixed collisions. - self.events.setup_collision_groups.params[ - "disable_robot_to_object_collisions" - ] = True - self.events.setup_collision_groups.params[ - "disable_robot_to_fixed_object_collisions" - ] = True - - # Replay object(s): collision OFF + kinematic + gravity OFF. - object_names = self.events.setup_collision_groups.params.get("object_names", []) - for name in object_names: - obj_cfg = getattr(self.scene, name, None) - if obj_cfg is None: - continue - spawn = getattr(obj_cfg, "spawn", None) - rigid_props = getattr(spawn, "rigid_props", None) - collision_props = getattr(spawn, "collision_props", None) - if spawn is None or rigid_props is None: - continue - new_rigid_props = rigid_props.replace( - disable_gravity=True, - kinematic_enabled=True, - ) - if collision_props is not None: - spawn = spawn.replace( - rigid_props=new_rigid_props, - collision_props=collision_props.replace(collision_enabled=False), - ) - else: - spawn = spawn.replace(rigid_props=new_rigid_props) - setattr(self.scene, name, obj_cfg.replace(spawn=spawn)) - - # Replay robot(s): disable gravity for static kinematic playback. - if hasattr(self.scene, "robot"): - self.scene.robot = _disable_gravity_in_articulation_cfg(self.scene.robot) - if hasattr(self.scene, "right_robot"): - self.scene.right_robot = _disable_gravity_in_articulation_cfg( - self.scene.right_robot - ) - if hasattr(self.scene, "left_robot"): - self.scene.left_robot = _disable_gravity_in_articulation_cfg( - self.scene.left_robot - ) - - -# ============================================================================= -# Trajectory loading -# ============================================================================= - - -def _get_spawn_root_z(env_cfg: ReplayEnvCfg) -> float: - """Read spawn root Z from the robot articulation cfg's initial state. - - Falls back to 0.0 for dual-hand layouts (floating wrist, no world-frame root). - """ - robot_cfg = getattr(env_cfg.scene, "robot", None) - if robot_cfg is not None: - init_state = getattr(robot_cfg, "init_state", None) - if init_state is not None: - pos = getattr(init_state, "pos", None) - if pos is not None and len(pos) >= 3: - return float(pos[2]) - return 0.0 - - -class Trajectory: - """Load replay trajectory tensors for single-robot or dual-hand layouts.""" - - def __init__( - self, - motion_file: str, - device: torch.device, - spawn_root_z: float = 0.0, - start_frame: int = 0, - end_frame: int | None = None, - ) -> None: - """Load trajectory arrays from Parquet into GPU tensors. - - Args: - motion_file: Path to Parquet partition directory. - device: Target torch device. - spawn_root_z: Initial root Z from the spawned robot cfg used to - calibrate the height offset so the robot stands on the ground. - start_frame: First frame to keep (motion_v1 only). - end_frame: One past the last frame to keep. None = full. - """ - replay = load_replay_trajectory( - motion_file, start_frame=start_frame, end_frame=end_frame - ) - self.fps = replay.fps - self.num_frames = replay.num_frames - self.robot_layout = replay.robot_layout - - if replay.object_traj is not None: - self.object_root_pos = torch.tensor( - replay.object_traj.root_position, - dtype=torch.float32, - device=device, - ) - self.object_root_wxyz = torch.tensor( - replay.object_traj.root_wxyz, - dtype=torch.float32, - device=device, - ) - else: - self.object_root_pos = torch.empty(0, 3, dtype=torch.float32, device=device) - self.object_root_wxyz = torch.empty( - 0, 4, dtype=torch.float32, device=device - ) - - if isinstance(replay, SingleRobotTrajectory): - self.robot_joint_names = list(replay.robot_joint_names) - self.robot_root_pos = torch.tensor( - replay.robot_root_position, dtype=torch.float32, device=device - ) - self.robot_root_wxyz = torch.tensor( - replay.robot_root_wxyz, dtype=torch.float32, device=device - ) - self.robot_joint_pos = torch.tensor( - replay.robot_joint_positions, dtype=torch.float32, device=device - ) - - first_root_z = float(self.robot_root_pos[0, 2].item()) - # Auto-detect legacy vs grounded retarget output: - # - Legacy data had root Z near 0 and needs a global lift. - # - New retargeted data is already world/ground-aligned and must - # keep robot/object in the same frame (no replay-time offset). - legacy_root_z_threshold = 0.3 - if first_root_z < legacy_root_z_threshold: - self.height_offset = spawn_root_z - first_root_z - self.robot_root_pos[:, 2] += self.height_offset - if self.object_root_pos.shape[0] > 0: - self.object_root_pos[:, 2] += self.height_offset - else: - self.height_offset = 0.0 - return - - if isinstance(replay, DualHandTrajectory): - self.right_joint_names = list(replay.right_joint_names) - self.left_joint_names = list(replay.left_joint_names) - self.right_wrist_pos = torch.tensor( - replay.right_wrist_position, dtype=torch.float32, device=device - ) - self.left_wrist_pos = torch.tensor( - replay.left_wrist_position, dtype=torch.float32, device=device - ) - if replay.wrist_orientation_format == "wxyz": - self.right_wrist_wxyz = torch.tensor( - replay.right_wrist_orientation, dtype=torch.float32, device=device - ) - self.left_wrist_wxyz = torch.tensor( - replay.left_wrist_orientation, dtype=torch.float32, device=device - ) - else: - right_wxyz = [ - R.from_euler("XYZ", xyz, degrees=False).as_quat(scalar_first=True) - for xyz in replay.right_wrist_orientation - ] - left_wxyz = [ - R.from_euler("XYZ", xyz, degrees=False).as_quat(scalar_first=True) - for xyz in replay.left_wrist_orientation - ] - self.right_wrist_wxyz = torch.tensor( - right_wxyz, dtype=torch.float32, device=device - ) - self.left_wrist_wxyz = torch.tensor( - left_wxyz, dtype=torch.float32, device=device - ) - self.right_finger_joints = torch.tensor( - replay.right_finger_joints, dtype=torch.float32, device=device - ) - self.left_finger_joints = torch.tensor( - replay.left_finger_joints, dtype=torch.float32, device=device - ) - self.height_offset = 0.0 - return - - raise ValueError(f"Unsupported replay trajectory type: {type(replay).__name__}") - - -def _build_joint_reorder( - parquet_names: list[str], sim_names: list[str] -) -> torch.Tensor | None: - """Map from Parquet joint order → Isaac joint order. None if already identical.""" - if parquet_names == sim_names: - return None - sim_name_to_idx = {n: i for i, n in enumerate(sim_names)} - indices: list[int] = [] - for pq_name in parquet_names: - if pq_name not in sim_name_to_idx: - raise ValueError( - f"Parquet joint '{pq_name}' not found in spawned robot joints: " - f"{sim_names}" - ) - indices.append(sim_name_to_idx[pq_name]) - return torch.tensor(indices, dtype=torch.long) - - -def _write_single_robot_frame( - robot: Any, - traj: Trajectory, - t: int, - env_origins: torch.Tensor, - num_envs: int, - device: torch.device, - sim_joint_names: list[str], - reorder: torch.Tensor | None, -) -> None: - root_pos = traj.robot_root_pos[t].unsqueeze(0).expand(num_envs, -1) - root_wxyz = traj.robot_root_wxyz[t].unsqueeze(0).expand(num_envs, -1) - root_pos_w = root_pos + env_origins - root_pose = torch.cat([root_pos_w, root_wxyz], dim=-1) - robot.write_root_pose_to_sim(root_pose) - robot.write_root_velocity_to_sim(torch.zeros(num_envs, 6, device=device)) - - joint_pos_t = traj.robot_joint_pos[t].unsqueeze(0).expand(num_envs, -1) - if reorder is not None: - sim_jpos = torch.zeros(num_envs, len(sim_joint_names), device=device) - sim_jpos[:, reorder] = joint_pos_t - else: - sim_jpos = joint_pos_t - robot.write_joint_state_to_sim(sim_jpos, torch.zeros_like(sim_jpos)) - - -def _write_dual_hand_frame( - right_robot: Any, - left_robot: Any, - traj: Trajectory, - t: int, - env_origins: torch.Tensor, - num_envs: int, - device: torch.device, - right_sim_joint_names: list[str], - left_sim_joint_names: list[str], - right_reorder: torch.Tensor | None, - left_reorder: torch.Tensor | None, -) -> None: - right_pos = traj.right_wrist_pos[t].unsqueeze(0).expand(num_envs, -1) + env_origins - right_wxyz = traj.right_wrist_wxyz[t].unsqueeze(0).expand(num_envs, -1) - right_pose = torch.cat([right_pos, right_wxyz], dim=-1) - right_robot.write_root_pose_to_sim(right_pose) - right_robot.write_root_velocity_to_sim(torch.zeros(num_envs, 6, device=device)) - - left_pos = traj.left_wrist_pos[t].unsqueeze(0).expand(num_envs, -1) + env_origins - left_wxyz = traj.left_wrist_wxyz[t].unsqueeze(0).expand(num_envs, -1) - left_pose = torch.cat([left_pos, left_wxyz], dim=-1) - left_robot.write_root_pose_to_sim(left_pose) - left_robot.write_root_velocity_to_sim(torch.zeros(num_envs, 6, device=device)) - - right_joints = traj.right_finger_joints[t].unsqueeze(0).expand(num_envs, -1) - if right_reorder is not None: - right_sim_joints = torch.zeros( - num_envs, len(right_sim_joint_names), device=device - ) - right_sim_joints[:, right_reorder] = right_joints - else: - right_sim_joints = right_joints - right_robot.write_joint_state_to_sim( - right_sim_joints, - torch.zeros_like(right_sim_joints), - ) - - left_joints = traj.left_finger_joints[t].unsqueeze(0).expand(num_envs, -1) - if left_reorder is not None: - left_sim_joints = torch.zeros( - num_envs, len(left_sim_joint_names), device=device - ) - left_sim_joints[:, left_reorder] = left_joints - else: - left_sim_joints = left_joints - left_robot.write_joint_state_to_sim( - left_sim_joints, - torch.zeros_like(left_sim_joints), - ) - - -def _write_object_frame( - obj: Any | None, - traj: Trajectory, - t: int, - env_origins: torch.Tensor, - num_envs: int, - device: torch.device, -) -> None: - if obj is None or t >= traj.object_root_pos.shape[0]: - return - obj_pos = traj.object_root_pos[t].unsqueeze(0).expand(num_envs, -1) - obj_wxyz = traj.object_root_wxyz[t].unsqueeze(0).expand(num_envs, -1) - obj_pos_w = obj_pos + env_origins - obj_pose = torch.cat([obj_pos_w, obj_wxyz], dim=-1) - obj.write_root_pose_to_sim(obj_pose) - obj.write_root_velocity_to_sim(torch.zeros(num_envs, 6, device=device)) - - -# ============================================================================= -# Contact-mask visualization -# ============================================================================= - - -# Replay-time anchor for contact markers. `replay_data.load_replay_trajectory` -# deliberately drops per-side wrist + contact_active tensors, so we re-read -# them directly from the motion_v1 parquet via `load_motion_data_parquet`. This -# keeps the primary replay loader lean and single-purpose while allowing the -# script to overlay optional diagnostics. -class ContactOverlay: - """Per-side wrist positions + binary contact mask, ready for per-frame draw. - - Attributes are set for every side that has both a wrist trajectory and a - contact mask on disk. If either is missing for a side, `has_left`/`has_right` - stays False and the marker for that side will never be shown. - """ - - def __init__( - self, - motion_file: str, - device: torch.device, - start_frame: int = 0, - end_frame: int | None = None, - ) -> None: - """Load contact overlay data, or no-op if the parquet lacks the fields.""" - self.has_left: bool = False - self.has_right: bool = False - self.left_wrist_pos: torch.Tensor | None = None - self.right_wrist_pos: torch.Tensor | None = None - self.left_active: torch.Tensor | None = None - self.right_active: torch.Tensor | None = None - - try: - md = load_motion_data_parquet(motion_file, device=str(device)) - md = md.trim(start_frame, end_frame) - except Exception as exc: - print(f"[WARN] Contact overlay disabled (motion_v1 load failed): {exc}") - return - - for side in ("left", "right"): - wrist = getattr(md, f"{side}_wrist_position", None) - active = getattr(md, f"{side}_hand_contact_active", None) - if wrist is None or active is None: - continue - # Align masks to the shorter of the two in case of drift. - n = min(wrist.shape[0], active.shape[0]) - setattr(self, f"{side}_wrist_pos", wrist[:n].to(device).float()) - setattr(self, f"{side}_active", active[:n].to(device).float()) - setattr(self, f"has_{side}", True) - - if not (self.has_left or self.has_right): - print( - "[INFO] No per-side wrist + contact_active in motion file; " - "skipping contact overlay." - ) - - -def _sphere_marker_cfg( - prim_path: str, - rgb: tuple[float, float, float], - radius: float = 0.03, -) -> VisualizationMarkersCfg: - """Colored sphere marker at a fixed radius (default ≈ 3 cm).""" - return VisualizationMarkersCfg( - prim_path=prim_path, - markers={ - "sphere": sim_utils.SphereCfg( - radius=radius, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=rgb), - ), - }, - ) - - -def _draw_contact_marker( - marker: VisualizationMarkers | None, - is_active: bool, - wrist_pos_t: torch.Tensor, - env_origins: torch.Tensor, -) -> None: - """Place `marker` at the wrist, toggling visibility from `is_active`. - - We keep the sphere anchored at the wrist regardless of state so that the - first `is_active=True` frame doesn't momentarily flash at (0, 0, 0) before - USD picks up the new transform. Visibility alone drives on/off. - """ - if marker is None: - return - translations = ( - wrist_pos_t.unsqueeze(0).expand(env_origins.shape[0], -1) + env_origins - ) - marker.visualize(translations=translations) - marker.set_visibility(bool(is_active)) - - -def _find_body_idx(body_names: list[str], candidates: tuple[str, ...]) -> int | None: - """Return the first body index matching one of ``candidates``, else None. - - Lets callers probe for a palm link that may be named differently across - URDF variants (``*_hand_palm_link`` on the dex-hand G1, ``*_wrist_yaw_link`` - on the no-hand variants). - """ - for name in candidates: - if name in body_names: - return body_names.index(name) - return None - - -def _frame_marker_cfg(prim_path: str, scale: float) -> VisualizationMarkersCfg: - """RGB xyz-axis frame marker (same asset used by the tracking command). - - Scale is uniform — FRAME_MARKER_CFG's ``markers["frame"].scale`` is a - ``(sx, sy, sz)`` tuple. We replicate to keep axes equal-length. - """ - cfg = FRAME_MARKER_CFG.replace(prim_path=prim_path) - cfg.markers["frame"].scale = (scale, scale, scale) - return cfg - - -# ============================================================================= -# Main -# ============================================================================= - - -def main() -> None: - """Run kinematic trajectory replay.""" - cfg = ReplayEnvCfg(motion_file=args_cli.motion_file) - cfg.scene.num_envs = args_cli.num_envs - - env = ManagerBasedEnv(cfg=cfg) - env.reset() - device = env.device - - spawn_root_z = _get_spawn_root_z(cfg) - traj = Trajectory( - args_cli.motion_file, - device, - spawn_root_z=spawn_root_z, - start_frame=args_cli.start_frame, - end_frame=args_cli.end_frame, - ) - - # Resolve object handle by scene-config name (not hardcoded "object") - object_names = cfg.events.setup_collision_groups.params.get("object_names", []) - obj = env.scene[object_names[0]] if object_names else None - - # Resolve robot handle(s) and reorder maps based on replay layout. - robot = None - right_robot = None - left_robot = None - sim_joint_names = [] - right_sim_joint_names = [] - left_sim_joint_names = [] - reorder = None - right_reorder = None - left_reorder = None - if traj.robot_layout == "single_robot": - robot = env.scene["robot"] - sim_joint_names = list(robot.joint_names) - reorder = _build_joint_reorder(traj.robot_joint_names, sim_joint_names) - else: - right_robot = env.scene["right_robot"] - left_robot = env.scene["left_robot"] - right_sim_joint_names = list(right_robot.joint_names) - left_sim_joint_names = list(left_robot.joint_names) - right_reorder = _build_joint_reorder( - traj.right_joint_names, right_sim_joint_names - ) - left_reorder = _build_joint_reorder(traj.left_joint_names, left_sim_joint_names) - - num_envs = env.num_envs - env_origins = env.scene.env_origins # (num_envs, 3) - - # Contact overlay (motion_v1 parquets only). Anchors at each wrist; shows - # while the per-frame contact-active mask is on. - contact_overlay = ContactOverlay( - args_cli.motion_file, - device, - start_frame=args_cli.start_frame, - end_frame=args_cli.end_frame, - ) - if traj.height_offset != 0.0: - # Apply the same Z lift the replay loader used so markers sit on the - # actual kinematic wrists (legacy whole-body data only). - if contact_overlay.has_left and contact_overlay.left_wrist_pos is not None: - contact_overlay.left_wrist_pos = contact_overlay.left_wrist_pos.clone() - contact_overlay.left_wrist_pos[:, 2] += traj.height_offset - if contact_overlay.has_right and contact_overlay.right_wrist_pos is not None: - contact_overlay.right_wrist_pos = contact_overlay.right_wrist_pos.clone() - contact_overlay.right_wrist_pos[:, 2] += traj.height_offset - left_contact_marker: VisualizationMarkers | None = None - right_contact_marker: VisualizationMarkers | None = None - if contact_overlay.has_left: - left_contact_marker = VisualizationMarkers( - _sphere_marker_cfg( - "/Visuals/Replay/left_hand_contact", rgb=(0.95, 0.25, 0.25) - ) - ) - left_contact_marker.set_visibility(False) - if contact_overlay.has_right: - right_contact_marker = VisualizationMarkers( - _sphere_marker_cfg( - "/Visuals/Replay/right_hand_contact", rgb=(0.25, 0.85, 0.35) - ) - ) - right_contact_marker.set_visibility(False) - - # Anchor frame markers (xyz axes, same asset the tracking command uses). - # Palm markers read from ``robot.data.{body_pos_w, body_quat_w}`` so they - # need an index resolved against the spawned articulation; if no palm-like - # body is found for a side, that side's marker is skipped. For dual-hand - # layouts we fall back to the wrist-root pose (which *is* the palm for the - # floating hand robots). - root_marker: VisualizationMarkers | None = None - left_palm_marker: VisualizationMarkers | None = None - right_palm_marker: VisualizationMarkers | None = None - object_marker: VisualizationMarkers | None = None - left_palm_idx: int | None = None - right_palm_idx: int | None = None - - if traj.robot_layout == "single_robot": - assert robot is not None - root_marker = VisualizationMarkers( - _frame_marker_cfg("/Visuals/Replay/robot_root", scale=0.20) - ) - body_names = list(robot.body_names) - left_palm_idx = _find_body_idx( - body_names, - ("left_hand_palm_link", "left_wrist_yaw_link", "left_wrist_roll_link"), - ) - right_palm_idx = _find_body_idx( - body_names, - ("right_hand_palm_link", "right_wrist_yaw_link", "right_wrist_roll_link"), - ) - if left_palm_idx is None: - print("[WARN] No palm-like body found for left arm; marker disabled.") - if right_palm_idx is None: - print("[WARN] No palm-like body found for right arm; marker disabled.") - - if (traj.robot_layout == "single_robot" and left_palm_idx is not None) or ( - traj.robot_layout == "dual_hand" - ): - left_palm_marker = VisualizationMarkers( - _frame_marker_cfg("/Visuals/Replay/left_palm", scale=0.10) - ) - if (traj.robot_layout == "single_robot" and right_palm_idx is not None) or ( - traj.robot_layout == "dual_hand" - ): - right_palm_marker = VisualizationMarkers( - _frame_marker_cfg("/Visuals/Replay/right_palm", scale=0.10) - ) - - if obj is not None and traj.object_root_pos.shape[0] > 0: - object_marker = VisualizationMarkers( - _frame_marker_cfg("/Visuals/Replay/object_root", scale=0.15) - ) - - # Foot-contact markers: small green spheres anchored to the ankle-roll - # links; visibility toggles with the Z-threshold proxy defined above. - # Single-robot only — dual-hand layouts have no legs. - left_foot_marker: VisualizationMarkers | None = None - right_foot_marker: VisualizationMarkers | None = None - left_ankle_idx: int | None = None - right_ankle_idx: int | None = None - if traj.robot_layout == "single_robot": - assert robot is not None - body_names = list(robot.body_names) - left_ankle_idx = _find_body_idx(body_names, ("left_ankle_roll_link",)) - right_ankle_idx = _find_body_idx(body_names, ("right_ankle_roll_link",)) - if left_ankle_idx is not None: - left_foot_marker = VisualizationMarkers( - _sphere_marker_cfg( - "/Visuals/Replay/left_foot_contact", - rgb=(0.2, 1.0, 0.3), - radius=0.02, - ) - ) - left_foot_marker.set_visibility(False) - if right_ankle_idx is not None: - right_foot_marker = VisualizationMarkers( - _sphere_marker_cfg( - "/Visuals/Replay/right_foot_contact", - rgb=(0.2, 1.0, 0.3), - radius=0.02, - ) - ) - right_foot_marker.set_visibility(False) - - actions = torch.zeros(num_envs, 0, device=device) - frame_f = 0.0 - frame_step = cfg.sim.dt * traj.fps * args_cli.speed - loop = not args_cli.no_loop - - print( - f"[INFO] Replaying {traj.num_frames} frames at {traj.fps:.0f} fps " - f"(speed={args_cli.speed}x, loop={loop})" - ) - if contact_overlay.has_left or contact_overlay.has_right: - sides = [s for s in ("left", "right") if getattr(contact_overlay, f"has_{s}")] - print(f"[INFO] Contact overlay enabled for: {', '.join(sides)}") - print("[INFO] Close the window to exit.") - - while simulation_app.is_running(): - frame_i = int(frame_f) - if frame_i >= traj.num_frames: - if loop: - frame_f -= traj.num_frames - frame_i = int(frame_f) - else: - break - t = frame_i - - if traj.robot_layout == "single_robot": - _write_single_robot_frame( - robot=robot, - traj=traj, - t=t, - env_origins=env_origins, - num_envs=num_envs, - device=device, - sim_joint_names=sim_joint_names, - reorder=reorder, - ) - else: - _write_dual_hand_frame( - right_robot=right_robot, - left_robot=left_robot, - traj=traj, - t=t, - env_origins=env_origins, - num_envs=num_envs, - device=device, - right_sim_joint_names=right_sim_joint_names, - left_sim_joint_names=left_sim_joint_names, - right_reorder=right_reorder, - left_reorder=left_reorder, - ) - - _write_object_frame( - obj=obj, - traj=traj, - t=t, - env_origins=env_origins, - num_envs=num_envs, - device=device, - ) - - if contact_overlay.has_left: - assert contact_overlay.left_wrist_pos is not None - assert contact_overlay.left_active is not None - tl = min(t, contact_overlay.left_wrist_pos.shape[0] - 1) - _draw_contact_marker( - left_contact_marker, - bool(contact_overlay.left_active[tl].item() > 0.5), - contact_overlay.left_wrist_pos[tl], - env_origins, - ) - if contact_overlay.has_right: - assert contact_overlay.right_wrist_pos is not None - assert contact_overlay.right_active is not None - tr = min(t, contact_overlay.right_wrist_pos.shape[0] - 1) - _draw_contact_marker( - right_contact_marker, - bool(contact_overlay.right_active[tr].item() > 0.5), - contact_overlay.right_wrist_pos[tr], - env_origins, - ) - - env.step(actions) - - # Anchor frame markers: placed *after* env.step so body_pos_w / - # body_quat_w reflect the joint state we just wrote (palms need FK, - # which is refreshed during env.step). Root / object markers use - # trajectory values directly; either timing works for those but we - # keep them grouped for clarity. - if root_marker is not None and traj.robot_layout == "single_robot": - pos = traj.robot_root_pos[t].unsqueeze(0).expand(num_envs, -1) + env_origins - quat = traj.robot_root_wxyz[t].unsqueeze(0).expand(num_envs, -1) - root_marker.visualize(translations=pos, orientations=quat) - - if traj.robot_layout == "single_robot": - assert robot is not None - if left_palm_marker is not None and left_palm_idx is not None: - left_palm_marker.visualize( - translations=robot.data.body_pos_w[:, left_palm_idx, :], - orientations=robot.data.body_quat_w[:, left_palm_idx, :], - ) - if right_palm_marker is not None and right_palm_idx is not None: - right_palm_marker.visualize( - translations=robot.data.body_pos_w[:, right_palm_idx, :], - orientations=robot.data.body_quat_w[:, right_palm_idx, :], - ) - else: - if left_palm_marker is not None: - left_palm_marker.visualize( - translations=( - traj.left_wrist_pos[t].unsqueeze(0).expand(num_envs, -1) - + env_origins - ), - orientations=traj.left_wrist_wxyz[t] - .unsqueeze(0) - .expand(num_envs, -1), - ) - if right_palm_marker is not None: - right_palm_marker.visualize( - translations=( - traj.right_wrist_pos[t].unsqueeze(0).expand(num_envs, -1) - + env_origins - ), - orientations=traj.right_wrist_wxyz[t] - .unsqueeze(0) - .expand(num_envs, -1), - ) - - if object_marker is not None and t < traj.object_root_pos.shape[0]: - pos = ( - traj.object_root_pos[t].unsqueeze(0).expand(num_envs, -1) + env_origins - ) - quat = traj.object_root_wxyz[t].unsqueeze(0).expand(num_envs, -1) - object_marker.visualize(translations=pos, orientations=quat) - - # Foot-contact markers: anchor at each ankle-roll link; visibility - # toggles on when the ankle-roll Z (above env origin) is below the - # contact threshold. All envs share the same kinematic trajectory, so - # env-0's Z is sufficient to drive the single USD visibility flag. - if left_foot_marker is not None and left_ankle_idx is not None: - assert robot is not None - ankle_pos = robot.data.body_pos_w[:, left_ankle_idx, :] - left_foot_marker.visualize(translations=ankle_pos) - z_rel = (ankle_pos[0, 2] - env_origins[0, 2]).item() - left_foot_marker.set_visibility(z_rel < FOOT_CONTACT_Z_THRESHOLD) - if right_foot_marker is not None and right_ankle_idx is not None: - assert robot is not None - ankle_pos = robot.data.body_pos_w[:, right_ankle_idx, :] - right_foot_marker.visualize(translations=ankle_pos) - z_rel = (ankle_pos[0, 2] - env_origins[0, 2]).item() - right_foot_marker.set_visibility(z_rel < FOOT_CONTACT_Z_THRESHOLD) - - frame_f += frame_step - if t > 0 and t % 100 == 0: - secs = t / traj.fps if traj.fps else 0 - print(f"[INFO] Frame {t}/{traj.num_frames} ({secs:.1f}s)") - - env.close() - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/replay_motion_viser.py b/robotic_grounding/scripts/replay_motion_viser.py deleted file mode 100644 index 60549e4d..00000000 --- a/robotic_grounding/scripts/replay_motion_viser.py +++ /dev/null @@ -1,76 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Viser-based replay for motion_v1 parquets. - -Loads a partition and opens a browser-accessible viser scene with a Frame -slider and Play/FPS/Loop/Step controls for data-quality inspection. - -Shows robot + object + per-side contact markers. The parquet carries no -source body mesh, so no human-body overlay is available in replay. - -Usage: - python scripts/replay_viser.py --motion_file - python scripts/replay_viser.py --motion_file --port 8080 --start-paused - python scripts/replay_viser.py --motion_file --start-frame 120 -""" - -from __future__ import annotations - -import argparse - -from robotic_grounding.retarget.viser_playback import ViserPlayback - - -def _parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument( - "--motion_file", - type=str, - required=True, - help="motion_v1 parquet partition directory or parquet file path.", - ) - parser.add_argument( - "--port", - type=int, - default=8080, - help="Viser HTTP port (default: 8080).", - ) - parser.add_argument( - "--device", - type=str, - default="cpu", - help="Torch device for MotionData tensors (default: cpu).", - ) - parser.add_argument( - "--start-paused", - action="store_true", - help="Open viewer paused so the user can scrub.", - ) - parser.add_argument( - "--start-frame", - type=int, - default=0, - help="Seek to this frame on open (default: 0).", - ) - return parser.parse_args() - - -def main() -> None: - """Open viser on the configured port and enter the tick loop.""" - args = _parse_args() - playback = ViserPlayback( - motion_file=args.motion_file, - port=args.port, - device=args.device, - start_paused=args.start_paused, - start_frame=args.start_frame, - ) - print( - f"[replay_viser] serving on http://localhost:{args.port} " - f"(motion_file={args.motion_file})" - ) - playback.run() - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/retarget/_offline_video.py b/robotic_grounding/scripts/retarget/_offline_video.py deleted file mode 100644 index aa66338b..00000000 --- a/robotic_grounding/scripts/retarget/_offline_video.py +++ /dev/null @@ -1,277 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Headless MP4 renderer that mirrors the viser scene content. - -Companion to ``vis_retargeted.py``. After the viser recording is written, -the same per-frame data (robot qpos, MANO verts, object poses) is fed into -``OfflineVideoRenderer`` which rasterizes via ``pyrender`` (off-screen EGL) -and writes ``.mp4`` next to ``.viser``. - -No browser or Isaac Sim required — everything runs inside the existing -container using pyrender + imageio. -""" - -from __future__ import annotations - -import os - -# Force EGL before pyrender / PyOpenGL is imported. The Isaac Sim image has a -# GPU but no X display; EGL gives us headless off-screen rendering. -os.environ.setdefault("PYOPENGL_PLATFORM", "egl") - -from pathlib import Path -from typing import Any - -import imageio.v3 as iio -import numpy as np -import pinocchio as pin -import pyrender -import trimesh - - -def _look_at( - eye: np.ndarray, - target: np.ndarray, - up: tuple[float, float, float] = (0, 0, 1), -) -> np.ndarray: - """Build a 4x4 camera pose in pyrender convention (-z forward).""" - eye = np.asarray(eye, dtype=np.float64) - target = np.asarray(target, dtype=np.float64) - up = np.asarray(up, dtype=np.float64) - f = target - eye - f /= np.linalg.norm(f) - s = np.cross(f, up) - s /= np.linalg.norm(s) - u = np.cross(s, f) - R = np.column_stack([s, u, -f]) - T = np.eye(4) - T[:3, :3] = R - T[:3, 3] = eye - return T - - -def _compose(rot: np.ndarray, trans: np.ndarray) -> np.ndarray: - T = np.eye(4) - T[:3, :3] = rot - T[:3, 3] = trans - return T - - -class OfflineVideoRenderer: - """Off-screen pyrender wrapper mirroring ``visualize_one_trajectory``. - - Lifecycle per sequence: - - r = OfflineVideoRenderer(fps=30) - r.add_robot("right", right_kin) - r.add_robot("left", left_kin) - for obj_name, mesh in object_meshes.items(): - r.add_object(obj_name, mesh) - for t in range(num_frames): - r.update_robot("right", right_qpos[t]) - r.update_robot("left", left_qpos[t]) - for name, T in object_poses[t].items(): - r.update_object(name, T) - if has_mano: - r.update_mano("right", mano_right_verts[t], mano_faces) - r.update_mano("left", mano_left_verts[t], mano_faces) - r.capture() - r.save(Path("out.mp4")) - """ - - def __init__( - self, - width: int = 1280, - height: int = 720, - fps: int = 30, - camera_eye: tuple[float, float, float] = (0.8, -0.8, 0.6), - camera_target: tuple[float, float, float] = (0.0, 0.0, 0.1), - ) -> None: - self.fps = max(1, int(fps)) - self._renderer = pyrender.OffscreenRenderer(width, height) - self._scene = pyrender.Scene( - bg_color=[0.92, 0.92, 0.95, 1.0], - ambient_light=[0.35, 0.35, 0.35], - ) - - camera = pyrender.PerspectiveCamera(yfov=np.pi / 3.0) - self._camera_node = self._scene.add( - camera, pose=_look_at(camera_eye, camera_target) - ) - self._camera_yfov = np.pi / 3.0 - - key_light = pyrender.DirectionalLight(color=np.ones(3), intensity=3.5) - key_light_pose = _look_at((1.5, -1.5, 2.0), (0, 0, 0)) - self._scene.add(key_light, pose=key_light_pose) - fill_light = pyrender.DirectionalLight(color=np.ones(3), intensity=1.5) - fill_light_pose = _look_at((-1.0, 1.0, 1.5), (0, 0, 0)) - self._scene.add(fill_light, pose=fill_light_pose) - - # name -> list of (pyrender.Node, pin.GeometryObject) for robot links. - self._robot_nodes: dict[str, list[tuple[pyrender.Node, Any]]] = {} - # name -> pin kinematics handle (captures model / data / visual_model / visual_data) - self._robot_kins: dict[str, Any] = {} - # Single node per object, geometry static, pose changes. - self._object_nodes: dict[str, pyrender.Node] = {} - # MANO nodes are recreated per frame (vertices change). - self._mano_nodes: dict[str, pyrender.Node] = {} - - self._frames: list[np.ndarray] = [] - - # ------------------------------------------------------------------ - # Camera framing - # ------------------------------------------------------------------ - def fit_camera( - self, - points: np.ndarray, - elevation_deg: float = 30.0, - azimuth_deg: float = 45.0, - padding: float = 1.5, - min_extent: float = 0.3, - aspect_ratio: float = 16.0 / 9.0, - ) -> None: - """Re-pose the camera to frame all ``points`` (N, 3). - - - ``target`` = centroid of the point cloud. - - ``eye`` = target + direction(elevation, azimuth) × computed distance. - - Distance is chosen so the bounding sphere fits inside the frustum - (both vertical and horizontal), with ``padding`` slack. - """ - points = np.asarray(points, dtype=np.float64).reshape(-1, 3) - if len(points) == 0: - return - lo = points.min(axis=0) - hi = points.max(axis=0) - target = 0.5 * (lo + hi) - diag = max(np.linalg.norm(hi - lo), min_extent) - # Half-angles: vertical = yfov/2, horizontal = derived from aspect. - tan_v = np.tan(self._camera_yfov / 2.0) - tan_h = tan_v * aspect_ratio - tan_half = min(tan_v, tan_h) - distance = padding * 0.5 * diag / max(tan_half, 1e-6) - - # Elevation from horizontal, azimuth around +Z up-axis. - el = np.deg2rad(elevation_deg) - az = np.deg2rad(azimuth_deg) - direction = np.array( - [np.cos(el) * np.cos(az), np.cos(el) * np.sin(az), np.sin(el)], - dtype=np.float64, - ) - eye = target + distance * direction - self._scene.set_pose(self._camera_node, _look_at(eye, target)) - - # ------------------------------------------------------------------ - # Robot - # ------------------------------------------------------------------ - def add_robot( - self, - name: str, - kinematics: Any, - color: tuple[int, int, int, int] = (200, 200, 215, 255), - ) -> None: - """Load every ``visual_model.geometryObjects[i].meshPath`` as a pyrender node. - - ``kinematics`` is a ``SharpaHandKinematics`` (or compatible); we need - its ``robot.model``, ``robot.data``, ``robot.visual_model``, and - ``robot.visual_data`` attributes — same surface the viser visualizer - uses. - """ - self._robot_kins[name] = kinematics - nodes: list[tuple[pyrender.Node, Any]] = [] - for geom in kinematics.robot.visual_model.geometryObjects: - mesh_path = geom.meshPath - if not mesh_path: - continue - try: - tm = trimesh.load(mesh_path, force="mesh") - except Exception as e: # noqa: BLE001 - print(f"[offline_video] skip {geom.name}: {e}") - continue - if not isinstance(tm, trimesh.Trimesh) or len(tm.vertices) == 0: - continue - # Apply the geometry's own mesh scale. - scale = np.asarray(geom.meshScale).reshape(3) - if not np.allclose(scale, 1.0): - tm.apply_scale(scale) - tm.visual.face_colors = np.array(color, dtype=np.uint8) - pyr = pyrender.Mesh.from_trimesh(tm, smooth=False) - node = self._scene.add(pyr, pose=np.eye(4), name=f"{name}:{geom.name}") - nodes.append((node, geom)) - self._robot_nodes[name] = nodes - - def update_robot(self, name: str, qpos: np.ndarray) -> None: - """Run pinocchio FK on ``qpos`` and set each link's pose.""" - kin = self._robot_kins[name] - pin.forwardKinematics(kin.robot.model, kin.robot.data, qpos) - pin.updateGeometryPlacements( - kin.robot.model, - kin.robot.data, - kin.robot.visual_model, - kin.robot.visual_data, - ) - for node, geom in self._robot_nodes[name]: - geom_id = kin.robot.visual_model.getGeometryId(geom.name) - M = kin.robot.visual_data.oMg[geom_id] - self._scene.set_pose(node, _compose(M.rotation, M.translation)) - - # ------------------------------------------------------------------ - # Rigid objects (geometry static, pose changes) - # ------------------------------------------------------------------ - def add_object(self, name: str, mesh: trimesh.Trimesh) -> None: - if name in self._object_nodes: - self._scene.remove_node(self._object_nodes[name]) - pyr = pyrender.Mesh.from_trimesh(mesh, smooth=False) - self._object_nodes[name] = self._scene.add( - pyr, pose=np.eye(4), name=f"object:{name}" - ) - - def update_object(self, name: str, T: np.ndarray) -> None: - """``T`` is a 4x4 world transform.""" - if name in self._object_nodes: - self._scene.set_pose(self._object_nodes[name], T) - - # ------------------------------------------------------------------ - # MANO hand meshes (vertices change per frame) - # ------------------------------------------------------------------ - def update_mano( - self, - side: str, - vertices: np.ndarray, - faces: np.ndarray, - color: tuple[int, int, int, int] = (230, 180, 170, 200), - ) -> None: - tm = trimesh.Trimesh(vertices=vertices, faces=faces, process=False) - # Use vertex colours so smooth=True doesn't trip the face/vertex check. - tm.visual.vertex_colors = np.tile( - np.array(color, dtype=np.uint8)[None, :], (len(vertices), 1) - ) - pyr = pyrender.Mesh.from_trimesh(tm, smooth=True) - if side in self._mano_nodes: - self._scene.remove_node(self._mano_nodes[side]) - self._mano_nodes[side] = self._scene.add( - pyr, pose=np.eye(4), name=f"mano:{side}" - ) - - # ------------------------------------------------------------------ - # Frame capture / save - # ------------------------------------------------------------------ - def capture(self) -> None: - color, _ = self._renderer.render(self._scene) - self._frames.append(color.copy()) - - def save(self, out_mp4: Path) -> None: - if not self._frames: - print(f"[offline_video] no frames captured, skipping {out_mp4}") - return - out_mp4.parent.mkdir(parents=True, exist_ok=True) - iio.imwrite( - str(out_mp4), - np.stack(self._frames, axis=0), - fps=self.fps, - codec="libx264", - ) - print(f" Saved → {out_mp4} ({len(self._frames)} frames @ {self.fps} fps)") - self._frames.clear() - - def close(self) -> None: - self._renderer.delete() diff --git a/robotic_grounding/scripts/retarget/arctic_to_dex3.py b/robotic_grounding/scripts/retarget/arctic_to_dex3.py deleted file mode 100644 index 280ee6db..00000000 --- a/robotic_grounding/scripts/retarget/arctic_to_dex3.py +++ /dev/null @@ -1,290 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget ARCTIC loaded data (ManoSharpaData with MANO+object only) to Dex3. - -Reads the arctic_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to arctic_processed (robot_name=dex3). - -Usage: - 1. (upstream) reconstruction load workflow writes arctic_loaded Parquet - 2. python scripts/retarget/arctic_to_dex3.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR, MESHES_DIR -from robotic_grounding.retarget.data_logger import ( - ManoDex3Data, - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_dex3_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -# Suppress warnings about joint limits being slightly out of bounds -logging.getLogger().setLevel(logging.ERROR) - -# Default paths: read from arctic_loaded (MANO+object only), write Dex3 robot -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_processed" - -ARCTIC_MESH_DIR = MESHES_DIR / "arctic" - -# Dex3 MANO retargeting does not need a link-to-site wrist offset -LINK_TO_SITE_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_name: str, - object_body_names: list[str], - mesh_dir: Path | None = None, -) -> dict[str, Any]: - """Load ARCTIC object part meshes and add them to the viser scene. - - Returns: - Dict mapping body name to viser mesh handle (for updating position/wxyz per frame). - """ - mesh_root = mesh_dir or ARCTIC_MESH_DIR - handles: dict[str, Any] = {} - for part in object_body_names: - mesh_path = mesh_root / object_name / f"{part}_watertight_tiny.obj" - if not mesh_path.exists(): - continue - mesh = trimesh.load(str(mesh_path)) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Retarget ARCTIC loaded Parquet data to Dex3 (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument( - "--mesh_dir", - type=Path, - default=ARCTIC_MESH_DIR, - help="Directory with ARCTIC object meshes for visualization.", - ) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument( - "--visualize_object_point_clouds", action="store_true", default=False - ) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.0) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded ARCTIC Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_dex3_kinematics = setup_dex3_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_dex3_kinematics = setup_dex3_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - viser_object_handles = _load_object_viser_handles( - viser_server, - data.object_name, - data.object_body_names, - mesh_dir=args.mesh_dir, - ) - - # Run IK for each frame and collect robot_* time series - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=LINK_TO_SITE_XYZW, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=LINK_TO_SITE_XYZW, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_dex3_kinematics, - left_dex3_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_dex3_kinematics.visualize(viser_server, right_qpos) - left_dex3_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - # Free-flyer qpos layout: [pos(3), quat_xyzw(4), fingers(7)] - # Convert xyzw -> wxyz for ManoDex3Data schema - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["robot_name"] = "dex3" - d["right_robot_finger_joint_names"] = list( - right_dex3_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_dex3_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_dex3_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_dex3_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_dex3_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_dex3_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoDex3Data(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/arctic_to_sharpa.py b/robotic_grounding/scripts/retarget/arctic_to_sharpa.py deleted file mode 100644 index 432e3a4f..00000000 --- a/robotic_grounding/scripts/retarget/arctic_to_sharpa.py +++ /dev/null @@ -1,295 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget ARCTIC loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the arctic_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to arctic_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes arctic_loaded Parquet - 2. python scripts/retarget/arctic_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR, MESHES_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) -from scipy.spatial.transform import Rotation as R -from tqdm import tqdm - -# Suppress warnings about joint limits being slightly out of bounds -logging.getLogger().setLevel(logging.ERROR) - -# Default paths: loader output (mano_object_only subdir) -> retarget output -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_processed" - -ARCTIC_MESH_DIR = MESHES_DIR / "arctic" - -# Rotation offset from MANO link frame to site (wxyz); used for wrist init (ARCTIC convention) -ARCTIC_MANO_LINK_TO_SITE_WXYZ = np.array([0.5, -0.5, 0.5, 0.5]) - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_name: str, - object_body_names: list[str], - mesh_dir: Path | None = None, -) -> dict[str, Any]: - """Load ARCTIC object part meshes and add them to the viser scene. - - Returns: - Dict mapping body name to viser mesh handle (for updating position/wxyz per frame). - """ - mesh_root = mesh_dir or ARCTIC_MESH_DIR - handles: dict[str, Any] = {} - for part in object_body_names: - mesh_path = mesh_root / object_name / f"{part}_watertight_tiny.obj" - if not mesh_path.exists(): - continue - mesh = trimesh.load(str(mesh_path)) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Retarget ARCTIC loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument( - "--mesh_dir", - type=Path, - default=ARCTIC_MESH_DIR, - help="Directory with ARCTIC object meshes for visualization.", - ) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument( - "--visualize_object_point_clouds", action="store_true", default=False - ) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.0) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded ARCTIC Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = R.from_quat( - ARCTIC_MANO_LINK_TO_SITE_WXYZ, scalar_first=True - ).as_quat( - scalar_first=False - ) # type: ignore[call-arg] - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - viser_object_handles = _load_object_viser_handles( - viser_server, - data.object_name, - data.object_body_names, - mesh_dir=args.mesh_dir, - ) - - # Run IK for each frame and collect robot_* time series - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - # Build new ManoSharpaData: same metadata + same MANO/object as loaded, new robot_* - # Fill robot metadata from kinematics (loader leaves these empty) - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/create_soma_task.py b/robotic_grounding/scripts/retarget/create_soma_task.py deleted file mode 100644 index 4a49af47..00000000 --- a/robotic_grounding/scripts/retarget/create_soma_task.py +++ /dev/null @@ -1,433 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Scaffold a ReconBody experiment for a retargeted SOMA sequence. - -Given a SOMA ``sequence_id`` (and optionally a robot name + soma subdir), -this script: - -1. Writes ``experiments//config.yaml`` from the same template used - by the existing ``recon_body_*`` whole-body experiments (e.g. - ``experiments/recon_body_snack_box_pick_and_place_01/config.yaml``). -2. Idempotently registers ```` in ``experiments/registry.yaml``, - inserting it under the ``# Whole-body experiments:`` section and - preserving every existing comment / blank line. - -The script is invoked by ``scripts/retarget/process_soma_sequence.sh`` -(stage 6) but is also runnable standalone: - - python scripts/retarget/create_soma_task.py 2026-03-06_10-24-18_snack_box_pick_and_place_01 - -By default the experiment id is derived from the sequence id by stripping -the leading ``YYYY-MM-DD_HH-MM-SS_`` timestamp and prepending -``recon_body_`` (so the example above becomes -``recon_body_snack_box_pick_and_place_01``). Use ``--experiment-id`` to -override. -""" - -from __future__ import annotations - -import argparse -import re -import sys -from dataclasses import dataclass -from pathlib import Path - -REPO_ROOT = Path(__file__).resolve().parents[2] -EXPERIMENTS_DIR = REPO_ROOT / "experiments" -REGISTRY_PATH = EXPERIMENTS_DIR / "registry.yaml" -DEFAULT_MOTION_ROOT_REL = ( - "source/robotic_grounding/robotic_grounding/assets/human_motion_data" -) -DEFAULT_SOMA_SUBDIR = "soma" -DEFAULT_ROBOT_NAME = "g1" - -# Matches the leading "YYYY-MM-DD_HH-MM-SS_" timestamp on every known SOMA -# sequence id (e.g. "2026-03-06_10-24-18_snack_box_pick_and_place_01"). -_TIMESTAMP_PREFIX = re.compile(r"^\d{4}-\d{2}-\d{2}_\d{2}-\d{2}-\d{2}_") - -# Matches the "# Whole-body experiments" section header in registry.yaml. -_WHOLE_BODY_HEADER = re.compile(r"^\s*#\s*Whole-body experiments") - - -def default_experiment_id(sequence_id: str) -> str: - """Derive ``recon_body_`` from a SOMA sequence id. - - Strips the leading ``YYYY-MM-DD_HH-MM-SS_`` timestamp prefix when - present; otherwise falls back to using the full sequence id as the - short name. - """ - short = _TIMESTAMP_PREFIX.sub("", sequence_id) - return f"recon_body_{short}" - - -def motion_file_relpath( - sequence_id: str, - *, - robot_name: str = DEFAULT_ROBOT_NAME, - soma_subdir: str = DEFAULT_SOMA_SUBDIR, - motion_root_rel: str = DEFAULT_MOTION_ROOT_REL, -) -> str: - """Build the repo-relative motion-file partition path used by experiments.""" - return ( - f"{motion_root_rel}/whole_body/{soma_subdir}/" - f"sequence_id={sequence_id}/robot_name={robot_name}" - ) - - -def render_config_yaml( - *, - exp_id: str, - sequence_id: str, - robot_name: str, - soma_subdir: str, - motion_root_rel: str, -) -> str: - """Render the YAML body for ``experiments//config.yaml``. - - Mirrors the format of the existing ``recon_body_*`` configs so newly - scaffolded experiments are visually indistinguishable from - hand-authored ones. - """ - motion_file = motion_file_relpath( - sequence_id, - robot_name=robot_name, - soma_subdir=soma_subdir, - motion_root_rel=motion_root_rel, - ) - return ( - f"# ReconBody: {exp_id} (auto-scaffolded from create_soma_task.py)\n" - "#\n" - "# Usage:\n" - f"# python robotic_grounding/experiments/run_experiment.py {exp_id} --local\n" - f"id: {exp_id}\n" - f'description: "ReconBody {exp_id} with SONIC JOINT_RESIDUAL"\n' - f"run_name: {exp_id}\n" - "task: SonicG1-ReconBody-v0\n" - f"# Use the explicit repo-relative partition path because whole_body/{soma_subdir} does not\n" - "# follow the 4-part dataset/dataset_retargeted/sequence_id/robot shorthand.\n" - f"motion_file: {motion_file}\n" - "video: true\n" - "num_envs: 4096\n" - "max_iterations: 20000\n" - "zero_actor: true\n" - "logger: wandb\n" - "log_project_name: v2d-whole-body-tpv\n" - "\n" - "train_overrides:\n" - " env.commands.motion.reset_freeze_steps: 50\n" - " env.commands.motion.voc_decay_steps: 10\n" - " env.commands.motion.voc_reset_scale: 1.0\n" - " env.commands.motion.initial_virtual_object_control_curriculum_scale: 1.0\n" - " # Frame range of the source motion to train on. -1 for motion_end_frame\n" - " # means run to the end of the sequence.\n" - " # Use replay_motion_viser.py to visualize the motion and get the frame range.\n" - " env.commands.motion.motion_start_frame: 0\n" - " env.commands.motion.motion_end_frame: -1\n" - " env.episode_length_s: 5.0\n" - " # Reset the shoulder spread to 0.2\n" - " env.commands.motion.reset_shoulder_spread: 0.2\n" - "\n" - "osmo:\n" - " build_image: false\n" - ) - - -@dataclass(frozen=True) -class ConfigWriteResult: - """Outcome of writing an experiment config.yaml.""" - - path: Path - # One of "wrote", "overwrote", "kept-existing". - action: str - - -def write_experiment_config( - *, - exp_id: str, - sequence_id: str, - robot_name: str, - soma_subdir: str, - motion_root_rel: str, - overwrite: bool, - experiments_dir: Path = EXPERIMENTS_DIR, -) -> ConfigWriteResult: - """Materialize ``experiments//config.yaml``. - - Creates the parent directory if needed. When the config already - exists, only overwrites it when ``overwrite=True`` so callers can - keep the operation safe-by-default. - """ - exp_dir = experiments_dir / exp_id - exp_config = exp_dir / "config.yaml" - if exp_config.exists() and not overwrite: - return ConfigWriteResult(path=exp_config, action="kept-existing") - - exp_dir.mkdir(parents=True, exist_ok=True) - body = render_config_yaml( - exp_id=exp_id, - sequence_id=sequence_id, - robot_name=robot_name, - soma_subdir=soma_subdir, - motion_root_rel=motion_root_rel, - ) - pre_existed = exp_config.exists() - exp_config.write_text(body, encoding="utf-8") - return ConfigWriteResult( - path=exp_config, - action="overwrote" if pre_existed else "wrote", - ) - - -@dataclass(frozen=True) -class RegistryUpdateResult: - """Outcome of updating experiments/registry.yaml for a sequence.""" - - path: Path - # One of "noop", "updated-in-place", "inserted-in-section", - # "appended-at-end". - action: str - previous_value: str | None = None - - -def _parse_top_level_value(line: str) -> str | None: - """Return the right-hand value of a ``key: value`` line, ignoring comments. - - Returns ``None`` if the line is not a top-level mapping entry. - """ - if not line or line.startswith((" ", "\t", "#")): - return None - if ":" not in line: - return None - rhs = line.split(":", 1)[1].strip() - if "#" in rhs: - rhs = rhs.split("#", 1)[0].strip() - return rhs - - -def update_registry( - *, - exp_id: str, - exp_dir_name: str | None = None, - registry_path: Path = REGISTRY_PATH, -) -> RegistryUpdateResult: - """Idempotently add / update ``exp_id`` in ``registry.yaml``. - - Behavior: - - If ``registry.yaml`` already maps ``exp_id`` to ``exp_dir_name``, - this is a no-op. - - If ``exp_id`` exists with a different value, the line is rewritten - in place (preserving every other line). - - Otherwise the new entry is inserted at the end of the contiguous - block of entries that follows the ``# Whole-body experiments:`` - section header. If that header is missing the entry is appended at - EOF instead. - """ - if exp_dir_name is None: - exp_dir_name = exp_id - - text = registry_path.read_text(encoding="utf-8") - # Preserve the trailing-newline policy of whatever was on disk so the - # rewrite is minimally disruptive (matches the rest of the file's - # formatting conventions). - trailing_newline = text.endswith("\n") - lines = text.splitlines() - key_prefix = f"{exp_id}:" - - found_idx = None - found_value = None - for i, line in enumerate(lines): - if line.startswith(key_prefix): - value = _parse_top_level_value(line) - if value is not None: - found_idx = i - found_value = value - break - - if found_idx is not None: - if found_value == exp_dir_name: - return RegistryUpdateResult( - path=registry_path, - action="noop", - previous_value=found_value, - ) - lines[found_idx] = f"{exp_id}: {exp_dir_name}" - _write_lines(registry_path, lines, trailing_newline) - return RegistryUpdateResult( - path=registry_path, - action="updated-in-place", - previous_value=found_value, - ) - - new_line = f"{exp_id}: {exp_dir_name}" - header_idx = next( - (i for i, line in enumerate(lines) if _WHOLE_BODY_HEADER.match(line)), - None, - ) - - if header_idx is not None: - # Append at the end of the contiguous block of top-level entries - # that follows the header (stop at first blank line or comment). - insert_idx = header_idx + 1 - while insert_idx < len(lines): - stripped = lines[insert_idx].strip() - if stripped == "" or stripped.startswith("#"): - break - insert_idx += 1 - lines.insert(insert_idx, new_line) - _write_lines(registry_path, lines, trailing_newline) - return RegistryUpdateResult(path=registry_path, action="inserted-in-section") - - if lines and lines[-1].strip() != "": - lines.append("") - lines.append(new_line) - _write_lines(registry_path, lines, trailing_newline) - return RegistryUpdateResult(path=registry_path, action="appended-at-end") - - -def _write_lines(path: Path, lines: list[str], trailing_newline: bool) -> None: - """Write ``lines`` back to ``path`` with a controlled trailing newline.""" - text = "\n".join(lines) + ("\n" if trailing_newline else "") - path.write_text(text, encoding="utf-8") - - -def parse_args(argv: list[str] | None = None) -> argparse.Namespace: - """CLI args.""" - parser = argparse.ArgumentParser( - description=( - "Scaffold experiments//config.yaml for a retargeted " - "SOMA sequence and register it in experiments/registry.yaml." - ), - ) - parser.add_argument( - "sequence_id", - type=str, - help=( - "SOMA sequence id, e.g. " - "'2026-03-06_10-24-18_snack_box_pick_and_place_01'." - ), - ) - parser.add_argument( - "--experiment-id", - type=str, - default=None, - help=( - "Override the experiment id (also used as the directory name " - "and run_name). Default: 'recon_body_' where " - " is with the leading " - "YYYY-MM-DD_HH-MM-SS_ timestamp stripped." - ), - ) - parser.add_argument( - "--robot-name", - type=str, - default=DEFAULT_ROBOT_NAME, - help=f"Robot config name (default: {DEFAULT_ROBOT_NAME}).", - ) - parser.add_argument( - "--soma-subdir", - type=str, - default=DEFAULT_SOMA_SUBDIR, - help=( - "Schema subfolder under /whole_body " - f"(default: {DEFAULT_SOMA_SUBDIR})." - ), - ) - parser.add_argument( - "--motion-root-rel", - type=str, - default=DEFAULT_MOTION_ROOT_REL, - help=( - "Repo-relative motion-data root encoded into config.motion_file " - f"(default: {DEFAULT_MOTION_ROOT_REL})." - ), - ) - parser.add_argument( - "--overwrite", - action="store_true", - help=( - "If the experiment config already exists, overwrite it. " - "Without this flag, an existing config is kept and only the " - "registry entry is reconciled." - ), - ) - parser.add_argument( - "--registry-path", - type=Path, - default=REGISTRY_PATH, - help=f"Path to registry.yaml (default: {REGISTRY_PATH}).", - ) - parser.add_argument( - "--experiments-dir", - type=Path, - default=EXPERIMENTS_DIR, - help=f"Path to experiments/ directory (default: {EXPERIMENTS_DIR}).", - ) - return parser.parse_args(argv) - - -def main(argv: list[str] | None = None) -> int: - """Entry point.""" - args = parse_args(argv) - - exp_id = args.experiment_id or default_experiment_id(args.sequence_id) - - print(f"[create_soma_task] sequence_id : {args.sequence_id}") - print(f"[create_soma_task] experiment_id : {exp_id}") - print(f"[create_soma_task] robot_name : {args.robot_name}") - print(f"[create_soma_task] soma_subdir : {args.soma_subdir}") - - cfg_result = write_experiment_config( - exp_id=exp_id, - sequence_id=args.sequence_id, - robot_name=args.robot_name, - soma_subdir=args.soma_subdir, - motion_root_rel=args.motion_root_rel, - overwrite=args.overwrite, - experiments_dir=args.experiments_dir, - ) - if cfg_result.action == "wrote": - print(f"[create_soma_task] wrote {cfg_result.path}") - elif cfg_result.action == "overwrote": - print(f"[create_soma_task] overwrote {cfg_result.path}") - else: - print( - f"[create_soma_task] kept existing {cfg_result.path} " - "(pass --overwrite to replace)" - ) - - reg_result = update_registry( - exp_id=exp_id, - registry_path=args.registry_path, - ) - if reg_result.action == "noop": - print( - f"[create_soma_task] registry already has '{exp_id}: {exp_id}'; " - "nothing to do." - ) - elif reg_result.action == "updated-in-place": - print( - f"[create_soma_task] updated existing entry to '{exp_id}: {exp_id}' " - f"(was '{reg_result.previous_value}')." - ) - elif reg_result.action == "inserted-in-section": - print( - f"[create_soma_task] inserted '{exp_id}: {exp_id}' under the " - "Whole-body experiments block in " - f"{reg_result.path}." - ) - else: - print( - f"[create_soma_task] appended '{exp_id}: {exp_id}' at end of " - f"{reg_result.path} (no Whole-body header found)." - ) - - print( - "[create_soma_task] launch with: " - f"python robotic_grounding/experiments/run_experiment.py {exp_id} --local" - ) - return 0 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/robotic_grounding/scripts/retarget/dexycb_to_sharpa.py b/robotic_grounding/scripts/retarget/dexycb_to_sharpa.py deleted file mode 100644 index d460f2d8..00000000 --- a/robotic_grounding/scripts/retarget/dexycb_to_sharpa.py +++ /dev/null @@ -1,279 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget DexYCB loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the dexycb_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to dexycb_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes dexycb_loaded Parquet - 2. python scripts/retarget/dexycb_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) - -logging.getLogger().setLevel(logging.ERROR) - -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "dexycb" / "dexycb_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "dexycb" / "dexycb_processed" - -# DexYCB uses no wrist link-to-site rotation offset (same as Hot3D, OakInk2). -DexYCB_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load DexYCB object meshes from stored schema paths and add to viser scene.""" - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the DexYCB-to-Sharpa retargeting script.""" - parser = argparse.ArgumentParser( - description="Retarget DexYCB loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded DexYCB Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = DexYCB_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for seq_idx, sequence_id in enumerate(sequence_ids): - print( - f"[{seq_idx + 1}/{len(sequence_ids)}] starting {sequence_id}", - flush=True, - ) - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - print( - f"[{seq_idx + 1}/{len(sequence_ids)}] {sequence_id}: {num_frames} frames", - flush=True, - ) - - if args.visualize: - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - viser_object_handles = _load_object_viser_handles( - viser_server, - object_mesh_paths, - data.object_body_names, - ) - - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - if t % 50 == 0: - print( - f" [{sequence_id}] frame {t}/{num_frames}", - flush=True, - ) - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/grab_to_sharpa.py b/robotic_grounding/scripts/retarget/grab_to_sharpa.py deleted file mode 100644 index 2d81384e..00000000 --- a/robotic_grounding/scripts/retarget/grab_to_sharpa.py +++ /dev/null @@ -1,267 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget GRAB loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the grab_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to grab_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes grab_loaded Parquet - 2. python scripts/retarget/grab_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -logging.getLogger().setLevel(logging.ERROR) - -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "grab" / "grab_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "grab" / "grab_processed" - -# GRAB uses no wrist link-to-site rotation offset (same as Hot3D, OakInk2, H2O). -GRAB_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load GRAB object meshes from stored schema paths and add to viser scene.""" - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the GRAB-to-Sharpa retargeting script.""" - parser = argparse.ArgumentParser( - description="Retarget GRAB loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded GRAB Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = GRAB_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - viser_object_handles = _load_object_viser_handles( - viser_server, - object_mesh_paths, - data.object_body_names, - ) - - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/h2o_to_sharpa.py b/robotic_grounding/scripts/retarget/h2o_to_sharpa.py deleted file mode 100644 index 9e3f4107..00000000 --- a/robotic_grounding/scripts/retarget/h2o_to_sharpa.py +++ /dev/null @@ -1,279 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget H2O loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the h2o_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to h2o_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes h2o_loaded Parquet - 2. python scripts/retarget/h2o_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) - -logging.getLogger().setLevel(logging.ERROR) - -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "h2o" / "h2o_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "h2o" / "h2o_processed" - -# H2O uses no wrist link-to-site rotation offset (same as Hot3D, OakInk2). -H2O_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load H2O object meshes from stored schema paths and add to viser scene.""" - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the H2O-to-Sharpa retargeting script.""" - parser = argparse.ArgumentParser( - description="Retarget H2O loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded H2O Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = H2O_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for seq_idx, sequence_id in enumerate(sequence_ids): - print( - f"[{seq_idx + 1}/{len(sequence_ids)}] starting {sequence_id}", - flush=True, - ) - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - print( - f"[{seq_idx + 1}/{len(sequence_ids)}] {sequence_id}: {num_frames} frames", - flush=True, - ) - - if args.visualize: - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - viser_object_handles = _load_object_viser_handles( - viser_server, - object_mesh_paths, - data.object_body_names, - ) - - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - if t % 50 == 0: - print( - f" [{sequence_id}] frame {t}/{num_frames}", - flush=True, - ) - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/hot3d_to_sharpa.py b/robotic_grounding/scripts/retarget/hot3d_to_sharpa.py deleted file mode 100644 index cf22cb98..00000000 --- a/robotic_grounding/scripts/retarget/hot3d_to_sharpa.py +++ /dev/null @@ -1,271 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget Hot3D loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the hot3d_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to hot3d_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes hot3d_loaded Parquet - 2. python scripts/retarget/hot3d_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -logging.getLogger().setLevel(logging.ERROR) - -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "hot3d" / "hot3d_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "hot3d" / "hot3d_processed" - -# Hot3D has no wrist link-to-site rotation offset (same as OakInk2). -HOT3D_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load Hot3D object meshes from stored schema paths and add to viser scene. - - Returns dict mapping body name to viser mesh handle. - """ - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the Hot3D-to-Sharpa retargeting script.""" - parser = argparse.ArgumentParser( - description="Retarget Hot3D loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded Hot3D Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = HOT3D_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - viser_object_handles = _load_object_viser_handles( - viser_server, - object_mesh_paths, - data.object_body_names, - ) - - # Collect IK results per frame - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/oakink2_to_sharpa.py b/robotic_grounding/scripts/retarget/oakink2_to_sharpa.py deleted file mode 100644 index 1fc145c0..00000000 --- a/robotic_grounding/scripts/retarget/oakink2_to_sharpa.py +++ /dev/null @@ -1,272 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget OakInk2 loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the oakink2_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to oakink2_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes oakink2_loaded Parquet - 2. python scripts/retarget/oakink2_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -logging.getLogger().setLevel(logging.ERROR) - -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "oakink2" / "oakink2_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "oakink2" / "oakink2_processed" - -# OakInk2 has no wrist link-to-site rotation offset (unlike ARCTIC) -OAKINK2_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load OakInk2 object meshes from stored schema paths and add to viser scene. - - Returns dict mapping body name to viser mesh handle. - """ - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse command-line arguments for the OakInk2-to-Sharpa retargeting script.""" - parser = argparse.ArgumentParser( - description="Retarget OakInk2 loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded OakInk2 Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = OAKINK2_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - print(f"Processing sequence {sequence_id}") - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - viser_object_handles = _load_object_viser_handles( - viser_server, - object_mesh_paths, - data.object_body_names, - ) - - # Collect IK results per frame - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/process_soma_sequence.sh b/robotic_grounding/scripts/retarget/process_soma_sequence.sh deleted file mode 100755 index 90dca282..00000000 --- a/robotic_grounding/scripts/retarget/process_soma_sequence.sh +++ /dev/null @@ -1,425 +0,0 @@ -#!/usr/bin/env bash -# process_soma_sequence.sh -# ---------------------------------------------------------------------- -# End-to-end driver for a single SOMA reconstruction sequence. -# -# Manual mode (default): run all 5 stages, pause for user confirmation -# between stages so each tool's GUI / viser server can be inspected. -# At each prompt: y=run (default), n=skip just this stage, -# s=skip every remaining stage, q=quit. -# Auto mode (--auto): non-interactively run only stage 2 (retarget + -# save) and stage 5 (Isaac Lab replay), plus stage 3 if --with-recon -# is passed. -# -# Stages -# ------ -# 1. soma_loader_probe.py --visualize (manual only) -# 2. soma_to_g1.py --visualize --save (auto: --save only) -# 3. reconstruct_support_surfaces.py (auto: only with --with-recon) -# 4. view_scene.py (Isaac Lab static) (manual only) -# 5. replay_motion.py (Isaac Lab playback) -# -# Usage -# ----- -# scripts/retarget/process_soma_sequence.sh [options] -# -# Options: -# --auto Non-interactive mode (runs 2, optionally 3, 5). -# --with-recon In auto mode, also run stage 3. -# --reconstructed-root P Override raw SOMA reconstructions root -# (default: reconstructed_data/). -# --motion-root P Override saved motion-data root -# (default: -# source/robotic_grounding/robotic_grounding/ -# assets/human_motion_data). -# --robot-name NAME Robot config name (default: g1). -# --soma-subdir DIR Schema subfolder under /whole_body -# (default: soma). -# -h, --help Show this help and exit. -# -# Example -# ------- -# # Manual walkthrough: -# scripts/retarget/process_soma_sequence.sh \ -# 2026-03-06_10-24-18_snack_box_pick_and_place_01 -# -# # Hands-off retarget + replay: -# scripts/retarget/process_soma_sequence.sh \ -# 2026-03-06_10-24-18_snack_box_pick_and_place_01 --auto -# -# # Same, but also rebuild support surfaces: -# scripts/retarget/process_soma_sequence.sh \ -# 2026-03-06_10-24-18_snack_box_pick_and_place_01 \ -# --auto --with-recon -# ---------------------------------------------------------------------- - -set -euo pipefail - -# ---- locate repo root (script lives at /scripts/retarget/) ------ -SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" - -# ---- defaults --------------------------------------------------------- -AUTO=0 -WITH_RECON=0 -ROBOT_NAME="g1" -SOMA_SUBDIR="soma" -RECON_ROOT="${REPO_ROOT}/reconstructed_data" -MOTION_ROOT="${REPO_ROOT}/source/robotic_grounding/robotic_grounding/assets/human_motion_data" -SEQUENCE_ID="" - -usage() { - # Print the contiguous block of leading "# ..." comments after the - # shebang. Stops at the first line that is not a comment, so help - # stays in sync if the banner grows or shrinks. - awk ' - NR == 1 { next } - /^#/ { sub(/^# ?/, ""); print; next } - { exit } - ' "${BASH_SOURCE[0]}" -} - -# ---- arg parsing ------------------------------------------------------ -while [[ $# -gt 0 ]]; do - case "$1" in - -h|--help) - usage - exit 0 - ;; - --auto) - AUTO=1 - shift - ;; - --with-recon) - WITH_RECON=1 - shift - ;; - --reconstructed-root) - [[ $# -ge 2 ]] || { echo "ERROR: --reconstructed-root needs a value" >&2; exit 2; } - RECON_ROOT="$2" - shift 2 - ;; - --motion-root) - [[ $# -ge 2 ]] || { echo "ERROR: --motion-root needs a value" >&2; exit 2; } - MOTION_ROOT="$2" - shift 2 - ;; - --robot-name) - [[ $# -ge 2 ]] || { echo "ERROR: --robot-name needs a value" >&2; exit 2; } - ROBOT_NAME="$2" - shift 2 - ;; - --soma-subdir) - [[ $# -ge 2 ]] || { echo "ERROR: --soma-subdir needs a value" >&2; exit 2; } - SOMA_SUBDIR="$2" - shift 2 - ;; - --) - shift - break - ;; - -*) - echo "ERROR: unknown option: $1" >&2 - echo "Run with --help for usage." >&2 - exit 2 - ;; - *) - if [[ -z "${SEQUENCE_ID}" ]]; then - SEQUENCE_ID="$1" - else - echo "ERROR: unexpected extra positional arg: $1" >&2 - exit 2 - fi - shift - ;; - esac -done - -if [[ -z "${SEQUENCE_ID}" ]]; then - echo "ERROR: sequence_id is required." >&2 - echo "" >&2 - usage >&2 - exit 2 -fi - -# ---- derived paths ---------------------------------------------------- -RECON_DIR="${RECON_ROOT}/${SEQUENCE_ID}" -SOMA_PARQUET_ROOT="${MOTION_ROOT}/whole_body/${SOMA_SUBDIR}" -MOTION_PARTITION="${SOMA_PARQUET_ROOT}/sequence_id=${SEQUENCE_ID}/robot_name=${ROBOT_NAME}" - -# ---- pretty printing -------------------------------------------------- -if [[ -t 1 ]]; then - BOLD='\033[1m' - DIM='\033[2m' - GREEN='\033[32m' - YELLOW='\033[33m' - BLUE='\033[34m' - RED='\033[31m' - RESET='\033[0m' -else - BOLD='' DIM='' GREEN='' YELLOW='' BLUE='' RED='' RESET='' -fi - -banner() { - local title="$1" - echo - printf "${BOLD}${BLUE}================================================================${RESET}\n" - printf "${BOLD}${BLUE} %s${RESET}\n" "$title" - printf "${BOLD}${BLUE}================================================================${RESET}\n" -} - -info() { printf "${DIM}[info]${RESET} %s\n" "$*"; } -warn() { printf "${YELLOW}[warn]${RESET} %s\n" "$*" >&2; } -error() { printf "${RED}[err ]${RESET} %s\n" "$*" >&2; } -ok() { printf "${GREEN}[ ok ]${RESET} %s\n" "$*"; } - -# ---- interactive helpers --------------------------------------------- -# SKIP_ALL is set by the "s" answer in confirm() and consumed by the -# manual-mode dispatcher to short-circuit every remaining stage. -SKIP_ALL=0 - -# Prompt user with [Y/n/s/q]: -# y / yes / -> run the stage (return 0) -# n / no -> skip just this stage (return 1) -# s / skip-all -> skip this and every later stage (return 1, set SKIP_ALL=1) -# q / quit / abort -> exit the script with code 130 -# In --auto mode this is bypassed entirely (we never call confirm()). -confirm() { - local prompt="$1" - local reply - while true; do - printf "${BOLD}%s${RESET} ${DIM}[Y/n/s/q]${RESET} " "$prompt" - if ! read -r reply; then - echo - error "stdin closed; aborting." - exit 130 - fi - case "${reply,,}" in - ""|y|yes) return 0 ;; - n|no) return 1 ;; - s|skip|skip-all|all) - SKIP_ALL=1 - return 1 - ;; - q|quit|abort) - warn "Aborting at user request." - exit 130 - ;; - *) - echo " Please answer y, n, s, or q." - ;; - esac - done -} - -# Echo a command (with a leading "$") then run it. Used so the user -# can copy/paste the exact command if they want to rerun a stage. -run_cmd() { - printf "${DIM}\$ %s${RESET}\n" "$*" - "$@" -} - -# ---- pre-flight ------------------------------------------------------- -banner "SOMA pipeline driver" -info "sequence_id : ${SEQUENCE_ID}" -info "mode : $([[ "${AUTO}" -eq 1 ]] && echo 'auto' || echo 'manual')" -if [[ "${AUTO}" -eq 1 ]]; then - info "stage 3 (recon) : $([[ "${WITH_RECON}" -eq 1 ]] && echo 'enabled' || echo 'skipped')" -fi -info "robot_name : ${ROBOT_NAME}" -info "reconstructed root : ${RECON_ROOT}" -info "motion root : ${MOTION_ROOT}" -info "raw SOMA dir : ${RECON_DIR}" -info "parquet partition : ${MOTION_PARTITION}" - -if [[ ! -d "${RECON_DIR}" ]]; then - error "Raw SOMA reconstruction directory not found: ${RECON_DIR}" - error "Pass --reconstructed-root if your data lives elsewhere." - exit 1 -fi - -cd "${REPO_ROOT}" - -# ---- pinocchio / cmeel loader-path fix -------------------------------- -# The Isaac Lab container ships pinocchio compiled against urdfdom v4 -# and tinyxml2 v10, but the system-installed cmeel-urdfdom is v6 and -# cmeel-tinyxml2 is v11. So ``import pinocchio`` dies with: -# ImportError: liburdfdom_sensor.so.4.0: cannot open shared object file -# (verified with ldd on -# .../cmeel.prefix/lib/python3.11/site-packages/pinocchio/pinocchio_pywrap_default*.so: -# liburdfdom_{model,sensor,world}.so.4.0 => not found, -# libtinyxml2.so.10 => not found). -# The fix is to install older cmeel wheels that ship the matching -# soversions into a separate prefix and prepend their lib/ directory -# to LD_LIBRARY_PATH so the loader finds them BEFORE walking into the -# v6/v11 prefix. -# -# Done once per host (cached at ${PINOCCHIO_DEPS_PREFIX}); subsequent -# runs just re-export LD_LIBRARY_PATH. If pinocchio already imports -# cleanly (different image, apt-installed pinocchio, system loader -# already configured), the whole step is skipped. -setup_pinocchio_ld_path() { - # Fast path: pinocchio imports cleanly with the existing env. - if python -c "import pinocchio" >/dev/null 2>&1; then - info "pinocchio import OK (no LD_LIBRARY_PATH fix needed)" - return 0 - fi - - local cache_dir="${PINOCCHIO_DEPS_PREFIX:-${HOME:-/tmp}/.cache/robotic_grounding/pinocchio_deps}" - local lib_dir="${cache_dir}/cmeel.prefix/lib" - - if [[ ! -f "${lib_dir}/liburdfdom_sensor.so.4.0" || ! -f "${lib_dir}/libtinyxml2.so.10" ]]; then - info "Pinocchio v4/v10 cmeel deps not cached; installing to ${cache_dir}" - mkdir -p "${cache_dir}" - # ``--no-deps`` keeps pip from upgrading cmeel-tinyxml2 to v11 - # (cmeel-urdfdom 4.0.1's metadata names tinyxml2 with a range - # that picks v11 by default; we want v10 specifically). - if ! python -m pip install --target "${cache_dir}" --no-deps \ - "cmeel-urdfdom==4.0.1" "cmeel-tinyxml2==10.0.0" >/dev/null 2>&1; then - warn "pip install failed; stage 2 will hit the original ImportError." - warn "Try running by hand:" - warn " python -m pip install --target '${cache_dir}' --no-deps cmeel-urdfdom==4.0.1 cmeel-tinyxml2==10.0.0" - return 0 - fi - fi - - export LD_LIBRARY_PATH="${lib_dir}${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}}" - info "Pinocchio v4 deps : ${lib_dir} (prepended to LD_LIBRARY_PATH)" - - if ! python -c "import pinocchio" >/dev/null 2>&1; then - warn "pinocchio still fails to import after prepending ${lib_dir}." - warn "Run by hand to see the full traceback:" - warn " LD_LIBRARY_PATH='${lib_dir}':\$LD_LIBRARY_PATH python -c 'import pinocchio'" - fi -} -setup_pinocchio_ld_path - -# ---- stage runners ---------------------------------------------------- -# Each stage is wrapped so it can be skipped (manual mode) or auto-skipped -# (auto mode for stages 1, 4, and conditionally 3). - -stage1_probe() { - banner "Stage 1 / 5 — Probe SOMA loader (visualize raw body+object)" - info "Tip: open http://localhost:8080 once viser is up; Ctrl-C to continue." - run_cmd python scripts/retarget/soma_loader_probe.py \ - "${RECON_DIR}" \ - --visualize || { - local rc=$? - # Ctrl-C inside the viser loop returns 130. That's the expected - # way to leave stage 1, so don't treat it as a failure. - if [[ ${rc} -ne 130 ]]; then - warn "Stage 1 exited with code ${rc}." - fi - } - ok "Stage 1 done." -} - -stage2_retarget() { - local extra=() - if [[ "${AUTO}" -eq 0 ]]; then - extra+=( "--visualize" ) - fi - extra+=( - "--save" - "--robot-name" "${ROBOT_NAME}" - "--motion-root" "${MOTION_ROOT}" - "--soma-subdir" "${SOMA_SUBDIR}" - ) - - banner "Stage 2 / 5 — Retarget SOMA -> ${ROBOT_NAME} (save parquet)" - if [[ "${AUTO}" -eq 0 ]]; then - info "Tip: open http://localhost:8080 once viser is up; Ctrl-C to continue." - fi - run_cmd python scripts/retarget/soma_to_g1.py \ - "${RECON_DIR}" \ - "${extra[@]}" || { - local rc=$? - # Same Ctrl-C tolerance as stage 1 — only meaningful when we - # asked for --visualize (i.e. manual mode). - if [[ "${AUTO}" -eq 0 && ${rc} -eq 130 ]]; then - : - else - error "Stage 2 (retarget) failed with code ${rc}." - exit ${rc} - fi - } - - if [[ ! -d "${MOTION_PARTITION}" ]]; then - error "Expected parquet partition was not produced: ${MOTION_PARTITION}" - error "Check the soma_to_g1.py output above." - exit 1 - fi - ok "Stage 2 done. Parquet at: ${MOTION_PARTITION}" -} - -stage3_recon() { - banner "Stage 3 / 5 — Reconstruct support surfaces" - run_cmd python scripts/reconstruct_support_surfaces.py \ - --input_dir "${SOMA_PARQUET_ROOT}" \ - --sequence_id "${SEQUENCE_ID}" - ok "Stage 3 done." -} - -stage4_view() { - banner "Stage 4 / 5 — Static Isaac Lab scene viewer" - info "Tip: this opens the Isaac Lab GUI. Close the window to continue." - run_cmd python scripts/view_scene.py \ - --motion_file "${MOTION_PARTITION}" - ok "Stage 4 done." -} - -stage5_replay() { - banner "Stage 5 / 5 — Isaac Lab motion replay" - info "Tip: this opens the Isaac Lab GUI. Close the window to finish." - run_cmd python scripts/replay_motion.py \ - --motion_file "${MOTION_PARTITION}" - ok "Stage 5 done." -} - -# ---- mode dispatch ---------------------------------------------------- -if [[ "${AUTO}" -eq 1 ]]; then - # Auto mode: 2 -> (3 if --with-recon) -> 5. No prompts, no GUI on stage 2. - stage2_retarget - if [[ "${WITH_RECON}" -eq 1 ]]; then - stage3_recon - fi - stage5_replay - banner "Auto pipeline complete." - exit 0 -fi - -# Manual mode: each stage is opt-in via [Y/n/s/q]. Once SKIP_ALL has -# been raised by an "s" answer, every later stage is silently skipped -# (no further prompts). -manual_stage() { - local label="$1" # "1", "2", ... - local prompt="$2" - local fn="$3" - local on_skip="${4-}" # optional shell snippet to run when skipped - - if [[ "${SKIP_ALL}" -eq 1 ]]; then - info "Stage ${label} skipped (skip-all)." - if [[ -n "${on_skip}" ]]; then eval "${on_skip}"; fi - return 0 - fi - - # `confirm` returns 1 on n/s; treat that as a normal "skipped" path - # rather than letting `set -e` abort the script. Real stage failures - # still propagate because the stage functions exit on their own. - if confirm "${prompt}"; then - "${fn}" - else - info "Stage ${label} skipped." - if [[ -n "${on_skip}" ]]; then eval "${on_skip}"; fi - fi - return 0 -} - -manual_stage 1 "Run stage 1 (probe + visualize raw SOMA)?" stage1_probe -manual_stage 2 "Run stage 2 (retarget + visualize + save parquet)?" stage2_retarget \ - 'if [[ ! -d "${MOTION_PARTITION}" ]]; then warn "No parquet at ${MOTION_PARTITION}; later stages will fail."; fi' -manual_stage 3 "Run stage 3 (reconstruct support surfaces)?" stage3_recon -manual_stage 4 "Run stage 4 (static Isaac Lab scene view)?" stage4_view -manual_stage 5 "Run stage 5 (Isaac Lab motion replay)?" stage5_replay - -banner "Manual pipeline complete." diff --git a/robotic_grounding/scripts/retarget/run_retarget.py b/robotic_grounding/scripts/retarget/run_retarget.py deleted file mode 100644 index 7a68df78..00000000 --- a/robotic_grounding/scripts/retarget/run_retarget.py +++ /dev/null @@ -1,104 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -r"""Generic retarget (IK) entry point. - -Dispatches to the correct dataset-specific retarget script using the -dataset registry. Replaces the per-dataset if-elif blocks in retarget.yaml. - -Usage:: - - python scripts/retarget/run_retarget.py --dataset taco --robot sharpa_wave \ - --input_dir /data/loaded --output_dir /data/processed --device cuda:0 --save - - # Same dataset, different robot (per-robot retargeter): - python scripts/retarget/run_retarget.py --dataset arctic --robot dex3 ... -""" - -from __future__ import annotations - -import importlib.util -import sys -from pathlib import Path -from types import ModuleType - -# Repo root is two levels up from scripts/retarget/ -REPO_ROOT = Path(__file__).resolve().parent.parent.parent - - -def _load_module_from_path(script_path: str) -> ModuleType: - """Dynamically import a Python module from a file path. - - Registers the module in ``sys.modules`` *before* executing it so that - features like ``@dataclass`` (which look up ``sys.modules[cls.__module__]`` - during class construction) work correctly. - """ - full_path = REPO_ROOT / script_path - if not full_path.exists(): - raise FileNotFoundError(f"Retarget script not found: {full_path}") - module_name = full_path.stem - spec = importlib.util.spec_from_file_location(module_name, str(full_path)) - if spec is None or spec.loader is None: - raise ImportError(f"Could not load module spec for {full_path}") - module = importlib.util.module_from_spec(spec) - sys.modules[module_name] = module - spec.loader.exec_module(module) - return module - - -def main() -> None: - """Parse --dataset, then delegate to the dataset's retarget script.""" - if "--dataset" not in sys.argv: - print("Error: --dataset is required", file=sys.stderr) - print( - "Usage: python scripts/retarget/run_retarget.py --dataset " - "[--robot ] [retarget args...]", - file=sys.stderr, - ) - sys.exit(1) - - idx = sys.argv.index("--dataset") - if idx + 1 >= len(sys.argv): - print("Error: --dataset requires a value", file=sys.stderr) - sys.exit(1) - - dataset_name = sys.argv[idx + 1] - remaining_argv = sys.argv[:idx] + sys.argv[idx + 2 :] - - robot_name = "sharpa_wave" - if "--robot" in remaining_argv: - ridx = remaining_argv.index("--robot") - if ridx + 1 >= len(remaining_argv): - print("Error: --robot requires a value", file=sys.stderr) - sys.exit(1) - robot_name = remaining_argv[ridx + 1] - remaining_argv = remaining_argv[:ridx] + remaining_argv[ridx + 2 :] - - # Import registry - source_dir = str(REPO_ROOT / "source" / "robotic_grounding") - if source_dir not in sys.path: - sys.path.insert(0, source_dir) - - from robotic_grounding.retarget.dataset_registry import ( # noqa: PLC0415 - get_dataset_config, - ) - - config = get_dataset_config(dataset_name) - if robot_name not in config.retarget_scripts: - available = ", ".join(sorted(config.retarget_scripts)) or "" - print( - f"Error: dataset '{dataset_name}' has no retargeter for robot " - f"'{robot_name}'. Available: {available}", - file=sys.stderr, - ) - sys.exit(1) - - module = _load_module_from_path(config.retarget_scripts[robot_name]) - - sys.argv = remaining_argv - args = module.parse_args() - module.main(args) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/retarget/soma_loader_probe.py b/robotic_grounding/scripts/retarget/soma_loader_probe.py deleted file mode 100644 index a9e6832b..00000000 --- a/robotic_grounding/scripts/retarget/soma_loader_probe.py +++ /dev/null @@ -1,467 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Loader-only verification probe for SOMA-to-G1 retargeting. - -Runs inside the retarget Docker image and prints the key SOMA schema / -shape / coordinate-frame information that milestone 2 of the SOMA-to-G1 -plan requires before the full retargeting script is exercised. - -What this probe checks: -- ``soma_params.npz`` contains the expected fields with sane shapes / dtypes -- the loader returns ``joints``, ``joints_wxyz``, ``vertices``, ``num_frames`` -- ``keep_root=False`` semantics: the reconstructed Hips world position lies - near ``transl[t]`` (after the loader's first-frame anchoring is undone) -- coordinate frame: foot Z is below pelvis Z and the ground plane is roughly - consistent with ``ground_plane.json`` - -Usage (inside Docker): - - python scripts/retarget/soma_loader_probe.py - python scripts/retarget/soma_loader_probe.py --visualize - -The ``--visualize`` flag opens a viser server (port 8080) that plays the -SOMA body mesh and the same ``object_mesh/output_aligned.glb`` asset consumed -by ``soma_to_g1.py`` in the loader-anchored source frame so you can confirm -the raw SOMA inputs without involving G1 IK. - -This script is read-only; it does not write parquet output. -""" - -from __future__ import annotations - -import argparse -import json -import time -from pathlib import Path - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget.params import SOMA_JOINTS_ORDER -from robotic_grounding.retarget.read_soma import SOMA -from robotic_grounding.retarget.robot_config import load_robot_config -from scipy.spatial.transform import Rotation as R - - -def parse_args() -> argparse.Namespace: - """CLI args.""" - p = argparse.ArgumentParser(description="SOMA loader probe") - p.add_argument( - "data_folder", - type=str, - help="Path to a SOMA reconstruction folder containing soma_params.npz.", - ) - p.add_argument( - "--identity-model-type", - type=str, - default="mhr", - ) - p.add_argument( - "--soma-data-root", - type=str, - default=None, - ) - p.add_argument( - "--visualize", - action="store_true", - help=( - "Open a viser server on port 8080 and play the SOMA body mesh + " - "object mesh in the loader-anchored source frame. Bypasses the G1 " - "retargeter entirely." - ), - ) - p.add_argument( - "--port", - type=int, - default=8080, - help="Viser port when --visualize is set.", - ) - p.add_argument( - "--fps", - type=float, - default=30.0, - help="Playback frame rate when --visualize is set.", - ) - p.add_argument( - "--anchor", - action="store_true", - help=( - "Apply the loader's first-frame anchoring (Hips at origin, " - "frame-0 root rotation removed). The G1 retargeter does NOT " - "use this anchoring, so the default is to keep the body in " - "raw SOMA world frame, matching what the IK consumes." - ), - ) - p.add_argument( - "--robot-frame", - action="store_true", - help=( - "Rotate the visualized body and object trajectory by the loaded " - "robot config's ``r_world`` so the scene matches the G1 robot " - "convention (X=forward, Y=left, Z=up). Diagnostics still " - "print in both source and robot frames regardless of this " - "flag; this only affects the viser scene." - ), - ) - p.add_argument( - "--robot-name", - type=str, - default="g1", - help=( - "Robot config folder under " - "`source/robotic_grounding/robotic_grounding/retarget/configs/`. " - "Used to resolve the source-to-robot rotation matrix." - ), - ) - return p.parse_args() - - -def _load_object_mesh(folder: Path) -> trimesh.Trimesh | None: - """Load the retargeter object mesh as a single ``trimesh.Trimesh``. - - Returns None when the GLB is missing so the visualizer still runs body-only. - """ - glb = folder / "object_mesh" / "output_aligned.glb" - if not glb.is_file(): - return None - mesh = trimesh.load(glb, force="scene") - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - if not isinstance(mesh, trimesh.Trimesh): - print(f"[probe] WARNING: could not extract a Trimesh from {glb}") - return None - return mesh - - -# Object poses in ``poses.npy`` are in OpenCV camera convention. SOMA -# body lives in a Y-up convention; convert before anchoring so body and -# object share a world frame. Mirrors ``soma_to_g1._convert_object_poses_cv_to_soma``. -_R_CV_TO_SOMA = np.diag([1.0, -1.0, -1.0, 1.0]) - - -def _convert_object_poses_cv_to_soma(object_poses_cv: np.ndarray) -> np.ndarray: - """Apply OpenCV->SOMA world-frame conversion to a (T, 4, 4) object trajectory.""" - return np.einsum("ij,tjk->tik", _R_CV_TO_SOMA, object_poses_cv) - - -def _apply_first_frame_anchor( - object_poses_world: np.ndarray, - transl_first: np.ndarray, - R_first_inv: np.ndarray, -) -> np.ndarray: - """Mirror of ``soma_to_g1._apply_first_frame_anchor_to_objects``. - - Inlined here so the probe stays a single-file dependency-light script. - """ - norm_transform = np.eye(4) - norm_transform[:3, :3] = R_first_inv - norm_transform[:3, 3] = -R_first_inv @ np.asarray(transl_first, dtype=np.float64) - return np.array([norm_transform @ pose for pose in object_poses_world]) - - -def _rotate_points_to_robot( - points: np.ndarray, R_src_to_robot: np.ndarray -) -> np.ndarray: - """Rotate ``(..., 3)`` points from SOMA frame to robot frame (Z-up, X-fwd). - - Mirrors ``WholeBodyKinematics.transform_source_position``: ``p @ R.T``. - """ - return np.asarray(points, dtype=np.float64) @ np.asarray(R_src_to_robot).T - - -def _rotate_object_poses_to_robot( - object_poses: np.ndarray, - R_src_to_robot: np.ndarray, -) -> np.ndarray: - """Rotate (T, 4, 4) homogeneous object poses from SOMA frame to robot frame. - - Applied as a left-multiply by the homogeneous form of ``R_SOMA_TO_ROBOT``, - so both translation and orientation columns end up in the robot frame. - """ - R_homog = np.eye(4) - R_homog[:3, :3] = np.asarray(R_src_to_robot, dtype=np.float64) - return np.einsum("ij,tjk->tik", R_homog, np.asarray(object_poses, dtype=np.float64)) - - -def _visualize_raw_soma( - *, - server: viser.ViserServer, - motion: dict, - soma_faces: np.ndarray | None, - object_mesh: trimesh.Trimesh | None, - object_poses: np.ndarray, - fps: float, -) -> None: - """Drive a viser scene with the loader-anchored body + object trajectory. - - Adds a slider so individual frames can be inspected manually. Frame 0 - is shown initially, then a self-advancing loop steps through frames at - ``fps`` until the user disconnects. - """ - vertices = motion["vertices"] - num_frames = motion["num_frames"] - - body_handle = None - if soma_faces is not None: - body_handle = server.scene.add_mesh_simple( - "/soma/body", - vertices=np.asarray(vertices[0], dtype=np.float32), - faces=np.asarray(soma_faces, dtype=np.int32), - color=(220, 200, 180), - opacity=0.55, - ) - - object_handle = None - if object_mesh is not None and object_poses.shape[0] > 0: - object_handle = server.scene.add_mesh_simple( - "/soma/object", - vertices=np.asarray(object_mesh.vertices, dtype=np.float32), - faces=np.asarray(object_mesh.faces, dtype=np.int32), - color=(120, 200, 255), - ) - - # Wireframe-style XYZ axes at the world origin so the source frame is - # easy to interpret (X red, Y green, Z blue per viser default). - server.scene.add_frame("/soma/origin", axes_length=0.5, axes_radius=0.005) - - frame_slider = server.gui.add_slider( - "frame", min=0, max=num_frames - 1, step=1, initial_value=0 - ) - - def _set_frame(t: int) -> None: - if body_handle is not None: - body_handle.vertices = np.asarray(vertices[t], dtype=np.float32) - if object_handle is not None and t < object_poses.shape[0]: - pose = object_poses[t] - xyzw = R.from_matrix(pose[:3, :3]).as_quat() - wxyz = np.array([xyzw[3], xyzw[0], xyzw[1], xyzw[2]], dtype=np.float64) - object_handle.position = np.asarray(pose[:3, 3], dtype=np.float64) - object_handle.wxyz = wxyz - - @frame_slider.on_update - def _on_slider(_: viser.GuiEvent) -> None: - _set_frame(int(frame_slider.value)) - - _set_frame(0) - print( - f"[probe] viser visualizer running on port {server.get_port()} " - f"({num_frames} frames, fps={fps}). Ctrl+C to stop." - ) - - period = 1.0 / max(fps, 1e-6) - frame_idx = 0 - try: - while True: - frame_slider.value = frame_idx - time.sleep(period) - frame_idx = (frame_idx + 1) % num_frames - except KeyboardInterrupt: - print("[probe] viser stopped.") - - -def main() -> int: - """Run the probe.""" - args = parse_args() - folder = Path(args.data_folder).resolve() - soma_npz = folder / "soma_params.npz" - poses_npy = folder / "poses.npy" - ground_json = folder / "ground_plane.json" - glb = folder / "object_mesh" / "output_aligned.glb" - print(f"[probe] folder : {folder}") - print(f"[probe] soma_params.npz : {soma_npz} (exists={soma_npz.is_file()})") - print(f"[probe] poses.npy : {poses_npy} (exists={poses_npy.is_file()})") - print(f"[probe] ground_plane.json: {ground_json} (exists={ground_json.is_file()})") - print(f"[probe] object_mesh/output_aligned.glb: {glb} (exists={glb.is_file()})") - - raw = np.load(soma_npz, allow_pickle=True) - print() - print("[probe] soma_params.npz fields:") - for key in raw.files: - arr = raw[key] - if arr.dtype == object: - print(f" {key}: dtype=object shape={getattr(arr, 'shape', None)}") - else: - print(f" {key}: dtype={arr.dtype} shape={getattr(arr, 'shape', None)}") - keep_root = bool(np.asarray(raw["keep_root"]).item()) - rotation_repr = ( - str(raw["rotation_repr"].item()) if "rotation_repr" in raw.files else "rotvec" - ) - unit = str(raw["unit"].item()) - identity_model_type = str(raw["identity_model_type"].item()) - print() - print( - f"[probe] keep_root={keep_root} rotation_repr={rotation_repr} unit={unit} " - f"identity_model_type={identity_model_type}" - ) - - soma_names = [str(n) for n in raw["joint_names"]] - if soma_names != SOMA_JOINTS_ORDER: - diff = [ - (i, a, b) - for i, (a, b) in enumerate(zip(soma_names, SOMA_JOINTS_ORDER, strict=False)) - if a != b - ] - print(f"[probe] WARNING: SOMA_JOINTS_ORDER mismatch at indices: {diff[:5]}") - else: - print(f"[probe] SOMA_JOINTS_ORDER matches export ({len(soma_names)} joints).") - - print() - print("[probe] running SOMA.load_motion ...") - device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - soma = SOMA( - data_root=args.soma_data_root, - identity_model_type=args.identity_model_type, - device=device, - ) - motion = soma.load_motion(soma_npz, normalize=args.anchor) - if not args.anchor: - print("[probe] body kept in raw SOMA world frame (matches retarget IK input).") - else: - print("[probe] --anchor: applied first-frame Hips/orient anchoring.") - joints = motion["joints"] - joints_wxyz = motion["joints_wxyz"] - vertices = motion["vertices"] - print( - f"[probe] joints.shape={joints.shape} joints_wxyz.shape={joints_wxyz.shape} " - f"vertices.shape={vertices.shape} num_frames={motion['num_frames']}" - ) - - hips_idx = SOMA_JOINTS_ORDER.index("Hips") - head_idx = SOMA_JOINTS_ORDER.index("Head") - left_foot_idx = SOMA_JOINTS_ORDER.index("LeftFoot") - right_foot_idx = SOMA_JOINTS_ORDER.index("RightFoot") - left_hand_idx = SOMA_JOINTS_ORDER.index("LeftHand") - right_hand_idx = SOMA_JOINTS_ORDER.index("RightHand") - print() - print("[probe] frame 0 source-frame positions (after first-frame anchoring):") - for label, idx in ( - ("Hips", hips_idx), - ("Head", head_idx), - ("LeftFoot", left_foot_idx), - ("RightFoot", right_foot_idx), - ("LeftHand", left_hand_idx), - ("RightHand", right_hand_idx), - ): - print(f" {label:>10s}: {joints[0, idx]}") - - # Transform frame-0 positions into the robot frame (x=forward, y=left, - # z=up) using the loaded robot config's ``r_world`` to confirm the IK - # targets land where the G1 expects them. - R_src_to_robot = np.asarray( - load_robot_config(args.robot_name).r_world, dtype=np.float64 - ) - print() - print( - "[probe] frame 0 robot-frame targets via robot_config.r_world (X=fwd Y=left Z=up):" - ) - for label, idx in ( - ("Hips", hips_idx), - ("Head", head_idx), - ("LeftFoot", left_foot_idx), - ("RightFoot", right_foot_idx), - ("LeftHand", left_hand_idx), - ("RightHand", right_hand_idx), - ): - p_robot = joints[0, idx] @ R_src_to_robot.T - print(f" {label:>10s}: {p_robot}") - body_height = float( - joints[0, head_idx, 0] - - min(joints[0, left_foot_idx, 0], joints[0, right_foot_idx, 0]) - ) - print(f" estimated body height (head_X - min_foot_X): {body_height:+.4f} m") - - print() - print("[probe] foot Z stats over sequence:") - print( - f" LeftFoot Z range : [{joints[:, left_foot_idx, 2].min():+.4f}, " - f"{joints[:, left_foot_idx, 2].max():+.4f}]" - ) - print( - f" RightFoot Z range: [{joints[:, right_foot_idx, 2].min():+.4f}, " - f"{joints[:, right_foot_idx, 2].max():+.4f}]" - ) - print( - f" Hips Z range : [{joints[:, hips_idx, 2].min():+.4f}, " - f"{joints[:, hips_idx, 2].max():+.4f}]" - ) - print( - f" Head Z range : [{joints[:, head_idx, 2].min():+.4f}, " - f"{joints[:, head_idx, 2].max():+.4f}]" - ) - - if ground_json.is_file(): - with ground_json.open() as f: - gp = json.load(f) - print() - print(f"[probe] ground_plane.json: plane={gp.get('plane')}") - stats = gp.get("foot_plane_dist_stats", {}) - if stats: - print( - f"[probe] foot_plane_dist (export-side): mean={stats.get('mean'):.4f} " - f"std={stats.get('std'):.4f} min={stats.get('min'):.4f} " - f"max={stats.get('max'):.4f}" - ) - - object_poses_for_viz: np.ndarray | None = None - if poses_npy.is_file(): - op_cv = np.load(poses_npy) - print() - print(f"[probe] poses.npy shape={op_cv.shape} dtype={op_cv.dtype}") - if op_cv.shape[0] > 0: - print(f" first translation (raw, OpenCV): {op_cv[0, :3, 3]}") - print(f" last translation (raw, OpenCV): {op_cv[-1, :3, 3]}") - # Bring object into the same SOMA world frame as the body before - # anchoring or visualization. - op = _convert_object_poses_cv_to_soma(op_cv) - print(f" first translation (SOMA frame ): {op[0, :3, 3]}") - print(f" last translation (SOMA frame ): {op[-1, :3, 3]}") - if args.anchor: - object_poses_for_viz = _apply_first_frame_anchor( - op, - transl_first=motion["first_frame_transl"], - R_first_inv=motion["first_frame_R_inv"], - ) - else: - object_poses_for_viz = op - - if args.visualize: - soma_faces = soma.faces # (F, 3) ints, exposed by SOMA wrapper - object_mesh = _load_object_mesh(folder) - if object_poses_for_viz is None and object_mesh is not None: - print("[probe] No poses.npy; object mesh will be drawn at origin only.") - object_poses_for_viz = np.tile(np.eye(4), (motion["num_frames"], 1, 1)) - elif object_poses_for_viz is None: - object_poses_for_viz = np.zeros((motion["num_frames"], 4, 4)) - - motion_for_viz = motion - if args.robot_frame: - print( - "[probe] --robot-frame: rotating body + object into robot frame " - "(X=forward, Y=left, Z=up) via R_SOMA_TO_ROBOT for visualization." - ) - motion_for_viz = dict(motion) - motion_for_viz["vertices"] = _rotate_points_to_robot( - motion["vertices"], R_src_to_robot - ) - object_poses_for_viz = _rotate_object_poses_to_robot( - object_poses_for_viz, R_src_to_robot - ) - - server = viser.ViserServer(host="0.0.0.0", port=args.port) - _visualize_raw_soma( - server=server, - motion=motion_for_viz, - soma_faces=soma_faces, - object_mesh=object_mesh, - object_poses=object_poses_for_viz, - fps=args.fps, - ) - - print() - print("[probe] done.") - return 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/robotic_grounding/scripts/retarget/soma_to_g1.py b/robotic_grounding/scripts/retarget/soma_to_g1.py deleted file mode 100644 index a02255c4..00000000 --- a/robotic_grounding/scripts/retarget/soma_to_g1.py +++ /dev/null @@ -1,1222 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Script to retarget SOMA-X (NVlabs SOMA) motion to G1 whole body using Pink IK. - -Reads the SOMA exporter schema: ``soma_params.npz`` + object pose file + -object glTF/OBJ + optional ``ground_plane.json``. - -The output parquet shares the same ``motion_v1`` schema used by training and -replay; only the dataset folder differs: - - HUMAN_MOTION_DATA_DIR / "whole_body" / "soma" / - sequence_id=/robot_name=g1/data.parquet - -Usage: - python scripts/retarget/soma_to_g1.py --save - python scripts/retarget/soma_to_g1.py --visualize - -Where ``data_folder`` contains: - - soma_params.npz (SOMA-X exported pose + identity parameters) - - poses.npy (object trajectory as 4x4 transforms) - - object_mesh/output_aligned.glb (object mesh; converted to .obj for sim) - - ground_plane.json (optional reconstruction-side ground plane fit) -""" - -from __future__ import annotations - -import argparse -import os -import pickle -import shutil -import time -from pathlib import Path - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.motion_schema import MotionData, save_motion_parquet -from robotic_grounding.retarget import G1_URDF_DIR, HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.ground_alignment import ( - FirstPassResult, - InteractionMaskConfig, - ObjectCorrectionConfig, - PlaneAlignmentConfig, - ReferencePlane, - compute_interaction_mask, - compute_plane_alignment_offsets, - correct_object_trajectory, - load_ground_plane_robot_frame, -) -from robotic_grounding.retarget.params import SOMA_JOINTS_ORDER -from robotic_grounding.retarget.read_soma import SOMA -from robotic_grounding.retarget.robot_config import load_robot_config -from robotic_grounding.retarget.viser_playback import LiveFrameState, ViserPlayback -from robotic_grounding.retarget.whole_body_kinematics import ( - WholeBodyKinematics, -) -from scipy.spatial.transform import Rotation as R -from tqdm import tqdm - -G1_URDF = G1_URDF_DIR / "main_with_hand.urdf" -PACKAGE_DIRS = [str(G1_URDF_DIR)] -# First-pass IK snapshot consumed by ``scripts/retarget/rerun_post_process.py``. -# Cache key is suffixed with a schema version. Bump when the cached -# FirstPassResult layout changes (field rename, new required field, etc.) -# so stale pickles from a previous schema are not read back into a -# differently-shaped dataclass. -FIRST_PASS_CACHE_DIR = Path( - os.environ.get("SOMA_FIRST_PASS_CACHE_DIR", "/tmp/soma_g1_processed_cache_v2") -) -REPO_ROOT = Path(__file__).resolve().parents[2] - - -def _usd_safe(name: str) -> str: - """Make a name safe for USD prim paths (no leading digits, no @ etc.).""" - safe = name.replace("@", "_") - if safe and (safe[0].isdigit() or not (safe[0].isalpha() or safe[0] == "_")): - return f"obj_{safe}" - return safe - - -def _convert_glb_to_obj(glb_path: Path, dst_dir: Path) -> Path: - """Convert a SOMA object ``.glb`` into a flat ``textured_mesh.obj`` next to it. - - The robotic_grounding pipeline prefers ``.obj`` next to the parquet so - Isaac Sim's URDF importer can resolve materials/textures without - glTF-specific handling. ``trimesh`` happily concatenates a glTF scene - into a single mesh; this is sufficient for retargeting because we use - the geometry only for contact distance and visualization. - """ - dst_dir.mkdir(parents=True, exist_ok=True) - mesh = trimesh.load(glb_path, force="scene") - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - if not isinstance(mesh, trimesh.Trimesh): - raise ValueError( - f"Could not convert {glb_path} to a single trimesh.Trimesh " - f"(got {type(mesh).__name__})." - ) - obj_path = dst_dir / "textured_mesh.obj" - mesh.export(obj_path) - return obj_path - - -def _build_object_urdf(mesh_path: str, urdf_path: Path) -> str: - """Write a simple rigid-object URDF that references ``mesh_path``.""" - urdf_path.parent.mkdir(parents=True, exist_ok=True) - urdf_text = f""" - - - - - - - - - - - - - - - - - - - - - -""" - urdf_path.write_text(urdf_text, encoding="utf-8") - return str(urdf_path.resolve()) - - -def _compute_mesh_radius(mesh_path: str) -> float: - """Compute max radius from mesh centroid.""" - mesh = trimesh.load(mesh_path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - vertices = np.asarray(mesh.vertices, dtype=np.float64) - if len(vertices) == 0: - return 0.0 - centered = vertices - vertices.mean(axis=0, keepdims=True) - return float(np.linalg.norm(centered, axis=1).max()) - - -def _get_robot_joint_position_names( - kin: WholeBodyKinematics, base_q_size: int -) -> list[str]: - """Return names aligned to ``q[base_q_size:]`` ordering.""" - indexed_names: list[tuple[int, str]] = [] - for joint_idx in range(1, kin.robot.model.njoints): - joint_name = str(kin.robot.model.names[joint_idx]) - q_start = int(kin.robot.model.idx_qs[joint_idx]) - q_size = int(kin.robot.model.nqs[joint_idx]) - for local_idx in range(q_size): - q_idx = q_start + local_idx - if q_idx < base_q_size: - continue - label = joint_name if q_size == 1 else f"{joint_name}[{local_idx}]" - indexed_names.append((q_idx, label)) - indexed_names.sort(key=lambda x: x[0]) - return [name for _, name in indexed_names] - - -def parse_args() -> argparse.Namespace: - """Parse command line arguments.""" - parser = argparse.ArgumentParser( - description="Retarget SOMA motion to G1 whole body" - ) - parser.add_argument( - "data_folder", - type=str, - help=( - "Path to folder containing soma_params.npz, poses.npy, and " - "object_mesh/output_aligned.glb" - ), - ) - parser.add_argument("--visualize", action="store_true", help="Enable visualization") - parser.add_argument("--save", action="store_true", help="Save retargeted data") - parser.add_argument( - "--scale", type=float, default=1.0, help="Scale factor from SOMA to robot" - ) - parser.add_argument( - "--contact-threshold", - type=float, - default=0.02, - help=( - "Distance (m) from palm link to nearest object mesh vertex below " - "which the side is flagged as in contact. Only used with --save." - ), - ) - parser.add_argument( - "--identity-model-type", - type=str, - default="mhr", - help=("SOMA identity model. Must match what was used at export time."), - ) - parser.add_argument( - "--soma-data-root", - type=str, - default=None, - help=( - "Override path to SOMA-X assets. Defaults to " - "/source/robotic_grounding/robotic_grounding/assets/body_models/soma." - ), - ) - parser.add_argument( - "--robot-name", - type=str, - default="g1", - help=( - "Robot config folder under " - "`source/robotic_grounding/robotic_grounding/retarget/configs/`. " - "Defaults to `g1`. The IK end-effector targets, per-bone " - "rotation offsets, URDF path, and ground anchoring parameters " - "all come from `/{frame_alignment,retargeter}.json` instead of " - "the legacy constants in `params.py`." - ), - ) - parser.add_argument( - "--motion-root", - type=Path, - default=HUMAN_MOTION_DATA_DIR, - help=( - "Root directory for saved motion data. The parquet is written " - "under /whole_body//sequence_id=.../" - "robot_name=... Defaults to the in-repo human_motion_data asset root." - ), - ) - parser.add_argument( - "--soma-subdir", - type=str, - default="soma", - help=( - "Dataset subfolder under /whole_body for saved SOMA " - "motion_v1 parquet. Defaults to `soma`." - ), - ) - parser.add_argument( - "--start-frame", - type=int, - default=0, - help=( - "First source frame to retarget (0-indexed, inclusive). " - "Use with --end-frame to focus on a specific segment. The " - "first-frame anchoring (body normalization + object trajectory " - "transform) always runs against the ORIGINAL sequence's frame " - "0, so saved positions stay comparable across different " - "[start, end) windows." - ), - ) - parser.add_argument( - "--end-frame", - type=int, - default=None, - help=( - "One-past-last source frame to retarget (Python-slice " - "semantics). Defaults to the full sequence length. Must be " - "strictly greater than --start-frame." - ), - ) - parser.add_argument( - "--diagnose-ik", - action="store_true", - help=( - "Print per-frame IK diagnostics (which task has the largest " - "residual, total iterations, max_iter saturation, and joint " - "position-limit saturation) for frames whose total residual or " - "iteration count exceeds the thresholds set by " - "--diagnose-ik-error-threshold / --diagnose-ik-iter-fraction. " - "Also prints a top-K worst-frame summary at the end. Use to " - "isolate which IK targets are dominating the QP at frames " - "where the robot looks weird." - ), - ) - parser.add_argument( - "--diagnose-ik-error-threshold", - type=float, - default=0.05, - help=( - "Per-frame total task residual (sum of task position errors, " - "in meters) above which --diagnose-ik prints a one-line " - "report. Default 0.05 m catches obvious failures while " - "keeping clean frames silent. Ignored without --diagnose-ik." - ), - ) - parser.add_argument( - "--diagnose-ik-iter-fraction", - type=float, - default=0.9, - help=( - "Fraction of `max_iter` above which --diagnose-ik flags a " - "frame as 'iter-saturated' and prints a one-line report. " - "Default 0.9 (e.g. 180/200) catches solves that almost ran " - "out of budget. Ignored without --diagnose-ik." - ), - ) - return parser.parse_args() - - -def load_data(folder_path: str) -> tuple[str, str, np.ndarray]: - """Locate SOMA params, raw object poses, and a usable object mesh path. - - Two transforms apply to the object trajectory between disk and the IK - loop. This function applies the first; ``main`` applies the second: - - 1. **CV -> SOMA world** (here, via ``_convert_object_poses_cv_to_soma``). - ``poses.npy`` is in OpenCV camera convention (X=right, Y=down, - Z=forward) while ``SOMALayer.transl`` is in SOMA's "Y up, - Z toward camera" frame. Without this flip the body and object - live in two different worlds. - 2. **First-frame anchoring** (deferred to ``main``). After SOMA loads - the motion we know the frame-0 root translation/rotation; the same - ``(p - transl_first) @ R_first_inv.T`` transform that ``SOMA.load_motion`` - applies to the body must be applied to the object trajectory or - the object slides off in the retargeted output. - - Returns: - Tuple of (soma_params_path, mesh_path, object_poses_world). - ``mesh_path`` is the converted ``.obj``; ``object_poses_world`` is - the ``poses.npy`` array of shape ``(T, 4, 4)`` after the - CV -> SOMA flip but **before** first-frame anchoring. - """ - folder = Path(folder_path).resolve() - soma_params_path = folder / "soma_params.npz" - poses_path = folder / "poses.npy" - object_glb_path = folder / "object_mesh" / "output_aligned.glb" - - required = { - "soma_params.npz": soma_params_path, - "poses.npy": poses_path, - "object_mesh/output_aligned.glb": object_glb_path, - } - missing = [name for name, p in required.items() if not p.is_file()] - if missing: - msg = ( - f"Data folder is missing required files: {missing}\n" - f" Resolved folder: {folder}\n" - f"Expected layout:\n" - f" {folder}/soma_params.npz\n" - f" {folder}/poses.npy\n" - f" {folder}/object_mesh/output_aligned.glb\n" - "If you run inside Docker, host paths like /home/... are not visible " - "unless bind-mounted. Mount your data and pass the in-container path." - ) - raise FileNotFoundError(msg) - - obj_dst_dir = folder / "object" - mesh_path = _convert_glb_to_obj(object_glb_path, obj_dst_dir) - - object_poses_world = np.load(poses_path) - object_poses_world = _convert_object_poses_cv_to_soma(object_poses_world) - return str(soma_params_path), str(mesh_path), object_poses_world - - -# The object trajectory stored in ``poses.npy`` is in OpenCV camera -# convention (X=right, Y=down, Z=forward), while the SOMA body wrapper -# expresses ``transl`` in the body model's "Y up, Z toward camera (negative -# Z forward)" frame. Confirmed empirically on the snack_box_pick sequence: -# negating Y and Z on the object brings body-object distance from -# ~6.6 m down to ~2.0 m at sequence start (which matches the recorded -# scene where the human is ~2 m away from the box) and ~0.35 m at the -# pick-up frame. -_R_CV_TO_SOMA = np.diag([1.0, -1.0, -1.0, 1.0]) - - -def _convert_object_poses_cv_to_soma(object_poses_cv: np.ndarray) -> np.ndarray: - """Convert a (T, 4, 4) object pose trajectory from OpenCV to SOMA world frame. - - Applied via left-multiplication ``T_soma = R_cv2soma @ T_cv``. This - flips the world Y and Z axes so the object lives in the same frame - that ``SOMALayer`` uses for ``transl``. Without this, the body and - object end up in two different worlds and the relative hand-object - pose is meaningless. - """ - return np.einsum("ij,tjk->tik", _R_CV_TO_SOMA, object_poses_cv) - - -def main() -> None: - """Main function.""" - args = parse_args() - save_dir = args.motion_root.expanduser().resolve() / "whole_body" / args.soma_subdir - - data_folder = Path(args.data_folder) - soma_params_path, object_mesh_path, object_poses_world = load_data(args.data_folder) - print(f"Loaded data from {data_folder}") - print(f" SOMA params: {soma_params_path}") - print(f" Object mesh: {object_mesh_path}") - print(f" Object poses: {len(object_poses_world)} frames") - - config = load_robot_config(args.robot_name) - kin = WholeBodyKinematics(config=config) - foot_frame_names = list(config.foot_frames) - ankle_roll_offset = float(config.ankle_roll_offset) - base_q_size = 7 - robot_joint_position_names = _get_robot_joint_position_names(kin, base_q_size) - - device = torch.device("cuda" if torch.cuda.is_available() else "cpu") - soma = SOMA( - data_root=args.soma_data_root, - identity_model_type=args.identity_model_type, - device=device, - ) - # First-frame anchoring: the SOMA exporter writes ``transl`` in raw - # world coordinates, so Hips can sit - # several meters off the origin with arbitrary heading. The in-loop - # ground-anchoring step below assumes the body is already centered at - # the origin with canonical heading; without this normalization the - # frame-0 ``ground_z_offset`` is computed against an off-origin pose - # and the retargeted robot ends up floating / sinking relative to the - # ground plane. The same transform is applied to the object trajectory - # below to keep relative hand-object pose intact. - motion = soma.load_motion(params_path=soma_params_path, normalize=True) - - # Apply the SAME first-frame transform to the object trajectory so the - # body and object stay co-located after anchoring. - # ``object_poses_world`` is already in the SOMA world frame (the CV->SOMA - # axis flip happened in ``load_data``), so we just left-multiply each - # 4x4 pose by ``T_anchor = [[R_first_inv, -R_first_inv @ transl_first], [0, 1]]``. - transl_first = motion["first_frame_transl"] - R_first_inv = motion["first_frame_R_inv"] - norm_transform = np.eye(4) - norm_transform[:3, :3] = R_first_inv - norm_transform[:3, 3] = -R_first_inv @ transl_first - object_poses = np.einsum("ij,tjk->tik", norm_transform, object_poses_world) - - # Optional reconstruction-side ground plane. The same chain of transforms - # that brings ``poses.npy`` into the robot frame must be applied to the - # plane equation. Returns ``None`` when ``ground_plane.json`` is absent so - # the post-process below falls back to the legacy horizontal plane. - ground_plane_path = Path(args.data_folder) / "ground_plane.json" - reconstructed_plane = load_ground_plane_robot_frame( - ground_plane_path, - cv_to_source=_R_CV_TO_SOMA, - first_frame_anchor=norm_transform, - source_to_robot=np.asarray(config.r_world, dtype=np.float64), - ) - if reconstructed_plane is not None: - print( - "[soma_to_g1] grounding plane: reconstructed " - f"(normal={reconstructed_plane.normal}, " - f"offset={reconstructed_plane.offset:+.4f})" - ) - else: - print( - "[soma_to_g1] grounding plane: fallback ReferencePlane.horizontal(z=0.0); " - f"no ground_plane.json found at {ground_plane_path}" - ) - - joint_pos = motion["joints"] - joint_rot_wxyz = motion["joints_wxyz"] - vertices = motion["vertices"] - num_frames = motion["num_frames"] - soma_joint_names = motion["joint_names"] - if soma_joint_names != SOMA_JOINTS_ORDER: - raise ValueError( - "SOMA joint_names do not match SOMA_JOINTS_ORDER. Update params.py " - f"to track the SOMA-X release in use. Got first 5: {soma_joint_names[:5]}" - ) - if len(object_poses) != num_frames: - raise ValueError( - f"poses.npy length ({len(object_poses)}) != motion frames ({num_frames})" - ) - - # Resolve and validate --start-frame / --end-frame against the original - # source length, then slice every per-frame array down to the requested - # window. The first-frame anchoring above already ran against the - # ORIGINAL frame 0, so positions in the saved trajectory stay - # comparable across different windows. - start_frame = int(args.start_frame) - end_frame = int(num_frames) if args.end_frame is None else int(args.end_frame) - if start_frame < 0 or start_frame >= num_frames: - raise ValueError( - f"--start-frame={start_frame} is out of range for a sequence " - f"with {num_frames} frames (valid range: [0, {num_frames - 1}])." - ) - if end_frame <= start_frame or end_frame > num_frames: - raise ValueError( - f"--end-frame={end_frame} must satisfy " - f"start_frame ({start_frame}) < end_frame <= num_frames " - f"({num_frames})." - ) - if (start_frame, end_frame) != (0, num_frames): - print( - f"[INFO] Frame range: iterating IK over [{start_frame}, " - f"{end_frame}) of {num_frames} source frames." - ) - joint_pos = joint_pos[start_frame:end_frame] - joint_rot_wxyz = joint_rot_wxyz[start_frame:end_frame] - vertices = vertices[start_frame:end_frame] - object_poses = object_poses[start_frame:end_frame] - n_iter_frames = int(end_frame - start_frame) - - sequence_id = data_folder.name - - head_idx = SOMA_JOINTS_ORDER.index("Head") - root_idx = SOMA_JOINTS_ORDER.index("Hips") - - object_name = f"{sequence_id}_object" - - _obj_verts = [] - with open(object_mesh_path) as _f: - for _line in _f: - if _line.startswith("v "): - _parts = _line.split() - _obj_verts.append( - [float(_parts[1]), float(_parts[2]), float(_parts[3])] - ) - object_mesh_vertices = np.array(_obj_verts, dtype=np.float64) - object_mesh_vertices_f32 = object_mesh_vertices.astype(np.float32) - contact_threshold = float(args.contact_threshold) - - builder: dict[str, list] | None = None - object_body_names: list[str] = ["object"] - safe_object_body_names: list[str] = [_usd_safe(n) for n in object_body_names] - object_mesh_radius: float = 0.0 - soma_identity_coeffs: list[float] = [] - soma_scale_params: list[float] = [] - frame_names_list: list[str] = list(kin.robot_frame_names.values()) - ee_link_name_candidates: list[str] = [ - "left_hand_palm_link", - "right_hand_palm_link", - ] - ee_frame_indices: list[int] = [ - frame_names_list.index(n) - for n in ee_link_name_candidates - if n in frame_names_list - ] - ee_link_names: list[str] = [frame_names_list[i] for i in ee_frame_indices] - - hand_sides: list[str] = [ - name.split("_")[0] for name in ee_link_names if "_hand_palm_link" in name - ] - - # SOMA fingertip joint names match the canonical MHR rig 1:1. - hand_side_to_fingertip_source_joints: dict[str, list[int]] = {} - for side, prefix in (("left", "Left"), ("right", "Right")): - candidates = [ - f"{prefix}HandThumbEnd", - f"{prefix}HandIndexEnd", - f"{prefix}HandMiddleEnd", - f"{prefix}HandRingEnd", - f"{prefix}HandPinkyEnd", - ] - hand_side_to_fingertip_source_joints[side] = [ - SOMA_JOINTS_ORDER.index(n) for n in candidates if n in SOMA_JOINTS_ORDER - ] - - if args.save: - params = np.load(soma_params_path, allow_pickle=True) - # Identity/scale are constant in time; pick frame 0 for source_payload. - soma_identity_coeffs = params["identity_coeffs"][0].astype(np.float32).tolist() - soma_scale_params = params["scale_params"][0].astype(np.float32).tolist() - - object_mesh_path = str(Path(object_mesh_path).resolve()) - object_mesh_radius = _compute_mesh_radius(object_mesh_path) - - # Copy mesh + materials next to the parquet partition so saved - # asset paths are portable. Place under a - # sibling ``object/`` folder relative to the parquet's ``robot_name=`` - # leaf so save_motion_parquet's rmtree of the leaf does not delete - # them. - mesh_dst_dir = save_dir / f"sequence_id={sequence_id}" / "object" - mesh_dst_dir.mkdir(parents=True, exist_ok=True) - for src in Path(object_mesh_path).parent.iterdir(): - if not src.is_file(): - continue - shutil.copy2(src, mesh_dst_dir / src.name) - urdf_dst = mesh_dst_dir / "textured_mesh.urdf" - _build_object_urdf(mesh_path="textured_mesh.obj", urdf_path=urdf_dst) - - copied_mesh_abs = (mesh_dst_dir / "textured_mesh.obj").resolve() - try: - stored_mesh_path = str(copied_mesh_abs.relative_to(REPO_ROOT)) - stored_urdf_path = str(urdf_dst.resolve().relative_to(REPO_ROOT)) - except ValueError: - stored_mesh_path = str(copied_mesh_abs) - stored_urdf_path = str(urdf_dst.resolve()) - - expected_joint_dim = kin.robot.model.nq - base_q_size - if len(robot_joint_position_names) != expected_joint_dim: - raise ValueError( - "robot_joint_names must align with robot_joint_positions. " - f"Expected {expected_joint_dim}, got {len(robot_joint_position_names)}." - ) - - builder = { - "robot_root_position": [], - "robot_root_wxyz": [], - "robot_joint_positions": [], - "ee_pose_w": [], - "object_articulation": [], - "object_root_axis_angle": [], - "object_root_position": [], - "object_body_position": [], - "object_body_wxyz": [], - "hand_contact_active_per_frame": [], - "ik_error_per_frame": [], - "ik_num_iterations": [], - "frame_task_errors": [], - # Source raw -- head + root values from the SOMA exporter, - # consumed by the post-process plane-alignment helper. - "soma_joints": [], - "soma_joints_wxyz": [], - "source_head_translation": [], - "source_head_wxyz": [], - "source_root_translation": [], - "source_root_wxyz": [], - } - - playback: ViserPlayback | None = None - if args.visualize: - server = viser.ViserServer(host="0.0.0.0", port=8080) - playback = ViserPlayback.for_live_retarget( - server=server, - pin_model=kin.robot.model, - pin_visual_model=kin.robot.visual_model, - pin_collision_model=kin.robot.collision_model, - object_mesh_path=object_mesh_path, - hand_sides=tuple(hand_sides) or ("left", "right"), - ) - - q = kin.robot.q0.copy() - ground_z_offset = 0.0 - object_z_lift = 0.0 - - foot_frame_idxs = [ - list(kin.robot_frame_names.values()).index(fn) for fn in foot_frame_names - ] - - ankle_xyz_per_frame: list[list[list[float]]] = [] - - # IK diagnostics scaffolding -- populated only when --diagnose-ik is - # set. Kept out-of-band so the hot loop is unchanged for normal runs. - diagnose_ik = bool(args.diagnose_ik) - ik_task_names: list[str] = list(kin.frame_tasks.keys()) - q_lower = np.asarray(kin.robot.model.lowerPositionLimit, dtype=np.float64) - q_upper = np.asarray(kin.robot.model.upperPositionLimit, dtype=np.float64) - # Free-flyer joints have +/-inf bounds, so the saturation check needs - # to skip rows where the URDF didn't author a finite limit. Index 0..6 - # is the free-flyer position+quat anyway. - finite_limit_mask = np.isfinite(q_lower) & np.isfinite(q_upper) - # `q_ik`/`q_lower`/`q_upper` all have length `nq`; joint names live at - # the joint level (one per Pinocchio joint), so the `q_idx -> joint - # name` map is built once here. - q_idx_to_joint_name: dict[int, str] = {} - for joint_idx in range(1, kin.robot.model.njoints): - name = str(kin.robot.model.names[joint_idx]) - q_start = int(kin.robot.model.idx_qs[joint_idx]) - q_size = int(kin.robot.model.nqs[joint_idx]) - for k in range(q_size): - label = name if q_size == 1 else f"{name}[{k}]" - q_idx_to_joint_name[q_start + k] = label - diag_iter_threshold = int(kin.max_iter * float(args.diagnose_ik_iter_fraction)) - diag_error_threshold = float(args.diagnose_ik_error_threshold) - # Per-frame diagnostic records, one tuple per "interesting" frame. - # Format: (frame_idx, total_pos_error, dominant_task, dominant_err, - # n_iter, n_saturated, sample_saturated_joint_name). - diag_offenders: list[tuple[int, float, str, float, int, int, str]] = [] - - # ``frame_idx`` here is local to the iterated window (0..n_iter_frames), - # NOT the absolute index into the original SOMA sequence. The - # ground-anchor initializer below fires on the FIRST iterated frame - # because that is the only time the loop has IK-solved foot placements - # to compute the offset from; if we used an absolute "== 0" check we - # would skip initialization entirely whenever ``--start-frame > 0``. - for frame_idx in tqdm(range(n_iter_frames), desc="Retargeting"): - positions = joint_pos[frame_idx] - rotations = joint_rot_wxyz[frame_idx] - - result = kin.compute( - source_joints=positions, - source_joints_wxyz=rotations, - source_to_robot_scale=args.scale, - qpos=q, - ) - q_ik = result["q"].copy() - q = q_ik.copy() - - if diagnose_ik: - # Identify the dominant frame-task residual and any joints - # whose IK solution sits within 1 mrad / 1e-3 of a position - # limit (counting as "saturated"). The saturation check uses - # `q_ik` -- the pre-clamp solution -- so we see what the - # solver actually wanted, not the post-clamp value used as - # the next warm-start. - task_errs = np.asarray(result["frame_task_errors"], dtype=np.float64) - total_err = float(task_errs.sum()) - dom_idx = int(np.argmax(task_errs)) if task_errs.size > 0 else -1 - dom_task = ik_task_names[dom_idx] if dom_idx >= 0 else "?" - dom_err = float(task_errs[dom_idx]) if dom_idx >= 0 else 0.0 - n_iter = int(result["num_optimization_iterations"]) - sat_tol = 1e-3 - sat_lower = (q_ik <= q_lower + sat_tol) & finite_limit_mask - sat_upper = (q_ik >= q_upper - sat_tol) & finite_limit_mask - sat_mask = sat_lower | sat_upper - n_sat = int(sat_mask.sum()) - sample_sat = "" - if n_sat > 0: - first_sat_idx = int(np.argmax(sat_mask)) - side = "lo" if sat_lower[first_sat_idx] else "hi" - sample_sat = ( - f"{q_idx_to_joint_name.get(first_sat_idx, f'q[{first_sat_idx}]')}" - f"({side})" - ) - iter_saturated = n_iter >= diag_iter_threshold - # ``frame_idx`` is local to the iterated window; show the - # absolute source index too so it lines up with viser/replay. - abs_idx = start_frame + frame_idx - if total_err >= diag_error_threshold or iter_saturated or n_sat > 0: - tqdm.write( - f"[ik-diag] frame={abs_idx:5d} (loop_idx={frame_idx:5d}) " - f"total_err={total_err:.4f} " - f"dom={dom_task}={dom_err:.4f} " - f"iters={n_iter}/{kin.max_iter}" - f"{' SAT_ITER' if iter_saturated else ''} " - f"sat_joints={n_sat}" - f"{f' (e.g. {sample_sat})' if sample_sat else ''}" - ) - diag_offenders.append( - ( - abs_idx, - total_err, - dom_task, - dom_err, - n_iter, - n_sat, - sample_sat, - ) - ) - - lowest_sole_z = ( - min(result["frame_pose"][i, 2] for i in foot_frame_idxs) - ankle_roll_offset - ) - - if frame_idx == 0: - ground_z_offset = -lowest_sole_z - else: - adjusted_lowest = lowest_sole_z + ground_z_offset - if adjusted_lowest < 0.0: - q[2] -= adjusted_lowest - - ankle_xyz_per_frame.append( - [ - [ - float(result["frame_pose"][foot_frame_idxs[0], 0]), - float(result["frame_pose"][foot_frame_idxs[0], 1]), - float(result["frame_pose"][foot_frame_idxs[0], 2]) - + ground_z_offset, - ], - [ - float(result["frame_pose"][foot_frame_idxs[1], 0]), - float(result["frame_pose"][foot_frame_idxs[1], 1]), - float(result["frame_pose"][foot_frame_idxs[1], 2]) - + ground_z_offset, - ], - ] - ) - - obj_pose = object_poses[frame_idx] - obj_position = kin.transform_source_position(obj_pose[:3, 3]) - obj_rotation_mat = kin.transform_world_rotation(obj_pose[:3, :3]) - obj_rotation = R.from_matrix(obj_rotation_mat) - - obj_position[2] += ground_z_offset - object_z_lift = 0.0 - - head_position = kin.transform_source_position(positions[head_idx]) - head_position[2] += ground_z_offset - head_rotation_mat = R.from_quat( - rotations[head_idx], scalar_first=True - ).as_matrix() - head_rotation_mat = kin.transform_source_rotation(head_rotation_mat) - head_rotation_wxyz = R.from_matrix(head_rotation_mat).as_quat(scalar_first=True) - - root_position = kin.transform_source_position(positions[root_idx]) - root_position[2] += ground_z_offset - root_rotation_mat = R.from_quat( - rotations[root_idx], scalar_first=True - ).as_matrix() - root_rotation_mat = kin.transform_source_rotation(root_rotation_mat) - root_rotation_wxyz = R.from_matrix(root_rotation_mat).as_quat(scalar_first=True) - - frame_pose = result["frame_pose"] - ee_pose_t: list[list[float]] = [] - for i in ee_frame_indices: - pose = list(frame_pose[i]) - pose[2] = float(pose[2]) + ground_z_offset - ee_pose_t.append(pose) - - object_translation_w_np = (obj_position + [0, 0, object_z_lift]).astype( - np.float32 - ) - object_rotation_w_np = obj_rotation_mat.astype(np.float32) - verts_w = ( - object_mesh_vertices_f32 @ object_rotation_w_np.T + object_translation_w_np - ) - threshold_sq = contact_threshold * contact_threshold - per_side_active: list[float] = [] - for side_idx, side in enumerate(("left", "right")[: len(ee_pose_t)]): - fingertip_joint_ids = hand_side_to_fingertip_source_joints.get(side, []) - points = np.empty((1 + len(fingertip_joint_ids), 3), dtype=np.float32) - points[0] = ee_pose_t[side_idx][:3] - for k, j in enumerate(fingertip_joint_ids, start=1): - p = kin.transform_source_position(positions[j]) - points[k, 0] = p[0] - points[k, 1] = p[1] - points[k, 2] = p[2] + ground_z_offset - diff = verts_w[:, None, :] - points[None, :, :] - sq_dists = np.einsum("vki,vki->vk", diff, diff) - min_sq = float(sq_dists.min()) - per_side_active.append(1.0 if min_sq < threshold_sq else 0.0) - - if builder is not None: - root_pos_robot = q_ik[:3].copy() - root_pos_robot[2] += ground_z_offset - root_pos_robot = root_pos_robot.tolist() - root_quat_xyzw = q_ik[3:7] - root_wxyz = [ - float(root_quat_xyzw[3]), - float(root_quat_xyzw[0]), - float(root_quat_xyzw[1]), - float(root_quat_xyzw[2]), - ] - joint_positions = q_ik[base_q_size:].tolist() - - obj_wxyz = obj_rotation.as_quat(scalar_first=True).tolist() - obj_body_pos = [(obj_position + [0, 0, object_z_lift]).tolist()] - obj_body_wxyz = [obj_wxyz] - - builder["robot_root_position"].append(root_pos_robot) - builder["robot_root_wxyz"].append(root_wxyz) - builder["robot_joint_positions"].append(joint_positions) - builder["ee_pose_w"].append(ee_pose_t) - builder["object_articulation"].append(0.0) - object_translation_w = object_translation_w_np.tolist() - builder["object_root_position"].append(object_translation_w) - builder["object_root_axis_angle"].append(obj_rotation.as_rotvec().tolist()) - builder["object_body_position"].append(obj_body_pos) - builder["object_body_wxyz"].append(obj_body_wxyz) - builder["hand_contact_active_per_frame"].append(per_side_active) - builder["ik_error_per_frame"].append( - float(np.sum(result["frame_task_errors"])) - ) - builder["ik_num_iterations"].append( - int(result["num_optimization_iterations"]) - ) - builder["frame_task_errors"].append(list(result["frame_task_errors"])) - builder["soma_joints"].append(positions.tolist()) - builder["soma_joints_wxyz"].append(rotations.tolist()) - builder["source_head_translation"].append(head_position.tolist()) - builder["source_head_wxyz"].append(head_rotation_wxyz.tolist()) - builder["source_root_translation"].append(root_position.tolist()) - builder["source_root_wxyz"].append(root_rotation_wxyz.tolist()) - - if playback is not None: - vertices_vis = kin.transform_source_position(vertices[frame_idx]).copy() - vertices_vis[:, 2] += ground_z_offset - q_vis = q_ik.copy() - q_vis[2] += ground_z_offset - - ik_target_poses: dict[str, tuple[np.ndarray, np.ndarray]] = {} - for frame_name, task in kin.frame_tasks.items(): - target_pos_vis = task.transform_target_to_world.translation.copy() - target_pos_vis[2] += ground_z_offset - target_wxyz = R.from_matrix( - task.transform_target_to_world.rotation - ).as_quat(scalar_first=True) - ik_target_poses[frame_name] = (target_pos_vis, target_wxyz) - - contact_wrists = [np.asarray(pose[:3]) for pose in ee_pose_t] - - playback.display( - LiveFrameState( - q=q_vis, - object_pos=obj_position, - object_wxyz=obj_rotation.as_quat(scalar_first=True), - head_pos=head_position, - head_wxyz=head_rotation_wxyz, - root_pos=root_position, - root_wxyz=root_rotation_wxyz, - contact_wrists=contact_wrists, - contact_active=list(per_side_active), - body_vertices=vertices_vis, - ik_target_poses=ik_target_poses, - ), - body_model=soma, - ) - - if frame_idx == 0 and args.visualize: - time.sleep(5) - - if builder is not None: - first_pass = FirstPassResult( - fps=float(kin.frequency), - robot_root_position=np.asarray( - builder["robot_root_position"], dtype=np.float64 - ), - robot_root_wxyz=np.asarray(builder["robot_root_wxyz"], dtype=np.float64), - robot_joint_positions=np.asarray( - builder["robot_joint_positions"], dtype=np.float64 - ), - ee_pose_w=np.asarray(builder["ee_pose_w"], dtype=np.float64), - object_root_position=np.asarray( - builder["object_root_position"], dtype=np.float64 - ), - object_root_axis_angle=np.asarray( - builder["object_root_axis_angle"], dtype=np.float64 - ), - object_body_position=np.asarray( - builder["object_body_position"], dtype=np.float64 - ), - object_body_wxyz=np.asarray(builder["object_body_wxyz"], dtype=np.float64), - hand_contact_active_per_frame=np.asarray( - builder["hand_contact_active_per_frame"], dtype=np.float64 - ), - source_head_translation=np.asarray( - builder["source_head_translation"], dtype=np.float64 - ), - source_root_translation=np.asarray( - builder["source_root_translation"], dtype=np.float64 - ), - ankle_frame_xyz=np.asarray(ankle_xyz_per_frame, dtype=np.float64), - ) - - first_pass_cache_extras = { - "object_articulation": list(builder["object_articulation"]), - "soma_joints": [list(f) for f in builder["soma_joints"]], - "soma_joints_wxyz": [list(f) for f in builder["soma_joints_wxyz"]], - "source_head_wxyz": [list(f) for f in builder["source_head_wxyz"]], - "source_root_wxyz": [list(f) for f in builder["source_root_wxyz"]], - "ik_error_per_frame": list(builder["ik_error_per_frame"]), - "ik_num_iterations": list(builder["ik_num_iterations"]), - "frame_task_errors": [list(f) for f in builder["frame_task_errors"]], - } - - plane = ( - reconstructed_plane - if reconstructed_plane is not None - else ReferencePlane.horizontal(z=0.0) - ) - sole_xyz = first_pass.ankle_frame_xyz.copy() - sole_xyz[..., 2] -= ankle_roll_offset - # Per-frame ground anchoring during the IK loop already drove the - # body's lowest sole to z = 0 of the robot world (see - # ``ground_z_offset`` initialization on frame 0). The reconstructed - # ``ground_plane.json`` carries the absolute scene height instead, so - # using its raw offset would drag the body and the carried object - # down to that absolute level even though the body is already on the - # floor. We therefore preserve the reconstructed plane's normal (it - # captures any scene tilt) but shift the offset so the plane sits - # exactly under the frame-0 lowest sole. Without this, the post- - # process injects the reconstruction's ground-Z bias (e.g. - # ``-1.04 m`` for the trash-can sequence) into ``robot_delta_z`` and - # the saved object ends up below the simulator's ground plane. - if reconstructed_plane is not None: - n = np.asarray(plane.normal, dtype=np.float64) - frame0_anchor = sole_xyz[0].min(axis=0) - adjusted_offset = -float(np.dot(n, frame0_anchor)) - plane = ReferencePlane( - normal=tuple(plane.normal), - offset=adjusted_offset, - ) - robot_delta_z = compute_plane_alignment_offsets( - sole_xyz, plane, PlaneAlignmentConfig() - ) - interaction_mask = compute_interaction_mask(first_pass, InteractionMaskConfig()) - corr_obj_root_pos, corr_obj_root_aa, corr_obj_body_pos, corr_obj_body_wxyz = ( - correct_object_trajectory( - first_pass, - interaction_mask, - robot_delta_z, - ObjectCorrectionConfig(), - ) - ) - - corr_root_pos = first_pass.robot_root_position.copy() - corr_root_pos[:, 2] += robot_delta_z - builder["robot_root_position"] = corr_root_pos.tolist() - - corr_ee_pose = first_pass.ee_pose_w.copy() - corr_ee_pose[..., 2] += robot_delta_z[:, None] - builder["ee_pose_w"] = corr_ee_pose.tolist() - - corr_nv_head = first_pass.source_head_translation.copy() - corr_nv_head[:, 2] += robot_delta_z - builder["source_head_translation"] = corr_nv_head.tolist() - - corr_nv_root = first_pass.source_root_translation.copy() - corr_nv_root[:, 2] += robot_delta_z - builder["source_root_translation"] = corr_nv_root.tolist() - - builder["object_root_position"] = corr_obj_root_pos.tolist() - builder["object_root_axis_angle"] = corr_obj_root_aa.tolist() - builder["object_body_position"] = corr_obj_body_pos.tolist() - builder["object_body_wxyz"] = corr_obj_body_wxyz.tolist() - - n_interact = int(np.count_nonzero(interaction_mask)) - pre_ankle_z = first_pass.ankle_frame_xyz[:, :, 2] - post_ankle_z = pre_ankle_z + robot_delta_z[:, None] - pre_sole_lowest = float(pre_ankle_z.min() - ankle_roll_offset) - pre_sole_highest = float(pre_ankle_z.max() - ankle_roll_offset) - post_sole_lowest = float(post_ankle_z.min() - ankle_roll_offset) - post_sole_highest = float(post_ankle_z.max() - ankle_roll_offset) - print( - "[INFO] Plane alignment: robot_delta_z range " - f"[{float(robot_delta_z.min()):+.4f}, " - f"{float(robot_delta_z.max()):+.4f}] m; " - f"{n_interact}/{len(interaction_mask)} interaction frames." - ) - print( - f"[INFO] Robot sole Z pre-offset : " - f"[{pre_sole_lowest:+.4f}, {pre_sole_highest:+.4f}] m" - ) - print( - f"[INFO] Robot sole Z post-offset: " - f"[{post_sole_lowest:+.4f}, {post_sole_highest:+.4f}] m " - f"(should be near 0)" - ) - - source_payload = pickle.dumps( - { - "soma_identity_coeffs": soma_identity_coeffs, - "soma_scale_params": soma_scale_params, - "soma_joints": builder.pop("soma_joints"), - "soma_joints_wxyz": builder.pop("soma_joints_wxyz"), - "source_head_translation": builder.pop("source_head_translation"), - "source_head_wxyz": builder.pop("source_head_wxyz"), - "source_root_translation": builder.pop("source_root_translation"), - "source_root_wxyz": builder.pop("source_root_wxyz"), - } - ) - - hand_contact_active: list[list[float]] = [] - if hand_sides: - per_frame = builder.pop("hand_contact_active_per_frame") - hand_contact_active = [ - [float(per_frame[t][s]) for t in range(len(per_frame))] - for s in range(len(hand_sides)) - ] - for side, series in zip(hand_sides, hand_contact_active, strict=True): - n_active = int(sum(series)) - print( - f"[INFO] {side}_hand_contact_active: {n_active}/{len(series)} " - f"frames (threshold={contact_threshold} m)" - ) - else: - builder.pop("hand_contact_active_per_frame", None) - - md = MotionData( - sequence_id=sequence_id, - robot_name=config.robot_name, - motion_kind="single_robot", - source_dataset="soma", - raw_motion_file=soma_params_path, - fps=float(kin.frequency), - coord_frame="robot_base_z_up", - robot_joint_names=robot_joint_position_names, - robot_root_position=builder["robot_root_position"], - robot_root_wxyz=builder["robot_root_wxyz"], - robot_joint_positions=builder["robot_joint_positions"], - ee_link_names=ee_link_names, - ee_pose_w=builder["ee_pose_w"], - hand_sides=hand_sides, - hand_contact_active=hand_contact_active, - object_name=object_name, - safe_object_name=_usd_safe(object_name), - object_body_names=object_body_names, - safe_object_body_names=safe_object_body_names, - object_mesh_paths=[stored_mesh_path], - object_urdf_paths=[stored_urdf_path], - object_mesh_radius=[object_mesh_radius], - object_articulation=builder["object_articulation"], - object_root_axis_angle=builder["object_root_axis_angle"], - object_root_position=builder["object_root_position"], - object_body_position=builder["object_body_position"], - object_body_wxyz=builder["object_body_wxyz"], - ik_error_per_frame=builder["ik_error_per_frame"], - ik_num_iterations=builder["ik_num_iterations"], - frame_task_errors=builder["frame_task_errors"], - source_kind="soma", - source_payload=source_payload, - ) - save_motion_parquet(md, root_path=str(save_dir), file_name="data.parquet") - print(f"Saved to {save_dir}") - - first_pass_cache_path = ( - FIRST_PASS_CACHE_DIR - / f"sequence_id={sequence_id}" - / f"robot_name={config.robot_name}" - / "first_pass_cache.pkl" - ) - first_pass_cache_path.parent.mkdir(parents=True, exist_ok=True) - first_pass_cache = { - "first_pass": first_pass, - "builder_extras": first_pass_cache_extras, - "metadata": { - "sequence_id": sequence_id, - "motion_params_path": soma_params_path, - "fps": float(kin.frequency), - "robot_joint_names": list(robot_joint_position_names), - "ee_link_names": list(ee_link_names), - "object_name": object_name, - "safe_object_name": _usd_safe(object_name), - "object_body_names": list(object_body_names), - "safe_object_body_names": list(safe_object_body_names), - "object_mesh_paths": [stored_mesh_path], - "object_urdf_paths": [stored_urdf_path], - "object_mesh_radius": [object_mesh_radius], - "soma_identity_coeffs": list(soma_identity_coeffs), - "soma_scale_params": list(soma_scale_params), - "contact_threshold": float(contact_threshold), - "g1_ankle_roll_offset": float(ankle_roll_offset), - "ground_plane_source": ( - "reconstructed" if reconstructed_plane is not None else "fallback" - ), - "ground_plane_normal": ( - list(reconstructed_plane.normal) - if reconstructed_plane is not None - else [0.0, 0.0, 1.0] - ), - "ground_plane_offset": ( - float(reconstructed_plane.offset) - if reconstructed_plane is not None - else 0.0 - ), - "ground_plane_json_path": str(ground_plane_path), - }, - } - with open(first_pass_cache_path, "wb") as _f: - pickle.dump(first_pass_cache, _f) - print(f"[INFO] Cached first-pass snapshot -> {first_pass_cache_path}") - - if diagnose_ik: - if not diag_offenders: - print( - f"[ik-diag] No frames flagged " - f"(total_err < {diag_error_threshold} m, iters < " - f"{diag_iter_threshold}/{kin.max_iter}, no saturated joints)." - ) - else: - # Top-K worst by total task residual, then by iteration count - # as a tie-breaker. K=10 is enough to spot a band of bad - # frames around contact while staying readable. - top_k = min(10, len(diag_offenders)) - worst = sorted( - diag_offenders, - key=lambda r: (-r[1], -r[4]), - )[:top_k] - print( - f"[ik-diag] {len(diag_offenders)}/{n_iter_frames} frames " - f"flagged. Worst {top_k} by total task error:" - ) - for ( - abs_idx, - total_err, - dom_task, - dom_err, - n_iter, - n_sat, - sample_sat, - ) in worst: - sat_str = ( - f" sat={n_sat} (e.g. {sample_sat})" - if sample_sat - else f" sat={n_sat}" - ) - print( - f" frame={abs_idx:5d} total_err={total_err:.4f} " - f"dom={dom_task}={dom_err:.4f} " - f"iters={n_iter}/{kin.max_iter}{sat_str}" - ) - # Per-task error contribution averaged across flagged frames - # -- useful to decide which weight to bump. - per_task_sum: dict[str, float] = dict.fromkeys(ik_task_names, 0.0) - per_task_count: dict[str, int] = dict.fromkeys(ik_task_names, 0) - for ( - _abs_idx, - _total_err, - dom_task, - dom_err, - _n_iter, - _n_sat, - _, - ) in diag_offenders: - per_task_sum[dom_task] += dom_err - per_task_count[dom_task] += 1 - print("[ik-diag] dominant-task tally across flagged frames:") - for name in ik_task_names: - if per_task_count[name] == 0: - continue - avg = per_task_sum[name] / per_task_count[name] - print( - f" {name:30s} dominant in {per_task_count[name]:4d} frames " - f"(avg dominant err = {avg:.4f})" - ) - - if (start_frame, end_frame) != (0, num_frames): - print( - f"Retargeting complete. Processed {n_iter_frames} frames " - f"(window [{start_frame}, {end_frame}) of {num_frames})." - ) - else: - print(f"Retargeting complete. Processed {num_frames} frames.") - if args.visualize: - print("Visualization server running. Press Ctrl+C to exit.") - # Catch the interrupt and return cleanly (exit 0) rather than letting - # it propagate as a KeyboardInterrupt traceback. The whole foreground - # process group receives the Ctrl+C, including the wrapper - # process_soma_sequence.sh; bash only continues to the next stage if - # this child exits normally, so a clean exit here is what lets the - # pipeline advance instead of aborting. - try: - while True: - time.sleep(1) - except KeyboardInterrupt: - print("\nVisualization stopped; continuing.") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/retarget/taco_to_dex3.py b/robotic_grounding/scripts/retarget/taco_to_dex3.py deleted file mode 100644 index 2eebb0df..00000000 --- a/robotic_grounding/scripts/retarget/taco_to_dex3.py +++ /dev/null @@ -1,299 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget TACO loaded data (ManoSharpaData with MANO+object only) to Dex3. - -Reads the taco_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to taco_processed (robot_name=dex3). - -Usage: - 1. (upstream) reconstruction load workflow writes taco_loaded Parquet - 2. python scripts/retarget/taco_to_dex3.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR, MESHES_DIR -from robotic_grounding.retarget.data_logger import ( - ManoDex3Data, - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_dex3_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -# Suppress warnings about joint limits being slightly out of bounds -logging.getLogger().setLevel(logging.ERROR) - -# Default paths: loader output -> retarget output -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "taco_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "taco_processed" - -# TACO object meshes: {name}_cm.obj in object_model_root (scale 0.01 cm -> m) -TACO_OBJECT_MODEL_DIR = MESHES_DIR / "taco" - -# Dex3 MANO retargeting does not need a link-to-site wrist offset -LINK_TO_SITE_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_name: str, - object_body_names: list[str], - object_model_root: Path, -) -> dict[str, Any]: - """Load TACO tool/target meshes and add them to the viser scene. - - object_name is expected to be "{tool_id}_{target_id}" (e.g. "035_024"). - Meshes are {id}_cm.obj; scale 0.01 (cm to m). - - Returns: - Dict mapping body name to viser mesh handle (for updating position/wxyz per frame). - """ - handles: dict[str, Any] = {} - parts = object_name.split("_", 1) - tool_id = parts[0] if parts else "" - target_id = parts[1] if len(parts) > 1 else parts[0] if parts else "" - name_per_body = dict(zip(object_body_names, [tool_id, target_id], strict=True)) - for part in object_body_names: - mesh_id = name_per_body.get(part, "") - if not mesh_id: - continue - mesh_path = object_model_root / f"{mesh_id}_cm.obj" - if not mesh_path.exists(): - continue - mesh = trimesh.load(str(mesh_path)) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - # TACO meshes are in cm; scale to meters - mesh.vertices *= 0.01 - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Retarget TACO loaded Parquet data to Dex3 (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument( - "--object_model_root", - type=Path, - default=TACO_OBJECT_MODEL_DIR, - help="Directory with {id}_cm.obj meshes for visualization.", - ) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded TACO Parquet, run Dex3 IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_dex3_kinematics = setup_dex3_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_dex3_kinematics = setup_dex3_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - viser_object_handles = _load_object_viser_handles( - viser_server, - data.object_name, - data.object_body_names, - args.object_model_root, - ) - - # Run IK for each frame and collect robot_* time series - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=LINK_TO_SITE_XYZW, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=LINK_TO_SITE_XYZW, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_dex3_kinematics, - left_dex3_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_dex3_kinematics.visualize(viser_server, right_qpos) - left_dex3_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - # Free-flyer qpos layout: [pos(3), quat_xyzw(4), fingers(7)] - # Convert xyzw -> wxyz for ManoDex3Data schema - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - d = data.to_dict() - d["robot_name"] = "dex3" - d["right_robot_finger_joint_names"] = list( - right_dex3_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_dex3_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_dex3_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_dex3_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_dex3_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_dex3_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoDex3Data(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/taco_to_sharpa.py b/robotic_grounding/scripts/retarget/taco_to_sharpa.py deleted file mode 100755 index b1eb592b..00000000 --- a/robotic_grounding/scripts/retarget/taco_to_sharpa.py +++ /dev/null @@ -1,298 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Retarget TACO loaded data (ManoSharpaData with MANO+object only) to Sharpa. - -Reads the taco_loaded Parquet (ManoSharpaData; produced upstream by -reconstruction's v2d_task_library_loader load workflow), -runs IK per frame to fill robot_* fields, and saves to taco_loaded/mano_object_robot_processed. - -Usage: - 1. (upstream) reconstruction load workflow writes taco_loaded Parquet - 2. python scripts/retarget/taco_to_sharpa.py --save -""" - -import argparse -import logging -from pathlib import Path -from typing import Any - -import numpy as np -import torch -import trimesh -import viser -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR, MESHES_DIR -from robotic_grounding.retarget.data_logger import ( - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.retarget_utils import ( - DEFAULT_PARTITION_COLS, - run_frame_ik, - setup_sharpa_kinematics, - wrist_pose_from_mano_joint0, -) -from tqdm import tqdm - -# Suppress warnings about joint limits being slightly out of bounds -logging.getLogger().setLevel(logging.ERROR) - -# Default paths: loader output (mano_object_only subdir) -> retarget output -DEFAULT_INPUT_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "taco_loaded" -DEFAULT_OUTPUT_DIR = HUMAN_MOTION_DATA_DIR / "taco" / "taco_processed" - -# TACO object meshes: {name}_cm.obj in object_model_root (scale 0.01 cm -> m) -TACO_OBJECT_MODEL_DIR = MESHES_DIR / "taco" - -# TACO uses MANO with no special wrist link-to-site offset (unlike ARCTIC) -TACO_LINK_TO_SITE_QUAT_XYZW = None - - -def _load_object_viser_handles( - viser_server: viser.ViserServer, - object_name: str, - object_body_names: list[str], - object_model_root: Path, -) -> dict[str, Any]: - """Load TACO tool/target meshes and add them to the viser scene. - - object_name is expected to be "{tool_id}_{target_id}" (e.g. "035_024"). - Meshes are {id}_cm.obj; scale 0.01 (cm to m). - - Returns: - Dict mapping body name to viser mesh handle (for updating position/wxyz per frame). - """ - handles: dict[str, Any] = {} - parts = object_name.split("_", 1) - tool_id = parts[0] if parts else "" - target_id = parts[1] if len(parts) > 1 else parts[0] if parts else "" - name_per_body = dict(zip(object_body_names, [tool_id, target_id], strict=True)) - for part in object_body_names: - mesh_id = name_per_body.get(part, "") - if not mesh_id: - continue - mesh_path = object_model_root / f"{mesh_id}_cm.obj" - if not mesh_path.exists(): - continue - mesh = trimesh.load(str(mesh_path)) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - # TACO meshes are in cm; scale to meters - mesh.vertices *= 0.01 - handles[part] = viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}", - mesh=mesh, - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - ) - return handles - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Retarget TACO loaded Parquet data to Sharpa (run IK, fill robot_*)." - ) - parser.add_argument("--input_dir", type=Path, default=DEFAULT_INPUT_DIR) - parser.add_argument("--output_dir", type=Path, default=DEFAULT_OUTPUT_DIR) - parser.add_argument( - "--object_model_root", - type=Path, - default=TACO_OBJECT_MODEL_DIR, - help="Directory with {id}_cm.obj meshes for visualization.", - ) - parser.add_argument("--device", type=str, default="cuda:0") - parser.add_argument("--visualize", action="store_true", default=False) - parser.add_argument("--save", action="store_true", default=False) - parser.add_argument("--mano_to_robot_scale", type=float, default=1.2) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main(args: argparse.Namespace) -> None: - """Read loaded TACO Parquet, run IK per frame, save retargeted Parquet.""" - device = torch.device(args.device) - - if args.visualize: - viser_server = viser.ViserServer() - - if args.save: - args.output_dir.mkdir(parents=True, exist_ok=True) - - right_sharpa_kinematics = setup_sharpa_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_sharpa_kinematics = setup_sharpa_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - print(f"Found {len(sequence_ids)} sequences in {args.input_dir}") - - link_to_site_xyzw = TACO_LINK_TO_SITE_QUAT_XYZW - - viser_object_handles: dict[str, Any] = {} - for sequence_id in tqdm(sequence_ids): - if args.visualize: - for handle in viser_object_handles.values(): - handle.remove() - viser_object_handles.clear() - - data = ManoSharpaData.from_parquet( - str(args.input_dir), - filters=[("sequence_id", "=", sequence_id)], - ) - num_frames = len(data.mano_right_trans) - - if args.visualize: - viser_object_handles = _load_object_viser_handles( - viser_server, - data.object_name, - data.object_body_names, - args.object_model_root, - ) - - # Run IK for each frame and collect robot_* time series - robot_right_wrist_position = [] - robot_right_wrist_wxyz = [] - robot_right_finger_joints = [] - robot_right_frames = [] - robot_right_frame_task_errors = [] - robot_right_num_optimization_iterations = [] - robot_left_wrist_position = [] - robot_left_wrist_wxyz = [] - robot_left_finger_joints = [] - robot_left_frames = [] - robot_left_frame_task_errors = [] - robot_left_num_optimization_iterations = [] - - right_qpos = None - left_qpos = None - - for t in range(num_frames): - right_joints = torch.tensor( - data.mano_right_joints[t], dtype=torch.float32, device=device - ) - right_joints_wxyz = torch.tensor( - data.mano_right_joints_wxyz[t], dtype=torch.float32, device=device - ) - left_joints = torch.tensor( - data.mano_left_joints[t], dtype=torch.float32, device=device - ) - left_joints_wxyz = torch.tensor( - data.mano_left_joints_wxyz[t], dtype=torch.float32, device=device - ) - - if right_qpos is None: - right_pos, right_quat_xyzw = wrist_pose_from_mano_joint0( - right_joints[0].cpu().numpy(), - right_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - right_pos = right_quat_xyzw = None - if left_qpos is None: - left_pos, left_quat_xyzw = wrist_pose_from_mano_joint0( - left_joints[0].cpu().numpy(), - left_joints_wxyz[0].cpu().numpy(), - link_to_site_quat_xyzw=link_to_site_xyzw, - ) - else: - left_pos = left_quat_xyzw = None - - right_qpos, left_qpos, right_results, left_results = run_frame_ik( - right_sharpa_kinematics, - left_sharpa_kinematics, - right_joints, - right_joints_wxyz, - left_joints, - left_joints_wxyz, - args.mano_to_robot_scale, - right_qpos_prev=right_qpos, - left_qpos_prev=left_qpos, - right_wrist_position=right_pos, - right_wrist_quat_xyzw=right_quat_xyzw, - left_wrist_position=left_pos, - left_wrist_quat_xyzw=left_quat_xyzw, - ) - - if args.visualize: - right_sharpa_kinematics.visualize(viser_server, right_qpos) - left_sharpa_kinematics.visualize(viser_server, left_qpos) - for obj_idx, obj_name in enumerate(data.object_body_names): - if obj_name in viser_object_handles: - viser_object_handles[obj_name].position = np.asarray( - data.object_body_position[t][obj_idx] - ) - viser_object_handles[obj_name].wxyz = np.asarray( - data.object_body_wxyz[t][obj_idx] - ) - - robot_right_wrist_position.append(right_results["q"][:3].tolist()) - robot_right_wrist_wxyz.append( - right_results["q"][3:7][[3, 0, 1, 2]].tolist() - ) - robot_right_finger_joints.append(right_results["q"][7:].tolist()) - robot_right_frames.append(right_results["frame_pose"].tolist()) - robot_right_frame_task_errors.append(right_results["frame_task_errors"]) - robot_right_num_optimization_iterations.append( - right_results["num_optimization_iterations"] - ) - robot_left_wrist_position.append(left_results["q"][:3].tolist()) - robot_left_wrist_wxyz.append(left_results["q"][3:7][[3, 0, 1, 2]].tolist()) - robot_left_finger_joints.append(left_results["q"][7:].tolist()) - robot_left_frames.append(left_results["frame_pose"].tolist()) - robot_left_frame_task_errors.append(left_results["frame_task_errors"]) - robot_left_num_optimization_iterations.append( - left_results["num_optimization_iterations"] - ) - - if args.save: - # Build new ManoSharpaData: same metadata + same MANO/object as loaded, new robot_* - d = data.to_dict() - d["right_robot_finger_joint_names"] = list( - right_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["right_robot_frame_names"] = list( - right_sharpa_kinematics.robot_frame_names.values() - ) - d["right_robot_frame_task_names"] = list( - right_sharpa_kinematics.frame_tasks.keys() - ) - d["left_robot_finger_joint_names"] = list( - left_sharpa_kinematics.robot_finger_joint_names.values() - ) - d["left_robot_frame_names"] = list( - left_sharpa_kinematics.robot_frame_names.values() - ) - d["left_robot_frame_task_names"] = list( - left_sharpa_kinematics.frame_tasks.keys() - ) - d["robot_right_wrist_position"] = robot_right_wrist_position - d["robot_right_wrist_wxyz"] = robot_right_wrist_wxyz - d["robot_right_finger_joints"] = robot_right_finger_joints - d["robot_right_frames"] = robot_right_frames - d["robot_right_frame_task_errors"] = robot_right_frame_task_errors - d["robot_right_num_optimization_iterations"] = ( - robot_right_num_optimization_iterations - ) - d["robot_left_wrist_position"] = robot_left_wrist_position - d["robot_left_wrist_wxyz"] = robot_left_wrist_wxyz - d["robot_left_finger_joints"] = robot_left_finger_joints - d["robot_left_frames"] = robot_left_frames - d["robot_left_frame_task_errors"] = robot_left_frame_task_errors - d["robot_left_num_optimization_iterations"] = ( - robot_left_num_optimization_iterations - ) - retargeted = ManoSharpaData(**d) - retargeted.save_to_parquet( - root_path=str(args.output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/retarget/verify_tips_distance.py b/robotic_grounding/scripts/retarget/verify_tips_distance.py deleted file mode 100644 index 4f3d8e95..00000000 --- a/robotic_grounding/scripts/retarget/verify_tips_distance.py +++ /dev/null @@ -1,68 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Verify that tips_distance data in processed parquet is present and non-zero.""" - -import numpy as np -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ManoSharpaData - -FINGERS = ["thumb", "index", "middle", "ring", "pinky"] - - -def main() -> None: - """Load processed parquet and verify tips_distance data.""" - processed_dir = str(HUMAN_MOTION_DATA_DIR / "arctic" / "arctic_processed") - data = ManoSharpaData.from_parquet( - processed_dir, - filters=[ - ("robot_name", "=", "sharpa_wave"), - ("sequence_id", "contains", "box_grab"), - ], - ) - - right = np.array(data.mano_right_tips_distance) # (T, 5) - left = np.array(data.mano_left_tips_distance) # (T, 5) - - print(f"Sequence: {data.sequence_id}") - print(f"Frames: {len(right)}") - print(f"Right shape: {right.shape}, Left shape: {left.shape}") - print(f"FPS: {data.fps}") - - # Frame count should match other time-series columns - n_joints = len(data.mano_right_joints) - print(f"Joint frames: {n_joints}") - assert len(right) == n_joints, ( - f"tips_distance frame count ({len(right)}) != joints frame count ({n_joints}). " - "Some frames may have been logged as None." - ) - - # Global stats - print( - f"\nRight - min: {right.min():.6f}, max: {right.max():.6f}, mean: {right.mean():.6f}" - ) - print( - f"Left - min: {left.min():.6f}, max: {left.max():.6f}, mean: {left.mean():.6f}" - ) - print(f"Right all-zero frames: {(right.sum(axis=1) == 0).sum()} / {len(right)}") - print(f"Left all-zero frames: {(left.sum(axis=1) == 0).sum()} / {len(left)}") - - # Per-finger stats - print("\nPer-finger mean distance (meters):") - for i, name in enumerate(FINGERS): - print( - f" Right {name:7s}: {right[:, i].mean():.4f} Left {name:7s}: {left[:, i].mean():.4f}" - ) - - # Assertions - assert right.min() >= 0, "Negative distance found in right" - assert left.min() >= 0, "Negative distance found in left" - assert right.max() < 1.0, f"Implausibly large right distance: {right.max()}" - assert left.max() < 1.0, f"Implausibly large left distance: {left.max()}" - assert (right > 0).any(), "Right tips_distance is all zeros" - assert (left > 0).any(), "Left tips_distance is all zeros" - - print("\nAll checks passed.") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/retarget/vis_retargeted.py b/robotic_grounding/scripts/retarget/vis_retargeted.py deleted file mode 100644 index d401fdac..00000000 --- a/robotic_grounding/scripts/retarget/vis_retargeted.py +++ /dev/null @@ -1,665 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Visualize retargeted hand-object data from any dataset (ARCTIC, TACO, etc.). - -Loads ManoSharpaData Parquet from a given directory; object meshes are loaded from -the object_mesh_paths field (one path per object body). Plays back robot hands + object poses in viser. -""" - -import argparse -import shutil -import sys -import time -from pathlib import Path -from typing import Any - -import numpy as np -import trimesh -import viser -from scipy.spatial.transform import Rotation - -try: - from pxr import Usd, UsdGeom - - _USD_AVAILABLE = True -except ImportError: - _USD_AVAILABLE = False -from robotic_grounding.retarget import HUMAN_MOTION_DATA_DIR -from robotic_grounding.retarget.data_logger import ( - ManoDex3Data, - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from robotic_grounding.retarget.dataset_registry import ( - get_all_dataset_names, - get_dataset_config, -) -from robotic_grounding.retarget.hand_kinematics import HandKinematics -from robotic_grounding.retarget.params import MANO_FINGERTIP_INDICES, MANO_HAND_LINKS -from robotic_grounding.retarget.retarget_utils import ( - setup_dex3_kinematics, - setup_sharpa_kinematics, -) - -# Per-robot dispatch tables for data class + kinematics setup. -ROBOT_DATA_CLASSES: dict[str, type] = { - "sharpa_wave": ManoSharpaData, - "dex3": ManoDex3Data, -} -ROBOT_KINEMATICS_SETUP = { - "sharpa_wave": setup_sharpa_kinematics, - "dex3": setup_dex3_kinematics, -} - -DEFAULT_HTML_DIR = HUMAN_MOTION_DATA_DIR / "html" - -FINGER_NAMES = ["thumb", "index", "middle", "ring", "pinky"] - - -def distance_to_color(d: float) -> tuple[int, int, int]: - """Map distance to a green-to-red color gradient. - - Green (0, 255, 0) at d <= 0.01m (contact), red (255, 0, 0) at d >= 0.05m (far). - """ - t = np.clip((d - 0.01) / (0.05 - 0.01), 0.0, 1.0) - r = int(255 * t) - g = int(255 * (1.0 - t)) - return (r, g, 0) - - -def load_object_meshes_from_paths( - viser_server: viser.ViserServer, - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, Any]: - """Load object meshes from schema paths (one per body) and add them to the viser scene. - - Paths ending with _cm.obj are scaled by 0.01 (cm -> m). Returns dict mapping body name to handle. - """ - handles: dict[str, Any] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - if not path or not Path(path).exists(): - continue - mesh = trimesh.load(path) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.to_geometry() - if path.endswith("_cm.obj"): - mesh.vertices *= 0.01 - # Use a FrameHandle parent so per-frame position updates are sent as - # full add_frame messages — property-setter updates on mesh handles are - # recorded as delta messages that the viser player doesn't replay on - # subsequent loop iterations, causing the object to freeze after loop 1. - frame_handle = viser_server.scene.add_frame( - name=f"/object/{part}", - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - show_axes=False, - ) - viser_server.scene.add_mesh_trimesh( - name=f"/object/{part}/mesh", - mesh=mesh, - ) - handles[part] = frame_handle - return handles - - -def load_support_surfaces_from_usd( - viser_server: viser.ViserServer, - usd_path: Path, -) -> dict[str, Any]: - """Load support-surface disks from a .usda file and add them to the viser scene. - - Each ``UsdGeom.Cylinder`` prim is converted to a trimesh cylinder and rendered - with the ``displayColor`` stored in the USD (falls back to light-blue if absent). - - Args: - viser_server: The running viser server. - usd_path: Path to the ``.usda`` file produced by ``reconstruct_support_surfaces.py``. - - Returns: - Dict mapping prim path to the viser mesh handle. - """ - stage = Usd.Stage.Open(str(usd_path)) - handles: dict[str, Any] = {} - for prim in stage.Traverse(): - if not prim.IsA(UsdGeom.Cylinder): - continue - cyl = UsdGeom.Cylinder(prim) - radius = cyl.GetRadiusAttr().Get() - height = cyl.GetHeightAttr().Get() - xf = UsdGeom.Xformable(prim) - ops = xf.GetOrderedXformOps() - translate = ops[0].Get() if ops else (0.0, 0.0, 0.0) - - display_color = cyl.GetDisplayColorAttr().Get() - if display_color and len(display_color) > 0: - c = display_color[0] - rgba = [int(c[0] * 255), int(c[1] * 255), int(c[2] * 255), 180] - else: - rgba = [150, 200, 255, 160] - - mesh = trimesh.creation.cylinder(radius=radius, height=height, sections=64) - mesh.apply_translation(translate) - mesh.visual.face_colors = rgba - - prim_path = str(prim.GetPath()) - handles[prim_path] = viser_server.scene.add_mesh_trimesh( - name=prim_path, - mesh=mesh, - ) - return handles - - -def _dataset_processed_dir(name: str) -> str: - """Resolve ``{name}/{name}_processed`` using the dataset registry.""" - cfg = get_dataset_config(name) - return f"{cfg.name}/{cfg.name}{cfg.processed_suffix}" - - -DATASET_DIRS: dict[str, str] = { - name: _dataset_processed_dir(name) for name in get_all_dataset_names() -} - - -def parse_args() -> argparse.Namespace: - """Parse the command line arguments.""" - parser = argparse.ArgumentParser( - description="Visualize retargeted Parquet data (hands + optional object meshes)." - ) - parser.add_argument( - "--dataset", - type=str, - choices=list(DATASET_DIRS), - default=None, - help=( - "Dataset shorthand; sets --input_dir to the corresponding processed directory " - f"under HUMAN_MOTION_DATA_DIR. Choices: {list(DATASET_DIRS)}. " - "Ignored when --input_dir is set explicitly." - ), - ) - parser.add_argument( - "--input_dir", - type=Path, - default=None, - help=( - "Root directory of retargeted Parquet (e.g. .../arctic_processed). " - "Defaults to arctic_processed when neither --input_dir nor --dataset is given." - ), - ) - parser.add_argument( - "-tid", - "--trajectory_id", - type=int, - default=0, - help="Row index when multiple rows match filters (default 0).", - ) - parser.add_argument( - "--robot", - type=str, - choices=list(ROBOT_DATA_CLASSES), - default="sharpa_wave", - help=( - "Which robot's retargeted output to visualize. Selects the data " - "class (ManoSharpaData / ManoDex3Data) and kinematics setup, and " - "filters the parquet by robot_name. Default: sharpa_wave." - ), - ) - # Adds --sequence_id, --sequence_pattern, --max_sequences (shared with - # the loader + retarget CLIs so workflow/retarget.yaml can pass the same - # FILTER_ARGS through every stage). - add_sequence_filter_args(parser) - parser.add_argument( - "--visualize_contacts", - action="store_true", - default=False, - ) - parser.add_argument( - "--visualize_fingertip_distances", - action="store_true", - default=False, - ) - parser.add_argument( - "--support_usd", - type=Path, - default=None, - help="Explicit path to a support-surface .usda file (overrides auto-discovery).", - ) - parser.add_argument( - "--save_html", - action="store_true", - default=False, - help=( - "Record the animation to a .viser file and build a local viser client for offline " - "playback. Outputs to --html_dir. After running, scp that directory to your local " - "machine, then: cd && python -m http.server 8000" - ), - ) - parser.add_argument( - "--html_dir", - type=Path, - default=None, - help=f"Output directory for --save_html (default: {DEFAULT_HTML_DIR}).", - ) - parser.add_argument( - "--save_mp4", - action="store_true", - default=False, - help=( - "Also render an offline MP4 via pyrender next to the .viser file. " - "Requires --save_html so the output directory exists. No browser or " - "Isaac Sim needed — uses the pinocchio/visual meshes." - ), - ) - return parser.parse_args() - - -def find_support_usd(input_dir: Path, sequence_id: str) -> Path | None: - """Try to find a support-surface .usda file for the given sequence. - - Looks in ``/reconstructed_stage/_support.usda``. - """ - candidate = input_dir.parent / "reconstructed_stage" / f"{sequence_id}_support.usda" - if candidate.exists(): - return candidate - return None - - -def visualize_one_trajectory( - viser_server: viser.ViserServer, - right_kinematics: HandKinematics, - left_kinematics: HandKinematics, - viser_object_handles: dict[str, Any], - input_dir: Path, - sequence_id: str, - trajectory_id: int, - data_class: type[ManoSharpaData] | type[ManoDex3Data] = ManoSharpaData, - robot_name: str = "sharpa_wave", - visualize_contacts: bool = False, - visualize_fingertip_distances: bool = False, - support_usd: Path | None = None, - serializer: Any = None, - mp4_out_path: Path | None = None, -) -> dict[str, Any]: - """Load one sequence and visualize playback (hands + objects from object_mesh_paths).""" - for _, handle in viser_object_handles.items(): - handle.remove() - viser_object_handles.clear() - - # Auto-discover or use explicit support-surface USD - usd_path = support_usd or find_support_usd(input_dir, sequence_id) - if usd_path is not None: - if not _USD_AVAILABLE: - print( - "WARNING: pxr (USD) is not available on this platform; skipping support surfaces." - ) - else: - print(f" Loading support surfaces from {usd_path}") - support_handles = load_support_surfaces_from_usd(viser_server, usd_path) - viser_object_handles.update(support_handles) - - contact_points_handles: list[Any] = [] - - logger_data = data_class.from_parquet( - root_path=str(input_dir), - filters=[ - ("sequence_id", "=", sequence_id), - ("robot_name", "=", robot_name), - ], - trajectory_id=trajectory_id, - ) - H = len(logger_data.robot_right_wrist_position) - - # Optional: world frame - viser_object_handles["frame"] = viser_server.scene.add_frame( - name="/object/frame", - position=np.array([0, 0, 0]), - wxyz=np.array([1, 0, 0, 0]), - axes_length=0.2, - axes_radius=0.007, - ) - - # Object meshes from schema paths (one per body); placeholders for any missing - object_mesh_paths = getattr(logger_data, "object_mesh_paths", None) or [] - if object_mesh_paths and len(object_mesh_paths) == len( - logger_data.object_body_names - ): - handles = load_object_meshes_from_paths( - viser_server, - object_mesh_paths, - logger_data.object_body_names, - ) - viser_object_handles.update(handles) - for part in logger_data.object_body_names: - if part not in viser_object_handles: - frame_handle = viser_server.scene.add_frame( - name=f"/object/{part}", - position=np.array([0.0, 0.0, 0.0]), - wxyz=np.array([1.0, 0.0, 0.0, 0.0]), - show_axes=False, - ) - viser_server.scene.add_icosphere( - name=f"/object/{part}/placeholder", - radius=0.02, - color=(128, 128, 128), - ) - viser_object_handles[part] = frame_handle - - # Optional: offline MP4 renderer mirroring the viser scene. - video_renderer = None - if mp4_out_path is not None: - # Same directory as this script — Python puts scripts/retarget/ on - # sys.path automatically when vis_retargeted.py is invoked directly. - sys.path.insert(0, str(Path(__file__).resolve().parent)) - from _offline_video import ( # type: ignore[import-not-found] # noqa: PLC0415 - OfflineVideoRenderer, - ) - - video_renderer = OfflineVideoRenderer(fps=int(round(float(logger_data.fps)))) - video_renderer.add_robot("right", right_kinematics) - video_renderer.add_robot("left", left_kinematics) - for obj_idx, obj_name in enumerate(logger_data.object_body_names): - if obj_idx < len(object_mesh_paths) and object_mesh_paths[obj_idx]: - try: - obj_mesh = trimesh.load(object_mesh_paths[obj_idx], force="mesh") - if getattr(logger_data, "dataset", None) == "taco": - obj_mesh.vertices *= 0.01 - video_renderer.add_object(obj_name, obj_mesh) - except Exception as e: # noqa: BLE001 - print(f" [mp4] skip object {obj_name}: {e}") - # Auto-fit the camera to the trajectory: use object positions + robot - # wrist positions across every frame so the subject stays in frame. - # Filter outliers (e.g., hidden hands placed at z=-100) by excluding - # points more than 5 m from the median before calling fit_camera. - frame_points = [ - np.asarray(logger_data.object_body_position).reshape(-1, 3), - np.asarray(logger_data.robot_right_wrist_position).reshape(-1, 3), - np.asarray(logger_data.robot_left_wrist_position).reshape(-1, 3), - ] - all_pts = np.concatenate(frame_points, axis=0) - if len(all_pts) > 0: - median = np.median(all_pts, axis=0) - inliers = all_pts[np.linalg.norm(all_pts - median, axis=1) < 5.0] - video_renderer.fit_camera(inliers if len(inliers) > 0 else all_pts) - - for frame_id in range(H): - # Right hand - right_qpos = right_kinematics.robot.q0.copy() - right_qpos[:3] = np.array(logger_data.robot_right_wrist_position[frame_id]) - right_qpos[3:7] = np.array(logger_data.robot_right_wrist_wxyz[frame_id])[ - [1, 2, 3, 0] - ] - right_qpos[7:] = np.array(logger_data.robot_right_finger_joints[frame_id]) - right_kinematics.visualize(viser_server, right_qpos) - # Left hand - left_qpos = left_kinematics.robot.q0.copy() - left_qpos[:3] = np.array(logger_data.robot_left_wrist_position[frame_id]) - left_qpos[3:7] = np.array(logger_data.robot_left_wrist_wxyz[frame_id])[ - [1, 2, 3, 0] - ] - left_qpos[7:] = np.array(logger_data.robot_left_finger_joints[frame_id]) - left_kinematics.visualize(viser_server, left_qpos) - - # Update object poses from Parquet. - # Use add_frame() (a full "add" message) rather than property-setter - # updates so the viser recorder replays positions correctly on every - # loop iteration — delta-update messages are not re-sent on loop. - for object_body_idx, object_body_name in enumerate( - logger_data.object_body_names - ): - if object_body_name not in viser_object_handles: - continue - viser_server.scene.add_frame( - name=f"/object/{object_body_name}", - position=np.asarray( - logger_data.object_body_position[frame_id][object_body_idx] - ), - wxyz=np.asarray( - logger_data.object_body_wxyz[frame_id][object_body_idx] - ), - show_axes=False, - ) - - # Fingertip distance spheres (if available) - if visualize_fingertip_distances: - for side, joints_data, dist_data in [ - ( - "right", - logger_data.mano_right_joints, - logger_data.mano_right_tips_distance, - ), - ( - "left", - logger_data.mano_left_joints, - logger_data.mano_left_tips_distance, - ), - ]: - if not dist_data: - continue - fingertip_positions = np.array(joints_data[frame_id])[ - MANO_FINGERTIP_INDICES - ] - distances = dist_data[frame_id] - for i, finger_name in enumerate(FINGER_NAMES): - viser_server.scene.add_icosphere( - name=f"/tips/{side}_{finger_name}", - radius=0.005, - color=distance_to_color(distances[i]), - position=fingertip_positions[i], - ) - - # Link contact visualization - if visualize_contacts: - for contact_handle in contact_points_handles: - contact_handle.remove() - contact_points_handles.clear() - for side, contact_positions, contact_normals in [ - ( - "right", - logger_data.mano_right_object_contact_positions[frame_id], - logger_data.mano_right_link_contact_normals[frame_id], - ), - ( - "left", - logger_data.mano_left_object_contact_positions[frame_id], - logger_data.mano_left_link_contact_normals[frame_id], - ), - ]: - for contact_position, (link_name, _) in zip( - contact_positions, MANO_HAND_LINKS.items(), strict=False - ): - if np.sum(contact_position) > 0.0: - contact_handle = viser_server.scene.add_icosphere( - name=f"/mano/{side}_contact_points/{link_name}", - radius=0.005, - color=np.array([0, 0, 255]), - position=np.array(contact_position[:3]), - ) - contact_points_handles.append(contact_handle) - normal_lines = np.stack( - [ - np.asarray(contact_positions), - ( - np.asarray(contact_positions) - + np.asarray(contact_normals) * 0.01 - ), - ], - axis=1, - ) - normal_handle = viser_server.scene.add_line_segments( - name=f"/mano/{side}_contact_normals", - points=normal_lines, - colors=np.zeros_like(normal_lines), - line_width=2.0, - ) - contact_points_handles.append(normal_handle) - - # Offline MP4 capture — mirror the current viser scene state. - if video_renderer is not None: - video_renderer.update_robot("right", right_qpos) - video_renderer.update_robot("left", left_qpos) - for object_body_idx, object_body_name in enumerate( - logger_data.object_body_names - ): - pos = np.asarray( - logger_data.object_body_position[frame_id][object_body_idx] - ) - wxyz = np.asarray( - logger_data.object_body_wxyz[frame_id][object_body_idx] - ) - # Convert wxyz -> scipy xyzw, then to rotation matrix. - rot = Rotation.from_quat(wxyz[[1, 2, 3, 0]]).as_matrix() - T = np.eye(4) - T[:3, :3] = rot - T[:3, 3] = pos - video_renderer.update_object(object_body_name, T) - video_renderer.capture() - - dt = 1.0 / logger_data.fps - if serializer is not None: - serializer.insert_sleep(dt) - else: - time.sleep(dt) - - if video_renderer is not None: - video_renderer.save(mp4_out_path) - video_renderer.close() - - return viser_object_handles - - -def _build_viser_client(html_dir: Path) -> None: - """Copy the viser JS client build into html_dir/viser-client/. - - Copies directly from the installed viser package rather than calling the - viser-build-client binary, which fails in Isaac Sim's environment because - imageio is only on sys.path when launched via isaaclab.sh. - """ - client_dir = html_dir / "viser-client" - - viser_client_build = Path(viser.__file__).parent / "client" / "build" - if not viser_client_build.is_dir(): - print( - f"WARNING: viser client build not found at {viser_client_build}; skipping." - ) - sys.exit(-1) - return - - # ``dirs_exist_ok`` makes this idempotent under parallel shards — the first - # shard populates the dir and the others no-op over the same files instead - # of racing on ``client_dir.exists()`` + ``copytree``. - print(f"Copying viser client → {client_dir}") - shutil.copytree(viser_client_build, client_dir, dirs_exist_ok=True) - print(f" Viser client ready at {client_dir}") - - -def main(args: argparse.Namespace) -> None: - """List or use sequence, setup kinematics, run visualization.""" - if args.input_dir is not None: - input_dir = args.input_dir - elif args.dataset is not None: - input_dir = HUMAN_MOTION_DATA_DIR / DATASET_DIRS[args.dataset] - else: - input_dir = HUMAN_MOTION_DATA_DIR / DATASET_DIRS["arctic"] - - if not input_dir.is_dir(): - raise FileNotFoundError(f"Input directory not found: {input_dir}") - - available = list_sequence_ids(str(input_dir)) - if not available: - raise ValueError(f"No sequences found in {input_dir}") - - sequence_ids = filter_sequence_ids(available, args) - if not sequence_ids: - # Small datasets sharded many ways routinely produce empty buckets - # (md5 over ~30 sequences into 8 shards often leaves one with zero). - # Treat that as a clean no-op so sibling shards can finish the stage. - num_shards = getattr(args, "num_shards", 1) or 1 - shard_id = getattr(args, "shard_id", 0) or 0 - if num_shards > 1: - print( - f"[vis_retargeted] Shard {shard_id}/{num_shards}: " - f"no sequences matched (available: {len(available)}); exiting cleanly." - ) - return - raise ValueError( - f"No sequences match the provided filters (available: {len(available)})." - ) - if len(sequence_ids) == len(available): - print( - f"No filter specified, will iterate through all {len(sequence_ids)} sequences." - ) - else: - print(f"Filter selected {len(sequence_ids)}/{len(available)} sequences.") - - # Resolve HTML output directory and build the viser client (once) - html_dir: Path | None = None - if args.save_html: - _html_dir: Path = args.html_dir or DEFAULT_HTML_DIR - (_html_dir / "recordings").mkdir(parents=True, exist_ok=True) - _build_viser_client(_html_dir) - index_src = DEFAULT_HTML_DIR / "index.html" - index_dst = _html_dir / "index.html" - if index_src.exists() and not index_dst.exists(): - shutil.copy2(index_src, index_dst) - html_dir = _html_dir - - viser_server = viser.ViserServer() - viser_object_handles: dict[str, Any] = {} - - support_usd = args.support_usd - if support_usd is not None and not support_usd.exists(): - raise FileNotFoundError(f"Support USD not found: {support_usd}") - - setup_kinematics = ROBOT_KINEMATICS_SETUP[args.robot] - data_class = ROBOT_DATA_CLASSES[args.robot] - right_kinematics = setup_kinematics( - side="right", frame_tasks_converged_threshold=1e-6 - ) - left_kinematics = setup_kinematics( - side="left", frame_tasks_converged_threshold=1e-6 - ) - - for seq_idx, sequence_id in enumerate(sequence_ids): - print( - f"[{seq_idx + 1}/{len(sequence_ids)}] Visualizing sequence: {sequence_id}" - ) - serializer = viser_server.get_scene_serializer() if args.save_html else None - mp4_out_path = ( - html_dir / "recordings" / f"{sequence_id}.mp4" - if args.save_mp4 and html_dir is not None - else None - ) - visualize_one_trajectory( - viser_server, - right_kinematics, - left_kinematics, - viser_object_handles, - input_dir=input_dir, - sequence_id=sequence_id, - trajectory_id=args.trajectory_id, - data_class=data_class, - robot_name=args.robot, - visualize_contacts=args.visualize_contacts, - visualize_fingertip_distances=args.visualize_fingertip_distances, - support_usd=support_usd, - serializer=serializer, - mp4_out_path=mp4_out_path, - ) - if serializer is not None and html_dir is not None: - out_path = html_dir / "recordings" / f"{sequence_id}.viser" - out_path.write_bytes(serializer.serialize()) - print(f" Saved → {out_path}") - print( - f"\n Playback instructions:\n" - f" scp -r {html_dir} :/tmp/viser_replay\n" - f" cd /tmp/viser_replay && python -m http.server 8000\n" - f" http://localhost:8000/viser-client/" - f"?playbackPath=http://localhost:8000/recordings/{sequence_id}.viser\n" - ) - - -if __name__ == "__main__": - args = parse_args() - main(args) diff --git a/robotic_grounding/scripts/rsl_rl/cli_args.py b/robotic_grounding/scripts/rsl_rl/cli_args.py deleted file mode 100644 index 9576e452..00000000 --- a/robotic_grounding/scripts/rsl_rl/cli_args.py +++ /dev/null @@ -1,124 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from __future__ import annotations - -import argparse -import random -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg - - -def add_rsl_rl_args(parser: argparse.ArgumentParser): - """Add RSL-RL arguments to the parser. - - Args: - parser: The parser to add the arguments to. - """ - # create a new argument group - arg_group = parser.add_argument_group( - "rsl_rl", description="Arguments for RSL-RL agent." - ) - # -- experiment arguments - arg_group.add_argument( - "--experiment_name", - type=str, - default=None, - help="Name of the experiment folder where logs will be stored.", - ) - arg_group.add_argument( - "--run_name", - type=str, - default=None, - help="Run name suffix to the log directory.", - ) - # -- load arguments - arg_group.add_argument( - "--resume", - action="store_true", - default=False, - help="Whether to resume from a checkpoint.", - ) - arg_group.add_argument( - "--load_run", - type=str, - default=None, - help="Name of the run folder to resume from.", - ) - arg_group.add_argument( - "--checkpoint", type=str, default=None, help="Checkpoint file to resume from." - ) - # -- logger arguments - arg_group.add_argument( - "--logger", - type=str, - default=None, - choices={"wandb", "tensorboard", "neptune"}, - help="Logger module to use.", - ) - arg_group.add_argument( - "--log_project_name", - type=str, - default=None, - help="Name of the logging project when using wandb or neptune.", - ) - - -def parse_rsl_rl_cfg( - task_name: str, args_cli: argparse.Namespace -) -> RslRlBaseRunnerCfg: - """Parse configuration for RSL-RL agent based on inputs. - - Args: - task_name: The name of the environment. - args_cli: The command line arguments. - - Returns: - The parsed configuration for RSL-RL agent based on inputs. - """ - from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry - - # load the default configuration - rslrl_cfg: RslRlBaseRunnerCfg = load_cfg_from_registry( - task_name, "rsl_rl_cfg_entry_point" - ) - rslrl_cfg = update_rsl_rl_cfg(rslrl_cfg, args_cli) - return rslrl_cfg - - -def update_rsl_rl_cfg(agent_cfg: RslRlBaseRunnerCfg, args_cli: argparse.Namespace): - """Update configuration for RSL-RL agent based on inputs. - - Args: - agent_cfg: The configuration for RSL-RL agent. - args_cli: The command line arguments. - - Returns: - The updated configuration for RSL-RL agent based on inputs. - """ - # override the default configuration with CLI arguments - if hasattr(args_cli, "seed") and args_cli.seed is not None: - # randomly sample a seed if seed = -1 - if args_cli.seed == -1: - args_cli.seed = random.randint(0, 10000) - agent_cfg.seed = args_cli.seed - if args_cli.resume is not None: - agent_cfg.resume = args_cli.resume - if args_cli.load_run is not None: - agent_cfg.load_run = args_cli.load_run - if args_cli.checkpoint is not None: - agent_cfg.load_checkpoint = args_cli.checkpoint - if args_cli.run_name is not None: - agent_cfg.run_name = args_cli.run_name - if args_cli.logger is not None: - agent_cfg.logger = args_cli.logger - # set the project name for wandb and neptune - if agent_cfg.logger in {"wandb", "neptune"} and args_cli.log_project_name: - agent_cfg.wandb_project = args_cli.log_project_name - agent_cfg.neptune_project = args_cli.log_project_name - - return agent_cfg diff --git a/robotic_grounding/scripts/rsl_rl/download_from_wandb.py b/robotic_grounding/scripts/rsl_rl/download_from_wandb.py deleted file mode 100644 index 2a3e8808..00000000 --- a/robotic_grounding/scripts/rsl_rl/download_from_wandb.py +++ /dev/null @@ -1,96 +0,0 @@ -import os -import json -import wandb -import re -import argparse - - -def extract_checkpoint_number(filename: str) -> int: - """Extract the checkpoint number from a filename.""" - match = re.search(r"(\d+)", os.path.basename(filename)) - return int(match.group(1)) if match else -1 - - -def download_run( - run_id: str, checkpoint: str | None = None, log_dir: str | None = None -) -> str | None: - """Download a run from wandb.""" - # 0. Initialize wandb API - api = wandb.Api() - try: - run = api.run(run_id) - except wandb.errors.CommError as e: - print(f"⚠️ [Wandb] Error: Could not access run '{run_id}': {e}") - return None - - # 1. Get configs - configs = run.config - log_dir = ( - os.path.join(log_dir, configs["log_dir"].split("/")[-1]) - if log_dir is not None - else configs["log_dir"] - ) - alg_cfg = configs["alg_cfg"] - policy_cfg = configs["policy_cfg"] - env_cfg = configs["env_cfg"] - runner_cfg = configs["runner_cfg"] - - # 2. Create a folder based on log_dir - print(f"📥 [Wandb] Creating folder {log_dir}") - os.makedirs(log_dir, exist_ok=True) - - # 3. Dump cfgs to log_dir/params - params_dir = os.path.join(log_dir, "params") - os.makedirs(params_dir, exist_ok=True) - print(f"📥 [Wandb] Dumping configs to {params_dir}") - for name, cfg in [ - ("alg_cfg", alg_cfg), - ("policy_cfg", policy_cfg), - ("env_cfg", env_cfg), - ("runner_cfg", runner_cfg), - ]: - with open(os.path.join(params_dir, f"{name}.json"), "w") as f: - json.dump(cfg, f, indent=4) - - # 4. Download checkpoint files to log_dir - files = [f for f in run.files() if f.name.endswith(".pt")] - if not files: - print("⚠️ [Wandb] No checkpoint files found") - return None - - if checkpoint: - matched = [f for f in files if checkpoint in f.name] - if not matched: - print(f"⚠️ [Wandb] No checkpoint matching '{checkpoint}' found") - return None - files = matched - else: - # Download only the latest checkpoint (highest iteration number) - files = [max(files, key=lambda f: extract_checkpoint_number(f.name))] - - print(f"📥 [Wandb] Downloading {[f.name for f in files]} to {log_dir}") - for file in files: - file.download(root=log_dir, replace=True) - - return os.path.join(log_dir, files[0].name) - - -if __name__ == "__main__": - - parser = argparse.ArgumentParser(description="Download a run from wandb") - parser.add_argument( - "run_id", help="Wandb run ID (e.g. //)" - ) - parser.add_argument( - "--checkpoint", - default=None, - help="Checkpoint name to download (default: latest)", - ) - parser.add_argument( - "--log_dir", - default=None, - help="Log directory to download to (default: None)", - ) - args = parser.parse_args() - - download_run(args.run_id, args.checkpoint, args.log_dir) diff --git a/robotic_grounding/scripts/rsl_rl/dummy_agent.py b/robotic_grounding/scripts/rsl_rl/dummy_agent.py deleted file mode 100644 index 8eff9643..00000000 --- a/robotic_grounding/scripts/rsl_rl/dummy_agent.py +++ /dev/null @@ -1,286 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - -"""Script to run an environment with dummy action agent.""" - -"""Launch Isaac Sim Simulator first.""" - -import argparse -import time - -from isaaclab.app import AppLauncher - -# add argparse arguments -parser = argparse.ArgumentParser(description="Zero agent for Isaac Lab environments.") -parser.add_argument( - "--disable_fabric", - action="store_true", - default=False, - help="Disable fabric and use USD I/O operations.", -) -parser.add_argument( - "--num_envs", type=int, default=15, help="Number of environments to simulate." -) -parser.add_argument("--task", type=str, default=None, help="Name of the task.") -parser.add_argument( - "--action_mode", - type=str, - default="zero", - choices=["zero"], - help="Action mode.", -) -parser.add_argument( - "--motion_file", - type=str, - default=None, - help="Motion file to load (e.g. arctic_processed/arctic_s01_mixer_use_01/sharpa_wave).", -) -parser.add_argument( - "--initial_virtual_object_control_curriculum_scale", - type=float, - default=1.0, - help="Initial VOC curriculum scale for dual-hands object tracking command.", -) -parser.add_argument( - "--disable_robot_to_object_collisions", - action="store_true", - default=False, - help="Disable robot-to-object collisions.", -) -parser.add_argument( - "--disable_robot_to_fixed_object_collisions", - action="store_true", - default=False, - help="Disable robot-to-fixed-object collisions.", -) -parser.add_argument( - "--use_primitive_urdfs", - action="store_true", - default=False, - help="Use primitive URDFs for the robot.", -) -parser.add_argument( - "--record_video", - action="store_true", - default=False, - help="Record an MP4 of the scene replay.", -) -parser.add_argument( - "--output_dir", - type=str, - default=None, - help="Directory to save recorded video (required with --record_video).", -) -parser.add_argument( - "--video_length", - type=int, - default=300, - help="Number of simulation steps to record (default: 300).", -) -parser.add_argument( - "--success_marker", - type=str, - default=None, - help=( - "Optional path to a sentinel file. Written ONLY after the recording " - "loop completes cleanly. CUDA assert, exception, or timeout-kill all " - "prevent the write from happening, so a present sentinel proves the " - "sequence played end-to-end. Used by workflow/retarget.yaml Stage 6 " - "to split processed parquets into renderable vs. invalid buckets." - ), -) -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -# Enable cameras when recording video (must be set before AppLauncher) -if args_cli.record_video: - args_cli.enable_cameras = True - if not args_cli.output_dir: - raise ValueError("--output_dir is required when using --record_video") - -################################### -# Debug — uncomment to override CLI args -################################### -# args_cli.task = "Sharpa-V2P-v0-Play" -# args_cli.headless = True -# args_cli.disable_robot_to_object_collisions = False -# args_cli.disable_robot_to_fixed_object_collisions = True -# args_cli.motion_file = "arctic/arctic_processed/arctic_s01_mixer_use_01/sharpa_wave" -################################### - -# launch omniverse app -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -"""All Omniverse/Isaac Sim imports after SimulationApp is created.""" - -import os - -import gymnasium as gym -import torch -import torch.nn.functional as F - -import isaaclab.utils.math as math_utils -import isaaclab_tasks # noqa: F401 -from isaaclab.envs import ManagerBasedRLEnvCfg -from robotic_grounding.tasks import * -from robotic_grounding.tasks.scene_utils import SceneConfig, apply_scene_config -from isaaclab_tasks.utils import parse_env_cfg - - -def _autoframe_viewer(env_cfg, motion_file: str) -> None: - """Re-point env_cfg.viewer at the centroid of object + wrist positions. - - Read from the parquet's object_body_position + robot_{side}_wrist_position - fields; pick an eye offset along (-y, +x, +z) so the camera is ~1.2× the - scene extent away, elevated ~30°. - """ - import numpy as np - import pyarrow.parquet as pq - - if not (isinstance(env_cfg, ManagerBasedRLEnvCfg) and hasattr(env_cfg, "viewer")): - return - try: - data = pq.read_table(motion_file).to_pydict() - pts = [] - obj = data.get("object_body_position", [None])[0] - if obj: - pts.append(np.asarray(obj).reshape(-1, 3)) - for side in ("right", "left"): - wrist = data.get(f"robot_{side}_wrist_position", [None])[0] - if wrist: - pts.append(np.asarray(wrist).reshape(-1, 3)) - if not pts: - return - all_pts = np.concatenate(pts, axis=0) - lo, hi = all_pts.min(axis=0), all_pts.max(axis=0) - center = 0.5 * (lo + hi) - extent = max(float(np.linalg.norm(hi - lo)), 0.3) - dist = 1.2 * extent - # Azimuth 135° (x<0, y>0), elevation 30°. - eye = center + dist * np.array([-0.61, 0.61, 0.5]) - env_cfg.viewer.lookat = tuple(float(c) for c in center) - env_cfg.viewer.eye = tuple(float(c) for c in eye) - print( - f"[INFO] viewer autoframe: lookat={env_cfg.viewer.lookat}, eye={env_cfg.viewer.eye}" - ) - except Exception as e: # noqa: BLE001 - print(f"[WARNING] viewer autoframe failed: {e}") - - -def main(): - """Zero actions agent with Isaac Lab environment.""" - # parse configuration - env_cfg = parse_env_cfg( - args_cli.task, - device=args_cli.device, - num_envs=args_cli.num_envs, - use_fabric=not args_cli.disable_fabric, - ) - - # Apply scene config from --motion_file - if args_cli.motion_file is not None: - env_cfg.motion_file = args_cli.motion_file - if hasattr(env_cfg, "motion_file") and env_cfg.motion_file is not None: - scene_config = SceneConfig.from_motion_file(env_cfg.motion_file) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - # Auto-frame the viewer on the actual motion bounding box. The default - # eye/lookat in v2p_hand_env_cfg targets standing-human scenes (lookat - # z=1.2); tabletop datasets like h2o / dexycb are at z<0.3 and end up - # off-screen otherwise. - _autoframe_viewer(env_cfg, scene_config.motion_file) - - # clamp viewer env_index to valid range (env_cfg may default to a higher index) - if isinstance(env_cfg, ManagerBasedRLEnvCfg) and hasattr(env_cfg, "viewer"): - env_cfg.viewer.env_index = min( - env_cfg.viewer.env_index, env_cfg.scene.num_envs - 1 - ) - - # V2P dual-hands specific config (skip for whole-body envs) - if hasattr(env_cfg, "commands") and hasattr( - env_cfg.commands, "dual_hands_object_tracking_command" - ): - env_cfg.commands.dual_hands_object_tracking_command.initial_virtual_object_control_curriculum_scale = float( - args_cli.initial_virtual_object_control_curriculum_scale - ) - if hasattr(env_cfg, "events") and hasattr(env_cfg.events, "setup_collision_groups"): - env_cfg.events.setup_collision_groups.params[ - "disable_robot_to_object_collisions" - ] = args_cli.disable_robot_to_object_collisions - env_cfg.events.setup_collision_groups.params[ - "disable_robot_to_fixed_object_collisions" - ] = args_cli.disable_robot_to_fixed_object_collisions - - # create environment (with RecordVideo wrapper if requested) - if args_cli.record_video: - os.makedirs(args_cli.output_dir, exist_ok=True) - env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array") - env = gym.wrappers.RecordVideo( - env, - video_folder=args_cli.output_dir, - step_trigger=lambda step: step == 0, - video_length=args_cli.video_length, - disable_logger=True, - ) - else: - env = gym.make(args_cli.task, cfg=env_cfg) - - print(f"[INFO]: Gym observation space: {env.observation_space}") - print(f"[INFO]: Gym action space: {env.action_space}") - env.reset() - - actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) - - if args_cli.record_video: - print( - f"[INFO] Recording {args_cli.video_length} steps to {args_cli.output_dir}" - ) - for step in range(args_cli.video_length): - with torch.inference_mode(): - env.step(actions) - if (step + 1) % 100 == 0: - print(f"[INFO] Step: {step + 1}/{args_cli.video_length}") - print(f"[INFO] Recording loop done; closing env to flush MP4") - else: - while simulation_app.is_running(): - with torch.inference_mode(): - env.step(actions) - - env.close() - - # Sentinel goes here — AFTER env.close() has returned AND the MP4 is on - # disk. gymnasium.wrappers.RecordVideo's step-loop uses a strict - # `len(recorded_frames) > video_length` check, so running exactly - # `video_length` steps never trips stop_recording() inside the loop; - # the moviepy flush runs inside close() instead. Writing the sentinel - # earlier lets it survive an Omniverse-shutdown deadlock, a silent - # moviepy/ffmpeg failure (disable_logger=True), or an outer - # `timeout --signal=KILL` that lands between touch and flush — all - # observed at least once (sequence_id=dataset_s01_box_use_02 in - # isaac/retargeted_arctic_exp_50). - if args_cli.record_video and args_cli.success_marker: - from pathlib import Path as _Path - - expected_mp4 = _Path(args_cli.output_dir) / "rl-video-step-0.mp4" - if expected_mp4.is_file() and expected_mp4.stat().st_size > 0: - marker = _Path(args_cli.success_marker) - marker.parent.mkdir(parents=True, exist_ok=True) - marker.touch() - print(f"[INFO] Wrote success marker {marker}") - else: - print( - f"[WARN] No MP4 at {expected_mp4}; withholding success marker " - f"{args_cli.success_marker}" - ) - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/eval.py b/robotic_grounding/scripts/rsl_rl/eval.py deleted file mode 100644 index 3cf7634a..00000000 --- a/robotic_grounding/scripts/rsl_rl/eval.py +++ /dev/null @@ -1,439 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Script to play a checkpoint if an RL agent from RSL-RL.""" - -"""Launch Isaac Sim Simulator first.""" - -import argparse -import sys - -from isaaclab.app import AppLauncher - -# local imports -import cli_args # isort: skip - -# add argparse arguments -parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") -parser.add_argument( - "--video", action="store_true", default=False, help="Record videos during training." -) -parser.add_argument( - "--video_length", - type=int, - default=200, - help="Length of the recorded video (in steps).", -) -parser.add_argument( - "--disable_fabric", - action="store_true", - default=False, - help="Disable fabric and use USD I/O operations.", -) -parser.add_argument( - "--num_envs", type=int, default=None, help="Number of environments to simulate." -) -parser.add_argument("--task", type=str, default=None, help="Name of the task.") -parser.add_argument( - "--agent", - type=str, - default="rsl_rl_cfg_entry_point", - help="Name of the RL agent configuration entry point.", -) -parser.add_argument( - "--seed", type=int, default=None, help="Seed used for the environment" -) -parser.add_argument( - "--use_pretrained_checkpoint", - action="store_true", - help="Use the pre-trained checkpoint from Nucleus.", -) -parser.add_argument( - "--real-time", - action="store_true", - default=False, - help="Run in real-time, if possible.", -) -parser.add_argument( - "--eval_episodes", - type=int, - default=None, - help="If set, collect this many completed episodes, print aggregate stats, then exit.", -) -parser.add_argument( - "--scene_config", - type=str, - default=None, - help="Path to the scene configuration file.", -) -parser.add_argument( - "--motion_file", - type=str, - default=None, - help="Motion file to load.", -) -parser.add_argument( - "--motion_start_frame", - type=int, - default=None, - help="First frame of the source motion to use (inclusive). 0 = beginning.", -) -parser.add_argument( - "--motion_end_frame", - type=int, - default=None, - help="One past the last frame to use. -1 means end of sequence.", -) -parser.add_argument( - "--wandb_id", - type=str, - default=None, - help="Wandb run ID to resume from.", -) -parser.add_argument( - "--use_primitive_urdfs", - action="store_true", - default=False, - help="Use primitive URDFs for the robot.", -) -# append RSL-RL cli arguments -cli_args.add_rsl_rl_args(parser) -# append AppLauncher cli args -AppLauncher.add_app_launcher_args(parser) -# parse the arguments -args_cli, hydra_args = parser.parse_known_args() -# always enable cameras to record video -if args_cli.video: - args_cli.enable_cameras = True - -# clear out sys.argv for Hydra -sys.argv = [sys.argv[0]] + hydra_args - -# launch omniverse app -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -"""Rest everything follows.""" - -import gymnasium as gym -import os -import time -import torch -from download_from_wandb import download_run - -from rsl_rl.runners import DistillationRunner, OnPolicyRunner - -from isaaclab.envs import ( - DirectMARLEnv, - DirectMARLEnvCfg, - DirectRLEnvCfg, - ManagerBasedRLEnvCfg, - multi_agent_to_single_agent, -) -from isaaclab.utils.assets import retrieve_file_path -from isaaclab.utils.dict import print_dict - -from isaaclab_rl.rsl_rl import ( - RslRlBaseRunnerCfg, - RslRlVecEnvWrapper, - export_policy_as_jit, - export_policy_as_onnx, -) - -# Optional: pretrained checkpoint download (not available in all Isaac Lab versions) -try: - from isaaclab_rl.utils.pretrained_checkpoint import ( - get_published_pretrained_checkpoint, - ) -except ImportError: - get_published_pretrained_checkpoint = None - -import isaaclab_tasks # noqa: F401 -import robotic_grounding.tasks # noqa: F401 -from robotic_grounding.tasks.scene_utils import SceneConfig, apply_scene_config -from viewer_utils import autoframe_viewer - -from isaaclab_tasks.utils import get_checkpoint_path -from isaaclab_tasks.utils.hydra import hydra_task_config - -# PLACEHOLDER: Extension template (do not remove this comment) - - -def _apply_eval_defaults(env_cfg) -> None: - """Force deterministic eval behaviour: frame 0 reset, VOC fully off. - - Eval should reproduce the policy's behaviour against the reference - trajectory, not exercise the training-time exploration helpers. We: - - Reset every env to frame 0 (`always_reset_to_first_frame`). - - Zero out the virtual-object-control scale + reset target so the policy - drives the object on its own. - - Disable the curriculum entirely so it cannot bump VOC back up the way - it does at training step 0 (see G1SonicReconBodyVOCCurriculumCfg). - """ - if hasattr(env_cfg, "commands") and hasattr(env_cfg.commands, "motion"): - motion_cfg = env_cfg.commands.motion - motion_cfg.always_reset_to_first_frame = True - motion_cfg.initial_virtual_object_control_curriculum_scale = 0.0 - motion_cfg.voc_reset_scale = 0.0 - motion_cfg.voc_decay_steps = 0 - if hasattr(env_cfg, "curriculum"): - env_cfg.curriculum = None - - -@hydra_task_config(args_cli.task, args_cli.agent) -def main( - env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, - agent_cfg: RslRlBaseRunnerCfg, -): - """Play with RSL-RL agent.""" - # grab task name for checkpoint path - task_name = args_cli.task.split(":")[-1] - train_task_name = task_name.replace("-Play", "") - - # override configurations with non-hydra CLI arguments - agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) - env_cfg.scene.num_envs = ( - args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs - ) - - # Apply scene config: motion_file (from Hydra override) takes priority, - # then --scene_config YAML, then the env_cfg default. - env_cfg.motion_file = args_cli.motion_file - if hasattr(env_cfg, "motion_file") and env_cfg.motion_file is not None: - scene_config = SceneConfig.from_motion_file(env_cfg.motion_file) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - autoframe_viewer(env_cfg, scene_config.motion_file) - elif args_cli.scene_config is not None: - env_cfg.scene_config_path = args_cli.scene_config - scene_config = SceneConfig.from_yaml(args_cli.scene_config) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - autoframe_viewer(env_cfg, scene_config.motion_file) - - # Apply optional motion frame range overrides. - if hasattr(env_cfg, "commands") and hasattr(env_cfg.commands, "motion"): - if args_cli.motion_start_frame is not None: - env_cfg.commands.motion.motion_start_frame = args_cli.motion_start_frame - if args_cli.motion_end_frame is not None: - env_cfg.commands.motion.motion_end_frame = args_cli.motion_end_frame - - # Eval-mode safety: deterministic reset + VOC off. - _apply_eval_defaults(env_cfg) - - # set the environment seed - # note: certain randomizations occur in the environment initialization so we set the seed here - env_cfg.seed = agent_cfg.seed - env_cfg.sim.device = ( - args_cli.device if args_cli.device is not None else env_cfg.sim.device - ) - - # specify directory for logging experiments - log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) - log_root_path = os.path.abspath(log_root_path) - print(f"[INFO] Loading experiment from directory: {log_root_path}") - if args_cli.use_pretrained_checkpoint: - if get_published_pretrained_checkpoint is None: - print( - "[ERROR] --use_pretrained_checkpoint requires isaaclab_rl.utils.pretrained_checkpoint " - "which is not available in this Isaac Lab version." - ) - return - resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name) - if not resume_path: - print( - "[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task." - ) - return - elif args_cli.checkpoint: - resume_path = retrieve_file_path(args_cli.checkpoint) - elif args_cli.wandb_id is not None: - resume_path = download_run(args_cli.wandb_id) - agent_cfg.load_checkpoint = resume_path - else: - resume_path = get_checkpoint_path( - log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint - ) - - log_dir = os.path.dirname(resume_path) - - # set the log directory for the environment (works for all environment types) - env_cfg.log_dir = log_dir - - # create isaac environment - env = gym.make( - args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None - ) - - # convert to single-agent instance if required by the RL algorithm - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - - # wrap for video recording - if args_cli.video: - video_kwargs = { - "video_folder": os.path.join(log_dir, "videos", "play"), - "step_trigger": lambda step: step == 0, - "video_length": args_cli.video_length, - "disable_logger": True, - } - print("[INFO] Recording videos during training.") - print_dict(video_kwargs, nesting=4) - env = gym.wrappers.RecordVideo(env, **video_kwargs) - - # wrap around environment for rsl-rl - env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) - - print(f"[INFO]: Loading model checkpoint from: {resume_path}") - # load previously trained model - if agent_cfg.class_name == "OnPolicyRunner": - runner = OnPolicyRunner( - env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device - ) - elif agent_cfg.class_name == "DistillationRunner": - runner = DistillationRunner( - env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device - ) - else: - raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") - runner.load(resume_path) - - # obtain the trained policy for inference - policy = runner.get_inference_policy(device=env.unwrapped.device) - - # extract the neural network module - # we do this in a try-except to maintain backwards compatibility. - try: - # version 2.3 onwards - policy_nn = runner.alg.policy - except AttributeError: - # version 2.2 and below - policy_nn = runner.alg.actor_critic - - # extract the normalizer - if hasattr(policy_nn, "actor_obs_normalizer"): - normalizer = policy_nn.actor_obs_normalizer - elif hasattr(policy_nn, "student_obs_normalizer"): - normalizer = policy_nn.student_obs_normalizer - else: - normalizer = None - - # export policy to onnx/jit - export_model_dir = os.path.join(os.path.dirname(resume_path), "exported") - export_policy_as_jit( - policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.pt" - ) - export_policy_as_onnx( - policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.onnx" - ) - - dt = env.unwrapped.step_dt - - # reset environment - obs = env.get_observations() - timestep = 0 - - # eval_episodes tracking - eval_episodes = args_cli.eval_episodes - if eval_episodes is not None: - num_envs = env.unwrapped.num_envs - _cmd = env.unwrapped.command_manager.get_term( - "dual_hands_object_tracking_command" - ) - _warmup = getattr(_cmd.cfg, "virtual_object_control_decay_steps", 20) - my_ep_len = torch.zeros( - num_envs, dtype=torch.float32, device=env.unwrapped.device - ) - # Snapshot tracking_lengths at episode start; updated on each reset so we always - # record the length that belonged to the episode that just completed, not the next one. - # (IsaacLab resets terminated envs inside env.step before returning dones=True, so - # reading tracking_lengths after step gives the NEW episode's length, not the old one.) - # Note: tracking_lengths is in trajectory frames (random start offset applied), while - # ep_len includes the warm-start steps. Ratio = min(ep_len - warmup, track_len) / track_len. - my_ep_tracking_len = _cmd.tracking_lengths.clone().float().squeeze(-1) - completed: list[tuple[float, float]] = [] # (episode_length, tracking_length) - - # simulate environment - while simulation_app.is_running(): - start_time = time.time() - # run everything in inference mode - with torch.inference_mode(): - # agent stepping - actions = policy(obs) - # env stepping - obs, _, dones, _ = env.step(actions) - # log which termination fired - if dones.any(): - term_mgr = env.unwrapped.termination_manager - term_dones = term_mgr._term_dones # (E, num_terms) - for i, name in enumerate(term_mgr.active_terms): - if term_dones[:, i].any(): - print( - f"[TERM] {name}: {term_dones[:, i].sum().item():.0f} envs" - ) - # reset recurrent states for episodes that have terminated - policy_nn.reset(dones) - if args_cli.video: - timestep += 1 - # Exit the play loop after recording one video - if timestep == args_cli.video_length: - break - - if eval_episodes is not None: - my_ep_len += 1 - done_mask = dones.bool() - if done_mask.any(): - done_ids = done_mask.nonzero(as_tuple=False).squeeze(-1) - for i in done_ids: - ep_len = my_ep_len[i].item() - track_len = my_ep_tracking_len[i].item() - completed.append((ep_len, track_len)) - my_ep_len[i] = 0.0 - # Update tracking_length snapshot for the new episode that just started - my_ep_tracking_len[i] = _cmd.tracking_lengths[i].float() - if len(completed) >= eval_episodes: - break - - # time delay for real-time evaluation - sleep_time = dt - (time.time() - start_time) - if args_cli.real_time and sleep_time > 0: - time.sleep(sleep_time) - - if eval_episodes is not None and completed: - data = completed[:eval_episodes] - lens = [e[0] for e in data] - traj_len = _cmd.retargeted_horizon - # Ratio = post-warmup steps / post-warmup trajectory length so a perfectly - # completing episode always gives ratio = 1.0. - _usable = max(traj_len - _warmup, 1) - ratios = [min(max(e[0] - _warmup, 0), _usable) / _usable for e in data] - full = sum(1 for r in ratios if r >= 0.99) - mean_len = sum(lens) / len(lens) - std_len = ( - sum((x - mean_len) ** 2 for x in lens) / max(len(lens) - 1, 1) - ) ** 0.5 - mean_ratio = sum(ratios) / len(ratios) - std_ratio = ( - sum((x - mean_ratio) ** 2 for x in ratios) / max(len(ratios) - 1, 1) - ) ** 0.5 - print("\n[eval] ===== Eval Summary =====") - print( - f" Episodes: {len(data)} (full trajectory: {traj_len} steps, warmup: {_warmup})" - ) - print(f" Episode length: mean={mean_len:.1f} std={std_len:.1f} steps") - print(f" Completion ratio: mean={mean_ratio:.3f} std={std_ratio:.3f}") - print(f" Full completions: {full}/{len(data)} ({100*full/len(data):.0f}%)") - - # close the simulator - env.close() - - -if __name__ == "__main__": - # run the main function - main() - # close sim app - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/eval_batch.py b/robotic_grounding/scripts/rsl_rl/eval_batch.py deleted file mode 100644 index c1abd8c2..00000000 --- a/robotic_grounding/scripts/rsl_rl/eval_batch.py +++ /dev/null @@ -1,179 +0,0 @@ -"""Batch eval: one Isaac Sim session, multiple checkpoints. - -Iterates over all model_*.pt files in a checkpoint directory (sorted by iteration), -runs --eval_episodes episodes for each, and prints a summary table. -""" - -import argparse -import sys - -from isaaclab.app import AppLauncher - -import cli_args # isort: skip - -parser = argparse.ArgumentParser(description="Batch eval across multiple checkpoints.") -parser.add_argument( - "--checkpoints_dir", - type=str, - required=True, - help="Directory containing model_*.pt files to evaluate in order.", -) -parser.add_argument( - "--eval_episodes", - type=int, - default=100, - help="Number of completed episodes to collect per checkpoint.", -) -parser.add_argument("--num_envs", type=int, default=32) -parser.add_argument("--task", type=str, default=None) -parser.add_argument("--motion_file", type=str, default=None) -parser.add_argument("--agent", type=str, default="rsl_rl_cfg_entry_point") -parser.add_argument("--seed", type=int, default=None) -parser.add_argument("--disable_fabric", action="store_true", default=False) -cli_args.add_rsl_rl_args(parser) -AppLauncher.add_app_launcher_args(parser) -args_cli, hydra_args = parser.parse_known_args() -args_cli.headless = True - -sys.argv = [sys.argv[0]] + hydra_args - -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -"""Rest everything follows.""" - -import glob -import os -import re -import torch - -import gymnasium as gym - -from rsl_rl.runners import DistillationRunner, OnPolicyRunner - -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab_rl.rsl_rl import RslRlVecEnvWrapper - -import isaaclab_tasks # noqa: F401 -import robotic_grounding.tasks # noqa: F401 -from robotic_grounding.tasks.scene_utils import SceneConfig, apply_scene_config -from viewer_utils import autoframe_viewer - -from isaaclab_tasks.utils.hydra import hydra_task_config - - -def _itr(path): - m = re.search(r"model_(\d+)\.pt", os.path.basename(path)) - return int(m.group(1)) if m else -1 - - -@hydra_task_config(args_cli.task, args_cli.agent) -def main(env_cfg: ManagerBasedRLEnvCfg, agent_cfg): - # Find and sort checkpoints - ckpts = sorted( - glob.glob(os.path.join(args_cli.checkpoints_dir, "model_*.pt")), key=_itr - ) - if not ckpts: - print(f"[eval_batch] No model_*.pt found in {args_cli.checkpoints_dir}") - return - - print( - f"[eval_batch] Found {len(ckpts)} checkpoints: " - f"{os.path.basename(ckpts[0])} → {os.path.basename(ckpts[-1])}" - ) - - # Configure env - agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) - env_cfg.scene.num_envs = args_cli.num_envs - env_cfg.motion_file = args_cli.motion_file - if env_cfg.motion_file is not None: - scene_cfg = SceneConfig.from_motion_file(env_cfg.motion_file) - apply_scene_config(env_cfg, scene_cfg) - autoframe_viewer(env_cfg, scene_cfg.motion_file) - env_cfg.seed = agent_cfg.seed - env_cfg.sim.device = args_cli.device if args_cli.device else env_cfg.sim.device - - env = gym.make(args_cli.task, cfg=env_cfg, render_mode=None) - env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) - - # Build runner (loads first checkpoint to initialise architecture) - agent_cfg.load_checkpoint = ckpts[0] - if agent_cfg.class_name == "OnPolicyRunner": - runner = OnPolicyRunner( - env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device - ) - else: - runner = DistillationRunner( - env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device - ) - - _cmd = env.unwrapped.command_manager.get_term("dual_hands_object_tracking_command") - _warmup = getattr(_cmd.cfg, "virtual_object_control_decay_steps", 20) - traj_len = _cmd.retargeted_horizon - num_envs = env.unwrapped.num_envs - - results = [] # list of (iter, mean_ratio, std_ratio, n_full, n_total) - - for ckpt_path in ckpts: - iteration = _itr(ckpt_path) - runner.load(ckpt_path) - policy = runner.get_inference_policy(device=env.unwrapped.device) - try: - policy_nn = runner.alg.policy - except AttributeError: - policy_nn = runner.alg.actor_critic - - # Reset tracking state - obs = env.get_observations() - my_ep_len = torch.zeros( - num_envs, dtype=torch.float32, device=env.unwrapped.device - ) - my_ep_tracking_len = _cmd.tracking_lengths.clone().float().squeeze(-1) - completed: list[tuple[float, float]] = [] - - while len(completed) < args_cli.eval_episodes: - with torch.inference_mode(): - actions = policy(obs) - obs, _, dones, _ = env.step(actions) - policy_nn.reset(dones) - my_ep_len += 1 - done_mask = dones.bool() - if done_mask.any(): - done_ids = done_mask.nonzero(as_tuple=False).squeeze(-1) - for i in done_ids: - ep_len = my_ep_len[i].item() - track_len = my_ep_tracking_len[i].item() - completed.append((ep_len, track_len)) - my_ep_len[i] = 0.0 - my_ep_tracking_len[i] = _cmd.tracking_lengths[i].float() - - data = completed[: args_cli.eval_episodes] - _usable = max(traj_len - _warmup, 1) - ratios = [min(max(e[0] - _warmup, 0), _usable) / _usable for e in data] - n_full = sum(1 for r in ratios if r >= 0.99) - mean_r = sum(ratios) / len(ratios) - std_r = ( - sum((x - mean_r) ** 2 for x in ratios) / max(len(ratios) - 1, 1) - ) ** 0.5 - results.append((iteration, mean_r, std_r, n_full, len(data))) - print( - f" step={iteration:6d} ratio={mean_r:.3f}±{std_r:.3f} " - f"full={n_full}/{len(data)} ({100*n_full/len(data):.0f}%)", - flush=True, - ) - - print("\n[eval_batch] ===== Full Table =====") - print( - f" Trajectory: {traj_len} steps, warmup: {_warmup}, episodes/ckpt: {args_cli.eval_episodes}" - ) - print(f" {'Step':>7} {'Ratio mean':>11} {'Ratio std':>10} {'Full%':>6}") - print(f" {'-'*7} {'-'*11} {'-'*10} {'-'*6}") - for itr, mr, sr, nf, nt in results: - print(f" {itr:>7d} {mr:>11.3f} {sr:>10.3f} {100*nf/nt:>5.1f}%") - - env.close() - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/eval_callback.py b/robotic_grounding/scripts/rsl_rl/eval_callback.py deleted file mode 100644 index a0e362ee..00000000 --- a/robotic_grounding/scripts/rsl_rl/eval_callback.py +++ /dev/null @@ -1,325 +0,0 @@ -"""Eval callback utility for RSL-RL training.""" - -import os - - -class EvalCallback: - """Runs inference episodes after each checkpoint save and logs completion stats to wandb. - - Two eval passes are run per checkpoint: - - Pass A (from_start): every reset goes to tc=0; measures whether the policy - can complete the full trajectory from scratch. - Logs: eval/completion_ratio_mean, eval/completion_ratio_std, - eval/full_completion_pct - - Pass B (random): resets use training behaviour (random tc); measures what - fraction of the trajectory the policy has learned to follow regardless - of starting position. - Logs: eval/completion_ratio_mean_random, eval/completion_ratio_std_random - - Both passes run a warm-up phase that waits until every env has reset at least - once, then collect `eval_episodes` completed episodes in inference mode. - - If the training env is wrapped with RecordVideo (i.e. --video is set), - a video is captured during pass A only (genuine from-frame-0 rollout). - """ - - def __init__(self, runner, env, eval_episodes: int, log_video: bool) -> None: - import re as _re - - self._re = _re - self.runner = runner - self.env = env # RslRlVecEnvWrapper - self.eval_episodes = eval_episodes - self.log_video = log_video - - isaac_env = env.unwrapped - self._cmd = isaac_env.command_manager.get_term( - "dual_hands_object_tracking_command" - ) - self._warmup_steps = getattr( - self._cmd.cfg, "virtual_object_control_decay_steps", 20 - ) - self._traj_len = self._cmd.retargeted_horizon - self._isaac_env = isaac_env - - # Find the RecordVideo wrapper so eval videos can be redirected to videos/eval - self._record_video_env = None - self._eval_video_folder = None - self._train_video_folder = None - self._logged_eval_videos: set = set() - if log_video: - w = getattr(env, "env", None) - while w is not None: - if hasattr(w, "video_folder") and hasattr(w, "start_recording"): - self._record_video_env = w - self._train_video_folder = w.video_folder - self._eval_video_folder = os.path.join( - os.path.dirname(w.video_folder), "eval" - ) - os.makedirs(self._eval_video_folder, exist_ok=True) - break - w = getattr(w, "env", None) - - def _collect_episodes( - self, - policy, - policy_nn, - obs, - from_start: bool, - log_video: bool, - ): - """Run one eval pass; returns (completed_list, final_obs). - - Pass A (from_start=True): always_reset_to_first_frame=True, logs eval video. - Pass B (from_start=False): training reset behaviour (random tc), no video. - The caller's finally block is responsible for restoring the original value. - """ - import torch - - device = self.env.unwrapped.device - num_envs = self.env.unwrapped.num_envs - - self._cmd.cfg.always_reset_to_first_frame = from_start - - my_ep_len = torch.zeros(num_envs, dtype=torch.float32, device=device) - my_ep_max_len = torch.zeros(num_envs, dtype=torch.float32, device=device) - env_first_done = torch.zeros(num_envs, dtype=torch.bool, device=device) - completed: list[tuple[float, float]] = [] - - # Warmup: step until every env has completed at least one episode so all - # collected episodes start from a genuine post-reset state. - env_reset_once = torch.zeros(num_envs, dtype=torch.bool, device=device) - while not env_reset_once.all(): - with torch.inference_mode(): - actions = policy(obs) - obs, _, dones, _ = self.env.step(actions) - policy_nn.reset(dones) - dones = ( - dones.clone() - ) # detach inference tensor before use outside inference_mode - env_reset_once |= dones.bool() - - if log_video and self._record_video_env is not None: - self._record_video_env.video_folder = self._eval_video_folder - - _video_triggered = False - while len(completed) < self.eval_episodes: - with torch.inference_mode(): - actions = policy(obs) - obs, _, dones, _ = self.env.step(actions) - policy_nn.reset(dones) - dones = ( - dones.clone() - ) # detach inference tensor before use outside inference_mode - - my_ep_len += 1 - done_mask = dones.bool() - if done_mask.any(): - done_ids = done_mask.nonzero(as_tuple=False).squeeze(-1) - for i in done_ids: - if env_first_done[i]: - completed.append((my_ep_len[i].item(), my_ep_max_len[i].item())) - else: - # Tail episode pre-dating warmup end; discard, record next. - env_first_done[i] = True - if log_video and not _video_triggered: - self._isaac_env.eval_video_trigger_pending = True - _video_triggered = True - my_ep_len[i] = 0.0 - my_ep_max_len[i] = self._cmd.tracking_lengths[i].float() - - # Drain: keep stepping until the in-progress recording finishes. - if log_video and self._record_video_env is not None: - _drain_limit = ( - self._record_video_env.video_length - if self._record_video_env.video_length != float("inf") - else 600 - ) * 3 - _drain_steps = 0 - while self._record_video_env.recording: - if _drain_steps >= _drain_limit: - print( - f"[eval] WARNING: drain loop hit {_drain_limit}-step limit " - "while waiting for eval recording to finish; aborting drain." - ) - break - with torch.inference_mode(): - actions = policy(obs) - obs, _, dones, _ = self.env.step(actions) - policy_nn.reset(dones) - _drain_steps += 1 - - return completed, obs - - def _compute_stats( - self, completed: list[tuple[float, float]] - ) -> tuple[float, float, float, int]: - """Return (mean_ratio, std_ratio, full_pct, n_full) from a completed-episodes list.""" - data = completed[: self.eval_episodes] - ratios = [ - min(max(e[0] - self._warmup_steps, 0), max(e[1] - self._warmup_steps, 1)) - / max(e[1] - self._warmup_steps, 1) - for e in data - ] - n_full = sum(1 for r in ratios if r >= 0.99) - mean_r = sum(ratios) / len(ratios) - std_r = ( - sum((x - mean_r) ** 2 for x in ratios) / max(len(ratios) - 1, 1) - ) ** 0.5 - full_pct = 100.0 * n_full / len(data) - return mean_r, std_r, full_pct, n_full - - def __call__(self, path: str, *args, **kwargs) -> None: - import copy as _copy - - import wandb - - if wandb.run is None: - return - - match = self._re.search(r"model_(\d+)\.pt", os.path.basename(path)) - iteration = int(match.group(1)) if match else None - - device = self.env.unwrapped.device - - policy = self.runner.get_inference_policy(device=device) - try: - policy_nn = self.runner.alg.policy - except AttributeError: - policy_nn = self.runner.alg.actor_critic - - obs = self.env.get_observations() - traj_len = self._cmd.retargeted_horizon - - # ── Save training state that eval steps would contaminate ──────────── # - _saved_episode_sums = { - k: v.clone() - for k, v in self._isaac_env.reward_manager._episode_sums.items() - } - _saved_metrics = {k: v.clone() for k, v in self._cmd.metrics.items()} - _saved_cws_buf = ( - _copy.copy(self._cmd._cws_reward_step_buf) - if hasattr(self._cmd, "_cws_reward_step_buf") - else None - ) - _orig_reset_to_first = self._cmd.cfg.always_reset_to_first_frame - - completed_start: list[tuple[float, float]] = [] - completed_random: list[tuple[float, float]] = [] - - try: - # Pass A: from-start (tc=0) — also captures eval video. - completed_start, obs = self._collect_episodes( - policy, policy_nn, obs, from_start=True, log_video=self.log_video - ) - # Pass B: random reset — measures coverage regardless of start position. - completed_random, obs = self._collect_episodes( - policy, policy_nn, obs, from_start=False, log_video=False - ) - - finally: - self._cmd.cfg.always_reset_to_first_frame = _orig_reset_to_first - - # ── Restore training state saved before eval ───────────────────── # - # Use assignment rather than copy_() because env.step() inside - # inference_mode may have replaced dict entries with inference tensors; - # inplace copy_ on those outside inference_mode raises an error. - for k, v in _saved_episode_sums.items(): - if k in self._isaac_env.reward_manager._episode_sums: - self._isaac_env.reward_manager._episode_sums[k] = v.clone() - for k, v in _saved_metrics.items(): - if k in self._cmd.metrics: - self._cmd.metrics[k] = v.clone() - if _saved_cws_buf is not None and hasattr( - self._cmd, "_cws_reward_step_buf" - ): - self._cmd._cws_reward_step_buf.clear() - self._cmd._cws_reward_step_buf.extend(_saved_cws_buf) - - try: - self.runner.alg.train_mode() - except AttributeError: - pass - - self._isaac_env.extras["log"] = dict() - - # Signal curriculum that eval has finished so deferred decay can fire. - self._isaac_env.pre_decay_eval_pending = False - - # Always restore the train video folder and clear any unconsumed pending - # trigger, regardless of whether an exception occurred above. - if self.log_video: - self._isaac_env.video_trigger_pending = False - self._isaac_env.eval_video_trigger_pending = False - if self._record_video_env is not None: - self._record_video_env.video_folder = self._train_video_folder - - # Log eval video (pass A only). - if self.log_video and self._eval_video_folder: - import glob as _glob - - new_videos = sorted( - [ - f - for f in _glob.glob(os.path.join(self._eval_video_folder, "*.mp4")) - if f not in self._logged_eval_videos - ], - key=os.path.getmtime, - ) - if new_videos: - src_path = new_videos[-1] - model_step = iteration if iteration is not None else 0 - dst_name = f"video_eval_{model_step}.mp4" - dst_path = os.path.join(self._eval_video_folder, dst_name) - if src_path != dst_path: - os.rename(src_path, dst_path) - log_video_data = {"eval/video": wandb.Video(dst_path, format="mp4")} - if iteration is not None: - wandb.log(log_video_data, step=iteration, commit=False) - else: - wandb.log(log_video_data, commit=False) - self._logged_eval_videos.update(new_videos) - self._logged_eval_videos.add(dst_path) - - # ── Stats and logging: pass A (from-start) ────────────────────────── # - if completed_start: - mean_r, std_r, full_pct, n_full = self._compute_stats(completed_start) - n = len(completed_start[: self.eval_episodes]) - print( - f"[eval/from_start] iter={iteration} traj_len={traj_len} " - f"warmup={self._warmup_steps} " - f"sample ep_lens={[round(e[0]) for e in completed_start[:5]]}\n" - f" ratio={mean_r:.3f}±{std_r:.3f} full={n_full}/{n} ({full_pct:.0f}%)" - ) - log_data: dict = { - "eval/completion_ratio_mean": mean_r, - "eval/completion_ratio_std": std_r, - "eval/full_completion_pct": full_pct, - } - if iteration is not None: - wandb.log(log_data, step=iteration, commit=False) - else: - wandb.log(log_data, commit=False) - - # ── Stats and logging: pass B (random reset) ──────────────────────── # - if completed_random: - mean_r_rnd, std_r_rnd, _, _ = self._compute_stats(completed_random) - print( - f"[eval/random] iter={iteration} " - f"sample ep_lens={[round(e[0]) for e in completed_random[:5]]}\n" - f" ratio={mean_r_rnd:.3f}±{std_r_rnd:.3f}" - ) - log_data_rnd: dict = { - "eval/completion_ratio_mean_random": mean_r_rnd, - "eval/completion_ratio_std_random": std_r_rnd, - } - if iteration is not None: - wandb.log(log_data_rnd, step=iteration, commit=False) - else: - wandb.log(log_data_rnd, commit=False) - - # Refresh runner's internal obs so the next collect_rollouts starts consistently. - if hasattr(self.runner, "obs"): - self.runner.obs = obs diff --git a/robotic_grounding/scripts/rsl_rl/misc/run_sonic_policy.py b/robotic_grounding/scripts/rsl_rl/misc/run_sonic_policy.py deleted file mode 100755 index 625de9a1..00000000 --- a/robotic_grounding/scripts/rsl_rl/misc/run_sonic_policy.py +++ /dev/null @@ -1,572 +0,0 @@ -from __future__ import annotations - -import argparse -import os -from typing import Any - -import gymnasium as gym -import torch -from isaaclab.app import AppLauncher - -# Parse arguments -parser = argparse.ArgumentParser( - description="Run SONIC policy with pretrained encoder/decoder." -) -parser.add_argument("--num_envs", type=int, default=1, help="Number of environments.") -parser.add_argument( - "--use_tracking", - action="store_true", - default=False, - help="Use tracking command for encoder input (vs dummy zeros).", -) -parser.add_argument( - "--use_hierarchical_action", - action="store_true", - default=False, - help="Test hierarchical action term (reads from command term).", -) -parser.add_argument( - "--use_residual_action", - action="store_true", - default=False, - help="Test residual action term (zero residuals).", -) -parser.add_argument( - "--use_latent_residual_action", - action="store_true", - default=False, - help="Test latent residual action term (zero latent residuals).", -) -parser.add_argument( - "--use_latent_hand_policy_action", - action="store_true", - default=False, - help="Test latent hand policy action term (dummy actions, overriden by reference motion).", -) -parser.add_argument( - "--disable_terminations", - action="store_true", - default=False, - help="Disable all termination conditions.", -) -parser.add_argument("--video", action="store_true", default=False, help="Record video.") -parser.add_argument( - "--video_length", type=int, default=350, help="Video length in steps." -) -parser.add_argument( - "--scene_config", - type=str, - default=None, - help="Path to scene configuration YAML file.", -) -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -# Enable cameras if recording video -if args_cli.video: - args_cli.enable_cameras = True - -# Launch app -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -# Imports after app launch (required by Isaac Lab) -from isaaclab.envs import ManagerBasedRLEnv # noqa: E402, PLC0415 -from robotic_grounding.assets import ( # noqa: E402, PLC0415 - MOTION_ASSET_DIR, - POLICY_ASSET_DIR, -) -from robotic_grounding.assets.g1 import ( # noqa: E402, PLC0415 - G1_CYLINDER_MODEL_12_DEX_DELAYED_CFG, - G1_HAND_JOINT_NAMES, - G1_MODEL_12_ACTION_SCALE, -) -from robotic_grounding.assets.policies.grasp import ( # noqa: E402, PLC0415 - G1GraspPolicy, - GraspPolicyCfg, -) -from robotic_grounding.tasks.v2p_whole_body import ( # noqa: E402, PLC0415 - G1_SONIC_JOINT_NAMES, - G1SonicEEEnvCfg, - G1SonicEnvCfg, -) -from robotic_grounding.tasks.v2p_whole_body.mdp.actions import ( # noqa: E402, PLC0415 - SONICActionCfg, - SONICActionType, - SonicPolicy, -) - - -def dummy_encoder_input(num_envs: int, device: str = "cpu") -> torch.Tensor: - """Create dummy encoder inputs for G1 mode (all zeros except mode indicator). - - For testing without tracking command. - """ - encoder_input = torch.zeros(num_envs, 1772, device=device) - encoder_input[:, 0] = 0.0 # G1 mode = 0 (scalar index, not one-hot) - return encoder_input - - -def run_policy( - env: Any, - policy: SonicPolicy, - num_steps: int = 350, - use_tracking: bool = False, -) -> None: - """Run the SONIC policy for a number of steps. - - Args: - env: Environment - policy: SonicPolicy instance - num_steps: Number of steps to run - use_tracking: If True, use tracking command observations. If False, use dummy zeros. - """ - print(f"\nRunning policy for {num_steps} steps...") - print( - f"Encoder mode: {'Tracking Command' if use_tracking else 'Dummy (all zeros)'}" - ) - - # Get unwrapped env for device access - unwrapped_env = env.unwrapped if hasattr(env, "unwrapped") else env - device = unwrapped_env.device - - # Reset environment - obs_dict, _ = env.reset() - - # Replace tokenizer observations with dummy if not using tracking - if not use_tracking: - obs_dict["sonic_tokenizer"] = dummy_encoder_input( - unwrapped_env.num_envs, device=device - ) - print("Using dummy encoder input (all zeros)") - else: - print("Using tracking observations from tokenizer group") - - print(f"Encoder obs shape: {obs_dict['sonic_tokenizer'].shape}") - print(f"Decoder obs shape: {obs_dict['sonic_policy'].shape}") - - # Run for specified steps - for step in range(num_steps): - # Run policy (encoder + decoder) - with torch.inference_mode(): - actions = policy(obs_dict) - - # Step environment - with torch.inference_mode(): - obs_dict, rewards, terminated, truncated, info = env.step(actions) - - if not use_tracking: - obs_dict["sonic_tokenizer"] = dummy_encoder_input( - unwrapped_env.num_envs, device=device - ) - - # Print progress - if (step + 1) % 50 == 0: - print(f"Step {step + 1}/{num_steps}") - - print(f"\nCompleted {num_steps} steps!") - - # Print metrics - with torch.inference_mode(): - info = unwrapped_env.command_manager.reset( - torch.arange(unwrapped_env.num_envs, device=device) - ) - print("Metrics:") - for key, value in info.items(): - print(f" {key}: {value}") - - -def run_hierarchical_action_test(env: Any, num_steps: int = 350) -> None: - """Test hierarchical action term by reading actions directly from command term. - - This is a pass-through test where actions are read from the tracking command - and should result in perfect tracking (no change from reference motion). - - Args: - env: Environment with hierarchical action term configured - num_steps: Number of steps to run - """ - print(f"\nRunning hierarchical action test for {num_steps} steps...") - print("Actions will be read directly from command term (pass-through test)") - - # Get unwrapped env for device access - unwrapped_env = env.unwrapped if hasattr(env, "unwrapped") else env - device = unwrapped_env.device - - # Get command term for reading actions - command = unwrapped_env.command_manager.get_term("motion") - - # Reset environment - obs_dict, _ = env.reset() - - print(f"Action space: {env.action_space}") - print(f"Action dim: {env.action_space.shape[-1]}") - - # Run for specified steps - for step in range(num_steps): - # Build action from command term - # Action structure: [joint_commands (N), base_ori_6d (6)] - - # Get current joint positions from command (first future frame) - joint_pos_multi_future = ( - command.command_joint_pos_multi_future - ) # (num_envs, num_future_frames, num_joints) - joint_commands = joint_pos_multi_future[ - :, 0, : - ] # (num_envs, num_joints) - absolute positions - - # Get current orientation from command (first future frame) - # Convert from command's orientation representation to 6D - ori_multi_future = ( - command.command_root_rot_dif_l_multi_future - ) # (num_envs, num_future_frames, 6) - base_ori_6d = ori_multi_future[:, 0, :] # (num_envs, 6) - - # Concatenate to form action - actions = torch.cat( - [joint_commands, base_ori_6d], dim=-1 - ) # (num_envs, num_joints + 6) - - # Step environment - with torch.inference_mode(): - obs_dict, rewards, terminated, truncated, info = env.step(actions) - - # Print progress - if (step + 1) % 50 == 0: - print(f"Step {step + 1}/{num_steps}") - - print(f"\nCompleted {num_steps} steps!") - - # Print metrics - with torch.inference_mode(): - info = unwrapped_env.command_manager.reset( - torch.arange(unwrapped_env.num_envs, device=device) - ) - print("Metrics:") - for key, value in info.items(): - print(f" {key}: {value}") - - -def run_residual_action_test(env: Any, num_steps: int = 350) -> None: - """Test residual action term with zero residuals (pass-through test). - - This is a pass-through test where zero residuals are applied to all joint positions, - which should result in base commanded positions being used. - - Args: - env: Environment with residual action term configured - num_steps: Number of steps to run - """ - print(f"\nRunning residual action test for {num_steps} steps...") - print("All residuals set to zero (pass-through test - base commanded positions)") - - # Get unwrapped env for device access - unwrapped_env = env.unwrapped if hasattr(env, "unwrapped") else env - device = unwrapped_env.device - - # Get action term to determine number of joints - action_term = unwrapped_env.action_manager.get_term("joint_pos") - num_joints = action_term._num_joints - - # Reset environment - obs_dict, _ = env.reset() - - print(f"Action space: {env.action_space}") - print(f"Action dim (all joints): {env.action_space.shape[-1]}") - print(f"Number of joints: {num_joints}") - - # Run for specified steps with zero residuals - for step in range(num_steps): - # Action structure: [joint_residuals (num_joints)] - # All zeros = no residual = base positions from command - actions = torch.zeros(unwrapped_env.num_envs, num_joints, device=device) - - # Step environment - with torch.inference_mode(): - obs_dict, rewards, terminated, truncated, info = env.step(actions) - - # Print progress - if (step + 1) % 50 == 0: - print(f"Step {step + 1}/{num_steps}") - - print(f"\nCompleted {num_steps} steps!") - - # Print metrics - with torch.inference_mode(): - info = unwrapped_env.command_manager.reset( - torch.arange(unwrapped_env.num_envs, device=device) - ) - print("Metrics:") - for key, value in info.items(): - print(f" {key}: {value}") - - -def run_latent_residual_action_test(env: Any, num_steps: int = 350) -> None: - """Test latent residual action term with zero latent residuals (pass-through test). - - This is a pass-through test where zero latent residuals are applied, - which should result in normal SONIC policy output (no modification to token state). - - Args: - env: Environment with latent residual action term configured - num_steps: Number of steps to run - """ - print(f"\nRunning latent residual action test for {num_steps} steps...") - print("All latent residuals set to zero (pass-through test - normal SONIC output)") - - # Get unwrapped env for device access - unwrapped_env = env.unwrapped if hasattr(env, "unwrapped") else env - device = unwrapped_env.device - - # Get action term to determine latent dimension - action_term = unwrapped_env.action_manager.get_term("joint_pos") - latent_dim = action_term.policy.encoder_output_dim - - # Reset environment - obs_dict, _ = env.reset() - - print(f"Action space: {env.action_space}") - print(f"Action dim (latent space): {env.action_space.shape[-1]}") - print(f"Encoder output dimension: {latent_dim}") - - # Run for specified steps with zero latent residuals - for step in range(num_steps): - # Action structure: [latent_residuals (encoder_output_dim)] - # All zeros = no latent modification = normal SONIC output - actions = torch.zeros(unwrapped_env.num_envs, latent_dim, device=device) - - # Step environment - with torch.inference_mode(): - obs_dict, rewards, terminated, truncated, info = env.step(actions) - - # Print progress - if (step + 1) % 50 == 0: - print(f"Step {step + 1}/{num_steps}") - - print(f"\nCompleted {num_steps} steps!") - - # Print metrics - with torch.inference_mode(): - info = unwrapped_env.command_manager.reset( - torch.arange(unwrapped_env.num_envs, device=device) - ) - print("Metrics:") - for key, value in info.items(): - print(f" {key}: {value}") - - -def run_latent_hand_policy_action_test(env: Any, num_steps: int = 350) -> None: - """Test latent hand policy action term with dummy actions, overriden by reference motion. - - Args: - env: Environment with latent hand policy action term configured - num_steps: Number of steps to run - """ - print(f"\nRunning latent hand policy action test for {num_steps} steps...") - print("Dummy actions, overriden by reference motion") - - # Get unwrapped env for device access - unwrapped_env = env.unwrapped if hasattr(env, "unwrapped") else env - device = unwrapped_env.device - - # Get action term to determine latent dimension - action_term = unwrapped_env.action_manager.get_term("joint_pos") - latent_dim = action_term.policy.encoder_output_dim - - # Reset environment - obs_dict, _ = env.reset() - - print(f"Action space: {env.action_space}") - print(f"Action dim (latent space): {env.action_space.shape[-1]}") - print(f"Encoder output dimension: {latent_dim}") - - # Run for specified steps - for step in range(num_steps): - # Action structure: [latent_state (encoder_output_dim)] - # All zeros = dummy actions - actions = torch.zeros(unwrapped_env.num_envs, latent_dim, device=device) - - # Step environment - with torch.inference_mode(): - obs_dict, rewards, terminated, truncated, info = env.step(actions) - - # Print progress - if (step + 1) % 50 == 0: - print(f"Step {step + 1}/{num_steps}") - - print(f"\nCompleted {num_steps} steps!") - - # Print metrics - with torch.inference_mode(): - info = unwrapped_env.command_manager.reset( - torch.arange(unwrapped_env.num_envs, device=device) - ) - print("Metrics:") - for key, value in info.items(): - print(f" {key}: {value}") - - -def main() -> None: - """Main function to run SONIC policy.""" - # Configuration - POLICY_DIR = f"{POLICY_ASSET_DIR}/sonic" - MOTION_FILE = f"{MOTION_ASSET_DIR}/apple_pick_and_place_retarget_motion_w_body.h5" - - print("=" * 60) - print("SONIC Policy Inference") - print("=" * 60) - print(f"Hierarchical action: {args_cli.use_hierarchical_action}") - print(f"Tracking mode: {args_cli.use_tracking}") - print( - f"Motion file: {MOTION_FILE if args_cli.use_tracking else 'N/A (using dummy input)'}" - ) - print(f"Num environments: {args_cli.num_envs}") - print(f"Terminations disabled: {args_cli.disable_terminations}") - print(f"Video recording: {args_cli.video}") - print("=" * 60) - - # Create environment - print("\nCreating environment...") - cfg = ( - G1SonicEEEnvCfg() if args_cli.use_latent_hand_policy_action else G1SonicEnvCfg() - ) - cfg.scene.num_envs = args_cli.num_envs - - # Override scene config if provided - if args_cli.scene_config is not None: - from robotic_grounding.tasks.scene_utils import ( # noqa: PLC0415 - SceneConfig, - apply_scene_config, - ) - - cfg.scene_config_path = args_cli.scene_config - scene_config = SceneConfig.from_yaml(args_cli.scene_config) - apply_scene_config(cfg, scene_config) - MOTION_FILE = scene_config.motion_file - - # Set motion file for tracking command (required) - cfg.commands.motion.motion_file = MOTION_FILE - - # Configure hierarchical action if requested - if args_cli.use_hierarchical_action: - print("\nUsing Hierarchical Action Term") - original_scale = cfg.actions.joint_pos.scale - cfg.actions.joint_pos = SONICActionCfg( - action_type=SONICActionType.HIERARCHICAL, - policy_dir=POLICY_DIR, - asset_name="robot", - joint_names=[".*"], # All joints (including hands) - sonic_joint_names=G1_SONIC_JOINT_NAMES, # SONIC controls only non-hand joints - command_name="motion", - use_default_offset=True, - scale=original_scale, - ) - elif args_cli.use_residual_action: - print("\nUsing Residual Action Term") - original_scale = cfg.actions.joint_pos.scale - cfg.actions.joint_pos = SONICActionCfg( - action_type=SONICActionType.RESIDUAL, - policy_dir=POLICY_DIR, - asset_name="robot", - joint_names=[".*"], # All joints (including hands) - sonic_joint_names=G1_SONIC_JOINT_NAMES, # SONIC controls only non-hand joints - command_name="motion", - use_default_offset=True, - scale=original_scale, - ) - elif args_cli.use_latent_residual_action: - print("\nUsing Latent Residual Action Term") - original_scale = cfg.actions.joint_pos.scale - cfg.actions.joint_pos = SONICActionCfg( - action_type=SONICActionType.LATENT_RESIDUAL, - policy_dir=POLICY_DIR, - asset_name="robot", - joint_names=[".*"], # All joints (including hands) - sonic_joint_names=G1_SONIC_JOINT_NAMES, # SONIC controls only non-hand joints - command_name="motion", - use_default_offset=True, - scale=original_scale, - ) - elif args_cli.use_latent_hand_policy_action: - print("\nUsing Latent Hand Policy Action Term") - original_scale = cfg.actions.joint_pos.scale - cfg.actions.joint_pos = SONICActionCfg( - action_type=SONICActionType.LATENT_HAND_POLICY, - policy_dir=POLICY_DIR, - asset_name="robot", - joint_names=[".*"], - sonic_joint_names=G1_SONIC_JOINT_NAMES, - command_name="motion", - use_default_offset=True, - scale=original_scale, - debug=True, - ) - cfg.actions.joint_pos.hand_policy_class = G1GraspPolicy - cfg.actions.joint_pos.hand_policy_cfg = GraspPolicyCfg( - asset_name="robot", - joint_names=G1_HAND_JOINT_NAMES, - ) - else: - # When not using hierarchical action, use non-hands robot and disable filtering - print("\nUsing Direct SONIC Policy Inference") - - # Switch to non-hands robot configuration - cfg.scene.robot = G1_CYLINDER_MODEL_12_DEX_DELAYED_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot" - ) - cfg.actions.joint_pos.scale = G1_MODEL_12_ACTION_SCALE - - # Disable terminations if requested - if args_cli.disable_terminations: - print("\nDisabling all termination conditions...") - cfg.terminations = None - - # Create environment with render mode for video - render_mode = "rgb_array" if args_cli.video else None - env = ManagerBasedRLEnv(cfg=cfg, render_mode=render_mode) - - print(f"Environment created with {env.num_envs} environments") - print(f"Action space: {env.action_space}") - print(f"Observation space: {env.observation_space}") - - # Record video - if args_cli.video: - video_dir = os.path.abspath(os.path.join("videos", "sonic_policy")) - video_kwargs = { - "video_folder": video_dir, - "step_trigger": lambda step: step == 0, - "video_length": args_cli.video_length, - "disable_logger": True, - } - print(f"\n[INFO] Recording video to: {video_dir}") - env = gym.wrappers.RecordVideo(env, **video_kwargs) - - # Run appropriate test - if args_cli.use_hierarchical_action: - run_hierarchical_action_test(env, num_steps=args_cli.video_length) - elif args_cli.use_residual_action: - run_residual_action_test(env, num_steps=args_cli.video_length) - elif args_cli.use_latent_residual_action: - run_latent_residual_action_test(env, num_steps=args_cli.video_length) - elif args_cli.use_latent_hand_policy_action: - run_latent_hand_policy_action_test(env, num_steps=args_cli.video_length) - else: - # Load SONIC policy for direct inference - print("\nLoading SONIC policy...") - policy = SonicPolicy(POLICY_DIR) - run_policy( - env, - policy, - num_steps=args_cli.video_length, - use_tracking=args_cli.use_tracking, - ) - - # Cleanup - env.close() - - print("\nDone!") - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/play.py b/robotic_grounding/scripts/rsl_rl/play.py deleted file mode 100644 index 7ee743a4..00000000 --- a/robotic_grounding/scripts/rsl_rl/play.py +++ /dev/null @@ -1,214 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - -"""Play an environment without loading a policy checkpoint. - -Supports Hydra overrides for env_cfg fields, e.g.: - python scripts/rsl_rl/play.py --task Sharpa-V2P-v0-Play \ - env_cfg.motion_file=arctic_processed/arctic_s01_ketchup_use_01/sharpa_wave - -Usage: - # Play mode with sinusoidal actions - python scripts/rsl_rl/play.py --task Sharpa-V2P-v0-Play --num_envs 4 - - # View mode: zero VOC, zero actions - python scripts/rsl_rl/play.py --task Sharpa-V2P-v0-Play --num_envs 1 --view - - # Override motion file - python scripts/rsl_rl/play.py --task Sharpa-V2P-v0-Play \ - env_cfg.motion_file=arctic_processed/arctic_s01_ketchup_use_01/sharpa_wave -""" - -import argparse -import os -import sys -import time - -import numpy as np -import torch - -from isaaclab.app import AppLauncher - -parser = argparse.ArgumentParser( - description="Play an IsaacLab environment without a policy checkpoint." -) -parser.add_argument("--task", type=str, required=True, help="Gym task ID to load.") -parser.add_argument( - "--num_envs", type=int, default=None, help="Number of environments." -) -parser.add_argument( - "--video", action="store_true", default=False, help="Record a video." -) -parser.add_argument( - "--video_length", type=int, default=400, help="Video length (steps)." -) -parser.add_argument( - "--disable_fabric", action="store_true", default=False, help="Disable Fabric." -) -parser.add_argument( - "--real-time", action="store_true", default=False, help="Run close to real-time." -) -parser.add_argument( - "--zero-actions", - action="store_true", - default=False, - help="Use zero actions instead of sinusoidal (useful for GUI-controlled envs).", -) -parser.add_argument( - "--view", - action="store_true", - default=False, - help="View mode: zero VOC, zero actions.", -) -parser.add_argument( - "--use_primitive_urdfs", - action="store_true", - default=False, - help="Use primitive URDFs for the robot.", -) - -AppLauncher.add_app_launcher_args(parser) -args_cli, hydra_args = parser.parse_known_args() - -if args_cli.video: - args_cli.enable_cameras = True - -# Clear sys.argv for Hydra -sys.argv = [sys.argv[0]] + hydra_args - -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -import gymnasium as gym # noqa: E402 - -from isaaclab.envs import ManagerBasedRLEnv, ManagerBasedRLEnvCfg # noqa: E402 -from isaaclab.utils.dict import print_dict # noqa: E402 -from isaaclab_tasks.utils.hydra import hydra_task_config # noqa: E402 - -import robotic_grounding.tasks # noqa: F401, E402 -from robotic_grounding.tasks.scene_utils import ( - SceneConfig, - apply_scene_config, -) # noqa: E402 - - -def prepare_env_for_playing( - env_cfg: ManagerBasedRLEnvCfg, view: bool = False -) -> ManagerBasedRLEnvCfg: - """Prepare environment for interactive playing.""" - if hasattr(env_cfg, "curriculum") and env_cfg.curriculum is not None: - env_cfg.curriculum = None - - if view: - if hasattr(env_cfg, "actions") and hasattr( - env_cfg.actions, "virtual_rigid_object_control" - ): - env_cfg.actions.virtual_rigid_object_control = None - env_cfg.terminations = None - - return env_cfg - - -def generate_sinusoidal_actions( - timestep: int, num_envs: int, action_dim: int, dt: float -) -> np.ndarray: - time_elapsed = timestep * dt - actions = np.zeros((num_envs, action_dim), dtype=np.float32) - for i in range(action_dim): - frequency = 0.3 + (i % 5) * 0.1 - phase_offset = i * np.pi / action_dim - actions[:, i] = 0.5 * np.sin( - 2 * np.pi * frequency * time_elapsed + phase_offset - ) - return actions - - -@hydra_task_config(args_cli.task, "rsl_rl_cfg_entry_point") -def main(env_cfg: ManagerBasedRLEnvCfg, agent_cfg=None): - is_debug_env = "Debug" in args_cli.task - - # Apply scene config from motion_file (after Hydra overrides are merged) - if hasattr(env_cfg, "motion_file"): - scene_config = SceneConfig.from_motion_file(env_cfg.motion_file) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - - # Apply non-hydra CLI overrides - if args_cli.num_envs is not None: - env_cfg.scene.num_envs = args_cli.num_envs - env_cfg.sim.device = args_cli.device - env_cfg.sim.use_fabric = not args_cli.disable_fabric - - if hasattr(env_cfg, "eval"): - env_cfg.eval() - - if isinstance(env_cfg, ManagerBasedRLEnvCfg): - env_cfg = prepare_env_for_playing(env_cfg, view=args_cli.view) - - render_mode = "rgb_array" if args_cli.video else None - env = ManagerBasedRLEnv(env_cfg, render_mode=render_mode) - - if args_cli.video: - video_dir = os.path.abspath(os.path.join("logs", "videos", "play")) - video_kwargs = { - "video_folder": video_dir, - "step_trigger": lambda step: step == 0, - "video_length": args_cli.video_length, - "disable_logger": True, - } - print("[INFO] Recording video.") - print_dict(video_kwargs, nesting=4) - env = gym.wrappers.RecordVideo(env, **video_kwargs) - - obs, _ = env.reset() - - dt = env.unwrapped.step_dt - timestep = 0 - num_envs = env.unwrapped.num_envs - action_dim = env.unwrapped.action_manager.total_action_dim - - use_zero_actions = args_cli.zero_actions or is_debug_env or args_cli.view - - print(f"[INFO] Environment loaded: {args_cli.task}") - print(f"[INFO] Number of environments: {num_envs}") - print(f"[INFO] Action dimension: {action_dim}") - print(f"[INFO] Control timestep: {dt:.4f}s ({1.0 / dt:.1f} Hz)") - - while simulation_app.is_running(): - start = time.time() - - if use_zero_actions: - actions = torch.zeros( - num_envs, action_dim, dtype=torch.float32, device=env.unwrapped.device - ) - else: - actions_np = generate_sinusoidal_actions(timestep, num_envs, action_dim, dt) - actions = torch.as_tensor( - actions_np, dtype=torch.float32, device=env.unwrapped.device - ) - - with torch.inference_mode(): - obs, _, _, _, _ = env.step(actions) - - timestep += 1 - - if args_cli.video and timestep == args_cli.video_length: - break - - if args_cli.real_time: - sleep_time = dt - (time.time() - start) - if sleep_time > 0: - time.sleep(sleep_time) - - env.close() - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/train.py b/robotic_grounding/scripts/rsl_rl/train.py deleted file mode 100644 index 74c14244..00000000 --- a/robotic_grounding/scripts/rsl_rl/train.py +++ /dev/null @@ -1,490 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Script to train RL agent with RSL-RL.""" - -"""Launch Isaac Sim Simulator first.""" - -import argparse -import sys - -from isaaclab.app import AppLauncher - -# local imports -import cli_args # isort: skip - -# add argparse arguments -parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.") -parser.add_argument( - "--video", action="store_true", default=False, help="Record videos during training." -) -parser.add_argument( - "--video_length", - type=int, - default=200, - help="Length of the recorded video (in steps).", -) -parser.add_argument( - "--video_interval", - type=int, - default=2000, - help="Interval between video recordings (in steps).", -) -parser.add_argument( - "--eval_video_only", - action="store_true", - default=False, - help="Record video during eval only; suppress interval-based and curriculum-triggered training videos.", -) -parser.add_argument( - "--num_envs", type=int, default=None, help="Number of environments to simulate." -) -parser.add_argument("--task", type=str, default=None, help="Name of the task.") -parser.add_argument( - "--agent", - type=str, - default="rsl_rl_cfg_entry_point", - help="Name of the RL agent configuration entry point.", -) -parser.add_argument( - "--seed", type=int, default=None, help="Seed used for the environment" -) -parser.add_argument( - "--max_iterations", type=int, default=None, help="RL Policy training iterations." -) -parser.add_argument( - "--distributed", - action="store_true", - default=False, - help="Run training with multiple GPUs or nodes.", -) -parser.add_argument( - "--zero-actor", - action="store_true", - default=False, - help="Make the last layer of the actor network a zero layer.", -) -parser.add_argument( - "--eval_episodes_per_save", - type=int, - default=0, - help="Number of completed episodes to collect for eval after each checkpoint save (0 to disable).", -) -parser.add_argument( - "--set-std", - type=float, - default=None, - help="Std of the policy network regardless of the checkpoint.", -) -parser.add_argument( - "--export_io_descriptors", - action="store_true", - default=False, - help="Export IO descriptors.", -) -parser.add_argument( - "--ray-proc-id", - "-rid", - type=int, - default=None, - help="Automatically configured by Ray integration, otherwise None.", -) -parser.add_argument( - "--scene_config", - type=str, - default=None, - help="Path to the scene configuration file.", -) -parser.add_argument( - "--motion_file", - type=str, - default=None, - help="Motion file to load.", -) -parser.add_argument( - "--wandb_id", - type=str, - default=None, - help="Wandb run ID to resume from.", -) -parser.add_argument( - "--use_primitive_urdfs", - action="store_true", - default=False, - help="Use primitive URDFs for the robot.", -) -# append RSL-RL cli arguments -cli_args.add_rsl_rl_args(parser) -# append AppLauncher cli args -AppLauncher.add_app_launcher_args(parser) -args_cli, hydra_args = parser.parse_known_args() - -# always enable cameras to record video -if args_cli.video: - args_cli.enable_cameras = True - -# clear out sys.argv for Hydra -sys.argv = [sys.argv[0]] + hydra_args - -# launch omniverse app -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -"""Check for minimum supported RSL-RL version.""" - -import importlib.metadata as metadata -import platform -from packaging import version - -# check minimum supported rsl-rl version -RSL_RL_VERSION = "3.0.1" -installed_version = metadata.version("rsl-rl-lib") -if version.parse(installed_version) < version.parse(RSL_RL_VERSION): - if platform.system() == "Windows": - cmd = [ - r".\isaaclab.bat", - "-p", - "-m", - "pip", - "install", - f"rsl-rl-lib=={RSL_RL_VERSION}", - ] - else: - cmd = [ - "./isaaclab.sh", - "-p", - "-m", - "pip", - "install", - f"rsl-rl-lib=={RSL_RL_VERSION}", - ] - print( - f"Please install the correct version of RSL-RL.\nExisting version is: '{installed_version}'" - f" and required version is: '{RSL_RL_VERSION}'.\nTo install the correct version, run:" - f"\n\n\t{' '.join(cmd)}\n" - ) - exit(1) - -"""Rest everything follows.""" - -import gymnasium as gym -import logging -import os -import time -import torch -from datetime import datetime -from zoneinfo import ZoneInfo -from download_from_wandb import download_run - -from rsl_rl.runners import DistillationRunner, OnPolicyRunner - -from isaaclab.envs import ( - DirectMARLEnv, - DirectMARLEnvCfg, - DirectRLEnvCfg, - ManagerBasedRLEnvCfg, - multi_agent_to_single_agent, -) -from isaaclab.utils.dict import print_dict -from isaaclab.utils.io import dump_yaml - -from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg, RslRlVecEnvWrapper - -import isaaclab_tasks # noqa: F401 -import robotic_grounding.tasks # noqa: F401 -from robotic_grounding.tasks.scene_utils import SceneConfig, apply_scene_config -from robotic_grounding.tasks.v2p_whole_body.utils import WandbVideoUploader -from viewer_utils import autoframe_viewer - -from isaaclab_tasks.utils import get_checkpoint_path -from isaaclab_tasks.utils.hydra import hydra_task_config -from eval_callback import EvalCallback - -# import logger -logger = logging.getLogger(__name__) - -# PLACEHOLDER: Extension template (do not remove this comment) - -torch.backends.cuda.matmul.allow_tf32 = True -torch.backends.cudnn.allow_tf32 = True -torch.backends.cudnn.deterministic = False -torch.backends.cudnn.benchmark = False - - -@hydra_task_config(args_cli.task, args_cli.agent) -def main( - env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, - agent_cfg: RslRlBaseRunnerCfg, -): - """Train with RSL-RL agent.""" - # override configurations with non-hydra CLI arguments - agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli) - env_cfg.scene.num_envs = ( - args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs - ) - - scene_config = None - # Apply scene config: motion_file (from Hydra override) takes priority, - # then --scene_config YAML, then the env_cfg default. - if args_cli.motion_file is not None: - env_cfg.motion_file = args_cli.motion_file - if hasattr(env_cfg, "motion_file") and env_cfg.motion_file is not None: - scene_config = SceneConfig.from_motion_file(env_cfg.motion_file) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - autoframe_viewer(env_cfg, scene_config.motion_file) - elif args_cli.scene_config is not None: - env_cfg.scene_config_path = args_cli.scene_config - scene_config = SceneConfig.from_yaml(args_cli.scene_config) - apply_scene_config( - env_cfg, scene_config, use_primitive_urdfs=args_cli.use_primitive_urdfs - ) - autoframe_viewer(env_cfg, scene_config.motion_file) - - # set max iterations - agent_cfg.max_iterations = ( - args_cli.max_iterations - if args_cli.max_iterations is not None - else agent_cfg.max_iterations - ) - - # set the environment seed - # note: certain randomizations occur in the environment initialization so we set the seed here - env_cfg.seed = agent_cfg.seed - env_cfg.sim.device = ( - args_cli.device if args_cli.device is not None else env_cfg.sim.device - ) - # check for invalid combination of CPU device with distributed training - if ( - args_cli.distributed - and args_cli.device is not None - and "cpu" in args_cli.device - ): - raise ValueError( - "Distributed training is not supported when using CPU device. " - "Please use GPU device (e.g., --device cuda) for distributed training." - ) - - # multi-gpu training configuration - if args_cli.distributed: - env_cfg.sim.device = f"cuda:{app_launcher.local_rank}" - agent_cfg.device = f"cuda:{app_launcher.local_rank}" - - # set seed to have diversity in different threads - seed = agent_cfg.seed + app_launcher.local_rank - env_cfg.seed = seed - agent_cfg.seed = seed - - # specify directory for logging experiments - log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name) - log_root_path = os.path.abspath(log_root_path) - print(f"[INFO] Logging experiment in directory: {log_root_path}") - # specify directory for logging runs: {time-stamp}_{run_name} - log_dir = datetime.now(ZoneInfo("America/Los_Angeles")).strftime( - "%Y-%m-%d_%H-%M-%S" - ) - # The Ray Tune workflow extracts experiment name using the logging line below, hence, do not change it (see PR #2346, comment-2819298849) - print(f"Exact experiment name requested from command line: {log_dir}") - if agent_cfg.run_name: - log_dir += f"_{agent_cfg.run_name}" - log_dir = os.path.join(log_root_path, log_dir) - - # set the IO descriptors export flag if requested - if isinstance(env_cfg, ManagerBasedRLEnvCfg): - env_cfg.export_io_descriptors = args_cli.export_io_descriptors - else: - logger.warning( - "IO descriptors are only supported for manager based RL environments. No IO descriptors will be exported." - ) - - # set the log directory for the environment (works for all environment types) - env_cfg.log_dir = log_dir - - # create isaac environment - env = gym.make( - args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None - ) - - # convert to single-agent instance if required by the RL algorithm - if isinstance(env.unwrapped, DirectMARLEnv): - env = multi_agent_to_single_agent(env) - - # save resume path before creating a new log_dir - if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": - if os.path.isabs(agent_cfg.load_checkpoint) or os.path.exists( - agent_cfg.load_checkpoint - ): - # allow path to be directly specified - resume_path = os.path.abspath(agent_cfg.load_checkpoint) - elif args_cli.wandb_id is not None: - resume_path = download_run(args_cli.wandb_id) - agent_cfg.load_checkpoint = resume_path - else: - # get checkpoint from log directory - resume_path = get_checkpoint_path( - log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint - ) - - # wrap for video recording - if args_cli.video: - _base_env = env # capture reference before RecordVideo wrapping - # Mutable reference so _video_step_trigger can see the RecordVideo wrapper - # after it is created below (closures capture variables, not values). - _record_video_ref = [None] - - def _video_step_trigger(step: int) -> bool: - # Use .unwrapped to reach the actual Isaac env: gym.make() wraps it in - # OrderEnforcing, and setting an attribute on that wrapper would create a - # shadow that permanently masks subsequent writes to the inner Isaac env. - _isaac_env = _base_env.unwrapped - # Eval-triggered recording — always honored, even with --eval_video_only. - eval_pending = getattr(_isaac_env, "eval_video_trigger_pending", False) - if eval_pending: - _isaac_env.eval_video_trigger_pending = False - return True - # Training-triggered recording (curriculum pre-decay + interval). - # Suppressed entirely by --eval_video_only. - if args_cli.eval_video_only: - return False - pending = getattr(_isaac_env, "video_trigger_pending", False) - if pending: - _isaac_env.video_trigger_pending = False - return True - # Only fire the interval trigger when not already recording, to avoid - # prematurely cutting an in-progress eval recording. - _rv = _record_video_ref[0] - if _rv is not None and _rv.recording: - return False - return step % args_cli.video_interval == 0 - - video_kwargs = { - "video_folder": os.path.join(log_dir, "videos", "train"), - "step_trigger": _video_step_trigger, - "video_length": args_cli.video_length, - "disable_logger": True, - } - print("[INFO] Recording videos during training.") - print_dict(video_kwargs, nesting=4) - env = gym.wrappers.RecordVideo(env, **video_kwargs) - _record_video_ref[0] = env # fill the reference now that the wrapper exists - - start_time = time.time() - - # wrap around environment for rsl-rl - env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions) - - # create runner from rsl-rl - if agent_cfg.class_name == "OnPolicyRunner": - runner = OnPolicyRunner( - env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device - ) - elif agent_cfg.class_name == "DistillationRunner": - runner = DistillationRunner( - env, agent_cfg.to_dict(), log_dir=log_dir, device=agent_cfg.device - ) - else: - raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}") - # write git state to logs - runner.add_git_repo_to_log(__file__) - # load the checkpoint - if agent_cfg.resume or agent_cfg.algorithm.class_name == "Distillation": - print(f"[INFO]: Loading model checkpoint from: {resume_path}") - # load previously trained model - runner.load(resume_path) - - # Reset the std of the policy network - if args_cli.set_std is not None: - with torch.no_grad(): - runner.alg.policy.std.fill_(args_cli.set_std) - - # set the actor network to zero if requested - if args_cli.zero_actor: - torch.nn.init.zeros_(runner.alg.policy.actor[-1].weight) - torch.nn.init.zeros_(runner.alg.policy.actor[-1].bias) - - # dump the configuration into log-directory - dump_yaml(os.path.join(log_dir, "params", "env.yaml"), env_cfg) - dump_yaml(os.path.join(log_dir, "params", "agent.yaml"), agent_cfg) - - # setup eval callback: run inference episodes at startup, before every VOC - # decay, and at most every 1000 training iterations as a fallback. - if args_cli.eval_episodes_per_save > 0: - _eval_cb = EvalCallback( - runner=runner, - env=env, - eval_episodes=args_cli.eval_episodes_per_save, - log_video=args_cli.video, - ) - - # Tell the curriculum to defer each VOC decay until after eval. - _eval_cb._isaac_env.pre_decay_eval_enabled = True - _eval_cb._isaac_env.pre_decay_eval_pending = False - - # Hook runner.log (called every iteration, after rollout+update) to fire eval - # when a pre-decay eval is pending OR every 1000 iters as a fallback. - _orig_log = runner.log - _last_eval_iter: list[int] = [0] - - def _monitored_log(locs, width=80, pad=35): - it = locs.get("it", 0) - pre_decay = getattr(_eval_cb._isaac_env, "pre_decay_eval_pending", False) - should_eval = pre_decay or (it - _last_eval_iter[0] >= 1000) - if should_eval: - try: - _eval_cb(f"model_{it}.pt") - except Exception as exc: - print(f"[eval] WARNING: eval callback raised an exception: {exc}") - _last_eval_iter[0] = it - _orig_log(locs, width=width, pad=pad) - - runner.log = _monitored_log - - # Fire one eval immediately at startup (after wandb is initialised). - _orig_learn = runner.learn - - def _learn_with_startup_eval(num_learning_iterations, **kw): - runner._prepare_logging_writer() - try: - _eval_cb("model_startup.pt") - except Exception as exc: - print(f"[eval] WARNING: startup eval failed: {exc}") - return _orig_learn(num_learning_iterations, **kw) - - runner.learn = _learn_with_startup_eval - - # setup video uploader for wandb (train folder only; eval videos logged directly by _EvalCallback) - video_uploader = None - if args_cli.video and agent_cfg.logger == "wandb": - video_folder = os.path.join(log_dir, "videos", "train") - os.makedirs(video_folder, exist_ok=True) - video_uploader = WandbVideoUploader( - video_folder, - check_interval=60.0, - num_steps_per_env=agent_cfg.num_steps_per_env, - wandb_key="train/video", - ) - video_uploader.start() - - # run training - runner.learn( - num_learning_iterations=agent_cfg.max_iterations, init_at_random_ep_len=True - ) - - print(f"Training time: {round(time.time() - start_time, 2)} seconds") - - # stop video uploader - if video_uploader is not None: - video_uploader.stop() - - # close the simulator - env.close() - - -if __name__ == "__main__": - # run the main function - main() - # close sim app - simulation_app.close() diff --git a/robotic_grounding/scripts/rsl_rl/viewer_utils.py b/robotic_grounding/scripts/rsl_rl/viewer_utils.py deleted file mode 100644 index a4f8a443..00000000 --- a/robotic_grounding/scripts/rsl_rl/viewer_utils.py +++ /dev/null @@ -1,49 +0,0 @@ -"""Shared viewer utilities for RSL-RL training/eval scripts.""" - -from __future__ import annotations - - -def autoframe_viewer(env_cfg, motion_file: str) -> None: - """Set viewer eye/lookat from the motion file's actual bounding box. - - Reads object_body_position and robot_{side}_wrist_position from the parquet, - computes the scene centroid + extent, and positions the camera at a - 135°-azimuth / ~30°-elevation offset with a 6 m minimum distance so the - cloned env grid stays visible in training video. Falls back silently if the - parquet is missing or the required fields are absent. - """ - import logging - - import numpy as np - import pyarrow.parquet as pq - from isaaclab.envs import ManagerBasedRLEnvCfg - - logger = logging.getLogger(__name__) - - if not (isinstance(env_cfg, ManagerBasedRLEnvCfg) and hasattr(env_cfg, "viewer")): - return - try: - data = pq.read_table(motion_file).to_pydict() - pts = [] - obj = data.get("object_body_position", [None])[0] - if obj: - pts.append(np.asarray(obj).reshape(-1, 3)) - for side in ("right", "left"): - wrist = data.get(f"robot_{side}_wrist_position", [None])[0] - if wrist: - pts.append(np.asarray(wrist).reshape(-1, 3)) - if not pts: - return - all_pts = np.concatenate(pts, axis=0) - lo, hi = all_pts.min(axis=0), all_pts.max(axis=0) - center = 0.5 * (lo + hi) - extent = max(float(np.linalg.norm(hi - lo)), 0.3) - dist = max(2.5 * extent, 6.0) - eye = center + dist * np.array([-0.60, 0.60, 0.57]) - env_cfg.viewer.lookat = tuple(float(c) for c in center) - env_cfg.viewer.eye = tuple(float(c) for c in eye) - logger.info( - f"viewer autoframe: lookat={env_cfg.viewer.lookat}, eye={env_cfg.viewer.eye}" - ) - except Exception as e: # noqa: BLE001 - logging.getLogger(__name__).warning(f"viewer autoframe failed: {e}") diff --git a/robotic_grounding/scripts/run_osmo.py b/robotic_grounding/scripts/run_osmo.py deleted file mode 100755 index a5969221..00000000 --- a/robotic_grounding/scripts/run_osmo.py +++ /dev/null @@ -1,181 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""OSMO workflow submission script. - -SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -SPDX-License-Identifier: Apache-2.0 - -Script to submit OSMO workflow for robotic grounding development environment. -""" - -import argparse -import os -import shlex -import subprocess -import sys -from pathlib import Path - -REPO_ROOT = Path(__file__).resolve().parent.parent -if str(REPO_ROOT) not in sys.path: - sys.path.insert(0, str(REPO_ROOT)) - -from experiments.utils import ( # noqa: E402 - DEFAULT_OSMO_IMAGE_LATEST, - DEFAULT_OSMO_IMAGE_REPO, - get_internal_config_value, - require_internal_config_value, -) - - -def run_command(cmd: str, check: bool = True) -> subprocess.CompletedProcess: - """Run a shell command and return the result.""" - args = shlex.split(cmd) - print(f"Running: {shlex.join(args)}") - result = subprocess.run(args, check=check, capture_output=True, text=True) - if result.stdout: - print(result.stdout) - if result.stderr: - print(result.stderr, file=sys.stderr) - return result - - -def main() -> None: - """Submit OSMO workflow for robotic grounding development environment.""" - parser = argparse.ArgumentParser( - description="Submit OSMO workflow for robotic grounding development" - ) - parser.add_argument( - "--experiment-name", required=True, help="Experiment name for the workflow" - ) - parser.add_argument( - "--image", - help="Docker image to use for the workflow (if not provided, will build new one)", - ) - parser.add_argument( - "--workflow-yaml", - default="workflow/train.yaml", - help="Path to OSMO workflow YAML file", - ) - parser.add_argument( - "--pool", - default=None, - help="OSMO pool to use for workflow execution (default: internal experiment_config.yaml)", - ) - parser.add_argument( - "--build-image", - action="store_true", - help="Build and push Docker image before submitting (uses experiment name as tag).", - ) - parser.add_argument( - "--priority", - default="NORMAL", - help="OSMO job priority (default: NORMAL)", - ) - parser.add_argument( - "--dry-run", action="store_true", help="Print commands without executing" - ) - parser.add_argument( - "--set", - action="append", - default=[], - dest="extra_sets", - help="Additional key=value pairs for osmo workflow submit (e.g., dataset=taco)", - ) - - args = parser.parse_args() - - # Get the repository root directory (assuming script is in scripts/) - os.chdir(REPO_ROOT) - pool = args.pool or get_internal_config_value("osmo", "runner_default_pool") - if pool is None: - raise SystemExit( - "OSMO pool is required. Pass --pool or provide osmo.runner_default_pool " - "in experiments/experiment_config.yaml." - ) - - # Determine image to use - if args.build_image: - # If --image is also specified, build to that exact tag; otherwise tag with experiment name. - if args.image: - image_name = args.image - image_version = image_name.split(":")[-1] - else: - image_version = args.experiment_name - image_repo = DEFAULT_OSMO_IMAGE_REPO or require_internal_config_value( - "osmo", "image_repo" - ) - image_name = f"{image_repo}:{image_version}" - - print(f"\nBuilding Docker image: {image_name} ...") - build_cmd = f"./workflow/run.sh build {image_version}" - - if args.dry_run: - print(f"[DRY RUN] {build_cmd}") - else: - result = run_command(build_cmd, check=False) - if result.returncode != 0: - print("Error: Docker build failed") - sys.exit(1) - - print(f"\nPushing Docker image: {image_name} ...") - push_cmd = f"./workflow/run.sh push {image_version}" - - if args.dry_run: - print(f"[DRY RUN] {push_cmd}") - else: - result = run_command(push_cmd, check=False) - if result.returncode != 0: - print("Error: Docker push failed") - sys.exit(1) - elif args.image: - image_name = args.image - print(f"Using existing Docker image: {image_name}") - else: - image_name = DEFAULT_OSMO_IMAGE_LATEST or require_internal_config_value( - "osmo", "image_latest" - ) - print(f"Using default Docker image: {image_name}") - - # Use provided image - print(f"\nUsing Docker image: {image_name}") - - # Check if workflow YAML exists - workflow_path = Path(args.workflow_yaml) - if not workflow_path.exists(): - print(f"Error: Workflow file not found: {args.workflow_yaml}") - sys.exit(1) - - # Submit OSMO workflow - print(f"\nSubmitting OSMO workflow: {args.workflow_yaml}") - workflow_name = f"robotic_grounding_{args.experiment_name}" - - all_sets = [ - f'workflow_name="{workflow_name}"', - f'image="{image_name}"', - ] + args.extra_sets - set_str = " ".join(all_sets) - - osmo_cmd = ( - f"osmo workflow submit {args.workflow_yaml} " - f"--set {set_str} " - f"--pool {pool} " - f"--priority {args.priority}" - ) - - print(f"\n{osmo_cmd}\n") - - if args.dry_run: - print("[DRY RUN] Would submit workflow with above command") - else: - result = run_command(osmo_cmd, check=False) - if result.returncode != 0: - print("Error: OSMO workflow submission failed") - sys.exit(1) - print("\n✅ Workflow submitted successfully!") - print("\nYou can check the workflow status with:") - print(" osmo workflow list") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/segment_to_atomic.py b/robotic_grounding/scripts/segment_to_atomic.py deleted file mode 100644 index 64ce142f..00000000 --- a/robotic_grounding/scripts/segment_to_atomic.py +++ /dev/null @@ -1,622 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -r"""Segment loaded/retargeted sequences into atomic hand-object interaction clips. - -ONLY TESTED ON HOT3D so far. The code uses generic ManoSharpaData fields -(``mano_*_tips_distance``, ``mano_*_object_contact_part_ids``, etc.) so it -should work on other datasets, but the contact thresholds, gap-bridging -windows, and per-body majority-vote heuristics have only been validated on -HOT3D's 30 Hz, multi-object kitchen sequences. Use on taco / arctic / oakink2 -will require re-tuning ``--threshold``, ``--gap_frames``, ``--min_segment_len``. - -Reads from a ManoSharpaData parquet dir and writes sliced parquets to -``_segmented/`` (or ``--output_dir``) plus a ``segment_manifest.csv`` -sidecar. - -Interaction modes ------------------ -A — one hand active, any number of objects -B — both hands active, touching the same object body -C — both hands active, touching different object bodies - -Contact detection ------------------ -A hand is considered "active" on frame t when: - min(mano_{side}_tips_distance[t]) < threshold - -Object assignment per segment: majority vote of non-zero values in -mano_{side}_object_contact_part_ids across all active frames of that segment. - -Grace-period smoothing: short contact gaps < gap_frames are bridged so that -brief lifts don't fragment a single interaction into many tiny clips. - -Usage ------ - python scripts/segment_to_atomic.py \\ - --input_dir ~/datasets/.../hot3d_processed \\ - --dry_run - - python scripts/segment_to_atomic.py \\ - --input_dir ~/datasets/.../taco_processed \\ - --sequence_id some_sequence_id -""" - -import argparse -import csv -import logging -import re -import sys -from collections import Counter -from pathlib import Path - -# Allow running directly from scripts/ without installing the package. -_REPO_ROOT = Path(__file__).resolve().parent.parent # robotic_grounding/ -_SOURCE_DIR = str(_REPO_ROOT / "source" / "robotic_grounding") -if _SOURCE_DIR not in sys.path: - sys.path.insert(0, _SOURCE_DIR) - -import numpy as np # noqa: E402 — sys.path setup must precede this import -from robotic_grounding.retarget.data_logger import ( # noqa: E402 - MANO_FIELDS, - OBJECT_FIELDS, - SHARPA_FIELDS, - ManoSharpaData, - add_sequence_filter_args, - filter_sequence_ids, - list_sequence_ids, -) -from tqdm import tqdm # noqa: E402 - -# Defined in retarget_utils.py but importing that module pulls in torch/pinocchio. -DEFAULT_PARTITION_COLS = ["sequence_id", "robot_name"] - -logging.getLogger().setLevel(logging.ERROR) - -# All time-series field names — used when slicing a sequence by frame range. -_TIMESERIES_FIELDS: list[str] = [ - name - for (name, _, _, is_ts) in (MANO_FIELDS + SHARPA_FIELDS + OBJECT_FIELDS) - if is_ts -] - -SEGMENT_STILL_PADDING_FRAMES = 10 # still frames prepended/appended to each segment - -MANIFEST_COLUMNS = [ - "segment_id", - "parent_sequence_id", - "segment_idx", - "mode", - "start_frame", - "end_frame", - "num_frames", - "duration_s", - "object_name", -] - - -# --------------------------------------------------------------------------- -# Core algorithms -# --------------------------------------------------------------------------- - - -def detect_contact(tips: np.ndarray, threshold: float) -> np.ndarray: - """Return bool[T] — True where the hand is within threshold of any object surface.""" - return tips.min(axis=1) < threshold - - -def fill_gaps(mask: np.ndarray, gap_frames: int) -> np.ndarray: - """Fill runs of False shorter than gap_frames inside a True region. - - Equivalent to binary closing with a flat window of size gap_frames. - """ - if gap_frames <= 0: - return mask - result = mask.copy() - T = len(mask) - i = 0 - while i < T: - if not result[i]: - # Find end of False run - j = i - while j < T and not result[j]: - j += 1 - gap_len = j - i - # Close the gap only if it is surrounded by True on both sides - if gap_len < gap_frames and i > 0 and j < T: - result[i:j] = True - i = j - else: - i += 1 - return result - - -def get_dominant_body(part_ids: np.ndarray, active_mask: np.ndarray) -> int | None: - """Majority vote of non-zero contact body IDs on active frames.""" - active_ids = part_ids[active_mask].ravel() - nonzero = active_ids[active_ids != 0] - if len(nonzero) == 0: - return None - return int(Counter(nonzero.tolist()).most_common(1)[0][0]) - - -def find_active_windows( - right_active: np.ndarray, left_active: np.ndarray -) -> list[tuple[int, int]]: - """Return list of (start, end) frame indices where at least one hand is active. - - end is exclusive (Python slice convention). - """ - combined = right_active | left_active - windows: list[tuple[int, int]] = [] - T = len(combined) - i = 0 - while i < T: - if combined[i]: - j = i - while j < T and combined[j]: - j += 1 - windows.append((i, j)) - i = j - else: - i += 1 - return windows - - -def _per_frame_dominant_body(cpid_r: np.ndarray, cpid_l: np.ndarray) -> np.ndarray: - """Return int[T] — dominant contacted body ID per frame (0 if no contact).""" - combined = np.concatenate([cpid_r, cpid_l], axis=1) # [T, 32] - T = len(combined) - result = np.zeros(T, dtype=np.int32) - for t in range(T): - nonzero = combined[t][combined[t] != 0] - if len(nonzero): - result[t] = Counter(nonzero.tolist()).most_common(1)[0][0] - return result - - -def split_by_object( - start: int, - end: int, - per_frame_body: np.ndarray, - gap_frames: int, -) -> list[tuple[int, int]]: - """Split [start, end) into sub-windows wherever the dominant touched body changes. - - Short zero-gaps (< gap_frames) between same-body runs are bridged. - Returns a list of (sub_start, sub_end) pairs in global frame coordinates. - """ - window = per_frame_body[start:end] - T = len(window) - sub_windows: list[tuple[int, int]] = [] - i = 0 - - while i < T: - if window[i] == 0: - i += 1 - continue - - current_body = int(window[i]) - sub_start = i - j = i + 1 - - while j < T: - if window[j] == current_body: - j += 1 - elif window[j] == 0: - # Look ahead: does the same body resume within gap_frames? - k = j - while k < T and window[k] == 0: - k += 1 - if k < T and int(window[k]) == current_body and (k - j) < gap_frames: - j = k + 1 # bridge the gap, stay in same sub-window - else: - break # gap too long or body changed — end sub-window - else: - break # different body — end sub-window - - sub_windows.append((start + sub_start, start + j)) - i = j - - return sub_windows if sub_windows else [(start, end)] - - -def classify_mode( - seg_right: np.ndarray, - seg_left: np.ndarray, - right_body: int | None, - left_body: int | None, -) -> str: - """Return interaction mode for a segment. - - seg_right / seg_left are bool[T] slices scoped to the segment. - """ - both_hands_active = seg_right.any() and seg_left.any() - if not both_hands_active: - return "A" - if right_body is None or left_body is None or right_body == left_body: - return "B" - return "C" - - -def _remap_paths( - paths: list[str] | None, old_prefix: str, new_prefix: str -) -> list[str] | None: - """Replace old_prefix with new_prefix in each path string.""" - if not paths: - return paths - return [p.replace(old_prefix, new_prefix, 1) if p else p for p in paths] - - -# Scalar per-body lists that are indexed [N_bodies] and must be filtered together -# with object_body_position / object_body_wxyz. -_PER_BODY_SCALAR_FIELDS = ( - "object_body_names", - "safe_object_body_names", - "object_mesh_paths", - "object_urdf_paths", - "object_mesh_radius", -) -# Timeseries with shape [T, N_bodies, *] that need body-axis filtering. -_PER_BODY_TS_FIELDS = ("object_body_position", "object_body_wxyz") -# Contact-part-id fields whose values are 1-indexed body IDs and must be remapped. -_CONTACT_PART_ID_FIELDS = ( - "mano_right_object_contact_part_ids", - "mano_left_object_contact_part_ids", -) - - -def _object_prefix(body_name: str) -> str: - """Strip trailing _body_N / _Body_N suffix to get the physical-object prefix.""" - return re.sub(r"_[Bb]ody_\d+$", "", body_name) - - -def _expand_to_object_siblings(body_1idx: set[int], body_names: list[str]) -> set[int]: - """Expand a set of body IDs to include all siblings of the same object. - - Bodies sharing the same name prefix before ``_body_N`` are siblings. - E.g. dominant body 1 ('dumbbell_body_0') also pulls in body 2 ('dumbbell_body_1'). - """ - prefix_to_ids: dict[str, list[int]] = {} - for i, name in enumerate(body_names): - prefix = _object_prefix(name) - prefix_to_ids.setdefault(prefix, []).append(i + 1) # 1-indexed - - expanded = set(body_1idx) - for bid in list(body_1idx): - if 1 <= bid <= len(body_names): - prefix = _object_prefix(body_names[bid - 1]) - for sibling in prefix_to_ids.get(prefix, []): - expanded.add(sibling) - return expanded - - -def _filter_to_touched_bodies( - d: dict, dominant_body_1idx: set[int] | None = None -) -> dict: - """Keep only the object bodies for this segment; remap contact IDs. - - dominant_body_1idx: 1-indexed body IDs determined by majority-vote in - segment_sequence (right_body / left_body). Sibling bodies of the same - physical object are automatically included. - - If dominant_body_1idx is None (no contact recorded), all bodies are kept. - """ - body_names: list[str] = d.get("object_body_names") or [] - if not body_names: - return d - - if dominant_body_1idx is None or not dominant_body_1idx: - return d - - # Expand dominant IDs to include all sibling bodies of the same object. - touched_1idx = _expand_to_object_siblings(dominant_body_1idx, body_names) - - # Sorted 0-indexed list of bodies to keep. - n_bodies = len(body_names) - touched_0idx: list[int] = sorted(v - 1 for v in touched_1idx if 1 <= v <= n_bodies) - if not touched_0idx: - return d - - # Build old-1idx → new-1idx remapping. - old1_to_new1: dict[int, int] = { - old0 + 1: new0 + 1 for new0, old0 in enumerate(touched_0idx) - } - - # Filter scalar per-body lists. - for field in _PER_BODY_SCALAR_FIELDS: - val = d.get(field) - if val is not None: - d[field] = [val[i] for i in touched_0idx if i < len(val)] - - # Filter timeseries body axis: shape [T, N_bodies, *] → [T, len(touched), *]. - for field in _PER_BODY_TS_FIELDS: - val = d.get(field) - if val is None: - continue - d[field] = [ - [frame_bodies[i] for i in touched_0idx if i < len(frame_bodies)] - for frame_bodies in val - ] - - # Remap contact part IDs: 0 stays 0; unmapped old IDs (other objects) → 0. - for field in _CONTACT_PART_ID_FIELDS: - val = d.get(field) - if val is None: - continue - d[field] = [[old1_to_new1.get(v, 0) for v in frame_ids] for frame_ids in val] - - return d - - -def slice_data( - data: ManoSharpaData, - start: int, - end: int, - parent_id: str, - seg_idx: int, - local_repo: Path | None = None, - dominant_body_1idx: set[int] | None = None, - pad_frames: int = 0, -) -> ManoSharpaData: - """Return a new ManoSharpaData containing only frames [start:end]. - - dominant_body_1idx: 1-indexed body IDs from majority-vote contact detection; - only those objects (and their siblings) are kept in the output. - - If local_repo is provided, rewrites object_mesh_paths and object_urdf_paths - so that OSMO-generated /workspace/video_to_data/ prefixes resolve locally. - - pad_frames: number of still frames to prepend (duplicating frame 0) and - append (duplicating the last frame). This gives support-surface reconstruction - a guaranteed window of stationary object poses at both ends of each clip. - """ - d = data.to_dict() - for field_name in _TIMESERIES_FIELDS: - if d.get(field_name) is not None: - sliced = list(d[field_name][start:end]) - if pad_frames > 0 and sliced: - sliced = [sliced[0]] * pad_frames + sliced + [sliced[-1]] * pad_frames - d[field_name] = sliced - d["sequence_id"] = f"{parent_id}_seg{seg_idx:03d}" - - _filter_to_touched_bodies(d, dominant_body_1idx=dominant_body_1idx) - - if local_repo is not None: - old_prefix = "/workspace/video_to_data" - new_prefix = str(local_repo) - d["object_mesh_paths"] = _remap_paths( - d.get("object_mesh_paths"), old_prefix, new_prefix - ) - d["object_urdf_paths"] = _remap_paths( - d.get("object_urdf_paths"), old_prefix, new_prefix - ) - - return ManoSharpaData(**d) - - -# --------------------------------------------------------------------------- -# Per-sequence segmentation -# --------------------------------------------------------------------------- - - -def segment_sequence( - data: ManoSharpaData, - threshold: float, - gap_frames: int, - min_frames: int, -) -> list[dict]: - """Compute segment metadata dicts for one sequence. - - Returns a list of dicts with keys matching MANIFEST_COLUMNS - (excluding segment_id, which is filled by the caller). - """ - fps = data.fps or 30.0 - tips_r = np.array(data.mano_right_tips_distance) # [T, 5] - tips_l = np.array(data.mano_left_tips_distance) # [T, 5] - cpid_r = np.array(data.mano_right_object_contact_part_ids) # [T, 16] - cpid_l = np.array(data.mano_left_object_contact_part_ids) # [T, 16] - - active_r = detect_contact(tips_r, threshold) - active_l = detect_contact(tips_l, threshold) - active_r = fill_gaps(active_r, gap_frames) - active_l = fill_gaps(active_l, gap_frames) - - windows = find_active_windows(active_r, active_l) - - # Split windows that span multiple objects (fast object transitions get - # bridged by gap-filling above; split_by_object undoes that). - per_frame_body = _per_frame_dominant_body(cpid_r, cpid_l) - refined: list[tuple[int, int]] = [] - for start, end in windows: - refined.extend(split_by_object(start, end, per_frame_body, gap_frames)) - - segments: list[dict] = [] - for start, end in refined: - num_frames = end - start - if num_frames < min_frames: - continue - - seg_r = active_r[start:end] - seg_l = active_l[start:end] - right_body = get_dominant_body(cpid_r[start:end], seg_r) - left_body = get_dominant_body(cpid_l[start:end], seg_l) - mode = classify_mode(seg_r, seg_l, right_body, left_body) - dominant = {b for b in (right_body, left_body) if b is not None} - - segments.append( - { - "parent_sequence_id": data.sequence_id, - "segment_idx": len(segments), - "mode": mode, - "start_frame": start, - "end_frame": end, - "num_frames": num_frames, - "duration_s": round(num_frames / fps, 2), - "object_name": data.object_name or "", - # Internal: passed to slice_data but excluded from manifest CSV. - "_dominant_bodies": dominant, - } - ) - return segments - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- - - -def parse_args() -> argparse.Namespace: - """Parse CLI arguments for the segmentation script.""" - parser = argparse.ArgumentParser( - description="Segment retargeted sequences into atomic hand-object interaction clips.", - formatter_class=argparse.ArgumentDefaultsHelpFormatter, - ) - parser.add_argument( - "--input_dir", - type=Path, - required=True, - help="Path to a _processed/ parquet directory.", - ) - parser.add_argument( - "--output_dir", - type=Path, - default=None, - help="Output directory. Defaults to {input_dir.name}_segmented/ next to input_dir.", - ) - parser.add_argument( - "--threshold", - type=float, - default=0.02, - help="Fingertip-to-surface distance threshold in metres for contact detection.", - ) - parser.add_argument( - "--gap_frames", - type=int, - default=15, - help="Fill contact gaps shorter than this many frames (0.5 s at 30 Hz).", - ) - parser.add_argument( - "--min_frames", - type=int, - default=30, - help="Drop segments shorter than this many frames (1 s at 30 Hz).", - ) - parser.add_argument( - "--pad_frames", - type=int, - default=SEGMENT_STILL_PADDING_FRAMES, - help=( - "Still frames to prepend/append to each segment (duplicate of first/last frame). " - "Ensures support-surface reconstruction has a stationary object window at both ends. " - f"Default: {SEGMENT_STILL_PADDING_FRAMES}." - ), - ) - parser.add_argument( - "--dry_run", - action="store_true", - help="Print segment manifest only; do not write parquet files.", - ) - parser.add_argument( - "--local_repo", - type=Path, - default=None, - help=( - "When set, rewrites /workspace/video_to_data/ in object_mesh_paths and " - "object_urdf_paths to this local monorepo root. Leave unset (default) when " - "vis_retargeted.py will be run inside the Docker container where " - "/workspace/video_to_data/ is already correctly mounted." - ), - ) - add_sequence_filter_args(parser) - return parser.parse_args() - - -def main() -> None: - """CLI entry point — segment all sequences under ``--input_dir`` to atomic clips.""" - args = parse_args() - - output_dir: Path = args.output_dir or ( - args.input_dir.parent / f"{args.input_dir.name}_segmented" - ) - - sequence_ids = list_sequence_ids(str(args.input_dir)) - sequence_ids = filter_sequence_ids(sequence_ids, args) - if not sequence_ids: - print("No sequences matched the filter.", file=sys.stderr) - sys.exit(1) - - print(f"Processing {len(sequence_ids)} sequence(s).") - print(f" input: {args.input_dir}") - if not args.dry_run: - print(f" output: {output_dir}") - print( - f" threshold: {args.threshold} m | gap_frames: {args.gap_frames}" - f" | min_frames: {args.min_frames}" - ) - - manifest_path = output_dir.parent / "segment_manifest.csv" - manifest_writer = None - manifest_file = None - - if not args.dry_run: - output_dir.mkdir(parents=True, exist_ok=True) - manifest_file = open(manifest_path, "w", newline="") - manifest_writer = csv.DictWriter(manifest_file, fieldnames=MANIFEST_COLUMNS) - manifest_writer.writeheader() - - total_segs = 0 - - for seq_id in tqdm(sequence_ids, desc="sequences"): - data = ManoSharpaData.from_parquet( - root_path=str(args.input_dir), - filters=[("sequence_id", "=", seq_id)], - ) - - segments = segment_sequence( - data, args.threshold, args.gap_frames, args.min_frames - ) - - for seg in segments: - seg_id = f"{seg['parent_sequence_id']}_seg{seg['segment_idx']:03d}" - # Exclude internal keys (prefixed with _) from the manifest row. - row = { - "segment_id": seg_id, - **{k: v for k, v in seg.items() if not k.startswith("_")}, - } - - if args.dry_run: - print( - f" {seg_id} mode={seg['mode']}" - f" [{seg['start_frame']}:{seg['end_frame']}]" - f" {seg['duration_s']:.1f}s" - ) - else: - sliced = slice_data( - data, - seg["start_frame"], - seg["end_frame"], - seg["parent_sequence_id"], - seg["segment_idx"], - local_repo=args.local_repo, - dominant_body_1idx=seg.get("_dominant_bodies"), - pad_frames=args.pad_frames, - ) - sliced.save_to_parquet( - root_path=str(output_dir), - partition_cols=DEFAULT_PARTITION_COLS, - ) - assert manifest_writer is not None # narrowed by args.dry_run branch - manifest_writer.writerow(row) - - total_segs += len(segments) - - if manifest_file is not None: - manifest_file.close() - - print(f"\nTotal segments: {total_segs} from {len(sequence_ids)} sequence(s).") - if not args.dry_run: - print(f"Manifest: {manifest_path}") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/setup_css_env.sh b/robotic_grounding/scripts/setup_css_env.sh deleted file mode 100755 index 97c75b7a..00000000 --- a/robotic_grounding/scripts/setup_css_env.sh +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env bash -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 -# -# Source this file to set CSS (PDX) credentials for list_css_sequences.py. -# -# Usage: -# source scripts/setup_css_env.sh - -export CSS_ENDPOINT_URL="https://pdx.s8k.io" -export CSS_ACCESS_KEY="${CSS_ACCESS_KEY:-REPLACE_ME}" -export CSS_SECRET_KEY="${CSS_SECRET_KEY:-REPLACE_ME}" -export CSS_REGION="us-east-1" - -if [ "${CSS_ACCESS_KEY}" = "REPLACE_ME" ] || [ "${CSS_SECRET_KEY}" = "REPLACE_ME" ]; then - echo "ERROR: CSS credentials have not been configured in this script." - echo "" - echo "To obtain credentials:" - echo " 1. Go to the CSS portal: https://pdx.s8k.io" - echo " 2. Generate or retrieve your access key and secret key" - echo " 3. Edit scripts/setup_css_env.sh and replace the REPLACE_ME values" - return 1 2>/dev/null || exit 1 -fi - -echo "CSS environment configured." diff --git a/robotic_grounding/scripts/setup_soma_assets.py b/robotic_grounding/scripts/setup_soma_assets.py deleted file mode 100644 index dd7013d2..00000000 --- a/robotic_grounding/scripts/setup_soma_assets.py +++ /dev/null @@ -1,206 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""One-time downloader for SOMA-X body-model assets. - -Drives ``soma.SOMALayer`` once with ``data_root=None`` so SOMA-X downloads -the asset bundle into its built-in HuggingFace cache, then copies the -specific files that ``robotic_grounding.retarget.read_soma`` requires -into the canonical repo path (``assets/body_models/soma/`` by default). - -We cannot pass ``--data-root`` straight through to ``SOMALayer``: SOMA-X -treats an existing-but-incomplete directory as "assets are supposed to -be here already" and crashes with ``FileNotFoundError`` for -``SOMA_neutral.npz`` instead of falling back to HuggingFace. The HF -cache path is also ephemeral inside the retarget Docker container -(``HOME=/tmp`` is tmpfs), so the bind-mounted repo path is the only -location that survives container restarts. - -The required asset list is the source of truth in -``robotic_grounding.retarget.read_soma._SOMA_REQUIRED_ASSETS`` / -``_SOMA_REQUIRED_BY_IDENTITY``; this script imports those constants via -``_missing_assets`` so it stays in sync when the manifest changes. - -Usage: - python scripts/setup_soma_assets.py - python scripts/setup_soma_assets.py --identity-model-type mhr --force -""" - -from __future__ import annotations - -import argparse -import shutil -import sys -from pathlib import Path - -import torch -from robotic_grounding.retarget import BODY_MODELS_DIR -from robotic_grounding.retarget.read_soma import ( - _SOMA_REQUIRED_ASSETS, - _SOMA_REQUIRED_BY_IDENTITY, - _missing_assets, -) - -DEFAULT_ROOT = BODY_MODELS_DIR / "soma" - - -def parse_args() -> argparse.Namespace: - """Parse command line arguments.""" - p = argparse.ArgumentParser( - description=( - "Populate assets/body_models/soma/ with the SOMA-X assets needed by scripts/retarget/soma_to_g1.py." - ) - ) - p.add_argument( - "--data-root", - type=Path, - default=DEFAULT_ROOT, - help=( - "Destination dir. Defaults to the canonical repo location " - f"({DEFAULT_ROOT}) so read_soma picks it up without needing " - "--soma-data-root at retarget time." - ), - ) - p.add_argument( - "--identity-model-type", - default="mhr", - choices=("mhr",), - help=( - "SOMA identity model variant. Only 'mhr' is exercised by the " - "current retarget scripts; passed straight through to SOMALayer." - ), - ) - p.add_argument( - "--force", - action="store_true", - help=( - "Wipe the destination directory before downloading. Use when " - "SOMA-X publishes new asset versions and you want to refresh " - "the on-disk cache. Does NOT clear the HuggingFace cache " - "itself; that is owned by SOMA-X / huggingface_hub." - ), - ) - return p.parse_args() - - -def _required_relpaths(identity_model_type: str) -> list[str]: - """Files that ``read_soma._missing_assets`` will check for.""" - return list(_SOMA_REQUIRED_ASSETS) + list( - _SOMA_REQUIRED_BY_IDENTITY.get(identity_model_type.lower(), ()) - ) - - -def _copy_required_assets( - src_root: Path, dst_root: Path, identity_model_type: str -) -> list[str]: - """Copy the required SOMA assets from ``src_root`` to ``dst_root``. - - Returns the list of files that could not be located under ``src_root`` - (typically zero; non-empty means the SOMA-X HF download did not match - the file layout this repo expects). - """ - not_found: list[str] = [] - for relpath in _required_relpaths(identity_model_type): - src = src_root / relpath - if not src.is_file(): - not_found.append(relpath) - continue - dst = dst_root / relpath - dst.parent.mkdir(parents=True, exist_ok=True) - # ``copy2`` follows symlinks so the destination becomes a real - # file (the HF cache stores blobs and exposes them via symlinks). - # This matters because the bind-mounted repo path may outlive - # the container's HF cache. - shutil.copy2(src, dst) - print(f" copied {relpath} ({src.stat().st_size / (1024 * 1024):.1f} MB)") - return not_found - - -def main() -> int: - """Download SOMA-X assets and stage them into ``--data-root``.""" - args = parse_args() - dst_root: Path = args.data_root.expanduser().resolve() - - if args.force and dst_root.is_dir(): - print(f"[setup_soma_assets] --force: removing {dst_root}") - shutil.rmtree(dst_root) - - if dst_root.is_dir(): - missing = _missing_assets(dst_root, args.identity_model_type) - if not missing: - print( - f"[setup_soma_assets] {dst_root} already populated with " - f"identity_model_type={args.identity_model_type!r}; nothing to do." - ) - return 0 - print( - f"[setup_soma_assets] {dst_root} exists but is missing: {missing}; downloading." - ) - else: - print(f"[setup_soma_assets] {dst_root} does not exist yet; will create.") - - try: - from soma import SOMALayer # noqa: PLC0415 - except ImportError as exc: - print( - "ERROR: py-soma-x is not installed. Install via " - "`pip install py-soma-x` (already baked into the retarget " - "Docker image at workflow/Dockerfile).", - file=sys.stderr, - ) - raise SystemExit(1) from exc - - # Pass ``data_root=None`` so SOMA-X downloads into its HuggingFace - # cache (``get_assets_dir()``). Passing the destination directly - # crashes when the directory exists but is empty -- SOMA-X assumes - # the assets are present and skips its own HF fallback. - device = "cuda" if torch.cuda.is_available() else "cpu" - print( - "[setup_soma_assets] running SOMALayer(data_root=None) to trigger " - f"HuggingFace download (identity_model_type={args.identity_model_type}, " - f"device={device})." - ) - layer = SOMALayer( - data_root=None, - identity_model_type=args.identity_model_type, - device=device, - ) - src_root = Path(layer.data_root).resolve() - print(f"[setup_soma_assets] SOMA-X downloaded to: {src_root}") - - if src_root == dst_root: - # Unlikely (HF cache lives under HOME, not the repo) but handled - # for completeness so we do not no-op silently when the source - # and destination coincide. - print( - "[setup_soma_assets] SOMA-X resolved to the destination directly; no copy needed." - ) - else: - print(f"[setup_soma_assets] copying required assets -> {dst_root}") - dst_root.mkdir(parents=True, exist_ok=True) - not_found = _copy_required_assets(src_root, dst_root, args.identity_model_type) - if not_found: - print( - f"ERROR: the SOMA-X snapshot at {src_root} is missing " - f"these expected files: {not_found}. The bundle layout " - "may have changed; update _SOMA_REQUIRED_BY_IDENTITY in " - "read_soma.py to match the current SOMA-X release.", - file=sys.stderr, - ) - return 1 - - missing_after = _missing_assets(dst_root, args.identity_model_type) - if missing_after: - print( - f"ERROR: {dst_root} is still missing required assets after " - f"copy: {missing_after}. Inspect {src_root} to see what was " - "actually downloaded.", - file=sys.stderr, - ) - return 1 - - print(f"[setup_soma_assets] done. {dst_root} now satisfies read_soma.") - return 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/robotic_grounding/scripts/sync_css_data.py b/robotic_grounding/scripts/sync_css_data.py deleted file mode 100644 index 172b6f65..00000000 --- a/robotic_grounding/scripts/sync_css_data.py +++ /dev/null @@ -1,213 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Download processed data from CSS (NVIDIA PDX storage) to local repo. - -Syncs files from the remote S3-compatible bucket to the local assets directory, -skipping files that already exist locally with the same size. - -By default all three outputs (loaded, processed, support_surfaces) are synced. -Use --loaded, --processed, or --support-surfaces to sync only specific outputs. - -Prerequisites: - - boto3: pip install boto3 - - CSS credentials configured via environment variables: - source scripts/setup_css_env.sh - -Usage: - python scripts/sync_css_data.py --dataset taco - python scripts/sync_css_data.py --dataset taco --component processed - python scripts/sync_css_data.py --dataset all --component loaded - python scripts/sync_css_data.py --dataset hot3d --component support_surfaces --dry-run -""" - -import argparse -import os -import re -import sys -from pathlib import Path - -import boto3 -from botocore.config import Config - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -ENDPOINT_URL = os.environ.get("CSS_ENDPOINT_URL", "https://pdx.s8k.io") -ACCESS_KEY = os.environ.get("CSS_ACCESS_KEY", "") -SECRET_KEY = os.environ.get("CSS_SECRET_KEY", "") -REGION = os.environ.get("CSS_REGION", "us-east-1") - -BUCKET = "datasets" -BASE_PREFIX = "v2d/human_motion_data" - -# Dataset registry — single source of truth. Add new datasets there, not here. -sys.path.insert( - 0, - str(Path(__file__).resolve().parent.parent / "source" / "robotic_grounding"), -) -from robotic_grounding.retarget.dataset_registry import ( # noqa: E402 - get_all_dataset_names, -) - -DATASETS = get_all_dataset_names() -STAGES = ("loaded", "processed", "support_surfaces") - -LOCAL_ASSETS_DIR = ( - Path(__file__).resolve().parent.parent - / "source" - / "robotic_grounding" - / "robotic_grounding" - / "assets" - / "human_motion_data" -) - - -def _remote_prefix(dataset: str, stage: str) -> str: - """Return the S3 prefix for a given dataset and stage.""" - if stage == "support_surfaces": - return f"{BASE_PREFIX}/{dataset}/support_surfaces/" - return f"{BASE_PREFIX}/{dataset}/{dataset}_{stage}/" - - -def _local_dir(dataset: str, stage: str) -> Path: - """Return the local directory for a given dataset and stage.""" - if stage == "support_surfaces": - return LOCAL_ASSETS_DIR / dataset / "support_surfaces" - return LOCAL_ASSETS_DIR / dataset / f"{dataset}_{stage}" - - -# --------------------------------------------------------------------------- -# S3 helpers -# --------------------------------------------------------------------------- -def get_s3_client() -> "boto3.client": - """Create an S3 client for CSS (PDX).""" - if not ACCESS_KEY or not SECRET_KEY: - print( - "Error: Set CSS_ACCESS_KEY and CSS_SECRET_KEY environment variables.\n" - " source scripts/setup_css_env.sh", - file=sys.stderr, - ) - sys.exit(1) - return boto3.client( - "s3", - endpoint_url=ENDPOINT_URL, - aws_access_key_id=ACCESS_KEY, - aws_secret_access_key=SECRET_KEY, - region_name=REGION, - config=Config(connect_timeout=10), - ) - - -def sync( - client: "boto3.client", - dataset: str, - stage: str, - pattern: str | None = None, - dry_run: bool = False, -) -> None: - """Download all objects under the remote prefix to the local directory. - - Skips files that already exist locally with the same size. - When *pattern* is given, only files whose top-level directory (sequence ID) - matches the regex are downloaded. - """ - prefix = _remote_prefix(dataset, stage) - local_root = _local_dir(dataset, stage) - regex = re.compile(pattern) if pattern else None - - print(f"Syncing s3://{BUCKET}/{prefix} -> {local_root}") - if regex: - print(f" filtering sequences: {pattern}") - - paginator = client.get_paginator("list_objects_v2") - downloaded = 0 - skipped = 0 - filtered = 0 - - for page in paginator.paginate(Bucket=BUCKET, Prefix=prefix): - for obj in page.get("Contents", []): - key = obj["Key"] - rel_path = key[len(prefix) :] - if not rel_path: - continue - - # Filter by sequence ID (first path component under the prefix) - if regex: - seq_id = rel_path.split("/")[0] - if not regex.search(seq_id): - filtered += 1 - continue - - local_path = local_root / rel_path - remote_size = obj["Size"] - - if local_path.exists() and local_path.stat().st_size == remote_size: - skipped += 1 - continue - - if dry_run: - print(f" [dry-run] would download: {rel_path} ({remote_size} bytes)") - downloaded += 1 - continue - - local_path.parent.mkdir(parents=True, exist_ok=True) - print(f" downloading: {rel_path} ({remote_size} bytes)") - client.download_file(BUCKET, key, str(local_path)) - downloaded += 1 - - summary = f" done: {downloaded} downloaded, {skipped} skipped (already up to date)" - if regex: - summary += f", {filtered} filtered out" - print(summary) - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- -def _parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser( - description="Download processed data from CSS (PDX) to local repo.", - ) - parser.add_argument( - "--dataset", - choices=[*DATASETS, "all"], - required=True, - help="Dataset to sync, or 'all' for every dataset.", - ) - parser.add_argument( - "--component", - choices=["all", "loaded", "processed", "support_surfaces"], - default="all", - help="Which output to sync: all (default), loaded, processed, or support_surfaces.", - ) - parser.add_argument( - "--pattern", - type=str, - default=None, - help="Regex pattern to filter sequence IDs (e.g., '.*screw.*').", - ) - parser.add_argument( - "--dry-run", - action="store_true", - default=False, - help="List files that would be downloaded without downloading.", - ) - return parser.parse_args() - - -def main() -> None: - """Entry point.""" - args = _parse_args() - - client = get_s3_client() - datasets = list(DATASETS) if args.dataset == "all" else [args.dataset] - stages = list(STAGES) if args.component == "all" else [args.component] - - for dataset in datasets: - for stage in stages: - sync(client, dataset, stage, pattern=args.pattern, dry_run=args.dry_run) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/train_tpv_wholebody_workflow.sh b/robotic_grounding/scripts/train_tpv_wholebody_workflow.sh deleted file mode 100755 index d8bd5137..00000000 --- a/robotic_grounding/scripts/train_tpv_wholebody_workflow.sh +++ /dev/null @@ -1,729 +0,0 @@ -#!/usr/bin/env bash -# train_tpv_wholebody_workflow.sh -# -# !!! RUN THIS ON THE HOST, NOT INSIDE THE DOCKER CONTAINER !!! -# -# This script orchestrates Docker (`./workflow/run.sh exec`) and OSMO submission -# (`python .../run_experiment.py --osmo`), neither of which work from inside the -# container (osmo CLI is not installed there, and `docker exec` from a container -# would target the host's docker daemon). The script aborts at startup if it -# detects it's running inside the container. -# -# Driver for the recurring third-person-view (TPV) whole-body ReconBody training -# flow. There are two ways to use it: -# -# A) Interactive driver `run` (recommended for a new dataset): -# Walks through all four stages below in order, prompting before each one -# so any stage can be skipped or re-prompted. Detected state is shown so -# the prompt defaults are sensible: e.g. init defaults to "skip" if a -# config already exists, set-frames pre-fills the current frame range and -# you can press Enter to keep it, etc. Resumable from any stage by -# re-running `run `. -# Examples: -# ./scripts/train_tpv_wholebody_workflow.sh run # asks for input -# ./scripts/train_tpv_wholebody_workflow.sh run # new dataset -# ./scripts/train_tpv_wholebody_workflow.sh run # existing exp -# ./scripts/train_tpv_wholebody_workflow.sh run --dry-run --yes ... # auto-accept defaults, no exec -# -# B) Per-stage subcommands (for re-running just one step or scripting): -# -# 1. init [host] Create experiments//config.yaml from -# a template and add it to registry.yaml. -# 2. replay [host->ctnr] Calls `./workflow/run.sh exec ...` to run -# scripts/replay_motion_viser.py inside the -# already-running container. -# 3. set-frames [host] Edit the chosen frames into the YAML. -# 4. submit [host] Submit the OSMO job via run_experiment.py. -# -# This script is intentionally scoped to whole-body TPV ReconBody training only -# (see experiments/recon_body_*/config.yaml). It is NOT a general experiment -# runner. -# -# Usage: -# ./scripts/train_tpv_wholebody_workflow.sh run [] [--dry-run] [--yes] -# ./scripts/train_tpv_wholebody_workflow.sh init [--exp-id NAME] [--force] -# ./scripts/train_tpv_wholebody_workflow.sh replay -# ./scripts/train_tpv_wholebody_workflow.sh set-frames -# ./scripts/train_tpv_wholebody_workflow.sh submit [--build-image] [--run-name NAME] [--dry-run] -# -# Global flags (must come before the subcommand): -# --dry-run Print every command but never execute (works for `run` too). -# --yes / -y Auto-accept all interactive prompt defaults (CI-friendly). - -set -euo pipefail - -# --- Resolve repo paths --- -SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -REPO_ROOT="$(cd "${SCRIPT_DIR}/.." && pwd)" -EXPERIMENTS_DIR="${REPO_ROOT}/experiments" -REGISTRY_FILE="${EXPERIMENTS_DIR}/registry.yaml" -TEMPLATE_CONFIG="${EXPERIMENTS_DIR}/recon_body_snack_box_pick_and_place_01/config.yaml" -WORKFLOW_RUN_SH="${REPO_ROOT}/workflow/run.sh" -REPLAY_SCRIPT="scripts/replay_motion_viser.py" -RUN_EXPERIMENT_SCRIPT="experiments/run_experiment.py" -# Container target — must match what `./workflow/run.sh start` produces. Both -# values are also positionally required by `./workflow/run.sh exec` (see -# workflow/run.sh, the `exec` case `shift 3`s blindly), so we pass them -# explicitly when invoking exec to avoid the "robotic-grounding----gpupython" -# corruption when the placeholders are omitted. -CONTAINER_VERSION="${CONTAINER_VERSION:-latest}" -CONTAINER_GPU="${CONTAINER_GPU:-0}" -CONTAINER_NAME="robotic-grounding-${CONTAINER_VERSION}-gpu${CONTAINER_GPU}" - -# --- Global flags (set by parse_global_flags) --- -DRY_RUN=0 -ASSUME_YES=0 - -# --- Host-vs-container guard --- -# Returns 0 if running inside a Docker container, 1 otherwise. Used to refuse -# to run the workflow script from inside the container shell, where docker exec -# and the OSMO CLI both fail. -running_inside_container() { - [[ -f /.dockerenv ]] && return 0 - if [[ -r /proc/1/cgroup ]] && grep -qE 'docker|containerd|kubepods' /proc/1/cgroup 2>/dev/null; then - return 0 - fi - return 1 -} - -# --- Pretty output helpers --- -_color() { - if [[ -t 1 ]]; then printf '\033[%sm%s\033[0m' "$1" "$2"; else printf '%s' "$2"; fi -} -log_info() { echo "[$(_color '36' INFO)] $*"; } -log_warn() { echo "[$(_color '33' WARN)] $*" >&2; } -log_error() { echo "[$(_color '31' ERROR)] $*" >&2; } -log_stage() { - echo - echo "$(_color '1;34' "===== $* =====")" -} -log_cmd() { echo "$(_color '90' '+') $*"; } - -die() { log_error "$*"; exit 1; } - -# Run a command, echoing it first. Honors --dry-run. -run_cmd() { - log_cmd "$*" - if (( DRY_RUN )); then - log_info "(dry-run) skipping execution" - return 0 - fi - eval "$@" -} - -# --- Prompt helpers (interactive) --- - -# prompt_yn -> echoes "y" or "n" -prompt_yn() { - local question="$1" default="${2:-y}" answer suffix - if [[ "${default}" == "y" ]]; then suffix="[Y/n]"; else suffix="[y/N]"; fi - if (( ASSUME_YES )); then - echo "${default}" - return 0 - fi - while true; do - read -r -p "${question} ${suffix} " answer < /dev/tty || answer="" - answer="${answer:-${default}}" - case "${answer,,}" in - y|yes) echo "y"; return 0 ;; - n|no) echo "n"; return 0 ;; - q|quit) die "user quit" ;; - *) echo "Please answer y, n, or q." >&2 ;; - esac - done -} - -# prompt_value -> echoes user value or default if empty. -prompt_value() { - local question="$1" default="$2" answer - if (( ASSUME_YES )); then - echo "${default}" - return 0 - fi - read -r -p "${question} [${default}] " answer < /dev/tty || answer="" - echo "${answer:-${default}}" -} - -# --- Path / id derivation --- - -# Strip trailing slash, then derive a short sequence name from a motion_file path -# Example: source/.../sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/robot_name=g1 -# -> sit_skinny_wood_chair_01 -derive_seq_name() { - local p="${1%/}" - # Drop trailing /robot_name=* - p="${p%/robot_name=*}" - local base="${p##*/}" - # Strip leading sequence_id= - base="${base#sequence_id=}" - # Strip leading date/time prefix YYYY-MM-DD_HH-MM-SS_ if present - base="$(echo "${base}" | sed -E 's/^[0-9]{4}-[0-9]{2}-[0-9]{2}_[0-9]{2}-[0-9]{2}-[0-9]{2}_//')" - echo "${base}" -} - -derive_exp_id_from_motion() { - local motion_file="$1" - echo "recon_body_$(derive_seq_name "${motion_file}")" -} - -# Ensure a motion_file path points at a robot_name=* partition (a directory that -# pyarrow can read as a parquet dataset). If the input already contains -# /robot_name=*, return it unchanged. Otherwise look under the path for a -# single robot_name=* subdir and append it. -# -# Why: pq.read_table() walks every file under the given path. If the path is -# the parent sequence_id=* dir, pyarrow tries to parse object/material.mtl, -# textured_mesh.obj, etc. as parquet and dies. -normalize_motion_file_to_robot_partition() { - local mf="${1%/}" - if [[ "${mf}" == */robot_name=* ]]; then - echo "${mf}" - return 0 - fi - local candidate_root="" - if [[ -d "${mf}" ]]; then - candidate_root="${mf}" - elif [[ -d "${REPO_ROOT}/${mf}" ]]; then - candidate_root="${REPO_ROOT}/${mf}" - fi - if [[ -z "${candidate_root}" ]]; then - log_warn "motion_file does not contain '/robot_name=*' and the directory is not present locally; leaving as-is: ${mf}" >&2 - echo "${mf}" - return 0 - fi - local matches=() - while IFS= read -r d; do - [[ -n "${d}" ]] && matches+=("${d}") - done < <(find "${candidate_root}" -mindepth 1 -maxdepth 1 -type d -name 'robot_name=*' -printf '%f\n' 2>/dev/null | sort) - if (( ${#matches[@]} == 0 )); then - die "motion_file '${mf}' has no robot_name=* subdirectory under ${candidate_root}; expected layout: /robot_name=" - fi - if (( ${#matches[@]} > 1 )); then - die "motion_file '${mf}' contains multiple robot_name=* subdirs (${matches[*]}); pass the specific one explicitly" - fi - echo "${mf}/${matches[0]}" -} - -config_path_for() { - echo "${EXPERIMENTS_DIR}/$1/config.yaml" -} - -# --- YAML helpers (Python inline) --- - -# Read a top-level scalar key (id, motion_file, run_name, ...) from a config. -yaml_get_top() { - local config="$1" key="$2" - python3 - "$config" "$key" <<'PY' -import sys, yaml -path, key = sys.argv[1], sys.argv[2] -with open(path) as f: - data = yaml.safe_load(f) or {} -val = data.get(key, "") -print("" if val is None else val) -PY -} - -# Read train_overrides. as a string. -yaml_get_override() { - local config="$1" key="$2" - python3 - "$config" "$key" <<'PY' -import sys, yaml -path, key = sys.argv[1], sys.argv[2] -with open(path) as f: - data = yaml.safe_load(f) or {} -overrides = data.get("train_overrides", {}) or {} -val = overrides.get(key, "") -print("" if val is None else val) -PY -} - -# Set the start/end frames in a config, preserving comments and key order via -# a surgical line-by-line edit (only the two frame lines are rewritten). -# A value of "-" for start/end keeps the existing line untouched. -# If the key is missing entirely, it is appended under train_overrides. -yaml_set_frames() { - local config="$1" start="$2" end="$3" - python3 - "$config" "$start" "$end" <<'PY' -import re, sys -path, start, end = sys.argv[1], sys.argv[2], sys.argv[3] -with open(path) as f: - lines = f.readlines() - -KEY_START = "env.commands.motion.motion_start_frame" -KEY_END = "env.commands.motion.motion_end_frame" - -def patch(lines, key, new_val): - pat = re.compile(rf"^(\s*){re.escape(key)}\s*:\s*([^#\n]*)(#.*)?$") - for i, line in enumerate(lines): - m = pat.match(line.rstrip("\n")) - if m: - indent = m.group(1) - trailing = m.group(3) or "" - sep = " " if trailing else "" - lines[i] = f"{indent}{key}: {new_val}{sep}{trailing}\n" - return True - return False - -def append_under_train_overrides(lines, key, new_val): - for i, line in enumerate(lines): - if line.startswith("train_overrides:"): - j = i + 1 - while j < len(lines) and (lines[j].startswith(" ") or lines[j].startswith("\t") or lines[j].strip() == ""): - j += 1 - insert_at = j - lines.insert(insert_at, f" {key}: {new_val}\n") - return True - lines.append("\ntrain_overrides:\n") - lines.append(f" {key}: {new_val}\n") - return True - -if start != "-": - if not patch(lines, KEY_START, start): - append_under_train_overrides(lines, KEY_START, start) -if end != "-": - if not patch(lines, KEY_END, end): - append_under_train_overrides(lines, KEY_END, end) - -with open(path, "w") as f: - f.writelines(lines) -PY -} - -# --- Container detection --- - -container_running() { - docker ps --format '{{.Names}}' 2>/dev/null | grep -qx "${CONTAINER_NAME}" -} - -# --- Stage: init --- - -cmd_init() { - local motion_file="" exp_id="" force=0 - while (( $# )); do - case "$1" in - --exp-id) exp_id="$2"; shift 2 ;; - --force) force=1; shift ;; - -*) die "init: unknown flag: $1" ;; - *) - if [[ -z "${motion_file}" ]]; then motion_file="$1"; else die "init: unexpected arg: $1"; fi - shift - ;; - esac - done - - [[ -n "${motion_file}" ]] || die "init: is required" - [[ -f "${TEMPLATE_CONFIG}" ]] || die "init: template config not found: ${TEMPLATE_CONFIG}" - - if [[ -z "${exp_id}" ]]; then - exp_id="$(derive_exp_id_from_motion "${motion_file}")" - fi - - # Normalize to a robot_name=* partition; pyarrow chokes on the parent dir. - local normalized_mf - normalized_mf="$(normalize_motion_file_to_robot_partition "${motion_file}")" - if [[ "${normalized_mf}" != "${motion_file}" ]]; then - log_info "Normalized motion_file: ${motion_file} -> ${normalized_mf}" - motion_file="${normalized_mf}" - fi - - local target_dir="${EXPERIMENTS_DIR}/${exp_id}" - local target_config="${target_dir}/config.yaml" - log_info "Initializing experiment '${exp_id}'" - log_info " motion_file: ${motion_file}" - log_info " config: ${target_config}" - - if [[ -e "${target_config}" && "${force}" -eq 0 ]]; then - die "init: ${target_config} already exists (use --force to overwrite)" - fi - - if (( DRY_RUN )); then - log_info "(dry-run) would create ${target_config}" - log_info "(dry-run) would ensure registry entry: ${exp_id}: ${exp_id}" - return 0 - fi - - mkdir -p "${target_dir}" - EXP_ID="${exp_id}" MOTION_FILE="${motion_file}" \ - python3 - "${TEMPLATE_CONFIG}" "${target_config}" <<'PY' -import os, sys, yaml -src, dst = sys.argv[1], sys.argv[2] -exp_id = os.environ["EXP_ID"] -motion_file = os.environ["MOTION_FILE"] -with open(src) as f: - data = yaml.safe_load(f) -data["id"] = exp_id -data["run_name"] = exp_id -data["description"] = f"ReconBody {exp_id} with SONIC JOINT_RESIDUAL" -data["motion_file"] = motion_file -overrides = data.setdefault("train_overrides", {}) -overrides["env.commands.motion.motion_start_frame"] = 0 -overrides["env.commands.motion.motion_end_frame"] = -1 -with open(dst, "w") as f: - f.write("# ReconBody: " + exp_id + " (TPV whole-body)\n") - f.write("#\n") - f.write("# Usage:\n") - f.write(f"# python robotic_grounding/experiments/run_experiment.py {exp_id} --local\n") - yaml.safe_dump(data, f, sort_keys=False, default_flow_style=False) -PY - log_info "Wrote ${target_config}" - - EXP_ID="${exp_id}" python3 - "${REGISTRY_FILE}" <<'PY' -import os, sys -exp_id = os.environ["EXP_ID"] -path = sys.argv[1] -with open(path) as f: - text = f.read() -needle = f"{exp_id}: {exp_id}" -if needle in text: - print(f"[INFO] registry already contains '{needle}', skipping") - sys.exit(0) -lines = text.splitlines() -inserted = False -out = [] -for line in lines: - out.append(line) - if not inserted and line.startswith("# Whole-body experiments:"): - out.append(needle) - inserted = True -if not inserted: - out.append("") - out.append("# Whole-body experiments:") - out.append(needle) -out.append("") -with open(path, "w") as f: - f.write("\n".join(line for line in out if line is not None)) -print(f"[INFO] added '{needle}' to {path}") -PY -} - -# --- Stage: replay --- - -cmd_replay() { - local exp_id="${1:-}" - [[ -n "${exp_id}" ]] || die "replay: is required" - local config; config="$(config_path_for "${exp_id}")" - [[ -f "${config}" ]] || die "replay: config not found: ${config}" - - local motion_file; motion_file="$(yaml_get_top "${config}" motion_file)" - [[ -n "${motion_file}" ]] || die "replay: motion_file is empty in ${config}" - - log_info "exp_id : ${exp_id}" - log_info "motion_file : ${motion_file}" - - if ! container_running; then - log_warn "container '${CONTAINER_NAME}' is not running." - log_warn "Start it first with: ${WORKFLOW_RUN_SH} start" - if (( DRY_RUN )); then - log_warn "(dry-run) continuing anyway to print the resolved command" - else - die "replay: container is not running" - fi - fi - - # NOTE: workflow/run.sh's `exec` case runs `shift 3` blindly, so the version - # and gpu placeholders MUST be passed positionally even if they are the - # defaults. Without them, $CONTAINER_NAME inside run.sh becomes - # "robotic-grounding----gpupython" and `docker exec` fails. - run_cmd "${WORKFLOW_RUN_SH} exec ${CONTAINER_VERSION} ${CONTAINER_GPU} -- python ${REPLAY_SCRIPT} --motion_file ${motion_file}" -} - -# --- Stage: set-frames --- - -cmd_set_frames() { - local exp_id="${1:-}" start="${2:-}" end="${3:-}" - [[ -n "${exp_id}" && -n "${start}" && -n "${end}" ]] || die "set-frames: usage: set-frames " - local config; config="$(config_path_for "${exp_id}")" - [[ -f "${config}" ]] || die "set-frames: config not found: ${config}" - - local cur_start cur_end - cur_start="$(yaml_get_override "${config}" env.commands.motion.motion_start_frame)" - cur_end="$(yaml_get_override "${config}" env.commands.motion.motion_end_frame)" - log_info "current motion_start_frame=${cur_start:-} motion_end_frame=${cur_end:-}" - log_info "new motion_start_frame=${start} motion_end_frame=${end} (- means keep current)" - - if (( DRY_RUN )); then - log_info "(dry-run) would update ${config}" - return 0 - fi - yaml_set_frames "${config}" "${start}" "${end}" - log_info "Updated ${config}" -} - -# --- Stage: submit --- - -cmd_submit() { - local exp_id="" run_name="" build_image=0 local_dry=0 - while (( $# )); do - case "$1" in - --build-image) build_image=1; shift ;; - --run-name) run_name="$2"; shift 2 ;; - --dry-run) local_dry=1; shift ;; - -*) die "submit: unknown flag: $1" ;; - *) - if [[ -z "${exp_id}" ]]; then exp_id="$1"; else die "submit: unexpected arg: $1"; fi - shift - ;; - esac - done - [[ -n "${exp_id}" ]] || die "submit: is required" - local config; config="$(config_path_for "${exp_id}")" - if [[ ! -f "${config}" ]]; then - if (( local_dry )) || (( DRY_RUN )); then - log_warn "config not found: ${config} (dry-run; continuing anyway)" - else - die "submit: config not found: ${config}" - fi - fi - - [[ -n "${run_name}" ]] || run_name="${exp_id}_motion_data" - - local cmd="python ${RUN_EXPERIMENT_SCRIPT} ${exp_id} --osmo --run-name ${run_name}" - if (( build_image )); then cmd+=" --build-image"; fi - - log_info "exp_id : ${exp_id}" - log_info "run_name : ${run_name}" - log_info "build_image: $(( build_image ))" - - if (( local_dry )) || (( DRY_RUN )); then - log_cmd "${cmd}" - log_info "(dry-run) skipping submission" - return 0 - fi - run_cmd "${cmd}" -} - -# --- Interactive driver: run --- - -cmd_run() { - local arg="" - while (( $# )); do - case "$1" in - -*) die "run: unknown flag: $1 (use --dry-run/--yes before the subcommand)" ;; - *) - if [[ -z "${arg}" ]]; then arg="$1"; else die "run: unexpected arg: $1"; fi - shift - ;; - esac - done - - if [[ -z "${arg}" ]]; then - arg="$(prompt_value "Enter exp_id or motion_file path" "")" - [[ -n "${arg}" ]] || die "run: nothing to do" - fi - - # Decide whether arg is a motion file path or an exp_id. - local exp_id="" motion_file="" - if [[ "${arg}" == */robot_name=* || -d "${REPO_ROOT}/${arg}" || -d "${arg}" ]]; then - motion_file="${arg}" - exp_id="$(derive_exp_id_from_motion "${motion_file}")" - log_info "Treating arg as motion_file path" - log_info " motion_file -> ${motion_file}" - log_info " derived exp_id -> ${exp_id}" - else - exp_id="${arg}" - log_info "Treating arg as exp_id: ${exp_id}" - fi - - local config; config="$(config_path_for "${exp_id}")" - - # ----- Stage 1: init ----- - log_stage "Stage 1/4: init [host]" - local needs_init=0 default_init="n" - if [[ ! -f "${config}" ]]; then - needs_init=1 - default_init="y" - log_info "Config does not exist yet: ${config}" - if [[ -z "${motion_file}" ]]; then - motion_file="$(prompt_value "Enter motion_file path for ${exp_id}" "")" - [[ -n "${motion_file}" ]] || die "init: motion_file is required when config does not exist" - fi - else - log_info "Config already exists: ${config}" - fi - - local ans; ans="$(prompt_yn "Run init for ${exp_id}?" "${default_init}")" - if [[ "${ans}" == "y" ]]; then - if [[ -z "${motion_file}" ]]; then - motion_file="$(yaml_get_top "${config}" motion_file)" - log_info "Using motion_file from existing config: ${motion_file}" - fi - local edited_exp_id; edited_exp_id="$(prompt_value "exp_id" "${exp_id}")" - exp_id="${edited_exp_id}" - config="$(config_path_for "${exp_id}")" - local force_args=() - if [[ -e "${config}" ]]; then - local overwrite; overwrite="$(prompt_yn "Overwrite existing ${config}?" "n")" - if [[ "${overwrite}" == "y" ]]; then force_args+=("--force"); else - log_info "Keeping existing config; skipping init." - fi - fi - if [[ ! -e "${config}" || ${#force_args[@]} -gt 0 ]]; then - cmd_init "${motion_file}" --exp-id "${exp_id}" "${force_args[@]+"${force_args[@]}"}" - fi - else - if (( needs_init )); then - die "run: cannot continue without a config (init was skipped)" - fi - log_info "init skipped" - fi - - # ----- Stage 2: replay ----- - log_stage "Stage 2/4: replay [host -> container via ./workflow/run.sh exec]" - local mfile="" - if [[ -f "${config}" ]]; then - mfile="$(yaml_get_top "${config}" motion_file)" - elif (( DRY_RUN )) && [[ -n "${motion_file}" ]]; then - mfile="${motion_file}" - log_info "(dry-run) using in-memory motion_file (config not yet created)" - fi - log_info "motion_file: ${mfile:-}" - if container_running; then - log_info "container '${CONTAINER_NAME}' is running" - else - log_warn "container '${CONTAINER_NAME}' is not running" - local start_it; start_it="$(prompt_yn "Start container with '${WORKFLOW_RUN_SH} start'?" "n")" - if [[ "${start_it}" == "y" ]]; then - log_warn "'./workflow/run.sh start' is interactive and drops you into a shell." - log_warn "Run it in another terminal, then come back and answer 'y' below." - fi - fi - ans="$(prompt_yn "Run replay (viser) for ${exp_id}?" "y")" - if [[ "${ans}" == "y" ]]; then - if [[ ! -f "${config}" ]]; then - log_warn "Skipping replay: ${config} not present (init was skipped or dry-run)" - elif container_running; then - cmd_replay "${exp_id}" - log_info "Replay finished. Note the start/end frames you want." - else - log_warn "Skipping replay: container is still not running" - fi - else - log_info "replay skipped — assuming frames are already chosen" - fi - - # ----- Stage 3: set-frames ----- - log_stage "Stage 3/4: set-frames [host]" - local cur_start="" cur_end="" - if [[ -f "${config}" ]]; then - cur_start="$(yaml_get_override "${config}" env.commands.motion.motion_start_frame)" - cur_end="$(yaml_get_override "${config}" env.commands.motion.motion_end_frame)" - log_info "current motion_start_frame=${cur_start:-} motion_end_frame=${cur_end:-}" - elif (( DRY_RUN )); then - log_info "(dry-run) config not present yet; defaulting current frames to 0/-1" - cur_start="0" - cur_end="-1" - fi - ans="$(prompt_yn "Update frame range in ${config}?" "y")" - if [[ "${ans}" == "y" ]]; then - local new_start new_end - new_start="$(prompt_value "motion_start_frame" "${cur_start:-0}")" - new_end="$(prompt_value "motion_end_frame (-1 = end of motion)" "${cur_end:--1}")" - if [[ -f "${config}" ]]; then - if (( DRY_RUN )); then - log_info "(dry-run) would update ${config} -> motion_start_frame=${new_start} motion_end_frame=${new_end}" - else - yaml_set_frames "${config}" "${new_start}" "${new_end}" - log_info "Updated ${config}" - fi - else - log_warn "Skipping set-frames: ${config} not present" - fi - else - log_info "set-frames skipped" - fi - - # ----- Stage 4: submit ----- - log_stage "Stage 4/4: submit [host -> OSMO; osmo CLI not in container]" - local default_run_name="${exp_id}_motion_data" - local run_name; run_name="$(prompt_value "OSMO --run-name" "${default_run_name}")" - local build_ans; build_ans="$(prompt_yn "Pass --build-image?" "n")" - local submit_args=("${exp_id}" --run-name "${run_name}") - if [[ "${build_ans}" == "y" ]]; then submit_args+=(--build-image); fi - local final_cmd="python ${RUN_EXPERIMENT_SCRIPT} ${exp_id} --osmo --run-name ${run_name}" - [[ "${build_ans}" == "y" ]] && final_cmd+=" --build-image" - log_info "Resolved command:" - log_cmd "${final_cmd}" - ans="$(prompt_yn "Submit now?" "y")" - if [[ "${ans}" == "y" ]]; then - cmd_submit "${submit_args[@]}" - else - log_info "submit skipped" - fi - - log_stage "Done" -} - -# --- Top-level CLI --- - -usage() { - sed -n '2,51p' "$0" -} - -# Banner shown at the top of every real invocation so it's obvious where the -# script runs. Suppressed for help/usage output to avoid duplicate text. -print_host_banner() { - if running_inside_container; then - echo "$(_color '1;31' '!!! Running INSIDE the container, but this script is host-only !!!')" - else - echo "$(_color '1;32' '[host]') train_tpv_wholebody_workflow.sh — running on host (not in container)" - fi -} - -# Abort if invoked from inside the container. Replay (docker exec) and submit -# (osmo CLI) cannot work from there. -require_host() { - if running_inside_container; then - log_error "This script must be run on the HOST, not inside the docker container." - log_error "Detected container indicators: /.dockerenv or docker cgroup." - log_error "" - log_error "Why: 'replay' shells out to './workflow/run.sh exec' (host-only docker" - log_error "command), and 'submit' calls the OSMO CLI which is not installed in" - log_error "the container image." - log_error "" - log_error "Fix: open a host shell (exit the container) and run again from:" - log_error " ${REPO_ROOT}" - exit 2 - fi -} - -parse_global_flags() { - local out=() - for tok in "$@"; do - case "${tok}" in - --dry-run) DRY_RUN=1 ;; - --yes|-y) ASSUME_YES=1 ;; - *) out+=("${tok}") ;; - esac - done - REMAINING_ARGS=("${out[@]+"${out[@]}"}") -} - -main() { - if (( $# == 0 )); then - usage - exit 1 - fi - REMAINING_ARGS=() - parse_global_flags "$@" - set -- "${REMAINING_ARGS[@]+"${REMAINING_ARGS[@]}"}" - if (( $# == 0 )); then - usage - exit 1 - fi - local sub="$1"; shift - case "${sub}" in - -h|--help|help) usage; exit 0 ;; - esac - require_host - print_host_banner - case "${sub}" in - run) cmd_run "$@" ;; - init) cmd_init "$@" ;; - replay) cmd_replay "$@" ;; - set-frames) cmd_set_frames "$@" ;; - submit) cmd_submit "$@" ;; - *) log_error "unknown subcommand: ${sub}"; usage; exit 1 ;; - esac -} - -main "$@" diff --git a/robotic_grounding/scripts/upload_object_assets.py b/robotic_grounding/scripts/upload_object_assets.py deleted file mode 100644 index e4cc2c64..00000000 --- a/robotic_grounding/scripts/upload_object_assets.py +++ /dev/null @@ -1,123 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Upload a dataset's object assets (rigid URDFs + meshes) to CSS/swift. - -The reconstruction ``v2d__load`` workflow fetches per-dataset object -assets from swift at runtime (the loader image stays lean and bakes nothing). It -expects them under:: - - .../human_motion_data//object_assets/urdfs// - .../human_motion_data//object_assets/meshes// - -This walks the committed ``assets/{urdfs,meshes}/`` trees and ``aws s3 -sync``s them there, preserving the ``../../meshes/...`` sibling refs inside the -URDFs (urdfs//x.urdf -> ../../meshes//x.stl). - -Only works for datasets whose meshes are committed (arctic, hot3d, taco, -oakink2). h2o/grab/dexycb keep object meshes inside the raw dataset on CSS and -are handled by the load workflow's mesh_dir pointing at the raw tree instead. - -Usage:: - - source scripts/setup_css_env.sh # or have aws creds for pdx.s8k.io - python scripts/upload_object_assets.py --dataset taco - python scripts/upload_object_assets.py --dataset taco --dry-run -""" - -from __future__ import annotations - -import argparse -import subprocess -import sys -from pathlib import Path - -ASSET_DIR = ( - Path(__file__).resolve().parents[1] - / "source/robotic_grounding/robotic_grounding/assets" -) -# TODO(public-release): internal NVIDIA CSS endpoint + bucket — replace before open-sourcing. -ENDPOINT = "https://pdx.s8k.io" -BUCKET = "s3://datasets/v2d/human_motion_data" -# Git-LFS pointer files are ~130 bytes of text beginning with this line; a sync -# of un-pulled pointers would silently upload stubs, so we refuse. -_LFS_MAGIC = b"version https://git-lfs" - - -# Geometry the loaders actually load as the object mesh. An un-pulled pointer -# here breaks the load, so we hard-fail. Other extensions (auxiliary .json, -# unused raw .ply scans like oakink2's) don't, so they're skipped — not blocked. -_PRIMARY_MESH_EXTS = {".obj", ".stl", ".glb"} - - -def _find_primary_stubs(root: Path) -> list[Path]: - stubs = [] - for p in root.rglob("*"): - if ( - p.is_file() - and p.suffix.lower() in _PRIMARY_MESH_EXTS - and p.stat().st_size < 300 - ): - with open(p, "rb") as f: - if f.read(len(_LFS_MAGIC)) == _LFS_MAGIC: - stubs.append(p) - return stubs - - -def _sync(src: Path, dst: str, dry_run: bool, extra: list[str] | None = None) -> None: - cmd = [ - "aws", - "s3", - "sync", - str(src), - dst, - "--endpoint-url", - ENDPOINT, - "--region", - "us-east-1", - ] + (extra or []) - print("+", " ".join(cmd)) - if not dry_run: - subprocess.run(cmd, check=True) - - -def main() -> None: - """Sync the given dataset's object_assets (urdfs + meshes) to swift.""" - ap = argparse.ArgumentParser(description=__doc__) - ap.add_argument("--dataset", required=True) - ap.add_argument("--dry-run", action="store_true") - args = ap.parse_args() - ds = args.dataset - - any_uploaded = False - for kind in ("urdfs", "meshes"): - src = ASSET_DIR / kind / ds - if not src.is_dir(): - print(f"WARNING: {src} missing — skipping {kind}/", file=sys.stderr) - continue - stubs = _find_primary_stubs(src) - if stubs: - print( - f"ERROR: {len(stubs)} un-pulled Git-LFS pointer(s) for primary " - f"geometry under {src} (e.g. {stubs[0]}). Run `git lfs pull` first.", - file=sys.stderr, - ) - sys.exit(1) - dst = f"{BUCKET}/{ds}/object_assets/{kind}/{ds}/" - # Skip un-pulled / unused .ply (e.g. oakink2 raw scans) — uploading 130B - # LFS-pointer stubs would just litter the bucket; loaders read .obj/.stl/.glb. - extra = ["--exclude", "*.ply"] if kind == "meshes" else [] - _sync(src, dst, args.dry_run, extra) - any_uploaded = True - - if not any_uploaded: - print( - f"ERROR: no urdfs/ or meshes/ found for '{ds}' under {ASSET_DIR}", - file=sys.stderr, - ) - sys.exit(1) - print(f"done: object_assets for '{ds}'{' (dry-run)' if args.dry_run else ''}") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/validate_training_assets.py b/robotic_grounding/scripts/validate_training_assets.py deleted file mode 100644 index 00b35477..00000000 --- a/robotic_grounding/scripts/validate_training_assets.py +++ /dev/null @@ -1,183 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -r"""Validate that all assets required for RL training exist on disk. - -Run this BEFORE starting Isaac Sim to catch missing URDFs, meshes, or -malformed parquets early — rather than 45 seconds into GPU startup. - -Usage:: - - # Validate a single motion file - python scripts/validate_training_assets.py \\ - --motion_file hot3d/hot3d_processed/P0001_4bf4e21a/sharpa_wave - - # Validate all processed sequences for a dataset - python scripts/validate_training_assets.py --dataset hot3d -""" - -from __future__ import annotations - -import argparse -import sys -from pathlib import Path - -import pyarrow.parquet as pq - -# Add source to path -REPO_ROOT = Path(__file__).resolve().parent.parent -sys.path.insert(0, str(REPO_ROOT / "source" / "robotic_grounding")) - -from robotic_grounding.assets import ASSET_DIR # noqa: E402 -from robotic_grounding.retarget.dataset_registry import ( # noqa: E402 - get_all_dataset_names, - get_dataset_config, -) - -HUMAN_MOTION_DATA_DIR = Path(ASSET_DIR) / "human_motion_data" - - -def validate_motion_file(motion_file: str) -> list[str]: - """Validate a single motion file (parquet partition directory). - - Returns a list of error strings. Empty list means all assets are valid. - """ - errors: list[str] = [] - - # Resolve the path - path = Path(motion_file) - if not path.is_absolute(): - # Try as relative to HUMAN_MOTION_DATA_DIR - parts = motion_file.strip("/").split("/") - if len(parts) >= 4: - dataset, dataset_retargeted, seq_id, robot = parts[:4] - path = ( - HUMAN_MOTION_DATA_DIR - / dataset - / dataset_retargeted - / f"sequence_id={seq_id}" - / f"robot_name={robot}" - ) - - if not path.exists(): - errors.append(f"Motion file not found: {path}") - return errors - - # Find parquet files - parquet_files = list(path.glob("*.parquet")) if path.is_dir() else [path] - if not parquet_files: - errors.append(f"No parquet files in {path}") - return errors - - # Read and validate - try: - data = pq.read_table(str(parquet_files[0])).to_pydict() - except Exception as e: - errors.append(f"Failed to read parquet {parquet_files[0]}: {e}") - return errors - - # Check URDF paths - urdf_paths = data.get("object_urdf_paths", [[]])[0] or [] - mesh_paths = data.get("object_mesh_paths", [[]])[0] or [] - - for p in urdf_paths: - if p and not Path(p).exists(): - errors.append(f"Missing URDF: {p}") - - for p in mesh_paths: - if p and not Path(p).exists(): - errors.append(f"Missing mesh: {p}") - - # Note: objects without explicit urdf_paths may be resolved at training - # time via the object registry or the mesh-derived URDF fallback in - # SceneConfig._build_scene_objects. We only flag paths that ARE in the - # parquet but point to missing files. - - return errors - - -def validate_dataset(dataset: str) -> dict[str, list[str]]: - """Validate all processed sequences for a dataset. - - Returns a dict mapping sequence_id to list of errors (empty = valid). - """ - config = get_dataset_config(dataset) - processed_dir = ( - HUMAN_MOTION_DATA_DIR / config.name / f"{config.name}{config.processed_suffix}" - ) - - if not processed_dir.exists(): - return {"__dataset__": [f"Processed directory not found: {processed_dir}"]} - - results: dict[str, list[str]] = {} - seq_dirs = sorted(processed_dir.glob("sequence_id=*/robot_name=*")) - - if not seq_dirs: - return {"__dataset__": [f"No sequences found in {processed_dir}"]} - - for seq_dir in seq_dirs: - seq_id = seq_dir.parent.name.replace("sequence_id=", "") - errors = validate_motion_file(str(seq_dir)) - if errors: - results[seq_id] = errors - - return results - - -def main() -> None: - """CLI entry point.""" - parser = argparse.ArgumentParser( - description="Validate training assets (URDFs, meshes, parquets)." - ) - parser.add_argument( - "--motion_file", - type=str, - help="Validate a single motion file path.", - ) - parser.add_argument( - "--dataset", - type=str, - choices=list(get_all_dataset_names()) + ["all"], - help="Validate all processed sequences for a dataset.", - ) - args = parser.parse_args() - - if not args.motion_file and not args.dataset: - parser.error("Provide --motion_file or --dataset") - - total_errors = 0 - - if args.motion_file: - errors = validate_motion_file(args.motion_file) - if errors: - print(f"FAIL: {args.motion_file}") - for e in errors: - print(f" - {e}") - total_errors += len(errors) - else: - print(f"OK: {args.motion_file}") - - if args.dataset: - datasets = ( - list(get_all_dataset_names()) if args.dataset == "all" else [args.dataset] - ) - for ds in datasets: - results = validate_dataset(ds) - if not results: - print(f"OK: {ds} (all sequences valid)") - else: - for seq_id, errors in results.items(): - print(f"FAIL: {ds}/{seq_id}") - for e in errors: - print(f" - {e}") - total_errors += len(errors) - - if total_errors: - print(f"\n{total_errors} error(s) found.") - sys.exit(1) - else: - print("\nAll assets valid.") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/scripts/view_scene.py b/robotic_grounding/scripts/view_scene.py deleted file mode 100644 index c293be03..00000000 --- a/robotic_grounding/scripts/view_scene.py +++ /dev/null @@ -1,141 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -r"""View a scene: spawns object, optional support surface, and robot from Parquet (no RL). - -Robot articulation is added when the motion path includes a ``robot_name=...`` -partition and that name exists in ``assets/robot_registry.py`` (e.g. ``g1``). - -Usage (Sharpa / Arctic-style Parquet under ``human_motion_data``): - python scripts/view_scene.py \ - --motion_file source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic_processed/sequence_id=arctic_s01_ketchup_use_01/robot_name=sharpa_wave - -SOMA→G1 retargeted data (after ``soma_to_g1.py --save``): - # Save motion first (writes under HUMAN_MOTION_DATA_DIR/whole_body/soma/...) - python scripts/retarget/soma_to_g1.py --save - - # Point at the partition directory (sequence_id=.../robot_name=g1) or use cwd-relative path - python scripts/view_scene.py \ - --motion_file source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=/robot_name=g1 - - # Headless (validate without GUI) - python scripts/view_scene.py --motion_file --headless - - # Record MP4 video - python scripts/view_scene.py --motion_file --headless \\ - --record_video --output_dir /tmp/videos --video_length 300 -""" - -import argparse -import os - -from isaaclab.app import AppLauncher - -parser = argparse.ArgumentParser(description="View a scene from a motion file.") -parser.add_argument( - "--motion_file", type=str, required=True, help="Path to parquet motion dir." -) -parser.add_argument("--num_envs", type=int, default=1, help="Number of environments.") -parser.add_argument( - "--record_video", - action="store_true", - default=False, - help="Record the scene replay as MP4 video.", -) -parser.add_argument( - "--output_dir", - type=str, - default=None, - help="Directory to save recorded video (required with --record_video).", -) -parser.add_argument( - "--video_length", - type=int, - default=300, - help="Number of simulation steps to record (default: 300).", -) -AppLauncher.add_app_launcher_args(parser) -args_cli = parser.parse_args() - -# Enable cameras when recording video (must be set before AppLauncher) -if args_cli.record_video: - args_cli.enable_cameras = True - if not args_cli.output_dir: - raise ValueError("--output_dir is required when using --record_video") - -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -import gymnasium as gym # noqa: E402 -import torch # noqa: E402 -from isaaclab.envs import ManagerBasedRLEnv # noqa: E402 -from robotic_grounding.tasks.scene_utils.scene_viewer_env_cfg import ( # noqa: E402 - SceneViewerEnvCfg, -) - -# Register for gym.make() + RecordVideo support -gym.register( - id="SceneViewer-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, -) - - -def main() -> None: - """View a scene from a motion file, optionally recording video.""" - cfg = SceneViewerEnvCfg(motion_file=args_cli.motion_file) - cfg.scene.num_envs = args_cli.num_envs - - if args_cli.record_video: - os.makedirs(args_cli.output_dir, exist_ok=True) - - # Use gym.make so RecordVideo can wrap it - env = gym.make( - "SceneViewer-v0", - cfg=cfg, - render_mode="rgb_array", - ) - env = gym.wrappers.RecordVideo( - env, - video_folder=args_cli.output_dir, - step_trigger=lambda step: step == 0, - video_length=args_cli.video_length, - disable_logger=True, - ) - env.reset() - - actions = torch.zeros(env.unwrapped.num_envs, 0, device=env.unwrapped.device) - - print(f"[INFO] Scene loaded from: {args_cli.motion_file}") - print( - f"[INFO] Recording {args_cli.video_length} steps to {args_cli.output_dir}" - ) - for step in range(args_cli.video_length): - env.step(actions) - if (step + 1) % 100 == 0: - print(f"[INFO] Step: {step + 1}/{args_cli.video_length}") - print(f"[INFO] Video saved to {args_cli.output_dir}") - - else: - # Interactive / headless mode — no gym wrapper needed - env = ManagerBasedRLEnv(cfg=cfg) - env.reset() - - actions = torch.zeros(env.num_envs, 0, device=env.device) - - print(f"[INFO] Scene loaded from: {args_cli.motion_file}") - print("[INFO] Close the window to exit.") - - step = 0 - while simulation_app.is_running(): - env.step(actions) - step += 1 - if step % 100 == 0: - print(f"[INFO] Step: {step}") - env.reset() - - env.close() - - -if __name__ == "__main__": - main() - simulation_app.close() diff --git a/robotic_grounding/scripts/vis_wrench_support.py b/robotic_grounding/scripts/vis_wrench_support.py deleted file mode 100644 index ced1a599..00000000 --- a/robotic_grounding/scripts/vis_wrench_support.py +++ /dev/null @@ -1,254 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import argparse -import threading -import time -from dataclasses import dataclass -from typing import Any - -import numpy as np -import torch -import trimesh -import viser - -# --------------------------------------------------------------------------- -# Pure helpers -# --------------------------------------------------------------------------- - - -def compute_faces(basis_dirs: np.ndarray) -> np.ndarray: - """Compute triangulation from the angular arrangement of 3D basis directions.""" - norms = np.linalg.norm(basis_dirs, axis=1, keepdims=True).clip(1e-8) - unit_dirs = basis_dirs / norms - hull = trimesh.convex.convex_hull(unit_dirs) - return hull.faces - - -def build_mesh( - supports: np.ndarray, basis_dirs: np.ndarray, faces: np.ndarray -) -> trimesh.Trimesh: - """Build mesh with vertices at supports * basis_dirs using precomputed triangulation.""" - vertices = supports[:, None] * basis_dirs # (N, 3) - return trimesh.Trimesh(vertices=vertices, faces=faces, process=False) - - -def make_basis_segments(basis_dirs: np.ndarray, scale: float = 0.3) -> np.ndarray: - """Create line segments from origin to each basis direction.""" - n = basis_dirs.shape[0] - points = np.zeros((n, 2, 3), dtype=np.float32) - points[:, 1, :] = basis_dirs * scale - return points - - -# --------------------------------------------------------------------------- -# Data container -# --------------------------------------------------------------------------- - - -@dataclass -class WrenchSupportData: - """Parsed wrench-support arrays ready for visualisation.""" - - force_basis: np.ndarray # (N, 3) - torque_basis: np.ndarray # (N, 3) - force_faces: np.ndarray - torque_faces: np.ndarray - sides: dict[str, np.ndarray] # side_name -> supports (H, N) - - @property - def num_timesteps(self) -> int: - """Return the number of timesteps from the first side's support array.""" - return next(iter(self.sides.values())).shape[0] - - @staticmethod - def from_pt(path: str, body_idx: int = 0) -> WrenchSupportData: - """Load wrench support data from a ``.pt`` file. - - Args: - path: Path to the ``.pt`` file. - body_idx: Which body index to slice from the (H, nb, N) arrays. - """ - data = torch.load(path) - basis_np = data["basis"][body_idx].cpu().numpy() # (N, 6) - force_basis = basis_np[:, :3] - torque_basis = basis_np[:, 3:] - - sides = { - "right": data["right_contact_wrench_supports"][:, body_idx].cpu().numpy(), - "left": data["left_contact_wrench_supports"][:, body_idx].cpu().numpy(), - } - return WrenchSupportData( - force_basis=force_basis, - torque_basis=torque_basis, - force_faces=compute_faces(force_basis), - torque_faces=compute_faces(torque_basis), - sides=sides, - ) - - -# --------------------------------------------------------------------------- -# Visualiser -# --------------------------------------------------------------------------- - - -class WrenchSupportVisualizer: - """Reusable viser-based visualiser for contact wrench supports.""" - - def __init__( - self, - server: viser.ViserServer, - data: WrenchSupportData, - *, - force_color: tuple[int, int, int] = (0, 255, 255), - torque_color: tuple[int, int, int] = (255, 215, 0), - force_x_offset: float = 0.0, - torque_x_offset: float = 2.0, - side_z_gap: float = 1.5, - scene_prefix: str = "/wrench", - ) -> None: - """Initialise the visualiser and populate the scene.""" - self._server = server - self._data = data - self._force_color = force_color - self._torque_color = torque_color - self._force_x = force_x_offset - self._torque_x = torque_x_offset - self._prefix = scene_prefix - self._mesh_handles: dict[str, Any] = {} - self._playing = threading.Event() - - # Assign z-offsets per side, symmetrically around zero. - side_names = list(data.sides.keys()) - n_sides = len(side_names) - self._side_z: dict[str, float] = {} - for i, name in enumerate(side_names): - self._side_z[name] = side_z_gap * (1 - 2 * i / max(n_sides - 1, 1)) - - self._add_static_scene() - self._add_gui() - self.update_frame(0) - - # -- static scene elements ------------------------------------------------ - - def _add_static_scene(self) -> None: - """Add reference frames, labels, and basis direction lines.""" - for side, z in self._side_z.items(): - for kind, x_off, basis_dirs in [ - ("force", self._force_x, self._data.force_basis), - ("torque", self._torque_x, self._data.torque_basis), - ]: - pos = (x_off, 0.0, z) - self._server.scene.add_frame( - f"{self._prefix}/{side}_{kind}_origin", - position=pos, - axes_length=0.2, - axes_radius=0.01, - ) - self._server.scene.add_label( - f"{self._prefix}/{side}_{kind}_label", - text=f"{side.title()} {kind.title()}", - position=(x_off, 0.15, z), - ) - self._server.scene.add_line_segments( - f"{self._prefix}/{side}_{kind}_basis", - points=make_basis_segments(basis_dirs), - colors=(200, 200, 200), - position=pos, - ) - - # -- dynamic mesh update -------------------------------------------------- - - def update_frame(self, t: int) -> None: - """Update the displayed wrench support meshes for timestep *t*.""" - for handle in self._mesh_handles.values(): - handle.remove() - self._mesh_handles.clear() - - for side, supports_all in self._data.sides.items(): - supports = supports_all[t] - z = self._side_z[side] - - force_mesh = build_mesh( - supports, self._data.force_basis, self._data.force_faces - ) - self._mesh_handles[f"{side}_force"] = self._server.scene.add_mesh_simple( - name=f"{self._prefix}/{side}/force", - vertices=np.array(force_mesh.vertices, dtype=np.float32), - faces=np.array(force_mesh.faces, dtype=np.uint32), - color=self._force_color, - opacity=1.0, - position=(self._force_x, 0.0, z), - ) - - torque_mesh = build_mesh( - supports, self._data.torque_basis, self._data.torque_faces - ) - self._mesh_handles[f"{side}_torque"] = self._server.scene.add_mesh_simple( - name=f"{self._prefix}/{side}/torque", - vertices=np.array(torque_mesh.vertices, dtype=np.float32), - faces=np.array(torque_mesh.faces, dtype=np.uint32), - color=self._torque_color, - opacity=1.0, - position=(self._torque_x, 0.0, z), - ) - - # -- GUI ------------------------------------------------------------------ - - def _add_gui(self) -> None: - """Wire up the timestep slider and play/pause button.""" - h = self._data.num_timesteps - self._time_slider = self._server.gui.add_slider( - label="Timestep", min=0, max=h - 1, step=1, initial_value=0 - ) - - @self._time_slider.on_update - def _on_slider_update(_: Any) -> None: - self.update_frame(int(self._time_slider.value)) - - play_button = self._server.gui.add_button(label="Play") - - @play_button.on_click - def _on_play_click(_: Any) -> None: - if self._playing.is_set(): - self._playing.clear() - else: - self._playing.set() - - threading.Thread(target=self._playback_loop, daemon=True).start() - - def _playback_loop(self) -> None: - """Advance the timestep slider continuously while playing.""" - h = self._data.num_timesteps - while True: - if self._playing.is_set(): - t = (int(self._time_slider.value) + 1) % h - self._time_slider.value = t - self.update_frame(t) - time.sleep(0.05) - - -# --------------------------------------------------------------------------- -# CLI entry point -# --------------------------------------------------------------------------- - - -def main() -> None: - """Launch the wrench support visualiser from a ``.pt`` data file.""" - parser = argparse.ArgumentParser(description="Visualise contact wrench supports.") - parser.add_argument( - "--data", type=str, default="data.pt", help="Path to the .pt data file." - ) - args = parser.parse_args() - - server = viser.ViserServer() - data = WrenchSupportData.from_pt(args.data) - WrenchSupportVisualizer(server, data) - - while True: - time.sleep(1.0) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/source/robotic_grounding/config/extension.toml b/robotic_grounding/source/robotic_grounding/config/extension.toml deleted file mode 100644 index af6e48b3..00000000 --- a/robotic_grounding/source/robotic_grounding/config/extension.toml +++ /dev/null @@ -1,26 +0,0 @@ -[package] - -# Semantic Versioning is used: https://semver.org/ -version = "0.1.0" - -# Description -category = "isaaclab" -readme = "README.md" - -title = "robotic_grounding" -author = "Xinghao Zhu" -maintainer = "Xinghao Zhu" -description="Training code for Video to Policy." -repository = "https://github.com/nvidia-isaac/video_to_policy" -keywords = ["Video to Policy", "RL", "isaaclab"] - -[dependencies] -"isaaclab" = {} -"isaaclab_assets" = {} -"isaaclab_mimic" = {} -"isaaclab_rl" = {} -"isaaclab_tasks" = {} -# NOTE: Add additional dependencies here - -[[python.module]] -name = "robotic_grounding" diff --git a/robotic_grounding/source/robotic_grounding/pyproject.toml b/robotic_grounding/source/robotic_grounding/pyproject.toml deleted file mode 100644 index 6bbbe71f..00000000 --- a/robotic_grounding/source/robotic_grounding/pyproject.toml +++ /dev/null @@ -1,8 +0,0 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers. -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -[build-system] -requires = ["setuptools", "wheel", "toml"] -build-backend = "setuptools.build_meta" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/__init__.py deleted file mode 100644 index aa92d4a2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/__init__.py +++ /dev/null @@ -1,16 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import os - -# Base asset directory -ASSET_DIR = os.path.abspath(os.path.dirname(__file__)) - -# Asset subdirectories -MOTION_ASSET_DIR = os.path.join(ASSET_DIR, "motion_data") -OBJECTS_ASSET_DIR = os.path.join(ASSET_DIR, "objects") -POLICY_ASSET_DIR = os.path.join(ASSET_DIR, "policies") - -# Scene config directory -SCENE_CONFIG_DIR = os.path.join( - os.path.dirname(ASSET_DIR), "tasks", "scene_utils", "config" -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/__init__.py deleted file mode 100644 index 515ebac7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Actuator configs and implementations used by robot assets.""" - -from .delayed_implicit_actuator import ( - DelayedImplicitActuator, - DelayedImplicitActuatorCfg, -) - -__all__ = [ - "DelayedImplicitActuator", - "DelayedImplicitActuatorCfg", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/delayed_implicit_actuator.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/delayed_implicit_actuator.py deleted file mode 100644 index d6248a2d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/actuators/delayed_implicit_actuator.py +++ /dev/null @@ -1,108 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from collections.abc import Sequence - -import torch -from isaaclab.actuators import ImplicitActuator, ImplicitActuatorCfg -from isaaclab.utils import DelayBuffer, configclass -from isaaclab.utils.types import ArticulationActions - - -class DelayedImplicitActuator(ImplicitActuator): - """Ideal PD actuator with delayed command application. - - This class extends the :class:`IdealPDActuator` class by adding a delay to the actuator commands. The delay - is implemented using a circular buffer that stores the actuator commands for a certain number of physics steps. - The most recent actuation value is pushed to the buffer at every physics step, but the final actuation value - applied to the simulation is lagged by a certain number of physics steps. - - The amount of time lag is configurable and can be set to a random value between the minimum and maximum time - lag bounds at every reset. The minimum and maximum time lag values are set in the configuration instance passed - to the class. - """ - - cfg: DelayedImplicitActuatorCfg - """The configuration for the actuator model.""" - - def __init__( - self, - cfg: DelayedImplicitActuatorCfg, - *args: object, - **kwargs: object, - ) -> None: - """Initialize delay buffers for positions, velocities, and efforts.""" - super().__init__(cfg, *args, **kwargs) - # instantiate the delay buffers - self.positions_delay_buffer = DelayBuffer( - cfg.max_delay, self._num_envs, device=self._device - ) - self.velocities_delay_buffer = DelayBuffer( - cfg.max_delay, self._num_envs, device=self._device - ) - self.efforts_delay_buffer = DelayBuffer( - cfg.max_delay, self._num_envs, device=self._device - ) - # all of the envs - self._ALL_INDICES = torch.arange( - self._num_envs, dtype=torch.long, device=self._device - ) - - def reset(self, env_ids: Sequence[int]) -> None: - """Reset delay buffers and draw new random time lags for the given envs.""" - super().reset(env_ids) - # number of environments (since env_ids can be a slice) - if env_ids is None or env_ids == slice(None): - num_envs = self._num_envs - else: - num_envs = len(env_ids) - # set a new random delay for environments in env_ids - time_lags = torch.randint( - low=self.cfg.min_delay, - high=self.cfg.max_delay + 1, - size=(num_envs,), - dtype=torch.int, - device=self._device, - ) - # set delays - self.positions_delay_buffer.set_time_lag(time_lags, env_ids) - self.velocities_delay_buffer.set_time_lag(time_lags, env_ids) - self.efforts_delay_buffer.set_time_lag(time_lags, env_ids) - # reset buffers - self.positions_delay_buffer.reset(env_ids) - self.velocities_delay_buffer.reset(env_ids) - self.efforts_delay_buffer.reset(env_ids) - - def compute( - self, - control_action: ArticulationActions, - joint_pos: torch.Tensor, - joint_vel: torch.Tensor, - ) -> ArticulationActions: - """Apply delayed setpoints and return updated articulation actions.""" - # apply delay based on the delay the model for all the setpoints - control_action.joint_positions = self.positions_delay_buffer.compute( - control_action.joint_positions - ) - control_action.joint_velocities = self.velocities_delay_buffer.compute( - control_action.joint_velocities - ) - control_action.joint_efforts = self.efforts_delay_buffer.compute( - control_action.joint_efforts - ) - # compte actuator model - return super().compute(control_action, joint_pos, joint_vel) - - -@configclass -class DelayedImplicitActuatorCfg(ImplicitActuatorCfg): - """Configuration for a delayed PD actuator.""" - - class_type: type = DelayedImplicitActuator - - min_delay: int = 0 - """Minimum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" - - max_delay: int = 0 - """Maximum number of physics time-steps with which the actuator command may be delayed. Defaults to 0.""" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/articulated_object.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/articulated_object.py deleted file mode 100644 index b9c0d703..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/articulated_object.py +++ /dev/null @@ -1,64 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import math - -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets.articulation import ArticulationCfg - -from robotic_grounding.assets import ASSET_DIR - -object_name = "box" - -ARTICULATED_OBJECT_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - asset_path=f"{ASSET_DIR}/objects/arctic/{object_name}.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - enable_gyroscopic_forces=False, - linear_damping=0.01, - angular_damping=0.01, - max_linear_velocity=1000.0, - max_angular_velocity=64 / math.pi * 180.0, - max_depenetration_velocity=1.0, - max_contact_impulse=1e3, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, - sleep_threshold=0.005, - stabilization_threshold=0.0005, - ), - collision_props=sim_utils.CollisionPropertiesCfg( - contact_offset=0.0, - rest_offset=0.0, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - collider_type="convex_decomposition", - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos={".*": 0.0}, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=1.0, - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - effort_limit_sim={".*": 75.0}, - velocity_limit_sim={".*": 15.0}, - stiffness={".*": 0.0}, - damping={".*": 0.0}, - armature={".*": 0.01}, - friction={".*": 0.01}, - ), - }, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/body_models/soma/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/body_models/soma/.gitignore deleted file mode 100644 index 25e7e6b8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/body_models/soma/.gitignore +++ /dev/null @@ -1,11 +0,0 @@ -# SOMA-X body-model assets are large binary blobs (~822 MB total) that -# users download on first use via `python scripts/setup_soma_assets.py`, -# which delegates to `soma.SOMALayer` (HuggingFace-backed). Keep the -# directory pinned in git so `read_soma._resolve_data_root` can find it, -# but never commit the assets themselves. -*.npz -*.pt -*.npy -*.obj -MHR/ -!.gitignore diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/g1.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/g1.py deleted file mode 100644 index e152550a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/g1.py +++ /dev/null @@ -1,1394 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""G1 robot configuration.""" - -import os - -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets.articulation import ArticulationCfg - -from robotic_grounding.assets import ASSET_DIR -from robotic_grounding.assets.actuators import DelayedImplicitActuatorCfg - -ARMATURE_5020 = 0.003609725 -ARMATURE_7520_14 = 0.010177520 -ARMATURE_7520_22 = 0.025101925 -ARMATURE_4010 = 0.00425 - -NATURAL_FREQ = 10 * 2.0 * 3.1415926535 # 10Hz -DAMPING_RATIO = 2.0 - -STIFFNESS_5020 = ARMATURE_5020 * NATURAL_FREQ**2 -STIFFNESS_7520_14 = ARMATURE_7520_14 * NATURAL_FREQ**2 -STIFFNESS_7520_22 = ARMATURE_7520_22 * NATURAL_FREQ**2 -STIFFNESS_4010 = ARMATURE_4010 * NATURAL_FREQ**2 - -DAMPING_5020 = 2.0 * DAMPING_RATIO * ARMATURE_5020 * NATURAL_FREQ -DAMPING_7520_14 = 2.0 * DAMPING_RATIO * ARMATURE_7520_14 * NATURAL_FREQ -DAMPING_7520_22 = 2.0 * DAMPING_RATIO * ARMATURE_7520_22 * NATURAL_FREQ -DAMPING_4010 = 2.0 * DAMPING_RATIO * ARMATURE_4010 * NATURAL_FREQ - -# Dex3-1 manual: https://marketing.unitree.com/article/en/Dex3-1/User_Manual.html -ARMATURE_1515 = 0.00149 -STIFFNESS_1515 = 2.0 -DAMPING_1515 = 0.2 -EFFORT_LIMIT_1515 = 0.76 -VELOCITY_LIMIT_1515 = 23.0 - -G1_CYLINDER_DEX_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 88.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 32.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_14, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_14, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_14, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": ImplicitActuatorCfg( - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": ImplicitActuatorCfg( - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": ImplicitActuatorCfg( - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) -asset_abs_path = os.path.abspath( - "groot/rl/data/robots/g1/g1_29dof_rev_1_0/g1_29dof_rev_1_0.usd" -) -G1_CYLINDER_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main_nodex.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - # spawn=sim_utils.UsdFileCfg( - # usd_path=asset_abs_path, - # activate_contact_sensors=True, - # rigid_props=sim_utils.RigidBodyPropertiesCfg( - # disable_gravity=False, - # retain_accelerations=False, - # linear_damping=0.0, - # angular_damping=0.0, - # max_linear_velocity=1000.0, - # max_angular_velocity=1000.0, - # max_depenetration_velocity=1.0, - # ), - # articulation_props=sim_utils.ArticulationRootPropertiesCfg( - # enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4 - # ), - # ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 88.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 32.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_14, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_14, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_14, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": ImplicitActuatorCfg( - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": ImplicitActuatorCfg( - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": ImplicitActuatorCfg( - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) - - -G1_CYLINDER_MODEL_12_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main_nodex.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 139.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 20.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_22, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_22, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_22, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": ImplicitActuatorCfg( - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": ImplicitActuatorCfg( - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": ImplicitActuatorCfg( - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) - -G1_CYLINDER_MODEL_12_DEX_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - # spawn=sim_utils.UsdFileCfg( - # usd_path=os.path.abspath("groot/rl/data/robots/g1/g1_29dof_with_hand_rev_1_0_homie.usd"), - # activate_contact_sensors=True, - # rigid_props=sim_utils.RigidBodyPropertiesCfg( - # disable_gravity=False, - # retain_accelerations=False, - # linear_damping=0.0, - # angular_damping=0.0, - # max_linear_velocity=1000.0, - # max_angular_velocity=1000.0, - # max_depenetration_velocity=1.0, - # ), - # articulation_props=sim_utils.ArticulationRootPropertiesCfg( - # enabled_self_collisions=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4 - # ), - # ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 139.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 20.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_22, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_22, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_22, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": ImplicitActuatorCfg( - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": ImplicitActuatorCfg( - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": ImplicitActuatorCfg( - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) - -G1_CYLINDER_MODEL_12_DEX_WAIST_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 139.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 20.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_22, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_22, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_22, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": ImplicitActuatorCfg( - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": ImplicitActuatorCfg( - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=300.0, - damping=5.0, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": ImplicitActuatorCfg( - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=300.0, - damping=5.0, - armature=ARMATURE_7520_14, - ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) - -G1_CYLINDER_MODEL_12_DEX_DELAYED_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - asset_path=f"{ASSET_DIR}/urdfs/g1/main.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 139.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 20.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_22, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_22, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_22, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - }, -) - -G1_CYLINDER_MODEL_12_HANDS_DEX_DELAYED_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - replace_cylinders_with_capsules=True, - merge_fixed_joints=False, # Keep palm_link separate from wrist_yaw_link - asset_path=f"{ASSET_DIR}/urdfs/g1/main_with_hand.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - linear_damping=0.0, - angular_damping=0.0, - max_linear_velocity=1000.0, - max_angular_velocity=1000.0, - max_depenetration_velocity=1.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=4, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0, damping=0 - ) - ), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.76), - joint_pos={ - ".*_hip_pitch_joint": -0.312, - ".*_knee_joint": 0.669, - ".*_ankle_pitch_joint": -0.363, - ".*_elbow_joint": 0.6, - "left_shoulder_roll_joint": 0.2, - "left_shoulder_pitch_joint": 0.2, - "right_shoulder_roll_joint": -0.2, - "right_shoulder_pitch_joint": 0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - joint_names_expr=[ - ".*_hip_yaw_joint", - ".*_hip_roll_joint", - ".*_hip_pitch_joint", - ".*_knee_joint", - ], - effort_limit_sim={ - ".*_hip_yaw_joint": 88.0, - ".*_hip_roll_joint": 139.0, - ".*_hip_pitch_joint": 139.0, - ".*_knee_joint": 139.0, - }, - velocity_limit_sim={ - ".*_hip_yaw_joint": 32.0, - ".*_hip_roll_joint": 20.0, - ".*_hip_pitch_joint": 20.0, - ".*_knee_joint": 20.0, - }, - stiffness={ - ".*_hip_pitch_joint": STIFFNESS_7520_22, - ".*_hip_roll_joint": STIFFNESS_7520_22, - ".*_hip_yaw_joint": STIFFNESS_7520_14, - ".*_knee_joint": STIFFNESS_7520_22, - }, - damping={ - ".*_hip_pitch_joint": DAMPING_7520_22, - ".*_hip_roll_joint": DAMPING_7520_22, - ".*_hip_yaw_joint": DAMPING_7520_14, - ".*_knee_joint": DAMPING_7520_22, - }, - armature={ - ".*_hip_pitch_joint": ARMATURE_7520_22, - ".*_hip_roll_joint": ARMATURE_7520_22, - ".*_hip_yaw_joint": ARMATURE_7520_14, - ".*_knee_joint": ARMATURE_7520_22, - }, - ), - "feet": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=50.0, - velocity_limit_sim=37.0, - joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=50, - velocity_limit_sim=37.0, - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - stiffness=2.0 * STIFFNESS_5020, - damping=2.0 * DAMPING_5020, - armature=2.0 * ARMATURE_5020, - ), - "waist_yaw": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - effort_limit_sim=88, - velocity_limit_sim=32.0, - joint_names_expr=["waist_yaw_joint"], - stiffness=STIFFNESS_7520_14, - damping=DAMPING_7520_14, - armature=ARMATURE_7520_14, - ), - "arms": DelayedImplicitActuatorCfg( - min_delay=0, - max_delay=2, - joint_names_expr=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_joint", - ".*_wrist_roll_joint", - ".*_wrist_pitch_joint", - ".*_wrist_yaw_joint", - ], - effort_limit_sim={ - ".*_shoulder_pitch_joint": 25.0, - ".*_shoulder_roll_joint": 25.0, - ".*_shoulder_yaw_joint": 25.0, - ".*_elbow_joint": 25.0, - ".*_wrist_roll_joint": 25.0, - ".*_wrist_pitch_joint": 5.0, - ".*_wrist_yaw_joint": 5.0, - }, - velocity_limit_sim={ - ".*_shoulder_pitch_joint": 37.0, - ".*_shoulder_roll_joint": 37.0, - ".*_shoulder_yaw_joint": 37.0, - ".*_elbow_joint": 37.0, - ".*_wrist_roll_joint": 37.0, - ".*_wrist_pitch_joint": 22.0, - ".*_wrist_yaw_joint": 22.0, - }, - stiffness={ - ".*_shoulder_pitch_joint": STIFFNESS_5020, - ".*_shoulder_roll_joint": STIFFNESS_5020, - ".*_shoulder_yaw_joint": STIFFNESS_5020, - ".*_elbow_joint": STIFFNESS_5020, - ".*_wrist_roll_joint": STIFFNESS_5020, - ".*_wrist_pitch_joint": STIFFNESS_4010, - ".*_wrist_yaw_joint": STIFFNESS_4010, - }, - damping={ - ".*_shoulder_pitch_joint": DAMPING_5020, - ".*_shoulder_roll_joint": DAMPING_5020, - ".*_shoulder_yaw_joint": DAMPING_5020, - ".*_elbow_joint": DAMPING_5020, - ".*_wrist_roll_joint": DAMPING_5020, - ".*_wrist_pitch_joint": DAMPING_4010, - ".*_wrist_yaw_joint": DAMPING_4010, - }, - armature={ - ".*_shoulder_pitch_joint": ARMATURE_5020, - ".*_shoulder_roll_joint": ARMATURE_5020, - ".*_shoulder_yaw_joint": ARMATURE_5020, - ".*_elbow_joint": ARMATURE_5020, - ".*_wrist_roll_joint": ARMATURE_5020, - ".*_wrist_pitch_joint": ARMATURE_4010, - ".*_wrist_yaw_joint": ARMATURE_4010, - }, - ), - "hands": DelayedImplicitActuatorCfg( - effort_limit_sim=EFFORT_LIMIT_1515, - velocity_limit_sim=VELOCITY_LIMIT_1515, - joint_names_expr=[ - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint", - ], - stiffness=STIFFNESS_1515, - damping=DAMPING_1515, - armature=ARMATURE_1515, - min_delay=0, - max_delay=0, - ), - }, -) - -G1_ACTION_SCALE = {} -for a in G1_CYLINDER_CFG.actuators.values(): - e = a.effort_limit_sim - s = a.stiffness - names = a.joint_names_expr - if not isinstance(e, dict): - e = {n: e for n in names} - if not isinstance(s, dict): - s = {n: s for n in names} - for n in names: - if n in e and n in s and s[n]: - G1_ACTION_SCALE[n] = 0.25 * e[n] / s[n] - - -G1_MODEL_12_ACTION_SCALE = {} -for a in G1_CYLINDER_MODEL_12_CFG.actuators.values(): - e = a.effort_limit_sim - s = a.stiffness - names = a.joint_names_expr - if not isinstance(e, dict): - e = {n: e for n in names} - if not isinstance(s, dict): - s = {n: s for n in names} - for n in names: - if n in e and n in s and s[n]: - G1_MODEL_12_ACTION_SCALE[n] = 0.25 * e[n] / s[n] - -G1_MODEL_12_DEX_WAIST_ACTION_SCALE = {} -for a in G1_CYLINDER_MODEL_12_DEX_WAIST_CFG.actuators.values(): - e = a.effort_limit_sim - s = a.stiffness - names = a.joint_names_expr - if not isinstance(e, dict): - e = {n: e for n in names} - if not isinstance(s, dict): - s = {n: s for n in names} - for n in names: - if "waist" in n: - G1_MODEL_12_DEX_WAIST_ACTION_SCALE[n] = 1.0 - elif n in e and n in s and s[n]: - G1_MODEL_12_DEX_WAIST_ACTION_SCALE[n] = 0.25 * e[n] / s[n] - -MUJOCO_JOINT_ORDER = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint", -] - -# G1 hand joints -G1_HAND_JOINT_NAMES = [ - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", - "right_hand_thumb_0_joint", - "right_hand_thumb_1_joint", - "right_hand_thumb_2_joint", - "right_hand_middle_0_joint", - "right_hand_middle_1_joint", - "right_hand_index_0_joint", - "right_hand_index_1_joint", -] - -# G1 dex hand contact bodies (used for contact sensors) -G1_DEX_CONTACT_BODIES = [ - ".*_hand_palm_link", - ".*_hand_thumb_0_link", - ".*_hand_thumb_1_link", - ".*_hand_thumb_2_link", - ".*_hand_middle_0_link", - ".*_hand_middle_1_link", - ".*_hand_index_0_link", - ".*_hand_index_1_link", -] - -# Dex 3 order -DEX3_PARQUET_JOINT_ORDER = [ - "base_x_joint", - "base_y_joint", - "base_z_joint", - "base_roll_joint", - "base_pitch_joint", - "base_yaw_joint", - "left_hand_thumb_0_joint", - "left_hand_thumb_1_joint", - "left_hand_thumb_2_joint", - "left_hand_middle_0_joint", - "left_hand_middle_1_joint", - "left_hand_index_0_joint", - "left_hand_index_1_joint", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/.gitignore deleted file mode 100644 index 1d781519..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -# Ignore all zip files in sub-directories -**/*.zip diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/.gitignore deleted file mode 100644 index d6b7ef32..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -* -!.gitignore diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/assets/.download_status.json b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/assets/.download_status.json deleted file mode 100644 index e0e6f3cb..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/hot3d/assets/.download_status.json +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6616b3553cb038eba2244be1ad02ed2ca5b2d2f6f7c9e30fb1571d7516bcd99c -size 21 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/html/index.html b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/html/index.html deleted file mode 100644 index 1e0ec6d2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/html/index.html +++ /dev/null @@ -1,62 +0,0 @@ - - - - - Viser Recordings - - - -

Viser Recordings

-
Loading…
-
- - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2/.gitignore deleted file mode 100644 index 2e202977..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/oakink2/.gitignore +++ /dev/null @@ -1 +0,0 @@ -anno_preview/ diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/.gitignore b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/.gitignore deleted file mode 100644 index 9bd8cf29..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/.gitignore +++ /dev/null @@ -1,29 +0,0 @@ -* -!.gitignore - -# CI test sequence: taco_empty__kettle__plate_20231031_060 -!taco_loaded/ -!taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/ -!taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/** - -!taco_processed/ -!taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/ -!taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/** - -!reconstructed_stage/ -!reconstructed_stage/taco_empty__kettle__plate_20231031_060_support.usda - -# Raw TACO pose data for the same sequence (CC BY 4.0 — see LICENSE + NOTICE). -!dataset/ -!dataset/Hand_Poses/ -!dataset/Hand_Poses/(empty, kettle, plate)/ -!dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/ -!dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/** -!dataset/Object_Poses/ -!dataset/Object_Poses/(empty, kettle, plate)/ -!dataset/Object_Poses/(empty, kettle, plate)/20231031_060/ -!dataset/Object_Poses/(empty, kettle, plate)/20231031_060/** - -# TACO license + attribution -!LICENSE -!NOTICE diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/LICENSE deleted file mode 100644 index 4ea99c21..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/LICENSE +++ /dev/null @@ -1,395 +0,0 @@ -Attribution 4.0 International - -======================================================================= - -Creative Commons Corporation ("Creative Commons") is not a law firm and -does not provide legal services or legal advice. Distribution of -Creative Commons public licenses does not create a lawyer-client or -other relationship. Creative Commons makes its licenses and related -information available on an "as-is" basis. Creative Commons gives no -warranties regarding its licenses, any material licensed under their -terms and conditions, or any related information. Creative Commons -disclaims all liability for damages resulting from their use to the -fullest extent possible. - -Using Creative Commons Public Licenses - -Creative Commons public licenses provide a standard set of terms and -conditions that creators and other rights holders may use to share -original works of authorship and other material subject to copyright -and certain other rights specified in the public license below. The -following considerations are for informational purposes only, are not -exhaustive, and do not form part of our licenses. - - Considerations for licensors: Our public licenses are - intended for use by those authorized to give the public - permission to use material in ways otherwise restricted by - copyright and certain other rights. Our licenses are - irrevocable. Licensors should read and understand the terms - and conditions of the license they choose before applying it. - Licensors should also secure all rights necessary before - applying our licenses so that the public can reuse the - material as expected. Licensors should clearly mark any - material not subject to the license. This includes other CC- - licensed material, or material used under an exception or - limitation to copyright. More considerations for licensors: - wiki.creativecommons.org/Considerations_for_licensors - - Considerations for the public: By using one of our public - licenses, a licensor grants the public permission to use the - licensed material under specified terms and conditions. If - the licensor's permission is not necessary for any reason--for - example, because of any applicable exception or limitation to - copyright--then that use is not regulated by the license. Our - licenses grant only permissions under copyright and certain - other rights that a licensor has authority to grant. Use of - the licensed material may still be restricted for other - reasons, including because others have copyright or other - rights in the material. A licensor may make special requests, - such as asking that all changes be marked or described. - Although not required by our licenses, you are encouraged to - respect those requests where reasonable. More considerations - for the public: - wiki.creativecommons.org/Considerations_for_licensees - -======================================================================= - -Creative Commons Attribution 4.0 International Public License - -By exercising the Licensed Rights (defined below), You accept and agree -to be bound by the terms and conditions of this Creative Commons -Attribution 4.0 International Public License ("Public License"). To the -extent this Public License may be interpreted as a contract, You are -granted the Licensed Rights in consideration of Your acceptance of -these terms and conditions, and the Licensor grants You such rights in -consideration of benefits the Licensor receives from making the -Licensed Material available under these terms and conditions. - - -Section 1 -- Definitions. - - a. Adapted Material means material subject to Copyright and Similar - Rights that is derived from or based upon the Licensed Material - and in which the Licensed Material is translated, altered, - arranged, transformed, or otherwise modified in a manner requiring - permission under the Copyright and Similar Rights held by the - Licensor. For purposes of this Public License, where the Licensed - Material is a musical work, performance, or sound recording, - Adapted Material is always produced where the Licensed Material is - synched in timed relation with a moving image. - - b. Adapter's License means the license You apply to Your Copyright - and Similar Rights in Your contributions to Adapted Material in - accordance with the terms and conditions of this Public License. - - c. Copyright and Similar Rights means copyright and/or similar rights - closely related to copyright including, without limitation, - performance, broadcast, sound recording, and Sui Generis Database - Rights, without regard to how the rights are labeled or - categorized. For purposes of this Public License, the rights - specified in Section 2(b)(1)-(2) are not Copyright and Similar - Rights. - - d. Effective Technological Measures means those measures that, in the - absence of proper authority, may not be circumvented under laws - fulfilling obligations under Article 11 of the WIPO Copyright - Treaty adopted on December 20, 1996, and/or similar international - agreements. - - e. Exceptions and Limitations means fair use, fair dealing, and/or - any other exception or limitation to Copyright and Similar Rights - that applies to Your use of the Licensed Material. - - f. Licensed Material means the artistic or literary work, database, - or other material to which the Licensor applied this Public - License. - - g. Licensed Rights means the rights granted to You subject to the - terms and conditions of this Public License, which are limited to - all Copyright and Similar Rights that apply to Your use of the - Licensed Material and that the Licensor has authority to license. - - h. Licensor means the individual(s) or entity(ies) granting rights - under this Public License. - - i. Share means to provide material to the public by any means or - process that requires permission under the Licensed Rights, such - as reproduction, public display, public performance, distribution, - dissemination, communication, or importation, and to make material - available to the public including in ways that members of the - public may access the material from a place and at a time - individually chosen by them. - - j. Sui Generis Database Rights means rights other than copyright - resulting from Directive 96/9/EC of the European Parliament and of - the Council of 11 March 1996 on the legal protection of databases, - as amended and/or succeeded, as well as other essentially - equivalent rights anywhere in the world. - - k. You means the individual or entity exercising the Licensed Rights - under this Public License. Your has a corresponding meaning. - - -Section 2 -- Scope. - - a. License grant. - - 1. Subject to the terms and conditions of this Public License, - the Licensor hereby grants You a worldwide, royalty-free, - non-sublicensable, non-exclusive, irrevocable license to - exercise the Licensed Rights in the Licensed Material to: - - a. reproduce and Share the Licensed Material, in whole or - in part; and - - b. produce, reproduce, and Share Adapted Material. - - 2. Exceptions and Limitations. For the avoidance of doubt, where - Exceptions and Limitations apply to Your use, this Public - License does not apply, and You do not need to comply with - its terms and conditions. - - 3. Term. The term of this Public License is specified in Section - 6(a). - - 4. Media and formats; technical modifications allowed. The - Licensor authorizes You to exercise the Licensed Rights in - all media and formats whether now known or hereafter created, - and to make technical modifications necessary to do so. The - Licensor waives and/or agrees not to assert any right or - authority to forbid You from making technical modifications - necessary to exercise the Licensed Rights, including - technical modifications necessary to circumvent Effective - Technological Measures. For purposes of this Public License, - simply making modifications authorized by this Section 2(a) - (4) never produces Adapted Material. - - 5. Downstream recipients. - - a. Offer from the Licensor -- Licensed Material. Every - recipient of the Licensed Material automatically - receives an offer from the Licensor to exercise the - Licensed Rights under the terms and conditions of this - Public License. - - b. No downstream restrictions. You may not offer or impose - any additional or different terms or conditions on, or - apply any Effective Technological Measures to, the - Licensed Material if doing so restricts exercise of the - Licensed Rights by any recipient of the Licensed - Material. - - 6. No endorsement. Nothing in this Public License constitutes or - may be construed as permission to assert or imply that You - are, or that Your use of the Licensed Material is, connected - with, or sponsored, endorsed, or granted official status by, - the Licensor or others designated to receive attribution as - provided in Section 3(a)(1)(A)(i). - - b. Other rights. - - 1. Moral rights, such as the right of integrity, are not - licensed under this Public License, nor are publicity, - privacy, and/or other similar personality rights; however, to - the extent possible, the Licensor waives and/or agrees not to - assert any such rights held by the Licensor to the limited - extent necessary to allow You to exercise the Licensed - Rights, but not otherwise. - - 2. Patent and trademark rights are not licensed under this - Public License. - - 3. To the extent possible, the Licensor waives any right to - collect royalties from You for the exercise of the Licensed - Rights, whether directly or through a collecting society - under any voluntary or waivable statutory or compulsory - licensing scheme. In all other cases the Licensor expressly - reserves any right to collect such royalties. - - -Section 3 -- License Conditions. - -Your exercise of the Licensed Rights is expressly made subject to the -following conditions. - - a. Attribution. - - 1. If You Share the Licensed Material (including in modified - form), You must: - - a. retain the following if it is supplied by the Licensor - with the Licensed Material: - - i. identification of the creator(s) of the Licensed - Material and any others designated to receive - attribution, in any reasonable manner requested by - the Licensor (including by pseudonym if - designated); - - ii. a copyright notice; - - iii. a notice that refers to this Public License; - - iv. a notice that refers to the disclaimer of - warranties; - - v. a URI or hyperlink to the Licensed Material to the - extent reasonably practicable; - - b. indicate if You modified the Licensed Material and - retain an indication of any previous modifications; and - - c. indicate the Licensed Material is licensed under this - Public License, and include the text of, or the URI or - hyperlink to, this Public License. - - 2. You may satisfy the conditions in Section 3(a)(1) in any - reasonable manner based on the medium, means, and context in - which You Share the Licensed Material. For example, it may be - reasonable to satisfy the conditions by providing a URI or - hyperlink to a resource that includes the required - information. - - 3. If requested by the Licensor, You must remove any of the - information required by Section 3(a)(1)(A) to the extent - reasonably practicable. - - 4. If You Share Adapted Material You produce, the Adapter's - License You apply must not prevent recipients of the Adapted - Material from complying with this Public License. - - -Section 4 -- Sui Generis Database Rights. - -Where the Licensed Rights include Sui Generis Database Rights that -apply to Your use of the Licensed Material: - - a. for the avoidance of doubt, Section 2(a)(1) grants You the right - to extract, reuse, reproduce, and Share all or a substantial - portion of the contents of the database; - - b. if You include all or a substantial portion of the database - contents in a database in which You have Sui Generis Database - Rights, then the database in which You have Sui Generis Database - Rights (but not its individual contents) is Adapted Material; and - - c. You must comply with the conditions in Section 3(a) if You Share - all or a substantial portion of the contents of the database. - -For the avoidance of doubt, this Section 4 supplements and does not -replace Your obligations under this Public License where the Licensed -Rights include other Copyright and Similar Rights. - - -Section 5 -- Disclaimer of Warranties and Limitation of Liability. - - a. UNLESS OTHERWISE SEPARATELY UNDERTAKEN BY THE LICENSOR, TO THE - EXTENT POSSIBLE, THE LICENSOR OFFERS THE LICENSED MATERIAL AS-IS - AND AS-AVAILABLE, AND MAKES NO REPRESENTATIONS OR WARRANTIES OF - ANY KIND CONCERNING THE LICENSED MATERIAL, WHETHER EXPRESS, - IMPLIED, STATUTORY, OR OTHER. THIS INCLUDES, WITHOUT LIMITATION, - WARRANTIES OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR - PURPOSE, NON-INFRINGEMENT, ABSENCE OF LATENT OR OTHER DEFECTS, - ACCURACY, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT - KNOWN OR DISCOVERABLE. WHERE DISCLAIMERS OF WARRANTIES ARE NOT - ALLOWED IN FULL OR IN PART, THIS DISCLAIMER MAY NOT APPLY TO YOU. - - b. TO THE EXTENT POSSIBLE, IN NO EVENT WILL THE LICENSOR BE LIABLE - TO YOU ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION, - NEGLIGENCE) OR OTHERWISE FOR ANY DIRECT, SPECIAL, INDIRECT, - INCIDENTAL, CONSEQUENTIAL, PUNITIVE, EXEMPLARY, OR OTHER LOSSES, - COSTS, EXPENSES, OR DAMAGES ARISING OUT OF THIS PUBLIC LICENSE OR - USE OF THE LICENSED MATERIAL, EVEN IF THE LICENSOR HAS BEEN - ADVISED OF THE POSSIBILITY OF SUCH LOSSES, COSTS, EXPENSES, OR - DAMAGES. WHERE A LIMITATION OF LIABILITY IS NOT ALLOWED IN FULL OR - IN PART, THIS LIMITATION MAY NOT APPLY TO YOU. - - c. The disclaimer of warranties and limitation of liability provided - above shall be interpreted in a manner that, to the extent - possible, most closely approximates an absolute disclaimer and - waiver of all liability. - - -Section 6 -- Term and Termination. - - a. This Public License applies for the term of the Copyright and - Similar Rights licensed here. However, if You fail to comply with - this Public License, then Your rights under this Public License - terminate automatically. - - b. Where Your right to use the Licensed Material has terminated under - Section 6(a), it reinstates: - - 1. automatically as of the date the violation is cured, provided - it is cured within 30 days of Your discovery of the - violation; or - - 2. upon express reinstatement by the Licensor. - - For the avoidance of doubt, this Section 6(b) does not affect any - right the Licensor may have to seek remedies for Your violations - of this Public License. - - c. For the avoidance of doubt, the Licensor may also offer the - Licensed Material under separate terms or conditions or stop - distributing the Licensed Material at any time; however, doing so - will not terminate this Public License. - - d. Sections 1, 5, 6, 7, and 8 survive termination of this Public - License. - - -Section 7 -- Other Terms and Conditions. - - a. The Licensor shall not be bound by any additional or different - terms or conditions communicated by You unless expressly agreed. - - b. Any arrangements, understandings, or agreements regarding the - Licensed Material not stated herein are separate from and - independent of the terms and conditions of this Public License. - - -Section 8 -- Interpretation. - - a. For the avoidance of doubt, this Public License does not, and - shall not be interpreted to, reduce, limit, restrict, or impose - conditions on any use of the Licensed Material that could lawfully - be made without permission under this Public License. - - b. To the extent possible, if any provision of this Public License is - deemed unenforceable, it shall be automatically reformed to the - minimum extent necessary to make it enforceable. If the provision - cannot be reformed, it shall be severed from this Public License - without affecting the enforceability of the remaining terms and - conditions. - - c. No term or condition of this Public License will be waived and no - failure to comply consented to unless expressly agreed to by the - Licensor. - - d. Nothing in this Public License constitutes or may be interpreted - as a limitation upon, or waiver of, any privileges and immunities - that apply to the Licensor or You, including from the legal - processes of any jurisdiction or authority. - - -======================================================================= - -Creative Commons is not a party to its public -licenses. Notwithstanding, Creative Commons may elect to apply one of -its public licenses to material it publishes and in those instances -will be considered the “Licensor.” The text of the Creative Commons -public licenses is dedicated to the public domain under the CC0 Public -Domain Dedication. Except for the limited purpose of indicating that -material is shared under a Creative Commons public license or as -otherwise permitted by the Creative Commons policies published at -creativecommons.org/policies, Creative Commons does not authorize the -use of the trademark "Creative Commons" or any other trademark or logo -of Creative Commons without its prior written consent including, -without limitation, in connection with any unauthorized modifications -to any of its public licenses or any other arrangements, -understandings, or agreements concerning use of licensed material. For -the avoidance of doubt, this paragraph does not form part of the -public licenses. - -Creative Commons may be contacted at creativecommons.org. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/NOTICE deleted file mode 100644 index 25c438da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/NOTICE +++ /dev/null @@ -1,22 +0,0 @@ -This directory contains TACO motion-capture data and data derived from it, -retained as fixtures for the retarget + train end-to-end tests -(tests/test_retarget_pipeline_e2e.py, tests/test_train_e2e.py). Only the single -sequence taco_empty__kettle__plate_20231031_060 is tracked: - - dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/ raw hand poses (MANO) - dataset/Object_Poses/(empty, kettle, plate)/20231031_060/ raw object poses - taco_loaded/... parquet derived from the raw poses (hand forward kinematics) - taco_processed/... the above + Sharpa robot trajectories from IK retargeting - reconstructed_stage/ support-surface geometry derived from the object poses - -TACO © Yun Liu et al., licensed under CC BY 4.0 -(https://creativecommons.org/licenses/by/4.0/). -Source: https://github.com/leolyliu/TACO-Instructions -Liu et al., "TACO: Benchmarking Generalizable Bimanual Tool-Action-Object -Understanding", CVPR 2024. - -Changes: the raw hand/object poses are redistributed in their original form and -as derived parquet (hand forward-kinematics + Sharpa robot retargeting). - -The full text of the CC BY 4.0 license is in the LICENSE file alongside this -notice. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand.pkl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand.pkl deleted file mode 100644 index 239aad42..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand.pkl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f29075406613965ff4057ce71d99d2253cdee4503a62f08e4866e02af76df7f -size 4990362 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand_shape.pkl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand_shape.pkl deleted file mode 100644 index fc0bfdee..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/left_hand_shape.pkl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:aa762b0b6c117ed84a85dc37da5a24aa1a40b7ece2c3ead79ea2c71a6fde2044 -size 445 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand.pkl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand.pkl deleted file mode 100644 index 0d79cc2b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand.pkl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:03509f85ad1e76fc16ca880d9549b481e9a86e856765fa31186051ca5f355b0b -size 4990362 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand_shape.pkl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand_shape.pkl deleted file mode 100644 index cd79c8e5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Hand_Poses/(empty, kettle, plate)/20231031_060/right_hand_shape.pkl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b37bed46676123fbe7042cb1a67c93ae7d5769982ec6b0de763f4729b6635590 -size 445 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/target_005.npy b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/target_005.npy deleted file mode 100644 index 86e4a673..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/target_005.npy +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:25d69c47efc27617349f69282fb65b5249e98b3cfa0a1254b2aaa4afb87b5570 -size 10048 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/tool_030.npy b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/tool_030.npy deleted file mode 100644 index a337c574..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/dataset/Object_Poses/(empty, kettle, plate)/20231031_060/tool_030.npy +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8282338f9320a9e7ea25f9c1656ccc9ea9bf4f70f322964133f6882d3d8379ec -size 10048 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/reconstructed_stage/taco_empty__kettle__plate_20231031_060_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/reconstructed_stage/taco_empty__kettle__plate_20231031_060_support.usda deleted file mode 100644 index 99cf5937..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/reconstructed_stage/taco_empty__kettle__plate_20231031_060_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:94e578218ce7f9d1dd1e51382ffe29bc124413b8bb0615b36234a175b407d35a -size 1019 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/ada492adc7b34598af4e1b902919c6cc-0.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/ada492adc7b34598af4e1b902919c6cc-0.parquet deleted file mode 100644 index e712cdcc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_loaded/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/ada492adc7b34598af4e1b902919c6cc-0.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b9683afa9cf34555446c225da1ec6b07ec5ffd669a82a757b885bdd96980bdb4 -size 488706 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/a0071139546d4b83b37fa56b75cd19a1-0.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/a0071139546d4b83b37fa56b75cd19a1-0.parquet deleted file mode 100644 index f2b53a5c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_processed/sequence_id=taco_empty__kettle__plate_20231031_060/robot_name=sharpa_wave/a0071139546d4b83b37fa56b75cd19a1-0.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f1185c62fb506d185d6f36d44355bc603dcd02bdd8a0eb20c9e1ae7fc50f5219 -size 1036397 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-01-26_11-09-38_bottle_pick_transfer_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-01-26_11-09-38_bottle_pick_transfer_support.usda deleted file mode 100644 index d03cb10f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-01-26_11-09-38_bottle_pick_transfer_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:de0d8a9a3cc1c48d7d12150a0ee4c8ae5783d27acc838e23618b034f72577c53 -size 1016 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_10-24-18_snack_box_pick_and_place_01_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_10-24-18_snack_box_pick_and_place_01_support.usda deleted file mode 100644 index db36010e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_10-24-18_snack_box_pick_and_place_01_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1a31d46961bbc4d502fafa09f397f153dc3ad07362850e890e25e960cfd29678 -size 569 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02_support.usda deleted file mode 100644 index a085c31b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c1ab0afd955233e301f2f6dbe7c44040ed6e1e0fda0eb378def6a4db3257f75c -size 1011 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-23_18-10-01_corn_can_right_left_handover_01_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-23_18-10-01_corn_can_right_left_handover_01_support.usda deleted file mode 100644 index 4db66adf..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/2026-03-23_18-10-01_corn_can_right_left_handover_01_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c25529df26f73f7ba2ec205fdb9d54716d5591f2079c7901d1e1763d94e41631 -size 569 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/apple_pick_optimized_support.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/apple_pick_optimized_support.usda deleted file mode 100644 index b76599c5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/reconstructed_stage/apple_pick_optimized_support.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b8f9709b2dba1368ff452a795467c6f8a1e7fa503bdc2234b07491ef94470ebe -size 489 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material.mtl deleted file mode 100644 index 4ebcddb2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ca4deadbb314a7d180f552345fc0411e5becebcbb61fac04171a2f364eb9f396 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material_0.png deleted file mode 100644 index 592ed4f6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0012f8c2799559672591904c1d93b21271ac931ee45276f88a671450a82d3d3a -size 932939 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.obj deleted file mode 100644 index 1e6b52bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1d882e5142b2044a0546b584841aa7e37e7f17a6ff21c96784c99757ea126d66 -size 27174748 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.urdf deleted file mode 100644 index 4e3f58b0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/object/textured_mesh.urdf +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/robot_name=g1/data.parquet deleted file mode 100644 index c5d07c80..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-01-26_11-09-38_bottle_pick_transfer/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b8346fde173321598ae245a5a471149dd3c5b87f630dfa547fd94ccd7e33b470 -size 2606306 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material.mtl deleted file mode 100644 index d21f55e2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0d90fe7d688a6da0b82b0f8053231a1c8986de34aa839f8113ed53a610c09227 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material_0.png deleted file mode 100644 index 883afd63..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fa4501f8153f19e7ea8c6ada1bc05f7eeca6a03493fa2df0ffb907788eb8c0b9 -size 10100589 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.obj deleted file mode 100644 index 89b8f5b0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce533749b88e8cfaacc92bf5375c7b8c525dba1f581cea34040587adee41a745 -size 19278770 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.urdf deleted file mode 100644 index 9dddb5cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/object/textured_mesh.urdf +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/robot_name=g1/data.parquet deleted file mode 100644 index ac596a03..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-20_17-37-45_blue_trash_can_drag_007/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c4cea0a72c3235f1f2d2b42f0c83b785e82fdebe37d0f02fdc8a8c26b03bf2c5 -size 3554045 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material.mtl deleted file mode 100644 index 4ebcddb2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ca4deadbb314a7d180f552345fc0411e5becebcbb61fac04171a2f364eb9f396 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material_0.png deleted file mode 100644 index c2208781..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c867c5e480f4b61aadce2bc2b2c372b320d53fcc4debd0e5da70c0ec0450d54d -size 127443 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.obj deleted file mode 100644 index bd0cc1dd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1fc96684f9ca24707e651c6fa43a57b682d29c1389d0810a343f80956cd323e5 -size 8921546 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.urdf deleted file mode 100644 index 9dddb5cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/object/textured_mesh.urdf +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/robot_name=g1/data.parquet deleted file mode 100644 index b42b1530..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-02-27_16-25-23_sit_skinny_wood_chair_01/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6c1035be6582676c5266a9e78495c6245552b1804425738e3fbc34dca1f22dd7 -size 3629432 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material.mtl deleted file mode 100644 index d21f55e2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0d90fe7d688a6da0b82b0f8053231a1c8986de34aa839f8113ed53a610c09227 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material_0.png deleted file mode 100644 index 36f85bf4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4e2d7fa813eba126345723dca5ab52b11b773f015d99968f7b7172e6bae4d265 -size 9037738 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.obj deleted file mode 100644 index 75675a62..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:380248270cb7e23aed66c33e74d100e1e4dd5e329ff3d8d63ac85753c859066a -size 22108986 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.urdf deleted file mode 100644 index 9dddb5cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/object/textured_mesh.urdf +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/robot_name=g1/data.parquet deleted file mode 100644 index c23ee3bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_10-24-18_snack_box_pick_and_place_01/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:04f55c50a6dfc04826b73b22cc42cde90d9209e003d242b3d300b0a631c9671d -size 3688593 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material.mtl deleted file mode 100644 index d21f55e2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0d90fe7d688a6da0b82b0f8053231a1c8986de34aa839f8113ed53a610c09227 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material_0.png deleted file mode 100644 index 8d229370..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:529bcede0665ed70718b828264088192bb6d7d427ab4657ae6b0c2b57013509c -size 2497731 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.obj deleted file mode 100644 index 64b4593a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8ba7865c64827fe432bafa7f21b40655f677ea31f071aac5b9ea89da8511e104 -size 22460876 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.urdf deleted file mode 100644 index 9dddb5cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/object/textured_mesh.urdf +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/robot_name=g1/data.parquet deleted file mode 100644 index 63862680..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-06_11-00-48_dark_blue_book_pick_handover_place_02/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a665cc2578db96d8544f18ea69e2148b4f86e8d0300cc027d11b3b4805fd3bd1 -size 3681201 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material.mtl deleted file mode 100644 index d21f55e2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0d90fe7d688a6da0b82b0f8053231a1c8986de34aa839f8113ed53a610c09227 -size 198 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material_0.png b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material_0.png deleted file mode 100644 index 2cd5f8e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/material_0.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:761c37d87ef450c163851089ce8c079a91e5758e3ef4234958278076fe7d25ce -size 1023053 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.obj deleted file mode 100644 index 848eb1b2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d0668549cbcf73a9c7935b6690485a6a51e475fcbf2e68d73108be0eb5b532ed -size 21210733 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.urdf deleted file mode 100644 index 9dddb5cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/object/textured_mesh.urdf +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/robot_name=g1/data.parquet deleted file mode 100644 index 78ef209b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=2026-03-23_18-10-01_corn_can_right_left_handover_01/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1679e920f20cc741ffccaf8164a960bf46e1f124d670d7bddb2416ca6ea8b082 -size 4388992 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_collision.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_collision.obj deleted file mode 100644 index ba88147d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_collision.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6c6c65338a190e0f536d8068e0f95f10c744c2c980e97242302ce285ca5c92ca -size 131549 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_rigid.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_rigid.urdf deleted file mode 100644 index dee52390..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_rigid.urdf +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_simple.usda b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_simple.usda deleted file mode 100644 index 61a6b1de..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/object/apple_simple.usda +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8b75bc085fd70fceb5f76654e5a0b7ef944c0ad30ae21dc88f7a349974e765f7 -size 1732 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/robot_name=g1/data.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/robot_name=g1/data.parquet deleted file mode 100644 index 3b3f6bb6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/whole_body/soma/sequence_id=apple_pick_optimized/robot_name=g1/data.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:80db3cb4ff18e185a21f22f02132935535838d617f3f6b78f4f45df7d4c9ac1b -size 123258 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/joint_order_registry.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/joint_order_registry.py deleted file mode 100644 index 74430226..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/joint_order_registry.py +++ /dev/null @@ -1,38 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Joint ordering registry for different robots.""" - -from __future__ import annotations - -from robotic_grounding.assets.g1 import ( - DEX3_PARQUET_JOINT_ORDER as G1_DEX3_PARQUET_JOINT_ORDER, -) -from robotic_grounding.assets.g1 import MUJOCO_JOINT_ORDER as G1_MUJOCO_JOINT_ORDER - -JOINT_ORDER_REGISTRY: dict[str, dict[str, list[str]]] = { - "g1": { - "mujoco": G1_MUJOCO_JOINT_ORDER, - }, - "g1_dex3": { - "parquet": G1_DEX3_PARQUET_JOINT_ORDER, - }, - "dex3": { - "parquet": G1_DEX3_PARQUET_JOINT_ORDER, - }, -} - - -def get_joint_order(robot_type: str, ordering: str) -> list[str] | None: - """Get joint ordering for a robot type. - - Args: - robot_type: Robot type (e.g., "g1", "h1"). - ordering: Ordering name (e.g., "mujoco", "isaaclab"). - - Returns: - List of joint names in the specified order, or None if not found. - """ - robot_orders = JOINT_ORDER_REGISTRY.get(robot_type) - if robot_orders is None: - return None - return robot_orders.get(ordering) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/LICENSE deleted file mode 100644 index e472bbd9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/NOTICE deleted file mode 100644 index 74248321..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/NOTICE +++ /dev/null @@ -1,6 +0,0 @@ -This directory contains Unitree G1 mesh assets used by the G1 and Dex3 robot -descriptions. - -The assets are from or derived from Unitree G1 Description assets developed by -Unitree Robotics and are licensed under the BSD 3-Clause License. The full -license text is in the LICENSE file alongside this notice. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.mtl deleted file mode 100644 index 73506182..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9d045ba774fedb5f59ba016b567466b88fe28a789208adcfbd40ec1739ee4485 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.obj deleted file mode 100644 index 05b3e70d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c8112287b6690fa5cdfc12608ceb697d7528998fa1c38e92ef882ac0ea1b8a18 -size 13952 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.mtl deleted file mode 100644 index ed05b042..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4efa2499c5ef03d34a550ab20c51a410bb0e52615c209193a8f1ffbe56e889a3 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.obj deleted file mode 100644 index 361c0446..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_index_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:69127ab143b0175ac96ba3fdc2cddd3835008010fb0a9ca603bafa2da98ae1c2 -size 17466 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.mtl deleted file mode 100644 index e6ffc0e1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2b30d986f70c1cfa366dc89c320b8a186f697ac94616014992ffd0f07b25b53e -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.obj deleted file mode 100644 index 8fb3d4f4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:595b39a96733f6cb83c7c45221e982aaca74d2059e07402c5c1e2b14a2b985e8 -size 12648 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.mtl deleted file mode 100644 index 98b14994..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f8fd90b501ea014ccbcbcacec8ccfca2874d9c8738acbb8562127ef0af4da8c -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.obj deleted file mode 100644 index a3b5eca7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_0_link_extra.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f5c5de7c17f68c1b7a3e7254aaf40ed7c63f7bacc4ef7b8c7473c3f44a96cba8 -size 1494 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.mtl deleted file mode 100644 index e77ca3bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0a4ab7b1ff7fad79d544ca937723b4a50dc935f70e3f22bef1839d21229fc6ba -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.obj deleted file mode 100644 index c8a93a59..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_middle_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3c43aee638420e3a11cfe29b2eda8b607df222ac0bb7121155d2cc7650527628 -size 16836 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.obj deleted file mode 100644 index e988421d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:03a0a5c819a15bf7944c1ee312402c0858552768842c6f613d473ae857a85c10 -size 12907 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.obj deleted file mode 100644 index 9dafb046..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_1.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bf8a8c11bf0d4c7a45ac07e403ea3a34e0a9696dba57d0cd79f627b4475c035c -size 2922 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.obj deleted file mode 100644 index 21a23e53..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_palm_link_extra_2.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0b959dda3c68e590d863004fe9e64e410b9f4e1c161e18886465db23dd0072b2 -size 2319 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.obj deleted file mode 100644 index c7424f5a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:409feca6952f279b6e92758cbbdeff6f3b9b95c77bbe4118561c71fcdbf76d74 -size 3746 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.mtl deleted file mode 100644 index 0ce2f01a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1ec38f8b35c25c7e615978049b072f51dcb414ae4c8013adb4880bf527d69445 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.obj deleted file mode 100644 index 30e1212a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fa5fbbc66ce92686856013c9fdeda65764f804014c39a1cc55fec9f76588ad59 -size 13835 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.mtl deleted file mode 100644 index f6a35392..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3205807dfbd2b23b585dddbb147f20b53237f90823fe5d3a78b7f318cacf1118 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.obj deleted file mode 100644 index 76eae491..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_1_link_extra.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b7ba61e9af3f2ae4ae47d6588c4e22ae8131f98736072d3eec7629b432b15c7c -size 874 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.mtl deleted file mode 100644 index 0ec3bdad..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:339b2337cb2851497124e8d86a1b1ec73a64f7082376766f4b7ae2cd56994e0a -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.obj deleted file mode 100644 index ebd84b7f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/left_hand_thumb_2_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:31f5cf5cd96c0e2662115e7dd5c7a523239c860fbb01909d6e3934d8c2037f28 -size 17720 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.mtl deleted file mode 100644 index 72672366..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:188cea82208e1ce9804bbf0c4cf01ccc1ae71e186e33472309545d96b95c52ee -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.obj deleted file mode 100644 index 4b35d269..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ee2532bb19241fdb109ebc05b4f6640229bee863943f58ce41d367e600a77ac1 -size 11863 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.mtl deleted file mode 100644 index 9521e09e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:594d7d3e8cd93c022eae9511bb5d71e997b56b10ae59b21e914f82ffd83e9452 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.obj deleted file mode 100644 index e501984f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_index_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9873d114ee406ab0ac872e2db7ee1623a4f64de009f56608665837131df8fa71 -size 16760 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.mtl deleted file mode 100644 index b14bf057..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce48eadea417167969735e132adb211df6808f30c9b07b0740cc3fbc96e3a419 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.obj deleted file mode 100644 index 6ef97b0c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bd136e2bba1ca008f7c792e019dd0c3c0d2359d9d408554ff1d46b6c3a276b57 -size 12802 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.mtl deleted file mode 100644 index 98b14994..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f8fd90b501ea014ccbcbcacec8ccfca2874d9c8738acbb8562127ef0af4da8c -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.obj deleted file mode 100644 index 5fd0c948..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_0_link_extra.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:dee3a5eeb3b9c942de357cf21add7b2b4d9d363ae88422e66ac185b3171b4972 -size 2179 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.mtl deleted file mode 100644 index 9f9f1994..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:340b38844ee8ff1423d617af7533bfb76633fbc20429b5542cf6999084d0d246 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.obj deleted file mode 100644 index 43db13f9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_middle_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:40a88ce4f3e9f9ccac8f8f559fa02de5016f168d4f07fc2f82958befeaca0fc4 -size 16762 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.mtl deleted file mode 100644 index 21e52a53..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4bd51852610b02daf4dbac661cd4601d6a649bbda7bc1307be1e9aaeb2848615 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.obj deleted file mode 100644 index 99fd9718..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d49b420c25d57970ab46c48df07fee53dca23b39c6d1c0c5a4690db056e1f2c5 -size 7788 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.obj deleted file mode 100644 index 4594ae08..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_1.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:13bf5e5c4d68b1f7f529d1ad9b359f79df0a4e3774b8f4b3069f5bafa5d2bddb -size 2338 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.mtl deleted file mode 100644 index f6a35392..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3205807dfbd2b23b585dddbb147f20b53237f90823fe5d3a78b7f318cacf1118 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.obj deleted file mode 100644 index 987294f8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_palm_link_extra_2.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:15fb13682f5c2a5e40140d9772c1f7fd75d94661ff49a3ddf04fad4bc695caa6 -size 2284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.mtl deleted file mode 100644 index 98b14994..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f8fd90b501ea014ccbcbcacec8ccfca2874d9c8738acbb8562127ef0af4da8c -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.obj deleted file mode 100644 index 73068052..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_0_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a4f5fcea27a522b05ce0b9c9c3df9ccfe1528664c16c09385dd7801bb86de227 -size 3210 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.mtl deleted file mode 100644 index efc338e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b5c2f444c26eff389b3934e2b1d7dc73001841fade992995758ea3e0e60619a6 -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.obj deleted file mode 100644 index fd5ac391..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6a7e46596af6836bd02e1c2e107109d274e043d141eb6c100a518cfb3101f987 -size 12589 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.mtl deleted file mode 100644 index 588fd601..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:361ed9728aa00a14b45e202f306743b0e3289c8da3ebdb4814135eef05d1b789 -size 237 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.obj deleted file mode 100644 index 9152a489..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_1_link_extra.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2b5ee17188c04180cb75461c38d1b1f22efc2927fb61dded60726fcc36c2a255 -size 1006 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.mtl b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.mtl deleted file mode 100644 index ccea6edb..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.mtl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a9702c90bcb44d55dbb6226c5414131d0965d0336827689eb66a364bb2bb333f -size 241 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.obj deleted file mode 100644 index 14dee836..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hand_thumb_2_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0890fb1133c0d003728c0b529cae5e80cf153faa30ca6e159dcc2d0f4f58b6ba -size 16947 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hip_pitch_link.obj b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hip_pitch_link.obj deleted file mode 100644 index 9e02734e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/collision/hands/right_hip_pitch_link.obj +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f87ae770a8ef1343cd89ca3f9774f64e0d73e1f5c69674dfe00069df4fe99771 -size 192161 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/head_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/head_link.STL deleted file mode 100644 index 401f8227..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/head_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:005fb67fbd3eff94aa8bf4a6e83238174e9f91b6721f7111594322f223724411 -size 932784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_pitch_link.STL deleted file mode 100644 index 7cd70521..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d49e3abc6f5b12e532062cd575b87b5ef40cd2a3fc18f54a1ca5bba4f773d51d -size 71184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_roll_link.STL deleted file mode 100644 index cb69f65c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_ankle_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c4092af943141d4d9f74232f3cfa345afc6565f46a077793b8ae0e68b39dc33f -size 653384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_elbow_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_elbow_link.STL deleted file mode 100644 index 7fa05272..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_elbow_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fa752198accd104d5c4c3a01382e45165b944fbbc5acce085059223324e5bed3 -size 88784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_0_link.STL deleted file mode 100644 index a87568d6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_1_link.STL deleted file mode 100644 index c6c91dd4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_index_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_0_link.STL deleted file mode 100644 index a87568d6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6b35f2f77211d5a366f0b9a4e47c4ee35e536731266f1a34e9efa12db579b892 -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_1_link.STL deleted file mode 100644 index c6c91dd4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_middle_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9e315ebc8a7a0cb98e033985b586b20d81cf8aa761181ae61ce56fcb14077a06 -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_palm_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_palm_link.STL deleted file mode 100644 index c8fcc3e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_palm_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:23a486b75bd78a9bf03cec25d84d87f97f3dae038cf21a743b6d469b337e4004 -size 2140184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_0_link.STL deleted file mode 100644 index 7fb38c18..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a90c721661c0622685488a3c74d1e122c7da89242d3a1daef75edb83422d05e0 -size 8884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_1_link.STL deleted file mode 100644 index 54f77542..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:445c54a45bc13ce36001556f66bc0f49c83cb40321205ae4d676bb2874325684 -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_2_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_2_link.STL deleted file mode 100644 index 3e4f124f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hand_thumb_2_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3d8dbe5085acfc213d21aa8b0782e89cd79084e9678f3a85fc7b04a86b029db5 -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_pitch_link.STL deleted file mode 100644 index 4cf7475b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4725168105ee768ee31638ef22b53f6be2d7641bfd7cfefe803488d884776fa4 -size 181684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_roll_link.STL deleted file mode 100644 index 585f6040..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:91f25922ee8a7c3152790051bebad17b4d9cd243569c38fe340285ff93a97acf -size 192184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_yaw_link.STL deleted file mode 100644 index b46a7413..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_hip_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a16d88aa6ddac8083aa7ad55ed317bea44b1fa003d314fba88965b7ed0f3b55b -size 296284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_knee_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_knee_link.STL deleted file mode 100644 index 2dcf84e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_knee_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8d92b9e3d3a636761150bb8025e32514c4602b91c7028d523ee42b3e632de477 -size 854884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_rubber_hand.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_rubber_hand.STL deleted file mode 100644 index 04a2fa22..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_rubber_hand.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cff2221a690fa69303f61fce68f2d155c1517b52efb6ca9262dd56e0bc6e70fe -size 2287484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_pitch_link.STL deleted file mode 100644 index 926d9807..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f0d1cfd02fcf0d42f95e678eeca33da3afbcc366ffba5c052847773ec4f31d52 -size 176784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_roll_link.STL deleted file mode 100644 index 4c6840b9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fb9df21687773522598dc384f1a2945c7519f11cbc8bd372a49170316d6eee88 -size 400284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_yaw_link.STL deleted file mode 100644 index 89b0e066..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_shoulder_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1aa97e9748e924336567992181f78c7cd0652fd52a4afcca3df6b2ef6f9e712e -size 249184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_pitch_link.STL deleted file mode 100644 index f9c9e4f6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b251d8e05047f695d0f536cd78c11973cfa4e78d08cfe82759336cc3471de3a9 -size 85984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_link.STL deleted file mode 100644 index 2097ca3e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:edc387c9a0ba8c2237e9b296d32531426fabeb6f53e58df45c76106bca74148c -size 356184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_rubber_hand.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_rubber_hand.STL deleted file mode 100644 index 7c588197..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_roll_rubber_hand.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e81030abd023bd9e4a308ef376d814a2c12d684d8a7670c335bbd5cd7809c909 -size 3484884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_yaw_link.STL deleted file mode 100644 index 692f4b07..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/left_wrist_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:83f8fb3a726bf9613d65dd14f0f447cb918c3c95b3938042a0c9c09749267d3b -size 318684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/logo_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/logo_link.STL deleted file mode 100644 index 6c25961c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/logo_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8571a0a19bc4916fa55f91449f51d5fdefd751000054865a842449429d5f155b -size 243384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis.STL deleted file mode 100644 index f98a88db..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5ba6bbc888e630550140d3c26763f10206da8c8bd30ed886b8ede41c61f57a31 -size 1060884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis_contour_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis_contour_link.STL deleted file mode 100644 index 8025bc07..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/pelvis_contour_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5cc5c2c7a312329e3feeb2b03d3fc09fc29705bd01864f6767e51be959662420 -size 1805184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_pitch_link.STL deleted file mode 100644 index 94a586a4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:15be426539ec1be70246d4d82a168806db64a41301af8b35c197a33348c787a9 -size 71184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_roll_link.STL deleted file mode 100644 index 3c38507b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_ankle_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4b66222ea56653e627711b56d0a8949b4920da5df091da0ceb343f54e884e3a5 -size 653784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_elbow_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_elbow_link.STL deleted file mode 100644 index 52aa0ebf..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_elbow_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1be925d7aa268bb8fddf5362b9173066890c7d32092c05638608126e59d1e2ab -size 88784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_0_link.STL deleted file mode 100644 index 5e2ef477..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_1_link.STL deleted file mode 100644 index 8572ae1c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_index_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_0_link.STL deleted file mode 100644 index 5e2ef477..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:12ab4300c95e437e834f9aef772b3b431c671bf34338930b52ad11aef73bbd0d -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_1_link.STL deleted file mode 100644 index 8572ae1c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_middle_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c9c34efce4563cacdcfd29fc838a982976d40f5442c71219811dcbbf3923a33d -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_palm_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_palm_link.STL deleted file mode 100644 index fd2f7f0a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_palm_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:86c0b231cc44477d64a6493e5a427ba16617a00738112dd187c652675b086fb9 -size 2140184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_0_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_0_link.STL deleted file mode 100644 index a385d96a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_0_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:544298d0ea1088f5b276a10cc6a6a9e533efdd91594955fdc956c46211d07f83 -size 8884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_1_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_1_link.STL deleted file mode 100644 index c118de72..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_1_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0a9a820da8dd10f298778b714f1364216e8a5976f4fd3a05689ea26327d44bf6 -size 475984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_2_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_2_link.STL deleted file mode 100644 index 0979fb67..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hand_thumb_2_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3f1bfb37668e8f61801c8d25f171fa1949e08666be86c67acad7e0079937cc45 -size 1521784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_pitch_link.STL deleted file mode 100644 index 064085fc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e4f3c99d7f4a7d34eadbef9461fc66e3486cb5442db1ec50c86317d459f1a9c6 -size 181284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_roll_link.STL deleted file mode 100644 index 6544025e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4c254ef66a356f492947f360dd931965477b631e3fcc841f91ccc46d945d54f6 -size 192684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_yaw_link.STL deleted file mode 100644 index 0ad7bee3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_hip_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e479c2936ca2057e9eb2f7dff6c189b7419d7b8484dea0b298cbb36a2a6aa668 -size 296284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_knee_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_knee_link.STL deleted file mode 100644 index 65e8a708..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_knee_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:63c4008449c9bbe701a6e2b557b7a252e90cf3a5abcf54cee46862b9a69f8ec8 -size 852284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_rubber_hand.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_rubber_hand.STL deleted file mode 100644 index 58148fb9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_rubber_hand.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:99533b778bca6246144fa511bb9d4e555e075c641f2a0251e04372869cd99d67 -size 2192684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_pitch_link.STL deleted file mode 100644 index 48a1c460..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:24cdb387e0128dfe602770a81c56cdce3a0181d34d039a11d1aaf8819b7b8c02 -size 176784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_roll_link.STL deleted file mode 100644 index 2a5d22f9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:962b97c48f9ce9e8399f45dd9522e0865d19aa9fd299406b2d475a8fc4a53e81 -size 401884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_yaw_link.STL deleted file mode 100644 index 0882a567..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_shoulder_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a0b76489271da0c72461a344c9ffb0f0c6e64f019ea5014c1624886c442a2fe5 -size 249984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_pitch_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_pitch_link.STL deleted file mode 100644 index 2efd25c4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_pitch_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d22f8f3b3127f15a63e5be1ee273cd5075786c3142f1c3d9f76cbf43d2a26477 -size 79584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_link.STL deleted file mode 100644 index 77d23a77..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a7ee9212ff5b94d6cb7f52bb1bbf3f352194d5b598acff74f4c77d340c5b344f -size 356084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_rubber_hand.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_rubber_hand.STL deleted file mode 100644 index 6f122afa..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_roll_rubber_hand.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0729aff1ac4356f9314de13a46906267642e58bc47f0d8a7f17f6590a6242ccf -size 3481584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_yaw_link.STL deleted file mode 100644 index 77edbb41..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/right_wrist_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bc9dece2d12509707e0057ba2e48df8f3d56db0c79410212963a25e8a50f61a6 -size 341484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_link.STL deleted file mode 100644 index cc2cbbf2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:82be7f93e85b3d303a1d1e1847e2c916939bd61c424ed1ebd28691ec33909dd1 -size 203584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_rod_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_rod_link.STL deleted file mode 100644 index dd439bf0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_L_rod_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c10de1effa7ea797ac268006aa2a739036c7e1f326b2012d711ee2c20c5a6e96 -size 74884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_link.STL deleted file mode 100644 index 422ffe47..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:54ded433a3a0c76027365856fdbd55215643de88846f7d436598a4071e682725 -size 203584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_rod_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_rod_link.STL deleted file mode 100644 index 6df46c89..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_constraint_R_rod_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cb83fd38a9f06c99e3f301c70f12d94ae770a8f0cf9501f83580a27f908990b4 -size 74884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link.STL deleted file mode 100644 index e4fb87c4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e96d023f0368a4e3450b86ca5d4f10227d8141156a373e7da8cb3c93266523e0 -size 2232984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_23dof_rev_1_0.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_23dof_rev_1_0.STL deleted file mode 100644 index edaf96a1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_23dof_rev_1_0.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3cd0d56fde14b73c1623304684805029971c4f84b596f9914e823ca70a107fd2 -size 7825434 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_rev_1_0.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_rev_1_0.STL deleted file mode 100644 index 836b9923..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/torso_link_rev_1_0.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:11ddb46f2098efbbd8816b1d65893632d8e78be936376c7cdcd6771899ccc723 -size 2570584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_L.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_L.STL deleted file mode 100644 index 6ec689bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_L.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8ebafdcb4de6871113f0ca2c356618d6e46b1d50f6c0bf9e37f47b9d8e100d99 -size 114684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_R.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_R.STL deleted file mode 100644 index 69fd76ad..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_constraint_R.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:791902a291ffbd35ab97383b7b44ea5d975de7c80eef838797c970790b382ca9 -size 114684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link.STL deleted file mode 100644 index 007f56d2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:34f0aa73f41131230be4d25876c944fdf6c24d62553f199ff8b980c15e8913df -size 24184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link_rev_1_0.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link_rev_1_0.STL deleted file mode 100644 index 36a5a70e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_roll_link_rev_1_0.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b67c347a05abc3e8ddae600d98a082bee273bb39f8e651647708b0a7140a8a97 -size 85884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_support_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_support_link.STL deleted file mode 100644 index 4a50f94f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_support_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1fae9e1bb609848a1667d32eed8d6083ae443538a306843056a2a660f1b2926a -size 150484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link.STL deleted file mode 100644 index c049debc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2883f20e03f09b669b5b4ce10677ee6b5191c0934b584d7cbaef2d0662856ffb -size 336284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link_rev_1_0.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link_rev_1_0.STL deleted file mode 100644 index dc628fbd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/g1/waist_yaw_link_rev_1_0.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ec6db442b11f25eed898b5add07940c85d804f300de24dcbd264ccd8be7d554c -size 619984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/LICENSE deleted file mode 100644 index b3ccbb77..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/LICENSE +++ /dev/null @@ -1,6 +0,0 @@ -Copyright 2025 Sharpa Group -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. -You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/NOTICE deleted file mode 100644 index e33ef383..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/NOTICE +++ /dev/null @@ -1,7 +0,0 @@ -Portions of this codebase include software, Mujoco, developed by Google-Deepmind (https://github.com/google-deepmind/mujoco): -Copyright 2021 DeepMind Technologies Limited -Box collision code (engine_collision_box.c) is Copyright 2016 Svetoslav Kolev. -Source code is licensed under the Apache License, Version 2.0: -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/DP_elastomer.STL deleted file mode 100644 index 34cc1dc5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4d9ed21e6c4e1e94a43bce9b9ef210cc84bbbe461b4cf637529ec7007b9319bf -size 136684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/MCP_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/MCP_VL.STL deleted file mode 100644 index 7d4d6bbd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/MCP_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:587e34505de3b4f5b7131e6554e0ab92a18a3c28d250ea46eaf728c4539bf77d -size 75184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer.STL deleted file mode 100644 index 7f9cd610..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1611f42070a8fb34226508417bb930af53bbe9e3b619113136b60cb0595ca4ed -size 271584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer_surface.STL deleted file mode 100644 index 115e5cc4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7f88a20e862895e3bf97d35c600b9eb5c2f2e49f8918ef9fe07bd82e6572fae8 -size 362534 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP.STL deleted file mode 100644 index eb632f86..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:09ae5e485d42c651c70d025ef6bcd000f727dd1535836d7d8fa9de0686db4583 -size 101684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP_visual.STL deleted file mode 100644 index bcaa80af..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f21c6a8d12a21b29ff05127ec1987135cc966a283707165d93a30f9e74d4a0ed -size 320284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MCP_VL_visual.STL deleted file mode 100644 index 7c8c128c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0e8b15c4d8426a26cb222b8c1592f5e913c079aebdc0abaecda273b7d8ed0f3a -size 322584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP.STL deleted file mode 100644 index 1a923dfe..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f0a96877af9ec35f20941ae696e9e22cedc6e341703657b0c6d0ebc982897a9 -size 37084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP_visual.STL deleted file mode 100644 index 037edac9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_MP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0e9ac7a8208ab2478cdff8ae40036b93e040a05f89ae0c43ece83a62fdf15293 -size 369384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP.STL deleted file mode 100644 index 20cce91b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6e4434712a5c668dbeda169e727629e95d4a9492fca431126543109451e2484f -size 82484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP_visual.STL deleted file mode 100644 index 8a6afa1c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:881e6eeeaecbf0a8bd6b11fa7f41a5aa139900635fe82b765586460c9a9c1580 -size 559934 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC.STL deleted file mode 100644 index 86b8b5d3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:01e17ed8392d99e76d1c00a849749610c3f7715f18ea12b107ba56eba3541fee -size 174184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual.STL deleted file mode 100644 index 0126b945..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5e2602bd27b43751934549c5c39a09e86a2ca302469671c1530a82e292672288 -size 1201049 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual_.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual_.STL deleted file mode 100644 index 6ad6a213..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_hand_C_MC_visual_.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0e7c42e037b4bc94e35afd2f1c1d3c569d7b14554911bb380fa631691b5d2550 -size 214484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC.STL deleted file mode 100644 index b160cd1f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f7a7192ae246a6b88f9096332216013c8f6be507ee0dc9bfd4444518453b0c13 -size 41984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC_visual.STL deleted file mode 100644 index 3430f51d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_pinky_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:98b520aa40d6f1fff0f433cfc6ef2baddb550a6452a8753301934812d111bbf1 -size 66184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_CMC_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_CMC_VL.STL deleted file mode 100644 index ad57ea17..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_CMC_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c862ce10c4958ef967ed46fed837fafb20dd2c84a23ef9ee4f793576ef1fb93 -size 317984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP.STL deleted file mode 100644 index 45320ec1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7bd0a9187d9cca7c3136d471784db39a3593b5264e16586002f73db42292dc6e -size 66484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP_visual.STL deleted file mode 100644 index 98040c7d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7bac46dd5699d6b2fed60fdb384199cdab7407343d61db3fc8737866096fa440 -size 335484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC.STL deleted file mode 100644 index d6865f49..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3f15b81f22feec350ae2f60a7c69ca36028bd580d6cf4722d4f759d401ea02f9 -size 171084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MCP_VL_visual.STL deleted file mode 100644 index 6a4bb555..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:47f990c04f09174666773687f9e4e1b4031dd150481c1d26719bbba874ae0709 -size 300784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC_visual.STL deleted file mode 100644 index 54875dfa..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:da4fb47f6126ca3b653689aea6d958e08dde7d0c232a3043773e18f447aa121a -size 580084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP.STL deleted file mode 100644 index b484ecf1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2847673ad531a6d1fe3d12b3433d080358727def9acade9f0319c18579676b13 -size 83784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP_visual.STL deleted file mode 100644 index 905ccea7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/left_thumb_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7f4a561b611af82e27bb5c8d6838d574e71e2ede42d25edf1f57c2b54ebde072 -size 772184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_DP_elastomer.STL deleted file mode 100644 index b8c90514..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fb80bbef551c88dcb555a472acc32a9f57467d123fa8a2b3e8f21f1541e5d271 -size 124184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer.STL deleted file mode 100644 index a05d390d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8fb5b2036eda97fe4c2ce50ad8e021775185961c478c0d6f25f5a776588d9b14 -size 270684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer_surface.STL deleted file mode 100644 index 1dbb5d07..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left/thumb_elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1b25efc390f9db9eca87ed167e7baa2e3220a417bbd49924beb077da458c6241 -size 506484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/DP_elastomer.STL deleted file mode 100644 index c0979458..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bc2c52f4d75e52d77ebb41350751304bc4078b4bceee49fec37cd0a0bfabe7dc -size 8884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/MCP_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/MCP_VL.STL deleted file mode 100644 index 02c89a45..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/MCP_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c780c2ce0b69766e9e005493d0db1190aa6a0087ec1e73bcf05668576a6103b -size 7484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer.STL deleted file mode 100644 index 35626808..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:665f81ba39533404966dd72888c5b1e2e938e27a657673b0784cc33ab1bf9237 -size 24184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer_surface.STL deleted file mode 100644 index 354e9bc9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6796dc4bad2b64e034956dee48acb2f4278077e477da94de1d4e0893573200cf -size 19984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP.STL deleted file mode 100644 index f584aa9b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f91744dce0b5de072952d997b3bdcff74a9bf6bc9eb6fefe73d25c4bbac45b8 -size 5984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP_visual.STL deleted file mode 100644 index e1555b4d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:18a860a673323f3b45844b2680a4d83008776af8834cce1ceb8483408315cc8a -size 320284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MCP_VL_visual.STL deleted file mode 100644 index 7c8c128c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0e8b15c4d8426a26cb222b8c1592f5e913c079aebdc0abaecda273b7d8ed0f3a -size 322584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP.STL deleted file mode 100644 index e0206b76..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:15903649bd1210be071e27a6022a82de439c91ebdb3df271a5deacfd0142ae79 -size 3684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP_visual.STL deleted file mode 100644 index e5097242..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_MP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eab2d95ab420ec6c872b5c7be28138b3eac342d0c6f3902c94b182aa86cb3c37 -size 369384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP.STL deleted file mode 100644 index cfc58682..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6736c44bd2e481d00340f65b4c517ce685fbf0c49fe7c03547a454aeffcb4439 -size 8284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP_visual.STL deleted file mode 100644 index b66b9e47..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1228664a6c513912f02b5d5b3718b464f5a996e4c44156b544a0290fc4264816 -size 559934 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC.STL deleted file mode 100644 index 939a18d2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:aeb16c79aab499e122ca337558def72b795ff901e2782a26f4c62df56c3cb917 -size 4384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_cut.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_cut.STL deleted file mode 100644 index e77881d6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_cut.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c1dbb13c1c6c279a4f55dfb28c39bc7e5af82db5c96b761376faa6eb6276d3ec -size 7984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual.STL deleted file mode 100644 index 4e57e6db..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:cf4fca6003ce167b414bc09e0ea16d316acde8216e6e093b40da44e58f1fa1e7 -size 285084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual_.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual_.STL deleted file mode 100644 index 6ad6a213..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_hand_C_MC_visual_.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0e7c42e037b4bc94e35afd2f1c1d3c569d7b14554911bb380fa631691b5d2550 -size 214484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC.STL deleted file mode 100644 index 5f744965..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:90286de188ff9ce5736dea105435921931789e4051d731a97d4e8ff115d9f4e9 -size 4184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC_visual.STL deleted file mode 100644 index f95f64d7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_pinky_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:96044387d460dea45dc5761b699400b1c97152f444083adbae18e7d273b26eb7 -size 66184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_CMC_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_CMC_VL.STL deleted file mode 100644 index 5a0cf93b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_CMC_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fd49dac3c518a9d882919e0e881a1756778c9d4556646b8cbd6b60c310a7120b -size 8684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP.STL deleted file mode 100644 index 29d41b48..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3a784cb6edff071682fd357557ba5064f222d5c5474a2ce616534679b7dc13a7 -size 3984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP_visual.STL deleted file mode 100644 index 916b43f0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0fed5a9ed33df5a38cc429837393bf6eda8fb8aad38cc804ba66a60ded278128 -size 335484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC.STL deleted file mode 100644 index 6aec42ed..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ce88167cde32e52250e95eac5b331031a2f385e16c761867d1d964a3df084658 -size 17084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MCP_VL_visual.STL deleted file mode 100644 index 6a4bb555..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:47f990c04f09174666773687f9e4e1b4031dd150481c1d26719bbba874ae0709 -size 300784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_cut.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_cut.STL deleted file mode 100644 index 65a1f2ad..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_cut.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:79d50b4a2554fc308ad3d9d3bb2094d28d1f2bd4b0135078cd8a104a9c429fa0 -size 19184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_visual.STL deleted file mode 100644 index 3244b10c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:151ceb60f87b9113996752f00add79821b6d5f1d437c97e3428b89c12b4b9ddb -size 580084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP.STL deleted file mode 100644 index a8f63749..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2076b34cd0576d6c4280eea04052e8515f7c35126e6ab81b5eedc82675cd0ab1 -size 8384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP_visual.STL deleted file mode 100644 index 905ccea7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/left_thumb_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7f4a561b611af82e27bb5c8d6838d574e71e2ede42d25edf1f57c2b54ebde072 -size 772184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_DP_elastomer.STL deleted file mode 100644 index eebdc9ed..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:030eea99379e96e37db932a9f536c27bc7f20f2c861e9771e78e91464191142d -size 9884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer.STL deleted file mode 100644 index f12a69e3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9eefe13dad72a16f469fd69bbf8a1df53d39e8176e2d072718ab09d40f8b1625 -size 26284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer_surface.STL deleted file mode 100644 index 0aa039b3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/left_cvx/thumb_elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8e7a96c154603ee3cba8772d736832c8a6b9508b5805d53007079abfa4014f9e -size 22684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/DP_elastomer.STL deleted file mode 100644 index 34cc1dc5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4d9ed21e6c4e1e94a43bce9b9ef210cc84bbbe461b4cf637529ec7007b9319bf -size 136684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/MCP_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/MCP_VL.STL deleted file mode 100644 index 4b845fee..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/MCP_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:350f66d656fef760a7f4f0c4a026951e9d831ca65a9c5e4e34825b694ed6b11a -size 75184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer.STL deleted file mode 100644 index 7f9cd610..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1611f42070a8fb34226508417bb930af53bbe9e3b619113136b60cb0595ca4ed -size 271584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer_surface.STL deleted file mode 100644 index 115e5cc4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7f88a20e862895e3bf97d35c600b9eb5c2f2e49f8918ef9fe07bd82e6572fae8 -size 362534 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP.STL deleted file mode 100644 index eb632f86..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:09ae5e485d42c651c70d025ef6bcd000f727dd1535836d7d8fa9de0686db4583 -size 101684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP_visual.STL deleted file mode 100644 index bcaa80af..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f21c6a8d12a21b29ff05127ec1987135cc966a283707165d93a30f9e74d4a0ed -size 320284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MCP_VL_visual.STL deleted file mode 100644 index 395831d7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ec48548aaafa6589112e9b609ed58f1b5e7ec0157ca15e0e4e8c93bff706a9d0 -size 322584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP.STL deleted file mode 100644 index 3a897e66..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:98ad7e6e990f61857d6ad28860bba7e0dcba78761a513d60fdc527e9d56bbe8e -size 37084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP_visual.STL deleted file mode 100644 index 53fe711e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_MP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:af867d754ae0f3d5c38610f72cf4699789c82bb0391c2a406e6924f76e850ec6 -size 369384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP.STL deleted file mode 100644 index 4ad70a34..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ed975b59e2569759ff28a397dab71d84667161d3b90b41d625bc5e1f5ac6536c -size 81784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP_visual.STL deleted file mode 100644 index d83073e8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:aa8b0ac6b6a7769033daa149416035712b23b79af0a7ddeb83d62142f8ff8a14 -size 560734 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC.STL deleted file mode 100644 index 57660985..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8c52941db271e9331f0b32ec581c078af7f8cb372fb97f021d5b1dec2d2426ae -size 174184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual.STL deleted file mode 100644 index 9fca66ed..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6cfe63272fa96b016cb482915c5ae578f63dfc5e000a84f64936a0e04717b505 -size 1197032 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual_.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual_.STL deleted file mode 100644 index a22cd649..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_hand_C_MC_visual_.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:496da9148b99a003ebbbb2eab879c3b9ab1509f74294599f22d79892e82879a4 -size 214484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC.STL deleted file mode 100644 index d284e130..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1f8207aff0f2f6ca2f1de1a8bbbbcd4183300bc3778d55821c2aae9b490b7dd3 -size 41884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC_visual.STL deleted file mode 100644 index ce80e4d4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_pinky_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4b3902eda84c7f10cc7fd45f0512264a731d52ed8b9be91bf2fc9981d0e7c03d -size 66184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_CMC_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_CMC_VL_visual.STL deleted file mode 100644 index eb860046..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_CMC_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ebaf499965a51e4b539b7ec5481efbddaf2003b69d0e5e72425a430cecd187b0 -size 317984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP.STL deleted file mode 100644 index 45320ec1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7bd0a9187d9cca7c3136d471784db39a3593b5264e16586002f73db42292dc6e -size 66484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP_visual.STL deleted file mode 100644 index 98040c7d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7bac46dd5699d6b2fed60fdb384199cdab7407343d61db3fc8737866096fa440 -size 335484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC.STL deleted file mode 100644 index 29db11e6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:deb6d97eb2d7223d72c23ebb32f874644dcf78848389f14dcabe3016aae5dd6c -size 149584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MCP_VL_visual.STL deleted file mode 100644 index e58fba88..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:30d656738130093b7023100c73fac065a173aaba2e4314034c050c984005d634 -size 300784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC_visual.STL deleted file mode 100644 index 6bf9abf4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:81f4bf8c39117b7bf7a4f0fdeb577948056139a84927adc55e9b049434a41a3b -size 625334 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP.STL deleted file mode 100644 index 9ed69f2c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:795b35f943c4272314d5e72497aaea2ef998458b0259a6609577d3694aeff869 -size 85884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP_visual.STL deleted file mode 100644 index 66b583ea..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/right_thumb_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1024905f9af3ad14160ce55d2e1d98fb7028274f7b267bee8b057754eb165faa -size 719834 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_DP_elastomer.STL deleted file mode 100644 index b8c90514..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:fb80bbef551c88dcb555a472acc32a9f57467d123fa8a2b3e8f21f1541e5d271 -size 124184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer.STL deleted file mode 100644 index a05d390d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8fb5b2036eda97fe4c2ce50ad8e021775185961c478c0d6f25f5a776588d9b14 -size 270684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer_surface.STL deleted file mode 100644 index 1dbb5d07..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right/thumb_elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1b25efc390f9db9eca87ed167e7baa2e3220a417bbd49924beb077da458c6241 -size 506484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/DP_elastomer.STL deleted file mode 100644 index c0979458..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bc2c52f4d75e52d77ebb41350751304bc4078b4bceee49fec37cd0a0bfabe7dc -size 8884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/MCP_VL.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/MCP_VL.STL deleted file mode 100644 index 0e33933e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/MCP_VL.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c0d59b020685550426fc7cea63282021bf562105507cc914194d7de7e2c14dde -size 7484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer.STL deleted file mode 100644 index 35626808..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:665f81ba39533404966dd72888c5b1e2e938e27a657673b0784cc33ab1bf9237 -size 24184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer_surface.STL deleted file mode 100644 index 354e9bc9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6796dc4bad2b64e034956dee48acb2f4278077e477da94de1d4e0893573200cf -size 19984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP.STL deleted file mode 100644 index f584aa9b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6f91744dce0b5de072952d997b3bdcff74a9bf6bc9eb6fefe73d25c4bbac45b8 -size 5984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP_visual.STL deleted file mode 100644 index e1555b4d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:18a860a673323f3b45844b2680a4d83008776af8834cce1ceb8483408315cc8a -size 320284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MCP_VL_visual.STL deleted file mode 100644 index 395831d7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ec48548aaafa6589112e9b609ed58f1b5e7ec0157ca15e0e4e8c93bff706a9d0 -size 322584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP.STL deleted file mode 100644 index d6fe5e60..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:90f791d7f6f9f8f5cd05ba55a5a271a06dd7bc9ca8e3d2763efaee2084d565c6 -size 3684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP_visual.STL deleted file mode 100644 index 74456a60..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_MP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:04c105813db0489d3467fcdced6058342b711fb951d8deb5c50445464bb12f3e -size 369384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP.STL deleted file mode 100644 index ebbea0a0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1d8ee6291531ce2193f1e3490a6d4554ab37e0230e0f41e329de315058d664cd -size 8184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP_visual.STL deleted file mode 100644 index c967761c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:798f4224031ba2e382a118ad31bdaad5199a938b57af87a565de4e53a100d6e2 -size 560734 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC.STL deleted file mode 100644 index 4e17b9f4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:eb4e37f595e6a721c93c7fcf5b10b6e1c2b1f0857b2e979054ece62092391b3d -size 4384 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_cut.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_cut.STL deleted file mode 100644 index 7c7be1d6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_cut.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e39653a9223259066e7de85343dbb4b72c8d961e20c215ee4234dbbafd39ade7 -size 6084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual.STL deleted file mode 100644 index 4f26aaf9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:37833d9a7dc83fd33ccb3c533ef0d44b4279946c8e264bd1998b8d534bbaf878 -size 285084 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual_.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual_.STL deleted file mode 100644 index a22cd649..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_hand_C_MC_visual_.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:496da9148b99a003ebbbb2eab879c3b9ab1509f74294599f22d79892e82879a4 -size 214484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC.STL deleted file mode 100644 index 81cbeb62..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1aad346f1aa1b6dd90c2ded5136de1dba3e6a44ce782b8841b482e5fe64b2583 -size 4184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC_visual.STL deleted file mode 100644 index 2b610ce6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_pinky_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3527221367ca9ca669a215186bb2df5469e2fcfd8a3219d2d9c44678f3d82df6 -size 66184 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_CMC_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_CMC_VL_visual.STL deleted file mode 100644 index eb860046..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_CMC_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ebaf499965a51e4b539b7ec5481efbddaf2003b69d0e5e72425a430cecd187b0 -size 317984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP.STL deleted file mode 100644 index 29d41b48..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3a784cb6edff071682fd357557ba5064f222d5c5474a2ce616534679b7dc13a7 -size 3984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP_visual.STL deleted file mode 100644 index 916b43f0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_DP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0fed5a9ed33df5a38cc429837393bf6eda8fb8aad38cc804ba66a60ded278128 -size 335484 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC.STL deleted file mode 100644 index 604f4696..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:297316c8d224f1f6633a30760574aea17a1cc7451826807ad21ee5f9ae307487 -size 14984 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MCP_VL_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MCP_VL_visual.STL deleted file mode 100644 index e58fba88..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MCP_VL_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:30d656738130093b7023100c73fac065a173aaba2e4314034c050c984005d634 -size 300784 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_cut.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_cut.STL deleted file mode 100644 index 83f11afc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_cut.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3641ed9abd22fe3dd92693ed6eeab767bdf48fdf23c5952a0c3cd5de0635d255 -size 16284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_visual.STL deleted file mode 100644 index 50b7abb0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_MC_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b1c5a8747735499c41c68c956ac783e67cf97393dc0679217cc834108f2517c3 -size 625334 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP.STL deleted file mode 100644 index 9e01226f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1e52f3351cc018c22168e589d6afa92c29b4996147f8973807802b14680342cb -size 8584 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP_visual.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP_visual.STL deleted file mode 100644 index 66b583ea..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/right_thumb_PP_visual.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:1024905f9af3ad14160ce55d2e1d98fb7028274f7b267bee8b057754eb165faa -size 719834 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_DP_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_DP_elastomer.STL deleted file mode 100644 index eebdc9ed..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_DP_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:030eea99379e96e37db932a9f536c27bc7f20f2c861e9771e78e91464191142d -size 9884 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer.STL deleted file mode 100644 index f12a69e3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9eefe13dad72a16f469fd69bbf8a1df53d39e8176e2d072718ab09d40f8b1625 -size 26284 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer_surface.STL b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer_surface.STL deleted file mode 100644 index 0aa039b3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/meshes/sharpa_wave/right_cvx/thumb_elastomer_surface.STL +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8e7a96c154603ee3cba8772d736832c8a6b9508b5805d53007079abfa4014f9e -size 22684 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/apple_pick_and_place_retarget_motion_w_body.h5 b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/apple_pick_and_place_retarget_motion_w_body.h5 deleted file mode 100644 index fbbe6737..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/apple_pick_and_place_retarget_motion_w_body.h5 +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5b3251f9f27c2d230721491cca154c681079c0859260d5ff066cc737ebec6d8c -size 1264000 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/nvhuman_dex3/bottle_handover.parquet b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/nvhuman_dex3/bottle_handover.parquet deleted file mode 100644 index 12b20b74..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/nvhuman_dex3/bottle_handover.parquet +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:f5ba2d1d210a7db8423e322f7b93c21ecd0430240dc5e179a5a6906a0ba0b47f -size 1777621 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion.yaml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion.yaml deleted file mode 100644 index d98b738a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion.yaml +++ /dev/null @@ -1,1071 +0,0 @@ -object_position: - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] - - [0.3654678449446503, 0.27083571689194097, 0.7123902039477208] - - [0.35343330136166917, 0.2676947272589954, 0.6891869578489374] - - [0.3580753322222763, 0.2606598891118305, 0.6947623694765742] - - [0.34853646959868084, 0.2591647346609303, 0.6795713436110852] - - [0.3613238362236649, 0.2515495500579143, 0.6828991693288131] - - [0.365112918033181, 0.2660998585368929, 0.6922504722726259] - - [0.37301536111086303, 0.2564471204102369, 0.6783268376822932] - - [0.3576267822213244, 0.26571979825114556, 0.6797506829922955] - - [0.36905637280350945, 0.24727574658503548, 0.6744869506636341] - - [0.3655675189258597, 0.25229003982548603, 0.6839067545328893] - - [0.3731897212415215, 0.2403275520988762, 0.6855015165178178] - - [0.36543937621788974, 0.2576389423604626, 0.6815629188512311] - - [0.3666567693357265, 0.25354788948776785, 0.671554054086742] - - [0.3686706070774893, 0.2593485757435323, 0.6872945332353319] - - [0.3696371248473067, 0.2610534304667004, 0.6672681738987404] - - [0.36330272831355553, 0.26234989702640615, 0.6734190694898584] - - [0.37573006930058456, 0.26370981807805793, 0.6685411504449197] - - [0.36399227889620167, 0.2591931254698121, 0.6705076748671268] - - [0.36938243677471677, 0.25774851345123784, 0.6813860790982907] - - [0.3569010080359717, 0.2573133981979947, 0.6635840325238366] - - [0.3596790334481823, 0.25645374675032073, 0.6696666968556774] - - [0.37925621579704694, 0.2509744501819136, 0.6698534928458255] - - [0.3547559636982936, 0.2597455922763272, 0.6603991954788793] - - [0.37189936767991805, 0.2530740579757589, 0.6840515389394642] - - [0.36715930061735563, 0.2534732452282263, 0.6791641535053454] - - [0.3622086381576212, 0.2509939481911423, 0.6689535162932935] - - [0.3784036487517679, 0.23623323588195055, 0.6699559316084034] - - [0.3636805741919496, 0.25462305593887247, 0.6897949824885561] - - [0.3574738367765595, 0.25117068852613345, 0.691406102758197] - - [0.3669442861198287, 0.2657440160736859, 0.6990559593872422] - - [0.3586515678914662, 0.255114387945912, 0.7072836850402863] - - [0.3508286277664592, 0.25636849750158397, 0.6947649949820608] - - [0.34415380732940065, 0.2652764009365778, 0.7098634258594707] - - [0.3514649221145531, 0.25224872279151433, 0.7041184507848504] - - [0.34375779125523065, 0.26131142754936404, 0.6812032932781579] - - [0.33388578143810216, 0.2598418848795022, 0.706632575409802] - - [0.33070741995718633, 0.26623656050823485, 0.7298749161165817] - - [0.3202197012696429, 0.2660999282846986, 0.7296387124428193] - - [0.3022560357577599, 0.26476540510525337, 0.7086241166076156] - - [0.3031634832088993, 0.26991341197737095, 0.7200395581652771] - - [0.3024169341684016, 0.2681498954114002, 0.7343370091621236] - - [0.3038710768589056, 0.26030366948493183, 0.7455069502449251] - - [0.2689100241550805, 0.2699620592152266, 0.7257056124968101] - - [0.2770353370624902, 0.2567116178976531, 0.7578528544959162] - - [0.259373150164849, 0.27304781068030415, 0.7428843586426046] - - [0.25340338489032566, 0.25849990565822734, 0.73656559247729] - - [0.24434370030696337, 0.26262240639181067, 0.7511423849257641] - - [0.22803624363697556, 0.25132720213939364, 0.7410798173672574] - - [0.21204537765824663, 0.24912874835952334, 0.7397427656387849] - - [0.20995932647562243, 0.25019356773908086, 0.7528571600471337] - - [0.200761011912441, 0.2496579982515765, 0.7566874269674848] - - [0.18403092050127298, 0.24713662214740587, 0.7585144053847769] - - [0.17595722423280855, 0.23894277013556825, 0.7642739201498885] - - [0.1743560750551203, 0.23110247979926046, 0.7615022043774821] - - [0.15538066822226204, 0.22954527372173603, 0.7602694487252402] - - [0.10668589916156815, 0.24522063464294164, 0.7405679998700492] - - [0.10242134340679784, 0.235234350036359, 0.7507418429678709] - - [0.08844394644033285, 0.23807690153881866, 0.755094320523109] - - [0.07661945988240028, 0.23204003824656663, 0.7609622262485991] - - [0.06327128332137398, 0.2231092919345461, 0.768545987888046] - - [0.036299634763728016, 0.23389850724070974, 0.757693860353231] - - [0.012642178473512088, 0.23632323749753126, 0.7590364893873779] - - [0.01644711681694952, 0.25079996814238353, 0.7881402541200607] - - [-0.02127447891425078, 0.23397069656188918, 0.7690579615327696] - - [-0.03478110475429892, 0.22501064134502444, 0.7645421087948664] - - [-0.03222096557925447, 0.21568732701550491, 0.7722459719629968] - - [-0.04823178674952687, 0.23320096654031078, 0.7782206109596355] - - [-0.05882563828982537, 0.22293345150146038, 0.7907754188942333] - - [-0.0785595279594006, 0.20610242739191217, 0.7817042927554307] - - [-0.11998329315926456, 0.2066347911363635, 0.7632476347841851] - - [-0.12118981406652175, 0.20725333241677973, 0.7829427938840454] - - [-0.1306507483896991, 0.2061671227780749, 0.7833670243215672] - - [-0.13767118673995662, 0.19892433476690646, 0.7811924204307111] - - [-0.1513088096597185, 0.19378776702270578, 0.7766495880898804] - - [-0.1748258727310317, 0.19609486223070174, 0.7875735740987492] - - [-0.18831929264320585, 0.18734713299421332, 0.7786601925095801] - - [-0.19547648301197967, 0.1896874589696435, 0.7825365948779462] - - [-0.18830769910976622, 0.2006376958275675, 0.8098876942718318] - - [-0.22865332377376762, 0.17843638025852648, 0.7742882673631809] - - [-0.23861542674950426, 0.1802469933729485, 0.7755839061495948] - - [-0.2622714343686138, 0.1761849354639361, 0.7721663449878048] - - [-0.2664116860586259, 0.18632226646102548, 0.7727720761911641] - - [-0.25529307201491447, 0.17520286742909202, 0.7754437799371152] - - [-0.29455657555625603, 0.1902176518680139, 0.7623058309425695] - - [-0.28144277084663377, 0.1817865157254769, 0.7632128021816443] - - [-0.3191521181814584, 0.17924245207511338, 0.7532912298349408] - - [-0.3200219517551228, 0.1948081932375949, 0.7534559687016034] - - [-0.3270246083101625, 0.1880563718475679, 0.7467519586982446] - - [-0.3354521307350213, 0.2010200219602767, 0.7502101453804773] - - [-0.35692220272205655, 0.20797727330472407, 0.7490539709480789] - - [-0.3449204811193346, 0.21946132033179086, 0.7529766960825062] - - [-0.3700673358203852, 0.21135217014873114, 0.7306359177580843] - - [-0.37706132924939584, 0.21084494135408965, 0.7342465714983679] - - [-0.3876917817211071, 0.21588199217731405, 0.7213427954768146] - - [-0.3925378133903767, 0.20887555453836243, 0.7266610256544551] - - [-0.38321933066934355, 0.20976891366861578, 0.7171220874451497] - - [-0.3641821216603489, 0.20858208483011767, 0.7669943097217211] - - [-0.39431691170638516, 0.21880461037863858, 0.7178237874630407] - - [-0.4100845812791278, 0.21848411880795338, 0.716046091630827] - - [-0.40813152869806907, 0.2256149704766053, 0.7188171043274401] - - [-0.4275576716869856, 0.2248410765565425, 0.7036023435873199] - - [-0.4249023368108643, 0.22755149098544872, 0.6911250297681938] - - [-0.41118909774267104, 0.23322544480729124, 0.7086713879762646] - - [-0.43262607882234566, 0.23196999204660804, 0.6996088817024575] - - [-0.4262272422233491, 0.23381763048205695, 0.6978848207013544] - - [-0.42699845216053, 0.23420278107586592, 0.6976439149960909] - - [-0.40626273633055326, 0.2256713873200534, 0.7042913507609633] - - [-0.448976983225292, 0.2497500028066722, 0.6723406803818024] - - [-0.4375351601915942, 0.23302543028197972, 0.6843437619293636] - - [-0.43764093212512556, 0.23604089806604414, 0.6921961477116657] - - [-0.43021737063952376, 0.23788018692154045, 0.6796016612172963] - - [-0.4350185996476702, 0.2339868965123215, 0.6824667489453323] - - [-0.4188027855185873, 0.21643689145775424, 0.6820563424624957] - - [-0.42078724995572264, 0.2373689075524551, 0.6948126621977226] - - [-0.43939068603444437, 0.23331981573388338, 0.6752346242175861] - - [-0.44183280929652674, 0.23208928316327526, 0.6775795742278429] - - [-0.4368151469051918, 0.23611922013282943, 0.686784969065869] - - [-0.43444799095169273, 0.23674285732619346, 0.670614013760267] - - [-0.4500289476195402, 0.23687697595473753, 0.6690593052735959] - - [-0.44047167450695246, 0.24475459833934637, 0.6619513269894545] - - [-0.4283750093598685, 0.23695725400557827, 0.6761944346573537] - - [-0.4355417674774262, 0.23975700039316342, 0.6612304425561402] - - [-0.43864497556401066, 0.2441586285460394, 0.6733412332520416] - - [-0.428238189507912, 0.24274588730926708, 0.6727912506838885] - - [-0.4444419991949541, 0.2620239203893813, 0.6636920430328761] - - [-0.43306566985930844, 0.24228913959334508, 0.6667703217202264] - - [-0.4331044951071657, 0.23786889709073517, 0.6597827117354796] - - [-0.4464188979172558, 0.23333970775770652, 0.6589400219519173] - - [-0.4249703689812572, 0.23268368513314336, 0.6700226845824775] - - [-0.44292647383081307, 0.23723707966844704, 0.6639993540806983] - - [-0.426593811101786, 0.2306030312657051, 0.6777753311661375] - - [-0.4460285430873926, 0.2378310679032445, 0.6513702489176343] - - [-0.43887482551324747, 0.23694781525900063, 0.6719631494848735] - - [-0.436316700724553, 0.23844903361613162, 0.6474394208179762] - - [-0.4318341400737083, 0.22343701065492977, 0.6592280744825609] - - [-0.43747378260062575, 0.2390440762024321, 0.6692725011971123] - - [-0.43493870114625227, 0.2103906704759906, 0.6626643380689304] - - [-0.4376194121758833, 0.23618400722481583, 0.6728283581901285] - - [-0.4228050551831278, 0.21108382514981883, 0.6739694347547257] - - [-0.4289658421685276, 0.22962645381703745, 0.6699106767660706] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] - - [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -object_wxyz: - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7078781754070083, 0.7063298414467313, 0.0005256632221748724, 0.0025233989888942517] - - [0.7096534703658574, 0.7045504873719239, -0.0007453341053435865, -8.496323396065958e-05] - - [0.7089769661576292, 0.7052226075340082, 0.002829261863289615, 0.0021749846360950096] - - [0.7065745426915899, 0.707623736629659, 0.004586371861778378, 0.00016784395999345803] - - [0.710691156119896, 0.7034896279324402, 0.004494049392536361, 0.0004769952827735559] - - [0.7082673751931439, 0.7059283761503434, -0.0036491606065423917, 0.0030226825207542527] - - [0.7055738609017788, 0.7086356960698464, -1.0903453093818506e-05, 0.0009884072422404338] - - [0.7085940698318786, 0.7056140495115426, -0.0017449416480597175, -0.00046098770269842966] - - [0.7060320377758478, 0.7081761386414522, -0.002162594329628692, 0.000800923674187634] - - [0.7055903216239855, 0.7086029370090738, 0.0040531691090639884, 0.0027834354399494943] - - [0.7069420807158608, 0.7072682074204896, -0.0010139989314884238, -0.0018839032627905891] - - [0.7033403377606438, 0.7108501265363549, -0.0012547241078773143, 0.0017007496596155034] - - [0.706527464330576, 0.7076563998905269, -0.0062301388642870955, 0.0015959982195660313] - - [0.7067877341478787, 0.7074205236731246, -0.002398326492871343, 0.0012448189359537812] - - [0.7067848859215027, 0.7074260027891754, 0.0006919440378811757, 0.001759779561864234] - - [0.7067094563338882, 0.7075021404980689, 0.0014586299829544187, -0.0005813067038361327] - - [0.7052132126784841, 0.7089860579436136, 0.003564599575321929, -0.0006228443896901096] - - [0.709571209546562, 0.704628796979837, -0.0016015571145962572, 0.0020957252170087973] - - [0.7070969902059514, 0.7070970972854367, 0.0014021053635409357, 0.005057227772615045] - - [0.7045694015230883, 0.7096312089722451, 0.0012947049774798523, -0.001956892692047418] - - [0.7064115916434005, 0.7077920387898082, -0.0020923930605162484, 0.002952102451537807] - - [0.7091178297891, 0.7050898103937011, -0.00048438006684565723, 0.00016772037046392004] - - [0.7079016472246508, 0.7062935030457264, -0.00324333642153471, 0.0037717609785692502] - - [0.7048154376623911, 0.7093857814164096, -0.0025786493310469257, -0.0006021001822747965] - - [0.707877817956212, 0.7063233643979483, -0.002820914772415927, -0.0028882851247625436] - - [0.7068095923125308, 0.7074027184760067, -0.0004832221384717243, -0.0011664493621831524] - - [0.7072294800697158, 0.7069832633304732, -0.0008857179762611753, -0.000585998789311155] - - [0.7061005686503387, 0.7081086468090357, 0.0019796873432071705, -0.0004605474655052339] - - [0.7066088303687388, 0.7075895384532807, 0.0011092738036790075, 0.004446957164690589] - - [0.7047336720104943, 0.7094719135366223, -0.00022208577196788804, 7.819973357275422e-05] - - [0.711422500574131, 0.7027520798846655, -0.0033378401209544347, 0.0025295687289008124] - - [0.7089759980652884, 0.705230578560603, -0.0014698770579545432, 0.0008394592544134925] - - [0.7083444636703944, 0.7058623884672813, 0.002150885498347612, -0.0013353000932240784] - - [0.7056000726091965, 0.7085986894542681, -0.0033407967229203833, -0.0022965005653047843] - - [0.7108121463339951, 0.703379598601931, 0.0004960514391686799, -0.0017282440844639199] - - [0.7069362599741067, 0.7072496554596939, 0.004476486222297707, -0.00436007535120928] - - [0.7093288463102598, 0.7048652924723025, 0.0038827573766375962, -0.0015593124271643464] - - [0.7065917260214504, 0.7076067341851502, -0.004154629142726029, 0.0018924879234360613] - - [0.7061590127535996, 0.7080382014142148, 0.0005093151615089344, -0.004592890489851818] - - [0.7052041852378885, 0.7089969727850437, 0.0008356626073248635, -0.0031066658360060406] - - [0.7105370929657358, 0.7036423057171203, -0.0030664303436345954, -0.0038912889694310594] - - [0.703789415523934, 0.710404905057886, 0.0003274366830659713, -0.002285224582776638] - - [0.7062357778039581, 0.7079699125898592, 0.002956491475870079, -0.00094243052722014] - - [0.7070402040592173, 0.7071732870054823, 0.00010117699284686246, 0.00028592477768178335] - - [0.7028226828657683, 0.7113475294544312, -0.004745797612266157, 0.001564031158275122] - - [0.7081530040523331, 0.7060514396255889, -0.001708055951163866, 0.002787471821835302] - - [0.7091598295528441, 0.7050461198867356, -0.0014881178307291697, -0.0003008098437059801] - - [0.7059844365500993, 0.7082272540543811, 0.000363267551176032, 5.620000505534747e-07] - - [0.7097763889227704, 0.7044198849816469, 0.002739026474825937, 0.0016127939005445657] - - [0.7077257636298117, 0.7064678664860871, -0.004428103753979303, -0.002790880567158402] - - [0.7066259770097202, 0.7075841330380537, -0.0017485417247680702, -0.0011687128308319281] - - [0.7053386844194263, 0.7088661495034443, -0.0018959360191705826, 0.0015898981868894234] - - [0.7058905177422703, 0.7083100344076024, -0.003863354326466691, 0.0007393324006979583] - - [0.7073418095842207, 0.7068714355548645, -0.00027926078115859173, 0.0005099256232087339] - - [0.7060872068998136, 0.7081218207832329, -0.0010658762612044217, 0.001790835239287523] - - [0.7091749981250707, 0.7050319231743399, -0.0002362073751717942, -0.0008680699705398001] - - [0.706883735562542, 0.7073204607661815, -0.0025900036073677447, -0.002538121347908791] - - [0.7116099452230099, 0.702537885064474, 0.004754293768890069, -0.005403942967862458] - - [0.7024536635194909, 0.7117161791224549, -0.004244322710686302, 0.0009574487490384224] - - [0.7070031130047884, 0.7071880732166029, -6.704570088268698e-05, 0.005623415919274848] - - [0.7082846652397833, 0.7059137494203952, -0.002764729533331554, -0.003311742142010889] - - [0.7071234261840476, 0.7070769095408621, 0.002560430921398188, 0.003485445362386216] - - [0.7062398033729321, 0.7079674505345758, 0.0025435743110631096, 0.0009794616170392617] - - [0.7117963537571297, 0.702376571687145, 0.003520767688463672, 0.0008405462223753271] - - [0.7056217197339316, 0.7085779554425357, 0.0031203234296144134, -0.0023522929876078325] - - [0.7095318017857614, 0.7046695384737068, 0.001783512358405895, -0.0015109220930598635] - - [0.7068251634964783, 0.7073825761934662, 0.0020023923423668944, 0.0020173177684519573] - - [0.7082670496423162, 0.7059402915925937, -0.00037068457978489563, -0.0024400184578755] - - [0.7069331603940732, 0.7072734976898226, -0.002743810641911471, 0.0014757045819037418] - - [0.7048630924444981, 0.7093333716076277, 0.003396749183812091, 0.00162816716475711] - - [0.711621133914919, 0.7025512818677119, -0.0024120128105698606, -0.003352656540923419] - - [0.7080807991502208, 0.706127896788494, 0.0011932901787589769, 0.0018844920779500084] - - [0.7060199183202774, 0.7081688137429927, 0.005484688540127801, 0.001650566111673311] - - [0.7095235584998059, 0.7046582491096992, -0.005701525533012069, -0.0007513334960694543] - - [0.7041059664154529, 0.7100935407197463, -0.001074695542515423, 0.0008924773084155721] - - [0.7049981922948214, 0.7092088852143806, -0.00039055738393668895, -0.00039173835592962674] - - [0.7084195331847454, 0.7057812980364728, -0.00230261347943141, -0.003036826538253605] - - [0.7091999184796288, 0.7050062816504145, -0.001172220885554474, -0.0004943279678280031] - - [0.7066901340609599, 0.7075216319223557, -0.0014802143698691439, 6.122294000821235e-05] - - [0.7068219645717508, 0.7073897712396169, 0.0013665794067860492, 0.0007445839228116789] - - [0.7076751871739634, 0.7065296872747949, 0.0012116480907223119, -0.003187846699703496] - - [0.7065916629323138, 0.7075930417642883, -0.004277053831474694, 0.004692113791503374] - - [0.7044056962632044, 0.7097973382350786, 0.0002935439282084853, 0.0005172410556602997] - - [0.7090614110447687, 0.7051441896263417, -0.0008657077397639701, 0.0016845633027366348] - - [0.7030282777890581, 0.7111480738845647, -0.0028267714677573377, 0.003415699385705937] - - [0.7061697170432334, 0.7080238783460573, 0.0051475570080764844, 0.00014518862098509126] - - [0.7086918478911568, 0.7055176632870725, 0.0007285512611762662, -0.00040091847818798227] - - [0.7084438017775468, 0.7057664893179642, -6.121905631941957e-06, -0.0010209021961127538] - - [0.7054795651681202, 0.7087302024974137, 0.00017093398311598468, 0.00023233542257735968] - - [0.7116438823138028, 0.7025399460486367, -0.0007781980365983952, 5.813031117342254e-05] - - [0.7054945268762791, 0.7087110116416173, 0.0020611086015349606, 0.0013879325859359496] - - [0.7072661488936405, 0.7069446652926064, -2.7480876112873348e-05, -0.0019580828084444705] - - [0.7047086950752476, 0.7094946722100725, 0.0012781172161500374, 0.0011539528783306937] - - [0.7078198282319424, 0.7063878698123424, 0.002688473471315256, 0.00020063419144381774] - - [0.7043453663135159, 0.7098495807310653, -0.002090746912495437, 0.002608920458271913] - - [0.7047322368472637, 0.7094531336046136, 0.005112513693676522, 0.0016086548972339305] - - [0.7099437703257733, 0.704251604221871, 0.002085133119328175, -0.002274455192243142] - - [0.7076343503148397, 0.7065783628615191, -0.0006460837403530858, 0.00047535905446260263] - - [0.7055934674965211, 0.7086165300142944, 0.0006638792666446608, 0.0001768645123145223] - - [0.7049597758588919, 0.7092444837334542, -0.0014094202999041401, -0.0014107618740333693] - - [0.708196071037066, 0.7060116263194802, -0.0003424450317170448, -0.0024064913849383106] - - [0.7077734641499209, 0.7064380105391713, 0.00081061308358428, 0.0011847434772934422] - - [0.70493447331349, 0.7092390947750188, -0.0035759865340886034, -0.0058742741894046505] - - [0.7055559365232924, 0.7086518529542959, -0.0008389202368854549, -0.0016333872837894256] - - [0.7067704816224667, 0.7074429197473119, -2.7225001680340282e-05, -2.9416733481244855e-05] - - [0.70476677210306, 0.7094089522141207, 0.006372630424614967, -0.0014577514768282264] - - [0.7101251397861923, 0.7040661214934288, -0.003458051170136424, 0.0011064768165705773] - - [0.7053810223245226, 0.7088078973694347, 0.005242264734964935, 0.001223368947078926] - - [0.7035417972095885, 0.7106534346321011, -0.0004931042511077011, 0.0006263169305942703] - - [0.707406064769176, 0.7068071753579057, 0.00034151411722974876, -0.0003996979794547269] - - [0.7058622394897474, 0.7083271600772595, 0.005396033968343609, 0.0014198509835320501] - - [0.7055449707881867, 0.7086485361605397, -0.004331987237668292, 0.002186385051651135] - - [0.7049913845058743, 0.70920056843141, 0.0035379828299221963, 0.003030542286538569] - - [0.7085121696269713, 0.7056936732566521, 0.00017214621874239686, 0.002629711332947631] - - [0.707194283969681, 0.707000522862098, -0.0026304562422330194, -0.004425617842517239] - - [0.7083815050855115, 0.705813535824683, -0.004782588613571037, 0.0001508164377819832] - - [0.7084122494660116, 0.705796655183753, -0.001768269001037382, 0.00019890367434290842] - - [0.7053963876506107, 0.7088116551746788, -0.0008515203960795788, -0.0011174484402846684] - - [0.705246390480236, 0.7089327151096259, 0.003391239469494745, -0.005516670803384226] - - [0.7087402313212804, 0.7054557077721554, -0.002987852923125549, -0.0032560118634466567] - - [0.7077287838752048, 0.7064657349991076, 0.0031553098101737877, -0.004022159456822161] - - [0.7078110731105958, 0.706384715193345, -0.0011677649099353603, -0.004770246152872954] - - [0.7064587133593324, 0.7077500360403708, -0.00040171297800384136, -0.00241069081671304] - - [0.7049269784154112, 0.7092580384915773, -0.0019497938754714816, -0.00521423446364085] - - [0.7076617584344183, 0.7065400080153966, -0.003566655059760775, 0.0018252930709836447] - - [0.7064801343451172, 0.7077199831886086, 0.0040354643866884555, 0.001400070856957062] - - [0.7056403021729525, 0.7085672892692428, -0.000203902037631835, 0.002029519859659797] - - [0.7094382244493119, 0.7047663506224655, 0.0010993211825692858, 0.0007669507400172105] - - [0.707677239277183, 0.7065342544446848, 0.0014992587000860467, 0.00015661630939057159] - - [0.7067246682770193, 0.7074866189951391, 0.00166333498725062, 0.0004006348513991882] - - [0.7087823068567854, 0.7054183281252477, 0.0021446529480285154, -0.0028327187446699374] - - [0.7053222470021231, 0.7088822231195971, 0.0025406851801771935, 0.00025796842639837296] - - [0.7056161956396247, 0.7085928731537922, 0.0013406446964379488, 0.00035670508047861815] - - [0.7096189804388667, 0.7045840979026423, 0.0014292080004231233, -0.0003300728927815452] - - [0.7069673234544375, 0.707231102991596, -0.004496832114013978, 0.0010719281216622263] - - [0.7096475846193087, 0.7045550483291362, -0.0013765752651933416, -0.0007710761629033172] - - [0.7055822279493846, 0.7086261266301739, 0.0015454996901289298, 0.0005862506349074593] - - [0.7094336094130924, 0.7047713017280188, 0.0008436115408858236, -0.000808959376992668] - - [0.7058188424339916, 0.7083857445426666, 0.002524698966985998, -0.0017391058041082632] - - [0.7099717370106347, 0.7042281565226741, 0.0016757317952981337, -0.00016771919614050814] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] - - [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -qpos: - - [0.15080165907860044, 1.356358388068401, 0.7140210986920054, 0.7106552914400213, 0.019246653038435087, -0.00686665397478744, -0.7032435368763137, -0.5292872651493421, -0.16778811604311847, -0.0010000000000000009, 0.06456522792824608, 0.0036959775501775893, -0.0010000000000000052, 0.15213075015919172, 0.13509511051294268, 0.0010000000000000178, 0.7139549255501549, 0.2876633827663385, 0.19000278858768518, 0.10839912412962392, -0.07281252105895568, -0.32493294469722567, 0.2637519678808252, -0.4133843124264621, 0.06681685426535636, -0.017884478093107455, -0.24479915090409407, 0.476826025874201, 0.9776955714951622, 0.9802920115089997, -0.09255921092912851, -0.09359915575180516, -0.01785647930185663, -0.06256550993584885, -0.022546720385611806, 0.047862247152728285, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15149076497385253, 1.3524989270473176, 0.7139818675459011, 0.7096502907234898, 0.01968090714177234, -0.007516595523141281, -0.7042390414922223, -0.5409266097940828, -0.16130307833947521, -0.0009999999999999985, 0.06453676132332174, 0.002481169912296335, -0.001000000000000006, 0.15612960945082005, 0.1387227662031046, 0.0010000000000000148, 0.7035495453121817, 0.2835632453757514, 0.19082675515403852, 0.10531570084060829, -0.07718379390583605, -0.3265128441044733, 0.26295460271277526, -0.41432147681772835, 0.0701269161697288, -0.01660713920436475, -0.2408152734388667, 0.4795604472152241, 0.9776671265141803, 0.9790643079604497, -0.09243817650191422, -0.0958468147995689, -0.01809472461923666, -0.06211485908006342, -0.021537165082620206, 0.048939898275102735, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1521675422474615, 1.348622717657764, 0.713968787317118, 0.7086442553022769, 0.020123877657392826, -0.008170676170391336, -0.705231585385985, -0.5525622582329898, -0.15479562899587582, -0.0010000000000000007, 0.06446213851912695, 0.0012425113896997067, -0.0010000000000000072, 0.16010393466064893, 0.14234184386344342, 0.0009999999999999857, 0.6931501672139029, 0.2794466708634165, 0.1916468095302649, 0.10222783708799153, -0.08154247475837974, -0.3280857987337679, 0.26214175585013694, -0.4152592465320656, 0.07340750361145063, -0.015349792580583336, -0.23683563220853093, 0.48229860324730583, 0.9776385917248595, 0.9778359703912582, -0.0923170142536462, -0.09809433597323804, -0.01833237034528908, -0.06166732604659737, -0.020527494414428304, 0.050019022626069845, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15270855751921925, 1.3439945370060773, 0.7136904768712606, 0.7077199959734768, 0.02045381084371221, -0.008460817790508734, -0.7061462054586053, -0.5590855015047211, -0.1481919491428727, -0.0009999999999999983, 0.0640385268864909, 0.000607693739744178, -0.0010000000000000276, 0.16237057529216212, 0.14502504130486077, 0.0009999999999999842, 0.6712951789496974, 0.2760050084376093, 0.19213501312446934, 0.09899604809879896, -0.08404342102828585, -0.33048458439270306, 0.2619300072010419, -0.4148238132504595, 0.0741268332083632, -0.013155318093780854, -0.23357992647863315, 0.48471905990846054, 0.9784501393380917, 0.9775829961266445, -0.09246454672825748, -0.09863168721457714, -0.018889791195208498, -0.06104026312141222, -0.019023978457036246, 0.05156500022533796, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15323253195362396, 1.3393387836185542, 0.7134474864253709, 0.7067959093083133, 0.02079090178959341, -0.008753509015748862, -0.7070577466287468, -0.5656038189226291, -0.1415568885814855, -0.001000000000000001, 0.06356338376401646, -4.573985562698511e-05, -0.0009999999999999903, 0.16459617885174513, 0.14769969920973933, 0.001000000000000004, 0.649422922544767, 0.27254452411683233, 0.1926196058257317, 0.09576740033935927, -0.08653127656703152, -0.3328772263117856, 0.26170441296575886, -0.41438252004309284, 0.07477972252146885, -0.01097399812368286, -0.23033424292508325, 0.48713422414563257, 0.9792664850606302, 0.9773355366527993, -0.09261300232619524, -0.09916045979227929, -0.01944904304726191, -0.060413577666168776, -0.017517877403781572, 0.05311373748879857, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15366627286128318, 1.3337840254229256, 0.7128857220321825, 0.7058682470576122, 0.02105273276495403, -0.008633434943907756, -0.707977587244123, -0.5678668710740241, -0.13511640055156138, -0.001000000000000001, 0.06306056045301127, -0.00020213642534555354, -0.000999999999999984, 0.16504581598735357, 0.1495682411387308, 0.0009999999999999625, 0.6160115023928933, 0.27023888921170824, 0.19317162029771182, 0.0920039413707048, -0.08487683330532027, -0.33636020013357, 0.2614818073988496, -0.4130271782318985, 0.07294283325616861, -0.009596754202182792, -0.22703930718502152, 0.49035761963911995, 0.9807615077570873, 0.9780383258882704, -0.09286938802481944, -0.09851565273559995, -0.020518201754760445, -0.06072488220868152, -0.015580641140298215, 0.054547560157952986, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1540773235318503, 1.3281859364480886, 0.7123763370279806, 0.704940338507683, 0.021322489353892977, -0.008514766081796455, -0.7088948930208182, -0.5701355536775038, -0.12863491878556457, -0.0009999999999999972, 0.06252651863996486, -0.00037608416985315585, -0.0010000000000000152, 0.16545026153702386, 0.15143071757325305, 0.0009999999999999848, 0.5825737269011018, 0.2679147990723946, 0.19372189879358254, 0.08824556271400484, -0.08319464020676433, -0.33983917678417375, 0.2612361809441317, -0.4116634075910242, 0.0710266600473893, -0.008246063407530499, -0.2237500104776571, 0.4935842842915866, 0.9822638643364635, 0.9787519230241789, -0.09312554517593565, -0.0978572141928058, -0.02159219680299118, -0.061046517535895094, -0.01363913791759111, 0.055980099768331855, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.154414928452906, 1.3216686480603133, 0.7116619940491864, 0.7040429222819132, 0.02149507832481892, -0.007960959632395818, -0.7097873965591998, -0.5688201515625831, -0.12220592292906367, -0.0009999999999999985, 0.061911354234833404, -9.679206536919402e-05, -0.0010000000000000202, 0.16320029255955482, 0.15244720698366904, 0.0010000000000000174, 0.5386150399086499, 0.2664445692251187, 0.1938962269769229, 0.08389072330980299, -0.07613222743319663, -0.3446052941608591, 0.2612837821197044, -0.40919950099483215, 0.06558173156590903, -0.006812474285668991, -0.22108938699920108, 0.497046062940627, 0.9849503186107674, 0.9800335709687981, -0.09418670116463249, -0.09753342077368798, -0.02288029497856692, -0.061983742589102324, -0.011189336032346996, 0.05687748779171944, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15475106313519407, 1.3150962401985595, 0.7110321017245477, 0.703014199793967, 0.021625543165048063, -0.007363236457741095, -0.7108088023651186, -0.567478251079871, -0.11572660416805049, 0.00023398609124790958, 0.061674803694197644, 0.00039647142537220905, -0.0010000000000000195, 0.16110271331075815, 0.1537814178304772, 0.0009999999999999885, 0.49465971805694586, 0.26493317082645457, 0.19385700075424486, 0.07990214074409925, -0.06906835983444336, -0.34932395820495554, 0.2615985252344997, -0.40652815649151025, 0.06008207406046516, -0.005388384918602053, -0.21923695752069558, 0.4996851977826956, 0.9876571317313658, 0.9813244259762637, -0.09526018415849756, -0.09721480240262839, -0.024173080937458073, -0.06293254471315365, -0.008733002498274963, 0.057765619183993554, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15502983178604143, 1.3077363828108652, 0.7101833653505384, 0.7021572576458922, 0.021592889720568467, -0.006362420819433503, -0.7116659695742314, -0.5636158587900472, -0.1093814444485188, 0.0010000000000000007, 0.061631620729089634, 0.0017023621724126478, -0.0009999999999999861, 0.15620896541480092, 0.15454542704133983, 0.0009999999999999692, 0.4438457331492974, 0.2648211620831281, 0.1936811680847679, 0.07531366229800843, -0.058848082107851085, -0.35566150786499706, 0.26198311064107627, -0.40350975817895574, 0.05221703502353484, -0.00513104172481821, -0.21767562187555353, 0.5032051870813558, 0.9909324552885526, 0.982402587030798, -0.09613980515636482, -0.09643129682476152, -0.025764136922880966, -0.06415175347989661, -0.006319414644845256, 0.058106643858676706, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1552587790784352, 1.3003169781517698, 0.7094391830053869, 0.7013888691751667, 0.021606802928062933, -0.005395375607467186, -0.7124308318612156, -0.5598248267906191, -0.10298033787995407, 0.001, 0.061469071058795316, 0.0028400677398258338, -0.0010000000000000015, 0.1511754511769854, 0.15511301855247864, 0.0009999999999999796, 0.39308655826505984, 0.264698020447281, 0.19362379666510401, 0.07051480885752344, -0.04862436176660678, -0.3620115775612249, 0.26214815358056215, -0.4006325637601664, 0.04439611086849846, -0.004918696776853456, -0.215659955110155, 0.5072157956081937, 0.9942195681618188, 0.983476031453136, -0.0970139116757293, -0.0956367361066679, -0.027362142493340347, -0.06537651360812195, -0.00391040140089821, 0.058435433786813575, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1555679822647201, 1.2924147050728192, 0.7087145618415603, 0.7008445251566585, 0.02131813737167564, -0.00409522245291645, -0.7129836728355022, -0.5543020358245442, -0.0965034805040883, 0.000999999999999999, 0.0619294551509966, 0.0047073216684151474, -0.0010000000000000022, 0.14445143665041144, 0.15507180898473225, 0.0010000000000000009, 0.3399170654929998, 0.2657209309478108, 0.1934613868651315, 0.06577266631868002, -0.038341451111984406, -0.3693037336233791, 0.26283109960180906, -0.39717543905979635, 0.03613218098587823, -0.004941655603754984, -0.21400771179629438, 0.5107136417860716, 0.9975369095005422, 0.984120781648834, -0.09691183344897934, -0.09574432123811179, -0.028519531791111677, -0.06646754068184474, -0.0021311105106349435, 0.058723019827462676, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15585556103353912, 1.2844805916035162, 0.7081203301046426, 0.7003133899405743, 0.021044847529546485, -0.00280561906087493, -0.7135197255606076, -0.5488274769349917, -0.0899860141532867, 0.0010000000000000013, 0.06256174965640643, 0.006537474236168946, -0.0010000000000000033, 0.13773649015192327, 0.15502773301768827, 0.000999999999999942, 0.2869107845127335, 0.2667208207268481, 0.19328784550755568, 0.061054944825093195, -0.02817354327548682, -0.37654442797984855, 0.26347319986532797, -0.39374386952322826, 0.02805386207069597, -0.00497752619113301, -0.2123977968618516, 0.5141559347684658, 1.0008549590215774, 0.9847531111825489, -0.09678155357094402, -0.09587796558547193, -0.029662463719603457, -0.06755506262177655, -0.00037123128577504704, 0.05900833257501851, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15633836806495957, 1.2764463426980004, 0.7075907421018774, 0.6999433576801317, 0.02046893746751417, -0.0013126023296000794, -0.7139037720264119, -0.543188821319387, -0.08369904413330498, 0.0009999999999999983, 0.06412414366074548, 0.008915354631947625, -0.00100000000000002, 0.13208362353008088, 0.1547573929532389, 0.0009999999999999979, 0.2377183855848972, 0.26928858903433045, 0.19298552551721412, 0.056137760854527145, -0.021207201066807242, -0.3841739276327414, 0.2644600222831448, -0.39024573147364583, 0.02182178986928738, -0.004757440131452642, -0.2113509399048322, 0.5174724087545035, 1.004240894341274, 0.9855492479185066, -0.09600074010666702, -0.09560571892909069, -0.030052567288459234, -0.06840330339025995, 0.000418866418595848, 0.05940305500506063, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15681994433468435, 1.2684166213412906, 0.7071803526764089, 0.6995867374465795, 0.019907236901337572, 0.00016856703517593725, -0.7142703061817421, -0.5376280954350355, -0.07740846246414032, 0.001000000000000001, 0.065840142073028, 0.01125036960703146, -0.0009999999999999992, 0.126535499636664, 0.15449139284282684, 0.00099999999999999, 0.18885317624921583, 0.2718599451434226, 0.19266857487232042, 0.05123443546972973, -0.014447600611650709, -0.39172468805017074, 0.26540246505584725, -0.3867830962157186, 0.015844505336909002, -0.0045228507263292035, -0.21036059362297338, 0.520738660771655, 1.0076298511538382, 0.9863513040446827, -0.09519841012621477, -0.09531904300560679, -0.030415586939156566, -0.06924307476082855, 0.0011759847608440528, 0.05980148642621865, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1573688179844473, 1.2605350105111797, 0.7066584856442636, 0.6994894002980001, 0.019136256733283456, 0.0017498327313308845, -0.7143845747455597, -0.5332069942691082, -0.07091852108181887, 0.0009999999999999996, 0.06817409747523677, 0.013847849835834588, -0.0010000000000000026, 0.12358070087991917, 0.15366989718142607, 0.000999999999999991, 0.14859551317245265, 0.2749094069824089, 0.1923270393675263, 0.04705663170580936, -0.01187827811801396, -0.399025955802725, 0.2666965207838637, -0.3834272512962575, 0.012320907829157825, -0.0041252181006114505, -0.2101549555741668, 0.5234211149655194, 1.0106072857194415, 0.9865140665459701, -0.09374664953635078, -0.09418661885393333, -0.030204911849152492, -0.06993603508543018, 0.0012590177749433045, 0.06007991888427893, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15792702267195377, 1.2526811462114567, 0.7062131616588433, 0.6994087470418116, 0.01837562115996844, 0.0033228031595924985, -0.7144779213436548, -0.5288881902780904, -0.06442637814099356, 0.001000000000000002, 0.0705894525167618, 0.016410279473698847, -0.0009999999999999983, 0.12077127310581162, 0.1528377146558997, 0.0009999999999999935, 0.10882094317790075, 0.27793740068085954, 0.19197705009482952, 0.04292571094326237, -0.00953732608666893, -0.4062349643616276, 0.26796313857037296, -0.38009969754276623, 0.009011196143647584, -0.0037080488511936328, -0.21001116503914607, 0.5260439284854014, 1.0135684206670612, 0.9866505880936534, -0.09226803369272783, -0.09301899875512852, -0.02997087005204556, -0.07062233980978996, 0.0013169820911932105, 0.060353695707930304, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.15861636718947247, 1.2451111559204022, 0.7055806294415965, 0.6993439985645598, 0.01745713081271197, 0.004869452304169297, -0.7145554622909103, -0.5260347015822903, -0.05753023312357678, 0.0010000000000000044, 0.07329402792928841, 0.01888236053700445, -0.001000000000000006, 0.12037498748808628, 0.15181450110516945, 0.0009999999999999551, 0.08071539380572929, 0.2810453687095907, 0.19210684170634593, 0.03906994267175696, -0.010333224554195745, -0.41292199419718384, 0.2691637064238201, -0.37679129806216066, 0.0076806287349971015, -0.0032585108872970367, -0.21031454010351147, 0.5281559735822605, 1.0157004512846899, 0.9871027238868252, -0.09141050172501616, -0.09094775209750382, -0.029117001205967805, -0.07161465324305807, 0.0007527580367372255, 0.060918371357061726, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1593219859899562, 1.2375722593325769, 0.704978315254919, 0.6992831709404789, 0.016540829625876687, 0.006411175321636967, -0.7146246179818547, -0.5232777621544721, -0.05062417582164149, 0.0010000000000000009, 0.07602045457359333, 0.021331017701851997, -0.000999999999999985, 0.1200970840997874, 0.1507957069232987, 0.000999999999999975, 0.05322574720965022, 0.2841301958950572, 0.192256097735058, 0.03524408551800595, -0.011299648087097425, -0.41953191777961607, 0.27034420426915534, -0.37348349804172926, 0.006478656508348412, -0.002794455463536152, -0.21065304516625377, 0.5302213902284184, 1.0177935156261226, 0.9875696239487418, -0.09058129346614065, -0.0888329005054578, -0.02823412194873243, -0.07262008815186259, 0.00016082050241178074, 0.06149862938070342, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16034365404110956, 1.2304182971316777, 0.7043469789526884, 0.6992763866512971, 0.015626718730360714, 0.0077674452539114305, -0.7146383753534757, -0.5222321803031541, -0.04327871360708149, 0.001000000000000001, 0.07794822030401947, 0.02290914057768847, -0.0009999999999999994, 0.1208227703378241, 0.15001313627012644, 0.0010000000000000193, 0.03908169231528961, 0.28713673530759265, 0.19216709938380733, 0.03144278460425787, -0.012286355083025703, -0.4248410688575499, 0.27050908682333635, -0.3706531518728696, 0.007275680924491433, -0.0023300371895821352, -0.21132071590694898, 0.5322035382275808, 1.020148815856266, 0.9886255301627234, -0.0896289996433096, -0.0860991626613818, -0.027541280479624366, -0.07313385587358805, -0.00024029333953924905, 0.062152890832128695, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16138801429107924, 1.2232905023344287, 0.7037244015533879, 0.6992729539808598, 0.01471556888328466, 0.009115327141317024, -0.7146451557762293, -0.5212835052530015, -0.0359128661657406, 0.0010000000000000035, 0.07983053808042041, 0.024442808395494356, -0.0009999999999999998, 0.12159725818499528, 0.1492516414224265, 0.0010000000000000258, 0.025671208595982535, 0.29012734824777947, 0.19206411321202277, 0.027655487205865154, -0.013281079459638625, -0.4300602590094246, 0.2706176510415347, -0.367834602292066, 0.00818258003246739, -0.0018601180703397458, -0.21201096743088538, 0.5341696190106913, 1.0225183364246406, 0.9897126285063532, -0.08866932274859493, -0.08333185714710771, -0.02685903311651087, -0.0736199944119674, -0.0006289976404466264, 0.06281294804818241, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1626086325373535, 1.216480889683943, 0.7030586575462403, 0.6992290329355281, 0.013857178738682169, 0.010258199925155111, -0.7146897980464993, -0.5210873729177529, -0.027810687060364124, 0.0009999999999999998, 0.08109730170187215, 0.025097797078239222, -0.0009999999999999972, 0.12195825250172457, 0.14880036392483578, 0.0010000000000000104, 0.023710079172323616, 0.2925168257322192, 0.19158957723615214, 0.02372432258712708, -0.01136721277673609, -0.43287617429256403, 0.270044536970408, -0.3654519109966255, 0.009745538278621105, -0.0008810221301109099, -0.2134169900914626, 0.536366420166955, 1.0249161554718145, 0.9914478680453546, -0.08780539975622018, -0.08112597485619481, -0.025931216874199553, -0.07392308795984606, -0.0008216846262114629, 0.06387875985086326, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16384236321728, 1.209689159823566, 0.702395785639074, 0.6991827726518373, 0.013001558269200119, 0.011391281914134738, -0.7147339705133755, -0.5209330939018684, -0.019662767660119266, 0.0009999999999999979, 0.0823291186675232, 0.025709349911520872, -0.0009999999999999966, 0.12229155303232976, 0.14837166381122174, 0.0010000000000000065, 0.022433276648485063, 0.2948661157094976, 0.191090325580373, 0.019795611998277198, -0.009280600621637953, -0.4355440096930617, 0.26943650012443754, -0.3630783344465614, 0.011348840170690126, 0.00012992174506407174, -0.21486657851224825, 0.5385712113841458, 1.0273158186695959, 0.9932215741062111, -0.08694673110628587, -0.07895290009203543, -0.0249890102074067, -0.07421433203835676, -0.0010007779846104609, 0.06497038926795366, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16509487073856716, 1.2030633597783609, 0.7015713075138935, 0.6991715369632768, 0.01225680777925449, 0.012353274237690762, -0.7147421417399984, -0.5208265018201215, -0.011002329542442048, 0.0010000000000000009, 0.0829074661703114, 0.025795791252549605, -0.0010000000000000096, 0.12095724584235099, 0.14834746861447365, 0.0009999999999999885, 0.029400331444877425, 0.29730690299016516, 0.1901898431963694, 0.016120267373506178, -0.0022883462497125264, -0.43554973299765354, 0.2683936830227724, -0.36112529711751545, 0.013209500246315897, 0.0011239610687610417, -0.21660937145732861, 0.5406892613425617, 1.0291599438014902, 0.9950958000916501, -0.08560995299200069, -0.07604609005497613, -0.02419338511948291, -0.07419865992003781, -0.0008234188468850508, 0.06648207106829258, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16635132080034973, 1.1964498925765708, 0.7007526608688888, 0.6991635663915364, 0.011519383513078385, 0.01330449709308057, -0.7147451305124416, -0.5207185880326626, -0.002307315817927187, 0.001000000000000001, 0.08343852757497948, 0.02584848875146699, -0.0010000000000000087, 0.11950701379895641, 0.14835160377592957, 0.0010000000000000046, 0.03690750430741206, 0.2997518913483449, 0.18925595848977467, 0.012470769145842907, 0.005029411576307471, -0.43537531720787187, 0.267326786798688, -0.3591888000245844, 0.015087858393835094, 0.0021186279199377316, -0.21837113549479728, 0.5427999050822361, 1.030967407899835, 0.9969763773483896, -0.08424140780996803, -0.07308939125438643, -0.02340746212419581, -0.07416127757540318, -0.0006222557044752723, 0.06802231095655131, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1678028205349222, 1.189981933936591, 0.7000086722004658, 0.6992348616617082, 0.01072486120668631, 0.01394353987577191, -0.7146755650536559, -0.5196625515333979, 0.0063569326987455875, 0.0009999999999999992, 0.08290732260396433, 0.02541723781941225, -0.0010000000000000033, 0.11650208622613703, 0.14882252751282757, 0.000999999999999997, 0.04947414299881186, 0.3042427088014555, 0.18778513950869863, 0.008663962315126837, 0.015493021225246746, -0.43155161516281787, 0.2659760415266198, -0.3570104898665954, 0.016556185023549014, 0.0036154253018742155, -0.2203679325302977, 0.5446334554724953, 1.0325643599103473, 1.0004231628946578, -0.08235545725608973, -0.06945334349173687, -0.022231682567667697, -0.07387832831548594, -0.0003821537887275367, 0.07049420725059449, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16927347247388927, 1.1835336701090127, 0.6993026923509419, 0.6993134416693553, 0.009926700196314305, 0.014558412617053002, -0.7145979453831678, -0.5185342572449458, 0.015015284472929486, 0.0009999999999999955, 0.08228451072231291, 0.024940024557305707, -0.0010000000000000187, 0.11337027833226383, 0.14932976822532187, 0.0010000000000000263, 0.06240823008299004, 0.30888062576714875, 0.18626138210375473, 0.004849952868101182, 0.026185043512090354, -0.4274527613499098, 0.2646053675140273, -0.3548074638059399, 0.01798833267651685, 0.00515314802814441, -0.2223809962589022, 0.5464451653036425, 1.0341461833928296, 1.0039829348952485, -0.08043292232438963, -0.06576488236074517, -0.021026792730484082, -0.0735745533371772, -0.00013944997827615262, 0.07303599041291604, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.17120871503820934, 1.1772180918451707, 0.6986906597149507, 0.699552362416358, 0.009152470440255873, 0.014985476773756954, -0.7143655646854739, -0.5164642978632179, 0.02241234286450643, 0.000999999999999995, 0.08031440992163452, 0.023796326953989498, -0.0010000000000000044, 0.10950455529908862, 0.15013299442356853, 0.001000000000000013, 0.07735072086588111, 0.3169326916578117, 0.18382838188927048, 0.0017243557501401253, 0.03629397734832505, -0.41917411731265786, 0.26292330017003734, -0.35255512904347863, 0.019045537043892346, 0.009684093463920254, -0.22465666276107255, 0.5467891370692318, 1.0357363736286735, 1.0076292870714785, -0.07807494170493974, -0.06189613843802694, -0.02016730193234829, -0.07278568344753052, 0.00024744099856062663, 0.07598939843719617, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1731877658456449, 1.1709300315947524, 0.6981319296522456, 0.6998044382763855, 0.008380527018624033, 0.015396332142526496, -0.7141193652969517, -0.5143214040932603, 0.029706770706260214, 0.0010000000000000005, 0.07822442821870218, 0.02259160823824746, -0.0010000000000000057, 0.10556962595786169, 0.1509800419068431, 0.0010000000000000308, 0.09246266655548599, 0.32525075483155663, 0.18130855920516106, -0.0013504021975745757, 0.04635586898492734, -0.4105520028781284, 0.2612183672126167, -0.35029481360070097, 0.020061887367309995, 0.014455634362416042, -0.2269444569315639, 0.547020822461916, 1.0373275229584231, 1.0112812634064807, -0.07568409661840245, -0.05801108521911676, -0.01933307955974598, -0.07195508720974672, 0.0006450355260720911, 0.07897759335246864, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1756480900692386, 1.16481701355808, 0.6979513266149252, 0.7003109575823132, 0.007638169305269869, 0.015524968012647477, -0.7136281920075846, -0.5104656011157038, 0.033972466386712655, 0.0010000000000000007, 0.07450519736744243, 0.02095667781802878, -0.0010000000000000026, 0.10128917211193994, 0.15197357396173886, 0.0009999999999999807, 0.10665436606060444, 0.34005379064941477, 0.17789080399548232, -0.0039012794944876103, 0.0528504433776581, -0.39717093595701763, 0.25984093084552495, -0.34815367308342665, 0.022864309212310824, 0.0225982945992746, -0.2295313381472833, 0.5459037093180436, 1.0387718309461833, 1.0151144198690034, -0.07262223820848109, -0.0532051362601688, -0.018446664650454202, -0.07074804266528963, 0.0003463983066966332, 0.08269544132751168, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.17562505889665325, 1.1537271100082653, 0.6953790790766645, 0.6982730882026361, 0.003396181780092311, 0.017831052415195465, -0.7156012952832304, -0.5152108306690721, 0.06985262860059206, 0.000999999999999999, 0.10225220877642745, 0.01388205135977135, -0.0009999999999999953, 0.09694639857616874, 0.15968143315112895, 0.0009999999999999517, 0.1161195043409712, 0.35327974170508436, 0.1780752557948968, -0.005707188107282141, 0.08628885693021653, -0.41308345429745646, 0.26515957579256216, -0.3459703390903944, 0.0077649703918351645, 0.03987659772426396, -0.22433979002085014, 0.5501627878725496, 1.0401901865802996, 1.0137954867829317, -0.06950096386166345, -0.04604798786507819, -0.01757112965116859, -0.07038261657194719, -4.9648900636258e-06, 0.08705828142756329, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.17287806542446144, 1.1442226177743215, 0.6967459612023406, 0.6969343344584732, -0.0020125924937040188, 0.018282108528143878, -0.7168990496800398, -0.5025104429667472, 0.09463824528766863, 0.001000000000000005, 0.12359068708280238, 0.019970043021698563, -0.0010000000000000122, 0.09345770946189248, 0.16323601627823903, 0.0010000000000000336, 0.11990320113274414, 0.37160159108833485, 0.18155528962943726, 0.0007361971194426682, 0.10466518288223246, -0.40436040474024737, 0.2713766904244834, -0.3600884453408905, -0.0010791661689933895, 0.049141302440429226, -0.22259656217437782, 0.5491978176267559, 1.039918650390476, 1.0015879759249855, -0.06559017879942707, -0.033465614510440465, -0.016331096998118265, -0.07288296394740965, -0.0011224979503678951, 0.09343060493774957, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16978461588475777, 1.1369040773504273, 0.6988681117892387, 0.69539793808057, -0.009359384250081305, 0.01804327311775032, -0.7183373510649075, -0.4826051121342283, 0.11669101319426023, 0.0009999999999999911, 0.1442407800922162, 0.033807335570504596, -0.0009999999999999948, 0.09016660088759955, 0.16618471484471603, 0.0010000000000000106, 0.12115490729964243, 0.38520221722160675, 0.1887321938610597, 0.014662887674880888, 0.12124786761081735, -0.38001692925474967, 0.27949380077746927, -0.3861001929144562, -0.007703216035676238, 0.05348439276680884, -0.22102439630669574, 0.5460162888933222, 1.0394962275394237, 0.978204693674296, -0.06160583475687063, -0.0154819076604071, -0.015047828087070704, -0.08022986194687547, -0.0023141178716934484, 0.10404849630661485, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1685616598671063, 1.131988580307788, 0.7016861447231297, 0.6937651552738989, -0.01864632220192159, 0.018067397923153716, -0.7197331402182422, -0.45938048010196597, 0.1314716744845694, 0.0009999999999999994, 0.16436531821948439, 0.05119171736810574, -0.001000000000000012, 0.08701532198356214, 0.1685158214456028, 0.0010000000000000198, 0.11878086008707876, 0.40287858507940916, 0.19849771584443374, 0.03543585884410838, 0.12889176076762715, -0.34044010198140506, 0.29139649673761786, -0.4210796963968806, -0.007488093944203087, 0.06051077953433767, -0.2194151224747646, 0.5391641772812505, 1.037244371654297, 0.9431337689897671, -0.05829538360911303, 0.00970833086781105, -0.0135364951866454, -0.09519330305970564, -0.0034300578946336734, 0.1213504360102254, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.16953202323191655, 1.128434859706133, 0.7044606786087722, 0.6918690368300776, -0.028935643347340865, 0.01897027944146114, -0.7211935197419688, -0.4368643495550697, 0.144035336013829, 0.0009999999999999955, 0.1836241214913561, 0.06922815091759905, -0.0009999999999999944, 0.08392884625178455, 0.1719895427168746, 0.0010000000000000063, 0.11580525736095441, 0.4173400898506902, 0.20866765946644186, 0.06008326395583906, 0.13443718610302813, -0.2954630686351042, 0.30587205106640514, -0.4630187121365578, -0.003560951157667941, 0.06963971962856191, -0.2168027849846486, 0.5288059634645779, 1.0348115233023405, 0.898638372361137, -0.05504674029823359, 0.043198029892149645, -0.011998878320224038, -0.11915187800587329, -0.004530072798608685, 0.1489508911925559, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.17254861393920215, 1.1251326657844622, 0.7077201614023688, 0.6901402536705386, -0.03950198866114908, 0.020692412434519927, -0.72230038572814, -0.4143041093541598, 0.1479266032135112, 0.0009999999999999992, 0.20121607584408294, 0.08667858254859043, -0.0010000000000000063, 0.08084275100175238, 0.17346403879508124, 0.0010000000000000035, 0.11091696410493512, 0.4412382999592158, 0.218032461974613, 0.08834521352325854, 0.13530700409407784, -0.2523688100225217, 0.32268327708374245, -0.5102485016579167, 0.0030540528777552077, 0.07987969343869121, -0.21468742901709983, 0.513096093265494, 1.0313911588716413, 0.847465167127492, -0.051869364224715435, 0.08584580893473649, -0.01076540197616383, -0.1525295905914866, -0.0060181916014082765, 0.18877252312295584, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.17674401201715995, 1.1215171086454572, 0.7108439872646962, 0.6887580365864695, -0.049120302808135785, 0.022945988916048918, -0.7229613022024435, -0.3939014327281533, 0.1485364227991125, 0.0010000000000000013, 0.21566442044312906, 0.1019532429211561, -0.0009999999999999983, 0.0779739068104992, 0.17433408611807044, 0.000999999999999995, 0.10681337798568585, 0.46724029907265324, 0.2251485529527953, 0.11602526551156743, 0.1320261843909475, -0.21300946117541716, 0.3391470984956956, -0.5578915534157557, 0.011686840439465763, 0.09141538629747906, -0.21322041455581894, 0.49342976848473014, 1.0278615513231883, 0.79824464444197, -0.048697398212579956, 0.13694919590453236, -0.009568980850200661, -0.19215753876883412, -0.007551321353537779, 0.24018738652720548, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18147257985774842, 1.1169901681470238, 0.7140698352882774, 0.6879691603869627, -0.0574380103923505, 0.02545843698262947, -0.7230153368394254, -0.3749819165922022, 0.13953320085198875, 0.0010000000000000085, 0.22726710420044433, 0.11502332382368001, -0.000999999999999992, 0.07536234532040012, 0.17192405036004774, 0.0009999999999999963, 0.1030446792819225, 0.5024293381553363, 0.23034551507665665, 0.14057299784620686, 0.12520819840900546, -0.17244451425299404, 0.3563771729162595, -0.6015771259559458, 0.02141417847932195, 0.10316995996900787, -0.2153975730644013, 0.4713769924708395, 1.0221761365321433, 0.7563243316826573, -0.04743212411070471, 0.1924300720465405, -0.008127368171285912, -0.233050028459023, -0.010185055706012826, 0.29786564772609403, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18633015349636323, 1.111533828513026, 0.7173311404193472, 0.6877266897578593, -0.06433780627254868, 0.02815525539107034, -0.7225648264845147, -0.35770612657314754, 0.1271676270598797, 0.0009999999999999998, 0.23591664621390843, 0.12624319335577688, -0.001000000000000012, 0.07265794008024042, 0.16805021804131895, 0.0009999999999999994, 0.10024884546445556, 0.5419504585229598, 0.23298651077194665, 0.158998602390684, 0.11468718314365856, -0.13633609721179302, 0.37236016412303113, -0.6369623723436661, 0.03169858162090515, 0.11492127228362767, -0.2190953333688895, 0.4500164451555941, 1.0162404901384787, 0.7264977058119746, -0.04638511465263241, 0.2477722994873648, -0.006659844150985404, -0.2700951895784355, -0.01295452557920169, 0.35645023491176675, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19076712864002207, 1.1051886312262107, 0.7205061001501437, 0.6879764536345825, -0.07014744714458514, 0.030871271276783074, -0.7216749264823012, -0.34131842698658577, 0.1071225066571195, 0.0009999999999999942, 0.24332055402739897, 0.1353790686764575, -0.001000000000000002, 0.0703536106596113, 0.15983300099207173, 0.0010000000000000015, 0.09767221944192457, 0.584002471925005, 0.23456298984558907, 0.17262751395677492, 0.10158194876198663, -0.09984926236222046, 0.3884079115723226, -0.6639309597880229, 0.041145541048517205, 0.12511367205143767, -0.22474686898482005, 0.4300229106462731, 1.0085201239956478, 0.7053312838377561, -0.045752682599666104, 0.29787801393466673, -0.005406100455785414, -0.30109823340294467, -0.01589789652751479, 0.40875074604503214, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19509451270492337, 1.0982832138970713, 0.7237752273380523, 0.6885540735298247, -0.07511021525093332, 0.03362279174696365, -0.7205006948404573, -0.3257607553641674, 0.0843282922251708, 0.0010000000000000009, 0.2490897405114812, 0.14350245232058886, -0.0009999999999999974, 0.06790573760994172, 0.1502940115651565, 0.000999999999999996, 0.09565751725020612, 0.6297000350486454, 0.23469193094427612, 0.18101903789364876, 0.08662336840702918, -0.0662060621539351, 0.403631886717107, -0.682855473658229, 0.05058176741988204, 0.13487202951958205, -0.23131592387386038, 0.41278325213856015, 1.000580398423002, 0.6922806529809928, -0.045178418927092026, 0.3411965399380814, -0.004176918439115869, -0.3245242582362686, -0.018874738126405523, 0.4534276926238786, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19893098356676744, 1.090578231689522, 0.726910253945332, 0.6895675666379228, -0.07949294831798272, 0.03613335791410536, -0.7189379824815345, -0.3097818870911894, 0.05583310703727645, 0.0009999999999999996, 0.2541448742065881, 0.14836320696683614, -0.001000000000000002, 0.06631189209674533, 0.1349054875439881, 0.000999999999999989, 0.0932613531756342, 0.6743812698316832, 0.23364382343269072, 0.18798865436314507, 0.07000681761283295, -0.04752293419232239, 0.4180165018240912, -0.6969970076247985, 0.057332558678979004, 0.13588075951445663, -0.23881121905310537, 0.3981558651113197, 0.9928624484886878, 0.6824264931066418, -0.045443540039186596, 0.3767165494198764, -0.003027090737255482, -0.3430024244498567, -0.02068763135049278, 0.48726467095282244, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20260500973059387, 1.0827319156987933, 0.730029402723097, 0.6907533367246305, -0.08338340429073879, 0.03851548568299262, -0.7172332905375204, -0.2943274087495592, 0.024902347957919325, 0.0009999999999999985, 0.258160795401602, 0.15175471604101426, -0.0009999999999999827, 0.06473209699735129, 0.11796266055011287, 0.0009999999999999985, 0.09116821120732926, 0.7218452752056848, 0.2318059848207057, 0.19235088174128023, 0.05280498048288374, -0.032183374077738816, 0.4316004148763603, -0.7065373696784696, 0.06373903603372509, 0.13522034966381244, -0.24683156165077078, 0.38571886548014545, 0.9851703325316694, 0.676132161140881, -0.045825955727400294, 0.4056995330020375, -0.0018846041853725235, -0.3565365040717075, -0.02235956373924439, 0.5131271947545152, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2057064007786396, 1.0746646721925355, 0.7329007105218989, 0.6923524567771527, -0.08684326872780058, 0.040474646461802016, -0.7151699974587182, -0.27907655936612813, -0.00925663233281772, 0.0010000000000000015, 0.2613072334854161, 0.15260572739009085, -0.000999999999999985, 0.06334105300173976, 0.09818759362463625, 0.0009999999999999957, 0.08897188898036204, 0.7609816608080479, 0.2294971809888552, 0.19723372715712095, 0.03618957704802113, -0.03199154555359658, 0.4439767173383279, -0.7139834689619979, 0.06721840470294403, 0.1274996116931425, -0.25487751666078395, 0.373777499744447, 0.9779340125321866, 0.6695549837623987, -0.0451674416500519, 0.42684409896705133, -0.001159972559952258, -0.36698230163796797, -0.022106295840483552, 0.5300863098409799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20862196591829227, 1.066693908902154, 0.7357045530765106, 0.6940580697785764, -0.08994075370719916, 0.04224164590796024, -0.7130285407673406, -0.264391166849768, -0.044915277181818335, 0.0009999999999999998, 0.2636038547086201, 0.15185115976423064, -0.0010000000000000007, 0.06194473640976687, 0.07748945726810086, 0.000999999999999999, 0.08700971607623953, 0.800524966783656, 0.22674195047045498, 0.20087039292570436, 0.019827961386219922, -0.03427282201239359, 0.4555582618645302, -0.7189727101790628, 0.07029853334762279, 0.11822630031443018, -0.26317462160006094, 0.3630401157997551, 0.9707573258610499, 0.664355546940694, -0.044379994325011565, 0.4431898484591609, -0.0004936837798509752, -0.37473191495598496, -0.021601577730326928, 0.5412511227884956, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21050725340820828, 1.0586812928690335, 0.737567298494393, 0.6962706037468351, -0.0923511916406361, 0.043496422196133214, -0.710483331976695, -0.25080539130735147, -0.0805931780032873, -0.0005194601353435944, 0.2650353861233651, 0.1502146544064345, -0.0009999999999999966, 0.05997441811638075, 0.05693602398712079, 0.0009999999999999987, 0.0850314765474476, 0.8240534965380808, 0.22461789878598554, 0.20650925207376702, 0.0036171084906868925, -0.044964767714807306, 0.46595341306483534, -0.7246359559922293, 0.07148313883935872, 0.10724450104979324, -0.27019555484438235, 0.35357492676915403, 0.9627120463125518, 0.6551523072722817, -0.043824005455113536, 0.4570699636041876, -0.00042117925668105563, -0.38369421280237836, -0.019999742915508368, 0.5474618160997728, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21213120292148163, 1.0507818075944395, 0.7393097992497911, 0.6984812791687778, -0.09453521413289198, 0.04452441852530133, -0.707958029896249, -0.2374473077360638, -0.11637754567124302, -0.0009999999999999994, 0.2660430961001778, 0.14769973204381903, -0.0009999999999999983, 0.058102357856155466, 0.03643826984907409, 0.0009999999999999861, 0.08322238204933328, 0.8454701810668623, 0.2220149191777905, 0.2127625775607003, -0.012287451822696305, -0.05660684502117267, 0.47597663247894934, -0.7288256702522864, 0.07231946486889189, 0.09562597474882019, -0.278169449551614, 0.3439997468452139, 0.9545446883127691, 0.6458633162252703, -0.04331140391645565, 0.468040533111044, -0.0004338608870159116, -0.3914635069702529, -0.018243645172282715, 0.5499589509793386, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2133697273300368, 1.0430650959763266, 0.7408784979679262, 0.7011475041532615, -0.09605924992686164, 0.044860319326553245, -0.7050903131323393, -0.22484212774732795, -0.14881850954316894, -0.001000000000000001, 0.26511082120053125, 0.14492027016766562, -0.000999999999999994, 0.05565293554412287, 0.019067642434566958, 0.0009999999999999987, 0.08148503601552502, 0.8437487042660566, 0.21840143780981153, 0.22117177047117506, -0.026352386079437153, -0.06550376250547019, 0.48324038174268397, -0.7337302466587198, 0.07135322968260843, 0.08633902830211092, -0.28638553144660667, 0.3333336500410614, 0.9484478410627668, 0.6349091109143207, -0.042875845466489065, 0.4756784558743115, -0.0011981255563923356, -0.3991256609845754, -0.01479830079061363, 0.5507044010426355, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2144442450850821, 1.0353464029550492, 0.7424685464236945, 0.7039499671216921, -0.09734042624792125, 0.044978026673249266, -0.7021084405729807, -0.2123445038949681, -0.1800068777782575, -0.0009999999999999983, 0.26343965213176784, 0.1416180163904234, -0.000999999999999996, 0.052943459877346805, 0.0020754423598715248, 0.0009999999999999974, 0.07982989286714853, 0.8375555439986211, 0.21447405651848203, 0.22995976886462885, -0.03988514840044762, -0.07352717988347439, 0.489676280227772, -0.7378794205778626, 0.07005997969682373, 0.07727015410104933, -0.294864549985798, 0.32282223568730545, 0.9426615228459477, 0.6237039336091904, -0.04245151299554008, 0.4810269663013499, -0.0020728442287480013, -0.40591378293942093, -0.01109541024183568, 0.5492633740471186, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21547063148625062, 1.0277306484218138, 0.7438515650437906, 0.7072952827378438, -0.09785957207126854, 0.04435357608403231, -0.6987056944518051, -0.20035997550709791, -0.2056671081598724, -0.0009999999999999966, 0.2592252889366982, 0.13707816161873382, -0.0010000000000000005, 0.0497811248987636, -0.012160504003304798, 0.0010000000000000035, 0.07824895355396963, 0.8074136795847581, 0.21005144243667515, 0.240593514851428, -0.05264118360843688, -0.07420349468546851, 0.4929792579210419, -0.7438297095471474, 0.06809956201295905, 0.070501887212945, -0.30316068580158734, 0.31136225429159875, 0.9383137887707568, 0.6112111995141918, -0.04229709542399378, 0.48510700132260576, -0.003581255673079037, -0.41234687792205155, -0.006576300476079462, 0.5497082931446058, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21641827553715362, 1.019999943932587, 0.7452364037575208, 0.7108493853306478, -0.09805986850558673, 0.043379141787555794, -0.6951227687405163, -0.188190442726818, -0.22891056521125147, -0.000999999999999998, 0.2537929286253005, 0.13182869506105477, -0.0010000000000000135, 0.046342895116950854, -0.02576095385471584, 0.0009999999999999979, 0.07677897140277572, 0.7717701870699999, 0.20543852378782138, 0.25257619545103266, -0.0652405254909398, -0.07318598964966266, 0.4951601659615478, -0.7497803480137288, 0.06608723106112449, 0.06421386315126675, -0.31190911190497106, 0.2994691620714701, 0.9341922171187386, 0.5977803447960364, -0.04217807871729962, 0.48790635076970273, -0.005184808769541606, -0.4183809758651896, -0.0019217513854826783, 0.549365294032932, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2173691751894177, 1.0122184438168786, 0.7463563219122319, 0.7150845794586659, -0.09711641064703289, 0.04133989868954078, -0.6910234871404868, -0.17622707295120157, -0.24400740099615578, -0.0010000000000000028, 0.24458968334795364, 0.12439578687034976, -0.001000000000000015, 0.04205323839841837, -0.03577602281779592, 0.0009999999999999913, 0.07553745869152102, 0.7123612602223341, 0.2001849158181313, 0.26863861685012824, -0.07801213644937594, -0.06125736860248635, 0.4939275303253849, -0.7574854634150691, 0.06402497957034851, 0.06128739426050857, -0.3213103113212565, 0.2858591764297075, 0.9315961889843821, 0.5810796501583353, -0.04348362485701385, 0.49046094106028276, -0.006953816002103786, -0.4249838812378361, 0.002317608111411488, 0.5505189265468163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2183145335485107, 1.0040305612762537, 0.7473911967520812, 0.7197262675059439, -0.09548244752435768, 0.03847291655219492, -0.686583597800506, -0.16349286999760893, -0.2553648140215709, -0.0010000000000000044, 0.23258781202231074, 0.11501572132905229, -0.0009999999999999972, 0.03742616726388061, -0.04467925098868063, 0.0010000000000000141, 0.07467559046161294, 0.6472610987332038, 0.19476750599138032, 0.2889341175984312, -0.09145481017190248, -0.047443582868111414, 0.4905588977784898, -0.7662584554351701, 0.06229540899755002, 0.059364345424352934, -0.3318929244636031, 0.2703393231804873, 0.9292505725713812, 0.561014568135306, -0.045016648911352, 0.4926924295458318, -0.00875241949639644, -0.43215124182878933, 0.006491145071983349, 0.5516418905722469, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.219166003214111, 0.9952233943998179, 0.7474842851023018, 0.7252860529592547, -0.09196870382929792, 0.033728616864295125, -0.6814427923921174, -0.15037396091650804, -0.25809138991566627, -0.0010000000000000013, 0.21435215796857662, 0.10103629983885719, -0.000999999999999982, 0.031716261540383074, -0.048946656993943745, 0.0009999999999999714, 0.075219310087049, 0.5655541527986301, 0.18911445952537612, 0.318958443028373, -0.10699480216506756, -0.024678439985164127, 0.4824702188482929, -0.7786607263104581, 0.06165102649299934, 0.061546251225813045, -0.3442173685265044, 0.25103118761189447, 0.9277448941956031, 0.5311396593151491, -0.047373123988935416, 0.4964004862543115, -0.01176122108072821, -0.44130249377525327, 0.009779942221774954, 0.5537892219441439, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2200793656466452, 0.9850211346029057, 0.7472594374179236, 0.7315241645641415, -0.0870971187539507, 0.027206525500498013, -0.675682095022272, -0.1347689303937141, -0.2570240570479342, -0.0010000000000000015, 0.1906213863339938, 0.08233673648639799, -0.0010000000000000122, 0.02564827244405581, -0.051710830452235325, 0.0010000000000000076, 0.07707422561001014, 0.4833842073447389, 0.18367662318525868, 0.3607401030651697, -0.12543908268224427, -0.003651749632442976, 0.47025005710498136, -0.793801702700788, 0.06185411022472114, 0.06524570809741262, -0.35886193389723786, 0.22625554768050887, 0.9263821123672394, 0.49168654045864474, -0.04986199093558954, 0.5004209099300223, -0.014983768449440183, -0.45237833677676, 0.012914653662498023, 0.556298419925874, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22104968981592296, 0.9726616968027482, 0.7463368153231318, 0.7386594995374165, -0.07991121724293589, 0.018031254182901916, -0.669082367855018, -0.11600389825437359, -0.24869735608478502, -0.0009999999999999887, 0.1589318346013692, 0.05659541804691907, -0.0009999999999999868, 0.019013427739051077, -0.04961584280563814, 0.000999999999999989, 0.08149892015508, 0.3984524443576959, 0.178624095949168, 0.41904408492088135, -0.14884969224501515, 0.017775554084301145, 0.45111246591077353, -0.8118605295936339, 0.06342681401558627, 0.07304880102537807, -0.3755397621840776, 0.19485396745665862, 0.9264275999244367, 0.438824984241287, -0.05189127729794561, 0.5052356502041933, -0.018783261281270745, -0.4655683026595675, 0.01578609575745508, 0.5607480032429014, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22211769619168653, 0.9571489584343499, 0.7452370319755293, 0.7459147863519009, -0.07180020420164461, 0.007105998694566734, -0.6621218671519268, -0.09157488717423275, -0.23941745089629374, -0.0009999999999999968, 0.12364812378122354, 0.02707499650590748, -0.0010000000000000022, 0.012285747978708478, -0.04635550166782541, 0.001000000000000007, 0.08819975270172059, 0.3254293503543272, 0.17517329781680682, 0.49574610818543197, -0.178203047599276, 0.028699868110574123, 0.42832079343493035, -0.8304016394754492, 0.06514225126249426, 0.08179776501144974, -0.3932828345738061, 0.15586876114232293, 0.9267222041232529, 0.3733794698538705, -0.05382802930326483, 0.5077556740500669, -0.02268936670231502, -0.480347210297734, 0.018610923124731654, 0.5648387156118282, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22322836537444965, 0.9387887661398012, 0.7438484384629915, 0.7525815370441538, -0.06335986398249052, -0.00492574890516495, -0.6554252777655912, -0.06257170573958888, -0.22747156072010896, -0.000999999999999994, 0.08796327659233007, -0.0029555659755997267, -0.0010000000000000046, 0.0057834305127815785, -0.03805161231530523, 0.0010000000000000002, 0.09872469583782348, 0.2646579403569549, 0.1735461255261732, 0.5826566595382197, -0.21292752130393316, 0.032979501451051524, 0.4030561737768798, -0.8455693410702609, 0.06718384759159896, 0.0926117585073869, -0.4102866273907177, 0.11475308923439842, 0.9296749188802271, 0.3023788319268836, -0.05593832563041558, 0.5059593644313032, -0.026683896522126653, -0.4938830512133927, 0.02152468479919885, 0.5698395836966665, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2245830985185411, 0.9179401641612623, 0.7425516375126389, 0.7580450445050536, -0.056474623963595046, -0.016459503828965372, -0.6495440031923985, -0.028062010402817285, -0.21695557737218704, -0.000999999999999996, 0.057491532777793834, -0.028224251796961957, -0.0010000000000000243, -0.00041251008678827426, -0.028781431757257857, 0.001000000000000017, 0.11040118703188523, 0.21748508096147054, 0.1746891445602557, 0.6731980679807849, -0.25165377380426196, 0.027029477952105523, 0.38042697839128337, -0.8562792965927752, 0.06820702580285459, 0.10263623041660788, -0.42438693577625336, 0.07522115992287692, 0.9331232132065089, 0.23462140223098962, -0.05807192794564756, 0.5004058585404046, -0.030695514089193385, -0.5073861833602822, 0.024451692608034346, 0.5766600483134329, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22640350633901685, 0.897467487864544, 0.740021916427541, 0.7623662945583641, -0.051829908117663616, -0.02579755529317084, -0.6445508356109401, 0.006020226224855417, -0.20905275152726485, -0.0010000000000000189, 0.03490555913652906, -0.0457981007214224, -0.0010000000000000273, -0.006852810246658608, -0.015531331313828039, 0.0009999999999999972, 0.12213188202918565, 0.18378719734938603, 0.17767991749386045, 0.7503119693069231, -0.29149125558351, 0.016293804467497227, 0.36401492965183174, -0.8632252681785266, 0.06726012472445599, 0.11164738232264244, -0.4339145431728047, 0.04276505924132051, 0.9359727946157584, 0.18113408406201031, -0.057211742138959965, 0.4964483974799335, -0.0345911931320866, -0.5179027434515342, 0.023415589444510002, 0.5909397375768976, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2287223283641949, 0.8771068852391406, 0.7373641205681069, 0.7659071499648754, -0.049256248327507, -0.033423136522815616, -0.6401897793454367, 0.04233022363117354, -0.20359033244427374, -0.0009999999999999872, 0.01877229745642986, -0.05704245603809161, -0.0009999999999999846, -0.013643806165529347, -0.0018435738544810667, 0.001000000000000008, 0.13015114208866335, 0.1565721146126926, 0.18181257896234337, 0.8169729488588174, -0.3293953196393298, 0.002477148242580295, 0.3528560490794435, -0.8673872566947234, 0.06566310610363636, 0.11942791156730967, -0.44076236325838286, 0.016809427785856458, 0.9387034654767047, 0.14126968335076628, -0.0557721136047584, 0.49347409165527256, -0.03846678402788032, -0.528379318135732, 0.02161282008419561, 0.6104269118199872, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23058524020012328, 0.8589521491500546, 0.7358325888604805, 0.7685497993451975, -0.04787419118052287, -0.03927245337744082, -0.6367864180013456, 0.07476580661209849, -0.19840277530088887, -0.0009999999999999974, 0.008141884321910351, -0.06280172015106054, -0.0010000000000000085, -0.01996460361787362, 0.01211304929156722, 0.000999999999999981, 0.13771461646647815, 0.13559619459850691, 0.18602540456017722, 0.8622775670202577, -0.3608385176191905, -0.005533520449660279, 0.34515626941932315, -0.8689862954683061, 0.06694251052436295, 0.12713862164316703, -0.4444307171294461, 0.0041860502463255365, 0.9441187427288443, 0.11549430204315249, -0.05627217091229304, 0.4879123310008739, -0.040795587642339065, -0.5350216568758349, 0.02253387274214586, 0.6267969052245803, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23261335196643163, 0.841716560362957, 0.7345949248237613, 0.7709189221405295, -0.04754390240130089, -0.04384336715980246, -0.633641343337247, 0.10715740533292885, -0.19495148452587008, -0.0010000000000000122, 0.0012049715660615154, -0.0649735203425949, -0.0010000000000000024, -0.02670382268162932, 0.02551675365358264, 0.0010000000000000106, 0.14159007443099905, 0.11728172421356421, 0.19000749401042966, 0.8976150087228503, -0.3882122283575609, -0.012520683701896108, 0.34056509754126274, -0.8694551571102993, 0.06887449155756993, 0.13425179185410469, -0.4468649140692691, -0.0026738007896903557, 0.9500817548801246, 0.09881586790381552, -0.05716443041464322, 0.48246279482124027, -0.04280672714989907, -0.5408534179213341, 0.024007114562412706, 0.6440143334124894, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23406224977536283, 0.8276972430676611, 0.7334400539737033, 0.77329413660209, -0.04731224745305312, -0.04675459673836896, -0.6305487587979238, 0.13130468956696495, -0.19305465624861154, -0.0009999999999999818, -0.0030056506409198683, -0.06540723502844753, -0.000999999999999993, -0.034439067960646406, 0.03636258334018506, 0.0010000000000000015, 0.1464769772990281, 0.1050192768352728, 0.19310915530663, 0.916099388142441, -0.41138776638759067, -0.015579557676205226, 0.33832378012719294, -0.8695645335331895, 0.0711941884044427, 0.13916134526830395, -0.4469794392900361, -0.0035395763639799803, 0.9553650818828316, 0.08878414465787889, -0.05752173739607372, 0.47828962044553214, -0.043076576191561626, -0.5410754795465834, 0.02442024809492338, 0.6585250715370656, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23545908371899646, 0.81465954399941, 0.732312546631415, 0.7757087319434456, -0.0472993140510503, -0.04896952846353468, -0.6274079401466398, 0.1536395406769295, -0.19224994796306746, -0.0010000000000000044, -0.005844513668276734, -0.0648024183260069, -0.0009999999999999966, -0.042712318033655286, 0.04639023423746164, 0.001000000000000012, 0.15021631532024743, 0.09428620496173422, 0.19572860052531077, 0.9289796059026142, -0.4324488406608565, -0.01746214493829441, 0.33734177002868493, -0.8693033434915942, 0.07368879325273216, 0.14339445758514416, -0.44657816378079357, -0.002209520668367844, 0.9605031233323439, 0.08183183464180904, -0.057764820284100106, 0.4744450299240953, -0.04297511518035002, -0.5397049193197738, 0.024607893603816787, 0.6724942380214594, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2361937044975438, 0.8044129514796576, 0.7312654321461095, 0.7780806524840338, -0.0468680014305694, -0.05028763617060783, -0.6243917378699814, 0.1676932493839203, -0.1915412963747356, -0.0009999999999999913, -0.007845562952843508, -0.06448955885429261, -0.0010000000000000221, -0.0510324835707435, 0.05252708438154285, 0.0009999999999999966, 0.15744746108986768, 0.08858603014845742, 0.19752387979372885, 0.9285793271965346, -0.4502493756635961, -0.014699687799508246, 0.3366101101021253, -0.8696769597499258, 0.07553136214460868, 0.14534952256119107, -0.44482299558918337, 0.003072622773103536, 0.965696346011206, 0.07793959017747269, -0.05817360464421948, 0.47044331272183415, -0.042049881976948394, -0.5338168415292694, 0.023510216332485137, 0.6813254886073555, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23678441895686891, 0.7948564369839553, 0.7302236067455984, 0.7804524669662292, -0.046352777525870886, -0.051343794815725236, -0.6213768434339066, 0.17995803925383116, -0.1910137523933512, -0.0009999999999999983, -0.009558931006290334, -0.06416583305419882, -0.0009999999999999718, -0.05946296607701898, 0.05776087942122962, 0.0010000000000000076, 0.16504434780226912, 0.08401196989283126, 0.19905107782692044, 0.9248695669175516, -0.46702637455832696, -0.010735892769641365, 0.33602603809676385, -0.8700589961969711, 0.0772604315789081, 0.14678830102666326, -0.44278814338012223, 0.00938215415813541, 0.9709014502364303, 0.07483145742043891, -0.05861839442847261, 0.4663574328224598, -0.04094169129902607, -0.5267272420996009, 0.02212925599131729, 0.6888730148965121, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23661807068360738, 0.7874078350086188, 0.729051575119962, 0.7827708395501984, -0.04530079251632852, -0.05186169764908411, -0.6184884924266777, 0.18462375802883296, -0.19018123939486611, -0.0010000000000000048, -0.011020130491261552, -0.06462911258185208, -0.0009999999999999911, -0.06734352371535635, 0.059102163894023285, 0.0010000000000000048, 0.1776433553715806, 0.08313268729621838, 0.20013968539963636, 0.9114923203620944, -0.4817087049575725, -0.003961873233402229, 0.3350584457076681, -0.8707724503713157, 0.0773343322623675, 0.14663060457987892, -0.4389301597276543, 0.018194981578793953, 0.9762197141813596, 0.07281514919740968, -0.058187764105350724, 0.46268266022885157, -0.039809695514003145, -0.5167650059913524, 0.019703650967019485, 0.6912575082319511, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23626958112146232, 0.7804583520586077, 0.7278512867429423, 0.7850675017842053, -0.04411689783127849, -0.052254247463168806, -0.6156234324569764, 0.1876901184066544, -0.18927580187831608, -0.0010000000000000106, -0.012471816518130695, -0.06529297923895498, -0.0010000000000000176, -0.07510297802495805, 0.05956134141880512, 0.0010000000000000009, 0.19113283294309039, 0.08309695751089374, 0.2011159086513507, 0.8955863045643014, -0.49579161567751856, 0.0035429830672883468, 0.33396349807438885, -0.8714268499810248, 0.07702115967001039, 0.14613924184050614, -0.4346532220197354, 0.02773950400898114, 0.9815642151121239, 0.07133993923717123, -0.05755534424057204, 0.45916485393038287, -0.03867316151450603, -0.5061617701473398, 0.017038700081809518, 0.6925175957269051, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.23527861038822998, 0.7750171179048707, 0.7265843620035538, 0.787273335088441, -0.042707653619624676, -0.052154418454733, -0.6129083690215683, 0.18403605761164565, -0.1880191567756587, -0.0009999999999999905, -0.013299109273594638, -0.06612288377248193, -0.0010000000000000037, -0.08203230944706626, 0.05726756250660975, 0.0009999999999999874, 0.21061533586865377, 0.08476364353777491, 0.2023306623024087, 0.8710210577235317, -0.5077324449700966, 0.010345551092174119, 0.33307311291427577, -0.8718885783684667, 0.07388709724280641, 0.14529432173488735, -0.4289813474256082, 0.039897067955183496, 0.98665890472612, 0.07263184131306497, -0.057407303149077216, 0.45713447199613416, -0.03766180871495972, -0.4947708231583205, 0.013837664850595078, 0.6905337382008813, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2341364592803149, 0.7699716405585976, 0.7253200775906915, 0.789442139369821, -0.041262765449046016, -0.051936359457837006, -0.6102303723520852, 0.17900756336868792, -0.18667116490113062, -0.0010000000000000083, -0.01402998533356567, -0.0669928952546289, -0.0010000000000000048, -0.08872881619439954, 0.05433707485940934, 0.0010000000000000117, 0.23113087071408778, 0.08684628910893484, 0.20362384314401547, 0.843666325587961, -0.5189566640661458, 0.017065244649898278, 0.33221208824097914, -0.8720638632818485, 0.07006942629710976, 0.1444017109220164, -0.4229575740750091, 0.053022868875017934, 0.991693812123342, 0.07559936477457457, -0.05737347594091945, 0.45567317216081876, -0.036679166861139686, -0.48347225591756054, 0.010509821519365544, 0.6881942281043518, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2325170157230341, 0.7663009480116362, 0.7245287301619183, 0.7915840011098794, -0.03993275144552439, -0.05099362686429464, -0.6076181321916673, 0.16737585302544752, -0.18597507579786396, -0.0009999999999999922, -0.0134732130297675, -0.0671904011135888, -0.0010000000000000189, -0.09487315964906907, 0.049117994687197374, 0.0010000000000000052, 0.2575674055573557, 0.08952486379278, 0.20506178946859596, 0.8068541516807074, -0.5256535247142279, 0.01825789541307276, 0.3319035226159773, -0.8713891633776497, 0.06293306258777191, 0.14163313467470184, -0.4160576383084123, 0.06971852746455533, 0.9966874816712104, 0.08377108234989628, -0.0582911712463828, 0.45467293449803753, -0.03567342569820974, -0.4718589414252695, 0.0073399268051285846, 0.6826458891995767, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2307828315887276, 0.7630082013744369, 0.7238943425365898, 0.7936863310099056, -0.038662986646955735, -0.04991766175263129, -0.6050416584638798, 0.15469014364535294, -0.18536073369605355, -0.0009999999999999874, -0.01270985524389271, -0.06725313692843359, -0.001000000000000009, -0.10084138873160003, 0.04342935652973679, 0.0009999999999999916, 0.2845483493482965, 0.09236959238717618, 0.20664357907745454, 0.7666812281649871, -0.5307870535358346, 0.01816607531182837, 0.33166764509128166, -0.870051487603628, 0.054974606433054844, 0.13843072195992162, -0.4088941509260912, 0.0877409835757096, 1.0016699001047642, 0.09488993863179332, -0.05944220392360004, 0.4539956097937017, -0.03465930280809518, -0.46058357429423386, 0.004207898929988494, 0.6771513884466788, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2289705922797589, 0.760822195952859, 0.7238329649802326, 0.7957045281784088, -0.03768758941208537, -0.04828171465263765, -0.6025801402891439, 0.13638221803509937, -0.18567808063733346, -0.0010000000000000128, -0.01082463196810258, -0.06674867053346696, -0.0009999999999999994, -0.10530956133713702, 0.03624865269162362, 0.0010000000000000024, 0.31732784786374835, 0.09507991681962355, 0.20856324760311332, 0.7170752730300559, -0.5294845085453599, 0.01376619535307344, 0.33258963240415435, -0.868545794628104, 0.04263430814807805, 0.13359993662846434, -0.40268032907532314, 0.10885175961689668, 1.006183100477512, 0.1127353441815642, -0.061305102721358085, 0.45450273573804945, -0.03315641219937515, -0.44864361446119, 0.0009159127477802902, 0.6716197120372986, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22712732929840912, 0.7589477862636538, 0.7239849054410715, 0.7976391413057341, -0.036805590625201764, -0.04664868967795392, -0.6002008401419568, 0.11757198279117632, -0.18599486685387892, -0.0009999999999999866, -0.008920615337466083, -0.0662130460494275, -0.000999999999999964, -0.10938704687397446, 0.028948004866848735, 0.0009999999999999944, 0.3503194058702519, 0.09775181182442479, 0.21085169866732023, 0.6637750354917479, -0.5259786614912794, 0.008336849812134083, 0.3336025978966042, -0.866251434838068, 0.02923299322190739, 0.12835636003836953, -0.39662738689791033, 0.13137125389207224, 1.0105746704660814, 0.13446616344104803, -0.06334415345841163, 0.45528192191402483, -0.03152212003584173, -0.4369069691280949, -0.002414407569000068, 0.6672467943315594, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22501642860660664, 0.7575369399382008, 0.7241586724463331, 0.7995382056741657, -0.03593085718718855, -0.04500651922139809, -0.5978478438505094, 0.0938294899200382, -0.18672098188911743, -0.000999999999999997, -0.006487876772987331, -0.06571642489731656, -0.0009999999999999966, -0.11194697937525183, 0.021034666720468387, 0.0009999999999999998, 0.39000443393712225, 0.0993829414657682, 0.21382306366739387, 0.6055638362552007, -0.5169894252320305, 0.003297860996040806, 0.33517710833806197, -0.8641822170782506, 0.012248690913935274, 0.12319024131387964, -0.391773973379546, 0.15564845413991735, 1.0136432415733816, 0.15853338599107333, -0.06575739861582505, 0.45607910232144244, -0.029452304232933196, -0.42611049362196574, -0.00631799217924843, 0.6616888742350064, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22278769147552951, 0.7562525618597952, 0.7244172125680113, 0.8013619756746, -0.035030343814626635, -0.043554173077484964, -0.5955626692151448, 0.0697150046113055, -0.18725879412719573, -0.0010000000000000044, -0.004371111641695805, -0.06542632787271348, -0.000999999999999986, -0.11422022709298517, 0.013266577507715325, 0.000999999999999997, 0.4303498275395403, 0.10070087447612802, 0.21730353862299512, 0.5447751598490892, -0.5061469246507112, -0.0015787257451113062, 0.33657504204023714, -0.8611093244975886, -0.005598209889594105, 0.11799479110706039, -0.3871662438809466, 0.18112675499252892, 1.016361066464779, 0.18536718903155391, -0.0682608913461053, 0.4564530621599304, -0.027257453351117036, -0.41566617752093377, -0.010368130199031786, 0.656805639934705, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22050357114012636, 0.7553158535625496, 0.724599423561518, 0.8031382199772927, -0.03423827714648665, -0.04232219465209703, -0.5933005746074886, 0.04253425879484627, -0.18780266924668632, -0.001000000000000002, -0.0016659515041230009, -0.06535064229582897, -0.0010000000000000037, -0.11529055945736205, 0.0055778440621532446, 0.0010000000000000026, 0.47295317565587114, 0.10101688249173162, 0.22152510166291575, 0.47851813745857824, -0.48559499850107163, -0.004535022567882819, 0.3382929593819146, -0.8578842989942684, -0.02599216967954026, 0.11237856839471595, -0.38388605428320655, 0.20914367878273488, 1.0181118733767625, 0.2164064177335695, -0.07129397447476311, 0.45714262314021786, -0.02486938234022909, -0.40585038576207405, -0.01508846559300347, 0.6513649746480739, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21810704781408236, 0.754493548083604, 0.724838408062433, 0.8048396702400198, -0.03342270718598812, -0.04136828592843227, -0.5911046377517603, 0.015242287156212151, -0.18807427834463983, -0.0009999999999999913, 0.000539297233641327, -0.06554857744515045, -0.0009999999999999686, -0.11634814332520779, -0.0017688726677782315, 0.0010000000000000057, 0.5155199845085554, 0.10101943367748842, 0.22630221632525763, 0.40929423120211167, -0.46223609024894113, -0.006870264425152256, 0.3397097135303511, -0.8530046125495109, -0.047052524783057145, 0.10654055906364134, -0.38092570429321654, 0.23873096406290895, 1.0195968060715, 0.2511168592272732, -0.07446098071177092, 0.45713925896530555, -0.02240936539949565, -0.3961427960838657, -0.019981620717025243, 0.6466731399659532, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2154356547620678, 0.7538979955746925, 0.7244519729078124, 0.8065553222451686, -0.03256847303991653, -0.04066896517078174, -0.5888580830673711, -0.012882182224658634, -0.18862778626568752, -0.0009999999999999974, 0.00475675317357388, -0.06602947775466625, -0.0009999999999999766, -0.1148852623240404, -0.009429668400291991, 0.0009999999999999937, 0.5527207443322003, 0.10046943515516699, 0.23189236064911928, 0.336969629320303, -0.42879258207606574, -0.009137915097212073, 0.3414194038764207, -0.8470516293921283, -0.07008625820059156, 0.10088455235714779, -0.37853205023955644, 0.2693957637423109, 1.018894976327882, 0.2875864836603602, -0.0772299582153028, 0.45606960565634974, -0.01978045893046047, -0.38791570175115686, -0.025406820840228914, 0.6397169671752031, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.212541438177162, 0.7533397728302735, 0.7239667179494967, 0.8082326848518612, -0.031613296647935975, -0.040194613300089735, -0.5866386619324186, -0.04069296454076152, -0.18912061204197655, -0.0010000000000000085, 0.008852614189682485, -0.06672154814844329, -0.0010000000000000378, -0.11327497307745822, -0.01693659344474824, 0.0009999999999999916, 0.5878422409589013, 0.09977238355604742, 0.23790507380653853, 0.26268916284086863, -0.3923910358499027, -0.011369960566958973, 0.34284785363706144, -0.8386983868577413, -0.09367625607993732, 0.09507723732364314, -0.3762670668989708, 0.3009730617146136, 1.0175727575060198, 0.32696497015395715, -0.07987595179219976, 0.4534165039748098, -0.01708276813763076, -0.37980824165450394, -0.030975236767365334, 0.6325898529561998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2097282651130509, 0.7524588582790567, 0.7233890566848346, 0.8097508601854579, -0.0301437291893595, -0.04035941922767917, -0.5846075754718502, -0.0679439654950396, -0.1887499001248039, -0.0010000000000000015, 0.014315665160048096, -0.06893446248702632, -0.0009999999999999788, -0.1067162512422672, -0.02355440373635497, 0.0010000000000000167, 0.6161222431993908, 0.09832440450245428, 0.24471810213511955, 0.18814559499402853, -0.3629146922084306, -0.013319962861785717, 0.34278713311313574, -0.8294328049335665, -0.10787072787749144, 0.09037219176487218, -0.3741842131698403, 0.33360662075517766, 1.0150093259860742, 0.3670485456410669, -0.0816103703145411, 0.44857115006359444, -0.014791013147972773, -0.37385497542390705, -0.03632203551081663, 0.6216990666885136, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20675946759319483, 0.7514441903245694, 0.7228313041517831, 0.8112267134235377, -0.028440797988302784, -0.04069353410147493, -0.5826202680307012, -0.09471011890239392, -0.18829682997220695, -0.001, 0.020239352120330022, -0.07148946624658446, -0.00100000000000004, -0.0990437568322926, -0.029916823283758567, 0.0009999999999999994, 0.6413173661719115, 0.09675436088878564, 0.2516580178072121, 0.11308047375492708, -0.3347216026326625, -0.015222708917241241, 0.3421511227766889, -0.8173554001514206, -0.119253918096931, 0.08570966543037291, -0.37217927391977074, 0.36663331453772835, 1.0120807382614947, 0.40930107619402994, -0.08307475372498585, 0.4416094206804373, -0.012604590625238768, -0.3680609801471922, -0.04160346086181149, 0.6096531340284456, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20393324681228397, 0.7500080539810559, 0.7225008951280117, 0.8125362952046637, -0.026376651277815473, -0.04137482577073724, -0.5808417728046851, -0.11962448048869614, -0.18719871489012796, -0.0009999999999999942, 0.026801662284508893, -0.07483478393403453, -0.0010000000000000367, -0.08895571970714376, -0.03520236094065341, 0.0010000000000000076, 0.6565388338507142, 0.09489714959421343, 0.25851871758867223, 0.03885337467116902, -0.3193672926844768, -0.01628882710120351, 0.3401950613374552, -0.8022709492569545, -0.12287581801461987, 0.08263990318994446, -0.3710939191450372, 0.3987546192953982, 1.009195903068245, 0.4535218753973448, -0.08431358195227247, 0.43284194157293326, -0.010991685087825076, -0.3638228319059163, -0.04597167901011908, 0.5963006152485915, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2010133736711138, 0.7483398109487348, 0.7222837934933584, 0.8138271130556493, -0.024083063828281792, -0.04203975291776578, -0.5790838413103614, -0.14356433856695627, -0.18620598099474395, -0.00100000000000001, 0.03378198413469867, -0.07830813853615616, -0.000999999999999952, -0.07829442119668255, -0.04028103094612015, 0.0009999999999999996, 0.667182674872261, 0.09307921732731997, 0.2650139055775984, -0.03450434693191018, -0.30695597330437446, -0.017259882185562393, 0.33777847144076956, -0.7838901225389164, -0.12416318025872229, 0.07981623905752205, -0.3703327255619359, 0.42974631259688967, 1.0063211251082, 0.4995793471220043, -0.08548517075761884, 0.4219066163292289, -0.009542818402711106, -0.3595016924640007, -0.050064932345720604, 0.5819787862108319, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1981243551000431, 0.7463143747720052, 0.7220066441233959, 0.8148647558154655, -0.021637289689313645, -0.04249402143992572, -0.577686347048782, -0.16432149349555236, -0.18559450381213127, -0.0009999999999999966, 0.04145232567251445, -0.08120695561840505, -0.0010000000000000046, -0.06659025516192675, -0.04436415184690947, 0.0009999999999999979, 0.6636320265624305, 0.09118957283448077, 0.2711839614192521, -0.10566879057209982, -0.2967464040230294, -0.01829764834263279, 0.3353134961565897, -0.7630664461428845, -0.12339509775978137, 0.07784017573820973, -0.36989404728225306, 0.45953291527563556, 1.002942497296333, 0.5446888988161819, -0.08748143100585766, 0.4112206886878014, -0.008687294569380562, -0.3553634348060198, -0.0537347249325808, 0.5650372703628364, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19522156242219932, 0.7439481545878444, 0.7217724623504963, 0.8158675057775183, -0.018986770729299815, -0.04265545261882044, -0.5763507854731592, -0.18350642535543538, -0.1854048741991656, -0.000999999999999998, 0.049427778932165455, -0.08385929417702216, -0.0009999999999999996, -0.05462121910390198, -0.048293918380106285, 0.0009999999999999929, 0.6539223685612183, 0.08937750107830823, 0.27669455554325023, -0.17426534688997616, -0.28650798424855267, -0.01970465198423915, 0.33282075040913695, -0.7397355266801146, -0.12199299669803307, 0.07600200657055, -0.36955749490217504, 0.48730862669731967, 0.9994058138921773, 0.589308084894655, -0.08973899033280992, 0.39917693510675023, -0.008003777060161973, -0.3506927647491543, -0.05727322089557353, 0.5463184038278872, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19267761697203595, 0.741249874964728, 0.7219899637520287, 0.8168086217213082, -0.016269146708139517, -0.042358189420566864, -0.5751215298840225, -0.1984395258325138, -0.18557693016254803, -0.000999999999999994, 0.05641666197830122, -0.08618826026527159, -0.0010000000000000263, -0.043140691144351054, -0.051782948150885535, 0.001000000000000015, 0.6289457602245445, 0.08771605373576466, 0.28116627630816027, -0.23767539339982144, -0.2729173994718643, -0.021172798513245373, 0.32997812578454894, -0.7141231255600882, -0.11978049038445258, 0.07436064300286098, -0.3691291264647649, 0.5111620982840708, 0.9961260159245604, 0.6320945229768196, -0.09144403792705885, 0.3855506462386366, -0.008140141763020423, -0.3463353934163366, -0.06004170544662631, 0.5244588341862247, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19037231641784963, 0.7381704381733166, 0.7223943754106124, 0.817756053263405, -0.013446267509304322, -0.04158415421075509, -0.5739032961742809, -0.2113390162117511, -0.18614171913422464, -0.0009999999999999968, 0.06317022079036244, -0.0882333875365489, -0.0010000000000000213, -0.031702661784005576, -0.05529128939999318, 0.000999999999999996, 0.5974227016151316, 0.08619970859229943, 0.28467896789868136, -0.29717214353998395, -0.25782002824477235, -0.02310966956706677, 0.32721957825485903, -0.687067135102258, -0.1172960045176047, 0.07281440562460945, -0.36851519807976535, 0.5318599775416069, 0.9929288272477471, 0.6731403414058358, -0.09297965777025102, 0.37050239459904216, -0.008535825976235444, -0.3414636078862328, -0.06256413229718222, 0.5007463299155681, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1885054641516152, 0.734752465190229, 0.7232924187013601, 0.818464291230451, -0.010829820924682137, -0.04002199225338328, -0.5730594725641006, -0.2193907954963541, -0.1876367192422314, -0.000999999999999999, 0.06908358884735441, -0.08890490895207409, -0.000999999999999966, -0.021564296705800966, -0.058274266204274824, 0.0010000000000000104, 0.5522870665059717, 0.08555970675234546, 0.2868283292555147, -0.3523006393259187, -0.23735252717479605, -0.025398743112161993, 0.32495794843673703, -0.6588988267578625, -0.11573291922733826, 0.07124060541485283, -0.36768582982981546, 0.5494778363923419, 0.9906208599824045, 0.7122345724820042, -0.09514653970486449, 0.3560189772161025, -0.009063564690757983, -0.3355305482527551, -0.06453597820757286, 0.4770526249722175, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1870537708872009, 0.7309280443553208, 0.7243380336546807, 0.8190606206981923, -0.008383937249393784, -0.03774620532183931, -0.5724025097791691, -0.22532307024770465, -0.1896837079258613, -0.0010000000000000078, 0.07514570886353403, -0.08854787298148568, -0.000999999999999998, -0.01170366930398148, -0.06120427621183329, 0.000999999999999979, 0.5016210123641253, 0.0853890798275814, 0.28797610303634646, -0.4036030206822998, -0.21516445760109304, -0.02836187582816502, 0.3234416190463267, -0.6303737071068389, -0.11446226256358105, 0.06979544534150124, -0.3663086589934455, 0.5641718498528399, 0.9886070091327354, 0.7488694954837837, -0.09752572029488511, 0.34152987688507924, -0.009629626153229674, -0.32895150036515336, -0.06632560027569043, 0.4529377381530413, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1861467324971567, 0.7267928469730001, 0.725374984239749, 0.8193421832623434, -0.006621218859173259, -0.03447660069289443, -0.5722288966771755, -0.22657848039558198, -0.19258200980678206, -0.0009999999999999998, 0.08128485698362843, -0.08575306253289468, -0.0009999999999999747, -0.004188653327886184, -0.06363281630089081, 0.0010000000000000115, 0.44173627505493906, 0.0862413398394085, 0.28850764933089484, -0.44959502677316576, -0.18818245544667686, -0.03240536045060181, 0.3239372680229505, -0.6020995360452565, -0.11467047110024578, 0.06793801699783258, -0.36452139002517453, 0.5756601642169924, 0.986397614663142, 0.781635838709164, -0.1004694912140857, 0.3281079785990008, -0.010596468806781887, -0.3224832929179216, -0.06783911419846914, 0.4291127569337467, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18582306380191158, 0.7222508843444749, 0.7263117415099243, 0.8193762387590335, -0.005498470874591226, -0.030322132474952027, -0.5724272132396505, -0.22642482356060561, -0.19586659926042208, -0.0010000000000000009, 0.08864004780989358, -0.08090373833837111, -0.000999999999999967, 0.002630191837308513, -0.06590054947753189, 0.001000000000000004, 0.37970703379261045, 0.0877990531121429, 0.2885998635521891, -0.4915853746162226, -0.16076035188542578, -0.03751497024286245, 0.3262014663717337, -0.5743699272321184, -0.11536841756904705, 0.06612737206285954, -0.3616562381056834, 0.5843779826251962, 0.9841215408012299, 0.810964526742104, -0.10361077032341229, 0.31553176417516304, -0.011694228132897212, -0.31599882520154043, -0.06925937136227331, 0.40570107029025476, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18605934211785488, 0.7174809882150177, 0.7269859079278549, 0.8191591861925912, -0.005544451819768423, -0.025124724651581942, -0.5729888610972239, -0.22392760488728397, -0.19951639123476572, -0.0009999999999999983, 0.09755865551812988, -0.07283541495688245, -0.0010000000000000165, 0.006572574337489959, -0.0681692205414648, 0.0010000000000000065, 0.3172894408453942, 0.09041113652667641, 0.2880814552889751, -0.5281354065491561, -0.1319344983751709, -0.04390212809609077, 0.33117601060914337, -0.5473455243548403, -0.11813756690726461, 0.06393379856336971, -0.357749927754785, 0.5895761465232439, 0.9821274957485148, 0.8369998259058058, -0.10636524312147261, 0.30426075628772736, -0.01291026188478293, -0.3107330082542495, -0.07052925466617602, 0.38437957893846336, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18686889582438432, 0.7122978787082381, 0.7274668943231063, 0.8186492505786928, -0.006479044335647802, -0.01919705816097615, -0.5739363200472741, -0.2218843207925688, -0.20300197830099953, -0.0009999999999999998, 0.10842337545875859, -0.06240907759082421, -0.0010000000000000254, 0.009586639475999753, -0.07027510368263855, 0.0010000000000000026, 0.25887885130032245, 0.09385828894235382, 0.28757309954566346, -0.5607990324655304, -0.1054877898324544, -0.05155919046870685, 0.3380802177593239, -0.5212424219949441, -0.12163675006743487, 0.06177064991496314, -0.35250294005440863, 0.5923611744856541, 0.9802323041287301, 0.8596187894868675, -0.10898794471929546, 0.294278020726181, -0.014164286393386669, -0.306122777467879, -0.0717470011935157, 0.36452714731571567, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.18804307094751224, 0.7069980416575663, 0.7277354334339768, 0.8180848480988482, -0.00813508949967873, -0.012964910925898644, -0.5748938273408407, -0.22045702783097718, -0.20597485115182895, -0.0010000000000000009, 0.11993104728676064, -0.050613212848737446, -0.0009999999999999747, 0.010416071632212451, -0.07230285852675522, 0.0010000000000000122, 0.20982896580740196, 0.09793369441499472, 0.28692031745210805, -0.5890152866872411, -0.08305017551666032, -0.061034571542792515, 0.3465097604164544, -0.4962769155428733, -0.12564333580745135, 0.05887032973524409, -0.34668374436005256, 0.5927680839788251, 0.9785891406192514, 0.8799747300665587, -0.11114979964930291, 0.28549627541932854, -0.01542564425718686, -0.30263481555248556, -0.07396744687582926, 0.348322422630234, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1895183446601893, 0.7013983939386489, 0.7279295689681667, 0.8174039517695357, -0.010158277252671204, -0.0066834275320113365, -0.575936559727917, -0.22027903129906456, -0.20820819460666884, -0.0009999999999999987, 0.13197603262456406, -0.03822764818718601, -0.0010000000000000115, 0.010509356306069506, -0.07407822882833029, 0.0009999999999999838, 0.16786341420279138, 0.10248723837900027, 0.2865685806881514, -0.6143158360755838, -0.06450333872355206, -0.07174650009853757, 0.3555301423938626, -0.47229236661810836, -0.1297908008005929, 0.05582714747357814, -0.3404289445567023, 0.591887996949651, 0.9770367359440871, 0.8978449202456665, -0.11315027388009041, 0.2777581593458715, -0.016684758262181038, -0.29988967558827795, -0.07655146308557312, 0.33396114051430664, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19110158244007394, 0.6959949425413137, 0.728377122401916, 0.8168153871491345, -0.011965860624801513, -0.0008607995248817425, -0.576774393086322, -0.22091219552247318, -0.20993433113793533, -0.0009999999999999979, 0.14254581104468497, -0.027199293874838888, -0.000999999999999994, 0.0102417399276458, -0.07555200390767652, 0.0010000000000000113, 0.13650601394489068, 0.10704345640565174, 0.28593383692949115, -0.6368908165042947, -0.05061625227490647, -0.08257514379933525, 0.36361110070090963, -0.44892798680135787, -0.13363139756195017, 0.052659929497292896, -0.33537378477512764, 0.5911344214506739, 0.9761845707032502, 0.9149830720406571, -0.11583374835434285, 0.2698896297412535, -0.017305377435145403, -0.29683788944213796, -0.07913264520925285, 0.32118426489567575, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19273020624524986, 0.6906276766408885, 0.7289383028689217, 0.8161204172490534, -0.013678974080275538, 0.004507128255778742, -0.5777023766717836, -0.22183473821961724, -0.21097165796140221, 0.0009999999999999998, 0.15247454044839756, -0.016860133890280293, -0.0010000000000000041, 0.010295000788989577, -0.07634137510346367, 0.001000000000000004, 0.11059138906917243, 0.11155747833439823, 0.28507741178121776, -0.6568934020522076, -0.03946451971448205, -0.09344842226998167, 0.3714631386789529, -0.42698118323907186, -0.1373348981250351, 0.049478972660510614, -0.3324154204258573, 0.5891247913616844, 0.9755926229965247, 0.93087927370236, -0.11877177700693828, 0.2623524830708099, -0.0176808722514485, -0.2939702075205401, -0.08171290365380174, 0.30927376735827705, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19421006020173046, 0.6856754272286727, 0.729520686477116, 0.8159282080146236, -0.01473836467907394, 0.009294992827191201, -0.5778905978478746, -0.2234277585119569, -0.21185146039058017, 0.001, 0.1598113909064985, -0.009163408937914614, -0.000999999999999996, 0.009595248333462097, -0.07770731366544041, 0.0010000000000000044, 0.09236802940521474, 0.11545104650546807, 0.2843277897061831, -0.6766201947517424, -0.03254533670082933, -0.10411323285894712, 0.3777097454505977, -0.40443565861094083, -0.1396005631047366, 0.04632616190520821, -0.3293669933555331, 0.5875264837125083, 0.974647302735829, 0.9484556412280154, -0.12188743359129613, 0.2539611130077046, -0.01770060949003049, -0.290945893933464, -0.08453305751902218, 0.29966710636033694, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.19565415895949112, 0.6808634435011621, 0.730119598099153, 0.8158845499379047, -0.015513295036856013, 0.01375453154861463, -0.5778430164946199, -0.22520647405704358, -0.2124834791615246, 0.0009999999999999994, 0.16604196084906267, -0.0026291401175501886, -0.0009999999999999948, 0.008783357686154982, -0.07908343451031788, 0.0009999999999999907, 0.07747221579976597, 0.11906744166847712, 0.28353940109081505, -0.6955942835047012, -0.02754507581012906, -0.11470275031777898, 0.38335448458557986, -0.38217187624355753, -0.14127245168304295, 0.043224533261960074, -0.3269608217192516, 0.5854024821044959, 0.9735665340730661, 0.966205661786917, -0.12507144887725746, 0.245395741978384, -0.017576776513482387, -0.2879734892294703, -0.08744380901368339, 0.29094801524297054, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.196894665339866, 0.676588046560249, 0.7308083136899096, 0.816239723296375, -0.015479180463105452, 0.017526531364716918, -0.5772399239342082, -0.22727878211434857, -0.2131899129790484, 0.0009999999999999987, 0.16948887555747144, 0.0010166290384843615, -0.0009999999999999979, 0.007939135959935797, -0.08067012792323554, 0.001000000000000005, 0.0680019209828164, 0.12176738780463639, 0.28201073271168464, -0.7132799681798964, -0.02767939775957887, -0.12408295799815762, 0.3875201112690527, -0.36097295470234564, -0.1407483636212701, 0.04055307432626964, -0.3259617782729954, 0.5835643113653933, 0.9725963714632893, 0.9854776865701667, -0.12825112515826045, 0.23643835682249942, -0.01750872657779171, -0.2854443251762341, -0.09039024265451859, 0.28370811138756125, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.1980897643151346, 0.6725016339900778, 0.7315339187682273, 0.8167545042558273, -0.015088330393552531, 0.021039658248959925, -0.5764041592880108, -0.2294774631008535, -0.21392191588110887, 0.0009999999999999972, 0.17175246762002805, 0.0034307860998629533, -0.0009999999999999933, 0.007085417196629938, -0.08233997526102543, 0.001000000000000002, 0.06066583672496351, 0.12408216444225487, 0.2801358376650019, -0.7303593418067946, -0.029876002708383902, -0.13301586860321563, 0.3910619430788804, -0.340258773609289, -0.13930840065546302, 0.038095043383495335, -0.32555781520531124, 0.5815842350363146, 0.9716695351152015, 1.0051592154032487, -0.1314299971291595, 0.22737148899924237, -0.017454864987419893, -0.28309165625051724, -0.09335020443402796, 0.27692542116466784, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.199128065353043, 0.6688130111756883, 0.7323198227610588, 0.8175225891475426, -0.014321327813366081, 0.0242831197576124, -0.5752060899347264, -0.2317758650991071, -0.2149623613693619, 0.0009999999999999979, 0.17269658800891677, 0.004424960364538599, -0.000999999999999996, 0.005633073912707906, -0.08477764763060995, 0.0009999999999999844, 0.05579918041841443, 0.1258225277912281, 0.27779609833200625, -0.7451336815411709, -0.033544475828539276, -0.14059246809083284, 0.39406591474386415, -0.3207035993972867, -0.13803792243906177, 0.03617672531922255, -0.3253913423681294, 0.5794017324439795, 0.9708139408619635, 1.0250317781417446, -0.1343869396020747, 0.2174580280279015, -0.0173128554452656, -0.28117187861987974, -0.09557133053707428, 0.2703863924520662, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20012902492780815, 0.6652718259341169, 0.7331284811604325, 0.8183889168770003, -0.013377307542490412, 0.027433575641099148, -0.5738536636663985, -0.23413928884121735, -0.2161377620260172, 0.000999999999999999, 0.1730565432751901, 0.004789856292048345, -0.000999999999999999, 0.003937000681600556, -0.08752641736993767, 0.001000000000000009, 0.05194076424229541, 0.12731706997316666, 0.2752252445358139, -0.7589342987719223, -0.03783972881284138, -0.14763924369798445, 0.39683152077244527, -0.3016201081047165, -0.13679742702120573, 0.03451451515368684, -0.325346107597325, 0.57693533896509, 0.9699871890497006, 1.0448945665518368, -0.13725398839089095, 0.20722545092909178, -0.017131962362106446, -0.27939904827180084, -0.09749283456913928, 0.2638216973892375, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20115567565232856, 0.6620779676975875, 0.7336398899650161, 0.8193049224381386, -0.012245323035684406, 0.030374014013127507, -0.5724219732025633, -0.2364894110596305, -0.21776206250592617, 0.0010000000000000007, 0.17257428615746018, 0.004203084010232926, -0.0010000000000000037, 0.0020366938620283675, -0.09031784143214915, 0.00099999999999999, 0.0492480099161495, 0.12895219048962425, 0.2732428886238543, -0.7711201850915844, -0.04314307316864984, -0.15464311620342883, 0.3999024123240485, -0.28380237405086434, -0.1353802581861263, 0.03308664641038696, -0.32559201175856567, 0.5744564742186987, 0.9681108190197362, 1.065135328769777, -0.14055336877805638, 0.1968571749812777, -0.01728982466771248, -0.2784471445582962, -0.0992463936723736, 0.25793657975694095, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2022156370501735, 0.6590154915142911, 0.7340271741009307, 0.8202371364967327, -0.0110137300642935, 0.033241123692705356, -0.5709507556325302, -0.2388570266422415, -0.21959369391541805, 0.0009999999999999987, 0.17169620396286897, 0.0031715173242534526, -0.0010000000000000005, 5.2106748471089495e-05, -0.09312484354719802, 0.0009999999999999777, 0.047051340083520105, 0.13066083678432547, 0.2714720245876891, -0.7826360408086533, -0.04889523357135763, -0.16165964323711227, 0.4030748880987191, -0.26649563937144544, -0.1338562311214491, 0.03178658433199619, -0.3259720148032975, 0.5718091071463037, 0.965797836267349, 1.0854736573752666, -0.14403468174555373, 0.18643122973506332, -0.017583715296298718, -0.2777814281675986, -0.1009303311189785, 0.25221156785773763, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20339622667539184, 0.6561816531383926, 0.7344874972192025, 0.8212076296667171, -0.009747559171642474, 0.03606530490267953, -0.5694052228858134, -0.24140453763753364, -0.221822626932703, 0.000999999999999995, 0.17035884901371567, 0.0016678159143596867, -0.0009999999999999998, -0.002319097904702145, -0.09617182220844403, 0.001000000000000016, 0.04549468657540182, 0.132870917608923, 0.26960689631331575, -0.7922648557613204, -0.05413635561694333, -0.16798577200979414, 0.40637371950236617, -0.25041900707952125, -0.1325147334082177, 0.0305513171715367, -0.3257257515394712, 0.5693750311560444, 0.9639233318048086, 1.1053324142348333, -0.1468871373100105, 0.17578716330525906, -0.01816114722344563, -0.27651991533702613, -0.10258765646939037, 0.2464235717642684, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20465011086074275, 0.6534330049542678, 0.734977768470957, 0.8221894243023221, -0.00844511298333823, 0.03888373900176646, -0.567821526074308, -0.24404897953430385, -0.2242482635184041, 0.001000000000000003, 0.16877825796390916, -8.976140974796639e-05, -0.0010000000000000002, -0.0048530778663374245, -0.09931960014899996, 0.0010000000000000074, 0.044222789374956564, 0.1353298349914665, 0.2676679320413626, -0.8011202251626903, -0.059187743462086224, -0.17405753336542473, 0.4097049342130213, -0.23483605706126678, -0.13122091146421302, 0.02937489632250686, -0.3252293010818088, 0.5669034241568034, 0.9622363185396604, 1.1249742112446486, -0.14947256411874812, 0.16506282409009665, -0.018857455661352863, -0.2749503240109235, -0.10423412679871003, 0.24048824595032173, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20597733055129003, 0.6508908663764694, 0.7354357509515472, 0.8232719512531834, -0.007120495586127183, 0.0416011716157383, -0.5660759095232567, -0.24695636758367548, -0.22628277026847968, 0.0009999999999999942, 0.16664733580166236, -0.002322449576654922, 0.0009999999999999994, -0.007925173305445874, -0.10283961040187037, 0.0009999999999999996, 0.04327068903012268, 0.13699662339180183, 0.26612923124690513, -0.808912391496871, -0.06378384235748712, -0.1790856464173889, 0.41162067000458574, -0.22073003378569603, -0.1298240411906247, 0.0286291114053343, -0.3251542216029229, 0.5651571293551931, 0.9600872460454237, 1.1445071168871246, -0.15192641881635569, 0.15474849413695393, -0.019593657045083923, -0.2740582337344399, -0.10578937042185338, 0.23452910726301568, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20732716611858784, 0.6482992817794997, 0.7358791614289208, 0.8243113508045797, -0.005654987780662357, 0.044276041258760286, -0.5643743883437504, -0.24974569950702813, -0.22874397817316164, 0.0009999999999999929, 0.1642599784716708, -0.004855343997034559, 0.001000000000000003, -0.010846496036903042, -0.10619214662091125, 0.0009999999999999816, 0.04224662493597054, 0.1395241875928055, 0.26483452121363876, -0.8154406499469317, -0.06826078353787471, -0.18445368501307915, 0.4153911129579128, -0.20647012170448742, -0.12845264522779362, 0.0280209600119484, -0.324267797385487, 0.562059639441782, 0.9577353543873721, 1.1634720388892483, -0.15432498676554687, 0.1444059212870006, -0.02034469799830071, -0.2731052585916348, -0.10730457060948793, 0.228482576914979, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20864455577533955, 0.6458134357304238, 0.7361066105188985, 0.8252865641595821, -0.004109093785678647, 0.04694502248719448, -0.5627444955124912, -0.25283793832102197, -0.23169807118111774, 0.000999999999999995, 0.1618207909218283, -0.007482197899053049, 0.0010000000000000007, -0.013542156436107452, -0.10937186075279151, 0.0010000000000000048, 0.04132651309735529, 0.14250187578115045, 0.2637820203254087, -0.8213041993167647, -0.07256068322228129, -0.18977097931308642, 0.41971122265642735, -0.19338418585659195, -0.12723410145558486, 0.02748420461349449, -0.3232302868814103, 0.559306221346326, 0.9551156853052735, 1.1807468973982367, -0.15680953664728137, 0.134888934195131, -0.021320753765339637, -0.2716835596085079, -0.10886722847844732, 0.22279376470793272, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.20996655706294506, 0.6433689414389803, 0.7362338084734358, 0.8262279673710099, -0.002506474531711458, 0.049620952361274735, -0.5611406460114526, -0.2560890920779307, -0.23490956766874288, 0.0010000000000000046, 0.15930744930072985, -0.010208492369215597, 0.0010000000000000009, -0.016133120572823262, -0.11247076152243023, 0.0010000000000000096, 0.04046232776483788, 0.14572501761527082, 0.2628150474358456, -0.8268927635366081, -0.0767999787091973, -0.19510007618512826, 0.4242540721007888, -0.18080743045889125, -0.12605647549058033, 0.027009679897280668, -0.32214233142390636, 0.5566298185549726, 0.9523753553475525, 1.1972467905180033, -0.15933634612236638, 0.12575724800618146, -0.02239485655279727, -0.27001333629096574, -0.11045194919263988, 0.21718534280787832, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21125528396248527, 0.6410064290146547, 0.7364013116217001, 0.8272230870215482, -0.0009292009292025535, 0.05222604297234434, -0.5594403822746647, -0.2594325755649186, -0.23811735213351445, 0.0009999999999999994, 0.15667485618125204, -0.013064312204933983, 0.001000000000000006, -0.019013995944725387, -0.11607743362969337, 0.0010000000000000154, 0.03959658038169399, 0.14919949324395768, 0.2619937667262847, -0.8313863585929929, -0.07993553156601112, -0.1998872847647498, 0.42897335247588414, -0.16882810172670576, -0.12540047992037187, 0.027059778212938444, -0.3210174529147508, 0.553169891528173, 0.9497706067736232, 1.2132643096085334, -0.16113185044225306, 0.11652337096998312, -0.023433537521428124, -0.2690783261074362, -0.11178083203936734, 0.2102073958970532, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2125467238631639, 0.6386779939567461, 0.7365846212213132, 0.828237910137404, 0.000655284604575382, 0.05480703737076679, -0.557689630052371, -0.26283798972823974, -0.24135171613220777, 0.0010000000000000104, 0.15393888307523865, -0.01603686877072039, 0.0010000000000000018, -0.02202455824199721, -0.1199173102251217, 0.0009999999999999955, 0.03873806867698209, 0.15282880731433096, 0.2612166965930834, -0.8353924514861683, -0.0825762021111135, -0.2044541333357091, 0.4337545695039057, -0.1571265617372522, -0.12495831035312255, 0.02738291899245031, -0.3198917995710695, 0.5492884878384128, 0.9472282762516486, 1.2290223571889232, -0.1625919359175677, 0.1072562034507656, -0.024454556610726253, -0.26841147376883834, -0.11299216006992212, 0.20253249639918605, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.213836605219866, 0.6365454222790132, 0.736393811613909, 0.8292336043533765, 0.0020967234346805814, 0.05723513187984261, -0.5559598662139599, -0.2660931811775194, -0.24430238078887653, 0.000999999999999999, 0.1512625716563769, -0.019003966003871272, 0.0009999999999999968, -0.02487346085240238, -0.1237436468524072, 0.0009999999999999825, 0.03776731689593597, 0.15617290677636014, 0.26137412448353886, -0.8379615921270789, -0.08511023381925394, -0.2090167615209641, 0.43903029947504635, -0.14748560248088005, -0.12457627361778484, 0.02763751513328394, -0.3194037216732343, 0.5451657326286543, 0.9439611911273673, 1.2420049369223092, -0.16415047774911512, 0.09980384851065735, -0.02549437503253537, -0.2682325669496915, -0.11445897928460831, 0.19482707046858222, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2151365802133613, 0.6345026874463879, 0.7360244785326101, 0.8302160735045164, 0.0034821527542607607, 0.05959479198019078, -0.5542360568167479, -0.2692869692772911, -0.2471325541062957, 0.0010000000000000037, 0.14858464782013361, -0.022004523249579803, 0.000999999999999998, -0.027642636967007424, -0.12756229654244347, 0.0010000000000000193, 0.03675012369491179, 0.1594045944868015, 0.2619687504249426, -0.8398690191256204, -0.08760482680491015, -0.21359679805764373, 0.4445168571910282, -0.1388237717892958, -0.12420585514352525, 0.02787860027092194, -0.3192180986530777, 0.5409130734135136, 0.940349419134713, 1.253642032421489, -0.1657601856311687, 0.09321260734586748, -0.026540103403696506, -0.2682367873621528, -0.11604845804196151, 0.18708752988393942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.2163953063566126, 0.6326170222138046, 0.735694958059418, 0.8311120527203728, 0.0048564524291806865, 0.061866254389194755, -0.5526316469951602, -0.2725278618912268, -0.25010281008723334, 0.0010000000000000189, 0.14592153807520558, -0.024986845328854753, 0.0009999999999999987, -0.030027559955915033, -0.13104579355843324, 0.0010000000000000028, 0.035910428280554914, 0.16264628826276525, 0.2627789683730397, -0.8407919180677144, -0.08957436271188775, -0.21738917558082207, 0.4496722680355233, -0.13176617437627874, -0.12401900811676131, 0.02812655154284081, -0.31942416948053437, 0.5374888961649368, 0.9371509335735311, 1.26367616153349, -0.1668600572061329, 0.08824143315484495, -0.02751255735859917, -0.26849605946557925, -0.11760358460939609, 0.1792398448299603, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21764195248799892, 0.6308076571422152, 0.7353828658826912, 0.8319618759955361, 0.006234857571294122, 0.06409944174004338, -0.5510824121759553, -0.2758044863113748, -0.253160832099018, 0.0010000000000000148, 0.14324217127720978, -0.027989806522362458, 0.0010000000000000018, -0.032222576991935016, -0.1343634658154229, 0.0009999999999999933, 0.035158964090672616, 0.1659166008354749, 0.2636842697576748, -0.8412456259622517, -0.09129129507080548, -0.22081222786712623, 0.45465122891627224, -0.12548553495384374, -0.12390947386472222, 0.02839103072415236, -0.319824077375581, 0.534456481772678, 0.9341533118040762, 1.2729144379615136, -0.16771445971225615, 0.0840623003210226, -0.0284479801991491, -0.26884991200522307, -0.11914335370089367, 0.17132567146903466, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.21887839559493294, 0.6291518057055644, 0.7352487788919464, 0.8327313550606041, 0.00759312099261215, 0.06630200538993587, -0.5496406816219087, -0.2790228279816035, -0.2563816208800374, 0.0009999999999999961, 0.14050705830120677, -0.031004516607638387, 0.0009999999999999994, -0.0342855920013789, -0.13743230343633653, 0.0010000000000000124, 0.03426065348126216, 0.1691602077623771, 0.26489978909918965, -0.8406464609073511, -0.09235595369811818, -0.22357783723162017, 0.4589360171877148, -0.12066893775151659, -0.12355825361496603, 0.02861025589159157, -0.320159105195846, 0.5318486220587599, 0.9317109997153404, 1.2807998111185452, -0.1679866535744723, 0.07992770147465408, -0.02934556607359464, -0.26907210924468883, -0.12034000123908355, 0.1632088150860557, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [0.22011748706515896, 0.6275703970407572, 0.7352027666863867, 0.8334574192300506, 0.008950594952701755, 0.06849352973950515, -0.5482492622559548, -0.28222453644863066, -0.25970215993986634, 0.0009999999999999948, 0.13772068300627832, -0.03405303639618982, 0.0010000000000000052, -0.03628020580460698, -0.140374789575737, 0.0009999999999999944, 0.0332926700154549, 0.1724119938509405, 0.2662562753503555, -0.8395356534015066, -0.0930998872331441, -0.22602990796339834, 0.46285856315840695, -0.11657478269043448, -0.12307436045460478, 0.028818416523663872, -0.32046093040893797, 0.5294488454372008, 0.9295461093496074, 1.2879992154938789, -0.16796861866620316, 0.07581148511418766, -0.030224849658937337, -0.2691792783869285, -0.12136535515376601, 0.1549712719589458, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.44317999482154846, 0.3182365596294403, 0.0366680808365345, 0.23186944425106049, 0.20954547822475433, -0.1814018040895462, -0.13917584717273712] - - [0.22106063160056594, 0.6260471392775484, 0.7351228198776739, 0.8341158523651925, 0.01035537218937828, 0.07067412516175828, -0.5469448593163, -0.2856520579616282, -0.2631646036471439, 0.0010000000000000005, 0.13518497685393877, -0.03686069863365323, 0.0009999999999999966, -0.038148193009237705, -0.14329616194745537, 0.000999999999999988, 0.032361939881080805, 0.17525310337877306, 0.2681091877356367, -0.8379655854486094, -0.09381942874417502, -0.22835088234062964, 0.4669555955050452, -0.11315119602981939, -0.12270553499126471, 0.029150410096909416, -0.32071222165280633, 0.527911230672461, 0.9269389523665598, 1.29455488622702, -0.16809127684055197, 0.07265927722934122, -0.03095657846526154, -0.2693254345141402, -0.12219278981438504, 0.14619112524218056, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015641260892152786, 0.45374879240989685, 0.7020354270935059, 0.027047906070947647, -0.045656319707632065, -0.20394200086593628, -0.24307167530059814] - - [0.22185832711684803, 0.6245524549985269, 0.7350237541504263, 0.8347359531178558, 0.011792071882578527, 0.07285541742058453, -0.5456820720580917, -0.2892086906223337, -0.26671507420444396, 0.0009999999999999883, 0.1327613165105301, -0.03956302897209926, 0.0010000000000000063, -0.0399501235816861, -0.1462059888669721, 0.0009999999999999966, 0.031452056763915964, 0.17789130425457325, 0.2702041139516095, -0.8361776718892526, -0.0945305176897876, -0.23061076309731626, 0.4711311449026522, -0.11005374449154678, -0.12238705231562273, 0.02955549138393998, -0.32093752800486025, 0.5268067591942526, 0.9241045964173762, 1.3007807268724902, -0.1682874632153916, 0.06999615008284635, -0.03161198960498227, -0.26945294511423373, -0.12291979067999807, 0.13711198754474802, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2969687581062317, 0.2321910262107849, 0.0799890086054802, 0.011026418767869473, 0.03245752677321434, -0.2803012430667877, -0.21059319376945496] - - [0.22271719363993855, 0.6230137590475967, 0.7350650674770429, 0.8353112130624184, 0.013272798446683469, 0.07516187699313868, -0.544453581492905, -0.2928752186650082, -0.2704635780918621, 0.0009999999999999994, 0.1302146495118795, -0.042393028788175954, 0.000999999999999995, -0.041723446448639304, -0.14919847562393096, 0.0010000000000000289, 0.030564189876358738, 0.18058516298033336, 0.2719658105804438, -0.8338422775977737, -0.09460218591287126, -0.2328002024211337, 0.47487182245420584, -0.10765879981872124, -0.12216248201678032, 0.03013597747031939, -0.32043328718117825, 0.5262131804473724, 0.9220933505126305, 1.3069567954330634, -0.1679684586089834, 0.06707288971311759, -0.032268488076468586, -0.2688630859565845, -0.12344982656486359, 0.1276942060082415, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5068114995956421, 0.0870138481259346, -0.11762022972106934, 0.00906062126159668, 0.029213707894086838, -0.3155962824821472, -0.1444578319787979] - - [0.22361878256731, 0.6214486333560582, 0.7351767970591019, 0.8358588809957646, 0.014788703605026389, 0.07754250938850654, -0.5432387914572416, -0.29661892079255203, -0.27434957907549623, 0.0009999999999999768, 0.12757551935460676, -0.04532387127037273, 0.0009999999999999918, -0.043478909841147155, -0.15223144651524675, 0.0010000000000000024, 0.02969277622612932, 0.18334663173410135, 0.273527749039284, -0.8312358070360237, -0.0943472238433282, -0.23497812426143855, 0.47837333833143747, -0.10560941328903989, -0.121973419582127, 0.030826583999258982, -0.31954096573669016, 0.5258843023141372, 0.9205149417985036, 1.313109761228276, -0.16737833330656715, 0.06402046135737617, -0.03292593558078803, -0.26784748600518776, -0.12387631649111332, 0.11806459369323227, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11406882107257843, 0.05570409446954727, -0.06862004101276398, 0.052874136716127396, 0.05513794347643852, -0.347802996635437, -0.17412054538726807] - - [0.2243656377087936, 0.6198610109317527, 0.7350414211091505, 0.8363796185310339, 0.016300668656721532, 0.07981823501191802, -0.5420631616950272, -0.30013962736593197, -0.2779638062766914, 0.0010000000000000044, 0.12491582068609634, -0.04826791478548613, 0.0010000000000000115, -0.04513341521671362, -0.15509726756632744, 0.0009999999999999803, 0.028776860213912344, 0.18578914187646048, 0.27529714642963726, -0.8297822654979237, -0.09434022701732993, -0.23732536781105934, 0.48193808004602734, -0.10327672274603303, -0.12155995340368039, 0.031420452105570845, -0.31837404918065276, 0.5251672360689295, 0.9185553619247875, 1.3206002652185473, -0.16666098615702074, 0.061425420527571495, -0.033583058733187496, -0.2651853840866323, -0.12452449743301218, 0.1092201379063625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4270801842212677, 0.032583773136138916, -0.08298105001449585, 0.19581831991672516, 0.01512994710355997, -0.17289260029792786, -0.17895960807800293] - - [0.2250406722536846, 0.6182576590754647, 0.7347702981620532, 0.8368831482393543, 0.017822735564357733, 0.08204684170634263, -0.5409041153993984, -0.3035604209389176, -0.2814561373078444, 0.0010000000000000115, 0.12221602184658356, -0.051250465870243136, 0.0009999999999999994, -0.046732763206860754, -0.15787337139848093, 0.0010000000000000096, 0.027840769697340145, 0.18807588006477316, 0.27715598005114966, -0.8289678059762869, -0.09447624958659855, -0.23977820380061476, 0.4855268669287313, -0.10077233522788916, -0.121009627881038, 0.03198089012539352, -0.31705980992932126, 0.524214909031536, 0.9163900798419128, 1.3287983348788106, -0.16587623472744498, 0.05910514085533354, -0.03424024883043768, -0.26159489279151593, -0.12529270552029137, 0.10078195167801292, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.22153756022453308, 0.4472527503967285, -0.04789876937866211, 0.006490321829915047, 0.29276320338249207, -0.29508787393569946, -0.29259562492370605] - - [0.22536729449310092, 0.6168693831953493, 0.7344432482147848, 0.8372047177116207, 0.019189126304286562, 0.0839541940220447, -0.5400664138596225, -0.30658760650458067, -0.2846956032923626, 0.001000000000000013, 0.11988166574727285, -0.05382358090991564, 0.0010000000000000143, -0.04791723459353995, -0.1600493357556147, 0.001000000000000009, 0.02722225498530091, 0.19007018282224789, 0.27890911138951063, -0.827225155203914, -0.09418920078728482, -0.24107848297402856, 0.48849857495726334, -0.09902348785165047, -0.12035390593832311, 0.032492380388219874, -0.31599580366281105, 0.5236689839282945, 0.9147937786092388, 1.3339949423990363, -0.1649414416563628, 0.05743987977475655, -0.03475427175237663, -0.26004030762270786, -0.1255698621505921, 0.09335192133876054, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4949386715888977, 0.49358177185058594, -0.10180144011974335, 0.3329377770423889, 0.5584545731544495, -0.12355132400989532, 0.021222764626145363] - - [0.22549985775473266, 0.6156009966490632, 0.7340849715862455, 0.8374247348322026, 0.02046922629603475, 0.08568368767230049, -0.5394062753926137, -0.3093991912948266, -0.2878013689509354, 0.001000000000000003, 0.11775065683202135, -0.05616937541823016, 0.0010000000000000018, -0.04887044675089735, -0.1618909573652199, 0.0010000000000000143, 0.02677855599934968, 0.19190879282371984, 0.28060475512053384, -0.8249673669243907, -0.09366529397581097, -0.2417420168299475, 0.4911293040416467, -0.0976991625304856, -0.11964064162044989, 0.03297444571182779, -0.3150684666547067, 0.5233559882984611, 0.9135119452216546, 1.3375314020747497, -0.16392312006994714, 0.05612259218938545, -0.03518903318834209, -0.259607467928535, -0.12557544731574713, 0.08648068684805005, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13531799614429474, 0.12883351743221283, 0.020396830514073372, 0.035042665898799896, 0.0, -0.1429031491279602, 0.0] - - [0.2253179983303412, 0.6144910896330793, 0.7333740737811543, 0.837527985155037, 0.02145513319748245, 0.08706629554054313, -0.5389860958526378, -0.3115260741362204, -0.29008313947043396, 0.00100000000000001, 0.1162301648852215, -0.05790330453022671, 0.0009999999999999994, -0.04943320888827879, -0.16342508316323992, 0.0010000000000000035, 0.026374419197419084, 0.1929891784877655, 0.2821997139704455, -0.8233376066447577, -0.0931693128255858, -0.24227363672247734, 0.493702585153991, -0.09630940845484337, -0.11913152060017375, 0.033287111480249, -0.31468009471629543, 0.5232388891422028, 0.9123103440725471, 1.339292495873405, -0.1633002929866277, 0.05607304385342981, -0.035331126660431124, -0.25981924135239104, -0.1256312597541463, 0.0812167921611829, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11978581547737122, 0.4101407825946808, -0.04309163987636566, 0.02792394533753395, 0.0333845429122448, -0.211534321308136, -0.4446060061454773] - - [0.22495685582975475, 0.6134710727368374, 0.7324641512565201, 0.8375653870578685, 0.02227264301020346, 0.0882515558699999, -0.5387019720239268, -0.3132625731201223, -0.29189217151224117, 0.0010000000000000052, 0.11505916636027368, -0.059284807781489604, 0.0010000000000000057, -0.049775314884246025, -0.16478676779583887, 0.000999999999999982, 0.025992487163601502, 0.19363163204866696, 0.283742058384475, -0.8220643548307157, -0.0926887884031834, -0.2427260752459145, 0.49624357807096847, -0.09488344827344396, -0.1187396499891534, 0.0335018274303939, -0.31459563848455285, 0.523234810131217, 0.9111541728023319, 1.340047781430585, -0.1629014772182803, 0.056733465507163724, -0.035307417284090606, -0.2604013337301925, -0.1257157456118184, 0.07686784765307486, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.14860768616199493, 0.11546307802200317, -0.4126807749271393, 0.0472538061439991, 0.0, -0.47667229175567627, -0.08503668755292892] - - [0.2244912858114404, 0.6126057063253981, 0.7313821546097073, 0.8374780256300591, 0.022897565406184484, 0.08917104726358084, -0.5386601734072821, -0.31448404277604164, -0.293211947842108, 0.0010000000000000044, 0.11420850983901742, -0.06034545183303246, 0.0009999999999999894, -0.04968017253302825, -0.1657812906158506, 0.0009999999999999968, 0.02567302129781942, 0.1938173721613923, 0.28526049368302026, -0.8210032371802461, -0.09201535080617035, -0.24269381208696864, 0.4982172708040792, -0.0935191746079405, -0.11838877579797452, 0.033755782401044, -0.3144372899221966, 0.5230454469103497, 0.9101400602736852, 1.3391874431845034, -0.1624596541349819, 0.058062859817749043, -0.03525213909876698, -0.26158960191544384, -0.1257960942187072, 0.07400732026627434, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.38155078887939453, 0.5811251401901245, -0.015938900411128998, 0.4061576724052429, 0.7378761172294617, -0.1826898157596588, -0.16060273349285126] - - [0.22396473540551637, 0.6118299840628398, 0.730200831738107, 0.8373188716711345, 0.023410670121296266, 0.08993580066691922, -0.538758386873234, -0.315405195917091, -0.29424398862178475, 0.0010000000000000009, 0.11354314883872334, -0.061219020184934005, 0.0009999999999999963, -0.04933118499297571, -0.16656357522254153, 0.0009999999999999998, 0.025389491121125516, 0.19373249923518662, 0.2867646644946486, -0.8200650965874766, -0.09122978935497005, -0.24237762010569258, 0.499862274996437, -0.09219006948178077, -0.11806168494952325, 0.03403200213608812, -0.3142362660284127, 0.5227514080862141, 0.9092084159835921, 1.3373890805864843, -0.16199268046829227, 0.059772079564224645, -0.03517853107795456, -0.26312677455291333, -0.12587385694646544, 0.0720091638635141, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4149182140827179, 0.4638213515281677, -0.009657265618443489, 0.63747239112854, 0.5448353886604309, -0.11947324126958847, -0.254296213388443] - - [0.2234533566065829, 0.6110844437217692, 0.7291056272803496, 0.8370827629631136, 0.02387597552904159, 0.09055562457612108, -0.5390009875688587, -0.3160535814725547, -0.29511548078776617, 0.0010000000000000182, 0.11299622347202364, -0.061946142387794445, 0.001000000000000001, -0.048790672787239295, -0.16704110121012275, 0.0010000000000000198, 0.0253278277383512, 0.19367881186647773, 0.2877535982182335, -0.819731894517773, -0.0906702570648928, -0.2418390283597319, 0.5009554857350058, -0.09063729895156698, -0.11751755855755276, 0.034107727823209574, -0.31395920127483057, 0.5236545088966386, 0.9086177443292882, 1.334723350393099, -0.16149454090933887, 0.06141364151977086, -0.03505568344471377, -0.2642138512758949, -0.12579074587726133, 0.07107145136713447, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.48455721139907837, 0.8098799586296082, 0.12209618836641312, 0.580166757106781, 0.6528713703155518, -0.14287050068378448, -0.06669650971889496] - - [0.2229518820572289, 0.6103566156865199, 0.7280616930104069, 0.8368012236605048, 0.02431389815547912, 0.09109004598739791, -0.5393284249497531, -0.3165403534947822, -0.29589088800789165, 0.0009999999999999953, 0.11251735860258048, -0.06258897423863913, 0.001000000000000006, -0.04813667012219482, -0.1673382937377852, 0.0009999999999999725, 0.02539830318947174, 0.19364234403703218, 0.28843571661458295, -0.819756103219788, -0.09024610536170523, -0.24116818207095367, 0.5017200219567902, -0.0889508673289562, -0.11684296162646494, 0.034065848346211736, -0.3136375714297159, 0.5252727244373497, 0.9082297496056604, 1.3315453887027795, -0.16097789612522162, 0.06301506253584287, -0.034903311108329636, -0.26502981501524836, -0.12561198658961276, 0.07076159951379145, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46120041608810425, 0.46544772386550903, 0.05338919907808304, 0.48362088203430176, 0.4387131631374359, -0.07902846485376358, -0.20560088753700256] - - [0.2226667242470824, 0.6096848667507775, 0.7272112392617671, 0.8363812373490017, 0.02480906120482892, 0.0915536231422238, -0.5398785700342438, -0.31680314621782607, -0.29677120718622074, 0.00099999999999999, 0.111923293897921, -0.06333044281498203, 0.0009999999999999985, -0.04696545070948354, -0.16705229850491068, 0.0010000000000000135, 0.025556509914786724, 0.1936892801806461, 0.28882598613027693, -0.8205063694947973, -0.0900883930594438, -0.24034833198022001, 0.5019128680798961, -0.08695300745378215, -0.11570759062134492, 0.03423565710674943, -0.3133638814321503, 0.5261109227947017, 0.9080326911496742, 1.3285199760555335, -0.16097359692155291, 0.06413441201238419, -0.03460234228358975, -0.2652605847649939, -0.12483685263949768, 0.07019829357436483, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4814380705356598, 0.6903944611549377, 0.06837189942598343, 0.6457006931304932, 0.47940388321876526, -0.0807262510061264, -0.0028687429148703814] - - [0.22251496671559748, 0.6090477043519166, 0.7264786204203778, 0.8358768828240415, 0.025341297445855497, 0.09197515682543303, -0.5405628787949289, -0.3169291483867157, -0.2977126693091188, 0.00100000000000002, 0.11125500045157045, -0.06413624339662348, 0.0009999999999999968, -0.04548003753065572, -0.16641132324638036, 0.0010000000000000206, 0.025768542590732812, 0.19378209151815592, 0.289037378068898, -0.8217016413990533, -0.09009649487179608, -0.23943627525807443, 0.5017564708271272, -0.08476540734497785, -0.11428899590221649, 0.03453773912770184, -0.3131213724764041, 0.5264755343790755, 0.9079518586264277, 1.3255933672593483, -0.16128203501481733, 0.06495774222711145, -0.034211181632553364, -0.2651326893787329, -0.12369850910280562, 0.06948085969531567, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46498337388038635, 0.5595044493675232, 0.1459413319826126, 0.37785807251930237, 0.7390245795249939, -0.09862460941076279, -0.19026923179626465] - - [0.22246930467033418, 0.6084973160943814, 0.7258238133855877, 0.8352476409123711, 0.0258414571125131, 0.09230094079308215, -0.5414555695284076, -0.3168745966856469, -0.2985954216138159, 0.0010000000000000033, 0.11074863580143422, -0.06479718324726802, 0.0010000000000000072, -0.043686164889314934, -0.1654460742725074, 0.0009999999999999926, 0.026128800247084655, 0.1937663142318075, 0.2892801894499926, -0.823299105339426, -0.09056895563436784, -0.23868396291064317, 0.5014110803049372, -0.0828738594897955, -0.11268304019109981, 0.03480513106830768, -0.3127384084496434, 0.5275134853113663, 0.9076866310880508, 1.3228544159733406, -0.16150119294272228, 0.06623853253442122, -0.03419883094187919, -0.2646908812048907, -0.12248814756884398, 0.06980486602227068, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5243017673492432, 0.5329734086990356, -0.12350758165121078, 0.41920971870422363, 0.571799099445343, -0.12007209658622742, -0.17542436718940735] - - [0.22249054956865225, 0.6080014248183502, 0.7252179051498145, 0.8345403507464575, 0.026325188863240836, 0.09256818577300792, -0.5424762837120056, -0.3167069188212672, -0.29943774097952564, 0.0010000000000000013, 0.11033849737592762, -0.06537266407632014, 0.0009999999999999957, -0.04170197543475334, -0.16427899592946127, 0.0009999999999999742, 0.026583117492369072, 0.19367754314904168, 0.2895429049024099, -0.8251441568695943, -0.09133549981455556, -0.2380303803283844, 0.500943714999067, -0.08116638023224376, -0.11095934788318225, 0.035052436293918784, -0.3122701390575967, 0.5289741571585357, 0.9073063076920925, 1.3202336181242196, -0.16166405957144064, 0.06780490027918211, -0.034422558207750226, -0.26404972706612817, -0.12123270223582389, 0.0707738095742551, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5052318572998047, 0.6215460896492004, -0.22236479818820953, 0.38139891624450684, 0.8451887965202332, -0.15141116082668304, -0.10900069028139114] - - [0.2226420368510856, 0.607642819702811, 0.7249668032039064, 0.8336863381555423, 0.026798405665220598, 0.09272384142686704, -0.5437381945915253, -0.3163545250490734, -0.30034234369649804, 0.0010000000000000141, 0.1100079217055801, -0.06588593410676206, 0.0010000000000000091, -0.03913648760390501, -0.16268141410162956, 0.0009999999999999898, 0.027247810671145244, 0.1935814753946322, 0.28935204903893214, -0.8263212328996349, -0.09225824510749248, -0.2367093732454488, 0.4997887654572077, -0.0806254295609812, -0.10904217902532477, 0.035541565912839364, -0.3120973649630771, 0.5309345475645916, 0.9074440421481283, 1.3160683728275486, -0.1619087341946024, 0.0693843509479354, -0.03436403087882229, -0.2636995810214164, -0.119279333993367, 0.0727287637241591, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4361809194087982, 0.47083941102027893, -0.009173264726996422, 0.4578735828399658, 0.3517833650112152, -0.10645413398742676, -0.2753402292728424] - - [0.222878441587141, 0.6073726807216887, 0.7249435968942506, 0.832738198178694, 0.02726939822362829, 0.09280863852699324, -0.5451513824887456, -0.31588099404254816, -0.3012772721533023, 0.0009999999999999976, 0.10971992309260625, -0.06636713394794956, 0.0010000000000000122, -0.03620238455328779, -0.16080943124813618, 0.0009999999999999918, 0.02804994112801405, 0.19347022307124484, 0.2888712657059529, -0.8270737935365855, -0.09328763266686078, -0.2349596424852711, 0.4981893452848779, -0.08082754211445999, -0.10699868251480685, 0.03618935923703046, -0.312118481778507, 0.5332158756627456, 0.9079125369751785, 1.3109218050724671, -0.16220727179886116, 0.07097363820767709, -0.034125078875961314, -0.2635283069663375, -0.11688042047171975, 0.07530886747329124, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5689274072647095, 0.5158655047416687, -0.029728008434176445, 0.7138344049453735, 0.4468128979206085, -0.09716890007257462, -0.18339762091636658] - - [0.22323772911747972, 0.6072597719128673, 0.7251200728922139, 0.8315816822841611, 0.027732706333081395, 0.09278166185828654, -0.5468952056031414, -0.3152348775966425, -0.3024804609243874, 0.0009999999999999953, 0.1095503298869134, -0.06673354523673634, 0.0009999999999999922, -0.03248503354648964, -0.1580742435982786, 0.0010000000000000167, 0.029051585590187088, 0.19352760022882953, 0.2879674878777867, -0.8282357745340101, -0.0947517416867625, -0.2329013153296269, 0.49612601740978984, -0.08191012412102094, -0.10453758063319947, 0.03656765118199068, -0.3128027220473349, 0.535177353671145, 0.9086500191163936, 1.3045082265150747, -0.16251905076194778, 0.07459081499023264, -0.03401003273062337, -0.26323245653714666, -0.1140647723438873, 0.07805859618599109, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3799096643924713, 0.46251943707466125, -0.46385127305984497, 0.4045693874359131, 0.2911392152309418, -0.2918880581855774, -0.18554256856441498] - - [0.22368505732800448, 0.6072478924559757, 0.7254284485418243, 0.8302871419903753, 0.02819733733650842, 0.09268465335667318, -0.5488512795327519, -0.31446953488214335, -0.3038396771205611, 0.0010000000000000028, 0.10944609436320128, -0.06703534921097495, 0.0010000000000000009, -0.02826119101361387, -0.15477605020467086, 0.000999999999999992, 0.03019135025700549, 0.19367579883692593, 0.2867871521989901, -0.8296815310883545, -0.09651881764111102, -0.23063969098124765, 0.49375494197397723, -0.08355525689067386, -0.10179564806228092, 0.036782976421817694, -0.31392275045956125, 0.5369298847332702, 0.9095630357947101, 1.2972805691785403, -0.16284038682724852, 0.07952237812977986, -0.033975290703213354, -0.26282216002857905, -0.11097691233029065, 0.08090231653835533, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4137186110019684, 0.4246889352798462, 0.14514727890491486, 0.4272441565990448, 0.6340752243995667, -0.1725335419178009, -0.17544367909431458] - - [0.22404289643228384, 0.6074537437871804, 0.7259021846178366, 0.8287835111079883, 0.02852978838976877, 0.09236018860828799, -0.5511565462281944, -0.31339651327186946, -0.3052341741081154, 0.0010000000000000083, 0.10976628239745091, -0.06689535414041439, 0.0010000000000000024, -0.023325803532406823, -0.15072413009929017, 0.0009999999999999848, 0.03152910978768397, 0.19381841987140236, 0.28548475393586953, -0.8301071935869814, -0.09830688733707685, -0.22795527589783202, 0.49100866451246156, -0.08595218084374279, -0.0989284119235169, 0.037110380658220476, -0.3157564294523135, 0.5391204135479809, 0.9106115191688442, 1.2877349273726162, -0.16352075261463064, 0.08510257193870203, -0.0339924718506045, -0.26370306402567595, -0.10755929019932467, 0.08394681049930888, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46440568566322327, 0.5203483700752258, 0.004747138824313879, 0.5412392020225525, 0.6212663650512695, -0.18633414804935455, -0.09978023916482925] - - [0.22435396569287913, 0.6078027062237716, 0.7264887630379877, 0.8271367645850313, 0.028785084809626788, 0.09189125856827991, -0.5536896135589395, -0.312112170265492, -0.30662923405187803, 0.0009999999999999818, 0.11035065847340296, -0.0664795076668713, 0.0009999999999999966, -0.01792000423683128, -0.14617113402579077, 0.0009999999999999968, 0.033008913713612, 0.1939335948540673, 0.28409310999599086, -0.8298572111755192, -0.1001346180770267, -0.22499054367569502, 0.4880017682507157, -0.08882625349566511, -0.0959627767729405, 0.03753420704238384, -0.31807327566111765, 0.5415922376591042, 0.9117500658639616, 1.2766364198825082, -0.16444269725399677, 0.09110813641252934, -0.03404579888761628, -0.26540779550809096, -0.10392147746953942, 0.08709701160949279, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4699329137802124, 0.5838613510131836, -0.006981401238590479, 0.44777774810791016, 0.3369303345680237, -0.17061325907707214, -0.15956591069698334] - - [0.22453937636972038, 0.6083243369120577, 0.7273615743138717, 0.8253481543800364, 0.028817693741774904, 0.09119786592659576, -0.5564646564152196, -0.3103171670049651, -0.3077822004053161, 0.0009999999999999887, 0.11142695509962625, -0.06548687722943447, 0.0010000000000000098, -0.012122145143952046, -0.14118768612008323, 0.0010000000000000009, 0.03453036861396098, 0.19403601220283972, 0.2825209072575648, -0.8276520642030768, -0.10159354125565778, -0.22147660926461865, 0.4843963928089244, -0.0922179605346699, -0.09306340148046334, 0.037652530188478354, -0.32039157596299794, 0.5450092794728039, 0.9136206312742001, 1.2632071630113981, -0.16593459018972287, 0.0982306004620408, -0.03386078492590337, -0.2674849668449774, -0.09986266699682886, 0.09137073101556009, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4470249116420746, 0.36737069487571716, -0.02712886966764927, 0.7421159744262695, 0.7081635594367981, -0.2732573449611664, -0.10461148619651794] - - [0.22465693701682946, 0.6089607734302197, 0.7284325290812661, 0.8234578437741115, 0.028712231024500504, 0.09036264106184556, -0.5593991244333648, -0.3081709803334635, -0.30875193781644517, 0.0010000000000000041, 0.11281393641084843, -0.06412922831219872, 0.000999999999999997, -0.0060587483523725566, -0.13591314074106314, 0.0010000000000000041, 0.03608769718169521, 0.1941098174317726, 0.2808176305899522, -0.8240996092684593, -0.10282658637097301, -0.21759146669581853, 0.4803625184345473, -0.09594999928972178, -0.09018906225125617, 0.03759124986230954, -0.3227094042777418, 0.5490445778030355, 0.9159926544047465, 1.248127838287038, -0.16782188289967367, 0.10612108293175829, -0.033517831111874015, -0.2697617859988913, -0.09551617822755126, 0.096375284098969, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.10824940353631973, 0.40894266963005066, -0.10374201834201813, 0.5201700925827026, -0.009407657198607922, -0.452745646238327, -0.3196319341659546] - - [0.22451719256691705, 0.6097399971387304, 0.7294607336573906, 0.8214571045177654, 0.02843890099004887, 0.08919629176827602, -0.5625330887001732, -0.3054472978269994, -0.3092723314725733, 0.0010000000000000035, 0.11452848056882058, -0.06236725088907082, 0.0009999999999999946, 0.0002976682681149236, -0.13021716011965825, 0.0010000000000000187, 0.03769472523690146, 0.19395280143449015, 0.279188107514294, -0.817677216717726, -0.10411233133892335, -0.21325320788566535, 0.4756650208738681, -0.10128825858944207, -0.08703795292893078, 0.03764193860592495, -0.3248578829876653, 0.555348894821046, 0.9188264181426083, 1.2287230839285899, -0.16949462595892675, 0.11544788781896259, -0.03318309767503563, -0.2729854711081209, -0.09079221610559497, 0.10335685828643079, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5150296092033386, 0.6253513693809509, -0.2695063054561615, 0.36105796694755554, 0.6598750352859497, -0.1794372946023941, -0.09744507074356079] - - [0.2242281666231239, 0.610608828232382, 0.7304610667735936, 0.8193748429561519, 0.028069277363271496, 0.08781609588823397, -0.5657961785853927, -0.3023257961883465, -0.30946108631458874, 0.0009999999999999994, 0.11642001624879562, -0.060377797346116605, 0.0009999999999999953, 0.006851395841519304, -0.12423225880992916, 0.001000000000000017, 0.039348604341532, 0.19360875181628157, 0.2775837931650164, -0.8092122804783374, -0.10547956773979304, -0.20861251074442638, 0.47047547254851946, -0.10778727121819665, -0.08365759466766859, 0.037821235644589994, -0.3268863023512704, 0.5631675817048245, 0.9219832122253053, 1.2061281983784093, -0.17102199149357827, 0.12582999655652904, -0.03285487464749133, -0.2767817598528345, -0.08580478463679077, 0.11168320642252236, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40284815430641174, 0.543919563293457, -0.07159546762704849, 0.5789600014686584, 0.5007007122039795, -0.1694386750459671, -0.24349939823150635] - - [0.22372476822664197, 0.6116517302828739, 0.7312539042961853, 0.8171475961677882, 0.027554569044772787, 0.08616706481041064, -0.5692853315727715, -0.2988056967991032, -0.3094017590206324, 0.0010000000000000135, 0.11855896054672571, -0.058076408745052883, 0.000999999999999999, 0.013874604131665684, -0.11787065100840313, 0.0009999999999999942, 0.04112836003246941, 0.19304001199368223, 0.27652137819128975, -0.7990593851842723, -0.10704140579196571, -0.20339313665860353, 0.4648231821962852, -0.11535094545308341, -0.08032809056091576, 0.03819404240403759, -0.32940871340179934, 0.5710374177911676, 0.9250838050513988, 1.182013300223315, -0.17320995369256584, 0.13792598822761568, -0.03281443252050146, -0.2814113715756965, -0.0803258938900287, 0.12067882223807806, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4820256531238556, 0.6963319778442383, -0.3346819281578064, 0.5179240107536316, 0.45890283584594727, -0.12358913570642471, -0.17460885643959045] - - [0.22310739386879633, 0.6128026121376141, 0.7318997779836504, 0.8148104558019263, 0.02696194908636719, 0.08434691477914244, -0.572924578268869, -0.2950018971681864, -0.3091392493778079, 0.0010000000000000078, 0.12081326229476903, -0.055620092142481876, 0.0009999999999999987, 0.021227465489305, -0.11124311909321603, 0.0010000000000000054, 0.04301840475349004, 0.19228776847692708, 0.27581263657572014, -0.7876490897793416, -0.10880504837267103, -0.19778990556150366, 0.45881575441045475, -0.12368695561405191, -0.07699275845824195, 0.03876722999011762, -0.3322923137924999, 0.5788651986382385, 0.928142250210827, 1.1566276078935618, -0.17587386653944911, 0.15125951980949756, -0.03298575595956485, -0.28653681085995664, -0.07449744150502555, 0.13006578841618358, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4091947376728058, 0.752021849155426, -0.2806150019168854, 0.6175856590270996, 0.4376969635486603, -0.11668689548969269, -0.36852607131004333] - - [0.22246127569153137, 0.6140301055279528, 0.7326504301111032, 0.8123811186415301, 0.026276720121311426, 0.0823996170876032, -0.5766773405971636, -0.2908908859451629, -0.30874732801158467, 0.0010000000000000035, 0.12301070536494907, -0.05307069524369212, 0.0009999999999999992, 0.028838314514325294, -0.10458553844513412, 0.0010000000000000057, 0.04504160304910349, 0.1916843873652412, 0.27516906986621387, -0.7744857847474114, -0.11013820513914627, -0.1919821449836897, 0.4521686307742726, -0.13355774512686164, -0.07357142803251399, 0.03976940745103347, -0.33473706041402695, 0.5871672515914017, 0.9316741251729008, 1.1300540589598091, -0.1781020026593066, 0.1654848454423362, -0.033188793159401396, -0.2917754655534969, -0.0681875801892233, 0.13945665901606577, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4221351146697998, 0.5134783983230591, -0.09474010765552521, 0.38676926493644714, 0.5300495028495789, -0.19312722980976105, -0.17674721777439117] - - [0.22178967394961038, 0.6152365711223396, 0.7334692659680138, 0.8098050354383814, 0.025645816078576612, 0.08036396995004098, -0.5806029013277273, -0.2864441123142046, -0.3088138196605693, 0.000999999999999992, 0.12510507989478617, -0.050563610387975445, -0.0009999999999999998, 0.036946200355578086, -0.0975990429431791, 0.000999999999999991, 0.04702403132348591, 0.1921703213026727, 0.27464633427518625, -0.7596193891994142, -0.11121016552320828, -0.18661796366804098, 0.4466909468751708, -0.14402500631017712, -0.07012001200690797, 0.04107838617509313, -0.3361184475128034, 0.5947449504945469, 0.935552877732289, 1.1020990255541325, -0.18001362848789662, 0.18033247518664755, -0.03341283054980348, -0.296842503124196, -0.06152607241829706, 0.14885605086476875, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5087016820907593, 0.6663009524345398, 0.03587309271097183, 0.6826770901679993, 0.38113078474998474, 0.0819728747010231, -0.22638611495494843] - - [0.2209783882317561, 0.6164550336486798, 0.7342112545736295, 0.8072108288897523, 0.024929107366790902, 0.0782652363822153, -0.5845201195023564, -0.28197937001755197, -0.30820652148962774, 0.0010000000000000028, 0.1272617262583313, -0.04776263933332352, -0.000999999999999999, 0.04498219721694331, -0.09072137891329377, 0.0009999999999999955, 0.04935725501557115, 0.19162268726418266, 0.2739602736843366, -0.744971470182014, -0.11220657854354191, -0.1808675495697338, 0.43951715127509444, -0.1561847251290268, -0.06638296513099502, 0.04298572831234249, -0.3385371517278248, 0.6038717162400896, 0.9394497311523182, 1.073639675387836, -0.18293519495905353, 0.19508420862651127, -0.03381247215054433, -0.3023316465277971, -0.05468141484726862, 0.1574289620979467, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2592923939228058, 0.49204087257385254, -0.4393715560436249, 0.42359328269958496, 0.37411487102508545, -0.2580447494983673, -0.4044051766395569] - - [0.22006691235493195, 0.6176128527456579, 0.7348920612530471, 0.8045407253814097, 0.024242525537041055, 0.07612763316766792, -0.5884990268697383, -0.27741426719138695, -0.30750601691310026, 0.0009999999999999894, 0.1294166248178344, -0.044832843113059996, -0.0010000000000000013, 0.05320003488031471, -0.08368601085444241, 0.0009999999999999844, 0.05187415619407168, 0.19106092260246219, 0.27321030176356065, -0.7301715066052947, -0.11322189783726903, -0.17530912963566697, 0.4322983367851532, -0.16923418379942687, -0.062448200821748026, 0.04535039436437097, -0.3411668069309332, 0.613338035114146, 0.943359134293695, 1.0444062415828368, -0.18661799832087378, 0.20972573484329526, -0.03435761471013949, -0.30794691401264257, -0.04770285950846875, 0.16534318411884677, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4079096019268036, 0.28512030839920044, 0.14320553839206696, 0.4054725468158722, 0.35675135254859924, -0.2538098990917206, -0.15494422614574432] - - [0.21920053320428445, 0.6187095153361544, 0.7353986293873288, 0.8017782503964788, 0.02357725860156286, 0.07397106305755932, -0.5925571971532634, -0.2726644221513676, -0.3068139217630901, 0.000999999999999998, 0.13129661838847875, -0.04211107438871287, -0.0010000000000000035, 0.0616116734962725, -0.07669403718240608, 0.000999999999999984, 0.05443375596214136, 0.19088831502375314, 0.2727125387380753, -0.7157735846137704, -0.11435419885042437, -0.17020122750423738, 0.425312373662739, -0.18361942511216872, -0.058495753760769834, 0.048470807777725104, -0.34366316124203483, 0.6219497269048634, 0.9465470560797185, 1.0150821249675142, -0.19015612121904038, 0.2251083297406114, -0.035103640689185575, -0.31309217579703197, -0.04094713801253234, 0.17267056684022275, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.18900077044963837, 0.34942781925201416, -0.273305743932724, 0.3934656083583832, 0.29941096901893616, -0.23621059954166412, -0.1791064590215683] - - [0.21840332445807625, 0.6197423155545865, 0.7357772539030564, 0.7989341238818637, 0.022933696048596585, 0.0718203195116599, -0.5966742436102902, -0.26776592716677855, -0.3060919455185269, 0.0009999999999999987, 0.13295357258693716, -0.039562982232951474, -0.0010000000000000002, 0.07017819396103897, -0.06972840590530198, 0.001000000000000009, 0.057055797792921754, 0.19098007912069534, 0.2724042995847775, -0.7016877901401138, -0.11564104254187034, -0.16546713225629225, 0.4185095921765013, -0.1990155899510625, -0.05450061679319165, 0.052220475861603236, -0.34603960300435127, 0.629806185758438, 0.9491791387803659, 0.9856236001447868, -0.19358022589497165, 0.2410056977413546, -0.036007557585070994, -0.3177719853894114, -0.0343636261070985, 0.17945234043941782, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4740794599056244, 0.7743831276893616, -0.4287719428539276, 0.32947492599487305, 0.6880061626434326, -0.22365984320640564, -0.15167562663555145] - - [0.21762932794659454, 0.6206769958061188, 0.7361941446424941, 0.7960384040380961, 0.022191169845079947, 0.06978737414818424, -0.600799578634081, -0.2628630488506548, -0.30537507010308296, 0.0010000000000000002, 0.13470412703350382, -0.037219871458034896, -0.0010000000000000005, 0.07849484554635301, -0.06396835548624553, 0.0009999999999999957, 0.05981387949510613, 0.19181806402832446, 0.272176665507159, -0.6880052674510869, -0.11653171869839533, -0.1613501257038635, 0.41239209354872264, -0.21472774996632307, -0.05081593576798651, 0.05633493976224345, -0.3482495101332574, 0.6356811323121471, 0.9511618582123685, 0.9574193430864006, -0.19691278413040206, 0.25560075105711305, -0.03714880469275045, -0.32150244413553264, -0.028188340949597965, 0.1844948468000547, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4387381672859192, 0.6486315727233887, -0.16383063793182373, 0.35874706506729126, 0.319232702255249, -0.16828736662864685, -0.13027994334697723] - - [0.21688986348810724, 0.6215241632307571, 0.7366475260543667, 0.7930910173671992, 0.0213720988737896, 0.06787130602919426, -0.6049325229966407, -0.25796130899999037, -0.30465649928442273, 0.0009999999999999957, 0.13655286562826524, -0.035011827048157584, -0.0009999999999999929, 0.0866238545246492, -0.05915441122385539, 0.00100000000000001, 0.062693857798932, 0.19321297766169462, 0.27200593473321827, -0.6746305473251758, -0.1171391767631837, -0.15772087997762135, 0.40682782362697706, -0.23063059926890161, -0.04736549315122354, 0.06074259867095711, -0.35030165978106587, 0.639924810769595, 0.9526329243306958, 0.9302304355217996, -0.2001690696548693, 0.26911917442496147, -0.038479001497933665, -0.3244960275692931, -0.022335121547662377, 0.18810769927264978, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.28922373056411743, 0.37548327445983887, 0.4609956443309784, 0.3597395420074463, 0.5408120155334473, -0.22080783545970917, -0.17144297063350677] - - [0.21602195097108548, 0.6223215401182893, 0.7370873675784467, 0.7900370205540516, 0.020508664294647008, 0.0662093642544466, -0.6091282467001086, -0.2534997821650691, -0.304251759230496, 0.001000000000000003, 0.13889895074879954, -0.033634509419317636, -0.0009999999999999953, 0.0946515229165963, -0.05791303329120058, 0.0009999999999999903, 0.06575553858845537, 0.1954957564786064, 0.2717092313158881, -0.6624672094941644, -0.1176958592828701, -0.1549943552606772, 0.402108894218892, -0.24621513486493424, -0.044306184101506815, 0.06501952486451752, -0.3520039720538113, 0.6424002138013709, 0.953556237344391, 0.9056023793601147, -0.20375837557544785, 0.2819564911613547, -0.04013858815720781, -0.3273317492554313, -0.016485625985763173, 0.19007696821789577, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5649714469909668, 0.6105594038963318, -0.29412156343460083, 0.39026495814323425, 0.4475876986980438, -0.09149907529354095, -0.16568654775619507] - - [0.2150593192930063, 0.6230751087166391, 0.737522393050035, 0.7868884903387534, 0.019605979065993973, 0.06477211563249667, -0.6133731999311293, -0.24939707181880374, -0.30412170673551175, 0.001000000000000001, 0.14168172217200545, -0.03289192503398095, -0.0009999999999999948, 0.10259870238077082, -0.05954914928181847, 0.0009999999999999753, 0.06897142284927361, 0.19849221426312388, 0.2713044668118309, -0.6513061894490826, -0.1182207331880354, -0.15296675053767142, 0.3980934957139017, -0.26145562307359205, -0.041559533541445856, 0.06915120454587859, -0.3534019095247472, 0.643409157000251, 0.9540379554361127, 0.8831404297902734, -0.20761111500853588, 0.2941969771910401, -0.04206979180707827, -0.33004170089180607, -0.010641296005099901, 0.19072753533748066, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4833468794822693, 0.6266980171203613, -0.3013025224208832, 0.6054883003234863, 0.38195669651031494, -0.21396134793758392, -0.1076045036315918] - - [0.21376622697662412, 0.6236956416896468, 0.7379400314949005, 0.7837576421675072, 0.018394850118404082, 0.06384951252985499, -0.6175020871080935, -0.2458755131618178, -0.30433592106362567, 0.0010000000000000018, 0.14607753628620898, -0.03305076537235889, -0.0010000000000000037, 0.1098575870633546, -0.06691768740974201, 0.0009999999999999987, 0.07223720693441917, 0.20294165273163967, 0.2709718161678957, -0.6406535096122095, -0.11877843235621574, -0.15262236628686474, 0.3960012823747548, -0.2761751180626101, -0.03975988398515677, 0.07088416028993258, -0.35352203091415246, 0.6431157818997485, 0.9531213818006622, 0.8627563010825265, -0.21057183641029412, 0.3056270224159497, -0.0440541597919347, -0.33213584149798864, -0.00559733329479402, 0.18999013227523556, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.28697818517684937, 0.40789955854415894, 0.19523970782756805, 0.43688517808914185, 0.44167596101760864, -0.09170684218406677, -0.21673914790153503] - - [0.21218938621841088, 0.6242214490280362, 0.7383475651020744, 0.7806316219507747, 0.016933084117829778, 0.06338080131470951, -0.6215387481866821, -0.24287035945427163, -0.3048852672657394, 0.0010000000000000052, 0.1518665168744659, -0.03393401838670072, -0.0009999999999999896, 0.11654892517396893, -0.07899365785851228, 0.001000000000000003, 0.07549998208621019, 0.2086743528327449, 0.2706760457711133, -0.6304642947990721, -0.11929800374161449, -0.15365735990354443, 0.39552363131841467, -0.29036760290676994, -0.038756249749644214, 0.07054477229899456, -0.3525627511367673, 0.6417068167730904, 0.9510523985408544, 0.8442311230838732, -0.21279489463393134, 0.31637068184520933, -0.0460774155776256, -0.3337507654063343, -0.0012109593463653946, 0.1881189257629271, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3676673471927643, 0.6668657660484314, 0.01729598455131054, 0.42364102602005005, 0.48405754566192627, -0.014953378587961197, -0.27096059918403625] - - [0.2104044547804245, 0.6246806324814458, 0.738731139465114, 0.7775575713894144, 0.01547204893505291, 0.06300988398379159, -0.6254555087271171, -0.24011160897583864, -0.30557164719676677, 0.0009999999999999985, 0.15796312381303476, -0.03635475349390769, -0.0009999999999999968, 0.12283319343599686, -0.08973389920934909, 0.0010000000000000076, 0.07884784776397488, 0.21474693751139246, 0.2706079785000168, -0.620997295768719, -0.12006868431235886, -0.15419574966230762, 0.39575388409240475, -0.3038739122642277, -0.037994688891719385, 0.07163636024579105, -0.35110159990367845, 0.6387160870695165, 0.9479789822980871, 0.8288270984115947, -0.21452404401840797, 0.3260751180824776, -0.048152826275275167, -0.33535503878232936, 0.0029613762028700627, 0.18506014623509548, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.16827240586280823, 0.8036423325538635, -0.6063616871833801, 0.4236948788166046, 0.31813845038414, -0.24425630271434784, -0.3720184564590454] - - [0.20844067078824738, 0.6250826353274442, 0.7390928308485535, 0.7745277986668593, 0.014021084511668966, 0.06271609787800367, -0.6292668665584908, -0.23755753184910025, -0.3063712844480495, 0.001, 0.16430346424676864, -0.04006698365242452, -0.0009999999999999894, 0.12877178950542217, -0.09935565379072972, 0.0009999999999999994, 0.08227203960558953, 0.22108303630645482, 0.27073438667535127, -0.6121459143170347, -0.12105053399098195, -0.15428954094655956, 0.39656453055738644, -0.3167610662882496, -0.037440742803078346, 0.07391387658795222, -0.34923449667252954, 0.6343934390942083, 0.94405813573729, 0.8161014431315836, -0.21583773292500258, 0.3348931590086608, -0.05027118660219332, -0.33696280255197764, 0.006951523759832289, 0.1810197036098323, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.605850875377655, 0.570005476474762, -0.13033127784729004, 0.7410562634468079, 0.3672734200954437, 0.06113952025771141, -0.24468080699443817] - - [0.2061546461364615, 0.6253554284150937, 0.7393875615000893, 0.7716330990753554, 0.012528155499335114, 0.06244438496823554, -0.63287131750229, -0.23505349473344225, -0.3071516032882603, 0.0009999999999999987, 0.17109145829416464, -0.04495603113464528, -0.0009999999999999907, 0.13401171776010573, -0.1052789183530472, 0.001000000000000001, 0.08570835338536943, 0.22727833216562568, 0.2711330157663844, -0.6037572195827112, -0.12181177723921578, -0.15323421269423348, 0.39796435060539553, -0.3292745637684491, -0.03705837701651835, 0.07750648843574509, -0.3475229688904405, 0.629156679591306, 0.9394412464784854, 0.805429990121289, -0.21784734525214808, 0.3425706029674983, -0.05238674144385219, -0.3381523396117187, 0.010525473417832627, 0.17645757072906007, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2610284686088562, 0.45623430609703064, -0.2220088392496109, 0.39080992341041565, 0.48014354705810547, -0.10364539921283722, -0.46485063433647156] - - [0.2035799348774323, 0.6255177556413984, 0.7396202616810124, 0.7688589916790746, 0.01100701774019613, 0.06218068547929012, -0.6362925890091976, -0.23259475826258502, -0.3079035279174807, 0.0010000000000000048, 0.1782451738348328, -0.05084843574843895, -0.0010000000000000009, 0.13864400843671695, -0.10801484034950946, 0.0009999999999999955, 0.08916435030418272, 0.23331653702871574, 0.27176844515416154, -0.5957535782618946, -0.12237930548086773, -0.1511907730195155, 0.3998483563909928, -0.3414492618691059, -0.0368312539939797, 0.0822521154800511, -0.34596276625588757, 0.6231431960008748, 0.934222425972036, 0.7965380101424517, -0.22045851252963986, 0.3492659250284479, -0.05450338125504699, -0.33899871109129254, 0.013732676276231655, 0.17144749232716947, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5003201365470886, 0.6574303507804871, -0.13275472819805145, 0.6121410131454468, 0.53969806432724, -0.02937616966664791, -0.10522118210792542] - - [0.20064701602024523, 0.6256864296757582, 0.7396305222479104, 0.766193805974537, 0.009303478036022633, 0.06202002818513882, -0.6395420338700191, -0.23034220176949727, -0.3086315726961889, 0.000999999999999999, 0.18634073107852767, -0.05835353046249413, -0.000999999999999995, 0.1425404682850456, -0.11075689301629986, 0.0010000000000000263, 0.09267146572033032, 0.23917600507976822, 0.2729755584487469, -0.5873814597711033, -0.12304827714292006, -0.1485076923154955, 0.4025912783052835, -0.3537051100049781, -0.03755276377481459, 0.08706790518727435, -0.3438711907526834, 0.6157088282141151, 0.9277804539461628, 0.7887679869064178, -0.22298815957806764, 0.35484340595188646, -0.05659861986997663, -0.3399879638209978, 0.015794884014277564, 0.16623432144048114, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.546917200088501, 0.3726036846637726, 0.07682618498802185, 0.6687809228897095, 0.3767825663089752, -0.1748247891664505, -0.1746160089969635] - - [0.1974073108006229, 0.625870993487988, 0.7394395075030906, 0.763625242636487, 0.0074458270850077295, 0.06194784015670944, -0.6426379334954522, -0.22827778885626174, -0.3093443340493689, 0.001000000000000003, 0.195235651705982, -0.06730797684947756, -0.0009999999999999983, 0.14578712312011402, -0.11348613118324476, 0.0009999999999999942, 0.09620648597078976, 0.2448801212182048, 0.27468570792329244, -0.5786608717067858, -0.12378583149177311, -0.14525291542110638, 0.4060759814199079, -0.3660332915186824, -0.039092548609536974, 0.09194471795509951, -0.3413150045724259, 0.6069999602752195, 0.9202572040082955, 0.7820026262842281, -0.22545390416104696, 0.3594482125297074, -0.05866786349873834, -0.34110291526759523, 0.016841506976662028, 0.16084435492327934, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6492553353309631, 0.274284303188324, 0.1300743818283081, 0.6511502265930176, 0.4283010959625244, -0.14607518911361694, -0.26570793986320496] - - [0.19381653353752049, 0.6261199635708569, 0.7392980071149468, 0.761233039358696, 0.005387114284498692, 0.061965524741936645, -0.6454885843544129, -0.22640984526293337, -0.30994905560719127, 0.0009999999999999985, 0.20498196725166345, -0.07793794538161489, -0.0010000000000000054, 0.1482505371177263, -0.11728820126379973, 0.000999999999999985, 0.09967907334727948, 0.2502685344432972, 0.2765418338205961, -0.5691814209635017, -0.1243529189062726, -0.14116081420409626, 0.40999580750036474, -0.37927732787388335, -0.041120387171281386, 0.0959945034467657, -0.3388668890574481, 0.5974422764063114, 0.9124566084636158, 0.7764167712860007, -0.22891633402025727, 0.3634915665979695, -0.06028274685372923, -0.3425301392763879, 0.01722261113771719, 0.15618731948486145, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3774341642856598, 0.5093996524810791, 0.018886800855398178, 0.3662133514881134, 0.37616240978240967, -0.17935554683208466, -0.17091697454452515] - - [0.18993136367791952, 0.6264343422650321, 0.7391969928783757, 0.7589990810445786, 0.00314702956556713, 0.062064438937795945, -0.6481191993744168, -0.22472120655349964, -0.3104735421637187, 0.0009999999999999961, 0.21546115468871133, -0.09011350237125339, -0.0009999999999999929, 0.1500095777060531, -0.12203321583843837, 0.0009999999999999979, 0.1030707292850378, 0.25537945778783366, 0.2785209248746548, -0.5589659807126582, -0.1247545754402675, -0.13630219994974863, 0.41431443494054826, -0.3933653175766478, -0.04355446609458514, 0.09929078682618367, -0.33650669617144835, 0.5870810097998942, 0.9044035649958155, 0.7719170606106983, -0.23328174656872513, 0.36705080713897065, -0.06148053273621691, -0.34423703255467003, 0.01699767891603651, 0.15219660926620357, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4955638647079468, 0.6388664245605469, -0.012293422594666481, 0.7181238532066345, 0.4337459206581116, -0.154298797249794, -0.24174955487251282] - - [0.18569494723990404, 0.6266991522783333, 0.7394022869563851, 0.7568672559299594, 0.0008812026520227824, 0.062206847608254304, -0.6506008672708504, -0.22304329987126392, -0.31100676323871973, 0.0010000000000000005, 0.22649911404161563, -0.10364419085441577, -0.0009999999999999968, 0.15144422656371292, -0.1271928254075881, 0.001000000000000004, 0.10626214551615698, 0.2598584663996066, 0.279966366450929, -0.5492450709981865, -0.12458487511727324, -0.12989937839717855, 0.41850744036890763, -0.40738205170703795, -0.04669767457627389, 0.10227565927469022, -0.33452681280436974, 0.5764153875203867, 0.8972284213076184, 0.7698428020528033, -0.2389754045650763, 0.3697684932810831, -0.061868871223413655, -0.3453567662924029, 0.016879185945167182, 0.149274026792686, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4420473873615265, 0.5654123425483704, -0.0796637311577797, 0.3657461702823639, 0.6950138211250305, -0.1295912265777588, -0.25665056705474854] - - [0.18114816992671393, 0.6269255087950791, 0.7398866414697531, 0.7548300459815529, -0.0014048651729223853, 0.06238405067027416, -0.652945524725685, -0.22137967035313122, -0.31154175793870165, 0.0009999999999999966, 0.23801381024938456, -0.11845861751056032, -0.000999999999999999, 0.1525831914357647, -0.13271080582306738, 0.0009999999999999992, 0.1092616546436398, 0.2637375647428371, 0.28091657720690016, -0.5399612097177807, -0.12387857778052447, -0.1220501796636925, 0.42257690586747987, -0.42137202801413653, -0.05046578709117929, 0.10497679454728129, -0.3329091969295712, 0.5654959940612684, 0.8908636345311105, 0.7700077286677192, -0.24589536481617733, 0.37172860044337686, -0.06150775138422431, -0.3459312337989765, 0.016867447455656638, 0.1473516310565556, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.26317283511161804, 0.7056712508201599, -0.13156840205192566, 0.7290399074554443, 0.06328712403774261, -0.25165295600891113, -0.25558918714523315] - - [0.17623124678678148, 0.6270794335723197, 0.7402697813443881, 0.7530450957639165, -0.003609940118833781, 0.06242457859248689, -0.6549910106755684, -0.2199140454224445, -0.3118581526532056, 0.0009999999999999985, 0.24947161516730773, -0.13440296289538603, -0.000999999999999994, 0.152878182824683, -0.13764143209552862, 0.001000000000000001, 0.1125611578019943, 0.2666049466961079, 0.2818587077387564, -0.5311804840113865, -0.1239671772284355, -0.11353592333655642, 0.426686852219977, -0.4361330089023534, -0.05414622885323046, 0.10747514903842692, -0.33193173919927227, 0.5550759648585476, 0.8845267213578443, 0.7706361001891455, -0.253335312726471, 0.3736183503806157, -0.060922003605854286, -0.34609827721063213, 0.01657672158683631, 0.14631508561902393, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.37116941809654236, 0.6069095134735107, -0.008125899359583855, 0.46871763467788696, 0.5387622117996216, -0.18947884440422058, -0.2943613827228546] - - [0.1709646680525483, 0.6271641389807359, 0.7405543606279332, 0.7514998999306218, -0.00574629704595081, 0.06233541324963037, -0.6567565581930053, -0.21863255900170486, -0.31198149127282543, 0.0009999999999999972, 0.2608824957185043, -0.15139967349034875, -0.0010000000000000035, 0.15237518324661234, -0.14201651437477336, 0.0009999999999999926, 0.11613056078062592, 0.2685066984668814, 0.2827917048068979, -0.5228468679066136, -0.12480608541107027, -0.10439094732441671, 0.4308499405023873, -0.45164344830358416, -0.057749315710164055, 0.1097873214722825, -0.3315477924898504, 0.5451318832321539, 0.8782159562079455, 0.7717066628083123, -0.26126480982635003, 0.3754386013787156, -0.06012063512831881, -0.34588016402283467, 0.016025835614577592, 0.14611920855053043, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3625740706920624, 0.5629050731658936, 0.022296320647001266, 0.48337945342063904, 0.5216774344444275, -0.15869095921516418, -0.27434539794921875] - - [0.1653764059864881, 0.6271618918674787, 0.7406910660312498, 0.7502102845577148, -0.007836460145494429, 0.062101656664233694, -0.6582298254225323, -0.2172372348792796, -0.31188844873507143, 0.0009999999999999968, 0.2721503459648721, -0.1687429895121177, -0.000999999999999994, 0.15121126593152243, -0.14531325430091926, 0.0010000000000000172, 0.11941584306933031, 0.26926677297280216, 0.28361720735477786, -0.51516324939894, -0.12549821054162094, -0.09431122937225755, 0.4352695357233587, -0.4671362142057124, -0.06181952340703226, 0.11246371447103395, -0.33160760291003805, 0.5349986397414555, 0.8720919814749984, 0.7742938354662647, -0.26837267016375016, 0.37697011111482814, -0.05932960528970608, -0.3453519900849175, 0.014852370633406609, 0.14701321714599447, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3700600266456604, 0.4666571617126465, 0.24863265454769135, 0.37386780977249146, 0.46164241433143616, 0.09952529519796371, -0.2303110510110855] - - [0.15947359721561194, 0.6270710231627119, 0.7406806828061341, 0.7491696066458029, -0.009893612424464097, 0.061728593937503504, -0.659421411240085, -0.21572245819830327, -0.31159392763048915, 0.0009999999999999994, 0.28330409489001523, -0.1864000075142629, -0.0009999999999999942, 0.14940373865787865, -0.14756181366102247, 0.001000000000000015, 0.12242049180458882, 0.26891045871993097, 0.2843459351649652, -0.5080775050773337, -0.12605280590038104, -0.08332217920259732, 0.43996069306792135, -0.48261528485322663, -0.06635435522407387, 0.1155036609877196, -0.33209294950882295, 0.5246991197837294, 0.8661476851679715, 0.7783306921511183, -0.2746880467893954, 0.3782148683638304, -0.05853913415442902, -0.3445257285105014, 0.013076804846447191, 0.1489772827166889, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4301838278770447, 0.33239808678627014, 0.035870034247636795, 0.3985308110713959, 0.7003883123397827, -0.033358383923769, -0.22143910825252533] - - [0.1531773994323091, 0.626824907532288, 0.7406118804944954, 0.7483452881080834, -0.011847027110598833, 0.06116407556000417, -0.6603771146670365, -0.2141860644885628, -0.3109318651794893, 0.001000000000000003, 0.29431262109338, -0.2037615752924195, -0.0009999999999999896, 0.1470270962686738, -0.1487975518388914, 0.0009999999999999916, 0.12554607153047143, 0.2671104634040152, 0.2848639796837539, -0.5010378426410365, -0.1262360861944024, -0.07116482371323352, 0.4443158546361136, -0.4983439967113086, -0.0714369760769011, 0.11948711033555971, -0.3331956251162771, 0.5148141733087687, 0.8609312817642066, 0.7823805518487843, -0.28136328089907553, 0.3796765209199266, -0.05801078199683612, -0.343722302379483, 0.011698105431159508, 0.15085395289697892, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5488494634628296, 0.5726338624954224, -0.0959063395857811, 0.6932507753372192, 0.44043561816215515, -0.1800856739282608, -0.1989917755126953] - - [0.146459677097813, 0.6264143198035689, 0.7404823019279885, 0.7477307302701149, -0.013725829063098358, 0.06042507053810552, -0.6611045057150309, -0.21261914172818283, -0.30991597808262844, 0.0010000000000000063, 0.3053002628284735, -0.22071721269034394, -0.0009999999999999961, 0.14406126181603318, -0.14904786385824847, 0.001000000000000012, 0.1287855675962817, 0.2639378544205637, 0.2851889010106072, -0.4940092989258916, -0.12604927858587114, -0.05789541453987761, 0.4483892987082901, -0.5142601359211698, -0.07712844597435713, 0.1243745599225218, -0.33488381842910664, 0.5053236327925466, 0.8564362561745522, 0.7863914542943067, -0.2883926002141795, 0.38135647354656005, -0.057748606152896155, -0.3429589515852098, 0.0107114609886104, 0.1526518822679629, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.370313435792923, 0.579965353012085, -0.2486954778432846, 0.5870153307914734, 0.42572712898254395, -0.24812519550323486, -0.35605403780937195] - - [0.13940283927403396, 0.6257941035046052, 0.7402838261010952, 0.7472048670261217, -0.015642487054940618, 0.059690905813980455, -0.661722898994979, -0.21101067589521186, -0.30894011833870605, 0.0010000000000000005, 0.3166926574379378, -0.23620552046481416, -0.0009999999999999877, 0.1402801554014867, -0.14796312868160189, 0.0009999999999999974, 0.1319918611605031, 0.2600438584149981, 0.28529109214091014, -0.48663050471966296, -0.12445985703798716, -0.04407072826557302, 0.45294460845717155, -0.5302249310083872, -0.08370699818289515, 0.12924537264611582, -0.3383766269679202, 0.49625127138021763, 0.8526087306102497, 0.79011331714935, -0.29631878469312617, 0.38405546298695875, -0.05826255741007062, -0.3424637747128321, 0.011417259748658518, 0.15430058776678343, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2384396195411682, 0.5373000502586365, -0.046213943511247635, 0.37241387367248535, 0.6158249378204346, -0.11448492109775543, -0.39463120698928833] - - [0.1319997482373771, 0.6249562570759214, 0.7400126024362217, 0.7467712988221689, -0.01759365042471705, 0.058961723148149285, -0.6622285148825786, -0.20936310435364122, -0.308017137387598, 0.000999999999999997, 0.32848071317616745, -0.250231677603481, -0.0009999999999999953, 0.13565149577121258, -0.14551450565391139, 0.0009999999999999887, 0.13516661533763508, 0.25545505434364607, 0.2851477131510716, -0.4789610505174026, -0.12147136249512126, -0.029712966904431478, 0.4579727465332278, -0.5462865525375578, -0.09119613379527385, 0.1341268918040858, -0.34370955554459387, 0.4876218896215036, 0.8494572895407762, 0.7935905768806234, -0.30514898455146006, 0.38780727277940596, -0.05958871325209873, -0.342243909792466, 0.013822537887033736, 0.15580727206122136, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4724765717983246, 0.4799695909023285, -0.21030882000923157, 0.6896644234657288, 0.6195845007896423, -0.13829578459262848, -0.22312995791435242] - - [0.12388468947617783, 0.6240014046336932, 0.7394887197710917, 0.7467755292021478, -0.01917946282696508, 0.05756069510389333, -0.6623029696216658, -0.20786123179592683, -0.30548399835025675, 0.0009999999999999998, 0.3394446619919095, -0.2636281962819842, -0.0009999999999999966, 0.13078345931370908, -0.14121527967174471, 0.0010000000000000143, 0.13945473780320286, 0.248302646729147, 0.2849567830545926, -0.471848740383914, -0.12194286783523224, -0.014362458754069756, 0.46182779891434117, -0.5623695821622613, -0.09790161616146667, 0.13940101306703445, -0.34701528555034067, 0.4784923944939802, 0.8467752185312419, 0.7960429299619849, -0.30878560309262987, 0.38820366657894195, -0.06047642129978315, -0.34164651589959555, 0.013498266703437982, 0.15765256793845575, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3725571632385254, 0.3930448889732361, 0.11997926235198975, 0.4106133282184601, 0.7423286437988281, -0.016824007034301758, -0.629560112953186] - - [0.11500148021653313, 0.6229153302182961, 0.7387028456296124, 0.7472089246457604, -0.020460021442266572, 0.0555079561797973, -0.6619509628764435, -0.20648336580132354, -0.30139609672458123, 0.000999999999999997, 0.34984145911328285, -0.2761554238561865, -0.000999999999999994, 0.12567384159222753, -0.13504862286836747, 0.0009999999999999896, 0.14488339099125963, 0.23879672118841747, 0.28478867568593147, -0.4651062625718345, -0.1259747207731113, 0.0018287210838222176, 0.46462933586867244, -0.5783214737093914, -0.10388585890961448, 0.1450208014257762, -0.34812088682419934, 0.46874548882567474, 0.8445782082147106, 0.7972442810986397, -0.3070542168525612, 0.3851534278229348, -0.060896660365973965, -0.34071015794547294, 0.010361719510732966, 0.1598920674984317, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6516837477684021, 0.3801821172237396, -0.29905056953430176, 0.48101648688316345, 0.3537856936454773, 0.12943299114704132, -0.455414742231369] - - [0.10604192366083873, 0.6216078564891023, 0.7378720600532492, 0.74764140251446, -0.02171688794493896, 0.053613527356287244, -0.6615786421194506, -0.20488764862085848, -0.2978884524324297, 0.0010000000000000015, 0.3604643657727112, -0.2867673870161536, -0.0009999999999999976, 0.12032484236831051, -0.12778212375991382, 0.0010000000000000044, 0.14942404579865637, 0.2302436298072348, 0.28423093869553206, -0.45926189791709604, -0.12836547640552795, 0.01701692301477333, 0.46764382690224815, -0.594186614031953, -0.11032938200575079, 0.1505928433624563, -0.34996314681393803, 0.46013892089879604, 0.8431098449515494, 0.7984343291383168, -0.3060885418029419, 0.3831213235892779, -0.06166354204555864, -0.3394101464072572, 0.00808997265122651, 0.16228801949702493, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.37822917103767395, 0.33603790402412415, 0.11980678886175156, 0.3989201784133911, 0.44176462292671204, 0.007534881588071585, -0.2795381247997284] - - [0.09702421283359462, 0.6200725827973467, 0.736996785484987, 0.7480862814814709, -0.02290357105760949, 0.05185586932484372, -0.6611757033562257, -0.2030646271269307, -0.2949209719490547, 0.0009999999999999992, 0.37114686406035785, -0.2955048305991716, -0.0010000000000000026, 0.11474002045992933, -0.11932037119959493, 0.001000000000000006, 0.1530231416190171, 0.2225479635392089, 0.28322919849299083, -0.4545190419285826, -0.1290291970384153, 0.0312289739462377, 0.4707803147670315, -0.6100946738594278, -0.1171970822581168, 0.15616704428434344, -0.352645410116462, 0.4528370599064019, 0.8424131171062914, 0.7997113894589934, -0.30593076620101395, 0.38214376608915857, -0.06279468497442779, -0.3376875733877855, 0.0067344533076408915, 0.16481098719684872, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.36326202750205994, 0.281648188829422, 0.12085757404565811, 0.3614368140697479, 0.4267965853214264, -0.18954798579216003, -0.4246158301830292] - - [0.08781088763630543, 0.6184231465924138, 0.7358091962053398, 0.7486045921229267, -0.02392673190423759, 0.05004067471839657, -0.6606925207890408, -0.20128441029233252, -0.2920920613962652, 0.001, 0.3815970501895027, -0.30276149551128306, -0.001000000000000014, 0.10884299095443274, -0.11013652770206044, 0.000999999999999992, 0.15627716057486174, 0.21575723547640918, 0.28272728180903856, -0.45108798034480063, -0.1297887897116032, 0.04307186705036062, 0.4733747622691132, -0.625432772778861, -0.12376831442960821, 0.16228756736327932, -0.35432013175031213, 0.4456898271074516, 0.8412712545194352, 0.8007105354303133, -0.3050296577356503, 0.38092892130071854, -0.0641951945939808, -0.3358170498662245, 0.005296267589224704, 0.16749729070887645, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3620782196521759, 0.5029668807983398, -0.007622112985700369, 0.35090434551239014, 0.563234806060791, -0.19966338574886322, -0.24577493965625763] - - [0.07838888550426448, 0.6166587751255311, 0.7342876016498128, 0.7492063681257702, -0.02475056375761022, 0.04814953621837736, -0.6601203297243834, -0.19956189013956077, -0.289378534506224, 0.0009999999999999976, 0.39172984205329575, -0.30847236802744205, -0.0010000000000000195, 0.10262478873923303, -0.10018462947528656, 0.0010000000000000059, 0.1591789196751668, 0.20988378381343464, 0.2827580646046079, -0.44919820103199354, -0.1306574932566784, 0.05238708485315206, 0.4753416907723464, -0.6401908314540262, -0.12999808339123506, 0.16900401673275023, -0.3549244062929734, 0.438771836074398, 0.8396489162128244, 0.8014622365423232, -0.3033236376821452, 0.37943769305334896, -0.06589028560205279, -0.3337674718357551, 0.003770796909343157, 0.17033397298020767, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6412706971168518, 0.38754546642303467, -0.4069325923919678, 0.6771702170372009, 0.3931654393672943, -0.019464965909719467, -0.2984432578086853] - - [0.0688898636945503, 0.6147545587148261, 0.7327165108156248, 0.749896382316633, -0.025306945893630008, 0.04627968407352427, -0.6594491376292929, -0.19795581206631363, -0.2867524049817903, 0.0010000000000000028, 0.4013039409865862, -0.3127979161302358, -0.0010000000000000067, 0.09616023144724561, -0.09004219990871169, 0.001000000000000011, 0.1615105929493209, 0.20513033608113096, 0.28255998764642265, -0.44907904490232886, -0.1306865553436088, 0.05992755807519392, 0.4762925440154194, -0.6544854068485456, -0.1362010089594967, 0.17546648156769518, -0.3541994266388254, 0.4332580255215805, 0.8385437710442225, 0.8030406685741854, -0.3018225977261312, 0.3779164216548239, -0.06754720024702417, -0.33173258309078146, 0.0033156851441539337, 0.1726578792146879, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3967585563659668, 0.6773607730865479, -0.012067417614161968, 0.6135303974151611, 0.580924928188324, -0.13522927463054657, -0.2923312485218048] - - [0.059301392450444045, 0.6127010097519522, 0.7311015278311862, 0.750692660023773, -0.025539501715208895, 0.044412391162073385, -0.658662131558994, -0.19649313902621035, -0.28416828609877204, 0.000999999999999996, 0.4101805999664205, -0.3156567936300866, -0.0009999999999999896, 0.08942734054441545, -0.07972362783721616, 0.0010000000000000165, 0.16324604221083547, 0.2015198258051262, 0.2821007909510606, -0.45103047973076793, -0.12979623124600836, 0.0655499169752952, 0.4760504251082105, -0.6682797336666033, -0.14235974318848854, 0.18164299586861155, -0.3520372712988401, 0.42938639320550626, 0.8380120443270958, 0.805534587506724, -0.300544241410011, 0.3763149143776105, -0.06916073000628548, -0.32968524316504627, 0.0040471678649811665, 0.174364468935249, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.31252920627593994, 0.4129663407802582, -0.03449169173836708, 0.3412918448448181, 0.39469847083091736, -0.045778222382068634, -0.3096914291381836] - - [0.04988279173329014, 0.6105730145867866, 0.7292364127513182, 0.7515365128067525, -0.02536679586012225, 0.04267172128874158, -0.6578210393323638, -0.19535100679607767, -0.2822135590247233, 0.0010000000000000022, 0.41815030181216306, -0.31787089126101786, -0.001000000000000001, 0.08292190286485836, -0.06991118509209374, 0.0010000000000000076, 0.16428164791545266, 0.19962261404148654, 0.28162212549127763, -0.45386530825154364, -0.129080284979108, 0.06773470838405006, 0.47587719432488057, -0.682834395711522, -0.1482886089338815, 0.18751076380843773, -0.349203393616494, 0.426437373027849, 0.8368406617080748, 0.8074867652722261, -0.2995636989274709, 0.37501037807695964, -0.07042476657669572, -0.327638615773605, 0.004658129170247854, 0.17574144194451422, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5988530516624451, 0.03368779644370079, 0.03090924583375454, 0.3060753345489502, 0.4573369324207306, -0.2688685655593872, -0.2976873517036438] - - [0.04064834817878536, 0.6083620543215513, 0.7271022575942038, 0.7524434773296854, -0.024714123879500793, 0.04105458678871172, -0.6569113687617895, -0.19457081289043288, -0.28092845028636315, 0.001000000000000002, 0.42503327539990005, -0.3194105790015289, -0.0009999999999999968, 0.0766884127647454, -0.06071926772239631, 0.000999999999999997, 0.16456074890512573, 0.19960126410278392, 0.2811141916113133, -0.4577090900342763, -0.12856794299107377, 0.06605357402176103, 0.47572718320273144, -0.6982414022502024, -0.1539455591270859, 0.19301956605621015, -0.34565008328309094, 0.4245514628241901, 0.8349450645324124, 0.8088017989392876, -0.2989207047434201, 0.3739986579024523, -0.07129319844574795, -0.32557820389225295, 0.005132362793419245, 0.17669463190975543, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3032926321029663, 0.45686185359954834, 0.17331162095069885, 0.45193493366241455, 0.42581266164779663, -0.018009234219789505, -0.31981900334358215] - - [0.031623673188066644, 0.6062017623665595, 0.7253847921230275, 0.7532622042091466, -0.023825291026343655, 0.03961127357289222, -0.6560940132507084, -0.19376379858962034, -0.28016508995164907, 0.0009999999999999987, 0.4311099852744244, -0.31991948364658673, -0.0009999999999999872, 0.0705890040298802, -0.053180095609136405, 0.0010000000000000005, 0.16379545297183223, 0.2010696921443414, 0.28064496590421545, -0.46251265129218977, -0.12641805321065783, 0.06113119885256214, 0.47496101956424225, -0.7126408301331654, -0.15881109414360917, 0.19688896098955214, -0.3415978090833583, 0.4237017950980429, 0.8336289576648117, 0.8115634734158443, -0.298506113093734, 0.3716953490708866, -0.07164104805754754, -0.322718796361337, 0.00677626112496406, 0.1764241135158274, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3586740791797638, 0.7250373363494873, 0.02569057233631611, 0.6047858595848083, 0.36947473883628845, 0.03339049220085144, -0.25038185715675354] - - [0.022838422737549827, 0.6040973769452949, 0.7241637553887077, 0.7539837718904782, -0.022663684591324534, 0.038368476616573784, -0.6553798052493492, -0.1929420322044729, -0.28001498791132223, 0.0010000000000000002, 0.4362525487252462, -0.3192369033134217, -0.0010000000000000243, 0.06462864722737306, -0.04756896394504235, 0.0009999999999999998, 0.16188235865143738, 0.20428921368203026, 0.28021423720398186, -0.46845576773415626, -0.12239821640758453, 0.05245118883311877, 0.47348157235088717, -0.7258368844667512, -0.1627570378852042, 0.19886156210902048, -0.3369711496351024, 0.4240825693630274, 0.8329839354706391, 0.8159415835642246, -0.2983580753209354, 0.36781864871827313, -0.07138432092956601, -0.31886417139308265, 0.009774649551525419, 0.17469673622207252, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4535175859928131, 0.5703408718109131, 0.027391286566853523, 0.3703688383102417, 0.5999191999435425, -0.12312120199203491, -0.244378000497818] - - [0.01424804160749109, 0.60198700354633, 0.7236279936129147, 0.7546575480304172, -0.021449883733914304, 0.03741212233351574, -0.6547000998862158, -0.19187233078382673, -0.2800584653348294, 0.0010000000000000005, 0.44091589991558044, -0.31749119829671896, -0.0009999999999999835, 0.058712945698437165, -0.04487369198093338, 0.0009999999999999935, 0.15836461153865256, 0.20860432070402832, 0.2796622480947702, -0.47338039400958853, -0.11416091080236836, 0.04108075713883086, 0.47173093355579604, -0.7375572600625653, -0.16780435816776296, 0.19841823323078378, -0.33136352006511177, 0.4239257081221559, 0.8337132747407389, 0.821392445196689, -0.2982819948045359, 0.3629054806598907, -0.0707241813820987, -0.31433279376370166, 0.013199193417730596, 0.17192004782122772, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.43215709924697876, 0.6066935658454895, -0.22154633700847626, 0.785950779914856, 0.42167404294013977, -0.19804657995700836, -0.1534649133682251] - - [0.005883971959550342, 0.5998671378051782, 0.7239264818274521, 0.7552751052984017, -0.020177476612542943, 0.036810904955045916, -0.6540621851402428, -0.19055276152477738, -0.28038356038346807, 0.0010000000000000018, 0.44504813923292874, -0.31446336260652163, -0.0009999999999999972, 0.052798756618994684, -0.04559458817291071, 0.0010000000000000052, 0.15302393444639506, 0.21432589465502427, 0.2789641902848987, -0.4770481751486112, -0.10096745283055175, 0.026452684029852222, 0.469664995866323, -0.7474867924938182, -0.17417104561473656, 0.19513282256676134, -0.324578259367329, 0.42307966957458304, 0.8360706789175406, 0.8280457594948062, -0.2983000510571235, 0.35672926533594446, -0.06958660336645989, -0.30892356976425855, 0.017127114725981735, 0.167861550730929, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3364584147930145, 0.443343847990036, 0.008958623744547367, 0.4263250529766083, 0.4252053499221802, 0.05307133123278618, -0.2838016152381897] - - [-0.002135456135368429, 0.5978992888754375, 0.7231745831951683, 0.7558001027175235, -0.01854162271883226, 0.03619039950808302, -0.6535385741810337, -0.1905127825291528, -0.28312670738651563, 0.0009999999999999998, 0.4485078690887216, -0.3122756528029152, -0.0010000000000000074, 0.0489997935342191, -0.04501615151552436, 0.000999999999999991, 0.14988047129512053, 0.22329270383469177, 0.27849704477907505, -0.4799731557015636, -0.09214470959413738, 0.00815213831087032, 0.4670981625711263, -0.7598703470572066, -0.17792620933318842, 0.19268693866987796, -0.3175517756158253, 0.4221618026660547, 0.8369560694469153, 0.8331350473120829, -0.29772235296388266, 0.3533054324308925, -0.06960131576062709, -0.3055540634112382, 0.020905246584763556, 0.16662143088015655, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.31445571780204773, 0.5410348773002625, -0.1407734602689743, 0.8388282060623169, 0.3774949014186859, -0.18047447502613068, -0.08239496499300003] - - [-0.009744244970624902, 0.5961222634298311, 0.7211256143506489, 0.7562127647371493, -0.016483101574103383, 0.03552405726942275, -0.6531528176209696, -0.19191841767546947, -0.2889298005444699, 0.0010000000000000009, 0.4511337528804202, -0.3110726695217926, -0.0009999999999999987, 0.04785544810589405, -0.04281567999937012, 0.0010000000000000033, 0.14925244247233968, 0.2364250746777778, 0.27831562559134515, -0.4819933194372865, -0.08852684587067165, -0.014664234842882588, 0.4639454722379881, -0.7752692402965009, -0.17851049044978684, 0.19122337945921938, -0.31023580835255815, 0.4211236809546237, 0.8360554362497463, 0.8365012615336326, -0.2964036404472547, 0.35337907794990814, -0.0710053557260216, -0.30485533999069464, 0.02450947090460909, 0.1688923539855249, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4498426616191864, 0.6566728949546814, -0.19464494287967682, 0.771453320980072, 0.43005090951919556, 0.10662239044904709, -0.2589794993400574] - - [-0.017360199285731695, 0.5945043016672729, 0.720023099773463, 0.7567005514085217, -0.014588876460101263, 0.035045570952456445, -0.6526585999879928, -0.19324822478001594, -0.2943955291666253, 0.0009999999999999996, 0.45377977730842217, -0.3091576392754791, -0.0009999999999999877, 0.050084610374592095, -0.04395656972426436, 0.0009999999999999944, 0.14736383168559175, 0.24905185541225858, 0.27795520764652826, -0.4817049980874706, -0.0779600150277471, -0.03548489428983042, 0.46154211600097844, -0.7878792281313769, -0.17905162448338782, 0.18752498911444979, -0.30406528167008934, 0.4187832522082813, 0.836374960404926, 0.8408076503467313, -0.2944965924664312, 0.3511056174096168, -0.07125302457233967, -0.3029527264133405, 0.027559036786748377, 0.1697794272011836, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.7331083416938782, 0.6579992175102234, 0.027102844789624214, 0.4018152952194214, 0.38251203298568726, -0.08675927668809891, -0.22494246065616608] - - [-0.024988593041431216, 0.5930827140637521, 0.7200848625321024, 0.7572674077770061, -0.01292179989392873, 0.0348314719194757, -0.6520474436497063, -0.19452011911888537, -0.29937638177089554, 0.0009999999999999996, 0.4565734563017154, -0.30630454394085604, -0.000999999999999995, 0.05653682612892788, -0.04922697462910059, 0.0010000000000000152, 0.1439703792462199, 0.2609379463717343, 0.27739188974026474, -0.47857239325007794, -0.058851932048374585, -0.053777530592303745, 0.460127915221392, -0.7970143812458673, -0.1795584771927233, 0.181040500089381, -0.29926886445046413, 0.4147821478307003, 0.8382096829226702, 0.846219802842907, -0.29187386352397116, 0.3459516104496484, -0.07006845739272995, -0.29950518375402785, 0.02990891427237245, 0.16900151617417744, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3836170732975006, 0.4338797926902771, -0.0006578384782187641, 0.416094571352005, 0.5391724109649658, -0.07823365926742554, -0.2760413885116577] - - [-0.03284649160258474, 0.5917965193240372, 0.7204997699865362, 0.757722934280647, -0.01114862403770413, 0.034706779830461144, -0.6515574437307814, -0.19583834249778748, -0.3046288061090788, 0.0010000000000000009, 0.45976322335116365, -0.3027173167498267, -0.000999999999999996, 0.06961723443007845, -0.055356409942917795, 0.0009999999999999894, 0.14024724840561678, 0.2719409958694982, 0.2779768446596665, -0.4741839962727439, -0.035287100046031396, -0.07065775428656447, 0.45897301151357717, -0.8052180314203901, -0.1761821446314966, 0.1740765492844484, -0.294941985200008, 0.4118168047499389, 0.8389210217657562, 0.8508123379779022, -0.28803557275021646, 0.3416364745486469, -0.06901638676793297, -0.29706689363417516, 0.03168799317916927, 0.16853983457515828, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4518938660621643, 0.6399761438369751, 0.008895039558410645, 0.367667019367218, 0.33950936794281006, -0.145926371216774, -0.28083524107933044] - - [-0.04103843936724243, 0.5906741785507659, 0.7213528537519014, 0.7580262495420553, -0.009285441472329988, 0.03470951238757633, -0.6512336257685761, -0.1971044674039625, -0.31002815866673833, 0.0009999999999999985, 0.46357066569518507, -0.2980043312849579, -0.001000000000000002, 0.0913045235011351, -0.06270100686297668, 0.001, 0.13603796327612444, 0.281475880207281, 0.28007083469747546, -0.46818726202622757, -0.006156903837558083, -0.08557522056759602, 0.45821633923375166, -0.8121839014293101, -0.16761180119385255, 0.16640145936633768, -0.29115415254943533, 0.41016741800866996, 0.8382086099703236, 0.8543764909603199, -0.28264568813653945, 0.3385513392158956, -0.06813755056535781, -0.29608173929678966, 0.03274642760784793, 0.1686440041021169, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3247949481010437, 0.19082234799861908, -0.17665883898735046, 0.36627084016799927, 0.648851215839386, -0.22097329795360565, -0.28713345527648926] - - [-0.049216401227002, 0.5894917664125026, 0.7222968778602479, 0.7582749520678775, -0.007671292345595571, 0.03478726592124361, -0.6509609008764892, -0.1989840397367838, -0.3149836565488517, 0.0009999999999999996, 0.46598295486722663, -0.29207347488647684, -0.0009999999999999953, 0.10835578519490559, -0.06950215108570273, 0.0009999999999999994, 0.13296653199350997, 0.29033994671142166, 0.2830277338281065, -0.45946759251256397, 0.021018642408466123, -0.0978108753250221, 0.45814781728403897, -0.818517125784847, -0.161621988514382, 0.15874347474688988, -0.28793011933351287, 0.4078227620629838, 0.8367977568055831, 0.8562821089370669, -0.27785807642209603, 0.3359967617963245, -0.06705787143329399, -0.2957544714788803, 0.03322438690770379, 0.16926381024264084, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5081801414489746, 0.6425208449363708, -0.0050923568196594715, 0.6179983019828796, 0.4072761833667755, 0.06243659555912018, -0.31194180250167847] - - [-0.05729879632073907, 0.5882518639511405, 0.7233252292069398, 0.758461709479996, -0.006350157655405676, 0.03493291632857493, -0.6507497230942281, -0.2018123326591722, -0.3193009319464243, 0.0010000000000000002, 0.4664272741826162, -0.28475809339380953, -0.000999999999999996, 0.11908470974171827, -0.07561648971877098, 0.0010000000000000054, 0.1313464954818591, 0.2980796680526943, 0.2870900534440912, -0.44721414690708466, 0.04591626845419739, -0.10636465073996632, 0.4589058842544061, -0.8242835927320986, -0.15926464308826763, 0.15121747851578118, -0.2854755995839971, 0.4045591374226469, 0.8344804597837605, 0.8561102627369626, -0.2738500555212438, 0.33430427757790654, -0.06572830148393471, -0.2963983919082282, 0.03294775758498589, 0.17068134120119452, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3002541959285736, 0.6964874267578125, -0.04884830489754677, 0.4417031407356262, 0.4116038978099823, -0.20954298973083496, -0.25367051362991333] - - [-0.06487591519567881, 0.5869986060859519, 0.724304702605871, 0.7585560798315695, -0.0052023049637933225, 0.03509598501136523, -0.6506411312003773, -0.2052400795005199, -0.3233291437479045, 0.0010000000000000005, 0.4639427122519689, -0.2771110733959807, -0.0010000000000000076, 0.12161299956591975, -0.08085769396967542, 0.0009999999999999933, 0.1311512681514418, 0.30532531325847706, 0.2917088877158404, -0.43323754816194543, 0.06502465147551424, -0.11407093022648417, 0.4604612190446228, -0.8295517438058839, -0.1615670582415788, 0.14377835107725417, -0.28408360261090243, 0.4008395378799749, 0.8317911192421098, 0.8559475313312962, -0.27006157449084717, 0.3336449948756483, -0.06426353300065084, -0.29726219001038456, 0.032314899600623816, 0.17119517030558618, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5090030431747437, 0.30536893010139465, 0.1315968632698059, 0.49033835530281067, 0.7920419573783875, -0.012338093481957912, -0.29448291659355164] - - [-0.07174472306247001, 0.5857481183246442, 0.7251974462505658, 0.7585364422807807, -0.00426200178055112, 0.03528230925501594, -0.6506607869900191, -0.20952154997719502, -0.3270062209164536, 0.0010000000000000009, 0.4573942816754809, -0.26911630036659245, -0.0010000000000000076, 0.11306683248656461, -0.08496446906915778, 0.0010000000000000015, 0.13275605604537186, 0.31183568961647473, 0.2970362036286573, -0.4170382181780045, 0.07648891900752743, -0.12055458617696078, 0.4630533340564372, -0.8342604382231149, -0.17029626964080635, 0.13651551544509244, -0.28411644962817195, 0.3965434874877551, 0.8286071195800983, 0.8558627544037306, -0.2665659800339081, 0.334440343220978, -0.06262439880464608, -0.2984521741032063, 0.031207713310936896, 0.17053761145138294, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4263420104980469, 0.553597629070282, -0.12025768309831619, 0.3989822566509247, 0.581350564956665, -0.1435377150774002, -0.31804266571998596] - - [-0.07883486706472057, 0.5846430515956733, 0.7259200631403694, 0.7584931703380684, -0.0032887381514862123, 0.03534171899926269, -0.6507136525769643, -0.21396848551658879, -0.32994051228677534, 0.000999999999999999, 0.4498455712543633, -0.260586246124337, -0.0010000000000000018, 0.10444266031945806, -0.08904346833123, 0.001000000000000002, 0.13567587780338258, 0.31631612179497703, 0.3029697685695305, -0.3990528847694828, 0.08226876209552494, -0.12551303878560005, 0.4664869590381824, -0.8380958770096844, -0.17787534336344202, 0.1292013061947511, -0.2860895107276633, 0.3911247397334066, 0.8242912438928701, 0.8536488668184666, -0.2626465924768337, 0.33464421817198003, -0.06113569583566646, -0.30066771908443396, 0.029534295439623073, 0.16923980265469116, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3686196208000183, 0.20634181797504425, 0.04161209985613823, 0.5527176856994629, 0.384952187538147, -0.33943238854408264, -0.1995936632156372] - - [-0.0862167459655231, 0.583730236365702, 0.7264098932791382, 0.7584208231469899, -0.002264839175652069, 0.03519427720945512, -0.650810332103183, -0.21866104980647708, -0.331660500939462, 0.0010000000000000005, 0.44087104511890285, -0.25137034245301537, -0.0010000000000000005, 0.09576422855803601, -0.09308731989777719, 0.0009999999999999926, 0.14046324467393972, 0.31766516085356095, 0.3097368788742184, -0.37843218206403345, 0.08037427590570806, -0.12820351574086625, 0.47105197860695236, -0.8408480765707846, -0.18361951968154314, 0.12184202668420932, -0.2907387328529098, 0.3841032371967053, 0.8184343710414537, 0.848353817727884, -0.2581545494836939, 0.3341554944325686, -0.05985480191486466, -0.3044334015897167, 0.027088200638252388, 0.1671655998332598, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.48366954922676086, 0.6286837458610535, 0.01868809387087822, 0.6518809795379639, 0.4518691897392273, -0.012481511570513248, -0.28445562720298767] - - [-0.09382975277399772, 0.5829525605785844, 0.7271226194621274, 0.7584513179872782, -0.0011356247496932886, 0.0349228553793023, -0.6507923653300983, -0.2227765546813827, -0.3324652362409995, 0.0009999999999999994, 0.43164384422204993, -0.24196908060501815, -0.0010000000000000018, 0.09260811905944573, -0.0974300721078388, 0.0009999999999999846, 0.14541024865344182, 0.31687587534911316, 0.3162924052324055, -0.35771238610597506, 0.07811944668657063, -0.12895816232224488, 0.47547839563870103, -0.8424302621565213, -0.18426675110622082, 0.11405532068544223, -0.29609821881485465, 0.37739740550022133, 0.8126715393024155, 0.8429103031258292, -0.2527797277453025, 0.33366945084006355, -0.058897656221780476, -0.3088726941607707, 0.023952739342331518, 0.16495740701574724, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4063132703304291, 0.631901741027832, -0.08417074382305145, 0.5511011481285095, 0.1827193945646286, -0.2993633449077606, -0.18052272498607635] - - [-0.1017338387681333, 0.5823680193981078, 0.7281426918401431, 0.7586618128866464, 0.00019396273294850435, 0.03442167560297565, -0.6505746416013215, -0.22621803503136143, -0.33189598505773077, 0.000735351599977351, 0.4219030614787104, -0.23256257770130748, -0.0010000000000000074, 0.09696466707458823, -0.10218765492882512, 0.0009999999999999963, 0.1507853786751008, 0.31290839143136856, 0.32261151629685825, -0.33703837322426244, 0.07534953808434464, -0.12690057216508796, 0.47952199135395024, -0.8425466096013893, -0.17769070081228938, 0.10576009932351896, -0.30238817135805307, 0.3713571865953285, 0.8070353356744668, 0.8371836941525957, -0.24617630126839649, 0.33324688602063723, -0.0583972244843825, -0.31435055275462576, 0.019859164279249247, 0.16263521870696127, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40541282296180725, 0.718976616859436, -0.0691426545381546, 0.3847941756248474, 0.415260910987854, -0.15061026811599731, -0.12379642575979233] - - [-0.10983746030804778, 0.5817357709690367, 0.7292798376970726, 0.7591021471504605, 0.001714156798743998, 0.03393025062864513, -0.6500844021742909, -0.22991144669505892, -0.3306883492329635, -0.0009999999999999996, 0.41122317901016264, -0.22345380800544226, -0.001000000000000002, 0.10219892596297571, -0.10787057257101847, 0.0010000000000000083, 0.1568969186028995, 0.30767007277544156, 0.328996249181327, -0.31841821207810067, 0.07069568059615747, -0.1244277882138993, 0.4827418381578933, -0.8412020471870714, -0.1701113589438373, 0.09787207874302761, -0.3074787858717611, 0.36704350356778637, 0.8016108820146718, 0.8315833867666236, -0.23935201933172442, 0.33336792272546584, -0.058204789298748186, -0.31998712749287334, 0.015812744862204554, 0.16070880966646045, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.29529792070388794, 0.3684244155883789, 0.09898333996534348, 0.42284727096557617, 0.368879109621048, 0.05465427041053772, -0.3656337857246399] - - [-0.11828992803410311, 0.5811949945703797, 0.7305764749136276, 0.7595733688657769, 0.003278849843859019, 0.033267989306445055, -0.6495619965338975, -0.23326491071458524, -0.32836518538387655, -0.0010000000000000009, 0.3997930459376797, -0.21406215164543524, -0.0009999999999999907, 0.10942298629367463, -0.11408459216786515, 0.0010000000000000074, 0.1644420891530908, 0.3002032108027345, 0.334633598229625, -0.2994454299242412, 0.06317783954607803, -0.12081075992769379, 0.4857995638405911, -0.8387159049950285, -0.1612373434556025, 0.09036928205442299, -0.31368637624059276, 0.36203500302184716, 0.7964796148585543, 0.8258535138485731, -0.23219808472691605, 0.3344513576457422, -0.05844146367832216, -0.3261230783673625, 0.011843495790290298, 0.15954984623438084, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5878175497055054, 0.36784014105796814, -0.06602686643600464, 0.41867774724960327, 0.36921966075897217, 0.12810005247592926, -0.25259676575660706] - - [-0.12668620856771567, 0.5805866920972359, 0.7319462193244183, 0.7600488668058291, 0.004987782629357284, 0.03267278595231271, -0.6490249079578789, -0.2365472815942159, -0.3263037760400103, -0.0009999999999999992, 0.3872249100638452, -0.20490631056406913, -0.0009999999999999946, 0.11593800923806184, -0.1204672930640045, 0.0010000000000000085, 0.1724161418761933, 0.29287984033671294, 0.3394269737828785, -0.282162909863206, 0.05394805621437142, -0.11831375014467563, 0.4888088988709766, -0.8351390905928262, -0.15250316592092278, 0.08267971172841257, -0.3213204863460773, 0.35812177088979086, 0.792048773085682, 0.8213070829370007, -0.22705769312378465, 0.3357781820012979, -0.05973689076972814, -0.33290376019654216, 0.009553529068630946, 0.15792192420524462, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.35594525933265686, 0.48310691118240356, -0.00907235685735941, 0.39650535583496094, 0.5359171032905579, -0.14977356791496277, -0.292618066072464] - - [-0.13499480440858877, 0.5798980279258068, 0.7334274357497212, 0.7605082662746907, 0.0068536457402511445, 0.03221919567721978, -0.6484921957108233, -0.2396735708027759, -0.3247419851862151, -0.0010000000000000002, 0.37313789592569124, -0.19593892635277171, -0.0010000000000000054, 0.12146589538817391, -0.12710835865158493, 0.0009999999999999987, 0.18083206647792677, 0.28594818812903106, 0.34300225700977294, -0.26763201196046826, 0.04236642350935909, -0.11751020237303568, 0.49183805242640793, -0.8298526086676695, -0.1440051498266399, 0.07469218875490673, -0.33094848497022566, 0.3558897234620158, 0.7886428597593433, 0.8187625766052473, -0.22484171713881668, 0.3373869321593882, -0.06254336724235948, -0.34046213104023915, 0.009722837769629152, 0.15555341071639534, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3177318572998047, 0.46394744515419006, 0.17770054936408997, 0.6041872501373291, 0.47767576575279236, -0.16600222885608673, -0.3421078026294708] - - [-0.14289020890840923, 0.5792700044551793, 0.7345507415071165, 0.7611087590493189, 0.00883966115775008, 0.03133295496987086, -0.6478067329241638, -0.24231249887872747, -0.32259611722106996, -0.0010000000000000007, 0.3573859549141354, -0.18823127246996363, -0.001000000000000005, 0.1279380027463904, -0.13210985659212474, 0.0009999999999999918, 0.19006708195123187, 0.2783517158456312, 0.34665814451279703, -0.254332522143531, 0.029526913158676588, -0.11445451947175972, 0.4938832605515298, -0.8253929983162106, -0.13532981712528336, 0.06744547971453393, -0.3376460158601236, 0.3529918177385284, 0.7847628538915232, 0.8151162684559209, -0.2176697264354656, 0.33725509678539134, -0.06406864823983958, -0.3462551096464787, 0.006549875990126951, 0.15347228193475626, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46061256527900696, 0.4071749746799469, 0.01990673691034317, 0.3936096131801605, 0.5827261805534363, -0.09907320141792297, -0.2726839780807495] - - [-0.15016888367472747, 0.5787563881867103, 0.7351549116447345, 0.7619039998403461, 0.010969852043349177, 0.02983393110902721, -0.646909494386975, -0.24418853097359153, -0.3196992660753868, -0.0009999999999999998, 0.3392673255282055, -0.18233022314222347, -0.0010000000000000037, 0.13582589871356235, -0.13472755251645224, 0.000999999999999995, 0.20042403709240805, 0.2699646608755582, 0.3504843566095415, -0.2429636510416222, 0.014903239336860209, -0.10805095134815441, 0.49453766321745907, -0.8220248341633006, -0.12638089554415044, 0.0612449056713249, -0.3398707889456836, 0.3490951201825552, 0.7801722882874329, 0.8099926741734476, -0.20310885721954075, 0.3346277877083337, -0.06377921002526017, -0.34934289778466543, -0.0016273334144454304, 0.15187974105520693, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.13265928626060486, 0.3997211158275604, -0.07170630991458893, 0.4036933481693268, 0.5363414883613586, -0.30203843116760254, -0.32340526580810547] - - [-0.1571827021509813, 0.5782061032085557, 0.7358530959209086, 0.7626407704628809, 0.013103313849957469, 0.02844468160666354, -0.6460636644187854, -0.245736945946515, -0.3172039140168231, -0.0010000000000000002, 0.32078215506942653, -0.17666191660759084, -0.0009999999999999937, 0.14292478739728842, -0.13738653675281687, 0.0009999999999999953, 0.20999009858116155, 0.26217291321277697, 0.35344892267019185, -0.23274745842603825, 0.0007386534214256509, -0.10293542841744914, 0.4951537843861673, -0.8182225935645068, -0.1173471205977677, 0.05523000555873908, -0.34365133329313474, 0.3458642737730574, 0.7764582003826715, 0.8057726440212951, -0.19078105323232772, 0.3321373008614891, -0.06435740861023265, -0.352746053672492, -0.0080836962414347, 0.149893706746499, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3906507194042206, 0.5431658625602722, 0.023600518703460693, 0.4637158215045929, 0.5729851722717285, -0.14668700098991394, -0.3038277328014374] - - [-0.16384988483900104, 0.5776101401485066, 0.7366976085781171, 0.7632515091118919, 0.015113445983246354, 0.02736080040490381, -0.6453449497672802, -0.24661950432538146, -0.3154695199772899, -0.0010000000000000015, 0.30218372961150763, -0.1708052242377359, -0.0009999999999999957, 0.1489058539844738, -0.14021728803424388, 0.0009999999999999957, 0.2176964908534513, 0.2554867279086216, 0.35515182062957973, -0.22462375605607018, -0.012290618509967348, -0.09990012531409283, 0.4959985675469613, -0.8134153066793649, -0.1083192448081529, 0.04931912354090699, -0.34956936621472373, 0.34374279445374006, 0.7740988687997948, 0.8032424373729055, -0.18184592066476196, 0.3298004516185074, -0.0661222567757614, -0.35647898232831665, -0.0118633083392225, 0.14726487013828837, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5958952903747559, 0.40038105845451355, -0.48244714736938477, 0.371391624212265, 0.3613201379776001, -0.4986889958381653, -0.5326074361801147] - - [-0.1701175096876121, 0.5770509777018996, 0.7373976967391644, 0.7638279866103251, 0.01703803781689006, 0.026333047586145113, -0.6446573374615326, -0.24710260803628747, -0.31394094593544225, -0.0010000000000000007, 0.2837344081147113, -0.16538696546052598, -0.0010000000000000052, 0.15475871267463998, -0.14292700735806002, 0.0009999999999999803, 0.2245616318415895, 0.24932875185569867, 0.35677039533476484, -0.21661104582475016, -0.02546602752686233, -0.09834409756593074, 0.4971619050257683, -0.80885717889555, -0.0996408746695473, 0.04389667545750374, -0.35549471292078405, 0.3411552404183947, 0.7716505748599778, 0.8008717356729397, -0.17379444116624243, 0.32849902889000615, -0.06811565912525142, -0.3602425881516367, -0.015299238295343937, 0.14498904859955927, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.41918694972991943, 0.7504457235336304, -0.08163828402757645, 0.6894639730453491, 0.4145931303501129, -0.24311895668506622, -0.2031557261943817] - - [-0.17585526648111466, 0.5765533938133998, 0.7378850143026386, 0.7643287733456979, 0.018730358536073525, 0.025500966264869473, -0.6440499985440009, -0.2468639440474682, -0.31280755485756606, -0.0009999999999999998, 0.2659386528867595, -0.16016930127030737, -0.001000000000000013, 0.16040106666306936, -0.14557712315094806, 0.0010000000000000104, 0.2296628129641909, 0.24408834130691942, 0.35828924624089953, -0.20893530516645048, -0.03853539228872919, -0.09916233701925949, 0.49904854558084333, -0.8043237651341719, -0.09166632773586833, 0.03902349354844371, -0.36123658040126855, 0.3379007622999586, 0.7690656188730682, 0.7988286002612912, -0.1671172593285542, 0.328780333185377, -0.07043098505382574, -0.36396093896127185, -0.018197737244944022, 0.14325865840595467, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.15809349715709686, 0.5055763721466064, 0.03637063130736351, 0.40229710936546326, 0.4572201073169708, -0.11728719621896744, -0.3346266448497772] - - [-0.1811663702120102, 0.5761184482485308, 0.7384451161313542, 0.7647725155806493, 0.02034418699512441, 0.02472910475615839, -0.6435041451660669, -0.2462705441663041, -0.31195340258448934, -0.001000000000000001, 0.24873337824202124, -0.1553622306435752, -0.0010000000000000046, 0.16613620963104356, -0.1480829417382816, 0.0010000000000000013, 0.23378355441792562, 0.23941856176010354, 0.3594189635240247, -0.20215186635872948, -0.0505354095763333, -0.10040887182782039, 0.5009064495223694, -0.7997717696963935, -0.08378100390372348, 0.034593093772316875, -0.36680403559631386, 0.3350522520417689, 0.7668713201633949, 0.7980590455493213, -0.16095141676287486, 0.32911338533318424, -0.07246491094824373, -0.36753999915140917, -0.021070833372757933, 0.14209834232165944, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3100137412548065, 0.679489016532898, -0.18350957334041595, 0.3797090947628021, 0.5833578109741211, -0.08306606858968735, -0.2723802626132965] - - [-0.18589825841130556, 0.5757875998261812, 0.7391368141595678, 0.7651126695255158, 0.02175851979747362, 0.024127697270355273, -0.6430762194111924, -0.2450598050673936, -0.31156865689414726, -0.0010000000000000022, 0.2328360811747235, -0.15082498733243907, -0.0010000000000000028, 0.17199998995984764, -0.15050477727438002, 0.0009999999999999968, 0.23606462527110836, 0.23567256747097481, 0.35996146675708807, -0.19693923063394597, -0.060589651492168814, -0.10235347871616253, 0.5028745217885763, -0.79487888545744, -0.07620109069009687, 0.030653865411124578, -0.37196408882102816, 0.3329183522876397, 0.7653074415061435, 0.7994119787057111, -0.15558890650549398, 0.32953053360415585, -0.07402675956330147, -0.37087199030248785, -0.023905579238452542, 0.1418695959302794, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2211790233850479, 0.19619564712047577, -0.3043927550315857, 0.4075949192047119, 0.5865529775619507, -0.1876332014799118, -0.26631397008895874] - - [-0.1902780505955726, 0.575511639837221, 0.7397071207077222, 0.7654791508772528, 0.023126098727240866, 0.02358476890484457, -0.6426123339974134, -0.2439679850658075, -0.3114356575589139, -0.0010000000000000013, 0.2179525169431804, -0.14674797639349899, -0.0010000000000000148, 0.17758243169761326, -0.15293731413839204, 0.0010000000000000007, 0.23805438860632439, 0.2324780617702876, 0.3603567678094762, -0.19139417682349205, -0.07012329317566994, -0.10476686538469913, 0.504878331180581, -0.7903089638367625, -0.06878547399488204, 0.02708228206990804, -0.3772191044350401, 0.3305196470060946, 0.7636246657409056, 0.800277629974362, -0.1504180838435688, 0.33031091319849626, -0.07568787327197879, -0.3744634694562959, -0.026051316313645972, 0.14123333615017775, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.41232386231422424, 0.42181286215782166, 0.02131127007305622, 0.5782487392425537, 0.45704585313796997, -0.13939537107944489, -0.23560035228729248] - - [-0.19416088855642324, 0.5753223379434326, 0.7400920007421034, 0.7658838948219165, 0.024373643270812304, 0.02318192622986043, -0.6420984219435448, -0.2430840515001319, -0.31171194514588757, -0.0009999999999999972, 0.20499044605206404, -0.14317238768633786, -0.0009999999999999972, 0.18270016933411293, -0.15549571294965214, 0.0010000000000000074, 0.23948563750270055, 0.230173294057934, 0.36053131868358407, -0.18536007646885796, -0.0786801776621705, -0.10795305751958174, 0.5070371672629228, -0.7860472361448725, -0.061750219335914285, 0.023951885548519528, -0.38255519394089177, 0.3277373632332511, 0.7617498349778731, 0.8003193211169226, -0.14555113240365, 0.33162875402348213, -0.07749159486476641, -0.37844846092823164, -0.027063894017931057, 0.13989890359933696, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.539772093296051, 0.4557092487812042, -0.29145991802215576, 0.7874041199684143, 0.4224141538143158, -0.05090416595339775, -0.4481375217437744] - - [-0.19773524066186102, 0.5751673650280626, 0.7404473191265577, 0.7663208607187061, 0.02561819349212374, 0.022818525942443147, -0.6415413949725746, -0.2423919695875704, -0.3123504221242015, -0.0009999999999999963, 0.1931417557910558, -0.14004332430079738, -0.0010000000000000022, 0.18728760856507412, -0.15799645556772804, 0.0010000000000000256, 0.24086595984688114, 0.2286492230425301, 0.3605318840849515, -0.1803721663507595, -0.08657946901150777, -0.11126926012709995, 0.509214664759157, -0.7823658140739616, -0.054845982792452154, 0.020936280053192508, -0.387553366854675, 0.32597097599263336, 0.7597954624907984, 0.8012236015830255, -0.14115595152121294, 0.3333975537803413, -0.07922813122727713, -0.3823036083777099, -0.02809336563271955, 0.13966245732466886, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.30232828855514526, 0.34920549392700195, -0.1163557693362236, 0.4989991784095764, 0.5989475846290588, -0.03074485808610916, -0.2469678372144699] - - [-0.20085968370173035, 0.5750692561576752, 0.7407654868843071, 0.7668125246338611, 0.026834564838018826, 0.022536350051210182, -0.6409138562404215, -0.24204733261440925, -0.31356813092297603, -0.0010000000000000002, 0.18333835805619716, -0.13750930093632485, -0.000999999999999991, 0.19098299382706685, -0.16047988497618026, 0.000999999999999995, 0.24217693398335594, 0.22838103554138825, 0.3602620655958433, -0.17717952889672542, -0.09331399728113345, -0.11477650837528232, 0.5114697777194588, -0.7795202263029121, -0.04824773709859678, 0.017984804363095387, -0.3919569856695318, 0.32593991488064616, 0.757707807270682, 0.8035794407386853, -0.13754976556444795, 0.3359367313136381, -0.08084272480325772, -0.38594748932661427, -0.029156492185347126, 0.14130127122250105, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5204397439956665, 0.6062929034233093, -0.00892680324614048, 0.7321615815162659, 0.2768024504184723, -0.0932355597615242, -0.1536238044500351] - - [-0.2036931477686343, 0.574972598122437, 0.7412212897638547, 0.7673513201994069, 0.028093955060190533, 0.022286078875373896, -0.6402234076989565, -0.24190156147860606, -0.3149422654839981, -0.0009999999999999987, 0.1745161570785527, -0.1354963941806758, -0.0010000000000000098, 0.19405828365826086, -0.16333783698668475, 0.0010000000000000013, 0.24358908147712377, 0.22860332522649238, 0.359428979431277, -0.1749498966084256, -0.099177419890951, -0.11858269929747267, 0.5134971968960215, -0.7767754104726128, -0.04204494545863534, 0.015153691018999984, -0.3964400486911288, 0.32658317255511665, 0.7562308714328881, 0.8071513952374358, -0.13440042499338442, 0.3384938991392255, -0.08254172441849356, -0.3890586491444944, -0.0299457512388924, 0.1431042857596736, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4390285909175873, 0.7105002403259277, -0.2822950482368469, 0.958102285861969, 0.16429848968982697, -0.041812434792518616, -0.24175573885440826] - - [-0.20606373290437194, 0.5748804119012412, 0.7419221806635841, 0.7679734843941295, 0.02941780877792386, 0.022093581499487272, -0.6394241107831105, -0.2421132299834917, -0.3165713733013506, -0.0009999999999999998, 0.1674743866867572, -0.13430749548392332, -0.0009999999999999913, 0.19608267566530338, -0.16688289368020615, 0.001, 0.2452012516618235, 0.22964510700932653, 0.35764416199033333, -0.17438464137623602, -0.1035092221948357, -0.12288908372608567, 0.5151461017820215, -0.7741346804711726, -0.03656504042194431, 0.01246180578578883, -0.40105236510252257, 0.32840022073561326, 0.7558072131659865, 0.8127860245044752, -0.13203206454992306, 0.3410687289858847, -0.0843779005092828, -0.391247192130668, -0.030265594353168345, 0.14517346648185453, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4814125597476959, 0.4380142092704773, -0.09557973593473434, 0.6075800657272339, 0.4002794325351715, -0.20563678443431854, -0.11841390281915665] - - [-0.2081180943878777, 0.5747909924905584, 0.7425272509586868, 0.7686000694359423, 0.03073190475927352, 0.022151289029282144, -0.638606924239999, -0.24286538137151503, -0.31889216246746677, -0.0009999999999999979, 0.16142985608686336, -0.1333463213529464, -0.0010000000000000145, 0.19672503140392109, -0.17107072502740406, 0.0009999999999999799, 0.24696377074243167, 0.23169881031518696, 0.35565335463785785, -0.17403405319216217, -0.10680237874530539, -0.12919309657347325, 0.5171722258903646, -0.7718268141394299, -0.03218295940527388, 0.009347502875616363, -0.40647472212967894, 0.3305765203647245, 0.7550899105224589, 0.8190918104057822, -0.13117492918466334, 0.3445503480146021, -0.0871211936851015, -0.3936143347869044, -0.029580248241838067, 0.146945165277096, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40386244654655457, 0.45788657665252686, 0.1381475329399109, 0.40199989080429077, 0.8155123591423035, -0.1780775934457779, -0.2799922823905945] - - [-0.20964912050471768, 0.5747036147699419, 0.7429670311373783, 0.7692398348626815, 0.03203743284278164, 0.02263981011907818, -0.6377547478106692, -0.24459864918266894, -0.32242492654279054, -0.0009999999999999998, 0.1571718668643382, -0.13277304176633137, -0.0009999999999999907, 0.19493334887883534, -0.17640717786452764, 0.0009999999999999974, 0.24902934609215477, 0.2355330531450561, 0.35329829858159273, -0.17407507839811698, -0.10824470210557403, -0.13902801536531784, 0.5198490515467565, -0.7700659868747632, -0.029777071142873788, 0.005433141058518878, -0.413346998995438, 0.3334165860804574, 0.7538543808039805, 0.8264812375798485, -0.1329764545303895, 0.3495957068935927, -0.09146577512312255, -0.3962785462339973, -0.027126459743127412, 0.1481685922900809, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5188000202178955, 0.3649124503135681, 0.2421163022518158, 0.7672780752182007, 0.7620994448661804, -0.2150706946849823, -0.23622603714466095] - - [-0.21106390615160422, 0.574700807258152, 0.7433795946741214, 0.7699774745174506, 0.03336025320964048, 0.02273744419740044, -0.6367925807300892, -0.24624682816674023, -0.3250325863854919, -0.0010000000000000009, 0.15304049036683237, -0.13286553891936023, -0.000999999999999995, 0.19279311741303506, -0.18113755472717413, 0.0010000000000000122, 0.2521370607031587, 0.23831794480100765, 0.35087256487078616, -0.17482691748802973, -0.11032100894662276, -0.14679026766682324, 0.5222582137299614, -0.768295553552877, -0.027634199642080907, 0.002205952677652072, -0.4196902313482333, 0.3359762871221483, 0.7526846446856806, 0.8343730546290161, -0.1329923489998158, 0.3533917788929362, -0.09507486488316666, -0.39858978566515413, -0.02548710364200107, 0.14909660029136582, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.42548075318336487, 0.49406227469444275, -0.08828715234994888, 0.43875652551651, 0.3628089427947998, -0.030956679955124855, -0.25648054480552673] - - [-0.21224553977731034, 0.5748526998421861, 0.7437444070321024, 0.7708926691413036, 0.034700796781637705, 0.022126044556909023, -0.6356341601260755, -0.2477091747520527, -0.3259363522459431, -0.0009999999999999998, 0.149125529716888, -0.1341715705138161, -0.0009999999999999885, 0.19004614691149224, -0.1848165773518931, 0.000999999999999994, 0.2571396890797023, 0.23915506231129693, 0.3483567292891906, -0.17675348658458664, -0.11356739112402608, -0.15075252953298246, 0.5242298551953514, -0.7665729950318575, -0.025933166533969723, 0.00026527489106255775, -0.4250564364315563, 0.3379872481451268, 0.7516329947904781, 0.8431909868360551, -0.12979520307093367, 0.35497823352589125, -0.09734992760187468, -0.4003335447513552, -0.025314411266541707, 0.14951077615449399, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6193217039108276, 0.5021198987960815, -0.13339169323444366, 0.5469133257865906, 0.5027331709861755, -0.1859203726053238, -0.17823302745819092] - - [-0.21333580909017358, 0.575028224860998, 0.7440537967296825, 0.7718092166318927, 0.03600564024775353, 0.02147701626118551, -0.6344705389259055, -0.24923525021755627, -0.32680374395752376, -0.0009999999999999996, 0.14565387365590238, -0.13557858026378228, -0.0010000000000000009, 0.18682655778312227, -0.18822345283557154, 0.0010000000000000007, 0.2625338754445692, 0.23992429839852958, 0.3456227482560996, -0.17944115181122341, -0.11664706861487616, -0.15408339972308221, 0.5260811842264376, -0.7648891321520843, -0.02495672437258239, -0.001558249530942731, -0.430236682363676, 0.34035659440314137, 0.7509348038015533, 0.8518434340781235, -0.12616277012292312, 0.35582781186344875, -0.09971938353712353, -0.4016124386162636, -0.02528942634210906, 0.14977800546540063, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4840320348739624, 0.5863711833953857, -0.02744484879076481, 0.699508011341095, 0.3803307116031647, -0.19361820816993713, -0.25176194310188293] - - [-0.2142637223962827, 0.5752453687491016, 0.7442593908293544, 0.7727324921397679, 0.03724080991845818, 0.02074912314189139, -0.6332985801001969, -0.25086786047067944, -0.3276053810285823, -0.0009999999999999994, 0.14300503583453586, -0.13717342919346084, -0.001000000000000004, 0.18273067578463728, -0.19114015304347123, 0.0010000000000000015, 0.268630606730675, 0.24056552907064932, 0.342501061664294, -0.18351535784457307, -0.11940736336383524, -0.1562347109536099, 0.5277178111218841, -0.763279201149969, -0.025333601684270384, -0.003175649402487863, -0.4350718071173225, 0.34339867646998423, 0.7508942584663993, 0.8601371894102191, -0.12172040307202199, 0.35530197168707706, -0.10226390540029975, -0.40202903384892047, -0.025537498800979334, 0.14976169153437674, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3503835201263428, 0.5573931336402893, 0.010655591264367104, 0.4510197639465332, 0.501114547252655, -0.30450883507728577, -0.3189675807952881] - - [-0.21509458360777164, 0.5754821994897805, 0.7444479690219812, 0.7736235818774609, 0.03847593238777783, 0.01998619456189058, -0.6321603500828828, -0.2525244832068224, -0.32835739297057753, -0.001, 0.1405085150194936, -0.13896028366560176, -0.0009999999999999983, 0.1784260655133623, -0.19386814504488006, 0.0009999999999999961, 0.27498118450110204, 0.24103626724357524, 0.3392583195738976, -0.18724137463321042, -0.12180256934301167, -0.15789098412658903, 0.5292098793220072, -0.7617744771010583, -0.02594046091623506, -0.004602589549454714, -0.4400357245608576, 0.34633948465366726, 0.7512000813340511, 0.867922014510388, -0.11782349731818881, 0.35491775812405485, -0.10473730681972457, -0.40239305080499166, -0.02589805688219375, 0.1501308015324478, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5584325790405273, 0.5525352954864502, 0.000600503699388355, 0.5607250332832336, 0.42111414670944214, -0.05365103483200073, -0.16674216091632843] - - [-0.21574090925962205, 0.5757555903281656, 0.7446011635557638, 0.7744564494231919, 0.03970386520415416, 0.019152035810847453, -0.6310895424256154, -0.25421826655012214, -0.32902105337282994, -0.0010000000000000024, 0.13831244488387928, -0.14110773485399664, -0.000999999999999984, 0.17371879031337537, -0.19624432726695162, 0.0009999999999999957, 0.28179206252354855, 0.24118780696220393, 0.33579591286584215, -0.19030471920019168, -0.12349383000134519, -0.15860195081055894, 0.5304427097736041, -0.7604888796582773, -0.02698023597645895, -0.005667790280954896, -0.4452331972665971, 0.3490978978678788, 0.752165689514104, 0.8747193307223177, -0.11496050999396426, 0.3548038253488175, -0.10706836650988459, -0.4026563658097435, -0.026481729098996383, 0.15124822081994083, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.43242794275283813, 0.41363590955734253, -0.10878012329339981, 0.7220767736434937, 0.5476739406585693, -0.2414834201335907, -0.21665377914905548] - - [-0.21629271389902185, 0.5760528761316446, 0.744696215101907, 0.7752239632488479, 0.040886585482445455, 0.018277439996418045, -0.6300968410645399, -0.2559277582670307, -0.3294866893167583, -0.0009999999999999983, 0.1362212065084387, -0.1433391283945664, -0.0010000000000000085, 0.1689925159709063, -0.1983477432545796, 0.0009999999999999907, 0.2890128961549334, 0.2409869841719389, 0.3326890138490473, -0.1920628208148085, -0.125162125124028, -0.15886717339311512, 0.5315713061189272, -0.7593208373009864, -0.02811124786506067, -0.006688127883934938, -0.4503281198123194, 0.3513767045373966, 0.7530464985130344, 0.8804623105176402, -0.11220728096611984, 0.354990400373925, -0.10951704020882631, -0.4029505513659528, -0.027096386185843705, 0.15274625800544758, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.35833898186683655, 0.34658682346343994, 0.013208452612161636, 0.5826573967933655, 0.375459223985672, -0.17363813519477844, -0.3088071346282959] - - [-0.21665939529022915, 0.5763944768763155, 0.7446758457507622, 0.7758666268654374, 0.041970910315621134, 0.01732221294497814, -0.6292609641018705, -0.2576611049187851, -0.3295732819765361, -0.0009999999999999992, 0.13436008014654932, -0.14571985852685151, -0.0010000000000000132, 0.1642246983693721, -0.19992361875974132, 0.00100000000000001, 0.297012890390707, 0.24011272222263905, 0.33028636283651785, -0.19126993180805077, -0.12677397683528152, -0.15826800811748723, 0.5325226960708225, -0.7583960898706226, -0.029410285285040312, -0.00762202993979148, -0.45520485826606816, 0.35272840156237856, 0.7537627733466539, 0.8841296184285788, -0.10966691270215279, 0.35576475290138243, -0.11219207391401895, -0.4033030905866908, -0.027775264778573495, 0.15500841505710633, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.24362793564796448, 0.7208894491195679, 0.011458830907940865, 0.4334750771522522, 0.21711081266403198, -0.019946862012147903, -0.3684675991535187] - - [-0.2169857424013847, 0.5767512255972742, 0.744603357428156, 0.7764550288877176, 0.04300803110247475, 0.016334247851266602, -0.6284911214351376, -0.2594704834237788, -0.32936520172580913, -0.0010000000000000013, 0.1325650789248982, -0.14812153238179845, -0.0009999999999999972, 0.15958521719519186, -0.20137174298140406, 0.000999999999999974, 0.305346963098281, 0.23875564269489777, 0.32810474257741, -0.18957266789985436, -0.12819100321562737, -0.15735190131027124, 0.5334962749657081, -0.7576001153182014, -0.030814395940899297, -0.00851835943642751, -0.46015670662250097, 0.35382586821275464, 0.7543062149438132, 0.8869222103528148, -0.10737195533768708, 0.3566275365799428, -0.11472290038875449, -0.4040109072807846, -0.02845422485674232, 0.15751981392369208, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.444545716047287, 0.4979526996612549, -0.14794960618019104, 0.4177856147289276, 0.40302062034606934, -0.21358707547187805, -0.16222582757472992] - - [-0.21723202933404565, 0.5771388453775136, 0.7444246946068066, 0.7769375622016167, 0.043947002327771566, 0.015280567139000419, -0.6278560262468794, -0.2614205314013243, -0.3285884203634506, -0.0009999999999999972, 0.13092252555835884, -0.15056177501283508, -0.000999999999999997, 0.15519669713287154, -0.2025652213892072, 0.000999999999999987, 0.31429637896897344, 0.2364523283951439, 0.32636271473261735, -0.1860653855807954, -0.12918477171987566, -0.15580791261734675, 0.5345308167580112, -0.757058131686128, -0.032411422319132605, -0.00934159573458021, -0.46524793734802605, 0.3544176568912723, 0.754504196742079, 0.8879496043430544, -0.10556470195325893, 0.35766111279322155, -0.11696112417064804, -0.4054266033174117, -0.02914006100936868, 0.16052302162352913, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3616107106208801, 0.03560412675142288, 0.20368057489395142, 0.3988410234451294, 0.41832461953163147, -0.19966475665569305, -0.4273531436920166] - - [-0.21748064784652438, 0.5775145765061559, 0.7442286162431456, 0.7773823038359804, 0.044837570148108616, 0.01422537367857932, -0.6272670760765169, -0.2635733272465873, -0.32765292858293815, -0.0009999999999999983, 0.1292722802470387, -0.1529375925606037, -0.000999999999999986, 0.15077699835258, -0.20367346424014773, 0.0010000000000000005, 0.32347437036871707, 0.23400688555809357, 0.3247737418763245, -0.18239255154821168, -0.12999909231838555, -0.1541894265842262, 0.535523627732501, -0.7565100534296042, -0.03387353054821366, -0.010285046285998292, -0.4701363354481244, 0.3549529274413199, 0.7546901754361931, 0.888516832628437, -0.10366437028021624, 0.3587347995103926, -0.11919833746465595, -0.4069346625991211, -0.029910383048295807, 0.163462986380733, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5872864127159119, 0.3633207380771637, -0.1769903153181076, 0.6201866865158081, 0.8556017279624939, -0.14121177792549133, -0.14533530175685883] - - [-0.2177326010129763, 0.5778745232418419, 0.7439975590185335, 0.7777531357767837, 0.04562924773136625, 0.013162849837730377, -0.626773301062673, -0.266112901527407, -0.3264020321461675, -0.0010000000000000002, 0.12760043813098232, -0.15519301710013778, -0.0010000000000000024, 0.14629348816115806, -0.2046021111985003, 0.000999999999999995, 0.3330132225085994, 0.2312515870435808, 0.3235004961676446, -0.178416060526828, -0.13037204934377913, -0.15239418423616627, 0.5364395927881848, -0.7559427943549022, -0.03503276326668562, -0.011473415581686915, -0.4746068191478589, 0.35538402196510893, 0.7548519844645032, 0.8881580388282035, -0.10157009773892942, 0.35987280120304443, -0.1214316324727822, -0.40860835428139813, -0.030855911180139323, 0.16624039222836484, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.33714550733566284, 0.5900288820266724, -0.21839116513729095, 0.367006778717041, 0.3734999895095825, -0.20869676768779755, -0.31603556871414185] - - [-0.21797073100061143, 0.5781840240059654, 0.7437589856761546, 0.7780563185513978, 0.0463610065006755, 0.012117246474430979, -0.62636426668223, -0.2688134685657199, -0.32502681294024993, -0.000999999999999998, 0.12581957893364026, -0.15730974260532402, -0.0010000000000000059, 0.1418136326379577, -0.20538209101458796, 0.0010000000000000306, 0.3425366123826188, 0.22850737725352813, 0.3223201000134687, -0.17474845344113615, -0.13085882942630114, -0.1505798665295559, 0.5372959494293862, -0.755440865564815, -0.03618433315756936, -0.012623869887764769, -0.47890394677992393, 0.3557848842066435, 0.75506808840802, 0.8878622977175978, -0.09968470284731044, 0.3610996285483464, -0.12349473324383384, -0.4100809380531008, -0.03177436280001653, 0.16913122987246787, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4595901370048523, 0.38019272685050964, 0.03064659982919693, 0.6417443752288818, 0.5888263583183289, -0.23084397614002228, -0.14400310814380646] - - [-0.21817546277600205, 0.5783977157117023, 0.7435075881354042, 0.7782205697101403, 0.04696607317423085, 0.011099514763137322, -0.6261339582091359, -0.2718099278779424, -0.32338147450327315, -0.0010000000000000009, 0.12380368138206281, -0.15915156739684144, -0.0009999999999999937, 0.13735043057170657, -0.20584143157711557, 0.0010000000000000057, 0.351907311921056, 0.22574536331309647, 0.32134024909713177, -0.171773511484571, -0.1315056658729816, -0.14868599393607346, 0.5380290827432437, -0.7550821543747719, -0.03727789651825648, -0.013689240071585592, -0.482839513360918, 0.3561328331233319, 0.7553991354064371, 0.8877341513253555, -0.09823440750786926, 0.3625110827009732, -0.12519508934302065, -0.4111135155363799, -0.0326405512842191, 0.17224635282776748, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.378512978553772, 0.2701737582683563, -0.08694326877593994, 0.8666203618049622, 0.4128510355949402, -0.051204681396484375, -0.2299373596906662] - - [-0.2183852716971848, 0.5785533866375949, 0.7432120781296262, 0.7783317386436378, 0.047493741519569845, 0.010134153515317278, -0.6259723221269222, -0.27508784194722957, -0.32164942334934965, -0.0009999999999999955, 0.12154688368163045, -0.16076238492823638, -0.0009999999999999885, 0.13259010030831336, -0.20621918153475696, 0.000999999999999983, 0.36099519555196585, 0.22304512695014836, 0.3204488949171518, -0.169459461563922, -0.13253747163746407, -0.1469725518475664, 0.538824839114682, -0.754635588972117, -0.03843850231318673, -0.014876658429609094, -0.48653059971389745, 0.35639253582471087, 0.7557336447284054, 0.8877944961027934, -0.09682067461434371, 0.36369254137883933, -0.1268874958789195, -0.4119857564581815, -0.03335905750258215, 0.17455420699254726, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5062863826751709, 0.621429443359375, -0.19734619557857513, 0.5185531973838806, 0.28047168254852295, -0.07218588888645172, -0.32234299182891846] - - [-0.21860560687153266, 0.5785900774196714, 0.7428243784632728, 0.7783315599076492, 0.04785612859291068, 0.009272509843284395, -0.6259583008228492, -0.27891699015502414, -0.3197048982811534, -0.0009999999999999966, 0.1187506903798963, -0.1618896186814254, -0.0009999999999999944, 0.12719953447601715, -0.2064139816985596, 0.0009999999999999861, 0.369342991298936, 0.22042278838222298, 0.319754990407937, -0.1685839852169784, -0.13432404110086635, -0.14561014731532676, 0.5397511561550676, -0.754005456566241, -0.039711726242509404, -0.01632605043935744, -0.4897038222366825, 0.3564677886444919, 0.7560760437661096, 0.8882699648667799, -0.09548269389572442, 0.3643710566206781, -0.12855965983427253, -0.4125170937343078, -0.03376080962242072, 0.17509768035353812, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.642845630645752, 0.5591713190078735, 0.28550776839256287, 0.6246072053909302, 0.5615995526313782, 0.16002260148525238, -0.2485101819038391] - - [-0.21881102372359515, 0.5785267524946678, 0.7424499548706439, 0.7782959397366208, 0.0481299375588453, 0.008502639171824198, -0.6259925274531533, -0.28294657948587815, -0.3176413926601351, -0.0009999999999999957, 0.11557988222447478, -0.16273561983737284, -0.0010000000000000013, 0.12149473462094586, -0.2066145411630481, 0.00099999999999998, 0.3768932935514092, 0.217853690521084, 0.31924225312233856, -0.16828966341908946, -0.13640403563525905, -0.14433249290529068, 0.5406099920074314, -0.753218590183, -0.04114616420472301, -0.017605989575548587, -0.4922606331585482, 0.3565903900737757, 0.7564140505754785, 0.8889811841580567, -0.09395543496775094, 0.36509064399408847, -0.1300230602004703, -0.41254960109608474, -0.03394428795211071, 0.17513346198340105, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3875086307525635, 0.42708468437194824, 0.017038723453879356, 0.3905268609523773, 0.5853323936462402, -0.1523910015821457, -0.2534206211566925] - - [-0.2189838197762376, 0.5782383735870888, 0.7421076868076594, 0.778183378669266, 0.0482085711385252, 0.00792838835942928, -0.6261339341464326, -0.28737017975130325, -0.31527769659914634, -0.0010000000000000022, 0.11159292656839656, -0.16296081146847508, -0.0009999999999999963, 0.1150863992065569, -0.206822740927185, 0.0009999999999999918, 0.3825844805285141, 0.21535325223343144, 0.31913923429285856, -0.16926925622526748, -0.13909056428602856, -0.14322377994520755, 0.5413127523698918, -0.7520927746154262, -0.04289782295048948, -0.01851077276028733, -0.49345704400375856, 0.35681724871320414, 0.7567425679344099, 0.8902029033946635, -0.0920057362674218, 0.36590448158143196, -0.13102168431693556, -0.41148412139565843, -0.03364527522463056, 0.17404700639616655, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.28610482811927795, 0.36359062790870667, 0.05500851571559906, 0.013300135731697083, 0.457319974899292, -0.2914963960647583, -0.20339499413967133] - - [-0.21916277024739597, 0.5778525585321417, 0.7417540466983896, 0.7780244055723368, 0.04815420997975294, 0.00748639124575435, -0.6263410814733069, -0.29180169104125103, -0.3128459132336272, -0.001000000000000001, 0.10740413589566727, -0.1626953263722671, -0.0010000000000000002, 0.1086494253723083, -0.2070007381669225, 0.0010000000000000035, 0.38688267368941764, 0.21294314077637552, 0.31930668802302214, -0.1702053084779517, -0.14142246084131208, -0.142408337005444, 0.5420388006947223, -0.7508745811488206, -0.044817365416599045, -0.019401168614326364, -0.4942169693746209, 0.3572364448898778, 0.7570186144209158, 0.8911908026238607, -0.09012005239777587, 0.3670260900998905, -0.1319538461159923, -0.41070112208318416, -0.03305042754531987, 0.17289750953264543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3580537736415863, -0.026889625936746597, 0.34946128726005554, 0.5317650437355042, 0.06241375952959061, -0.4075752794742584, -0.10208185017108917] - - [-0.219356141326489, 0.57723488621266, 0.7413745935694932, 0.77775770280354, 0.04779349534673868, 0.007341699087675443, -0.6267015533623701, -0.2962316244300087, -0.3102235986823106, -0.0009999999999999944, 0.1028054988789203, -0.16129890325186375, -0.001, 0.10211157310824039, -0.20710018732369595, 0.000999999999999993, 0.3879846219967296, 0.210711039463024, 0.3201014374882786, -0.1710142333352557, -0.1429424209353017, -0.14226035896115807, 0.5428148704325235, -0.7494505461788289, -0.04709138373147462, -0.02026200131539447, -0.4939772361931361, 0.3580995262150809, 0.7571756765609691, 0.8916188732092877, -0.08837968782628594, 0.36884503420518394, -0.13273231876990096, -0.41056813746126425, -0.03178357762997455, 0.17161205777435032, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3194640576839447, -0.1330048143863678, -0.027054402977228165, 0.4875822961330414, 0.01712772063910961, -0.32797905802726746, -0.22415509819984436] - - [-0.2195547640466666, 0.5765048350684775, 0.7410412378157305, 0.7774176468746313, 0.04727411747385332, 0.007347001816766302, -0.6271626437450528, -0.3004893618057809, -0.3075391522336185, -0.0009999999999999972, 0.0981975651469069, -0.15927659555487203, -0.0010000000000000078, 0.09579362219469688, -0.20705679476918157, 0.001000000000000008, 0.387494280145058, 0.2085631641200681, 0.32133783523886117, -0.1708465622731165, -0.14380946710899092, -0.14220900507485829, 0.54360616166794, -0.7481332582330321, -0.04935630559593361, -0.021148718523943642, -0.4930361760309619, 0.35912310014631904, 0.7572116101141996, 0.89081759575332, -0.08663132190778436, 0.3713501946553633, -0.13342883383795637, -0.410890752623198, -0.030322022336912537, 0.17065901736573372, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2438219040632248, 0.1463942974805832, -0.1048196479678154, 0.07426423579454422, 0.06569000333547592, -0.13036715984344482, -0.032515089958906174] - - [-0.2197621780592169, 0.5755082414805246, 0.7408101763092789, 0.7769022183241292, 0.046379150092723864, 0.007705479420930072, -0.6278634749582256, -0.30438661125663835, -0.3047102330861587, -0.0010000000000000028, 0.09360442324362371, -0.15577628563898932, -0.0010000000000000046, 0.08995381175901658, -0.20667235859154334, 0.0009999999999999994, 0.38332596340033936, 0.20662402392178514, 0.32361297889331697, -0.16838962500568294, -0.14316076800299718, -0.14239259661318293, 0.5444356575165583, -0.7470819773535149, -0.05158352744442337, -0.022096102251770965, -0.49044126309708935, 0.3605369333359035, 0.756964398158186, 0.8871336031035251, -0.08486554981372772, 0.3754715015372638, -0.13393499968757586, -0.4122887600665675, -0.028407119759496488, 0.17048987013365177, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17834128439426422, 0.15755265951156616, -0.08150579035282135, 0.0, 0.0, -0.2606666684150696, -0.12234604358673096] - - [-0.21995708655521648, 0.5744020417708572, 0.7406103040560791, 0.7763142600224732, 0.04529656751700465, 0.008227552301710293, -0.6286626265653632, -0.30791867100319104, -0.3018600708343353, -0.0009999999999999957, 0.08909725837432651, -0.1516259812206979, -0.0010000000000000013, 0.08412200236023333, -0.20619206844859364, 0.0010000000000000015, 0.3773988947508264, 0.20489027780338762, 0.3262234116254022, -0.16459675630165096, -0.14184224595354122, -0.14276690867479774, 0.5452614663996257, -0.745856275450267, -0.05399853789550406, -0.023212994855043745, -0.4872898050050432, 0.36131844663074, 0.7567904588699572, 0.8820251679985495, -0.08349005117843485, 0.3801850382451727, -0.1344395564811651, -0.4138722936315112, -0.026301403324303585, 0.17000652698858187, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.21810923516750336, -0.11061020940542221, 0.12776292860507965, 0.04663792997598648, 0.0, -0.30800101161003113, -0.31837114691734314] - - [-0.22011595596553085, 0.5730403096187302, 0.7404775038140469, 0.7755503452360512, 0.04376050188231084, 0.00914866023624799, -0.6297007086665327, -0.31064008398682497, -0.29899476291958255, -0.001000000000000002, 0.0848044885767683, -0.1459150424187117, -0.0009999999999999972, 0.07829706775261323, -0.20548377273222618, 0.001000000000000004, 0.36729402171279396, 0.20368935523931497, 0.3296342075629603, -0.1576420167065732, -0.1389157393538128, -0.14360528582911664, 0.5460905110258268, -0.744200139637086, -0.0568652022633151, -0.0247269372440628, -0.4827893365431065, 0.36059485210057224, 0.7567924778315169, 0.8735414547710111, -0.08305373906949474, 0.3863426413974255, -0.13494482809695324, -0.4159067128188649, -0.02373887862175609, 0.1687825954725205, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.08967452496290207, 0.42029324173927307, -0.2026807963848114, 0.0, 0.32021912932395935, -0.05994643643498421, -0.027339909225702286] - - [-0.22028843989744473, 0.5715385991438648, 0.7403779494275946, 0.7747020463854795, 0.04203597572341275, 0.010200906092661092, -0.6308451930434446, -0.3127341468433803, -0.2960447363545417, -0.0010000000000000013, 0.08076024420469924, -0.139545292934704, -0.0009999999999999905, 0.0723632023844327, -0.20470137691947157, 0.000999999999999978, 0.3554913055948884, 0.20275447960303045, 0.3332924242742613, -0.14991447703002422, -0.1352228277716374, -0.1447160978212318, 0.546933958112271, -0.7420421935168755, -0.0601195669426902, -0.02621044020191076, -0.4777336165845342, 0.35949132939272743, 0.7569735051849384, 0.8642396418057271, -0.08300083120606523, 0.39274080964283653, -0.13535122848002562, -0.417885138077753, -0.02093338600008966, 0.1673960920091696, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.012466778978705406, 0.29738566279411316, 0.0496203675866127, 0.016210773959755898, 0.3461754024028778, -0.2420661747455597, -0.007943837903439999] - - [-0.22046970139770727, 0.5697344389397909, 0.7403545576998731, 0.7736552402722887, 0.03986162479720228, 0.011575507024413526, -0.6322457020058598, -0.31336978809659055, -0.2929844491324231, -0.001000000000000001, 0.07721494366419904, -0.13164115920597733, -0.001000000000000004, 0.06620112450614543, -0.20374232630000713, 0.0009999999999999992, 0.33945612446709633, 0.2025161182627662, 0.33752556203501677, -0.14040583926131275, -0.12954623147101105, -0.146448120849336, 0.5478112954064364, -0.738683088270416, -0.0643110964506457, -0.02757171434050515, -0.47133681345837997, 0.35742553925935155, 0.7575968200815655, 0.8530529154221608, -0.08389779219021662, 0.39980050394334044, -0.13551936281204516, -0.41971637803544193, -0.017530042760227574, 0.1656530708092699, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.26821479201316833, 0.3935849070549011, 0.061330538243055344, 0.5341399908065796, 0.016164736822247505, -0.4154812693595886, -0.3059277832508087] - - [-0.22065086354973626, 0.5677998576119808, 0.7403086236268202, 0.7725200639248493, 0.037438116452505124, 0.013054105474482871, -0.633751314476189, -0.31328302922095924, -0.2896655152252214, -0.0010000000000000009, 0.07422708445047276, -0.12299636281535538, -0.0010000000000000076, 0.0599034995140995, -0.2026187020602957, 0.000999999999999999, 0.32237873620106144, 0.20245974903893305, 0.34212319026920074, -0.13040553978756053, -0.12352247998356757, -0.14854605707398183, 0.5486244319401861, -0.7348196292674936, -0.06866049093052909, -0.029171448678115136, -0.46419427926604506, 0.3552601940048667, 0.7581648598545879, 0.8412071540250827, -0.0850809433552269, 0.40723109428025206, -0.13553729170412526, -0.4215554420967501, -0.013959887710160435, 0.16397602372871203, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.004438521806150675, 0.38535425066947937, 0.045771561563014984, 0.04724656045436859, 0.026866035535931587, -0.23526744544506073, -0.15503624081611633] - - [-0.22077378250259885, 0.5656384309942958, 0.7402061350334533, 0.7711912134040243, 0.03444198801633786, 0.014785306834349431, -0.6354992183565032, -0.3113870466936308, -0.2859079553555732, -0.0009999999999999983, 0.07231246315272438, -0.11274301258986348, -0.0010000000000000117, 0.05339132394785411, -0.2010888360187299, 0.0009999999999999864, 0.3021885215081139, 0.20288062322136569, 0.34753951747774814, -0.1193995518791201, -0.11620568618438214, -0.1514086129368577, 0.549225094843611, -0.7298251732669319, -0.07335095561848465, -0.03125971152500368, -0.45526295492464214, 0.3527060720622348, 0.7585936307986993, 0.8279175152490954, -0.08699162152398257, 0.41568707977536623, -0.13517591452131136, -0.4233390623638149, -0.009965168831967547, 0.1624978521489289, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03984205052256584, 0.13271503150463104, -0.36997494101524353, 0.34222131967544556, 0.05212089791893959, -0.17622950673103333, -0.0051981015130877495] - - [-0.2208978599451343, 0.5633962125347566, 0.7401580428535428, 0.7697643891465452, 0.03117115751964274, 0.016549293392864584, -0.6373517592577346, -0.3087915239923566, -0.28179961238990336, -0.0010000000000000007, 0.0712025834825897, -0.1017881276831847, -0.0009999999999999963, 0.04692179954360282, -0.19929624144914446, 0.0009999999999999812, 0.2819257836024548, 0.20352850733689767, 0.3528757350900492, -0.1076976117780991, -0.10881621973144309, -0.15442361058158274, 0.5493416340990076, -0.7246107128714468, -0.07813260859615763, -0.03362886923034614, -0.44600542925404757, 0.35012345264655476, 0.7597350751333483, 0.8139355595270824, -0.08940363865866804, 0.4246603982996134, -0.13483904695784762, -0.42507900814356564, -0.005720584744124744, 0.1616942691249834, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2340935468673706, 0.2838091254234314, 0.14383907616138458, 0.29653993248939514, 0.0016301018185913563, -0.2952231764793396, -0.1245737075805664] - - [-0.2209293279507458, 0.5611054958415153, 0.7402547704241029, 0.7681248351952041, 0.02727519218214428, 0.01839888483106472, -0.6394542849060758, -0.30420222130909197, -0.2771601237104795, -0.0010000000000000013, 0.07167838731985123, -0.08939658662823959, -0.0010000000000000128, 0.04075138371101375, -0.1968312774544735, 0.0009999999999999937, 0.2602769481505375, 0.20479325831084538, 0.35783879073036334, -0.09459854374746435, -0.1003530903753659, -0.1575745625389876, 0.5481687756759864, -0.7190368786929713, -0.08307954295591649, -0.036549480916756916, -0.4360249936781271, 0.34723148311906266, 0.7627389321572778, 0.79854341297888, -0.09312412903083454, 0.43506631712174026, -0.13456880916149122, -0.4265222306334401, -0.0008265392393854793, 0.16264402445828133, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11833060532808304, 0.28058937191963196, 0.24955600500106812, 0.1883169412612915, -0.007465505041182041, -0.3314324915409088, -0.0314331129193306] - - [-0.22091315379651702, 0.5588690382233034, 0.7403323985555817, 0.7664028445453461, 0.02312458646556084, 0.020152167062627093, -0.6416274803468741, -0.2991155337372257, -0.27183658831832114, -0.00044540169349374203, 0.07285996631916444, -0.07665342253384687, -0.000999999999999997, 0.03481762975449989, -0.19423589579830172, 0.0009999999999999918, 0.23961355728458006, 0.20568419574669208, 0.3622878105581732, -0.07935799849009549, -0.09224679905400315, -0.16086602551652415, 0.5468069180773958, -0.714010059020241, -0.08796870252600149, -0.03955136346031226, -0.42673472518995675, 0.34321242585301903, 0.7661914663490322, 0.781944926660928, -0.09718622822940585, 0.44636070501671715, -0.13427697232709754, -0.4288974230166474, 0.0040120323899502045, 0.16425326247425515, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3352317810058594, 0.13051696121692657, 0.16685238480567932, 0.6061336994171143, 0.0, -0.4661073088645935, -0.07021920382976532] - - [-0.22063110864688598, 0.5569429025553353, 0.7403604003061482, 0.7645401322564853, 0.01838194834229763, 0.02172851348639126, -0.6439474837640481, -0.29232077809406964, -0.2652435087962121, 0.0009999999999999987, 0.07543435536672613, -0.0633436882582239, -0.0009999999999999946, 0.02959811745869483, -0.19142744378522145, 0.0009999999999999803, 0.21961342032189143, 0.2056907882103655, 0.3654277872398724, -0.05950715756408199, -0.0837804932235514, -0.1643298502871139, 0.5448297312606285, -0.7104504424819325, -0.09261939324599738, -0.042533137371262957, -0.4188274160831172, 0.33634426824381525, 0.7708743445093542, 0.7628108734650291, -0.10216028460813037, 0.4600391266819735, -0.13391971788581755, -0.43336836753663033, 0.008718618990975594, 0.16757591146822956, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.22024481075985902, 0.5551301441444577, 0.7404469882117641, 0.7627310257004455, 0.013614838019530943, 0.023199879640270822, -0.6461561608499349, -0.285463186984868, -0.25821455147119654, 0.0009999999999999994, 0.07820442812821089, -0.05037327450568, -0.0010000000000000057, 0.02472135105114694, -0.18862755039457915, 0.0009999999999999905, 0.20095917655079634, 0.20518549824054177, 0.36854391460702207, -0.038666501246219516, -0.07570126959479705, -0.1676064039218873, 0.5418697291047576, -0.7076551167287213, -0.0964517361741405, -0.04536711466769012, -0.41084409257167864, 0.3296173573867082, 0.775980816545132, 0.7424971880042541, -0.10726163207420347, 0.47483956613153316, -0.13355962612211378, -0.4385766788285575, 0.013474102360003704, 0.17188603205913347, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2194361691160154, 0.5538455770967503, 0.7406666884957317, 0.7609190193734579, 0.00866838823386265, 0.02437068302485457, -0.6483310688298569, -0.27764070422981924, -0.250333716071023, -0.0010000000000000018, 0.08164426356521978, -0.03814694917967515, -0.0009999999999999894, 0.021394386837500147, -0.18545036711357205, 0.001000000000000021, 0.18435459984669864, 0.20331146383238158, 0.3711025092143305, -0.014068299955338237, -0.0676623123311297, -0.16998404433406006, 0.5368716112067351, -0.7074469517703252, -0.09802018665698961, -0.047658214900796185, -0.40389030323945374, 0.3213536089003633, 0.7822823858957938, 0.7194678304296156, -0.11270148388359194, 0.49300794424620553, -0.13317423385904725, -0.4457562544100027, 0.018391970096237692, 0.17919436857066215, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2185068777929622, 0.552859209648923, 0.7409984883360076, 0.7589036943213358, 0.0038131610087127803, 0.025245895967947428, -0.6507021494396075, -0.2696553761895126, -0.24221062688728906, -0.0010000000000000009, 0.08525907666485502, -0.026411518787522507, -0.0010000000000000098, 0.0197954563553315, -0.18119388150292356, 0.0010000000000000009, 0.16934493580638318, 0.2005793397169377, 0.3725995791595514, 0.015690539794069126, -0.06056908389790847, -0.17125511566000565, 0.531790773009127, -0.7078210718782576, -0.09831689674666212, -0.04968804575721268, -0.3997018309878347, 0.3096896464302922, 0.7888934468612786, 0.693982875534781, -0.1182267310507387, 0.5112018652431249, -0.13273839419956474, -0.45471549172916054, 0.02339689681569468, 0.18657407944445306, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2171522717198078, 0.5525894343827754, 0.7416159230156514, 0.7567211022552303, -0.0007450295414929539, 0.02563226369021236, -0.6532347245751151, -0.2612890735815916, -0.2340455518087036, -0.0009999999999999998, 0.08916686151827331, -0.0162708964330284, -0.0010000000000000026, 0.02192952212067967, -0.1752448678994605, 0.0009999999999999918, 0.15733666571138552, 0.19621807889217183, 0.37296221833909265, 0.055121788134626884, -0.05530393456780958, -0.17015094655585195, 0.5252354225499938, -0.7097487471265058, -0.09507808814574312, -0.050840457004049856, -0.39876501195997105, 0.2928559246720182, 0.7964245794291293, 0.6631339548414452, -0.12395632594076997, 0.5297285078942058, -0.13214240234304114, -0.46866278023718794, 0.02870426938571235, 0.19458646501184992, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2155476486599784, 0.5524826828792984, 0.742309534086467, 0.7547097523567228, -0.004936866313695027, 0.025679709219399487, -0.6555374661932462, -0.2529537937900153, -0.22556707359581346, -0.0010000000000000005, 0.09198999150556252, -0.007804674407132062, -0.000999999999999998, 0.025111672746633396, -0.16889019418322354, 0.0010000000000000022, 0.14653490957786677, 0.19103263111345734, 0.37282739569312623, 0.09548418691534596, -0.05204976039288984, -0.16794097565906607, 0.5179797341717075, -0.7112044065355726, -0.08999523759768983, -0.05120752552262522, -0.398012554971959, 0.275347888941055, 0.8038477456616825, 0.6323759762949369, -0.12863976224982468, 0.5462488040246101, -0.13084412809592644, -0.4807315036664265, 0.033435895768025546, 0.20202663719056038, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2131827861745805, 0.5529542575165005, 0.7431816532984447, 0.7531864217809923, -0.008327970469150221, 0.02467723295334042, -0.6572913304816794, -0.24432390532765594, -0.21590899292291663, -0.0010000000000000041, 0.09210379869233053, -0.0037608802950666424, -0.0009999999999999985, 0.03126966834566311, -0.1614573454971535, 0.000999999999999994, 0.13913843272251505, 0.18323041305985394, 0.371693601662695, 0.13915101807496863, -0.054624911615865, -0.1622660255733934, 0.5087931982673626, -0.7137982424305962, -0.07954120047696686, -0.049313258878313115, -0.39755153702873575, 0.25569177294448847, 0.8110076848805005, 0.6025543245948417, -0.13019031656609642, 0.5586063889174138, -0.1274473370602021, -0.4888330313715529, 0.036557634009328314, 0.20952821901039814, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2113002619503391, 0.5532726432484383, 0.7444526043170515, 0.7516809388372359, -0.011372050374556247, 0.023987049938511182, -0.658992461333434, -0.23613601647646462, -0.20863133225183916, -0.0009999999999999957, 0.0927643658608331, 0.0009885554653857162, -0.0010000000000000072, 0.03451307562259659, -0.15126783103062963, 0.0009999999999999905, 0.13141857562869055, 0.17851639599723082, 0.3698001612859197, 0.1800227531504713, -0.05482008915784352, -0.15571126925392628, 0.4996968519023078, -0.7103120567796332, -0.07027574644052427, -0.04823144443951188, -0.3969487320050766, 0.2399676439794758, 0.8181598059844509, 0.5799381386752833, -0.13274464305737066, 0.5643887198218809, -0.1247710743527233, -0.4927538996532073, 0.040028760889420506, 0.21291762459974653, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20965925796352392, 0.5535287264138935, 0.7460156359773251, 0.7504577484041172, -0.01398915823662829, 0.023724380504632107, -0.6603443231237195, -0.22864945175464133, -0.20349010624837205, -0.0009999999999999987, 0.09441363379591794, 0.005398183282976504, -0.0010000000000000028, 0.035833567293952935, -0.1421865679492265, 0.0010000000000000165, 0.12472293389137278, 0.17627144748551568, 0.366360329574578, 0.21960975386960307, -0.05567413200152465, -0.15116057667611157, 0.49088236374026917, -0.7027391050471368, -0.06202874766598919, -0.048185395394226616, -0.3970091006380315, 0.2291589444666766, 0.8258765455393748, 0.5667459815566224, -0.13803778761314528, 0.5684602965609454, -0.12434408777568769, -0.49488615814231895, 0.04503058460504657, 0.21774827033327163, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20833842870619992, 0.5538320112434267, 0.7477949280599373, 0.7492354363242537, -0.01645411182247295, 0.023547379585043913, -0.6616804697698256, -0.2214447970691923, -0.19956975898690313, -0.000999999999999997, 0.09651182103997526, 0.010138679848355382, -0.0009999999999999948, 0.036215723759319675, -0.1326252054242976, 0.000999999999999996, 0.11809101293628833, 0.17560824374741968, 0.3625929338360891, 0.255514237978073, -0.05548982586827653, -0.14642860255595888, 0.4818149297727052, -0.6905881942697597, -0.05464693224518349, -0.04867464640990376, -0.39654001344394524, 0.2224270829536129, 0.833860004484461, 0.5600647527138117, -0.14402109202956068, 0.5675772964267642, -0.12391909998722266, -0.49407752159825374, 0.05012399117182826, 0.21951655205541573, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20726333509102246, 0.5541806042715466, 0.7499843399205031, 0.7481775946530257, -0.018547109522854918, 0.023423238372148204, -0.6628255000312836, -0.21488413124629474, -0.1969390421576327, -0.0010000000000000041, 0.09886284921633558, 0.014491246468195917, -0.0009999999999999894, 0.035308172122009034, -0.12347155421963497, 0.0009999999999999955, 0.11262321234568587, 0.17643489186834413, 0.3579835041704425, 0.2876962017431096, -0.054144185318762784, -0.14190848273068857, 0.47213391244925673, -0.6736833800100878, -0.048689928443187334, -0.04970984323855598, -0.39498885654101434, 0.22193604720842386, 0.8425699206894623, 0.5628309914944926, -0.15214021860560067, 0.5624972512480053, -0.12349458248804132, -0.4902353776696715, 0.05548728623168657, 0.21939022868330293, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20647679934488633, 0.5546461338954771, 0.752193968492561, 0.7471259035272787, -0.02047925062279276, 0.02325965847259558, -0.6639596921954022, -0.20865799379129932, -0.19524708060277035, -0.0009999999999999953, 0.10135348962389468, 0.018925820630325683, -0.0009999999999999972, 0.03407260207303246, -0.11423286658148106, 0.0010000000000000005, 0.10722124061499624, 0.178482499220961, 0.3532667599671168, 0.3152583318132887, -0.05197498155491675, -0.138066858611742, 0.462271259316894, -0.6524969452280612, -0.04331303637943392, -0.05106310682857493, -0.3929892560780039, 0.2253211042003651, 0.8511373032774082, 0.5720369832635516, -0.16023176024672228, 0.5520664684700686, -0.1231391745595685, -0.48323494149213747, 0.060629060511502304, 0.21481038734828625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20596620721463058, 0.5551735325920747, 0.7543391109076105, 0.7461867823923833, -0.022098753608010453, 0.02305263499208514, -0.6649703052706903, -0.20301683602262766, -0.1945272376794983, -0.001000000000000006, 0.1038388083329596, 0.02309248204561906, -0.0009999999999999976, 0.032064109520200744, -0.10545296710479146, 0.0010000000000000046, 0.10231858876626262, 0.18170299049865646, 0.3481747442919664, 0.3387726579751482, -0.048493360190509774, -0.13560073878229098, 0.45236281734869344, -0.6263775282474808, -0.0390327936505443, -0.05255372821144014, -0.38979018244969715, 0.23367483883785953, 0.8592423645819932, 0.5884626634248822, -0.16815865676680336, 0.5367105071657481, -0.1229390107632594, -0.4738033315672749, 0.06513562537766215, 0.20569012198420938, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20557277493474968, 0.5557633636447233, 0.7565072255690073, 0.7452374159634486, -0.02344414816051005, 0.022753763211451047, -0.6659983723890519, -0.1978234275685038, -0.19458527812191528, -0.001000000000000005, 0.1059548411949095, 0.026876674436767094, -0.0010000000000000154, 0.03039615144235104, -0.09661655256070982, 0.000999999999999983, 0.09774688957888675, 0.18582292640636522, 0.3425906699200499, 0.3566671853942329, -0.04488140822103488, -0.13372509394120913, 0.44188659381726386, -0.596857545174644, -0.035014546152111095, -0.05386579071052005, -0.3867242520924239, 0.24479458184468272, 0.8677132956017729, 0.6109137600703408, -0.1764066147762922, 0.5170286662176496, -0.12280460962205024, -0.4596806435875706, 0.06964563345360417, 0.19213429143178154, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20522121654522035, 0.5563122353177634, 0.7587257587475595, 0.7442369701571603, -0.02436749208805393, 0.02237934830563388, -0.6670957370198397, -0.19331541801773935, -0.1956831284201988, -0.0010000000000000063, 0.1075775726247608, 0.030105668474057497, -0.0010000000000000059, 0.02935037306709543, -0.0876798449463008, 0.0009999999999999953, 0.09397086133546631, 0.19097635509414834, 0.33569354344068375, 0.3685391758487981, -0.04094712569443212, -0.13150701346528929, 0.43052947002240805, -0.563782732513963, -0.0317510088872456, -0.054218857775805665, -0.3839105204931432, 0.259085805882211, 0.87742997109238, 0.6400967199382002, -0.18566864705084943, 0.49411353600086677, -0.12286534910632117, -0.44048042131808035, 0.07426118643320716, 0.17604436730010228, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20483201016975927, 0.5566812962436117, 0.7608481721199999, 0.7432943710784171, -0.024916730230344224, 0.022047926957736826, -0.6681366053395325, -0.18927365473566635, -0.197397955428451, -0.0009999999999999827, 0.10856441282342578, 0.03274657096573882, -0.000999999999999985, 0.028662715946170564, -0.07895274180859167, 0.0010000000000000028, 0.09040186964682742, 0.19680737740567836, 0.3281627611159946, 0.37493208127849936, -0.037758890471065974, -0.13072989191787196, 0.418894009843215, -0.5282281401264632, -0.028524164779582757, -0.05440898844703069, -0.38106671596740416, 0.27406075595919654, 0.8869252397446434, 0.6736892188227929, -0.19460988664669748, 0.4690977066479564, -0.12287416627734754, -0.41588987101711017, 0.07828464647812682, 0.1563951342428731, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20431508511256952, 0.5567847218448676, 0.7626690244365042, 0.7424363380314419, -0.025013382474502136, 0.02178018259105927, -0.669095089142004, -0.18592789913020133, -0.19989308398753833, -0.0009999999999999992, 0.10873552356812147, 0.034692690150065014, -0.0009999999999999979, 0.028553499846456405, -0.0704200435104179, 0.0009999999999999929, 0.08721466172521505, 0.20341253465388415, 0.31971367097031367, 0.37619076661075945, -0.035341356775851505, -0.1311430284118018, 0.4073756107344628, -0.4907153392778419, -0.025723607566330232, -0.054163260456151475, -0.3780158267268166, 0.28909102819267196, 0.8957203811219745, 0.7107444519126616, -0.20237943692852936, 0.4433462684793357, -0.12265272387836242, -0.3860808347438825, 0.08042881635342132, 0.13596041422112654, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20364725065188874, 0.5564798756503627, 0.7644356627647593, 0.7416333537233348, -0.024601910248271942, 0.021654714128187317, -0.6700043193989436, -0.18301057444216284, -0.20292380886042238, -0.0009999999999999957, 0.10804884295849679, 0.03586656147263653, -0.0009999999999999937, 0.028901630788268975, -0.06209843775325045, 0.0009999999999999972, 0.08413166288204815, 0.21048102255202192, 0.31032820136337325, 0.3720504335716717, -0.034168070333689106, -0.1333205449971919, 0.395496873776916, -0.45175558768465685, -0.022824514163391804, -0.053695734136545506, -0.3752266304231971, 0.3028557452649758, 0.9043370107569239, 0.7512649956661318, -0.21019592996962053, 0.41876774628593955, -0.12225251052741147, -0.3520534848653132, 0.08223418355840162, 0.11514395639596221, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2027134105379033, 0.5557733428320114, 0.7661217775637815, 0.7408104843360216, -0.02363042634833775, 0.02158374524586916, -0.6709512435263739, -0.1805433708795464, -0.20645985962272836, -0.0010000000000000115, 0.10619678297029346, 0.036148716271570805, -0.0009999999999999742, 0.030096421017217236, -0.05363248725349937, 0.0009999999999999788, 0.08118616650235538, 0.21771082304239775, 0.30001733581981027, 0.36269783743946893, -0.033932519739379365, -0.1366274662887202, 0.3835188232976298, -0.4123505865369606, -0.020220400798845767, -0.05284230821798119, -0.3732614316568285, 0.31450040491806436, 0.9123768612271685, 0.7946903739295391, -0.21812564318151648, 0.3966247469119409, -0.12120945088349694, -0.31526951865474606, 0.08296729339338917, 0.0967760314990305, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20163396824858887, 0.5546701273481087, 0.767650736633307, 0.7400866665262168, -0.022206380295235462, 0.021649971614606946, -0.6717960117724604, -0.17847067540133532, -0.21049109416391415, -0.0010000000000000224, 0.10349900173404333, 0.03570367417546081, -0.0009999999999999907, 0.03151745970560209, -0.04542488663952687, 0.0009999999999999963, 0.07827421583707198, 0.22542998315807186, 0.28886866346971, 0.3503670556331891, -0.03508685266631532, -0.14164114565495972, 0.37138041237072805, -0.3737338259716792, -0.01740664336946555, -0.05182073561822691, -0.37139526571364795, 0.3232290563327973, 0.9201687073242566, 0.8384823696856675, -0.22571664406393835, 0.37806509751942363, -0.11999073753467558, -0.2779266438614017, 0.0831953156850013, 0.08181244337678618, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.2003854401786844, 0.5533072016001073, 0.7688950092363613, 0.7394864638166309, -0.020411987711708376, 0.021739053867832498, -0.6725106200845921, -0.17692852835510453, -0.21507082713346362, -0.000999999999999998, 0.09977265941054489, 0.0345885837762092, -0.0009999999999999935, 0.033077229793400345, -0.03719307704386692, 0.0009999999999999703, 0.07550075739037981, 0.2335730354944907, 0.2771264580465322, 0.3360119883363313, -0.03733920414259713, -0.14758410608800027, 0.35986615832889723, -0.3379762491864764, -0.014599691185525494, -0.050617608668167426, -0.36980982636588794, 0.3283187901275481, 0.9271108458694634, 0.8806769692002212, -0.23203036628406165, 0.3627027919276486, -0.11813710642580812, -0.24196215804951562, 0.08169110107287045, 0.07220747814364331, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19897636593792886, 0.5517532909306415, 0.7698865958613365, 0.7389479769050512, -0.018417837313784966, 0.021848512681322055, -0.6731562323786564, -0.1754902304009297, -0.21965019431973773, -0.0009999999999999926, 0.09539932184515368, 0.03292827279169527, -0.0009999999999999911, 0.03479038867268538, -0.029262779217670214, 0.0010000000000000024, 0.07266834824385626, 0.24159109498491224, 0.26517252548010817, 0.3221632860628715, -0.04052513460921061, -0.15434869118095265, 0.3485380858272685, -0.3052499720223723, -0.011552800430102659, -0.048742369677107864, -0.36832654822507455, 0.33102890138105323, 0.9337243636427612, 0.9195119870248993, -0.23789413890851704, 0.35136479642043617, -0.11642907162706935, -0.20959643948814613, 0.07993893507336924, 0.06685164220908904, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1974812706406378, 0.5500880292749699, 0.7703348933878672, 0.7385132457471993, -0.016218838600042636, 0.021833198269084652, -0.6736901710606631, -0.17437041650910987, -0.22402605427936623, -0.0010000000000000048, 0.09007799333170412, 0.030537326325108437, -0.0009999999999999883, 0.036502723042106736, -0.02162885915711226, 0.001000000000000004, 0.06989849979011331, 0.24878390715280416, 0.25346192478446505, 0.3099817469502888, -0.044198233792925576, -0.16129867078335133, 0.3384289886162625, -0.2785986352080959, -0.00856928474065532, -0.0449028575639915, -0.36735418992586816, 0.33231614142373944, 0.9391341704745535, 0.9521862157044153, -0.24204273135595764, 0.34348697451314825, -0.11529779297026499, -0.1823221153708845, 0.07732231523306002, 0.06594401062650353, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1959122344809913, 0.548389775610371, 0.7705543690975044, 0.7380655271336171, -0.014184887025205468, 0.021896023489633536, -0.6742244661775478, -0.17317998909465723, -0.22804104280262807, -0.00099999999999998, 0.08500740035579323, 0.02850753809640321, -0.0009999999999999924, 0.03822134616707189, -0.014176477518276562, 0.0010000000000000145, 0.06715331609556577, 0.2554612002866795, 0.24218488864762633, 0.29973359562756574, -0.04817503236152198, -0.1684196259124599, 0.3293364077757777, -0.25528011316222193, -0.005601641080761083, -0.04129050559146482, -0.3672188665838426, 0.33307394547154057, 0.9439634828528821, 0.9803811361833759, -0.24662842586689146, 0.3383124588603611, -0.11478477369493627, -0.15959531258555318, 0.07480819144658359, 0.06786876242482573, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19472911258914058, 0.546642437751157, 0.7701857265561999, 0.737603766310501, -0.0123773108929045, 0.02218798737243001, -0.6747556441527446, -0.17272130173570044, -0.23203767157880495, -0.000999999999999993, 0.08109973672693693, 0.027926506461138173, -0.0010000000000000028, 0.03947495641530847, -0.006593023634964299, 0.0009999999999999861, 0.0651254898952498, 0.2615241109281503, 0.23207776048642947, 0.29112131629561144, -0.05189185531223061, -0.17648688887167593, 0.3235499624116556, -0.23826444771924615, -0.003684499210909675, -0.03924431893274884, -0.3708697599149621, 0.33621783806022676, 0.9465336131893641, 1.0016240388288242, -0.2529408086422832, 0.3351846901496329, -0.11671814609700736, -0.1423112456771152, 0.07271564320114667, 0.07219599348700022, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19323782239735673, 0.5449332993622932, 0.7698451686692691, 0.7371243570496804, -0.010741229567022626, 0.02236108663230829, -0.6753016289304402, -0.17172850299283643, -0.23494654504381213, -0.0009999999999999753, 0.07681935811246121, 0.02701498326051624, -0.0009999999999999985, 0.04117965717510056, 0.000931669416153436, 0.001000000000000013, 0.06308441127017693, 0.26602484193370657, 0.22235749175339115, 0.2839132944041771, -0.05651702526877793, -0.18299717630752002, 0.3178312665306756, -0.2235542708180769, -0.001050319514892506, -0.03660178432507827, -0.373535535138408, 0.3374241843330121, 0.9490927545962419, 1.020415856877785, -0.2571490211649623, 0.3315662470671189, -0.11746930555014937, -0.1281664207642667, 0.07025382795710353, 0.07759798149773886, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19101176495470137, 0.5433410223114986, 0.7697333643162197, 0.7366785146099741, -0.008928483674978237, 0.02191290192522951, -0.67582902647084, -0.16998003029816613, -0.2357367874993966, -0.000999999999999991, 0.07001464891992065, 0.023597400018687226, -0.0010000000000000013, 0.04425780666946663, 0.009087593822674505, 0.000999999999999985, 0.0615154738117557, 0.2667772229593292, 0.21314340767440637, 0.27546879959028464, -0.06429856849151364, -0.18549027661841733, 0.3110498645760367, -0.21330084822640677, 0.00393820576837449, -0.03183261439017736, -0.37275977300909013, 0.3324795075728457, 0.9516108926647115, 1.0382747441146696, -0.2527916969673411, 0.3208578598280866, -0.11348249234529151, -0.11705936036463037, 0.06632567569979603, 0.08267660842822405, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18868657158323088, 0.5417762368737056, 0.7695478384881754, 0.7361216416421773, -0.0076768803354769505, 0.021864778477892352, -0.676452456331155, -0.1680956749528084, -0.23607533092546457, -0.0009999999999999848, 0.06466040594923331, 0.02164231679119984, 0.0010000000000000002, 0.04713523639297173, 0.0167983753564575, 0.0009999999999999957, 0.05989454658949674, 0.26677804782791525, 0.20455838968870343, 0.2683785525359588, -0.0720657321094437, -0.1872307025285079, 0.3042204999410971, -0.20620891142558082, 0.008896511197028851, -0.027393436783348976, -0.37270482917186315, 0.32813918303319795, 0.9535933685075798, 1.0529792981776989, -0.2487348098394144, 0.31100979911745896, -0.11013796384270894, -0.1079194893875464, 0.062240597371140814, 0.0885509692058863, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1870261552298916, 0.5401436326102126, 0.7690434572447842, 0.7354511849497505, -0.006381284733697126, 0.021965252868129125, -0.6771915249230417, -0.16658573128878748, -0.2372614817733454, -0.0009999999999999764, 0.0603807766505818, 0.020834491983743268, 0.0009999999999999996, 0.04992724978222172, 0.024571505375805228, 0.0010000000000000106, 0.058177161672957715, 0.26769244068107034, 0.19740587437062365, 0.2610193981714226, -0.07896583855568619, -0.1902837162362176, 0.30211421405443056, -0.20321660675761455, 0.013114325685553882, -0.02478162436211972, -0.37556295038534443, 0.3275357981016853, 0.9533195970668102, 1.0620180128449297, -0.24600553378460155, 0.30207865556905017, -0.10943673542019887, -0.09988808806136759, 0.0575328977776089, 0.0955129695365202, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18538729153246236, 0.538547801473734, 0.7685963272118286, 0.7347535649043918, -0.005340827920765172, 0.02208689992551507, -0.6779534226398504, -0.16481274418739347, -0.2378068731347231, -0.0009999999999999979, 0.05655003621019218, 0.020485159417231398, 0.001000000000000001, 0.05262881966921234, 0.03203543698281148, 0.0009999999999999822, 0.056485867198989424, 0.26788859751666644, 0.19068968740710826, 0.25463344867159426, -0.08548022263601578, -0.19270879600624832, 0.3005770881516244, -0.2012171713802332, 0.017309939503888434, -0.02231942512295506, -0.3784947608908538, 0.3270825899463779, 0.9530207759706106, 1.0696118718033771, -0.2434428120767932, 0.29349742973867704, -0.10884815192159704, -0.0932600921241344, 0.0530783090634474, 0.10243866629167801, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1840021915320049, 0.5370207889397566, 0.7683653871245698, 0.7340991897154524, -0.004424418358556362, 0.022174671249815196, -0.6786656674212037, -0.16322939034014836, -0.23774838246381122, -0.0010000000000000234, 0.052991925082741045, 0.020315019008610054, 0.0009999999999999998, 0.054858472686152834, 0.039018336052134096, 0.0010000000000000271, 0.05537718774097437, 0.2671690020709843, 0.18465025339202112, 0.24729672302602398, -0.0910132650001367, -0.1946837333122668, 0.29998744072342504, -0.20194789749331946, 0.021418055968864584, -0.020256846505441635, -0.3823064214321062, 0.3263966676802752, 0.9526151371251359, 1.075615485565695, -0.24161558499074595, 0.2848378598868695, -0.10873096830132357, -0.08836982222267865, 0.04972857195264209, 0.10806563058723416, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18264707830384083, 0.5355491410742139, 0.768140517276758, 0.7334350088483131, -0.0037186720654197912, 0.022236796076034623, -0.6793855931457616, -0.16137710031160324, -0.23718212927717167, -0.0010000000000000252, 0.04973688753348782, 0.020430988810640073, 0.0009999999999999905, 0.05698575460333065, 0.04567157208501491, 0.0009999999999999562, 0.05421794822559696, 0.2660005826931941, 0.17905140020789723, 0.24093028523734775, -0.0962223242499641, -0.1962618836478598, 0.29981835439960697, -0.20349839079407428, 0.025428761930639348, -0.01835177963142318, -0.38602452133531634, 0.3258435040533224, 0.9520595620223901, 1.0802911581607768, -0.23983236798605068, 0.2763118468995647, -0.10865611865206248, -0.08448346747613203, 0.046470291455527654, 0.11349001289799264, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18145536663508116, 0.5341571327157361, 0.7678818245121849, 0.7328738969931252, -0.0030591578875713604, 0.02213383559966261, -0.6799974896871143, -0.15942378758482656, -0.23619083357695092, -0.0009999999999999755, 0.046237947861058885, 0.020132255908018995, 0.0010000000000000009, 0.05855461711875406, 0.05156265972552759, 0.0009999999999999799, 0.05312743780163067, 0.26460682034334554, 0.17425289846707864, 0.2344489939704339, -0.10088461843634128, -0.19811899552620976, 0.3002469094614653, -0.207702793210002, 0.029164952948003753, -0.01676891602278684, -0.38976377599051504, 0.324790533766905, 0.9508288757599941, 1.0823982955716251, -0.23824942048868172, 0.26745044662836326, -0.10877743087469167, -0.08249494118406818, 0.04362480625044286, 0.11735246278433632, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18029431876010238, 0.5328235901423182, 0.7676340139999539, 0.7322949303000217, -0.0025670051942453228, 0.022021430360596134, -0.680626624623291, -0.15732688187443591, -0.2348293420132382, -0.0009999999999999827, 0.04302361997906334, 0.020083020591335054, 0.0010000000000000102, 0.060059230384818925, 0.057178779889496725, 0.0009999999999999935, 0.05204809436718262, 0.2628143312849181, 0.16983317470487502, 0.2287165076463315, -0.10521998934993759, -0.19954600464577374, 0.3009297551524905, -0.21259095986148335, 0.03269466951503505, -0.015287307856275205, -0.3933531531430248, 0.32383071967533994, 0.9495223481098045, 1.0833620725264763, -0.23672723723496208, 0.25863279902498426, -0.10885560581411625, -0.08108773701133029, 0.040929898074525746, 0.12095893539434234, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17925021958588455, 0.5315807134741416, 0.7673622934600542, 0.7317525616820869, -0.002114520552946362, 0.02187063183807061, -0.6812161130930021, -0.15545081043712314, -0.23322896502248472, -0.0009999999999999844, 0.03981688864994382, 0.01981045393611739, 0.0009999999999999907, 0.061151490366746934, 0.062086685457589885, 0.0009999999999999764, 0.05122697576692663, 0.2605445337480117, 0.1661519986648763, 0.22287256778861061, -0.10892889178999138, -0.20072065950398663, 0.301788366538782, -0.21995542564135415, 0.035445524773803375, -0.013898836143577974, -0.3965525069324173, 0.3222685134641821, 0.9478640825849779, 1.0813362775920654, -0.23548671615754022, 0.24948960126960038, -0.10873659846451546, -0.08095758301872649, 0.03895037939990811, 0.1229241587485577, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1782635371485177, 0.530380473971231, 0.7670883293579052, 0.7312026020625277, -0.0018021490085276455, 0.021723597962459304, -0.6818119918918306, -0.15350776090534207, -0.2314277111122976, -0.000999999999999991, 0.03691710177152118, 0.019830386922977127, 0.0009999999999999924, 0.062245752326857494, 0.06684819335327519, 0.0010000000000000195, 0.050487996916792274, 0.25813638005739736, 0.16282043321210973, 0.21743171995417812, -0.11261192472943773, -0.20167707428689152, 0.30293458793152683, -0.22768282228355902, 0.03819241355889103, -0.012683242431594074, -0.3997246059470817, 0.32079227692827605, 0.9461047109751688, 1.0786572363635512, -0.23453659541041671, 0.24067704332507553, -0.10875846475085778, -0.08101467138717404, 0.0370137508515339, 0.12467794096529358, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17749686033579756, 0.5292151531897373, 0.7667191279292879, 0.7307098968708327, -0.0015864878063361306, 0.021629741713272237, -0.6823435234138853, -0.15191071032926715, -0.2298179675144107, -0.0010000000000000505, 0.03445401264098772, 0.020193703030856328, 0.0009999999999999799, 0.06323898863313676, 0.0713564590616633, 0.001000000000000017, 0.0502758480745967, 0.2561389408783265, 0.160332616170856, 0.2110732165969258, -0.11699345557277586, -0.20304611520481955, 0.3047153765159343, -0.23661866725382835, 0.04113105156041495, -0.01196909269869221, -0.4029835064396237, 0.31886707676912107, 0.9438546343168761, 1.0744161286623681, -0.23500692272327603, 0.23320328681976182, -0.10947567411039101, -0.08122630722991167, 0.03529533447497315, 0.12504405277234779, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17676936535255838, 0.5281091659433343, 0.7664874246850999, 0.730201067438846, -0.0015104545453557556, 0.021475507789999782, -0.6828930532692493, -0.15015011891874047, -0.2279977935680385, -0.0010000000000000501, 0.032189831916122486, 0.020764597847086688, 0.0010000000000000085, 0.06429737654801775, 0.0758669556066154, 0.0009999999999999799, 0.05015704481147363, 0.2541267034433516, 0.15811455711810307, 0.20481182036874745, -0.12109826249173547, -0.20391094991861336, 0.30637071064709553, -0.24571341389721285, 0.04422102066247287, -0.011506676702111803, -0.40594498876422835, 0.3170658432319749, 0.9419427352565198, 1.0702226583118915, -0.23578587650333924, 0.22599931075483293, -0.11009566949418588, -0.08143815772228531, 0.033801664234326435, 0.12528582657628595, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17618057132834988, 0.5271407594539881, 0.7668774146933715, 0.7296892988739747, -0.0016660735722809606, 0.02103729000562475, -0.6834531320704278, -0.1480376556081607, -0.22596956631958615, -0.000999999999999986, 0.030030197690649445, 0.0214944726756689, 0.0010000000000000063, 0.06557102536405643, 0.08076587258766126, 0.0010000000000000098, 0.05058174159018751, 0.25278324046427814, 0.1565962011569952, 0.1967598840106306, -0.1245314270791337, -0.20332295752717036, 0.3067227509387693, -0.25520783444602985, 0.04827695911380018, -0.012042428829575208, -0.40757830481819124, 0.31511598867079105, 0.9417614933217183, 1.0674445712843053, -0.23813504038066924, 0.22004087863227478, -0.11023958113521222, -0.08125703580202227, 0.03347262903988211, 0.12459757858506124, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17564309458070254, 0.5262144783756683, 0.7672182087853536, 0.7292151250683787, -0.00191618168203656, 0.02057856806094792, -0.6839723328876907, -0.14584498686560546, -0.22378334979416642, -0.0010000000000000065, 0.028031855947709954, 0.022325552649854342, 0.0009999999999999861, 0.06675457303297638, 0.08539393881077545, 0.0010000000000000035, 0.050933014214193995, 0.2513710614720774, 0.15533002173704233, 0.189063292031587, -0.12792427747088903, -0.20282837046920618, 0.3071622899020742, -0.26479835470605034, 0.05211222605125377, -0.01251814103920273, -0.40891102417286246, 0.31333364711969774, 0.9414644844068574, 1.0642013738805236, -0.24044142400896001, 0.2141397560742733, -0.1103209662802325, -0.08129158968295438, 0.03320904460298776, 0.12365327524615945, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17531253474401023, 0.525297889533647, 0.7672142371616827, 0.7290174786864374, -0.002224050365697699, 0.020062491782676304, -0.6841973880344104, -0.1436213997475002, -0.22146953815773368, -0.0009999999999999948, 0.02604416837109983, 0.022943994065892696, 0.0009999999999999907, 0.06732237939244935, 0.08888784946485201, 0.0009999999999999935, 0.050916622093665646, 0.25011553431736927, 0.15470005303444612, 0.18123301584927914, -0.13184228624208108, -0.20378064484668576, 0.30750377705352, -0.27453731234831463, 0.05499652208421069, -0.01247331764242579, -0.40876993941558165, 0.3116539609860746, 0.9405535071213345, 1.0595017829858704, -0.24252103662985278, 0.2084640673330893, -0.11006350505052565, -0.08223705624624267, 0.0332986064692465, 0.12116380965006063, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17500579147258305, 0.5244196767340448, 0.767217586044189, 0.7288350416483915, -0.0026008494016108593, 0.019507519349738215, -0.684406439432444, -0.14130553033159432, -0.21903372244263025, -0.0010000000000000159, 0.024135824993683067, 0.023593492645390436, 0.0010000000000000122, 0.06782988726849663, 0.09224304810440585, 0.0009999999999999736, 0.05083418844634258, 0.2488135828417293, 0.15426111606854015, 0.173678630350492, -0.13547160920785276, -0.2046239547071338, 0.3077733277678641, -0.2842286017738494, 0.05769820358777154, -0.01241845253099843, -0.40841413472945853, 0.3101007818842704, 0.9397327130801494, 1.0545614401679348, -0.2446094761142717, 0.20276118213834857, -0.10981213301648486, -0.08335096867484439, 0.03351758163274345, 0.11836720369600691, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17476431954046717, 0.5235810470687424, 0.7671769097311896, 0.7287929398716859, -0.0030295314927628954, 0.018786062588271726, -0.6844696900408003, -0.13877894240303018, -0.21643556871816655, -0.0010000000000000033, 0.02199338391189545, 0.02387831565930867, 0.0010000000000000063, 0.06789306007103056, 0.09507038085990295, 0.001000000000000012, 0.05040782512939597, 0.2476358509117019, 0.15433094078164436, 0.1661501604298583, -0.13809567609595358, -0.20560549316468923, 0.307242551270951, -0.2933280621524018, 0.059602328770752916, -0.012163220109085856, -0.40695456420084897, 0.3086374397406597, 0.9394138716814017, 1.049019990858029, -0.2467493077386716, 0.19689286417798035, -0.10958796483771067, -0.08532600415910149, 0.034456156990433175, 0.11369289761311935, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.174586790853865, 0.5227737457230193, 0.7671628645450639, 0.7287827780962026, -0.003504729580626051, 0.01803816348310684, -0.6844983592961376, -0.1362255189378204, -0.2137690898271413, -0.0010000000000000195, 0.01996210761475756, 0.024236272163579688, 0.0010000000000000083, 0.06788207265393846, 0.09781164704134375, 0.001000000000000015, 0.049999328136651755, 0.2464791500254526, 0.15448315678406588, 0.1588493244319452, -0.1405570615607634, -0.2064733025829617, 0.3067436602159194, -0.3022738869318948, 0.061473015908184415, -0.012073404705020106, -0.40543910825148965, 0.3072878554253357, 0.9392083407846696, 1.0433359619905294, -0.24884613063729857, 0.19129964695880103, -0.10934205058698789, -0.08723549023414466, 0.035420132343252995, 0.10893247747316652, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1747085557658181, 0.5219967784443335, 0.76721702588889, 0.729000983684152, -0.004005328019940349, 0.017199356859819374, -0.6842848129679623, -0.1337571756138336, -0.21110010115543712, -0.0009999999999999794, 0.01802923751860342, 0.024686317845382545, 0.0009999999999999764, 0.06733871624907778, 0.10024382384689387, 0.0010000000000000052, 0.04970931858099854, 0.2457534131698619, 0.154672323277207, 0.15174701939383917, -0.14255808470190295, -0.2073210216616614, 0.3061806935292953, -0.3100184902395552, 0.06338686747000384, -0.012865654480597323, -0.4036181083284113, 0.3061832266132921, 0.939662006173545, 1.037389488756943, -0.25069182127294704, 0.18735799891327282, -0.10895220597152154, -0.0888257300896403, 0.03653261433767182, 0.1035539930627344, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17488652783117095, 0.5212457718662111, 0.7672690807623839, 0.7292174801780565, -0.00452937968602059, 0.01635608665349724, -0.6840715092384769, -0.13129018586459895, -0.20841141492632648, -0.0009999999999999963, 0.016227451762503557, 0.025208699205065646, 0.0010000000000000083, 0.06682521707512837, 0.10262748565611333, 0.0009999999999999994, 0.04944514382890923, 0.2450186377307413, 0.1549968406764871, 0.14485811377170704, -0.14455263869926302, -0.20812432437320053, 0.30568380851676513, -0.3175237353502604, 0.06523368397200159, -0.013669592449944194, -0.401802142211959, 0.30512254255112503, 0.9401032870699675, 1.0313117880424072, -0.2526143766072498, 0.18349794526902005, -0.10852192665785668, -0.09048173275393596, 0.037689181768446635, 0.09810234219677762, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1753172502602698, 0.5205179320691475, 0.7672205609235337, 0.7294549892389756, -0.005016752549373633, 0.015556942065105711, -0.683833482963365, -0.12903484752034614, -0.20588919974079645, -0.000999999999999953, 0.01475344000901027, 0.025963498894756656, 0.0010000000000000085, 0.06639326006991636, 0.1048616499324288, 0.0009999999999999929, 0.049346271861506176, 0.24446003184688167, 0.15578921209040028, 0.1384103387843926, -0.14690919647965647, -0.20919660339651627, 0.30543713534289785, -0.3232573024043045, 0.06693647047594935, -0.014500325102292905, -0.400004924406641, 0.3040603736223479, 0.9404704455933324, 1.024886041067178, -0.2549960163215018, 0.1801744924838537, -0.10782906258199874, -0.0925724147940285, 0.03911516357584446, 0.09208283753342186, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1757737129831205, 0.5198182138211735, 0.7671935607347697, 0.7296990282816702, -0.0055068673022517205, 0.014759176802869974, -0.6835869873230495, -0.12676437710909758, -0.20339237376600167, -0.000999999999999996, 0.013332430846973874, 0.026707198488961057, 0.0010000000000000028, 0.06596011493461151, 0.10701519166167424, 0.001000000000000054, 0.0491702394451027, 0.24392613763662668, 0.15657940569465512, 0.13212286895639075, -0.14914509107877327, -0.2102086889334236, 0.3051633949212578, -0.32880088377370204, 0.06849286783406246, -0.015314182190752165, -0.3981500532838317, 0.30304954669269085, 0.940966643757774, 1.018419152576139, -0.25723783011069346, 0.17679677834086413, -0.10715080779953559, -0.09464777855168728, 0.040527749507826547, 0.08613565289274323, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17630171462491306, 0.519189855980517, 0.767219144209355, 0.7300079134683205, -0.005881429426540734, 0.014028404058304042, -0.6832693897293409, -0.12455286429515403, -0.20126186005519223, -0.0010000000000000087, 0.011821492484088848, 0.027254332558896067, 0.0009999999999999848, 0.06542891594496328, 0.10876524487542219, 0.0009999999999999681, 0.04851309322481135, 0.24371382565124555, 0.15708359615132636, 0.12623580836721962, -0.15095289056241779, -0.2112451517287239, 0.30461228898970943, -0.33288466026362884, 0.06939314526738728, -0.016009410841284516, -0.3959235197234641, 0.3022053892992822, 0.9422880658560491, 1.0120383184734192, -0.2585748677195257, 0.17314910354498683, -0.10653616555987463, -0.09668251582740164, 0.04185387632008253, 0.08057135088711422, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.176853870847108, 0.5185866783142395, 0.7672634419869881, 0.7303455227241831, -0.006264240115420585, 0.013296655446618344, -0.6829197432249635, -0.12234139155975254, -0.19914837328372917, -0.0010000000000000334, 0.010364113822057559, 0.027782086619049077, 0.0009999999999999788, 0.0648549590448998, 0.11039913814861281, 0.0010000000000000228, 0.04784319807950621, 0.24357215401594476, 0.15756993308895403, 0.1205222349186952, -0.15265128759469096, -0.21219533880614821, 0.3040311485129487, -0.3368257385293517, 0.07029636510420495, -0.01672388095745934, -0.39369251065576105, 0.3014860366736683, 0.9436788097664138, 1.0057281087752055, -0.2597099804729828, 0.16967551416739926, -0.10592302817114359, -0.09864593841677459, 0.04315753324896108, 0.07511814555174931, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17747666118380753, 0.5180568699409362, 0.7673527887701099, 0.7308914860204934, -0.006609937026346323, 0.012616438268002236, -0.6823450519207225, -0.12025895378557899, -0.1972893765073837, -0.0010000000000000302, 0.008894891170528899, 0.028153964781320227, 0.0010000000000000048, 0.06391853730398836, 0.11134388758754202, 0.0010000000000000228, 0.047096996664781426, 0.2440329904893331, 0.15773095799291623, 0.11548586296070153, -0.15388757051033913, -0.21292105975114398, 0.30320967089054013, -0.33957929642547174, 0.07150021862982402, -0.01759986483959672, -0.39140111347857137, 0.30154981853483076, 0.9455366851800242, 1.0001652427537298, -0.25947441515293884, 0.1674410512537613, -0.1053054058350652, -0.10021730932150447, 0.0443149861719044, 0.07037674696300025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1781144396421987, 0.5175459916492422, 0.7674267668655756, 0.7314391276299985, -0.006946947821280826, 0.011955356402846258, -0.6817665377100262, -0.11820964952858734, -0.19547174326608424, -0.0010000000000000492, 0.007501317278191215, 0.028523897639623354, 0.0009999999999999955, 0.06298605988905655, 0.11222036258795925, 0.0009999999999999998, 0.04632476196239715, 0.2444837901233153, 0.15784159529367356, 0.11058096343633254, -0.15506784915255478, -0.21366522705167615, 0.3024204572069423, -0.3421656710922132, 0.07260644861314122, -0.01850540890330651, -0.38916245002696304, 0.30161400506985536, 0.9474276828101627, 0.9946387396276336, -0.259258805636288, 0.16510210855785606, -0.10468769558368676, -0.10179492996717716, 0.04537283587381281, 0.0658039340508194, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17876637752985752, 0.5170839452649293, 0.7673207736703727, 0.7320101092739676, -0.0071582575274325095, 0.011478541776628777, -0.6811594544220665, -0.11651546659091762, -0.19402674998707647, -0.0009999999999999887, 0.006291698314467222, 0.028917912976196683, 0.0009999999999999987, 0.06200699753730639, 0.11269131999091932, 0.0009999999999999658, 0.045382525135658255, 0.24495584524253575, 0.1574317996703434, 0.10623580615598631, -0.15608161763023104, -0.21483092806497028, 0.3018357296218128, -0.34338143594766046, 0.07331532380308255, -0.01969321284498161, -0.3872570270088992, 0.301687250743096, 0.9495562137436194, 0.9895946333990278, -0.2591592090640264, 0.16206529887768262, -0.10407957650857112, -0.1034275056328968, 0.045739431891763556, 0.062357480757100466, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.17942302148916642, 0.5166410008391925, 0.7672440580433406, 0.7325891109722849, -0.007372424863438163, 0.010995796692097756, -0.6805423824359323, -0.11479332867931541, -0.19257558210506065, -0.0009999999999999885, 0.005130919963734816, 0.029287889930757784, 0.0009999999999999916, 0.06104745594663927, 0.11312450552731582, 0.0009999999999999935, 0.044408619521079294, 0.24543260057903657, 0.15698758048989248, 0.10198148122013805, -0.1569860405100179, -0.21591207954242017, 0.3012027731640274, -0.34449945714545477, 0.07394506433119089, -0.020893370296901668, -0.38538414798219456, 0.30197295948029174, 0.9518028725695384, 0.9847355891113506, -0.25907519247693234, 0.15916628954362708, -0.10347163902318302, -0.10494239591760565, 0.04604116084739883, 0.05901552079755927, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18003177467020692, 0.5162716191150991, 0.7673176485046354, 0.7332354324802653, -0.007564314064117901, 0.010510816531015568, -0.679851531177262, -0.11296701086791319, -0.19112366678107776, -0.0009999999999999612, 0.004012343032362023, 0.02956610535409222, 0.0010000000000000206, 0.060162030713232006, 0.11333532351228173, 0.001000000000000003, 0.04321307651048397, 0.246017083358717, 0.15617130097555057, 0.0981079540199569, -0.15725896012591517, -0.21660540829480165, 0.3002009663098353, -0.34472655087998344, 0.07426984111541453, -0.02228408228849082, -0.3837177526902489, 0.30386542270099304, 0.9549301420724423, 0.9814255274507871, -0.2590854526200416, 0.15731680116025418, -0.10286983536278005, -0.1056218165931879, 0.045871166804891346, 0.056434539830813236, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18064173812723078, 0.515915182241635, 0.7674001162628649, 0.7338992006472125, -0.007752555520845002, 0.010034920351043383, -0.6791400161570699, -0.11115821963758604, -0.18967983121277837, -0.001000000000000015, 0.002938901423381746, 0.029814165228253094, 0.0009999999999999933, 0.05924236286652439, 0.1134560390772382, 0.0009999999999999907, 0.04199078600601042, 0.24657349632333964, 0.15530566556530345, 0.09438084780607157, -0.1574444186451349, -0.2173047051132851, 0.299186392199382, -0.34480554988524886, 0.07453524603051072, -0.023661785929884313, -0.3820792336510296, 0.3057257341389744, 0.9581015060249081, 0.9782197116875163, -0.2590068807125365, 0.1553274448065115, -0.10223209564835414, -0.10626948263695464, 0.045633500915567234, 0.05400764021990716, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1811841030405124, 0.5155998590224009, 0.7674937918606412, 0.7347060571307115, -0.007884694434938798, 0.009668019025373427, -0.6782708681767458, -0.10958174244771807, -0.1883206503000901, -0.0009999999999999716, 0.0019172837682813078, 0.029960066484399597, 0.0010000000000000284, 0.05798385387832976, 0.11291659459309679, 0.000999999999999991, 0.040572347626466256, 0.246968714593822, 0.15396053212589603, 0.09157022904664476, -0.15708858267393386, -0.21825893024246595, 0.2980610301246181, -0.34358336733837713, 0.07461450562952385, -0.025065403520471943, -0.3806260677632573, 0.307382648616933, 0.9616242998995417, 0.975977901587612, -0.2582031135830116, 0.15220880261620115, -0.10131685438001782, -0.10665085000916022, 0.044885143104589784, 0.052743046033421725, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1817296985175675, 0.515293793416575, 0.7675865861180697, 0.735521479856293, -0.008017876633837445, 0.00930229576534996, -0.6773900896955805, -0.10800548625776851, -0.18695405864447256, -0.0010000000000000382, 0.0009500661994088477, 0.030089061148674188, 0.0010000000000000024, 0.05669941676815381, 0.11233810081123591, 0.001000000000000015, 0.0391321205665375, 0.24732511213905425, 0.1525673748404353, 0.08886453269838425, -0.15662765554383434, -0.2191914280213364, 0.29688075181727797, -0.34224298247671864, 0.07461591306427827, -0.02644882493932543, -0.37922267831166145, 0.3091382163127391, 0.9652462789789376, 0.9738873509036783, -0.25733714379619826, 0.14910168102850968, -0.1004286266930394, -0.10702243454022829, 0.04410489503501608, 0.05159551058274382, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1822356394338519, 0.5150144223218565, 0.7676211383413638, 0.7364154525878925, -0.008147802003469927, 0.008973635521153146, -0.6764209993619462, -0.1065208219742823, -0.1855243392109465, -0.0010000000000000816, 0.00015444744009101614, 0.030236339895413973, 0.0009999999999999957, 0.05513233054338274, 0.11146925227010925, 0.000999999999999978, 0.037525390900523654, 0.24740822687672845, 0.15070374867792097, 0.08681965103562422, -0.15540197541634618, -0.22011800405268173, 0.2952583857853494, -0.33978647779910703, 0.07422938740796524, -0.027805654570238394, -0.37819891469426314, 0.3117721467542121, 0.9696954056853699, 0.9731670894325254, -0.255936464427771, 0.14604566541999792, -0.09976600709065458, -0.10731085566076178, 0.043077607063909854, 0.05139770080483018, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1827521332237755, 0.5147398050939628, 0.7676419483922201, 0.7373167026613306, -0.008272218432400742, 0.008650355923337778, -0.6754412052290184, -0.10504996161319971, -0.18411638690250093, -0.000999999999999996, -0.0005876366929290143, 0.0303696632412319, 0.0010000000000000126, 0.05355546706364224, 0.11055486574599603, 0.001000000000000014, 0.03590645473246777, 0.24749291810424753, 0.14884197843081245, 0.08487753509787425, -0.15418187882585424, -0.2210767202092616, 0.29363792205005973, -0.3372460349850296, 0.07376307119743322, -0.029140667108607767, -0.37723940541250317, 0.3144418236046779, 0.9741482380546138, 0.972521561358905, -0.2544413067989286, 0.1430066139604924, -0.09914593954221657, -0.10758437436404007, 0.04199379071995017, 0.051318904796346015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18330646440178117, 0.5144598842717636, 0.7674997674083768, 0.738291687807888, -0.008346358276587607, 0.0083920890719743, -0.6743777093426999, -0.10378372429635217, -0.182890414084663, -0.000999999999999981, -0.001123556420881751, 0.030551172108054216, 0.0009999999999999879, 0.05183454767735995, 0.10925967206906644, 0.000999999999999984, 0.034194897513063004, 0.24763358746679093, 0.14694625769285602, 0.08362055717375923, -0.15309112954072354, -0.22246469252455625, 0.2920395495145955, -0.3338561300528361, 0.07283173757282667, -0.030441405900578436, -0.376834195571584, 0.3174626767922441, 0.9786373990283982, 0.9726231733577495, -0.2520951425971959, 0.14006881764579313, -0.09889866724180928, -0.107701596478717, 0.04043687808209477, 0.05228333406963799, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18386478295298148, 0.5141879969547903, 0.767386993343667, 0.7392662613965837, -0.008422787664329376, 0.008120628767575598, -0.6733115971057521, -0.10247423182189577, -0.1816433431986535, -0.0009999999999999662, -0.001638385681860188, 0.030702090808326303, 0.0009999999999999996, 0.05011095045505476, 0.10796310368911384, 0.0009999999999999868, 0.032456456256901134, 0.24776945381777274, 0.14507881416788138, 0.08242874722246668, -0.15193770653036354, -0.22377820977197405, 0.2903488781487858, -0.3303341795384478, 0.07186345281194169, -0.03172852935981314, -0.3764536289524456, 0.32051388518264856, 0.9832074260277469, 0.9729061622196331, -0.24968756570045425, 0.1371300520974399, -0.09865957649303604, -0.10778134036346726, 0.03884994698551588, 0.053317646673255545, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18441289661479632, 0.5139531887008747, 0.7675155520125224, 0.7402499639940963, -0.00853176810809764, 0.007734297569192474, -0.6722331294876802, -0.10083592348617307, -0.18017943186005805, -0.000999999999999995, -0.0022103044250853516, 0.03074946247627904, 0.0009999999999999998, 0.048311211577588616, 0.1066643385661429, 0.0009999999999999972, 0.030485378671865997, 0.24789913066950595, 0.1434383079709286, 0.08164792947290962, -0.15024218594020142, -0.22451333382689262, 0.28782026321886756, -0.32547184490942555, 0.07076959700484847, -0.033053413007946515, -0.3762801549381473, 0.32390190766155197, 0.9885378981350624, 0.9749699528808734, -0.24670329287822246, 0.13413694735698115, -0.09846938829893989, -0.10746173968093954, 0.03698414363799626, 0.05500267337572557, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18496760644654708, 0.5137244076758524, 0.7676448117012938, 0.7412367521205901, -0.008637857153612004, 0.007343188838538496, -0.6711479287811256, -0.09919576526759027, -0.1787148180767763, -0.0010000000000000087, -0.00276823208168738, 0.030754825252981347, 0.0009999999999999898, 0.046485055947393, 0.10531455692436803, 0.0010000000000000106, 0.028531912678412888, 0.2480418340744275, 0.14183745380504356, 0.0809293916952272, -0.14856695980844722, -0.22526251466148953, 0.28526794140014594, -0.3205489838604651, 0.06965491492622337, -0.03430686740441182, -0.37613780305637967, 0.3273247414824077, 0.9938473961919657, 0.9771337970902513, -0.24371379244460545, 0.13112343154203304, -0.0982940028133201, -0.107151064265997, 0.035069667387784144, 0.05677825467064475, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1855471220996078, 0.5135226981990318, 0.7677511346309925, 0.7422686042137222, -0.008736982650567061, 0.006908488938725975, -0.6700098932951366, -0.09760937472599085, -0.17722157596111612, -0.0010000000000000204, -0.003423002708144407, 0.030538077021338695, 0.0010000000000000072, 0.04435695907623541, 0.10346581709961412, 0.000999999999999996, 0.026774727157243744, 0.24834978006412622, 0.14059198535271827, 0.0806068192780306, -0.1471220604174235, -0.22626122480338265, 0.28251215333379276, -0.31493395324298196, 0.06852745630957566, -0.03501731891995627, -0.37629399708020517, 0.331117060885288, 0.9989465700314942, 0.9803603244718376, -0.24066542115482037, 0.127877133417341, -0.09822249140984766, -0.10687122675658818, 0.0326774700405643, 0.0594711409557404, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18613562162379074, 0.5133226836378091, 0.7678463398332162, 0.7432954908138522, -0.008830378377984985, 0.006480814422904412, -0.6688735581542397, -0.09602249992341037, -0.17574673103422064, -0.0010000000000000167, -0.004042390904572897, 0.030311418409728935, 0.001, 0.042254490074497435, 0.1016193088569284, 0.0009999999999999983, 0.025012137752352958, 0.24866538487468343, 0.13940918186660614, 0.08032573704899319, -0.1457332813336379, -0.22726236265073615, 0.2797613126745573, -0.3092803910742829, 0.06741816331101756, -0.03572273750983461, -0.37642410877153837, 0.3349717633498078, 1.003938564469858, 0.9836246636814944, -0.23760999580713468, 0.12467384298578259, -0.09817247338483012, -0.10657160479182823, 0.030297099128141436, 0.06222043513109742, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18678144158401766, 0.5131116499583288, 0.7677946241005865, 0.7442875206815623, -0.008899279811567363, 0.006124569236780867, -0.6677719513637812, -0.09450673186297935, -0.17444788534214103, -0.0009999999999999946, -0.004499191569596423, 0.030180715454277956, 0.0009999999999999868, 0.04040051423107621, 0.09981047325295513, 0.0010000000000000154, 0.023218645392085326, 0.2490955627042483, 0.1388616603465934, 0.08027474382409007, -0.1449627634078797, -0.22838234949670577, 0.2771125562702199, -0.3031590620642639, 0.06668619742345654, -0.03654568256504568, -0.37625621210099436, 0.3394899557657015, 1.0077717589802184, 0.9873452899936447, -0.2344835874913381, 0.12193605229602195, -0.09828397807799633, -0.10595565616481004, 0.028037988020538925, 0.06559919435596487, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18743426947297884, 0.5129054110125397, 0.7677611418828202, 0.7452854499517806, -0.008963767915189267, 0.005764908379451629, -0.6666603443930893, -0.09298678801631285, -0.17314999490377017, -0.0010000000000000187, -0.004953050397592636, 0.030012529341858246, 0.0010000000000000113, 0.03852957267144224, 0.09798069602802256, 0.0010000000000000128, 0.02143254233471331, 0.24953457248089778, 0.13831339726531816, 0.08027728561477179, -0.14415224221768747, -0.2294614305800198, 0.2744593042521009, -0.2970287265846481, 0.06591620555389517, -0.03733725419541543, -0.37603624770336885, 0.34405667537779433, 1.0115768502975633, 0.9911371642190541, -0.23134060078386104, 0.11919286511564052, -0.09834444674087714, -0.10535189775724987, 0.025778510237798556, 0.06901222365457942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18813436174902667, 0.5127293220791435, 0.7679182543901302, 0.746370872155722, -0.009009302142794709, 0.0053572194879725876, -0.6654477093444416, -0.09148949712855016, -0.1718386795167552, -0.0009999999999999807, -0.005598148055907428, 0.029600463972470056, 0.0009999999999999966, 0.03643329325617877, 0.09591946681865594, 0.0009999999999999731, 0.019765761310359783, 0.2500978176024638, 0.1377484593470301, 0.08060934137288427, -0.14289351887484206, -0.23013693082322248, 0.2717848864396541, -0.2907856757306077, 0.06487705170434482, -0.03793260912374469, -0.3751981168306832, 0.3491874348782332, 1.015053940014427, 0.9957987597849218, -0.22800665246283927, 0.11642607055609772, -0.09773242402715186, -0.10478642053248756, 0.023507668170898183, 0.07285376834770148, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18884121806360796, 0.5125547868260169, 0.7680759180613448, 0.7474557161466329, -0.009050490753530349, 0.004956969554075819, -0.6642314878637453, -0.09000546548310467, -0.17053407994010153, -0.000999999999999999, -0.0062201714637137515, 0.029174635066643587, 0.0010000000000000076, 0.03432165087365869, 0.09384524661352768, 0.0009999999999999666, 0.01812027727263984, 0.2506511659232004, 0.1371917874231929, 0.08099791590954548, -0.14165845528465876, -0.23079635508747076, 0.2691356431908391, -0.2845554094774437, 0.06382541443109734, -0.03849923884196376, -0.3743162851725088, 0.35435314291778836, 1.018471700972419, 1.0004675629689586, -0.2246796621843775, 0.11370599394556655, -0.09715633666929115, -0.10423108606625082, 0.021243544772805847, 0.07667930962241253, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.18960401961063345, 0.512375362043669, 0.7682233478883677, 0.7485593934408437, -0.009080790507312359, 0.004633680289589877, -0.6629893684986312, -0.08875575463299688, -0.16928818990356784, -0.0009999999999999879, -0.006762046703098097, 0.028765620908514776, 0.0010000000000000002, 0.03200047026590702, 0.09162468022158408, 0.0010000000000000007, 0.016770936257151828, 0.2511013264094563, 0.13677412175955442, 0.0817454688865305, -0.14071820130145976, -0.23132225247442906, 0.2668352839068026, -0.27850121660049515, 0.06277331474163851, -0.038871715017243735, -0.3728828367633381, 0.35989427467037927, 1.0211403821360385, 1.0052617263876837, -0.2214685325576677, 0.11164783673850122, -0.09694277118259548, -0.1036761993994432, 0.019070471354208283, 0.0803505227109729, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19037174246623076, 0.5121971485662887, 0.7683588015919978, 0.7496611370679459, -0.009098153240785563, 0.004316190585219199, -0.6617452483217654, -0.08753036024650063, -0.1680601927217572, -0.0009999999999999731, -0.007305456426831244, 0.02833203805080327, 0.0009999999999999929, 0.029674172968492084, 0.08940537299801106, 0.0010000000000000137, 0.015442201003863607, 0.2515493940553332, 0.13635799647468755, 0.08254876786643169, -0.1398228872358618, -0.23188749271077336, 0.26456913604114185, -0.27252307933557457, 0.06172277181862622, -0.03925099954809947, -0.37145786274278886, 0.3654232186273497, 1.0237256186017938, 1.0099921574820288, -0.2183204884323369, 0.10957744349944448, -0.09670412115431658, -0.10316650143354211, 0.016948523667108556, 0.08400176395072088, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19117954413552926, 0.5120095592392284, 0.7683158360085812, 0.7507644739239829, -0.008975157780225281, 0.004053317308411628, -0.6604965721752563, -0.08670199538117411, -0.16704780816460865, -0.0010000000000000143, -0.00808604195732124, 0.027726292273024564, 0.00100000000000001, 0.027261550490580204, 0.08722150722779967, 0.0010000000000000026, 0.014425765457165453, 0.2519790309491976, 0.13602062106183135, 0.0836248019305461, -0.13954256830299724, -0.23305276744430636, 0.262768928504972, -0.2675811669736552, 0.06084858260605227, -0.03989057533872491, -0.37016321289157, 0.3706767604077585, 1.0251630095880073, 1.0138720657363642, -0.21604851010441298, 0.10741654211668444, -0.09603295731539771, -0.10318877393818732, 0.015525873374412085, 0.08743239188336031, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19199211057711385, 0.5118237638580463, 0.7682802382710103, 0.7518620664341794, -0.008846001484631157, 0.0037925135955588973, -0.6592501787301139, -0.08587966481642871, -0.16603375858433778, -0.001000000000000027, -0.00885375497297244, 0.02710648352242271, 0.001000000000000001, 0.02486388444796281, 0.08504000601808634, 0.0009999999999999987, 0.013420512437401697, 0.2523865068752495, 0.1356735431218641, 0.08473267747292092, -0.13923690562662655, -0.2341876126032648, 0.2609994187233827, -0.26270935376690124, 0.05995599000111347, -0.04051124100527909, -0.36888680888479447, 0.3758979675381238, 1.0265884428032381, 1.0177258909550015, -0.21372961709453, 0.10525199730049692, -0.09538058922160338, -0.10322115851024602, 0.014127324363631665, 0.09083807533338725, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19285094185912507, 0.51164200247161, 0.7683496314577781, 0.7529083816069612, -0.008669381110360029, 0.003526412087642296, -0.658058793083823, -0.08520522850942483, -0.16496629735926785, -0.0009999999999999523, -0.009629257668183216, 0.026422891911182654, 0.0009999999999999907, 0.02267035924425589, 0.08290322413080686, 0.0009999999999999987, 0.012608762425678067, 0.2524691215289304, 0.13524427091816726, 0.08588776280770544, -0.1385445514153639, -0.23491101614851317, 0.2596915357291359, -0.25886273656126996, 0.058938491900302904, -0.04099436876364738, -0.36789216100011013, 0.3805485389226929, 1.0278338466442625, 1.021209892034458, -0.21072383618484464, 0.103077610926179, -0.09496521254652363, -0.10330679462903852, 0.013099555848635973, 0.09391801406978537, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19370894000171687, 0.511460801602774, 0.7684149586655442, 0.7539433886479695, -0.008488125927744603, 0.0032654278038909412, -0.656876438467318, -0.08453916107370806, -0.16391271902939417, -0.0010000000000000141, -0.010392967913133325, 0.025737100360254898, 0.0010000000000000148, 0.0204957559546527, 0.08079680664737746, 0.0010000000000000063, 0.01180497311812469, 0.25255229318697847, 0.13483134124642374, 0.08707858746872991, -0.1378749802488842, -0.235657451567021, 0.2584353565806709, -0.2551010440181452, 0.05793480115170105, -0.041475577916996534, -0.3669177345789707, 0.38515046009515386, 1.0290312879889083, 1.0246038751105384, -0.20774937533110366, 0.10091244724073613, -0.0945630792857414, -0.1034085275338959, 0.012054491060033085, 0.09697026488553759, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1945323091643652, 0.5112725531494472, 0.7684085952170798, 0.754825341472199, -0.008273928218499649, 0.0030536692666821874, -0.655866542131264, -0.08406113777162302, -0.1630768597327867, -0.0010000000000000245, -0.011143707171809448, 0.025162458225630433, 0.000999999999999992, 0.018617864469779564, 0.07920741060493405, 0.0010000000000000208, 0.011150891116494414, 0.2526615427491366, 0.13473335925616156, 0.08856925427624847, -0.137571291143881, -0.2368292645373341, 0.25803418917716936, -0.25274385796969484, 0.05730511459040007, -0.04207816680734948, -0.3662855472822033, 0.38883051137573704, 1.0294279913939008, 1.0265118270459255, -0.20530927267890914, 0.09893592513688221, -0.09433285194354467, -0.1036934399806433, 0.010725046934283116, 0.09962466574589562, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19536189528101727, 0.5110843046341189, 0.7684072790124856, 0.7556985595961477, -0.008058583657723854, 0.0028470660670869698, -0.6548638335322453, -0.08359981769068181, -0.16224031507951509, -0.0010000000000000072, -0.011869521524217254, 0.024594068227929554, 0.0010000000000000113, 0.016759968935021283, 0.07763847092749226, 0.0009999999999999916, 0.010519119279775448, 0.2527394245645334, 0.13464861001021955, 0.09007491247020136, -0.13726816609090212, -0.23797336667078967, 0.2576459547216719, -0.2504377763544663, 0.05663077485677915, -0.04264814553302183, -0.3656752569585254, 0.39243428622248094, 1.0297971503182537, 1.0283847442722844, -0.20293585426054767, 0.09697666246221646, -0.09408697309662087, -0.10398457564620542, 0.009439470860707252, 0.1022448092348342, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19628918519439978, 0.5108858540932449, 0.7684994919282799, 0.7564388949135072, -0.007859934848438064, 0.002701673219790567, -0.6540115600262913, -0.08348403243604816, -0.16138004570078407, -0.0010000000000000232, -0.012305841402029904, 0.0242871333339454, 0.0009999999999999976, 0.015251439545753268, 0.0764536403714998, 0.0010000000000000198, 0.01031343177368834, 0.2522444473961259, 0.1348214386090185, 0.0916926639895263, -0.13697866785376753, -0.238627600959817, 0.25750442741411755, -0.24905574824838225, 0.05526728208131527, -0.04275021514287114, -0.3654700791576492, 0.3945746820074247, 1.0296598858125021, 1.029621435763068, -0.20181328339360222, 0.09534212098660091, -0.09349577930780463, -0.1043117520331512, 0.008978034575765695, 0.10425774413960959, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19722019534712076, 0.5106869592526748, 0.7685913191110944, 0.7571718665982025, -0.007658538110199698, 0.0025609956630579596, -0.6531657925269986, -0.08338098464274028, -0.16053048933688782, -0.001000000000000012, -0.012730595978309027, 0.023977628513691458, 0.0010000000000000141, 0.013757671929256358, 0.07528356070048822, 0.001000000000000008, 0.010115879377974316, 0.25174087436904347, 0.13500978532496394, 0.0933070566906727, -0.13670378618800164, -0.2392734438575684, 0.25738955236764804, -0.24773221131220868, 0.05392361140004916, -0.042861644637245136, -0.3652661934934501, 0.3966805597605873, 1.0294871961376537, 1.0308152867266944, -0.20072505018319994, 0.0937355569298272, -0.09291031185694118, -0.10462654306662407, 0.00850996055644662, 0.10626514712322471, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.19821479433722858, 0.5104644136155552, 0.7686699438288928, 0.7577738966678292, -0.0074316264248644, 0.0024889683595483852, -0.6524701506536836, -0.0835673214337315, -0.15990557479429374, -0.0009999999999999816, -0.01307608908712473, 0.023756235419370483, 0.0009999999999999916, 0.012554218436216675, 0.07442872043977881, 0.0009999999999999707, 0.010098318403177256, 0.2510723514986098, 0.13552119442933028, 0.09478511140129915, -0.13673663511647993, -0.2397673400601243, 0.25784492202420967, -0.24763063421398607, 0.05314864889355635, -0.04332218301486211, -0.3650910988818905, 0.39802203126169666, 1.0285696437645444, 1.0311195986066832, -0.20036947952849682, 0.09271234831789088, -0.09242996648575007, -0.10461463686850049, 0.007913835258404603, 0.10817334147056401, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.1992111280840634, 0.5102426802953228, 0.7687499353418045, 0.7583676603271136, -0.00720700664472302, 0.002419378255845913, -0.6517827072223215, -0.08375277132343663, -0.1592876030174607, -0.0009999999999999488, -0.0134088310187247, 0.023536579773193348, 0.000999999999999992, 0.011358687497986002, 0.07358650577830719, 0.0009999999999999814, 0.010081584364603309, 0.25040803097124015, 0.1360405020355054, 0.09624896947546309, -0.13675609500355965, -0.24024975292306625, 0.2583033600074274, -0.24756873260432533, 0.05236922428422705, -0.04377782008901178, -0.36490873718057965, 0.39935093867689014, 1.0276517330891843, 1.0314098385802892, -0.20002776638915198, 0.09169032541675913, -0.0919514397039148, -0.10458233670244184, 0.0073185044068582415, 0.11007311217494593, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20023949526479168, 0.5100260891363475, 0.7688626337602765, 0.758781427321579, -0.007063619319521479, 0.002374849362825509, -0.6513027029918124, -0.08393160804586818, -0.1588352787224454, -0.0010000000000000176, -0.013609245694833565, 0.023503998427383096, 0.0009999999999999996, 0.010329660560685908, 0.0730500309768851, 0.0010000000000000024, 0.010100586365308542, 0.24985583935373062, 0.13673800886548246, 0.09734058053139294, -0.13646530206912957, -0.24046758565454784, 0.2588369975069497, -0.248470779097163, 0.051640515972468375, -0.044270927913201384, -0.36456328570335317, 0.40039485489335364, 1.0267324229074708, 1.0313730863017112, -0.200021180541134, 0.09069284955354451, -0.0915226587992695, -0.10402011744992166, 0.006753254457226376, 0.11176869926546996, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20127006681126533, 0.5098088834838097, 0.768971631655965, 0.759195332961207, -0.006918895701526999, 0.002332180458542552, -0.6508218928604838, -0.08411874723413709, -0.15838193515464757, -0.0009999999999999731, -0.013807885667590104, 0.02345842347045487, 0.0009999999999999987, 0.009301769571572778, 0.07250534584178424, 0.0009999999999999916, 0.010125365960746658, 0.24929137619777103, 0.13743500081658655, 0.09844356742563756, -0.1361728236426209, -0.24068798358796534, 0.25937577893129427, -0.24940394852749304, 0.050901455755012884, -0.0447459722024624, -0.36422387660353767, 0.40140891198518563, 1.0258006786935663, 1.03130972985249, -0.20002088431545387, 0.08968867960542481, -0.09109509286276911, -0.10348186092978424, 0.00618496655299503, 0.11343881197690181, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20235527534179143, 0.5095606654510741, 0.7689776609677971, 0.7596241784656967, -0.006749302728673337, 0.0023147904005019673, -0.6503231474800767, -0.08454935139934153, -0.15790121727037068, -0.001000000000000006, -0.01414115171546798, 0.023187929145510232, 0.0009999999999999833, 0.008292967368537935, 0.07172985157713141, 0.0010000000000000102, 0.010329407802043332, 0.24838536613655157, 0.13809481190956524, 0.09984965407780277, -0.1358668960048009, -0.24098581449375683, 0.26005862820797443, -0.25121636688886234, 0.05002807361088598, -0.04486114576654113, -0.3640737543395793, 0.4016063174309742, 1.0245183827050537, 1.0305071347703567, -0.20020122152563025, 0.08852031859061975, -0.09071001137144302, -0.10358504975652141, 0.005534367246311922, 0.1143741534253707, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20344321652273434, 0.5093110524530543, 0.7689779870560021, 0.7600506854593273, -0.006579373417662582, 0.002302421121839768, -0.6498264123865806, -0.08498774339176908, -0.15742830260143983, -0.0009999999999999532, -0.014460519912350691, 0.022923156818629106, 0.0010000000000000024, 0.00729136141597277, 0.0709661647610199, 0.000999999999999958, 0.01053482719283122, 0.2474770897759357, 0.13875310621742357, 0.10124486471204833, -0.13557467951377647, -0.24128595643622122, 0.2607392372340527, -0.2530397064716575, 0.049133740061392386, -0.044969713027549124, -0.36392403455866645, 0.40180989477315243, 1.023227274663559, 1.029696596784041, -0.2003905415568018, 0.08735008199481713, -0.0903223562304559, -0.10369447624194565, 0.004899262650710826, 0.11529761894463492, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20461769817542805, 0.5090018449458379, 0.768786009069252, 0.7604108395545797, -0.006415525492751035, 0.002431126569703795, -0.6494061023305471, -0.08570088528514794, -0.1572255801708415, -0.0010000000000000178, -0.014489072693972777, 0.023005473191830893, 0.0010000000000000022, 0.006524408103616169, 0.07061323773862119, 0.0009999999999999903, 0.010802988955809363, 0.24649924743451476, 0.13932399138930812, 0.10222642995649199, -0.13577979603872165, -0.24167274519337387, 0.2613436868207782, -0.25525426360426884, 0.04769049931084059, -0.04502493140663765, -0.36382480758452224, 0.4023421790417689, 1.0216425140057734, 1.0286342402546038, -0.20087549649621922, 0.08607717885057668, -0.08986878098023528, -0.1039559510980665, 0.004807456395389673, 0.11579780367090285, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20579085280941936, 0.5086939087588083, 0.7685981173406753, 0.7607676744891214, -0.00625315166599844, 0.0025599990724873667, -0.6489891293014112, -0.08641671701342304, -0.1570269924852304, -0.0009999999999999384, -0.014512888299300112, 0.023083535982443695, 0.0010000000000000189, 0.0057562909777843195, 0.07026047419782967, 0.0010000000000000005, 0.011078520873881515, 0.24552586351722538, 0.13989050231979813, 0.10318887650779977, -0.13597083359254128, -0.24203958769128522, 0.2619279969619184, -0.2574667392031413, 0.046266339601863875, -0.04507686022765422, -0.3637220101728986, 0.4028749695749833, 1.0200750431898955, 1.027597997029422, -0.20135878707736046, 0.08482119258177491, -0.08941841062369973, -0.10421749578630925, 0.004739724860544541, 0.11628295971222771, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20690515307982316, 0.5084334834183949, 0.7685920230768588, 0.7609945249571003, -0.006158316546450964, 0.0026763318145216876, -0.6487235508062349, -0.08725747788068496, -0.1570161285471651, -0.0009999999999999874, -0.014527522963898297, 0.023143760043257404, 0.0009999999999999842, 0.004916534102208615, 0.06991137891837423, 0.000999999999999965, 0.0116859166348002, 0.2447560210902825, 0.1402486177250728, 0.10336278705827036, -0.13557973625021186, -0.2415449864067459, 0.26162567647514556, -0.2595530158345076, 0.04585475611170447, -0.04516762589487419, -0.36350284712876696, 0.40354510937680954, 1.0192646346814371, 1.0277072381589931, -0.20175089692869, 0.0842442400081791, -0.08912572205579335, -0.10441199983253502, 0.0057226416023994945, 0.11611865222167457, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20801792328632346, 0.5081722198767642, 0.7685817066065732, 0.7612230502102123, -0.006062013985911222, 0.002796240129882101, -0.6484557817278146, -0.08810289381910863, -0.15701157216300196, -0.0010000000000000332, -0.014539830816495843, 0.023198592074418583, 0.0009999999999999983, 0.004076871366552716, 0.06955888995767745, 0.000999999999999986, 0.0122856683509064, 0.2439857207754333, 0.14060285526911687, 0.10353995845444387, -0.1351940782300779, -0.24106131632436875, 0.26132790320148225, -0.2616450544693995, 0.04542564531123872, -0.04525607613888716, -0.3632958205603084, 0.40420815631446605, 1.0184345233520669, 1.02779617782172, -0.2021360473805433, 0.0836582953051283, -0.08883513025322785, -0.10461481802276364, 0.006696399847085326, 0.11594106984836243, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.20903838361223612, 0.5078406950492332, 0.7683254976377869, 0.761553174936814, -0.005873524638618575, 0.0031102132539951874, -0.6480683528961219, -0.0892366470981203, -0.15740687019410887, -0.0009999999999999944, -0.014658468211495584, 0.023158054497649707, 0.001000000000000014, 0.003250342337620416, 0.0690266668508732, 0.0009999999999999898, 0.0124550043005642, 0.24319508767814135, 0.14072344762223493, 0.1040124424027635, -0.13518709096825646, -0.24120945154711726, 0.26130716681321764, -0.2640550209454933, 0.04421347079542957, -0.04542407246195826, -0.36383072384441817, 0.4044351887778706, 1.0164605780089369, 1.0267199773976006, -0.20209595316500914, 0.08255009938765238, -0.08863836930150833, -0.10528770739259562, 0.007138424112139696, 0.1149959056328099, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.21005696413603278, 0.5075099197704603, 0.7680714650085205, 0.7618829340135006, -0.005685656097129359, 0.0034253135226918544, -0.6476807357031689, -0.09037327422255702, -0.1578063907134372, -0.001000000000000019, -0.014774102404211828, 0.023113184394142693, 0.0009999999999999853, 0.0024245389492468473, 0.0684899865567933, 0.000999999999999984, 0.012626148277197135, 0.24240777746668787, 0.14083878001229042, 0.10447171423890583, -0.13517288615912904, -0.24134784600181916, 0.2612760789700727, -0.2664608195506908, 0.04300454163713772, -0.04558597363621705, -0.36435871792133034, 0.4046717233995956, 1.0144930460130022, 1.0256627907670444, -0.2020575271202782, 0.08146083594699705, -0.0884373060976762, -0.10594782315060139, 0.007582026501620829, 0.11404491953733861, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.21091412141523955, 0.5072502819041463, 0.7680300712598468, 0.7622027923122165, -0.005557762001962837, 0.003830577007386299, -0.6473031294167982, -0.0917911479609728, -0.15860392879548585, -0.0009999999999999846, -0.014829774797172757, 0.022994344843086482, 0.0009999999999999983, 0.0016458162304244124, 0.06758514416491046, 0.000999999999999979, 0.012961516789920632, 0.24193927592553832, 0.14052087569009886, 0.10395883744729986, -0.13454426705573663, -0.24063919514042179, 0.2602690486968137, -0.2683964155178928, 0.042356903455890355, -0.04550087623032995, -0.3643499718793841, 0.4057464858270893, 1.0131143314619806, 1.0262762906999596, -0.20207367510153165, 0.08202235607186764, -0.08786061416897285, -0.10552799223904767, 0.008247433181354678, 0.1126478423141877, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.21177005178192634, 0.5069909844649988, 0.767989003679091, 0.7625223933496509, -0.005429293388011812, 0.004236807334061215, -0.6469251671385512, -0.09321317437088368, -0.15940338453512914, -0.0010000000000000083, -0.014884591917285422, 0.022869892073079334, 0.001000000000000005, 0.000868762144520856, 0.06667808788207602, 0.0010000000000000085, 0.013300862041238226, 0.24147076792522107, 0.14019873533856964, 0.10343812029014046, -0.13391385924446153, -0.23992989114496413, 0.2592604417292902, -0.2703305801303151, 0.04170505826887161, -0.0454105834688773, -0.36434117916528885, 0.4068257138417118, 1.011736357023813, 1.0268921647161666, -0.20209311928859006, 0.08258541991272303, -0.08728372835799283, -0.1051069316079119, 0.008909299321792892, 0.11124739911301158, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.21241535085546306, 0.5067734806613466, 0.7680269570506808, 0.7628570734159785, -0.0052175325472780905, 0.004791965412653088, -0.6465283442826082, -0.09542736101573038, -0.16065583031764663, -0.000999999999999936, -0.015145500744851772, 0.022380605413461957, 0.0009999999999999955, 0.00043279940441265483, 0.06545661667279254, 0.0009999999999999796, 0.014358919816786789, 0.24100735232160123, 0.13931212194223566, 0.10254046860052475, -0.13310967402700144, -0.23905187993832158, 0.2580054338765616, -0.2723161475318489, 0.04083610580906401, -0.044907337744123615, -0.3647168084461133, 0.40834522838961457, 1.0105229614709528, 1.0279622526405687, -0.20259670780784966, 0.08333215893778785, -0.08665294934539827, -0.10449784339195278, 0.008993391589914045, 0.1091786129034864, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - [-0.21306068346433843, 0.5065562912786956, 0.7680645571501519, 0.763191289805969, -0.005005485640432194, 0.005346795236192113, -0.6461311105793518, -0.09764084753545176, -0.16190757833712074, -0.000999999999999961, -0.01540659652311303, 0.021886870168284715, 0.001, -3.243436196080112e-06, 0.06423545816131196, 0.0009999999999999861, 0.015416953490236284, 0.24054454631093553, 0.13842254643462612, 0.10163341309443061, -0.13230538090925692, -0.23817434517836694, 0.2567496252247842, -0.2742987550721412, 0.039964738595664934, -0.044401496218164416, -0.3650880653598781, 0.4098661435088383, 1.0093091895328938, 1.0290319312162102, -0.2031004727986001, 0.08408063421066587, -0.08602202692772555, -0.10388869280661762, 0.00907741809163619, 0.10711084824811719, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion_with_ee.yaml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion_with_ee.yaml deleted file mode 100644 index de94fcf8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/motion_data/object_pick_and_place_optimized_motion_with_ee.yaml +++ /dev/null @@ -1,5847 +0,0 @@ -left_hand_palm_link_position: -- [0.38888630270957947, 1.3037428855895996, 0.6463831663131714] -- [0.38887712359428406, 1.2996150255203247, 0.646492063999176] -- [0.394931435585022, 1.2959394454956055, 0.6480656862258911] -- [0.3959328532218933, 1.291133165359497, 0.6478613615036011] -- [0.3968004286289215, 1.2863606214523315, 0.647768497467041] -- [0.39727532863616943, 1.2805078029632568, 0.6469662189483643] -- [0.397935152053833, 1.27470064163208, 0.6467350125312805] -- [0.3986370265483856, 1.2681304216384888, 0.6459270119667053] -- [0.3993299603462219, 1.2615736722946167, 0.6448185443878174] -- [0.3999900221824646, 1.2543004751205444, 0.6433814167976379] -- [0.40074312686920166, 1.2470273971557617, 0.6424672603607178] -- [0.4013541042804718, 1.23933744430542, 0.641242504119873] -- [0.4017307162284851, 1.232433795928955, 0.6389425992965698] -- [0.40220367908477783, 1.2247111797332764, 0.6369833946228027] -- [0.40277379751205444, 1.2168346643447876, 0.6346753239631653] -- [0.4013589918613434, 1.2108120918273926, 0.6316851377487183] -- [0.4021148681640625, 1.2036341428756714, 0.6307980418205261] -- [0.40284013748168945, 1.1967490911483765, 0.6289316415786743] -- [0.40331536531448364, 1.1897778511047363, 0.6256896257400513] -- [0.4039146900177002, 1.1833436489105225, 0.6239216327667236] -- [0.4049413800239563, 1.1768202781677246, 0.6224297285079956] -- [0.4056873321533203, 1.1704820394515991, 0.6207582354545593] -- [0.40529176592826843, 1.164520263671875, 0.6187396049499512] -- [0.40142887830734253, 1.1593247652053833, 0.6158229112625122] -- [0.4007156789302826, 1.1530389785766602, 0.6137562394142151] -- [0.40136614441871643, 1.1465668678283691, 0.6123117208480835] -- [0.40195900201797485, 1.1405384540557861, 0.6117755174636841] -- [0.4031398594379425, 1.1340867280960083, 0.611153781414032] -- [0.4047602117061615, 1.1276800632476807, 0.6105971336364746] -- [0.4068397283554077, 1.1209025382995605, 0.6102811694145203] -- [0.40771257877349854, 1.1086950302124023, 0.6059175729751587] -- [0.40617847442626953, 1.0990196466445923, 0.6058372259140015] -- [0.4056883454322815, 1.0919264554977417, 0.6063825488090515] -- [0.4089021384716034, 1.0876240730285645, 0.6073364615440369] -- [0.4147481620311737, 1.0845167636871338, 0.6075774431228638] -- [0.42247939109802246, 1.081161618232727, 0.6071173548698425] -- [0.43134963512420654, 1.0770264863967896, 0.6069015264511108] -- [0.4374522268772125, 1.0715793371200562, 0.6075934171676636] -- [0.44289806485176086, 1.0648012161254883, 0.6080175042152405] -- [0.44685715436935425, 1.0571949481964111, 0.6090673804283142] -- [0.45218998193740845, 1.047925591468811, 0.6105954051017761] -- [0.45732825994491577, 1.037893533706665, 0.6123886704444885] -- [0.46264541149139404, 1.0282553434371948, 0.6147018074989319] -- [0.46795618534088135, 1.0192503929138184, 0.6169834733009338] -- [0.4728781282901764, 1.0103175640106201, 0.6192912459373474] -- [0.4766544699668884, 1.0013335943222046, 0.6210740804672241] -- [0.48000386357307434, 0.9926372766494751, 0.6229338645935059] -- [0.48246559500694275, 0.9845715165138245, 0.624845027923584] -- [0.4845866560935974, 0.9765653610229492, 0.6268060803413391] -- [0.4859795570373535, 0.9691725373268127, 0.6290843486785889] -- [0.48704543709754944, 0.9617724418640137, 0.6313588619232178] -- [0.4876059591770172, 0.9547634720802307, 0.6334053874015808] -- [0.4873806834220886, 0.9477617144584656, 0.6361433267593384] -- [0.4858856797218323, 0.9407389760017395, 0.6389617919921875] -- [0.4831172525882721, 0.9329670667648315, 0.6427327990531921] -- [0.4827272593975067, 0.924738883972168, 0.6479255557060242] -- [0.48472267389297485, 0.9123713374137878, 0.6552128195762634] -- [0.4853051006793976, 0.8973153233528137, 0.6622049808502197] -- [0.4799026548862457, 0.8800976872444153, 0.665795087814331] -- [0.4767102897167206, 0.8619958162307739, 0.6669973134994507] -- [0.47576799988746643, 0.8450424671173096, 0.6663376688957214] -- [0.47576022148132324, 0.8306928277015686, 0.6657299399375916] -- [0.4768032133579254, 0.8173445463180542, 0.6648585200309753] -- [0.4781809151172638, 0.8071944713592529, 0.6638602018356323] -- [0.47978475689888, 0.7980408668518066, 0.6625311970710754] -- [0.4808212220668793, 0.7914465069770813, 0.6613055467605591] -- [0.4817351698875427, 0.7855821847915649, 0.6600537300109863] -- [0.48029929399490356, 0.783163845539093, 0.6579068899154663] -- [0.4804770052433014, 0.7789981365203857, 0.6568182706832886] -- [0.4800502359867096, 0.7770050764083862, 0.6555718779563904] -- [0.4786306619644165, 0.7763504385948181, 0.6541030406951904] -- [0.47421029210090637, 0.7778000235557556, 0.6515871286392212] -- [0.4712385833263397, 0.778239905834198, 0.6496046185493469] -- [0.46979984641075134, 0.7795583605766296, 0.6487264633178711] -- [0.4683021605014801, 0.7815024256706238, 0.6479354500770569] -- [0.4654843807220459, 0.7841964960098267, 0.6469112038612366] -- [0.46390610933303833, 0.786600649356842, 0.6467625498771667] -- [0.46108460426330566, 0.7897469401359558, 0.6463771462440491] -- [0.45847243070602417, 0.7929579019546509, 0.6459841132164001] -- [0.4540509581565857, 0.7963508367538452, 0.6452687978744507] -- [0.44934365153312683, 0.7995042204856873, 0.6444452404975891] -- [0.44498249888420105, 0.8023375272750854, 0.643922746181488] -- [0.44026991724967957, 0.8048343062400818, 0.64350426197052] -- [0.4354845881462097, 0.8067176938056946, 0.6436722874641418] -- [0.4307863414287567, 0.8081444501876831, 0.6440756320953369] -- [0.42609643936157227, 0.8086768984794617, 0.6444352865219116] -- [0.42153406143188477, 0.8086111545562744, 0.644841194152832] -- [0.4174083471298218, 0.8078814148902893, 0.6455744504928589] -- [0.4139654338359833, 0.8064969778060913, 0.6464011669158936] -- [0.4110625684261322, 0.8044129610061646, 0.6476912498474121] -- [0.4088480472564697, 0.8013210892677307, 0.6489444971084595] -- [0.4080442786216736, 0.7981950044631958, 0.6497670412063599] -- [0.4081701338291168, 0.793982982635498, 0.6502897143363953] -- [0.4097086787223816, 0.7890491485595703, 0.6501125693321228] -- [0.41241273283958435, 0.7837867736816406, 0.6494150757789612] -- [0.41609784960746765, 0.7783466577529907, 0.6480419635772705] -- [0.42079398036003113, 0.7728488445281982, 0.6469751000404358] -- [0.42610636353492737, 0.7680580019950867, 0.6456283926963806] -- [0.43011870980262756, 0.7630127668380737, 0.6450105309486389] -- [0.43392035365104675, 0.7583839893341064, 0.6438100337982178] -- [0.4366118907928467, 0.7536150217056274, 0.6432861685752869] -- [0.43859970569610596, 0.7496100068092346, 0.6431535482406616] -- [0.440603107213974, 0.7457420825958252, 0.6433075070381165] -- [0.4426211714744568, 0.7424469590187073, 0.6437684297561646] -- [0.4448736011981964, 0.739434003829956, 0.6436001062393188] -- [0.44715359807014465, 0.7366642355918884, 0.6440704464912415] -- [0.4491988718509674, 0.7341421246528625, 0.6443954110145569] -- [0.4515462815761566, 0.7318232655525208, 0.6447812914848328] -- [0.4538796544075012, 0.7296025156974792, 0.64525306224823] -- [0.4559125304222107, 0.7276254296302795, 0.6461131572723389] -- [0.45823928713798523, 0.7256762981414795, 0.6468055844306946] -- [0.4603967070579529, 0.7237780094146729, 0.6472455859184265] -- [0.4623624384403229, 0.7218862771987915, 0.6476256251335144] -- [0.4643985331058502, 0.7201741933822632, 0.647979199886322] -- [0.46624717116355896, 0.7185177803039551, 0.6483898758888245] -- [0.46810171008110046, 0.7172420024871826, 0.6485509276390076] -- [0.469149112701416, 0.7162048816680908, 0.6482886075973511] -- [0.4702853858470917, 0.7153364419937134, 0.6483362913131714] -- [0.4715483486652374, 0.7145293354988098, 0.6481471061706543] -- [0.47292158007621765, 0.7138490080833435, 0.6481716632843018] -- [0.4742034375667572, 0.7120183706283569, 0.6481285095214844] -- [0.47432035207748413, 0.7108095288276672, 0.6481885313987732] -- [0.47630515694618225, 0.7117964029312134, 0.6486101150512695] -- [0.47767069935798645, 0.7118284702301025, 0.649002194404602] -- [0.4788450002670288, 0.7113872766494751, 0.649338960647583] -- [0.48013532161712646, 0.7113116383552551, 0.6495450735092163] -- [0.481376588344574, 0.7112828493118286, 0.649643063545227] -- [0.48209720849990845, 0.711261510848999, 0.6501650214195251] -- [0.4827598035335541, 0.7112085223197937, 0.6501684784889221] -- [0.483004629611969, 0.7111751437187195, 0.6497430205345154] -- [0.48303893208503723, 0.7111732363700867, 0.6491087079048157] -- [0.4830542504787445, 0.7109795808792114, 0.6481030583381653] -- [0.4827677309513092, 0.7108049988746643, 0.6472161412239075] -- [0.4826340675354004, 0.7103112936019897, 0.64628666639328] -- [0.48238658905029297, 0.7097208499908447, 0.6453120112419128] -- [0.48221006989479065, 0.708998441696167, 0.6444659233093262] -- [0.4823317229747772, 0.7079373002052307, 0.6437838673591614] -- [0.4823478162288666, 0.7069367170333862, 0.6434259414672852] -- [0.48257336020469666, 0.7059409618377686, 0.6428412199020386] -- [0.4828386604785919, 0.7045462727546692, 0.6424157023429871] -- [0.48302987217903137, 0.7032501697540283, 0.6427584886550903] -- [0.4833117127418518, 0.701707124710083, 0.6426293253898621] -- [0.48360466957092285, 0.7001016139984131, 0.6424334049224854] -- [0.4837794601917267, 0.6983014941215515, 0.6427721977233887] -- [0.48373305797576904, 0.6965182423591614, 0.643266499042511] -- [0.48346173763275146, 0.6948007941246033, 0.6435716152191162] -- [0.4829842150211334, 0.6929404139518738, 0.6441323757171631] -- [0.48224639892578125, 0.6912798881530762, 0.6449214816093445] -- [0.4811761677265167, 0.689568281173706, 0.6454342007637024] -- [0.4796297550201416, 0.6879920363426208, 0.6455917358398438] -- [0.4778805375099182, 0.6864671111106873, 0.64582359790802] -- [0.4758743941783905, 0.685044527053833, 0.6461047530174255] -- [0.47421538829803467, 0.683685839176178, 0.6461451053619385] -- [0.4728359580039978, 0.6832396388053894, 0.6467880606651306] -- [0.47744596004486084, 0.6858558654785156, 0.6490106582641602] -- [0.47438845038414, 0.6842909455299377, 0.6489233374595642] -- [0.46499505639076233, 0.6766884922981262, 0.6470143795013428] -- [0.46532607078552246, 0.6773024201393127, 0.6477042436599731] -- [0.4713197946548462, 0.6808158159255981, 0.6499150991439819] -- [0.46415090560913086, 0.6746987104415894, 0.6485685110092163] -- [0.46349719166755676, 0.6726585626602173, 0.64897620677948] -- [0.46851101517677307, 0.6732178330421448, 0.6506823897361755] -- [0.45995408296585083, 0.6654514074325562, 0.6486154198646545] -- [0.45428138971328735, 0.659040629863739, 0.6479024291038513] -- [0.4599442481994629, 0.6598453521728516, 0.6495421528816223] -- [0.4642343819141388, 0.6594170331954956, 0.6511483192443848] -- [0.45454707741737366, 0.652118980884552, 0.6494140028953552] -- [0.4644976258277893, 0.6525009870529175, 0.6519779562950134] -- [0.46329402923583984, 0.648941695690155, 0.6520803570747375] -- [0.45723578333854675, 0.6455585360527039, 0.6515931487083435] -- [0.4464825689792633, 0.6404074430465698, 0.6490567326545715] -- [0.44400593638420105, 0.6379631757736206, 0.6495300531387329] -- [0.4343268573284149, 0.6336970925331116, 0.6482106447219849] -- [0.4362523555755615, 0.6336236000061035, 0.6498032212257385] -- [0.433681458234787, 0.631195604801178, 0.6502341032028198] -- [0.4304560422897339, 0.6287779808044434, 0.6501984596252441] -- [0.4286806285381317, 0.6275057792663574, 0.6499684453010559] -- [0.4172886610031128, 0.6243029832839966, 0.6478973627090454] -- [0.41125616431236267, 0.6221343278884888, 0.646837055683136] -- [0.4025367498397827, 0.619726836681366, 0.6451311707496643] -- [0.3991004228591919, 0.618019163608551, 0.6446719765663147] -- [0.3938097357749939, 0.615874171257019, 0.6441918611526489] -- [0.3851507604122162, 0.6142035722732544, 0.6436111330986023] -- [0.3774474561214447, 0.611906886100769, 0.6430308818817139] -- [0.369101345539093, 0.6102720499038696, 0.6430255174636841] -- [0.3581072688102722, 0.6083061099052429, 0.6418147087097168] -- [0.35093241930007935, 0.6056530475616455, 0.6411526203155518] -- [0.3329690992832184, 0.6034667491912842, 0.6368151307106018] -- [0.3327369689941406, 0.6013708710670471, 0.6394869089126587] -- [0.3181866705417633, 0.5989025235176086, 0.6364871263504028] -- [0.31325337290763855, 0.5965986251831055, 0.6374126672744751] -- [0.29790613055229187, 0.5946273803710938, 0.634992241859436] -- [0.29096829891204834, 0.5926946401596069, 0.6362701058387756] -- [0.2841152846813202, 0.591314435005188, 0.6377403736114502] -- [0.2790948152542114, 0.5902677774429321, 0.6400803327560425] -- [0.2685320973396301, 0.5881856083869934, 0.6407746076583862] -- [0.2583293616771698, 0.5862870216369629, 0.6398017406463623] -- [0.2511390745639801, 0.5849823951721191, 0.6400035619735718] -- [0.24731747806072235, 0.5846656560897827, 0.6417415738105774] -- [0.23668453097343445, 0.5838723182678223, 0.6420932412147522] -- [0.2330145388841629, 0.5842612385749817, 0.6452094912528992] -- [0.22084029018878937, 0.5837811827659607, 0.6457135081291199] -- [0.21324484050273895, 0.5840006470680237, 0.6469805240631104] -- [0.21157869696617126, 0.5853355526924133, 0.6503531336784363] -- [0.2001565545797348, 0.5848962068557739, 0.6500459909439087] -- [0.19712987542152405, 0.5867252349853516, 0.6528177857398987] -- [0.18585990369319916, 0.5863475203514099, 0.6525201797485352] -- [0.1782367080450058, 0.5874607563018799, 0.6541095972061157] -- [0.16952188313007355, 0.5884299874305725, 0.656177818775177] -- [0.16633164882659912, 0.5909998416900635, 0.6602821350097656] -- [0.15695950388908386, 0.5922771692276001, 0.6623891592025757] -- [0.14718951284885406, 0.59334397315979, 0.6644409894943237] -- [0.13886120915412903, 0.5942089557647705, 0.6672263741493225] -- [0.13130895793437958, 0.5952234268188477, 0.670007586479187] -- [0.1233658641576767, 0.5963554382324219, 0.6721153259277344] -- [0.11288781464099884, 0.5959765911102295, 0.6724914312362671] -- [0.1075892522931099, 0.5974639654159546, 0.6749138832092285] -- [0.094733826816082, 0.595089316368103, 0.6755391359329224] -- [0.09059113264083862, 0.5965176820755005, 0.6773249506950378] -- [0.08309799432754517, 0.595528244972229, 0.6778135895729065] -- [0.08285082876682281, 0.5981944799423218, 0.6817030310630798] -- [0.07667618244886398, 0.5978893637657166, 0.6821549534797668] -- [0.07275813072919846, 0.5976263284683228, 0.6837615966796875] -- [0.06982080638408661, 0.5983174443244934, 0.6846948862075806] -- [0.06241185963153839, 0.5963091850280762, 0.6844843626022339] -- [0.0587746687233448, 0.5958021879196167, 0.6852579116821289] -- [0.05999772250652313, 0.5977309942245483, 0.6875126361846924] -- [0.05782834440469742, 0.597504198551178, 0.6892856359481812] -- [0.0574285089969635, 0.5978503227233887, 0.6905521750450134] -- [0.054387517273426056, 0.596425473690033, 0.6909432411193848] -- [0.05305010825395584, 0.5966342091560364, 0.6920120716094971] -- [0.05044187605381012, 0.5958446264266968, 0.6926602125167847] -- [0.05012701451778412, 0.5965681672096252, 0.693936288356781] -- [0.049540020525455475, 0.5961769819259644, 0.6950286626815796] -- [0.05025110021233559, 0.597444474697113, 0.696357011795044] -- [0.051316846162080765, 0.5980649590492249, 0.6976433396339417] -- [0.04431832581758499, 0.5954237580299377, 0.6962690353393555] -- [0.04860289394855499, 0.5980518460273743, 0.6984521746635437] -- [0.04667230695486069, 0.5980333685874939, 0.6985912919044495] -- [0.04884709417819977, 0.5995019674301147, 0.6997407078742981] -- [0.045873720198869705, 0.5985704660415649, 0.6994206309318542] -- [0.04789453744888306, 0.6001632213592529, 0.7005313634872437] -- [0.04811367765069008, 0.6004029512405396, 0.700826108455658] -- [0.04410335794091225, 0.5987416505813599, 0.7001211643218994] -- [0.04330211505293846, 0.5983068346977234, 0.7000572085380554] -- [0.04574555158615112, 0.6001825332641602, 0.7007485628128052] -- [0.04611048847436905, 0.599821150302887, 0.7008519172668457] -- [0.04275120049715042, 0.5972572565078735, 0.6998646855354309] -- [0.04088879004120827, 0.5965113043785095, 0.6989673972129822] -- [0.044703409075737, 0.5979213714599609, 0.6997359991073608] -- [0.04408213123679161, 0.5967466831207275, 0.6989859342575073] -- [0.04292929172515869, 0.5938642024993896, 0.6977770328521729] -- [0.043396688997745514, 0.5933083891868591, 0.6970556378364563] -- [0.04332718998193741, 0.5923863649368286, 0.696139931678772] -- [0.04341833293437958, 0.5902338027954102, 0.6950176954269409] -- [0.043065521866083145, 0.5878646373748779, 0.6935656070709229] -- [0.04573706537485123, 0.5865700840950012, 0.6926249265670776] -- [0.04534045234322548, 0.5841104388237, 0.6908134818077087] -- [0.04635496437549591, 0.5820432305335999, 0.6889920234680176] -- [0.04725879803299904, 0.5800515413284302, 0.6872662305831909] -- [0.04812338948249817, 0.5782386660575867, 0.6847382187843323] -- [0.04893547669053078, 0.5762697458267212, 0.6823220252990723] -- [0.04953065142035484, 0.574240505695343, 0.6797769069671631] -- [0.04981816187500954, 0.5726978182792664, 0.6774319410324097] -- [0.04974338412284851, 0.5715569853782654, 0.6753377318382263] -- [0.04985860735177994, 0.5706406235694885, 0.6733226180076599] -- [0.05007220059633255, 0.5703475475311279, 0.672150194644928] -- [0.050021760165691376, 0.5700369477272034, 0.6714155077934265] -- [0.05065345764160156, 0.5694637894630432, 0.6712489724159241] -- [0.050725191831588745, 0.5690383911132812, 0.6712585687637329] -- [0.05094533786177635, 0.5689038038253784, 0.6718854308128357] -- [0.051923718303442, 0.568211555480957, 0.6726865172386169] -- [0.05343690887093544, 0.5673410296440125, 0.6737146377563477] -- [0.054072305560112, 0.5665611028671265, 0.674443781375885] -- [0.05447153002023697, 0.5653755068778992, 0.6753524541854858] -- [0.05592905730009079, 0.5635805726051331, 0.6766186952590942] -- [0.05539568141102791, 0.5605857372283936, 0.6774356961250305] -- [0.05310526117682457, 0.5579882264137268, 0.6776873469352722] -- [0.05405602231621742, 0.552596390247345, 0.6791455149650574] -- [0.05413475260138512, 0.5471481084823608, 0.6802144050598145] -- [0.05275357514619827, 0.5410318374633789, 0.680884063243866] -- [0.05214224383234978, 0.535284698009491, 0.6816503405570984] -- [0.04825827479362488, 0.5281863808631897, 0.6822742819786072] -- [0.041784800589084625, 0.5202071666717529, 0.6825294494628906] -- [0.03807109594345093, 0.5141134262084961, 0.6823325753211975] -- [0.03202374652028084, 0.5112441182136536, 0.68068927526474] -- [0.028706440702080727, 0.507861852645874, 0.6801895499229431] -- [0.029081840068101883, 0.503221869468689, 0.6807529926300049] -- [0.030256643891334534, 0.4980645179748535, 0.681125819683075] -- [0.03209056705236435, 0.4927304983139038, 0.6817578077316284] -- [0.032503753900527954, 0.4886767268180847, 0.6820586323738098] -- [0.03304366022348404, 0.4848037660121918, 0.6823739409446716] -- [0.035300012677907944, 0.47992143034935, 0.6834171414375305] -- [0.03580415993928909, 0.4766213595867157, 0.6838024258613586] -- [0.03638629615306854, 0.4735788404941559, 0.6841946840286255] -- [0.03699983283877373, 0.4706600606441498, 0.6845024228096008] -- [0.037573594599962234, 0.468018501996994, 0.6847318410873413] -- [0.03807617351412773, 0.4655875861644745, 0.6847332119941711] -- [0.03852290287613869, 0.4636300504207611, 0.6852507591247559] -- [0.03894729167222977, 0.46177226305007935, 0.6856640577316284] -- [0.03933136910200119, 0.46019411087036133, 0.6859051585197449] -- [0.0397023968398571, 0.4587165415287018, 0.6860823631286621] -- [0.039938367903232574, 0.45766139030456543, 0.6861415505409241] -- [0.04011981189250946, 0.45675018429756165, 0.686387836933136] -- [0.04017472639679909, 0.45612767338752747, 0.6866437792778015] -- [0.0408211313188076, 0.4546654522418976, 0.6872696280479431] -- [0.04010528326034546, 0.4552345275878906, 0.6870254874229431] -- [0.04000070318579674, 0.454940527677536, 0.687171995639801] -- [0.03987269476056099, 0.4547751545906067, 0.6873143315315247] -- [0.039716318249702454, 0.4546522796154022, 0.6874592304229736] -- [0.0394749790430069, 0.45468467473983765, 0.687606930732727] -- [0.039278216660022736, 0.45473524928092957, 0.6877489686012268] -- [0.03900749236345291, 0.45479318499565125, 0.6877102851867676] -- [0.03756716474890709, 0.45595383644104004, 0.6871422529220581] -- [0.03722170740365982, 0.456154465675354, 0.6871823072433472] -- [0.036867931485176086, 0.4563728868961334, 0.6872184872627258] -- [0.03653837740421295, 0.45664629340171814, 0.6872098445892334] -- [0.03618302941322327, 0.4569343328475952, 0.6871923804283142] -- [0.03570718318223953, 0.4572679102420807, 0.6870718002319336] -- [0.034208789467811584, 0.45844095945358276, 0.6865115761756897] -- [0.03597385063767433, 0.4568258821964264, 0.6872975826263428] -- [0.03536232188344002, 0.4572228193283081, 0.6870554089546204] -- [0.034427061676979065, 0.4578537046909332, 0.686882495880127] -- [0.03350093215703964, 0.45850053429603577, 0.6867141723632812] -- [0.03370197117328644, 0.45789635181427, 0.6871911287307739] -- [0.03270214796066284, 0.4585358202457428, 0.6870582103729248] -- [0.030088694766163826, 0.4608348309993744, 0.6860417127609253] -- [0.030552707612514496, 0.45984378457069397, 0.6866603493690491] -- [0.02935008704662323, 0.46063169836997986, 0.6865829229354858] -- [0.028182774782180786, 0.4613931179046631, 0.6865288019180298] -- [0.027082841843366623, 0.46206405758857727, 0.686521053314209] -- [0.025992847979068756, 0.4627309739589691, 0.6865097284317017] -- [0.02494093030691147, 0.4632953405380249, 0.6864644885063171] -- [0.023888470605015755, 0.4638531804084778, 0.686424970626831] -- [0.022929983213543892, 0.46434342861175537, 0.6864909529685974] -- [0.02193516306579113, 0.46485093235969543, 0.6865877509117126] -- [0.02156922034919262, 0.4652937948703766, 0.6867506504058838] -- [0.02097693830728531, 0.4657362699508667, 0.686912477016449] -- [0.020327933132648468, 0.4661023020744324, 0.6870892643928528] -- [0.01955726556479931, 0.46645134687423706, 0.6872544884681702] -- [0.01749192550778389, 0.46665284037590027, 0.6872867345809937] -- [0.0169498473405838, 0.46698349714279175, 0.6874601244926453] -- [0.01657729223370552, 0.4672447741031647, 0.6876496076583862] -- [0.016224650666117668, 0.46749675273895264, 0.6878435611724854] -- [0.016332020983099937, 0.46754682064056396, 0.6881086230278015] -- [0.01597767509520054, 0.4676271378993988, 0.6883566975593567] -- [0.014897152781486511, 0.4675779640674591, 0.6882597804069519] -- [0.013780118897557259, 0.46755948662757874, 0.688257098197937] -- [0.012635286897420883, 0.4674469828605652, 0.6882668137550354] -- [0.011114850640296936, 0.46736133098602295, 0.6881764531135559] -- [0.009411902166903019, 0.4670688509941101, 0.6879346966743469] -- [0.007759887725114822, 0.46675053238868713, 0.687750518321991] -- [0.005888411775231361, 0.46629247069358826, 0.6875993013381958] -- [0.004156262613832951, 0.46579304337501526, 0.6874815225601196] -- [0.0041483547538518906, 0.46533143520355225, 0.6875309944152832] -- [0.0026383139193058014, 0.4647531509399414, 0.6873940825462341] -left_hand_palm_link_wxyz: -- [0.605830729007721, 0.47541749477386475, 0.3217238783836365, -0.5508547425270081] -- [0.6052213311195374, 0.47565439343452454, 0.3216572105884552, -0.5513588190078735] -- [0.5896639823913574, 0.5043022036552429, 0.29774197936058044, -0.5561703443527222] -- [0.5904529690742493, 0.504948616027832, 0.2973687946796417, -0.5549449324607849] -- [0.5908435583114624, 0.5062258839607239, 0.2973703444004059, -0.5533626079559326] -- [0.5903875231742859, 0.5077472925186157, 0.2963957190513611, -0.5529781579971313] -- [0.5899660587310791, 0.5093912482261658, 0.29530882835388184, -0.552497148513794] -- [0.5894832611083984, 0.5110458135604858, 0.2944691479206085, -0.5519324541091919] -- [0.5893369913101196, 0.511925995349884, 0.2935364842414856, -0.5517699122428894] -- [0.5882688760757446, 0.5129535794258118, 0.29325246810913086, -0.5521061420440674] -- [0.5845757722854614, 0.5120448470115662, 0.2951287031173706, -0.5558598637580872] -- [0.5828249454498291, 0.5124898552894592, 0.29603081941604614, -0.5568077564239502] -- [0.5835207104682922, 0.5205548405647278, 0.29261547327041626, -0.5503656268119812] -- [0.5831590890884399, 0.5205303430557251, 0.294132798910141, -0.5499629974365234] -- [0.5835499167442322, 0.5198870897293091, 0.2954760491847992, -0.5494366884231567] -- [0.5698981285095215, 0.531712532043457, 0.29083847999572754, -0.5548970103263855] -- [0.567654013633728, 0.5336675643920898, 0.2922242283821106, -0.5545923113822937] -- [0.564704179763794, 0.5355643630027771, 0.2927875220775604, -0.5554775595664978] -- [0.5609463453292847, 0.5370185971260071, 0.2919102609157562, -0.5583356022834778] -- [0.5586708784103394, 0.537796676158905, 0.2934681475162506, -0.5590509176254272] -- [0.5568087100982666, 0.5384842157363892, 0.29482707381248474, -0.5595316886901855] -- [0.5550106763839722, 0.5388746857643127, 0.2962152063846588, -0.5602085590362549] -- [0.5502920150756836, 0.5406429767608643, 0.2971250116825104, -0.5626726746559143] -- [0.5376488566398621, 0.5457426905632019, 0.2961733937263489, -0.5704208612442017] -- [0.5342586636543274, 0.546444296836853, 0.2969839572906494, -0.5725090503692627] -- [0.5326054692268372, 0.546633243560791, 0.2974112331867218, -0.5736461281776428] -- [0.5292843580245972, 0.5482154488563538, 0.2976171672344208, -0.5751014351844788] -- [0.526573121547699, 0.5491476655006409, 0.2968323230743408, -0.5771030783653259] -- [0.5231746435165405, 0.5510808229446411, 0.2950417995452881, -0.5792653560638428] -- [0.5203917622566223, 0.552168607711792, 0.2927781045436859, -0.5818789005279541] -- [0.5180112719535828, 0.5526155233383179, 0.291598916053772, -0.5841664671897888] -- [0.5161440372467041, 0.5524161458015442, 0.289266973733902, -0.5871595144271851] -- [0.5163960456848145, 0.5504549741744995, 0.28748682141304016, -0.5896487832069397] -- [0.5185532569885254, 0.5482177138328552, 0.2854086756706238, -0.5908482074737549] -- [0.5212035179138184, 0.5451591610908508, 0.2836899757385254, -0.592172384262085] -- [0.5230327844619751, 0.5418428778648376, 0.2808854579925537, -0.5949338674545288] -- [0.52388596534729, 0.5402945280075073, 0.27721261978149414, -0.5973094701766968] -- [0.531484067440033, 0.5267724990844727, 0.2854183316230774, -0.5988087058067322] -- [0.5349757671356201, 0.5165293216705322, 0.28875747323036194, -0.6030069589614868] -- [0.5328705310821533, 0.5103790760040283, 0.28846392035484314, -0.610205352306366] -- [0.5383134484291077, 0.5017877221107483, 0.2890678346157074, -0.612264096736908] -- [0.5472444891929626, 0.4906897246837616, 0.29240062832832336, -0.6117587685585022] -- [0.5557157397270203, 0.4792252480983734, 0.29789504408836365, -0.6105585098266602] -- [0.5560460090637207, 0.47840967774391174, 0.2941843569278717, -0.6126928925514221] -- [0.5564554929733276, 0.47759783267974854, 0.2903224527835846, -0.6147929430007935] -- [0.557668149471283, 0.4767085611820221, 0.28600946068763733, -0.6164038181304932] -- [0.5590316653251648, 0.4757217764854431, 0.28182801604270935, -0.6178553700447083] -- [0.5612191557884216, 0.47461652755737305, 0.27799108624458313, -0.6184602379798889] -- [0.563510537147522, 0.4735024869441986, 0.27403026819229126, -0.6189980506896973] -- [0.5664854049682617, 0.47197195887565613, 0.2711305022239685, -0.618728518486023] -- [0.5695103406906128, 0.4704640805721283, 0.26800599694252014, -0.6184611916542053] -- [0.573786199092865, 0.4677519202232361, 0.26564672589302063, -0.6175832748413086] -- [0.5773517489433289, 0.4658501446247101, 0.2637444734573364, -0.6165121793746948] -- [0.581085741519928, 0.4636169672012329, 0.2622579336166382, -0.6153204441070557] -- [0.5848393440246582, 0.4611814618110657, 0.2618046998977661, -0.6137855648994446] -- [0.5804253816604614, 0.47135263681411743, 0.25264590978622437, -0.6140871047973633] -- [0.5907363891601562, 0.4749346077442169, 0.248149573802948, -0.6032322645187378] -- [0.6044118404388428, 0.47582074999809265, 0.24605593085289001, -0.5896925330162048] -- [0.6085355281829834, 0.47414323687553406, 0.24598728120326996, -0.5868242383003235] -- [0.6100794672966003, 0.4739852249622345, 0.24264200031757355, -0.5867416262626648] -- [0.6135164499282837, 0.4721207916736603, 0.2438596934080124, -0.5841505527496338] -- [0.6171492338180542, 0.4714870750904083, 0.24634592235088348, -0.5797761082649231] -- [0.6209723949432373, 0.47026753425598145, 0.24927948415279388, -0.5754140615463257] -- [0.6244460344314575, 0.4694383442401886, 0.2529999613761902, -0.5706889033317566] -- [0.627631664276123, 0.4691067039966583, 0.2570039629936218, -0.5656554698944092] -- [0.6303136348724365, 0.46906164288520813, 0.261893630027771, -0.5604441165924072] -- [0.6325914859771729, 0.46918925642967224, 0.26686131954193115, -0.5554046034812927] -- [0.6181395053863525, 0.48555788397789, 0.26251086592674255, -0.5596650838851929] -- [0.6207042336463928, 0.48567041754722595, 0.26629745960235596, -0.554919958114624] -- [0.6232611536979675, 0.48543357849121094, 0.27235814929008484, -0.5492911338806152] -- [0.6226007342338562, 0.4870685636997223, 0.2797377407550812, -0.5448662042617798] -- [0.612623393535614, 0.4957117736339569, 0.2837030589580536, -0.5463284850120544] -- [0.6098916530609131, 0.4976128935813904, 0.28963974118232727, -0.5445385575294495] -- [0.6111258268356323, 0.49717217683792114, 0.29658210277557373, -0.5397999882698059] -- [0.6113219857215881, 0.49771803617477417, 0.3034307658672333, -0.5352494120597839] -- [0.6095343828201294, 0.4992796778678894, 0.3097281754016876, -0.5322179794311523] -- [0.6109204292297363, 0.49847012758255005, 0.3164079487323761, -0.5274368524551392] -- [0.6095013618469238, 0.49899420142173767, 0.32325872778892517, -0.5244201421737671] -- [0.6094615459442139, 0.498781681060791, 0.32994723320007324, -0.5204882025718689] -- [0.6064494848251343, 0.49942678213119507, 0.33606958389282227, -0.5194699168205261] -- [0.6035479307174683, 0.4999329447746277, 0.3413633108139038, -0.5189103484153748] -- [0.602729082107544, 0.4991615414619446, 0.34725555777549744, -0.516690194606781] -- [0.6016503572463989, 0.4989027678966522, 0.35263490676879883, -0.5145496726036072] -- [0.5999877452850342, 0.49850013852119446, 0.35769662261009216, -0.513386070728302] -- [0.6005489230155945, 0.49665263295173645, 0.363639771938324, -0.5103362202644348] -- [0.601894199848175, 0.4935568869113922, 0.3698897361755371, -0.5072539448738098] -- [0.6026699542999268, 0.4911282956600189, 0.3753531277179718, -0.5046699643135071] -- [0.602242112159729, 0.49016109108924866, 0.37940114736557007, -0.5030916333198547] -- [0.601519763469696, 0.49007800221443176, 0.3825715482234955, -0.5016337037086487] -- [0.5992456674575806, 0.4914120137691498, 0.3850535452365875, -0.5011511445045471] -- [0.597967803478241, 0.4913083612918854, 0.38817840814590454, -0.5003677606582642] -- [0.5931106209754944, 0.4955570697784424, 0.3881528377532959, -0.5019763112068176] -- [0.5900020599365234, 0.49714797735214233, 0.3894880712032318, -0.5030311942100525] -- [0.5878357291221619, 0.4974232614040375, 0.3908849358558655, -0.504210352897644] -- [0.5857403874397278, 0.49739763140678406, 0.3924464285373688, -0.5054596066474915] -- [0.5839669704437256, 0.49675455689430237, 0.3937318027019501, -0.5071415901184082] -- [0.5836220383644104, 0.4957624673843384, 0.39595431089401245, -0.506779134273529] -- [0.5846906304359436, 0.4940640330314636, 0.3986690938472748, -0.5050746202468872] -- [0.5866681337356567, 0.4903770685195923, 0.4035514295101166, -0.5024908185005188] -- [0.5890686511993408, 0.48590514063835144, 0.40758731961250305, -0.5007662177085876] -- [0.5892429947853088, 0.48371413350105286, 0.41076910495758057, -0.5000820159912109] -- [0.588397204875946, 0.4821672737598419, 0.4135218560695648, -0.5003028512001038] -- [0.5883346796035767, 0.4804362952709198, 0.41636931896209717, -0.49967971444129944] -- [0.5889776349067688, 0.479096919298172, 0.4189758896827698, -0.49802666902542114] -- [0.5885369181632996, 0.47885191440582275, 0.4200226068496704, -0.49790158867836] -- [0.590351402759552, 0.4771982431411743, 0.42241793870925903, -0.4953078329563141] -- [0.5911574959754944, 0.4763149321079254, 0.42443057894706726, -0.4934729039669037] -- [0.5932456851005554, 0.47441574931144714, 0.4268849194049835, -0.49067121744155884] -- [0.5953317284584045, 0.47221675515174866, 0.4294614791870117, -0.4880104959011078] -- [0.5967695713043213, 0.4709683358669281, 0.43139535188674927, -0.4857498109340668] -- [0.599343478679657, 0.46833133697509766, 0.43420636653900146, -0.4826156497001648] -- [0.6008043885231018, 0.46632593870162964, 0.4364180266857147, -0.48074254393577576] -- [0.6020176410675049, 0.4643101394176483, 0.43868735432624817, -0.47910770773887634] -- [0.6038797497749329, 0.46229109168052673, 0.44085606932640076, -0.47672009468078613] -- [0.6051138043403625, 0.4604071378707886, 0.4429111182689667, -0.4750708043575287] -- [0.6066965460777283, 0.4579851031303406, 0.4453078508377075, -0.47314879298210144] -- [0.6050982475280762, 0.45815226435661316, 0.44652044773101807, -0.4738902151584625] -- [0.6041755676269531, 0.4575589597225189, 0.44826647639274597, -0.4739922285079956] -- [0.6039950847625732, 0.45658615231513977, 0.45021852850914, -0.47330981492996216] -- [0.6046154499053955, 0.4560639262199402, 0.45175352692604065, -0.47155529260635376] -- [0.6159611940383911, 0.44617682695388794, 0.45718151330947876, -0.46098050475120544] -- [0.6225770711898804, 0.4357067048549652, 0.4641038775444031, -0.45515361428260803] -- [0.6150585412979126, 0.44611015915870667, 0.460980087518692, -0.4584604501724243] -- [0.6147016286849976, 0.4471111595630646, 0.46240144968032837, -0.45652857422828674] -- [0.6181318759918213, 0.4421691596508026, 0.46721717715263367, -0.4517824649810791] -- [0.6188826560974121, 0.4423738121986389, 0.46923840045928955, -0.4484470784664154] -- [0.6180059909820557, 0.44366124272346497, 0.4708153009414673, -0.44672825932502747] -- [0.6172759532928467, 0.44522032141685486, 0.4722306430339813, -0.4446881413459778] -- [0.6159241795539856, 0.4470224678516388, 0.47302475571632385, -0.44390955567359924] -- [0.6151631474494934, 0.4485725164413452, 0.47387921810150146, -0.4424876272678375] -- [0.6136831641197205, 0.44995319843292236, 0.47473248839378357, -0.4422261416912079] -- [0.6133414506912231, 0.4514967203140259, 0.47497761249542236, -0.44086170196533203] -- [0.6125040650367737, 0.452348530292511, 0.4754566252231598, -0.4406364858150482] -- [0.6121068000793457, 0.45338886976242065, 0.47571414709091187, -0.4398406147956848] -- [0.611274003982544, 0.45464643836021423, 0.4754784405231476, -0.4399554431438446] -- [0.6110357642173767, 0.45526739954948425, 0.47562459111213684, -0.43948614597320557] -- [0.610264778137207, 0.45638176798820496, 0.4753386974334717, -0.4397108554840088] -- [0.6101449728012085, 0.456735223531723, 0.4753660559654236, -0.43948036432266235] -- [0.6099445223808289, 0.4576318562030792, 0.4747909605503082, -0.43944746255874634] -- [0.6093342900276184, 0.45847228169441223, 0.47397369146347046, -0.4402996599674225] -- [0.6098538041114807, 0.458432137966156, 0.4738693833351135, -0.43973419070243835] -- [0.6089266538619995, 0.45960545539855957, 0.4727455675601959, -0.4410020112991333] -- [0.6076735854148865, 0.46120598912239075, 0.4713607132434845, -0.4425392150878906] -- [0.6072230935096741, 0.4622395634651184, 0.4698009788990021, -0.44373586773872375] -- [0.6068334579467773, 0.4626460075378418, 0.4686812460422516, -0.44502755999565125] -- [0.6055033802986145, 0.46397867798805237, 0.46674343943595886, -0.44748184084892273] -- [0.6045954823493958, 0.46425896883010864, 0.4654855728149414, -0.4497233033180237] -- [0.6050114631652832, 0.4646449089050293, 0.46401169896125793, -0.4502878785133362] -- [0.604533314704895, 0.4651728570461273, 0.4622522294521332, -0.4521908462047577] -- [0.603833019733429, 0.46565866470336914, 0.46034666895866394, -0.45456403493881226] -- [0.6036673188209534, 0.46584177017211914, 0.45855313539505005, -0.45640549063682556] -- [0.6032964587211609, 0.4662383496761322, 0.45646587014198303, -0.4585782289505005] -- [0.6030304431915283, 0.4666881859302521, 0.4545776844024658, -0.46034255623817444] -- [0.5960296392440796, 0.47618359327316284, 0.44831106066703796, -0.4658486545085907] -- [0.5882868766784668, 0.4898602366447449, 0.4415760934352875, -0.46793797612190247] -- [0.5861655473709106, 0.49132972955703735, 0.43977856636047363, -0.47074371576309204] -- [0.6030364036560059, 0.4665940999984741, 0.4462708830833435, -0.46848610043525696] -- [0.5919480323791504, 0.48388609290122986, 0.4368630051612854, -0.4739223122596741] -- [0.5731825828552246, 0.5091345310211182, 0.4235953092575073, -0.48250460624694824] -- [0.5891367793083191, 0.4904218018054962, 0.42939314246177673, -0.47752031683921814] -- [0.5887266993522644, 0.4919174611568451, 0.4254957139492035, -0.47997021675109863] -- [0.5740063190460205, 0.5131624937057495, 0.40993866324424744, -0.4890103340148926] -- [0.596211314201355, 0.4840586483478546, 0.42001450061798096, -0.48353588581085205] -- [0.6024676561355591, 0.4667299687862396, 0.42290645837783813, -0.49025073647499084] -- [0.5986126661300659, 0.4854567050933838, 0.4093392491340637, -0.4882989525794983] -- [0.5852837562561035, 0.5029665231704712, 0.3944566249847412, -0.4988701343536377] -- [0.6039446592330933, 0.47646215558052063, 0.4036150574684143, -0.49530741572380066] -- [0.5932141542434692, 0.498834490776062, 0.3835746645927429, -0.5021268725395203] -- [0.5966038107872009, 0.4940548539161682, 0.3803417384624481, -0.5052857995033264] -- [0.6046720743179321, 0.48083534836769104, 0.3838050663471222, -0.5058286190032959] -- [0.6102407574653625, 0.46681302785873413, 0.3876016139984131, -0.5093689560890198] -- [0.6111149787902832, 0.4620416462421417, 0.3860762417316437, -0.513810396194458] -- [0.6109772324562073, 0.45300188660621643, 0.38894492387771606, -0.5198248028755188] -- [0.6103613376617432, 0.4615597426891327, 0.37964150309562683, -0.5198979377746582] -- [0.614709734916687, 0.4556020498275757, 0.3777320384979248, -0.5214183926582336] -- [0.6213704943656921, 0.4503237009048462, 0.37484198808670044, -0.5201929807662964] -- [0.6160814166069031, 0.45617783069610596, 0.3678922653198242, -0.5263084173202515] -- [0.6196591854095459, 0.4449926018714905, 0.3701481223106384, -0.5300890207290649] -- [0.6213489174842834, 0.44108280539512634, 0.36748021841049194, -0.5332256555557251] -- [0.6206229329109192, 0.4382682740688324, 0.36572781205177307, -0.5375788807868958] -- [0.6227954030036926, 0.4373590350151062, 0.35967931151390076, -0.5398830771446228] -- [0.6291430592536926, 0.4338739812374115, 0.35630208253860474, -0.5375695824623108] -- [0.6298345923423767, 0.43335026502609253, 0.35226768255233765, -0.5398364067077637] -- [0.6314958333969116, 0.4303053319454193, 0.35033389925956726, -0.5415868759155273] -- [0.6246452927589417, 0.43049970269203186, 0.3477575480937958, -0.5509564280509949] -- [0.6259458065032959, 0.4304195046424866, 0.34499460458755493, -0.5512799024581909] -- [0.6333072781562805, 0.42805272340774536, 0.3415360152721405, -0.5468508005142212] -- [0.6247442960739136, 0.4296379089355469, 0.3433184027671814, -0.5542907118797302] -- [0.6366766095161438, 0.423238068819046, 0.33827918767929077, -0.548707127571106] -- [0.6304870843887329, 0.42410942912101746, 0.33965104818344116, -0.5543052554130554] -- [0.6439143419265747, 0.4221782386302948, 0.3350098729133606, -0.5430544018745422] -- [0.6365887522697449, 0.424530029296875, 0.33890971541404724, -0.5474205017089844] -- [0.6413958668708801, 0.42317625880241394, 0.338196724653244, -0.5432826280593872] -- [0.6471153497695923, 0.4216894507408142, 0.33973485231399536, -0.5366561412811279] -- [0.6518397927284241, 0.4233563244342804, 0.3392997980117798, -0.5298582315444946] -- [0.6546500325202942, 0.4215187132358551, 0.3426481783390045, -0.5256876349449158] -- [0.6540453433990479, 0.4227842688560486, 0.34535592794418335, -0.5236480832099915] -- [0.6567636132240295, 0.42284122109413147, 0.3460199534893036, -0.5197468996047974] -- [0.6623571515083313, 0.4237622320652008, 0.34465107321739197, -0.5127612352371216] -- [0.6591055393218994, 0.426731139421463, 0.34668686985969543, -0.5131165385246277] -- [0.6614502668380737, 0.4269796907901764, 0.34577062726020813, -0.5105041265487671] -- [0.659313440322876, 0.43203213810920715, 0.3468249440193176, -0.5082975625991821] -- [0.6579991579055786, 0.43440285325050354, 0.347260057926178, -0.5076826810836792] -- [0.6603073477745056, 0.44033417105674744, 0.34295156598091125, -0.5024780631065369] -- [0.6554040908813477, 0.44104278087615967, 0.34700146317481995, -0.5054864287376404] -- [0.6478169560432434, 0.44839999079704285, 0.3446827232837677, -0.5103572010993958] -- [0.6485511660575867, 0.4498825967311859, 0.34589242935180664, -0.5072921514511108] -- [0.6436391472816467, 0.45519566535949707, 0.34472453594207764, -0.509598433971405] -- [0.6396308541297913, 0.45764169096946716, 0.3453714847564697, -0.5120106935501099] -- [0.6329548954963684, 0.4708479940891266, 0.33788347244262695, -0.5133273005485535] -- [0.6284813284873962, 0.47460806369781494, 0.3372431993484497, -0.5157763361930847] -- [0.6237640976905823, 0.47808346152305603, 0.33718737959861755, -0.5183233618736267] -- [0.6194633841514587, 0.4826779365539551, 0.3352011740207672, -0.520506739616394] -- [0.6137616634368896, 0.487598180770874, 0.33277809619903564, -0.524216890335083] -- [0.6057374477386475, 0.49362054467201233, 0.331020325422287, -0.5290051102638245] -- [0.605797290802002, 0.49237287044525146, 0.3316921889781952, -0.5296780467033386] -- [0.5918391346931458, 0.5014719367027283, 0.3275834321975708, -0.5393896698951721] -- [0.6019476056098938, 0.4909009337425232, 0.3332960903644562, -0.5344052314758301] -- [0.5922077298164368, 0.5017074346542358, 0.3273083567619324, -0.5389329195022583] -- [0.5946271419525146, 0.4974607229232788, 0.32839474081993103, -0.5395445227622986] -- [0.5767436027526855, 0.5179247856140137, 0.31806761026382446, -0.5458512902259827] -- [0.5775442123413086, 0.5153389573097229, 0.31989148259162903, -0.5463860034942627] -- [0.5785865783691406, 0.51712965965271, 0.31765514612197876, -0.5448940992355347] -- [0.5718418955802917, 0.5227462649345398, 0.31434473395347595, -0.5485622882843018] -- [0.579484760761261, 0.5119690299034119, 0.31870439648628235, -0.548190176486969] -- [0.5808974504470825, 0.5090035796165466, 0.3196660876274109, -0.5488961935043335] -- [0.5694974660873413, 0.5241345763206482, 0.3110477328300476, -0.5515475869178772] -- [0.5687627792358398, 0.5253978371620178, 0.3101845979690552, -0.551589846611023] -- [0.5635331869125366, 0.5299196243286133, 0.30685216188430786, -0.5544882416725159] -- [0.5701899528503418, 0.524466335773468, 0.3070634603500366, -0.5527480840682983] -- [0.567963719367981, 0.5267046093940735, 0.3045691251754761, -0.5542895793914795] -- [0.5707684755325317, 0.522990345954895, 0.3043941557407379, -0.5550212264060974] -- [0.5651517510414124, 0.5289255976676941, 0.3006877899169922, -0.5571606159210205] -- [0.5675321817398071, 0.5283797979354858, 0.2984478771686554, -0.556462824344635] -- [0.5597181916236877, 0.5349113941192627, 0.2953035831451416, -0.5598045587539673] -- [0.5551397800445557, 0.5417863726615906, 0.28970521688461304, -0.5606765747070312] -- [0.569133996963501, 0.5234906673431396, 0.29750436544418335, -0.5599420070648193] -- [0.55707186460495, 0.5394166707992554, 0.28894972801208496, -0.5614344477653503] -- [0.5494567155838013, 0.5408627986907959, 0.2850167453289032, -0.5694999098777771] -- [0.5491347312927246, 0.5460636615753174, 0.28379401564598083, -0.5654435157775879] -- [0.5567008256912231, 0.5390335321426392, 0.28577232360839844, -0.5637917518615723] -- [0.5461950302124023, 0.5483222603797913, 0.2806898355484009, -0.5676501989364624] -- [0.5437853336334229, 0.5522537231445312, 0.2770827114582062, -0.5679244995117188] -- [0.5554977059364319, 0.5399916172027588, 0.2819930613040924, -0.5659602284431458] -- [0.5562026500701904, 0.5389498472213745, 0.2811325192451477, -0.566688597202301] -- [0.5423479676246643, 0.5516027212142944, 0.27561813592910767, -0.5706377625465393] -- [0.5395405292510986, 0.5537364482879639, 0.27326613664627075, -0.5723612904548645] -- [0.5455580949783325, 0.5429295301437378, 0.2757096290588379, -0.5758281946182251] -- [0.5464562773704529, 0.5408470034599304, 0.2761549651622772, -0.5767220258712769] -- [0.5346941947937012, 0.5539309978485107, 0.27043405175209045, -0.5780379176139832] -- [0.5358373522758484, 0.5526199340820312, 0.2701297104358673, -0.5783764123916626] -- [0.5475960969924927, 0.5458725094795227, 0.2700052559375763, -0.5738106369972229] -- [0.549984872341156, 0.5451483726501465, 0.2728123962879181, -0.570879340171814] -- [0.5465328097343445, 0.5456241369247437, 0.27299046516418457, -0.5736479759216309] -- [0.5528029799461365, 0.5408037304878235, 0.27510133385658264, -0.5711910724639893] -- [0.5623404383659363, 0.5314310789108276, 0.2809356153011322, -0.5678285956382751] -- [0.5587338209152222, 0.5377407073974609, 0.27634793519973755, -0.567699670791626] -- [0.5676732659339905, 0.5285486578941345, 0.2828420102596283, -0.564254879951477] -- [0.5705414414405823, 0.526236355304718, 0.2846084535121918, -0.5626327991485596] -- [0.5734310746192932, 0.5238320827484131, 0.2869080901145935, -0.560767650604248] -- [0.5764482021331787, 0.5203273296356201, 0.2888258695602417, -0.5599521994590759] -- [0.5792890787124634, 0.5170581936836243, 0.2909735441207886, -0.5589357614517212] -- [0.5815040469169617, 0.5134240984916687, 0.293163537979126, -0.558841347694397] -- [0.583279550075531, 0.5100340843200684, 0.2955368459224701, -0.5588452219963074] -- [0.5841069221496582, 0.5068795680999756, 0.29777592420578003, -0.5596621632575989] -- [0.5847530961036682, 0.5034653544425964, 0.29975980520248413, -0.5610083341598511] -- [0.5850265026092529, 0.5008823275566101, 0.3019326329231262, -0.561869740486145] -- [0.5805141925811768, 0.5033347606658936, 0.3004001975059509, -0.5651699304580688] -- [0.578582227230072, 0.5047568678855896, 0.2991100549697876, -0.5665652751922607] -- [0.5763450860977173, 0.5057865977287292, 0.2987888753414154, -0.5680944323539734] -- [0.5745751261711121, 0.5059393048286438, 0.29985925555229187, -0.5691865086555481] -- [0.5781369209289551, 0.5045451521873474, 0.30106469988822937, -0.5661729574203491] -- [0.5837317705154419, 0.5026981234550476, 0.30200764536857605, -0.5615541934967041] -- [0.5868849754333496, 0.5029292106628418, 0.30135029554367065, -0.5584049224853516] -- [0.5877446532249451, 0.504534125328064, 0.29893988370895386, -0.5573474764823914] -- [0.5930190682411194, 0.5029398202896118, 0.29865431785583496, -0.5533401966094971] -- [0.6002613306045532, 0.49581146240234375, 0.30177584290504456, -0.5502621531486511] -- [0.599294900894165, 0.4937947690486908, 0.30339744687080383, -0.552233874797821] -- [0.610810399055481, 0.483193963766098, 0.30711060762405396, -0.5469162464141846] -- [0.6218051314353943, 0.4713576138019562, 0.3129480183124542, -0.5415198802947998] -- [0.6275354027748108, 0.46163615584373474, 0.3167519271373749, -0.541072428226471] -- [0.627930760383606, 0.45882731676101685, 0.3164196014404297, -0.5431932210922241] -- [0.6373149156570435, 0.43904489278793335, 0.32779181003570557, -0.5418685674667358] -- [0.6463299989700317, 0.41469448804855347, 0.34031549096107483, -0.5426520109176636] -- [0.6469957828521729, 0.40539786219596863, 0.3441571593284607, -0.5464473366737366] -- [0.6268531084060669, 0.42015984654426575, 0.33789128065109253, -0.5624502897262573] -- [0.6172111630439758, 0.42312586307525635, 0.3391743004322052, -0.5700663328170776] -- [0.615278422832489, 0.42245805263519287, 0.33873555064201355, -0.5729047060012817] -- [0.6182524561882019, 0.41587966680526733, 0.34067919850349426, -0.5733633637428284] -- [0.6219627261161804, 0.4096446931362152, 0.3414091169834137, -0.5734050273895264] -- [0.6194449663162231, 0.40975913405418396, 0.3393293023109436, -0.5772701501846313] -- [0.6170452237129211, 0.4098348617553711, 0.33725300431251526, -0.5809913277626038] -- [0.6221842765808105, 0.40379875898361206, 0.33641868829727173, -0.5802204012870789] -- [0.6191820502281189, 0.4052143394947052, 0.33371296525001526, -0.5839953422546387] -- [0.6174153089523315, 0.40562939643859863, 0.3315679728984833, -0.5867927670478821] -- [0.6157835721969604, 0.4058533310890198, 0.32945388555526733, -0.5895369052886963] -- [0.6145699620246887, 0.40574735403060913, 0.32756176590919495, -0.5919256806373596] -- [0.6130501627922058, 0.40599340200424194, 0.3255983889102936, -0.5944110155105591] -- [0.6131923794746399, 0.40502339601516724, 0.3247888386249542, -0.5953680276870728] -- [0.6134875416755676, 0.40377143025398254, 0.32411065697669983, -0.5962833166122437] -- [0.614556074142456, 0.40238016843795776, 0.3237413763999939, -0.5963241457939148] -- [0.6157084107398987, 0.4010782241821289, 0.3233022391796112, -0.5962507128715515] -- [0.6168661713600159, 0.4003908336162567, 0.3228267729282379, -0.595773458480835] -- [0.6176857948303223, 0.39986562728881836, 0.3224986791610718, -0.5954545736312866] -- [0.6189001798629761, 0.3996163010597229, 0.3221628963947296, -0.5945420265197754] -- [0.6205637454986572, 0.4009520411491394, 0.3175983726978302, -0.5943644642829895] -- [0.6218019723892212, 0.39880824089050293, 0.3222194314002991, -0.5920208096504211] -- [0.6234899163246155, 0.3982800841331482, 0.32255977392196655, -0.5904136896133423] -- [0.6252334117889404, 0.3978668451309204, 0.3230348825454712, -0.5885859131813049] -- [0.6270069479942322, 0.39737626910209656, 0.32354283332824707, -0.5867489576339722] -- [0.6287899613380432, 0.3969005048274994, 0.3240818381309509, -0.584862470626831] -- [0.630576491355896, 0.39651304483413696, 0.3245159089565277, -0.582957923412323] -- [0.6319302320480347, 0.39634376764297485, 0.3250879943370819, -0.5812859535217285] -- [0.6288332939147949, 0.3994947075843811, 0.3261882960796356, -0.5818709135055542] -- [0.6298885345458984, 0.3993054926395416, 0.32709887623786926, -0.5803461670875549] -- [0.6309000849723816, 0.3991307318210602, 0.3280157446861267, -0.5788481831550598] -- [0.6316189169883728, 0.3991188108921051, 0.3290035128593445, -0.5775103569030762] -- [0.6322990655899048, 0.39911311864852905, 0.3300030827522278, -0.5761983394622803] -- [0.6326889991760254, 0.39921367168426514, 0.3310750126838684, -0.5750845670700073] -- [0.6271697878837585, 0.4051845073699951, 0.32964858412742615, -0.5777674317359924] -- [0.6392030119895935, 0.3941892087459564, 0.33400285243988037, -0.5696283578872681] -- [0.6394650340080261, 0.39404118061065674, 0.33529555797576904, -0.5686762928962708] -- [0.6390766501426697, 0.394318163394928, 0.3365824222564697, -0.5681605339050293] -- [0.6385873556137085, 0.394775390625, 0.3376968502998352, -0.5677316188812256] -- [0.641857922077179, 0.3928220570087433, 0.3365783393383026, -0.5660602450370789] -- [0.6424869894981384, 0.39133867621421814, 0.3390273451805115, -0.564911425113678] -- [0.6363328099250793, 0.3959101438522339, 0.3413632810115814, -0.5672800540924072] -- [0.643408477306366, 0.3886936902999878, 0.3433569371700287, -0.5630707740783691] -- [0.6429495215415955, 0.38867640495300293, 0.3446843922138214, -0.5627958178520203] -- [0.6425456404685974, 0.3886481523513794, 0.3459393382072449, -0.5625066161155701] -- [0.6424807906150818, 0.38833948969841003, 0.34701719880104065, -0.562129557132721] -- [0.6424357295036316, 0.38804692029953003, 0.3480702340602875, -0.5617321133613586] -- [0.6427538394927979, 0.3876475989818573, 0.34912940859794617, -0.5609862208366394] -- [0.6430670022964478, 0.3872639834880829, 0.3501488268375397, -0.56025630235672] -- [0.6432743072509766, 0.3872794508934021, 0.35062190890312195, -0.5597115159034729] -- [0.6432889699935913, 0.387462317943573, 0.35108262300491333, -0.5592789649963379] -- [0.6435575485229492, 0.3877462148666382, 0.35118699073791504, -0.5587076544761658] -- [0.6435562968254089, 0.38795289397239685, 0.35149022936820984, -0.5583747625350952] -- [0.6439348459243774, 0.38796523213386536, 0.35188087821006775, -0.5576832890510559] -- [0.6442166566848755, 0.38775885105133057, 0.3523886799812317, -0.5571805834770203] -- [0.6440829038619995, 0.38593411445617676, 0.35425904393196106, -0.5574158430099487] -- [0.6445986032485962, 0.38659462332725525, 0.3542124629020691, -0.556390643119812] -- [0.6452276110649109, 0.3872486352920532, 0.35404589772224426, -0.5553116798400879] -- [0.645923912525177, 0.38787057995796204, 0.3538874089717865, -0.5541682243347168] -- [0.6464512944221497, 0.38904473185539246, 0.3528819680213928, -0.5533706545829773] -- [0.6467377543449402, 0.3895181715488434, 0.35253840684890747, -0.5529218316078186] -- [0.6472330689430237, 0.3890780210494995, 0.35298487544059753, -0.5523668527603149] -- [0.647427499294281, 0.3889579772949219, 0.3533734083175659, -0.5519750118255615] -- [0.6479386687278748, 0.3886738121509552, 0.3536362051963806, -0.5514068007469177] -- [0.6484876871109009, 0.38791319727897644, 0.35432785749435425, -0.5508526563644409] -- [0.6485955119132996, 0.38705992698669434, 0.354991614818573, -0.5508986115455627] -- [0.6485474705696106, 0.386155903339386, 0.3556647002696991, -0.5511553883552551] -- [0.6483445763587952, 0.3844408094882965, 0.35675424337387085, -0.5518884062767029] -- [0.6483075618743896, 0.3827112019062042, 0.35773414373397827, -0.5524994134902954] -- [0.6487933397293091, 0.38343575596809387, 0.3568163514137268, -0.5520200133323669] -- [0.6487606167793274, 0.3823312222957611, 0.35746005177497864, -0.5524081587791443] -object_position: -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3872363567352295, 0.24797320365905762, 0.6667783856391907] -- [0.3654678449446503, 0.27083571689194097, 0.7123902039477208] -- [0.35343330136166917, 0.2676947272589954, 0.6891869578489374] -- [0.3580753322222763, 0.2606598891118305, 0.6947623694765742] -- [0.34853646959868084, 0.2591647346609303, 0.6795713436110852] -- [0.3613238362236649, 0.2515495500579143, 0.6828991693288131] -- [0.365112918033181, 0.2660998585368929, 0.6922504722726259] -- [0.37301536111086303, 0.2564471204102369, 0.6783268376822932] -- [0.3576267822213244, 0.26571979825114556, 0.6797506829922955] -- [0.36905637280350945, 0.24727574658503548, 0.6744869506636341] -- [0.3655675189258597, 0.25229003982548603, 0.6839067545328893] -- [0.3731897212415215, 0.2403275520988762, 0.6855015165178178] -- [0.36543937621788974, 0.2576389423604626, 0.6815629188512311] -- [0.3666567693357265, 0.25354788948776785, 0.671554054086742] -- [0.3686706070774893, 0.2593485757435323, 0.6872945332353319] -- [0.3696371248473067, 0.2610534304667004, 0.6672681738987404] -- [0.36330272831355553, 0.26234989702640615, 0.6734190694898584] -- [0.37573006930058456, 0.26370981807805793, 0.6685411504449197] -- [0.36399227889620167, 0.2591931254698121, 0.6705076748671268] -- [0.36938243677471677, 0.25774851345123784, 0.6813860790982907] -- [0.3569010080359717, 0.2573133981979947, 0.6635840325238366] -- [0.3596790334481823, 0.25645374675032073, 0.6696666968556774] -- [0.37925621579704694, 0.2509744501819136, 0.6698534928458255] -- [0.3547559636982936, 0.2597455922763272, 0.6603991954788793] -- [0.37189936767991805, 0.2530740579757589, 0.6840515389394642] -- [0.36715930061735563, 0.2534732452282263, 0.6791641535053454] -- [0.3622086381576212, 0.2509939481911423, 0.6689535162932935] -- [0.3784036487517679, 0.23623323588195055, 0.6699559316084034] -- [0.3636805741919496, 0.25462305593887247, 0.6897949824885561] -- [0.3574738367765595, 0.25117068852613345, 0.691406102758197] -- [0.3669442861198287, 0.2657440160736859, 0.6990559593872422] -- [0.3586515678914662, 0.255114387945912, 0.7072836850402863] -- [0.3508286277664592, 0.25636849750158397, 0.6947649949820608] -- [0.34415380732940065, 0.2652764009365778, 0.7098634258594707] -- [0.3514649221145531, 0.25224872279151433, 0.7041184507848504] -- [0.34375779125523065, 0.26131142754936404, 0.6812032932781579] -- [0.33388578143810216, 0.2598418848795022, 0.706632575409802] -- [0.33070741995718633, 0.26623656050823485, 0.7298749161165817] -- [0.3202197012696429, 0.2660999282846986, 0.7296387124428193] -- [0.3022560357577599, 0.26476540510525337, 0.7086241166076156] -- [0.3031634832088993, 0.26991341197737095, 0.7200395581652771] -- [0.3024169341684016, 0.2681498954114002, 0.7343370091621236] -- [0.3038710768589056, 0.26030366948493183, 0.7455069502449251] -- [0.2689100241550805, 0.2699620592152266, 0.7257056124968101] -- [0.2770353370624902, 0.2567116178976531, 0.7578528544959162] -- [0.259373150164849, 0.27304781068030415, 0.7428843586426046] -- [0.25340338489032566, 0.25849990565822734, 0.73656559247729] -- [0.24434370030696337, 0.26262240639181067, 0.7511423849257641] -- [0.22803624363697556, 0.25132720213939364, 0.7410798173672574] -- [0.21204537765824663, 0.24912874835952334, 0.7397427656387849] -- [0.20995932647562243, 0.25019356773908086, 0.7528571600471337] -- [0.200761011912441, 0.2496579982515765, 0.7566874269674848] -- [0.18403092050127298, 0.24713662214740587, 0.7585144053847769] -- [0.17595722423280855, 0.23894277013556825, 0.7642739201498885] -- [0.1743560750551203, 0.23110247979926046, 0.7615022043774821] -- [0.15538066822226204, 0.22954527372173603, 0.7602694487252402] -- [0.10668589916156815, 0.24522063464294164, 0.7405679998700492] -- [0.10242134340679784, 0.235234350036359, 0.7507418429678709] -- [0.08844394644033285, 0.23807690153881866, 0.755094320523109] -- [0.07661945988240028, 0.23204003824656663, 0.7609622262485991] -- [0.06327128332137398, 0.2231092919345461, 0.768545987888046] -- [0.036299634763728016, 0.23389850724070974, 0.757693860353231] -- [0.012642178473512088, 0.23632323749753126, 0.7590364893873779] -- [0.01644711681694952, 0.25079996814238353, 0.7881402541200607] -- [-0.02127447891425078, 0.23397069656188918, 0.7690579615327696] -- [-0.03478110475429892, 0.22501064134502444, 0.7645421087948664] -- [-0.03222096557925447, 0.21568732701550491, 0.7722459719629968] -- [-0.04823178674952687, 0.23320096654031078, 0.7782206109596355] -- [-0.05882563828982537, 0.22293345150146038, 0.7907754188942333] -- [-0.0785595279594006, 0.20610242739191217, 0.7817042927554307] -- [-0.11998329315926456, 0.2066347911363635, 0.7632476347841851] -- [-0.12118981406652175, 0.20725333241677973, 0.7829427938840454] -- [-0.1306507483896991, 0.2061671227780749, 0.7833670243215672] -- [-0.13767118673995662, 0.19892433476690646, 0.7811924204307111] -- [-0.1513088096597185, 0.19378776702270578, 0.7766495880898804] -- [-0.1748258727310317, 0.19609486223070174, 0.7875735740987492] -- [-0.18831929264320585, 0.18734713299421332, 0.7786601925095801] -- [-0.19547648301197967, 0.1896874589696435, 0.7825365948779462] -- [-0.18830769910976622, 0.2006376958275675, 0.8098876942718318] -- [-0.22865332377376762, 0.17843638025852648, 0.7742882673631809] -- [-0.23861542674950426, 0.1802469933729485, 0.7755839061495948] -- [-0.2622714343686138, 0.1761849354639361, 0.7721663449878048] -- [-0.2664116860586259, 0.18632226646102548, 0.7727720761911641] -- [-0.25529307201491447, 0.17520286742909202, 0.7754437799371152] -- [-0.29455657555625603, 0.1902176518680139, 0.7623058309425695] -- [-0.28144277084663377, 0.1817865157254769, 0.7632128021816443] -- [-0.3191521181814584, 0.17924245207511338, 0.7532912298349408] -- [-0.3200219517551228, 0.1948081932375949, 0.7534559687016034] -- [-0.3270246083101625, 0.1880563718475679, 0.7467519586982446] -- [-0.3354521307350213, 0.2010200219602767, 0.7502101453804773] -- [-0.35692220272205655, 0.20797727330472407, 0.7490539709480789] -- [-0.3449204811193346, 0.21946132033179086, 0.7529766960825062] -- [-0.3700673358203852, 0.21135217014873114, 0.7306359177580843] -- [-0.37706132924939584, 0.21084494135408965, 0.7342465714983679] -- [-0.3876917817211071, 0.21588199217731405, 0.7213427954768146] -- [-0.3925378133903767, 0.20887555453836243, 0.7266610256544551] -- [-0.38321933066934355, 0.20976891366861578, 0.7171220874451497] -- [-0.3641821216603489, 0.20858208483011767, 0.7669943097217211] -- [-0.39431691170638516, 0.21880461037863858, 0.7178237874630407] -- [-0.4100845812791278, 0.21848411880795338, 0.716046091630827] -- [-0.40813152869806907, 0.2256149704766053, 0.7188171043274401] -- [-0.4275576716869856, 0.2248410765565425, 0.7036023435873199] -- [-0.4249023368108643, 0.22755149098544872, 0.6911250297681938] -- [-0.41118909774267104, 0.23322544480729124, 0.7086713879762646] -- [-0.43262607882234566, 0.23196999204660804, 0.6996088817024575] -- [-0.4262272422233491, 0.23381763048205695, 0.6978848207013544] -- [-0.42699845216053, 0.23420278107586592, 0.6976439149960909] -- [-0.40626273633055326, 0.2256713873200534, 0.7042913507609633] -- [-0.448976983225292, 0.2497500028066722, 0.6723406803818024] -- [-0.4375351601915942, 0.23302543028197972, 0.6843437619293636] -- [-0.43764093212512556, 0.23604089806604414, 0.6921961477116657] -- [-0.43021737063952376, 0.23788018692154045, 0.6796016612172963] -- [-0.4350185996476702, 0.2339868965123215, 0.6824667489453323] -- [-0.4188027855185873, 0.21643689145775424, 0.6820563424624957] -- [-0.42078724995572264, 0.2373689075524551, 0.6948126621977226] -- [-0.43939068603444437, 0.23331981573388338, 0.6752346242175861] -- [-0.44183280929652674, 0.23208928316327526, 0.6775795742278429] -- [-0.4368151469051918, 0.23611922013282943, 0.686784969065869] -- [-0.43444799095169273, 0.23674285732619346, 0.670614013760267] -- [-0.4500289476195402, 0.23687697595473753, 0.6690593052735959] -- [-0.44047167450695246, 0.24475459833934637, 0.6619513269894545] -- [-0.4283750093598685, 0.23695725400557827, 0.6761944346573537] -- [-0.4355417674774262, 0.23975700039316342, 0.6612304425561402] -- [-0.43864497556401066, 0.2441586285460394, 0.6733412332520416] -- [-0.428238189507912, 0.24274588730926708, 0.6727912506838885] -- [-0.4444419991949541, 0.2620239203893813, 0.6636920430328761] -- [-0.43306566985930844, 0.24228913959334508, 0.6667703217202264] -- [-0.4331044951071657, 0.23786889709073517, 0.6597827117354796] -- [-0.4464188979172558, 0.23333970775770652, 0.6589400219519173] -- [-0.4249703689812572, 0.23268368513314336, 0.6700226845824775] -- [-0.44292647383081307, 0.23723707966844704, 0.6639993540806983] -- [-0.426593811101786, 0.2306030312657051, 0.6777753311661375] -- [-0.4460285430873926, 0.2378310679032445, 0.6513702489176343] -- [-0.43887482551324747, 0.23694781525900063, 0.6719631494848735] -- [-0.436316700724553, 0.23844903361613162, 0.6474394208179762] -- [-0.4318341400737083, 0.22343701065492977, 0.6592280744825609] -- [-0.43747378260062575, 0.2390440762024321, 0.6692725011971123] -- [-0.43493870114625227, 0.2103906704759906, 0.6626643380689304] -- [-0.4376194121758833, 0.23618400722481583, 0.6728283581901285] -- [-0.4228050551831278, 0.21108382514981883, 0.6739694347547257] -- [-0.4289658421685276, 0.22962645381703745, 0.6699106767660706] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -- [-0.4543498754501343, 0.22332751750946045, 0.6869237422943115] -object_wxyz: -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7078781754070083, 0.7063298414467313, 0.0005256632221748724, 0.0025233989888942517] -- [0.7096534703658574, 0.7045504873719239, -0.0007453341053435865, -8.496323396065958e-05] -- [0.7089769661576292, 0.7052226075340082, 0.002829261863289615, 0.0021749846360950096] -- [0.7065745426915899, 0.707623736629659, 0.004586371861778378, 0.00016784395999345803] -- [0.710691156119896, 0.7034896279324402, 0.004494049392536361, 0.0004769952827735559] -- [0.7082673751931439, 0.7059283761503434, -0.0036491606065423917, 0.0030226825207542527] -- [0.7055738609017788, 0.7086356960698464, -1.0903453093818506e-05, 0.0009884072422404338] -- [0.7085940698318786, 0.7056140495115426, -0.0017449416480597175, -0.00046098770269842966] -- [0.7060320377758478, 0.7081761386414522, -0.002162594329628692, 0.000800923674187634] -- [0.7055903216239855, 0.7086029370090738, 0.0040531691090639884, 0.0027834354399494943] -- [0.7069420807158608, 0.7072682074204896, -0.0010139989314884238, -0.0018839032627905891] -- [0.7033403377606438, 0.7108501265363549, -0.0012547241078773143, 0.0017007496596155034] -- [0.706527464330576, 0.7076563998905269, -0.0062301388642870955, 0.0015959982195660313] -- [0.7067877341478787, 0.7074205236731246, -0.002398326492871343, 0.0012448189359537812] -- [0.7067848859215027, 0.7074260027891754, 0.0006919440378811757, 0.001759779561864234] -- [0.7067094563338882, 0.7075021404980689, 0.0014586299829544187, -0.0005813067038361327] -- [0.7052132126784841, 0.7089860579436136, 0.003564599575321929, -0.0006228443896901096] -- [0.709571209546562, 0.704628796979837, -0.0016015571145962572, 0.0020957252170087973] -- [0.7070969902059514, 0.7070970972854367, 0.0014021053635409357, 0.005057227772615045] -- [0.7045694015230883, 0.7096312089722451, 0.0012947049774798523, -0.001956892692047418] -- [0.7064115916434005, 0.7077920387898082, -0.0020923930605162484, 0.002952102451537807] -- [0.7091178297891, 0.7050898103937011, -0.00048438006684565723, 0.00016772037046392004] -- [0.7079016472246508, 0.7062935030457264, -0.00324333642153471, 0.0037717609785692502] -- [0.7048154376623911, 0.7093857814164096, -0.0025786493310469257, -0.0006021001822747965] -- [0.707877817956212, 0.7063233643979483, -0.002820914772415927, -0.0028882851247625436] -- [0.7068095923125308, 0.7074027184760067, -0.0004832221384717243, -0.0011664493621831524] -- [0.7072294800697158, 0.7069832633304732, -0.0008857179762611753, -0.000585998789311155] -- [0.7061005686503387, 0.7081086468090357, 0.0019796873432071705, -0.0004605474655052339] -- [0.7066088303687388, 0.7075895384532807, 0.0011092738036790075, 0.004446957164690589] -- [0.7047336720104943, 0.7094719135366223, -0.00022208577196788804, 7.819973357275422e-05] -- [0.711422500574131, 0.7027520798846655, -0.0033378401209544347, 0.0025295687289008124] -- [0.7089759980652884, 0.705230578560603, -0.0014698770579545432, 0.0008394592544134925] -- [0.7083444636703944, 0.7058623884672813, 0.002150885498347612, -0.0013353000932240784] -- [0.7056000726091965, 0.7085986894542681, -0.0033407967229203833, -0.0022965005653047843] -- [0.7108121463339951, 0.703379598601931, 0.0004960514391686799, -0.0017282440844639199] -- [0.7069362599741067, 0.7072496554596939, 0.004476486222297707, -0.00436007535120928] -- [0.7093288463102598, 0.7048652924723025, 0.0038827573766375962, -0.0015593124271643464] -- [0.7065917260214504, 0.7076067341851502, -0.004154629142726029, 0.0018924879234360613] -- [0.7061590127535996, 0.7080382014142148, 0.0005093151615089344, -0.004592890489851818] -- [0.7052041852378885, 0.7089969727850437, 0.0008356626073248635, -0.0031066658360060406] -- [0.7105370929657358, 0.7036423057171203, -0.0030664303436345954, -0.0038912889694310594] -- [0.703789415523934, 0.710404905057886, 0.0003274366830659713, -0.002285224582776638] -- [0.7062357778039581, 0.7079699125898592, 0.002956491475870079, -0.00094243052722014] -- [0.7070402040592173, 0.7071732870054823, 0.00010117699284686246, 0.00028592477768178335] -- [0.7028226828657683, 0.7113475294544312, -0.004745797612266157, 0.001564031158275122] -- [0.7081530040523331, 0.7060514396255889, -0.001708055951163866, 0.002787471821835302] -- [0.7091598295528441, 0.7050461198867356, -0.0014881178307291697, -0.0003008098437059801] -- [0.7059844365500993, 0.7082272540543811, 0.000363267551176032, 5.620000505534747e-07] -- [0.7097763889227704, 0.7044198849816469, 0.002739026474825937, 0.0016127939005445657] -- [0.7077257636298117, 0.7064678664860871, -0.004428103753979303, -0.002790880567158402] -- [0.7066259770097202, 0.7075841330380537, -0.0017485417247680702, -0.0011687128308319281] -- [0.7053386844194263, 0.7088661495034443, -0.0018959360191705826, 0.0015898981868894234] -- [0.7058905177422703, 0.7083100344076024, -0.003863354326466691, 0.0007393324006979583] -- [0.7073418095842207, 0.7068714355548645, -0.00027926078115859173, 0.0005099256232087339] -- [0.7060872068998136, 0.7081218207832329, -0.0010658762612044217, 0.001790835239287523] -- [0.7091749981250707, 0.7050319231743399, -0.0002362073751717942, -0.0008680699705398001] -- [0.706883735562542, 0.7073204607661815, -0.0025900036073677447, -0.002538121347908791] -- [0.7116099452230099, 0.702537885064474, 0.004754293768890069, -0.005403942967862458] -- [0.7024536635194909, 0.7117161791224549, -0.004244322710686302, 0.0009574487490384224] -- [0.7070031130047884, 0.7071880732166029, -6.704570088268698e-05, 0.005623415919274848] -- [0.7082846652397833, 0.7059137494203952, -0.002764729533331554, -0.003311742142010889] -- [0.7071234261840476, 0.7070769095408621, 0.002560430921398188, 0.003485445362386216] -- [0.7062398033729321, 0.7079674505345758, 0.0025435743110631096, 0.0009794616170392617] -- [0.7117963537571297, 0.702376571687145, 0.003520767688463672, 0.0008405462223753271] -- [0.7056217197339316, 0.7085779554425357, 0.0031203234296144134, -0.0023522929876078325] -- [0.7095318017857614, 0.7046695384737068, 0.001783512358405895, -0.0015109220930598635] -- [0.7068251634964783, 0.7073825761934662, 0.0020023923423668944, 0.0020173177684519573] -- [0.7082670496423162, 0.7059402915925937, -0.00037068457978489563, -0.0024400184578755] -- [0.7069331603940732, 0.7072734976898226, -0.002743810641911471, 0.0014757045819037418] -- [0.7048630924444981, 0.7093333716076277, 0.003396749183812091, 0.00162816716475711] -- [0.711621133914919, 0.7025512818677119, -0.0024120128105698606, -0.003352656540923419] -- [0.7080807991502208, 0.706127896788494, 0.0011932901787589769, 0.0018844920779500084] -- [0.7060199183202774, 0.7081688137429927, 0.005484688540127801, 0.001650566111673311] -- [0.7095235584998059, 0.7046582491096992, -0.005701525533012069, -0.0007513334960694543] -- [0.7041059664154529, 0.7100935407197463, -0.001074695542515423, 0.0008924773084155721] -- [0.7049981922948214, 0.7092088852143806, -0.00039055738393668895, -0.00039173835592962674] -- [0.7084195331847454, 0.7057812980364728, -0.00230261347943141, -0.003036826538253605] -- [0.7091999184796288, 0.7050062816504145, -0.001172220885554474, -0.0004943279678280031] -- [0.7066901340609599, 0.7075216319223557, -0.0014802143698691439, 6.122294000821235e-05] -- [0.7068219645717508, 0.7073897712396169, 0.0013665794067860492, 0.0007445839228116789] -- [0.7076751871739634, 0.7065296872747949, 0.0012116480907223119, -0.003187846699703496] -- [0.7065916629323138, 0.7075930417642883, -0.004277053831474694, 0.004692113791503374] -- [0.7044056962632044, 0.7097973382350786, 0.0002935439282084853, 0.0005172410556602997] -- [0.7090614110447687, 0.7051441896263417, -0.0008657077397639701, 0.0016845633027366348] -- [0.7030282777890581, 0.7111480738845647, -0.0028267714677573377, 0.003415699385705937] -- [0.7061697170432334, 0.7080238783460573, 0.0051475570080764844, 0.00014518862098509126] -- [0.7086918478911568, 0.7055176632870725, 0.0007285512611762662, -0.00040091847818798227] -- [0.7084438017775468, 0.7057664893179642, -6.121905631941957e-06, -0.0010209021961127538] -- [0.7054795651681202, 0.7087302024974137, 0.00017093398311598468, 0.00023233542257735968] -- [0.7116438823138028, 0.7025399460486367, -0.0007781980365983952, 5.813031117342254e-05] -- [0.7054945268762791, 0.7087110116416173, 0.0020611086015349606, 0.0013879325859359496] -- [0.7072661488936405, 0.7069446652926064, -2.7480876112873348e-05, -0.0019580828084444705] -- [0.7047086950752476, 0.7094946722100725, 0.0012781172161500374, 0.0011539528783306937] -- [0.7078198282319424, 0.7063878698123424, 0.002688473471315256, 0.00020063419144381774] -- [0.7043453663135159, 0.7098495807310653, -0.002090746912495437, 0.002608920458271913] -- [0.7047322368472637, 0.7094531336046136, 0.005112513693676522, 0.0016086548972339305] -- [0.7099437703257733, 0.704251604221871, 0.002085133119328175, -0.002274455192243142] -- [0.7076343503148397, 0.7065783628615191, -0.0006460837403530858, 0.00047535905446260263] -- [0.7055934674965211, 0.7086165300142944, 0.0006638792666446608, 0.0001768645123145223] -- [0.7049597758588919, 0.7092444837334542, -0.0014094202999041401, -0.0014107618740333693] -- [0.708196071037066, 0.7060116263194802, -0.0003424450317170448, -0.0024064913849383106] -- [0.7077734641499209, 0.7064380105391713, 0.00081061308358428, 0.0011847434772934422] -- [0.70493447331349, 0.7092390947750188, -0.0035759865340886034, -0.0058742741894046505] -- [0.7055559365232924, 0.7086518529542959, -0.0008389202368854549, -0.0016333872837894256] -- [0.7067704816224667, 0.7074429197473119, -2.7225001680340282e-05, -2.9416733481244855e-05] -- [0.70476677210306, 0.7094089522141207, 0.006372630424614967, -0.0014577514768282264] -- [0.7101251397861923, 0.7040661214934288, -0.003458051170136424, 0.0011064768165705773] -- [0.7053810223245226, 0.7088078973694347, 0.005242264734964935, 0.001223368947078926] -- [0.7035417972095885, 0.7106534346321011, -0.0004931042511077011, 0.0006263169305942703] -- [0.707406064769176, 0.7068071753579057, 0.00034151411722974876, -0.0003996979794547269] -- [0.7058622394897474, 0.7083271600772595, 0.005396033968343609, 0.0014198509835320501] -- [0.7055449707881867, 0.7086485361605397, -0.004331987237668292, 0.002186385051651135] -- [0.7049913845058743, 0.70920056843141, 0.0035379828299221963, 0.003030542286538569] -- [0.7085121696269713, 0.7056936732566521, 0.00017214621874239686, 0.002629711332947631] -- [0.707194283969681, 0.707000522862098, -0.0026304562422330194, -0.004425617842517239] -- [0.7083815050855115, 0.705813535824683, -0.004782588613571037, 0.0001508164377819832] -- [0.7084122494660116, 0.705796655183753, -0.001768269001037382, 0.00019890367434290842] -- [0.7053963876506107, 0.7088116551746788, -0.0008515203960795788, -0.0011174484402846684] -- [0.705246390480236, 0.7089327151096259, 0.003391239469494745, -0.005516670803384226] -- [0.7087402313212804, 0.7054557077721554, -0.002987852923125549, -0.0032560118634466567] -- [0.7077287838752048, 0.7064657349991076, 0.0031553098101737877, -0.004022159456822161] -- [0.7078110731105958, 0.706384715193345, -0.0011677649099353603, -0.004770246152872954] -- [0.7064587133593324, 0.7077500360403708, -0.00040171297800384136, -0.00241069081671304] -- [0.7049269784154112, 0.7092580384915773, -0.0019497938754714816, -0.00521423446364085] -- [0.7076617584344183, 0.7065400080153966, -0.003566655059760775, 0.0018252930709836447] -- [0.7064801343451172, 0.7077199831886086, 0.0040354643866884555, 0.001400070856957062] -- [0.7056403021729525, 0.7085672892692428, -0.000203902037631835, 0.002029519859659797] -- [0.7094382244493119, 0.7047663506224655, 0.0010993211825692858, 0.0007669507400172105] -- [0.707677239277183, 0.7065342544446848, 0.0014992587000860467, 0.00015661630939057159] -- [0.7067246682770193, 0.7074866189951391, 0.00166333498725062, 0.0004006348513991882] -- [0.7087823068567854, 0.7054183281252477, 0.0021446529480285154, -0.0028327187446699374] -- [0.7053222470021231, 0.7088822231195971, 0.0025406851801771935, 0.00025796842639837296] -- [0.7056161956396247, 0.7085928731537922, 0.0013406446964379488, 0.00035670508047861815] -- [0.7096189804388667, 0.7045840979026423, 0.0014292080004231233, -0.0003300728927815452] -- [0.7069673234544375, 0.707231102991596, -0.004496832114013978, 0.0010719281216622263] -- [0.7096475846193087, 0.7045550483291362, -0.0013765752651933416, -0.0007710761629033172] -- [0.7055822279493846, 0.7086261266301739, 0.0015454996901289298, 0.0005862506349074593] -- [0.7094336094130924, 0.7047713017280188, 0.0008436115408858236, -0.000808959376992668] -- [0.7058188424339916, 0.7083857445426666, 0.002524698966985998, -0.0017391058041082632] -- [0.7099717370106347, 0.7042281565226741, 0.0016757317952981337, -0.00016771919614050814] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -- [0.7071068286895752, 0.7071068286895752, 0.0, 0.0] -qpos: -- [0.15080165907860044, 1.356358388068401, 0.7140210986920054, 0.7106552914400213, - 0.019246653038435087, -0.00686665397478744, -0.7032435368763137, -0.5292872651493421, - -0.16778811604311847, -0.0010000000000000009, 0.06456522792824608, 0.0036959775501775893, - -0.0010000000000000052, 0.15213075015919172, 0.13509511051294268, 0.0010000000000000178, - 0.7139549255501549, 0.2876633827663385, 0.19000278858768518, 0.10839912412962392, - -0.07281252105895568, -0.32493294469722567, 0.2637519678808252, -0.4133843124264621, - 0.06681685426535636, -0.017884478093107455, -0.24479915090409407, 0.476826025874201, - 0.9776955714951622, 0.9802920115089997, -0.09255921092912851, -0.09359915575180516, - -0.01785647930185663, -0.06256550993584885, -0.022546720385611806, 0.047862247152728285, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15149076497385253, 1.3524989270473176, 0.7139818675459011, 0.7096502907234898, - 0.01968090714177234, -0.007516595523141281, -0.7042390414922223, -0.5409266097940828, - -0.16130307833947521, -0.0009999999999999985, 0.06453676132332174, 0.002481169912296335, - -0.001000000000000006, 0.15612960945082005, 0.1387227662031046, 0.0010000000000000148, - 0.7035495453121817, 0.2835632453757514, 0.19082675515403852, 0.10531570084060829, - -0.07718379390583605, -0.3265128441044733, 0.26295460271277526, -0.41432147681772835, - 0.0701269161697288, -0.01660713920436475, -0.2408152734388667, 0.4795604472152241, - 0.9776671265141803, 0.9790643079604497, -0.09243817650191422, -0.0958468147995689, - -0.01809472461923666, -0.06211485908006342, -0.021537165082620206, 0.048939898275102735, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1521675422474615, 1.348622717657764, 0.713968787317118, 0.7086442553022769, 0.020123877657392826, - -0.008170676170391336, -0.705231585385985, -0.5525622582329898, -0.15479562899587582, - -0.0010000000000000007, 0.06446213851912695, 0.0012425113896997067, -0.0010000000000000072, - 0.16010393466064893, 0.14234184386344342, 0.0009999999999999857, 0.6931501672139029, - 0.2794466708634165, 0.1916468095302649, 0.10222783708799153, -0.08154247475837974, - -0.3280857987337679, 0.26214175585013694, -0.4152592465320656, 0.07340750361145063, - -0.015349792580583336, -0.23683563220853093, 0.48229860324730583, 0.9776385917248595, - 0.9778359703912582, -0.0923170142536462, -0.09809433597323804, -0.01833237034528908, - -0.06166732604659737, -0.020527494414428304, 0.050019022626069845, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15270855751921925, 1.3439945370060773, 0.7136904768712606, 0.7077199959734768, - 0.02045381084371221, -0.008460817790508734, -0.7061462054586053, -0.5590855015047211, - -0.1481919491428727, -0.0009999999999999983, 0.0640385268864909, 0.000607693739744178, - -0.0010000000000000276, 0.16237057529216212, 0.14502504130486077, 0.0009999999999999842, - 0.6712951789496974, 0.2760050084376093, 0.19213501312446934, 0.09899604809879896, - -0.08404342102828585, -0.33048458439270306, 0.2619300072010419, -0.4148238132504595, - 0.0741268332083632, -0.013155318093780854, -0.23357992647863315, 0.48471905990846054, - 0.9784501393380917, 0.9775829961266445, -0.09246454672825748, -0.09863168721457714, - -0.018889791195208498, -0.06104026312141222, -0.019023978457036246, 0.05156500022533796, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15323253195362396, 1.3393387836185542, 0.7134474864253709, 0.7067959093083133, - 0.02079090178959341, -0.008753509015748862, -0.7070577466287468, -0.5656038189226291, - -0.1415568885814855, -0.001000000000000001, 0.06356338376401646, -4.573985562698511e-05, - -0.0009999999999999903, 0.16459617885174513, 0.14769969920973933, 0.001000000000000004, - 0.649422922544767, 0.27254452411683233, 0.1926196058257317, 0.09576740033935927, - -0.08653127656703152, -0.3328772263117856, 0.26170441296575886, -0.41438252004309284, - 0.07477972252146885, -0.01097399812368286, -0.23033424292508325, 0.48713422414563257, - 0.9792664850606302, 0.9773355366527993, -0.09261300232619524, -0.09916045979227929, - -0.01944904304726191, -0.060413577666168776, -0.017517877403781572, 0.05311373748879857, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15366627286128318, 1.3337840254229256, 0.7128857220321825, 0.7058682470576122, - 0.02105273276495403, -0.008633434943907756, -0.707977587244123, -0.5678668710740241, - -0.13511640055156138, -0.001000000000000001, 0.06306056045301127, -0.00020213642534555354, - -0.000999999999999984, 0.16504581598735357, 0.1495682411387308, 0.0009999999999999625, - 0.6160115023928933, 0.27023888921170824, 0.19317162029771182, 0.0920039413707048, - -0.08487683330532027, -0.33636020013357, 0.2614818073988496, -0.4130271782318985, - 0.07294283325616861, -0.009596754202182792, -0.22703930718502152, 0.49035761963911995, - 0.9807615077570873, 0.9780383258882704, -0.09286938802481944, -0.09851565273559995, - -0.020518201754760445, -0.06072488220868152, -0.015580641140298215, 0.054547560157952986, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1540773235318503, 1.3281859364480886, 0.7123763370279806, 0.704940338507683, - 0.021322489353892977, -0.008514766081796455, -0.7088948930208182, -0.5701355536775038, - -0.12863491878556457, -0.0009999999999999972, 0.06252651863996486, -0.00037608416985315585, - -0.0010000000000000152, 0.16545026153702386, 0.15143071757325305, 0.0009999999999999848, - 0.5825737269011018, 0.2679147990723946, 0.19372189879358254, 0.08824556271400484, - -0.08319464020676433, -0.33983917678417375, 0.2612361809441317, -0.4116634075910242, - 0.0710266600473893, -0.008246063407530499, -0.2237500104776571, 0.4935842842915866, - 0.9822638643364635, 0.9787519230241789, -0.09312554517593565, -0.0978572141928058, - -0.02159219680299118, -0.061046517535895094, -0.01363913791759111, 0.055980099768331855, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.154414928452906, 1.3216686480603133, 0.7116619940491864, 0.7040429222819132, - 0.02149507832481892, -0.007960959632395818, -0.7097873965591998, -0.5688201515625831, - -0.12220592292906367, -0.0009999999999999985, 0.061911354234833404, -9.679206536919402e-05, - -0.0010000000000000202, 0.16320029255955482, 0.15244720698366904, 0.0010000000000000174, - 0.5386150399086499, 0.2664445692251187, 0.1938962269769229, 0.08389072330980299, - -0.07613222743319663, -0.3446052941608591, 0.2612837821197044, -0.40919950099483215, - 0.06558173156590903, -0.006812474285668991, -0.22108938699920108, 0.497046062940627, - 0.9849503186107674, 0.9800335709687981, -0.09418670116463249, -0.09753342077368798, - -0.02288029497856692, -0.061983742589102324, -0.011189336032346996, 0.05687748779171944, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15475106313519407, 1.3150962401985595, 0.7110321017245477, 0.703014199793967, - 0.021625543165048063, -0.007363236457741095, -0.7108088023651186, -0.567478251079871, - -0.11572660416805049, 0.00023398609124790958, 0.061674803694197644, 0.00039647142537220905, - -0.0010000000000000195, 0.16110271331075815, 0.1537814178304772, 0.0009999999999999885, - 0.49465971805694586, 0.26493317082645457, 0.19385700075424486, 0.07990214074409925, - -0.06906835983444336, -0.34932395820495554, 0.2615985252344997, -0.40652815649151025, - 0.06008207406046516, -0.005388384918602053, -0.21923695752069558, 0.4996851977826956, - 0.9876571317313658, 0.9813244259762637, -0.09526018415849756, -0.09721480240262839, - -0.024173080937458073, -0.06293254471315365, -0.008733002498274963, 0.057765619183993554, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15502983178604143, 1.3077363828108652, 0.7101833653505384, 0.7021572576458922, - 0.021592889720568467, -0.006362420819433503, -0.7116659695742314, -0.5636158587900472, - -0.1093814444485188, 0.0010000000000000007, 0.061631620729089634, 0.0017023621724126478, - -0.0009999999999999861, 0.15620896541480092, 0.15454542704133983, 0.0009999999999999692, - 0.4438457331492974, 0.2648211620831281, 0.1936811680847679, 0.07531366229800843, - -0.058848082107851085, -0.35566150786499706, 0.26198311064107627, -0.40350975817895574, - 0.05221703502353484, -0.00513104172481821, -0.21767562187555353, 0.5032051870813558, - 0.9909324552885526, 0.982402587030798, -0.09613980515636482, -0.09643129682476152, - -0.025764136922880966, -0.06415175347989661, -0.006319414644845256, 0.058106643858676706, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1552587790784352, 1.3003169781517698, 0.7094391830053869, 0.7013888691751667, - 0.021606802928062933, -0.005395375607467186, -0.7124308318612156, -0.5598248267906191, - -0.10298033787995407, 0.001, 0.061469071058795316, 0.0028400677398258338, -0.0010000000000000015, - 0.1511754511769854, 0.15511301855247864, 0.0009999999999999796, 0.39308655826505984, - 0.264698020447281, 0.19362379666510401, 0.07051480885752344, -0.04862436176660678, - -0.3620115775612249, 0.26214815358056215, -0.4006325637601664, 0.04439611086849846, - -0.004918696776853456, -0.215659955110155, 0.5072157956081937, 0.9942195681618188, - 0.983476031453136, -0.0970139116757293, -0.0956367361066679, -0.027362142493340347, - -0.06537651360812195, -0.00391040140089821, 0.058435433786813575, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1555679822647201, 1.2924147050728192, 0.7087145618415603, 0.7008445251566585, - 0.02131813737167564, -0.00409522245291645, -0.7129836728355022, -0.5543020358245442, - -0.0965034805040883, 0.000999999999999999, 0.0619294551509966, 0.0047073216684151474, - -0.0010000000000000022, 0.14445143665041144, 0.15507180898473225, 0.0010000000000000009, - 0.3399170654929998, 0.2657209309478108, 0.1934613868651315, 0.06577266631868002, - -0.038341451111984406, -0.3693037336233791, 0.26283109960180906, -0.39717543905979635, - 0.03613218098587823, -0.004941655603754984, -0.21400771179629438, 0.5107136417860716, - 0.9975369095005422, 0.984120781648834, -0.09691183344897934, -0.09574432123811179, - -0.028519531791111677, -0.06646754068184474, -0.0021311105106349435, 0.058723019827462676, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15585556103353912, 1.2844805916035162, 0.7081203301046426, 0.7003133899405743, - 0.021044847529546485, -0.00280561906087493, -0.7135197255606076, -0.5488274769349917, - -0.0899860141532867, 0.0010000000000000013, 0.06256174965640643, 0.006537474236168946, - -0.0010000000000000033, 0.13773649015192327, 0.15502773301768827, 0.000999999999999942, - 0.2869107845127335, 0.2667208207268481, 0.19328784550755568, 0.061054944825093195, - -0.02817354327548682, -0.37654442797984855, 0.26347319986532797, -0.39374386952322826, - 0.02805386207069597, -0.00497752619113301, -0.2123977968618516, 0.5141559347684658, - 1.0008549590215774, 0.9847531111825489, -0.09678155357094402, -0.09587796558547193, - -0.029662463719603457, -0.06755506262177655, -0.00037123128577504704, 0.05900833257501851, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15633836806495957, 1.2764463426980004, 0.7075907421018774, 0.6999433576801317, - 0.02046893746751417, -0.0013126023296000794, -0.7139037720264119, -0.543188821319387, - -0.08369904413330498, 0.0009999999999999983, 0.06412414366074548, 0.008915354631947625, - -0.00100000000000002, 0.13208362353008088, 0.1547573929532389, 0.0009999999999999979, - 0.2377183855848972, 0.26928858903433045, 0.19298552551721412, 0.056137760854527145, - -0.021207201066807242, -0.3841739276327414, 0.2644600222831448, -0.39024573147364583, - 0.02182178986928738, -0.004757440131452642, -0.2113509399048322, 0.5174724087545035, - 1.004240894341274, 0.9855492479185066, -0.09600074010666702, -0.09560571892909069, - -0.030052567288459234, -0.06840330339025995, 0.000418866418595848, 0.05940305500506063, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15681994433468435, 1.2684166213412906, 0.7071803526764089, 0.6995867374465795, - 0.019907236901337572, 0.00016856703517593725, -0.7142703061817421, -0.5376280954350355, - -0.07740846246414032, 0.001000000000000001, 0.065840142073028, 0.01125036960703146, - -0.0009999999999999992, 0.126535499636664, 0.15449139284282684, 0.00099999999999999, - 0.18885317624921583, 0.2718599451434226, 0.19266857487232042, 0.05123443546972973, - -0.014447600611650709, -0.39172468805017074, 0.26540246505584725, -0.3867830962157186, - 0.015844505336909002, -0.0045228507263292035, -0.21036059362297338, 0.520738660771655, - 1.0076298511538382, 0.9863513040446827, -0.09519841012621477, -0.09531904300560679, - -0.030415586939156566, -0.06924307476082855, 0.0011759847608440528, 0.05980148642621865, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1573688179844473, 1.2605350105111797, 0.7066584856442636, 0.6994894002980001, - 0.019136256733283456, 0.0017498327313308845, -0.7143845747455597, -0.5332069942691082, - -0.07091852108181887, 0.0009999999999999996, 0.06817409747523677, 0.013847849835834588, - -0.0010000000000000026, 0.12358070087991917, 0.15366989718142607, 0.000999999999999991, - 0.14859551317245265, 0.2749094069824089, 0.1923270393675263, 0.04705663170580936, - -0.01187827811801396, -0.399025955802725, 0.2666965207838637, -0.3834272512962575, - 0.012320907829157825, -0.0041252181006114505, -0.2101549555741668, 0.5234211149655194, - 1.0106072857194415, 0.9865140665459701, -0.09374664953635078, -0.09418661885393333, - -0.030204911849152492, -0.06993603508543018, 0.0012590177749433045, 0.06007991888427893, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15792702267195377, 1.2526811462114567, 0.7062131616588433, 0.6994087470418116, - 0.01837562115996844, 0.0033228031595924985, -0.7144779213436548, -0.5288881902780904, - -0.06442637814099356, 0.001000000000000002, 0.0705894525167618, 0.016410279473698847, - -0.0009999999999999983, 0.12077127310581162, 0.1528377146558997, 0.0009999999999999935, - 0.10882094317790075, 0.27793740068085954, 0.19197705009482952, 0.04292571094326237, - -0.00953732608666893, -0.4062349643616276, 0.26796313857037296, -0.38009969754276623, - 0.009011196143647584, -0.0037080488511936328, -0.21001116503914607, 0.5260439284854014, - 1.0135684206670612, 0.9866505880936534, -0.09226803369272783, -0.09301899875512852, - -0.02997087005204556, -0.07062233980978996, 0.0013169820911932105, 0.060353695707930304, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.15861636718947247, 1.2451111559204022, 0.7055806294415965, 0.6993439985645598, - 0.01745713081271197, 0.004869452304169297, -0.7145554622909103, -0.5260347015822903, - -0.05753023312357678, 0.0010000000000000044, 0.07329402792928841, 0.01888236053700445, - -0.001000000000000006, 0.12037498748808628, 0.15181450110516945, 0.0009999999999999551, - 0.08071539380572929, 0.2810453687095907, 0.19210684170634593, 0.03906994267175696, - -0.010333224554195745, -0.41292199419718384, 0.2691637064238201, -0.37679129806216066, - 0.0076806287349971015, -0.0032585108872970367, -0.21031454010351147, 0.5281559735822605, - 1.0157004512846899, 0.9871027238868252, -0.09141050172501616, -0.09094775209750382, - -0.029117001205967805, -0.07161465324305807, 0.0007527580367372255, 0.060918371357061726, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1593219859899562, 1.2375722593325769, 0.704978315254919, 0.6992831709404789, - 0.016540829625876687, 0.006411175321636967, -0.7146246179818547, -0.5232777621544721, - -0.05062417582164149, 0.0010000000000000009, 0.07602045457359333, 0.021331017701851997, - -0.000999999999999985, 0.1200970840997874, 0.1507957069232987, 0.000999999999999975, - 0.05322574720965022, 0.2841301958950572, 0.192256097735058, 0.03524408551800595, - -0.011299648087097425, -0.41953191777961607, 0.27034420426915534, -0.37348349804172926, - 0.006478656508348412, -0.002794455463536152, -0.21065304516625377, 0.5302213902284184, - 1.0177935156261226, 0.9875696239487418, -0.09058129346614065, -0.0888329005054578, - -0.02823412194873243, -0.07262008815186259, 0.00016082050241178074, 0.06149862938070342, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16034365404110956, 1.2304182971316777, 0.7043469789526884, 0.6992763866512971, - 0.015626718730360714, 0.0077674452539114305, -0.7146383753534757, -0.5222321803031541, - -0.04327871360708149, 0.001000000000000001, 0.07794822030401947, 0.02290914057768847, - -0.0009999999999999994, 0.1208227703378241, 0.15001313627012644, 0.0010000000000000193, - 0.03908169231528961, 0.28713673530759265, 0.19216709938380733, 0.03144278460425787, - -0.012286355083025703, -0.4248410688575499, 0.27050908682333635, -0.3706531518728696, - 0.007275680924491433, -0.0023300371895821352, -0.21132071590694898, 0.5322035382275808, - 1.020148815856266, 0.9886255301627234, -0.0896289996433096, -0.0860991626613818, - -0.027541280479624366, -0.07313385587358805, -0.00024029333953924905, 0.062152890832128695, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16138801429107924, 1.2232905023344287, 0.7037244015533879, 0.6992729539808598, - 0.01471556888328466, 0.009115327141317024, -0.7146451557762293, -0.5212835052530015, - -0.0359128661657406, 0.0010000000000000035, 0.07983053808042041, 0.024442808395494356, - -0.0009999999999999998, 0.12159725818499528, 0.1492516414224265, 0.0010000000000000258, - 0.025671208595982535, 0.29012734824777947, 0.19206411321202277, 0.027655487205865154, - -0.013281079459638625, -0.4300602590094246, 0.2706176510415347, -0.367834602292066, - 0.00818258003246739, -0.0018601180703397458, -0.21201096743088538, 0.5341696190106913, - 1.0225183364246406, 0.9897126285063532, -0.08866932274859493, -0.08333185714710771, - -0.02685903311651087, -0.0736199944119674, -0.0006289976404466264, 0.06281294804818241, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1626086325373535, 1.216480889683943, 0.7030586575462403, 0.6992290329355281, - 0.013857178738682169, 0.010258199925155111, -0.7146897980464993, -0.5210873729177529, - -0.027810687060364124, 0.0009999999999999998, 0.08109730170187215, 0.025097797078239222, - -0.0009999999999999972, 0.12195825250172457, 0.14880036392483578, 0.0010000000000000104, - 0.023710079172323616, 0.2925168257322192, 0.19158957723615214, 0.02372432258712708, - -0.01136721277673609, -0.43287617429256403, 0.270044536970408, -0.3654519109966255, - 0.009745538278621105, -0.0008810221301109099, -0.2134169900914626, 0.536366420166955, - 1.0249161554718145, 0.9914478680453546, -0.08780539975622018, -0.08112597485619481, - -0.025931216874199553, -0.07392308795984606, -0.0008216846262114629, 0.06387875985086326, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16384236321728, 1.209689159823566, 0.702395785639074, 0.6991827726518373, 0.013001558269200119, - 0.011391281914134738, -0.7147339705133755, -0.5209330939018684, -0.019662767660119266, - 0.0009999999999999979, 0.0823291186675232, 0.025709349911520872, -0.0009999999999999966, - 0.12229155303232976, 0.14837166381122174, 0.0010000000000000065, 0.022433276648485063, - 0.2948661157094976, 0.191090325580373, 0.019795611998277198, -0.009280600621637953, - -0.4355440096930617, 0.26943650012443754, -0.3630783344465614, 0.011348840170690126, - 0.00012992174506407174, -0.21486657851224825, 0.5385712113841458, 1.0273158186695959, - 0.9932215741062111, -0.08694673110628587, -0.07895290009203543, -0.0249890102074067, - -0.07421433203835676, -0.0010007779846104609, 0.06497038926795366, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16509487073856716, 1.2030633597783609, 0.7015713075138935, 0.6991715369632768, - 0.01225680777925449, 0.012353274237690762, -0.7147421417399984, -0.5208265018201215, - -0.011002329542442048, 0.0010000000000000009, 0.0829074661703114, 0.025795791252549605, - -0.0010000000000000096, 0.12095724584235099, 0.14834746861447365, 0.0009999999999999885, - 0.029400331444877425, 0.29730690299016516, 0.1901898431963694, 0.016120267373506178, - -0.0022883462497125264, -0.43554973299765354, 0.2683936830227724, -0.36112529711751545, - 0.013209500246315897, 0.0011239610687610417, -0.21660937145732861, 0.5406892613425617, - 1.0291599438014902, 0.9950958000916501, -0.08560995299200069, -0.07604609005497613, - -0.02419338511948291, -0.07419865992003781, -0.0008234188468850508, 0.06648207106829258, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16635132080034973, 1.1964498925765708, 0.7007526608688888, 0.6991635663915364, - 0.011519383513078385, 0.01330449709308057, -0.7147451305124416, -0.5207185880326626, - -0.002307315817927187, 0.001000000000000001, 0.08343852757497948, 0.02584848875146699, - -0.0010000000000000087, 0.11950701379895641, 0.14835160377592957, 0.0010000000000000046, - 0.03690750430741206, 0.2997518913483449, 0.18925595848977467, 0.012470769145842907, - 0.005029411576307471, -0.43537531720787187, 0.267326786798688, -0.3591888000245844, - 0.015087858393835094, 0.0021186279199377316, -0.21837113549479728, 0.5427999050822361, - 1.030967407899835, 0.9969763773483896, -0.08424140780996803, -0.07308939125438643, - -0.02340746212419581, -0.07416127757540318, -0.0006222557044752723, 0.06802231095655131, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1678028205349222, 1.189981933936591, 0.7000086722004658, 0.6992348616617082, - 0.01072486120668631, 0.01394353987577191, -0.7146755650536559, -0.5196625515333979, - 0.0063569326987455875, 0.0009999999999999992, 0.08290732260396433, 0.02541723781941225, - -0.0010000000000000033, 0.11650208622613703, 0.14882252751282757, 0.000999999999999997, - 0.04947414299881186, 0.3042427088014555, 0.18778513950869863, 0.008663962315126837, - 0.015493021225246746, -0.43155161516281787, 0.2659760415266198, -0.3570104898665954, - 0.016556185023549014, 0.0036154253018742155, -0.2203679325302977, 0.5446334554724953, - 1.0325643599103473, 1.0004231628946578, -0.08235545725608973, -0.06945334349173687, - -0.022231682567667697, -0.07387832831548594, -0.0003821537887275367, 0.07049420725059449, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16927347247388927, 1.1835336701090127, 0.6993026923509419, 0.6993134416693553, - 0.009926700196314305, 0.014558412617053002, -0.7145979453831678, -0.5185342572449458, - 0.015015284472929486, 0.0009999999999999955, 0.08228451072231291, 0.024940024557305707, - -0.0010000000000000187, 0.11337027833226383, 0.14932976822532187, 0.0010000000000000263, - 0.06240823008299004, 0.30888062576714875, 0.18626138210375473, 0.004849952868101182, - 0.026185043512090354, -0.4274527613499098, 0.2646053675140273, -0.3548074638059399, - 0.01798833267651685, 0.00515314802814441, -0.2223809962589022, 0.5464451653036425, - 1.0341461833928296, 1.0039829348952485, -0.08043292232438963, -0.06576488236074517, - -0.021026792730484082, -0.0735745533371772, -0.00013944997827615262, 0.07303599041291604, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.17120871503820934, 1.1772180918451707, 0.6986906597149507, 0.699552362416358, - 0.009152470440255873, 0.014985476773756954, -0.7143655646854739, -0.5164642978632179, - 0.02241234286450643, 0.000999999999999995, 0.08031440992163452, 0.023796326953989498, - -0.0010000000000000044, 0.10950455529908862, 0.15013299442356853, 0.001000000000000013, - 0.07735072086588111, 0.3169326916578117, 0.18382838188927048, 0.0017243557501401253, - 0.03629397734832505, -0.41917411731265786, 0.26292330017003734, -0.35255512904347863, - 0.019045537043892346, 0.009684093463920254, -0.22465666276107255, 0.5467891370692318, - 1.0357363736286735, 1.0076292870714785, -0.07807494170493974, -0.06189613843802694, - -0.02016730193234829, -0.07278568344753052, 0.00024744099856062663, 0.07598939843719617, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1731877658456449, 1.1709300315947524, 0.6981319296522456, 0.6998044382763855, - 0.008380527018624033, 0.015396332142526496, -0.7141193652969517, -0.5143214040932603, - 0.029706770706260214, 0.0010000000000000005, 0.07822442821870218, 0.02259160823824746, - -0.0010000000000000057, 0.10556962595786169, 0.1509800419068431, 0.0010000000000000308, - 0.09246266655548599, 0.32525075483155663, 0.18130855920516106, -0.0013504021975745757, - 0.04635586898492734, -0.4105520028781284, 0.2612183672126167, -0.35029481360070097, - 0.020061887367309995, 0.014455634362416042, -0.2269444569315639, 0.547020822461916, - 1.0373275229584231, 1.0112812634064807, -0.07568409661840245, -0.05801108521911676, - -0.01933307955974598, -0.07195508720974672, 0.0006450355260720911, 0.07897759335246864, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1756480900692386, 1.16481701355808, 0.6979513266149252, 0.7003109575823132, 0.007638169305269869, - 0.015524968012647477, -0.7136281920075846, -0.5104656011157038, 0.033972466386712655, - 0.0010000000000000007, 0.07450519736744243, 0.02095667781802878, -0.0010000000000000026, - 0.10128917211193994, 0.15197357396173886, 0.0009999999999999807, 0.10665436606060444, - 0.34005379064941477, 0.17789080399548232, -0.0039012794944876103, 0.0528504433776581, - -0.39717093595701763, 0.25984093084552495, -0.34815367308342665, 0.022864309212310824, - 0.0225982945992746, -0.2295313381472833, 0.5459037093180436, 1.0387718309461833, - 1.0151144198690034, -0.07262223820848109, -0.0532051362601688, -0.018446664650454202, - -0.07074804266528963, 0.0003463983066966332, 0.08269544132751168, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.17562505889665325, 1.1537271100082653, 0.6953790790766645, 0.6982730882026361, - 0.003396181780092311, 0.017831052415195465, -0.7156012952832304, -0.5152108306690721, - 0.06985262860059206, 0.000999999999999999, 0.10225220877642745, 0.01388205135977135, - -0.0009999999999999953, 0.09694639857616874, 0.15968143315112895, 0.0009999999999999517, - 0.1161195043409712, 0.35327974170508436, 0.1780752557948968, -0.005707188107282141, - 0.08628885693021653, -0.41308345429745646, 0.26515957579256216, -0.3459703390903944, - 0.0077649703918351645, 0.03987659772426396, -0.22433979002085014, 0.5501627878725496, - 1.0401901865802996, 1.0137954867829317, -0.06950096386166345, -0.04604798786507819, - -0.01757112965116859, -0.07038261657194719, -4.9648900636258e-06, 0.08705828142756329, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.17287806542446144, 1.1442226177743215, 0.6967459612023406, 0.6969343344584732, - -0.0020125924937040188, 0.018282108528143878, -0.7168990496800398, -0.5025104429667472, - 0.09463824528766863, 0.001000000000000005, 0.12359068708280238, 0.019970043021698563, - -0.0010000000000000122, 0.09345770946189248, 0.16323601627823903, 0.0010000000000000336, - 0.11990320113274414, 0.37160159108833485, 0.18155528962943726, 0.0007361971194426682, - 0.10466518288223246, -0.40436040474024737, 0.2713766904244834, -0.3600884453408905, - -0.0010791661689933895, 0.049141302440429226, -0.22259656217437782, 0.5491978176267559, - 1.039918650390476, 1.0015879759249855, -0.06559017879942707, -0.033465614510440465, - -0.016331096998118265, -0.07288296394740965, -0.0011224979503678951, 0.09343060493774957, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16978461588475777, 1.1369040773504273, 0.6988681117892387, 0.69539793808057, - -0.009359384250081305, 0.01804327311775032, -0.7183373510649075, -0.4826051121342283, - 0.11669101319426023, 0.0009999999999999911, 0.1442407800922162, 0.033807335570504596, - -0.0009999999999999948, 0.09016660088759955, 0.16618471484471603, 0.0010000000000000106, - 0.12115490729964243, 0.38520221722160675, 0.1887321938610597, 0.014662887674880888, - 0.12124786761081735, -0.38001692925474967, 0.27949380077746927, -0.3861001929144562, - -0.007703216035676238, 0.05348439276680884, -0.22102439630669574, 0.5460162888933222, - 1.0394962275394237, 0.978204693674296, -0.06160583475687063, -0.0154819076604071, - -0.015047828087070704, -0.08022986194687547, -0.0023141178716934484, 0.10404849630661485, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1685616598671063, 1.131988580307788, 0.7016861447231297, 0.6937651552738989, - -0.01864632220192159, 0.018067397923153716, -0.7197331402182422, -0.45938048010196597, - 0.1314716744845694, 0.0009999999999999994, 0.16436531821948439, 0.05119171736810574, - -0.001000000000000012, 0.08701532198356214, 0.1685158214456028, 0.0010000000000000198, - 0.11878086008707876, 0.40287858507940916, 0.19849771584443374, 0.03543585884410838, - 0.12889176076762715, -0.34044010198140506, 0.29139649673761786, -0.4210796963968806, - -0.007488093944203087, 0.06051077953433767, -0.2194151224747646, 0.5391641772812505, - 1.037244371654297, 0.9431337689897671, -0.05829538360911303, 0.00970833086781105, - -0.0135364951866454, -0.09519330305970564, -0.0034300578946336734, 0.1213504360102254, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.16953202323191655, 1.128434859706133, 0.7044606786087722, 0.6918690368300776, - -0.028935643347340865, 0.01897027944146114, -0.7211935197419688, -0.4368643495550697, - 0.144035336013829, 0.0009999999999999955, 0.1836241214913561, 0.06922815091759905, - -0.0009999999999999944, 0.08392884625178455, 0.1719895427168746, 0.0010000000000000063, - 0.11580525736095441, 0.4173400898506902, 0.20866765946644186, 0.06008326395583906, - 0.13443718610302813, -0.2954630686351042, 0.30587205106640514, -0.4630187121365578, - -0.003560951157667941, 0.06963971962856191, -0.2168027849846486, 0.5288059634645779, - 1.0348115233023405, 0.898638372361137, -0.05504674029823359, 0.043198029892149645, - -0.011998878320224038, -0.11915187800587329, -0.004530072798608685, 0.1489508911925559, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.17254861393920215, 1.1251326657844622, 0.7077201614023688, 0.6901402536705386, - -0.03950198866114908, 0.020692412434519927, -0.72230038572814, -0.4143041093541598, - 0.1479266032135112, 0.0009999999999999992, 0.20121607584408294, 0.08667858254859043, - -0.0010000000000000063, 0.08084275100175238, 0.17346403879508124, 0.0010000000000000035, - 0.11091696410493512, 0.4412382999592158, 0.218032461974613, 0.08834521352325854, - 0.13530700409407784, -0.2523688100225217, 0.32268327708374245, -0.5102485016579167, - 0.0030540528777552077, 0.07987969343869121, -0.21468742901709983, 0.513096093265494, - 1.0313911588716413, 0.847465167127492, -0.051869364224715435, 0.08584580893473649, - -0.01076540197616383, -0.1525295905914866, -0.0060181916014082765, 0.18877252312295584, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.17674401201715995, 1.1215171086454572, 0.7108439872646962, 0.6887580365864695, - -0.049120302808135785, 0.022945988916048918, -0.7229613022024435, -0.3939014327281533, - 0.1485364227991125, 0.0010000000000000013, 0.21566442044312906, 0.1019532429211561, - -0.0009999999999999983, 0.0779739068104992, 0.17433408611807044, 0.000999999999999995, - 0.10681337798568585, 0.46724029907265324, 0.2251485529527953, 0.11602526551156743, - 0.1320261843909475, -0.21300946117541716, 0.3391470984956956, -0.5578915534157557, - 0.011686840439465763, 0.09141538629747906, -0.21322041455581894, 0.49342976848473014, - 1.0278615513231883, 0.79824464444197, -0.048697398212579956, 0.13694919590453236, - -0.009568980850200661, -0.19215753876883412, -0.007551321353537779, 0.24018738652720548, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18147257985774842, 1.1169901681470238, 0.7140698352882774, 0.6879691603869627, - -0.0574380103923505, 0.02545843698262947, -0.7230153368394254, -0.3749819165922022, - 0.13953320085198875, 0.0010000000000000085, 0.22726710420044433, 0.11502332382368001, - -0.000999999999999992, 0.07536234532040012, 0.17192405036004774, 0.0009999999999999963, - 0.1030446792819225, 0.5024293381553363, 0.23034551507665665, 0.14057299784620686, - 0.12520819840900546, -0.17244451425299404, 0.3563771729162595, -0.6015771259559458, - 0.02141417847932195, 0.10316995996900787, -0.2153975730644013, 0.4713769924708395, - 1.0221761365321433, 0.7563243316826573, -0.04743212411070471, 0.1924300720465405, - -0.008127368171285912, -0.233050028459023, -0.010185055706012826, 0.29786564772609403, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18633015349636323, 1.111533828513026, 0.7173311404193472, 0.6877266897578593, - -0.06433780627254868, 0.02815525539107034, -0.7225648264845147, -0.35770612657314754, - 0.1271676270598797, 0.0009999999999999998, 0.23591664621390843, 0.12624319335577688, - -0.001000000000000012, 0.07265794008024042, 0.16805021804131895, 0.0009999999999999994, - 0.10024884546445556, 0.5419504585229598, 0.23298651077194665, 0.158998602390684, - 0.11468718314365856, -0.13633609721179302, 0.37236016412303113, -0.6369623723436661, - 0.03169858162090515, 0.11492127228362767, -0.2190953333688895, 0.4500164451555941, - 1.0162404901384787, 0.7264977058119746, -0.04638511465263241, 0.2477722994873648, - -0.006659844150985404, -0.2700951895784355, -0.01295452557920169, 0.35645023491176675, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19076712864002207, 1.1051886312262107, 0.7205061001501437, 0.6879764536345825, - -0.07014744714458514, 0.030871271276783074, -0.7216749264823012, -0.34131842698658577, - 0.1071225066571195, 0.0009999999999999942, 0.24332055402739897, 0.1353790686764575, - -0.001000000000000002, 0.0703536106596113, 0.15983300099207173, 0.0010000000000000015, - 0.09767221944192457, 0.584002471925005, 0.23456298984558907, 0.17262751395677492, - 0.10158194876198663, -0.09984926236222046, 0.3884079115723226, -0.6639309597880229, - 0.041145541048517205, 0.12511367205143767, -0.22474686898482005, 0.4300229106462731, - 1.0085201239956478, 0.7053312838377561, -0.045752682599666104, 0.29787801393466673, - -0.005406100455785414, -0.30109823340294467, -0.01589789652751479, 0.40875074604503214, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19509451270492337, 1.0982832138970713, 0.7237752273380523, 0.6885540735298247, - -0.07511021525093332, 0.03362279174696365, -0.7205006948404573, -0.3257607553641674, - 0.0843282922251708, 0.0010000000000000009, 0.2490897405114812, 0.14350245232058886, - -0.0009999999999999974, 0.06790573760994172, 0.1502940115651565, 0.000999999999999996, - 0.09565751725020612, 0.6297000350486454, 0.23469193094427612, 0.18101903789364876, - 0.08662336840702918, -0.0662060621539351, 0.403631886717107, -0.682855473658229, - 0.05058176741988204, 0.13487202951958205, -0.23131592387386038, 0.41278325213856015, - 1.000580398423002, 0.6922806529809928, -0.045178418927092026, 0.3411965399380814, - -0.004176918439115869, -0.3245242582362686, -0.018874738126405523, 0.4534276926238786, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19893098356676744, 1.090578231689522, 0.726910253945332, 0.6895675666379228, - -0.07949294831798272, 0.03613335791410536, -0.7189379824815345, -0.3097818870911894, - 0.05583310703727645, 0.0009999999999999996, 0.2541448742065881, 0.14836320696683614, - -0.001000000000000002, 0.06631189209674533, 0.1349054875439881, 0.000999999999999989, - 0.0932613531756342, 0.6743812698316832, 0.23364382343269072, 0.18798865436314507, - 0.07000681761283295, -0.04752293419232239, 0.4180165018240912, -0.6969970076247985, - 0.057332558678979004, 0.13588075951445663, -0.23881121905310537, 0.3981558651113197, - 0.9928624484886878, 0.6824264931066418, -0.045443540039186596, 0.3767165494198764, - -0.003027090737255482, -0.3430024244498567, -0.02068763135049278, 0.48726467095282244, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20260500973059387, 1.0827319156987933, 0.730029402723097, 0.6907533367246305, - -0.08338340429073879, 0.03851548568299262, -0.7172332905375204, -0.2943274087495592, - 0.024902347957919325, 0.0009999999999999985, 0.258160795401602, 0.15175471604101426, - -0.0009999999999999827, 0.06473209699735129, 0.11796266055011287, 0.0009999999999999985, - 0.09116821120732926, 0.7218452752056848, 0.2318059848207057, 0.19235088174128023, - 0.05280498048288374, -0.032183374077738816, 0.4316004148763603, -0.7065373696784696, - 0.06373903603372509, 0.13522034966381244, -0.24683156165077078, 0.38571886548014545, - 0.9851703325316694, 0.676132161140881, -0.045825955727400294, 0.4056995330020375, - -0.0018846041853725235, -0.3565365040717075, -0.02235956373924439, 0.5131271947545152, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2057064007786396, 1.0746646721925355, 0.7329007105218989, 0.6923524567771527, - -0.08684326872780058, 0.040474646461802016, -0.7151699974587182, -0.27907655936612813, - -0.00925663233281772, 0.0010000000000000015, 0.2613072334854161, 0.15260572739009085, - -0.000999999999999985, 0.06334105300173976, 0.09818759362463625, 0.0009999999999999957, - 0.08897188898036204, 0.7609816608080479, 0.2294971809888552, 0.19723372715712095, - 0.03618957704802113, -0.03199154555359658, 0.4439767173383279, -0.7139834689619979, - 0.06721840470294403, 0.1274996116931425, -0.25487751666078395, 0.373777499744447, - 0.9779340125321866, 0.6695549837623987, -0.0451674416500519, 0.42684409896705133, - -0.001159972559952258, -0.36698230163796797, -0.022106295840483552, 0.5300863098409799, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20862196591829227, 1.066693908902154, 0.7357045530765106, 0.6940580697785764, - -0.08994075370719916, 0.04224164590796024, -0.7130285407673406, -0.264391166849768, - -0.044915277181818335, 0.0009999999999999998, 0.2636038547086201, 0.15185115976423064, - -0.0010000000000000007, 0.06194473640976687, 0.07748945726810086, 0.000999999999999999, - 0.08700971607623953, 0.800524966783656, 0.22674195047045498, 0.20087039292570436, - 0.019827961386219922, -0.03427282201239359, 0.4555582618645302, -0.7189727101790628, - 0.07029853334762279, 0.11822630031443018, -0.26317462160006094, 0.3630401157997551, - 0.9707573258610499, 0.664355546940694, -0.044379994325011565, 0.4431898484591609, - -0.0004936837798509752, -0.37473191495598496, -0.021601577730326928, 0.5412511227884956, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21050725340820828, 1.0586812928690335, 0.737567298494393, 0.6962706037468351, - -0.0923511916406361, 0.043496422196133214, -0.710483331976695, -0.25080539130735147, - -0.0805931780032873, -0.0005194601353435944, 0.2650353861233651, 0.1502146544064345, - -0.0009999999999999966, 0.05997441811638075, 0.05693602398712079, 0.0009999999999999987, - 0.0850314765474476, 0.8240534965380808, 0.22461789878598554, 0.20650925207376702, - 0.0036171084906868925, -0.044964767714807306, 0.46595341306483534, -0.7246359559922293, - 0.07148313883935872, 0.10724450104979324, -0.27019555484438235, 0.35357492676915403, - 0.9627120463125518, 0.6551523072722817, -0.043824005455113536, 0.4570699636041876, - -0.00042117925668105563, -0.38369421280237836, -0.019999742915508368, 0.5474618160997728, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21213120292148163, 1.0507818075944395, 0.7393097992497911, 0.6984812791687778, - -0.09453521413289198, 0.04452441852530133, -0.707958029896249, -0.2374473077360638, - -0.11637754567124302, -0.0009999999999999994, 0.2660430961001778, 0.14769973204381903, - -0.0009999999999999983, 0.058102357856155466, 0.03643826984907409, 0.0009999999999999861, - 0.08322238204933328, 0.8454701810668623, 0.2220149191777905, 0.2127625775607003, - -0.012287451822696305, -0.05660684502117267, 0.47597663247894934, -0.7288256702522864, - 0.07231946486889189, 0.09562597474882019, -0.278169449551614, 0.3439997468452139, - 0.9545446883127691, 0.6458633162252703, -0.04331140391645565, 0.468040533111044, - -0.0004338608870159116, -0.3914635069702529, -0.018243645172282715, 0.5499589509793386, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2133697273300368, 1.0430650959763266, 0.7408784979679262, 0.7011475041532615, - -0.09605924992686164, 0.044860319326553245, -0.7050903131323393, -0.22484212774732795, - -0.14881850954316894, -0.001000000000000001, 0.26511082120053125, 0.14492027016766562, - -0.000999999999999994, 0.05565293554412287, 0.019067642434566958, 0.0009999999999999987, - 0.08148503601552502, 0.8437487042660566, 0.21840143780981153, 0.22117177047117506, - -0.026352386079437153, -0.06550376250547019, 0.48324038174268397, -0.7337302466587198, - 0.07135322968260843, 0.08633902830211092, -0.28638553144660667, 0.3333336500410614, - 0.9484478410627668, 0.6349091109143207, -0.042875845466489065, 0.4756784558743115, - -0.0011981255563923356, -0.3991256609845754, -0.01479830079061363, 0.5507044010426355, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2144442450850821, 1.0353464029550492, 0.7424685464236945, 0.7039499671216921, - -0.09734042624792125, 0.044978026673249266, -0.7021084405729807, -0.2123445038949681, - -0.1800068777782575, -0.0009999999999999983, 0.26343965213176784, 0.1416180163904234, - -0.000999999999999996, 0.052943459877346805, 0.0020754423598715248, 0.0009999999999999974, - 0.07982989286714853, 0.8375555439986211, 0.21447405651848203, 0.22995976886462885, - -0.03988514840044762, -0.07352717988347439, 0.489676280227772, -0.7378794205778626, - 0.07005997969682373, 0.07727015410104933, -0.294864549985798, 0.32282223568730545, - 0.9426615228459477, 0.6237039336091904, -0.04245151299554008, 0.4810269663013499, - -0.0020728442287480013, -0.40591378293942093, -0.01109541024183568, 0.5492633740471186, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21547063148625062, 1.0277306484218138, 0.7438515650437906, 0.7072952827378438, - -0.09785957207126854, 0.04435357608403231, -0.6987056944518051, -0.20035997550709791, - -0.2056671081598724, -0.0009999999999999966, 0.2592252889366982, 0.13707816161873382, - -0.0010000000000000005, 0.0497811248987636, -0.012160504003304798, 0.0010000000000000035, - 0.07824895355396963, 0.8074136795847581, 0.21005144243667515, 0.240593514851428, - -0.05264118360843688, -0.07420349468546851, 0.4929792579210419, -0.7438297095471474, - 0.06809956201295905, 0.070501887212945, -0.30316068580158734, 0.31136225429159875, - 0.9383137887707568, 0.6112111995141918, -0.04229709542399378, 0.48510700132260576, - -0.003581255673079037, -0.41234687792205155, -0.006576300476079462, 0.5497082931446058, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21641827553715362, 1.019999943932587, 0.7452364037575208, 0.7108493853306478, - -0.09805986850558673, 0.043379141787555794, -0.6951227687405163, -0.188190442726818, - -0.22891056521125147, -0.000999999999999998, 0.2537929286253005, 0.13182869506105477, - -0.0010000000000000135, 0.046342895116950854, -0.02576095385471584, 0.0009999999999999979, - 0.07677897140277572, 0.7717701870699999, 0.20543852378782138, 0.25257619545103266, - -0.0652405254909398, -0.07318598964966266, 0.4951601659615478, -0.7497803480137288, - 0.06608723106112449, 0.06421386315126675, -0.31190911190497106, 0.2994691620714701, - 0.9341922171187386, 0.5977803447960364, -0.04217807871729962, 0.48790635076970273, - -0.005184808769541606, -0.4183809758651896, -0.0019217513854826783, 0.549365294032932, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2173691751894177, 1.0122184438168786, 0.7463563219122319, 0.7150845794586659, - -0.09711641064703289, 0.04133989868954078, -0.6910234871404868, -0.17622707295120157, - -0.24400740099615578, -0.0010000000000000028, 0.24458968334795364, 0.12439578687034976, - -0.001000000000000015, 0.04205323839841837, -0.03577602281779592, 0.0009999999999999913, - 0.07553745869152102, 0.7123612602223341, 0.2001849158181313, 0.26863861685012824, - -0.07801213644937594, -0.06125736860248635, 0.4939275303253849, -0.7574854634150691, - 0.06402497957034851, 0.06128739426050857, -0.3213103113212565, 0.2858591764297075, - 0.9315961889843821, 0.5810796501583353, -0.04348362485701385, 0.49046094106028276, - -0.006953816002103786, -0.4249838812378361, 0.002317608111411488, 0.5505189265468163, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2183145335485107, 1.0040305612762537, 0.7473911967520812, 0.7197262675059439, - -0.09548244752435768, 0.03847291655219492, -0.686583597800506, -0.16349286999760893, - -0.2553648140215709, -0.0010000000000000044, 0.23258781202231074, 0.11501572132905229, - -0.0009999999999999972, 0.03742616726388061, -0.04467925098868063, 0.0010000000000000141, - 0.07467559046161294, 0.6472610987332038, 0.19476750599138032, 0.2889341175984312, - -0.09145481017190248, -0.047443582868111414, 0.4905588977784898, -0.7662584554351701, - 0.06229540899755002, 0.059364345424352934, -0.3318929244636031, 0.2703393231804873, - 0.9292505725713812, 0.561014568135306, -0.045016648911352, 0.4926924295458318, -0.00875241949639644, - -0.43215124182878933, 0.006491145071983349, 0.5516418905722469, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.219166003214111, 0.9952233943998179, 0.7474842851023018, 0.7252860529592547, - -0.09196870382929792, 0.033728616864295125, -0.6814427923921174, -0.15037396091650804, - -0.25809138991566627, -0.0010000000000000013, 0.21435215796857662, 0.10103629983885719, - -0.000999999999999982, 0.031716261540383074, -0.048946656993943745, 0.0009999999999999714, - 0.075219310087049, 0.5655541527986301, 0.18911445952537612, 0.318958443028373, -0.10699480216506756, - -0.024678439985164127, 0.4824702188482929, -0.7786607263104581, 0.06165102649299934, - 0.061546251225813045, -0.3442173685265044, 0.25103118761189447, 0.9277448941956031, - 0.5311396593151491, -0.047373123988935416, 0.4964004862543115, -0.01176122108072821, - -0.44130249377525327, 0.009779942221774954, 0.5537892219441439, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2200793656466452, 0.9850211346029057, 0.7472594374179236, 0.7315241645641415, - -0.0870971187539507, 0.027206525500498013, -0.675682095022272, -0.1347689303937141, - -0.2570240570479342, -0.0010000000000000015, 0.1906213863339938, 0.08233673648639799, - -0.0010000000000000122, 0.02564827244405581, -0.051710830452235325, 0.0010000000000000076, - 0.07707422561001014, 0.4833842073447389, 0.18367662318525868, 0.3607401030651697, - -0.12543908268224427, -0.003651749632442976, 0.47025005710498136, -0.793801702700788, - 0.06185411022472114, 0.06524570809741262, -0.35886193389723786, 0.22625554768050887, - 0.9263821123672394, 0.49168654045864474, -0.04986199093558954, 0.5004209099300223, - -0.014983768449440183, -0.45237833677676, 0.012914653662498023, 0.556298419925874, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22104968981592296, 0.9726616968027482, 0.7463368153231318, 0.7386594995374165, - -0.07991121724293589, 0.018031254182901916, -0.669082367855018, -0.11600389825437359, - -0.24869735608478502, -0.0009999999999999887, 0.1589318346013692, 0.05659541804691907, - -0.0009999999999999868, 0.019013427739051077, -0.04961584280563814, 0.000999999999999989, - 0.08149892015508, 0.3984524443576959, 0.178624095949168, 0.41904408492088135, -0.14884969224501515, - 0.017775554084301145, 0.45111246591077353, -0.8118605295936339, 0.06342681401558627, - 0.07304880102537807, -0.3755397621840776, 0.19485396745665862, 0.9264275999244367, - 0.438824984241287, -0.05189127729794561, 0.5052356502041933, -0.018783261281270745, - -0.4655683026595675, 0.01578609575745508, 0.5607480032429014, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22211769619168653, 0.9571489584343499, 0.7452370319755293, 0.7459147863519009, - -0.07180020420164461, 0.007105998694566734, -0.6621218671519268, -0.09157488717423275, - -0.23941745089629374, -0.0009999999999999968, 0.12364812378122354, 0.02707499650590748, - -0.0010000000000000022, 0.012285747978708478, -0.04635550166782541, 0.001000000000000007, - 0.08819975270172059, 0.3254293503543272, 0.17517329781680682, 0.49574610818543197, - -0.178203047599276, 0.028699868110574123, 0.42832079343493035, -0.8304016394754492, - 0.06514225126249426, 0.08179776501144974, -0.3932828345738061, 0.15586876114232293, - 0.9267222041232529, 0.3733794698538705, -0.05382802930326483, 0.5077556740500669, - -0.02268936670231502, -0.480347210297734, 0.018610923124731654, 0.5648387156118282, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22322836537444965, 0.9387887661398012, 0.7438484384629915, 0.7525815370441538, - -0.06335986398249052, -0.00492574890516495, -0.6554252777655912, -0.06257170573958888, - -0.22747156072010896, -0.000999999999999994, 0.08796327659233007, -0.0029555659755997267, - -0.0010000000000000046, 0.0057834305127815785, -0.03805161231530523, 0.0010000000000000002, - 0.09872469583782348, 0.2646579403569549, 0.1735461255261732, 0.5826566595382197, - -0.21292752130393316, 0.032979501451051524, 0.4030561737768798, -0.8455693410702609, - 0.06718384759159896, 0.0926117585073869, -0.4102866273907177, 0.11475308923439842, - 0.9296749188802271, 0.3023788319268836, -0.05593832563041558, 0.5059593644313032, - -0.026683896522126653, -0.4938830512133927, 0.02152468479919885, 0.5698395836966665, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2245830985185411, 0.9179401641612623, 0.7425516375126389, 0.7580450445050536, - -0.056474623963595046, -0.016459503828965372, -0.6495440031923985, -0.028062010402817285, - -0.21695557737218704, -0.000999999999999996, 0.057491532777793834, -0.028224251796961957, - -0.0010000000000000243, -0.00041251008678827426, -0.028781431757257857, 0.001000000000000017, - 0.11040118703188523, 0.21748508096147054, 0.1746891445602557, 0.6731980679807849, - -0.25165377380426196, 0.027029477952105523, 0.38042697839128337, -0.8562792965927752, - 0.06820702580285459, 0.10263623041660788, -0.42438693577625336, 0.07522115992287692, - 0.9331232132065089, 0.23462140223098962, -0.05807192794564756, 0.5004058585404046, - -0.030695514089193385, -0.5073861833602822, 0.024451692608034346, 0.5766600483134329, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22640350633901685, 0.897467487864544, 0.740021916427541, 0.7623662945583641, - -0.051829908117663616, -0.02579755529317084, -0.6445508356109401, 0.006020226224855417, - -0.20905275152726485, -0.0010000000000000189, 0.03490555913652906, -0.0457981007214224, - -0.0010000000000000273, -0.006852810246658608, -0.015531331313828039, 0.0009999999999999972, - 0.12213188202918565, 0.18378719734938603, 0.17767991749386045, 0.7503119693069231, - -0.29149125558351, 0.016293804467497227, 0.36401492965183174, -0.8632252681785266, - 0.06726012472445599, 0.11164738232264244, -0.4339145431728047, 0.04276505924132051, - 0.9359727946157584, 0.18113408406201031, -0.057211742138959965, 0.4964483974799335, - -0.0345911931320866, -0.5179027434515342, 0.023415589444510002, 0.5909397375768976, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2287223283641949, 0.8771068852391406, 0.7373641205681069, 0.7659071499648754, - -0.049256248327507, -0.033423136522815616, -0.6401897793454367, 0.04233022363117354, - -0.20359033244427374, -0.0009999999999999872, 0.01877229745642986, -0.05704245603809161, - -0.0009999999999999846, -0.013643806165529347, -0.0018435738544810667, 0.001000000000000008, - 0.13015114208866335, 0.1565721146126926, 0.18181257896234337, 0.8169729488588174, - -0.3293953196393298, 0.002477148242580295, 0.3528560490794435, -0.8673872566947234, - 0.06566310610363636, 0.11942791156730967, -0.44076236325838286, 0.016809427785856458, - 0.9387034654767047, 0.14126968335076628, -0.0557721136047584, 0.49347409165527256, - -0.03846678402788032, -0.528379318135732, 0.02161282008419561, 0.6104269118199872, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23058524020012328, 0.8589521491500546, 0.7358325888604805, 0.7685497993451975, - -0.04787419118052287, -0.03927245337744082, -0.6367864180013456, 0.07476580661209849, - -0.19840277530088887, -0.0009999999999999974, 0.008141884321910351, -0.06280172015106054, - -0.0010000000000000085, -0.01996460361787362, 0.01211304929156722, 0.000999999999999981, - 0.13771461646647815, 0.13559619459850691, 0.18602540456017722, 0.8622775670202577, - -0.3608385176191905, -0.005533520449660279, 0.34515626941932315, -0.8689862954683061, - 0.06694251052436295, 0.12713862164316703, -0.4444307171294461, 0.0041860502463255365, - 0.9441187427288443, 0.11549430204315249, -0.05627217091229304, 0.4879123310008739, - -0.040795587642339065, -0.5350216568758349, 0.02253387274214586, 0.6267969052245803, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23261335196643163, 0.841716560362957, 0.7345949248237613, 0.7709189221405295, - -0.04754390240130089, -0.04384336715980246, -0.633641343337247, 0.10715740533292885, - -0.19495148452587008, -0.0010000000000000122, 0.0012049715660615154, -0.0649735203425949, - -0.0010000000000000024, -0.02670382268162932, 0.02551675365358264, 0.0010000000000000106, - 0.14159007443099905, 0.11728172421356421, 0.19000749401042966, 0.8976150087228503, - -0.3882122283575609, -0.012520683701896108, 0.34056509754126274, -0.8694551571102993, - 0.06887449155756993, 0.13425179185410469, -0.4468649140692691, -0.0026738007896903557, - 0.9500817548801246, 0.09881586790381552, -0.05716443041464322, 0.48246279482124027, - -0.04280672714989907, -0.5408534179213341, 0.024007114562412706, 0.6440143334124894, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23406224977536283, 0.8276972430676611, 0.7334400539737033, 0.77329413660209, - -0.04731224745305312, -0.04675459673836896, -0.6305487587979238, 0.13130468956696495, - -0.19305465624861154, -0.0009999999999999818, -0.0030056506409198683, -0.06540723502844753, - -0.000999999999999993, -0.034439067960646406, 0.03636258334018506, 0.0010000000000000015, - 0.1464769772990281, 0.1050192768352728, 0.19310915530663, 0.916099388142441, -0.41138776638759067, - -0.015579557676205226, 0.33832378012719294, -0.8695645335331895, 0.0711941884044427, - 0.13916134526830395, -0.4469794392900361, -0.0035395763639799803, 0.9553650818828316, - 0.08878414465787889, -0.05752173739607372, 0.47828962044553214, -0.043076576191561626, - -0.5410754795465834, 0.02442024809492338, 0.6585250715370656, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23545908371899646, 0.81465954399941, 0.732312546631415, 0.7757087319434456, -0.0472993140510503, - -0.04896952846353468, -0.6274079401466398, 0.1536395406769295, -0.19224994796306746, - -0.0010000000000000044, -0.005844513668276734, -0.0648024183260069, -0.0009999999999999966, - -0.042712318033655286, 0.04639023423746164, 0.001000000000000012, 0.15021631532024743, - 0.09428620496173422, 0.19572860052531077, 0.9289796059026142, -0.4324488406608565, - -0.01746214493829441, 0.33734177002868493, -0.8693033434915942, 0.07368879325273216, - 0.14339445758514416, -0.44657816378079357, -0.002209520668367844, 0.9605031233323439, - 0.08183183464180904, -0.057764820284100106, 0.4744450299240953, -0.04297511518035002, - -0.5397049193197738, 0.024607893603816787, 0.6724942380214594, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2361937044975438, 0.8044129514796576, 0.7312654321461095, 0.7780806524840338, - -0.0468680014305694, -0.05028763617060783, -0.6243917378699814, 0.1676932493839203, - -0.1915412963747356, -0.0009999999999999913, -0.007845562952843508, -0.06448955885429261, - -0.0010000000000000221, -0.0510324835707435, 0.05252708438154285, 0.0009999999999999966, - 0.15744746108986768, 0.08858603014845742, 0.19752387979372885, 0.9285793271965346, - -0.4502493756635961, -0.014699687799508246, 0.3366101101021253, -0.8696769597499258, - 0.07553136214460868, 0.14534952256119107, -0.44482299558918337, 0.003072622773103536, - 0.965696346011206, 0.07793959017747269, -0.05817360464421948, 0.47044331272183415, - -0.042049881976948394, -0.5338168415292694, 0.023510216332485137, 0.6813254886073555, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23678441895686891, 0.7948564369839553, 0.7302236067455984, 0.7804524669662292, - -0.046352777525870886, -0.051343794815725236, -0.6213768434339066, 0.17995803925383116, - -0.1910137523933512, -0.0009999999999999983, -0.009558931006290334, -0.06416583305419882, - -0.0009999999999999718, -0.05946296607701898, 0.05776087942122962, 0.0010000000000000076, - 0.16504434780226912, 0.08401196989283126, 0.19905107782692044, 0.9248695669175516, - -0.46702637455832696, -0.010735892769641365, 0.33602603809676385, -0.8700589961969711, - 0.0772604315789081, 0.14678830102666326, -0.44278814338012223, 0.00938215415813541, - 0.9709014502364303, 0.07483145742043891, -0.05861839442847261, 0.4663574328224598, - -0.04094169129902607, -0.5267272420996009, 0.02212925599131729, 0.6888730148965121, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23661807068360738, 0.7874078350086188, 0.729051575119962, 0.7827708395501984, - -0.04530079251632852, -0.05186169764908411, -0.6184884924266777, 0.18462375802883296, - -0.19018123939486611, -0.0010000000000000048, -0.011020130491261552, -0.06462911258185208, - -0.0009999999999999911, -0.06734352371535635, 0.059102163894023285, 0.0010000000000000048, - 0.1776433553715806, 0.08313268729621838, 0.20013968539963636, 0.9114923203620944, - -0.4817087049575725, -0.003961873233402229, 0.3350584457076681, -0.8707724503713157, - 0.0773343322623675, 0.14663060457987892, -0.4389301597276543, 0.018194981578793953, - 0.9762197141813596, 0.07281514919740968, -0.058187764105350724, 0.46268266022885157, - -0.039809695514003145, -0.5167650059913524, 0.019703650967019485, 0.6912575082319511, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23626958112146232, 0.7804583520586077, 0.7278512867429423, 0.7850675017842053, - -0.04411689783127849, -0.052254247463168806, -0.6156234324569764, 0.1876901184066544, - -0.18927580187831608, -0.0010000000000000106, -0.012471816518130695, -0.06529297923895498, - -0.0010000000000000176, -0.07510297802495805, 0.05956134141880512, 0.0010000000000000009, - 0.19113283294309039, 0.08309695751089374, 0.2011159086513507, 0.8955863045643014, - -0.49579161567751856, 0.0035429830672883468, 0.33396349807438885, -0.8714268499810248, - 0.07702115967001039, 0.14613924184050614, -0.4346532220197354, 0.02773950400898114, - 0.9815642151121239, 0.07133993923717123, -0.05755534424057204, 0.45916485393038287, - -0.03867316151450603, -0.5061617701473398, 0.017038700081809518, 0.6925175957269051, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.23527861038822998, 0.7750171179048707, 0.7265843620035538, 0.787273335088441, - -0.042707653619624676, -0.052154418454733, -0.6129083690215683, 0.18403605761164565, - -0.1880191567756587, -0.0009999999999999905, -0.013299109273594638, -0.06612288377248193, - -0.0010000000000000037, -0.08203230944706626, 0.05726756250660975, 0.0009999999999999874, - 0.21061533586865377, 0.08476364353777491, 0.2023306623024087, 0.8710210577235317, - -0.5077324449700966, 0.010345551092174119, 0.33307311291427577, -0.8718885783684667, - 0.07388709724280641, 0.14529432173488735, -0.4289813474256082, 0.039897067955183496, - 0.98665890472612, 0.07263184131306497, -0.057407303149077216, 0.45713447199613416, - -0.03766180871495972, -0.4947708231583205, 0.013837664850595078, 0.6905337382008813, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2341364592803149, 0.7699716405585976, 0.7253200775906915, 0.789442139369821, - -0.041262765449046016, -0.051936359457837006, -0.6102303723520852, 0.17900756336868792, - -0.18667116490113062, -0.0010000000000000083, -0.01402998533356567, -0.0669928952546289, - -0.0010000000000000048, -0.08872881619439954, 0.05433707485940934, 0.0010000000000000117, - 0.23113087071408778, 0.08684628910893484, 0.20362384314401547, 0.843666325587961, - -0.5189566640661458, 0.017065244649898278, 0.33221208824097914, -0.8720638632818485, - 0.07006942629710976, 0.1444017109220164, -0.4229575740750091, 0.053022868875017934, - 0.991693812123342, 0.07559936477457457, -0.05737347594091945, 0.45567317216081876, - -0.036679166861139686, -0.48347225591756054, 0.010509821519365544, 0.6881942281043518, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2325170157230341, 0.7663009480116362, 0.7245287301619183, 0.7915840011098794, - -0.03993275144552439, -0.05099362686429464, -0.6076181321916673, 0.16737585302544752, - -0.18597507579786396, -0.0009999999999999922, -0.0134732130297675, -0.0671904011135888, - -0.0010000000000000189, -0.09487315964906907, 0.049117994687197374, 0.0010000000000000052, - 0.2575674055573557, 0.08952486379278, 0.20506178946859596, 0.8068541516807074, -0.5256535247142279, - 0.01825789541307276, 0.3319035226159773, -0.8713891633776497, 0.06293306258777191, - 0.14163313467470184, -0.4160576383084123, 0.06971852746455533, 0.9966874816712104, - 0.08377108234989628, -0.0582911712463828, 0.45467293449803753, -0.03567342569820974, - -0.4718589414252695, 0.0073399268051285846, 0.6826458891995767, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2307828315887276, 0.7630082013744369, 0.7238943425365898, 0.7936863310099056, - -0.038662986646955735, -0.04991766175263129, -0.6050416584638798, 0.15469014364535294, - -0.18536073369605355, -0.0009999999999999874, -0.01270985524389271, -0.06725313692843359, - -0.001000000000000009, -0.10084138873160003, 0.04342935652973679, 0.0009999999999999916, - 0.2845483493482965, 0.09236959238717618, 0.20664357907745454, 0.7666812281649871, - -0.5307870535358346, 0.01816607531182837, 0.33166764509128166, -0.870051487603628, - 0.054974606433054844, 0.13843072195992162, -0.4088941509260912, 0.0877409835757096, - 1.0016699001047642, 0.09488993863179332, -0.05944220392360004, 0.4539956097937017, - -0.03465930280809518, -0.46058357429423386, 0.004207898929988494, 0.6771513884466788, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2289705922797589, 0.760822195952859, 0.7238329649802326, 0.7957045281784088, - -0.03768758941208537, -0.04828171465263765, -0.6025801402891439, 0.13638221803509937, - -0.18567808063733346, -0.0010000000000000128, -0.01082463196810258, -0.06674867053346696, - -0.0009999999999999994, -0.10530956133713702, 0.03624865269162362, 0.0010000000000000024, - 0.31732784786374835, 0.09507991681962355, 0.20856324760311332, 0.7170752730300559, - -0.5294845085453599, 0.01376619535307344, 0.33258963240415435, -0.868545794628104, - 0.04263430814807805, 0.13359993662846434, -0.40268032907532314, 0.10885175961689668, - 1.006183100477512, 0.1127353441815642, -0.061305102721358085, 0.45450273573804945, - -0.03315641219937515, -0.44864361446119, 0.0009159127477802902, 0.6716197120372986, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22712732929840912, 0.7589477862636538, 0.7239849054410715, 0.7976391413057341, - -0.036805590625201764, -0.04664868967795392, -0.6002008401419568, 0.11757198279117632, - -0.18599486685387892, -0.0009999999999999866, -0.008920615337466083, -0.0662130460494275, - -0.000999999999999964, -0.10938704687397446, 0.028948004866848735, 0.0009999999999999944, - 0.3503194058702519, 0.09775181182442479, 0.21085169866732023, 0.6637750354917479, - -0.5259786614912794, 0.008336849812134083, 0.3336025978966042, -0.866251434838068, - 0.02923299322190739, 0.12835636003836953, -0.39662738689791033, 0.13137125389207224, - 1.0105746704660814, 0.13446616344104803, -0.06334415345841163, 0.45528192191402483, - -0.03152212003584173, -0.4369069691280949, -0.002414407569000068, 0.6672467943315594, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22501642860660664, 0.7575369399382008, 0.7241586724463331, 0.7995382056741657, - -0.03593085718718855, -0.04500651922139809, -0.5978478438505094, 0.0938294899200382, - -0.18672098188911743, -0.000999999999999997, -0.006487876772987331, -0.06571642489731656, - -0.0009999999999999966, -0.11194697937525183, 0.021034666720468387, 0.0009999999999999998, - 0.39000443393712225, 0.0993829414657682, 0.21382306366739387, 0.6055638362552007, - -0.5169894252320305, 0.003297860996040806, 0.33517710833806197, -0.8641822170782506, - 0.012248690913935274, 0.12319024131387964, -0.391773973379546, 0.15564845413991735, - 1.0136432415733816, 0.15853338599107333, -0.06575739861582505, 0.45607910232144244, - -0.029452304232933196, -0.42611049362196574, -0.00631799217924843, 0.6616888742350064, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22278769147552951, 0.7562525618597952, 0.7244172125680113, 0.8013619756746, -0.035030343814626635, - -0.043554173077484964, -0.5955626692151448, 0.0697150046113055, -0.18725879412719573, - -0.0010000000000000044, -0.004371111641695805, -0.06542632787271348, -0.000999999999999986, - -0.11422022709298517, 0.013266577507715325, 0.000999999999999997, 0.4303498275395403, - 0.10070087447612802, 0.21730353862299512, 0.5447751598490892, -0.5061469246507112, - -0.0015787257451113062, 0.33657504204023714, -0.8611093244975886, -0.005598209889594105, - 0.11799479110706039, -0.3871662438809466, 0.18112675499252892, 1.016361066464779, - 0.18536718903155391, -0.0682608913461053, 0.4564530621599304, -0.027257453351117036, - -0.41566617752093377, -0.010368130199031786, 0.656805639934705, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22050357114012636, 0.7553158535625496, 0.724599423561518, 0.8031382199772927, - -0.03423827714648665, -0.04232219465209703, -0.5933005746074886, 0.04253425879484627, - -0.18780266924668632, -0.001000000000000002, -0.0016659515041230009, -0.06535064229582897, - -0.0010000000000000037, -0.11529055945736205, 0.0055778440621532446, 0.0010000000000000026, - 0.47295317565587114, 0.10101688249173162, 0.22152510166291575, 0.47851813745857824, - -0.48559499850107163, -0.004535022567882819, 0.3382929593819146, -0.8578842989942684, - -0.02599216967954026, 0.11237856839471595, -0.38388605428320655, 0.20914367878273488, - 1.0181118733767625, 0.2164064177335695, -0.07129397447476311, 0.45714262314021786, - -0.02486938234022909, -0.40585038576207405, -0.01508846559300347, 0.6513649746480739, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21810704781408236, 0.754493548083604, 0.724838408062433, 0.8048396702400198, - -0.03342270718598812, -0.04136828592843227, -0.5911046377517603, 0.015242287156212151, - -0.18807427834463983, -0.0009999999999999913, 0.000539297233641327, -0.06554857744515045, - -0.0009999999999999686, -0.11634814332520779, -0.0017688726677782315, 0.0010000000000000057, - 0.5155199845085554, 0.10101943367748842, 0.22630221632525763, 0.40929423120211167, - -0.46223609024894113, -0.006870264425152256, 0.3397097135303511, -0.8530046125495109, - -0.047052524783057145, 0.10654055906364134, -0.38092570429321654, 0.23873096406290895, - 1.0195968060715, 0.2511168592272732, -0.07446098071177092, 0.45713925896530555, - -0.02240936539949565, -0.3961427960838657, -0.019981620717025243, 0.6466731399659532, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2154356547620678, 0.7538979955746925, 0.7244519729078124, 0.8065553222451686, - -0.03256847303991653, -0.04066896517078174, -0.5888580830673711, -0.012882182224658634, - -0.18862778626568752, -0.0009999999999999974, 0.00475675317357388, -0.06602947775466625, - -0.0009999999999999766, -0.1148852623240404, -0.009429668400291991, 0.0009999999999999937, - 0.5527207443322003, 0.10046943515516699, 0.23189236064911928, 0.336969629320303, - -0.42879258207606574, -0.009137915097212073, 0.3414194038764207, -0.8470516293921283, - -0.07008625820059156, 0.10088455235714779, -0.37853205023955644, 0.2693957637423109, - 1.018894976327882, 0.2875864836603602, -0.0772299582153028, 0.45606960565634974, - -0.01978045893046047, -0.38791570175115686, -0.025406820840228914, 0.6397169671752031, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.212541438177162, 0.7533397728302735, 0.7239667179494967, 0.8082326848518612, - -0.031613296647935975, -0.040194613300089735, -0.5866386619324186, -0.04069296454076152, - -0.18912061204197655, -0.0010000000000000085, 0.008852614189682485, -0.06672154814844329, - -0.0010000000000000378, -0.11327497307745822, -0.01693659344474824, 0.0009999999999999916, - 0.5878422409589013, 0.09977238355604742, 0.23790507380653853, 0.26268916284086863, - -0.3923910358499027, -0.011369960566958973, 0.34284785363706144, -0.8386983868577413, - -0.09367625607993732, 0.09507723732364314, -0.3762670668989708, 0.3009730617146136, - 1.0175727575060198, 0.32696497015395715, -0.07987595179219976, 0.4534165039748098, - -0.01708276813763076, -0.37980824165450394, -0.030975236767365334, 0.6325898529561998, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2097282651130509, 0.7524588582790567, 0.7233890566848346, 0.8097508601854579, - -0.0301437291893595, -0.04035941922767917, -0.5846075754718502, -0.0679439654950396, - -0.1887499001248039, -0.0010000000000000015, 0.014315665160048096, -0.06893446248702632, - -0.0009999999999999788, -0.1067162512422672, -0.02355440373635497, 0.0010000000000000167, - 0.6161222431993908, 0.09832440450245428, 0.24471810213511955, 0.18814559499402853, - -0.3629146922084306, -0.013319962861785717, 0.34278713311313574, -0.8294328049335665, - -0.10787072787749144, 0.09037219176487218, -0.3741842131698403, 0.33360662075517766, - 1.0150093259860742, 0.3670485456410669, -0.0816103703145411, 0.44857115006359444, - -0.014791013147972773, -0.37385497542390705, -0.03632203551081663, 0.6216990666885136, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20675946759319483, 0.7514441903245694, 0.7228313041517831, 0.8112267134235377, - -0.028440797988302784, -0.04069353410147493, -0.5826202680307012, -0.09471011890239392, - -0.18829682997220695, -0.001, 0.020239352120330022, -0.07148946624658446, -0.00100000000000004, - -0.0990437568322926, -0.029916823283758567, 0.0009999999999999994, 0.6413173661719115, - 0.09675436088878564, 0.2516580178072121, 0.11308047375492708, -0.3347216026326625, - -0.015222708917241241, 0.3421511227766889, -0.8173554001514206, -0.119253918096931, - 0.08570966543037291, -0.37217927391977074, 0.36663331453772835, 1.0120807382614947, - 0.40930107619402994, -0.08307475372498585, 0.4416094206804373, -0.012604590625238768, - -0.3680609801471922, -0.04160346086181149, 0.6096531340284456, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20393324681228397, 0.7500080539810559, 0.7225008951280117, 0.8125362952046637, - -0.026376651277815473, -0.04137482577073724, -0.5808417728046851, -0.11962448048869614, - -0.18719871489012796, -0.0009999999999999942, 0.026801662284508893, -0.07483478393403453, - -0.0010000000000000367, -0.08895571970714376, -0.03520236094065341, 0.0010000000000000076, - 0.6565388338507142, 0.09489714959421343, 0.25851871758867223, 0.03885337467116902, - -0.3193672926844768, -0.01628882710120351, 0.3401950613374552, -0.8022709492569545, - -0.12287581801461987, 0.08263990318994446, -0.3710939191450372, 0.3987546192953982, - 1.009195903068245, 0.4535218753973448, -0.08431358195227247, 0.43284194157293326, - -0.010991685087825076, -0.3638228319059163, -0.04597167901011908, 0.5963006152485915, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2010133736711138, 0.7483398109487348, 0.7222837934933584, 0.8138271130556493, - -0.024083063828281792, -0.04203975291776578, -0.5790838413103614, -0.14356433856695627, - -0.18620598099474395, -0.00100000000000001, 0.03378198413469867, -0.07830813853615616, - -0.000999999999999952, -0.07829442119668255, -0.04028103094612015, 0.0009999999999999996, - 0.667182674872261, 0.09307921732731997, 0.2650139055775984, -0.03450434693191018, - -0.30695597330437446, -0.017259882185562393, 0.33777847144076956, -0.7838901225389164, - -0.12416318025872229, 0.07981623905752205, -0.3703327255619359, 0.42974631259688967, - 1.0063211251082, 0.4995793471220043, -0.08548517075761884, 0.4219066163292289, -0.009542818402711106, - -0.3595016924640007, -0.050064932345720604, 0.5819787862108319, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1981243551000431, 0.7463143747720052, 0.7220066441233959, 0.8148647558154655, - -0.021637289689313645, -0.04249402143992572, -0.577686347048782, -0.16432149349555236, - -0.18559450381213127, -0.0009999999999999966, 0.04145232567251445, -0.08120695561840505, - -0.0010000000000000046, -0.06659025516192675, -0.04436415184690947, 0.0009999999999999979, - 0.6636320265624305, 0.09118957283448077, 0.2711839614192521, -0.10566879057209982, - -0.2967464040230294, -0.01829764834263279, 0.3353134961565897, -0.7630664461428845, - -0.12339509775978137, 0.07784017573820973, -0.36989404728225306, 0.45953291527563556, - 1.002942497296333, 0.5446888988161819, -0.08748143100585766, 0.4112206886878014, - -0.008687294569380562, -0.3553634348060198, -0.0537347249325808, 0.5650372703628364, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19522156242219932, 0.7439481545878444, 0.7217724623504963, 0.8158675057775183, - -0.018986770729299815, -0.04265545261882044, -0.5763507854731592, -0.18350642535543538, - -0.1854048741991656, -0.000999999999999998, 0.049427778932165455, -0.08385929417702216, - -0.0009999999999999996, -0.05462121910390198, -0.048293918380106285, 0.0009999999999999929, - 0.6539223685612183, 0.08937750107830823, 0.27669455554325023, -0.17426534688997616, - -0.28650798424855267, -0.01970465198423915, 0.33282075040913695, -0.7397355266801146, - -0.12199299669803307, 0.07600200657055, -0.36955749490217504, 0.48730862669731967, - 0.9994058138921773, 0.589308084894655, -0.08973899033280992, 0.39917693510675023, - -0.008003777060161973, -0.3506927647491543, -0.05727322089557353, 0.5463184038278872, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19267761697203595, 0.741249874964728, 0.7219899637520287, 0.8168086217213082, - -0.016269146708139517, -0.042358189420566864, -0.5751215298840225, -0.1984395258325138, - -0.18557693016254803, -0.000999999999999994, 0.05641666197830122, -0.08618826026527159, - -0.0010000000000000263, -0.043140691144351054, -0.051782948150885535, 0.001000000000000015, - 0.6289457602245445, 0.08771605373576466, 0.28116627630816027, -0.23767539339982144, - -0.2729173994718643, -0.021172798513245373, 0.32997812578454894, -0.7141231255600882, - -0.11978049038445258, 0.07436064300286098, -0.3691291264647649, 0.5111620982840708, - 0.9961260159245604, 0.6320945229768196, -0.09144403792705885, 0.3855506462386366, - -0.008140141763020423, -0.3463353934163366, -0.06004170544662631, 0.5244588341862247, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19037231641784963, 0.7381704381733166, 0.7223943754106124, 0.817756053263405, - -0.013446267509304322, -0.04158415421075509, -0.5739032961742809, -0.2113390162117511, - -0.18614171913422464, -0.0009999999999999968, 0.06317022079036244, -0.0882333875365489, - -0.0010000000000000213, -0.031702661784005576, -0.05529128939999318, 0.000999999999999996, - 0.5974227016151316, 0.08619970859229943, 0.28467896789868136, -0.29717214353998395, - -0.25782002824477235, -0.02310966956706677, 0.32721957825485903, -0.687067135102258, - -0.1172960045176047, 0.07281440562460945, -0.36851519807976535, 0.5318599775416069, - 0.9929288272477471, 0.6731403414058358, -0.09297965777025102, 0.37050239459904216, - -0.008535825976235444, -0.3414636078862328, -0.06256413229718222, 0.5007463299155681, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1885054641516152, 0.734752465190229, 0.7232924187013601, 0.818464291230451, -0.010829820924682137, - -0.04002199225338328, -0.5730594725641006, -0.2193907954963541, -0.1876367192422314, - -0.000999999999999999, 0.06908358884735441, -0.08890490895207409, -0.000999999999999966, - -0.021564296705800966, -0.058274266204274824, 0.0010000000000000104, 0.5522870665059717, - 0.08555970675234546, 0.2868283292555147, -0.3523006393259187, -0.23735252717479605, - -0.025398743112161993, 0.32495794843673703, -0.6588988267578625, -0.11573291922733826, - 0.07124060541485283, -0.36768582982981546, 0.5494778363923419, 0.9906208599824045, - 0.7122345724820042, -0.09514653970486449, 0.3560189772161025, -0.009063564690757983, - -0.3355305482527551, -0.06453597820757286, 0.4770526249722175, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1870537708872009, 0.7309280443553208, 0.7243380336546807, 0.8190606206981923, - -0.008383937249393784, -0.03774620532183931, -0.5724025097791691, -0.22532307024770465, - -0.1896837079258613, -0.0010000000000000078, 0.07514570886353403, -0.08854787298148568, - -0.000999999999999998, -0.01170366930398148, -0.06120427621183329, 0.000999999999999979, - 0.5016210123641253, 0.0853890798275814, 0.28797610303634646, -0.4036030206822998, - -0.21516445760109304, -0.02836187582816502, 0.3234416190463267, -0.6303737071068389, - -0.11446226256358105, 0.06979544534150124, -0.3663086589934455, 0.5641718498528399, - 0.9886070091327354, 0.7488694954837837, -0.09752572029488511, 0.34152987688507924, - -0.009629626153229674, -0.32895150036515336, -0.06632560027569043, 0.4529377381530413, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1861467324971567, 0.7267928469730001, 0.725374984239749, 0.8193421832623434, - -0.006621218859173259, -0.03447660069289443, -0.5722288966771755, -0.22657848039558198, - -0.19258200980678206, -0.0009999999999999998, 0.08128485698362843, -0.08575306253289468, - -0.0009999999999999747, -0.004188653327886184, -0.06363281630089081, 0.0010000000000000115, - 0.44173627505493906, 0.0862413398394085, 0.28850764933089484, -0.44959502677316576, - -0.18818245544667686, -0.03240536045060181, 0.3239372680229505, -0.6020995360452565, - -0.11467047110024578, 0.06793801699783258, -0.36452139002517453, 0.5756601642169924, - 0.986397614663142, 0.781635838709164, -0.1004694912140857, 0.3281079785990008, -0.010596468806781887, - -0.3224832929179216, -0.06783911419846914, 0.4291127569337467, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18582306380191158, 0.7222508843444749, 0.7263117415099243, 0.8193762387590335, - -0.005498470874591226, -0.030322132474952027, -0.5724272132396505, -0.22642482356060561, - -0.19586659926042208, -0.0010000000000000009, 0.08864004780989358, -0.08090373833837111, - -0.000999999999999967, 0.002630191837308513, -0.06590054947753189, 0.001000000000000004, - 0.37970703379261045, 0.0877990531121429, 0.2885998635521891, -0.4915853746162226, - -0.16076035188542578, -0.03751497024286245, 0.3262014663717337, -0.5743699272321184, - -0.11536841756904705, 0.06612737206285954, -0.3616562381056834, 0.5843779826251962, - 0.9841215408012299, 0.810964526742104, -0.10361077032341229, 0.31553176417516304, - -0.011694228132897212, -0.31599882520154043, -0.06925937136227331, 0.40570107029025476, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18605934211785488, 0.7174809882150177, 0.7269859079278549, 0.8191591861925912, - -0.005544451819768423, -0.025124724651581942, -0.5729888610972239, -0.22392760488728397, - -0.19951639123476572, -0.0009999999999999983, 0.09755865551812988, -0.07283541495688245, - -0.0010000000000000165, 0.006572574337489959, -0.0681692205414648, 0.0010000000000000065, - 0.3172894408453942, 0.09041113652667641, 0.2880814552889751, -0.5281354065491561, - -0.1319344983751709, -0.04390212809609077, 0.33117601060914337, -0.5473455243548403, - -0.11813756690726461, 0.06393379856336971, -0.357749927754785, 0.5895761465232439, - 0.9821274957485148, 0.8369998259058058, -0.10636524312147261, 0.30426075628772736, - -0.01291026188478293, -0.3107330082542495, -0.07052925466617602, 0.38437957893846336, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18686889582438432, 0.7122978787082381, 0.7274668943231063, 0.8186492505786928, - -0.006479044335647802, -0.01919705816097615, -0.5739363200472741, -0.2218843207925688, - -0.20300197830099953, -0.0009999999999999998, 0.10842337545875859, -0.06240907759082421, - -0.0010000000000000254, 0.009586639475999753, -0.07027510368263855, 0.0010000000000000026, - 0.25887885130032245, 0.09385828894235382, 0.28757309954566346, -0.5607990324655304, - -0.1054877898324544, -0.05155919046870685, 0.3380802177593239, -0.5212424219949441, - -0.12163675006743487, 0.06177064991496314, -0.35250294005440863, 0.5923611744856541, - 0.9802323041287301, 0.8596187894868675, -0.10898794471929546, 0.294278020726181, - -0.014164286393386669, -0.306122777467879, -0.0717470011935157, 0.36452714731571567, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.18804307094751224, 0.7069980416575663, 0.7277354334339768, 0.8180848480988482, - -0.00813508949967873, -0.012964910925898644, -0.5748938273408407, -0.22045702783097718, - -0.20597485115182895, -0.0010000000000000009, 0.11993104728676064, -0.050613212848737446, - -0.0009999999999999747, 0.010416071632212451, -0.07230285852675522, 0.0010000000000000122, - 0.20982896580740196, 0.09793369441499472, 0.28692031745210805, -0.5890152866872411, - -0.08305017551666032, -0.061034571542792515, 0.3465097604164544, -0.4962769155428733, - -0.12564333580745135, 0.05887032973524409, -0.34668374436005256, 0.5927680839788251, - 0.9785891406192514, 0.8799747300665587, -0.11114979964930291, 0.28549627541932854, - -0.01542564425718686, -0.30263481555248556, -0.07396744687582926, 0.348322422630234, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1895183446601893, 0.7013983939386489, 0.7279295689681667, 0.8174039517695357, - -0.010158277252671204, -0.0066834275320113365, -0.575936559727917, -0.22027903129906456, - -0.20820819460666884, -0.0009999999999999987, 0.13197603262456406, -0.03822764818718601, - -0.0010000000000000115, 0.010509356306069506, -0.07407822882833029, 0.0009999999999999838, - 0.16786341420279138, 0.10248723837900027, 0.2865685806881514, -0.6143158360755838, - -0.06450333872355206, -0.07174650009853757, 0.3555301423938626, -0.47229236661810836, - -0.1297908008005929, 0.05582714747357814, -0.3404289445567023, 0.591887996949651, - 0.9770367359440871, 0.8978449202456665, -0.11315027388009041, 0.2777581593458715, - -0.016684758262181038, -0.29988967558827795, -0.07655146308557312, 0.33396114051430664, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19110158244007394, 0.6959949425413137, 0.728377122401916, 0.8168153871491345, - -0.011965860624801513, -0.0008607995248817425, -0.576774393086322, -0.22091219552247318, - -0.20993433113793533, -0.0009999999999999979, 0.14254581104468497, -0.027199293874838888, - -0.000999999999999994, 0.0102417399276458, -0.07555200390767652, 0.0010000000000000113, - 0.13650601394489068, 0.10704345640565174, 0.28593383692949115, -0.6368908165042947, - -0.05061625227490647, -0.08257514379933525, 0.36361110070090963, -0.44892798680135787, - -0.13363139756195017, 0.052659929497292896, -0.33537378477512764, 0.5911344214506739, - 0.9761845707032502, 0.9149830720406571, -0.11583374835434285, 0.2698896297412535, - -0.017305377435145403, -0.29683788944213796, -0.07913264520925285, 0.32118426489567575, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19273020624524986, 0.6906276766408885, 0.7289383028689217, 0.8161204172490534, - -0.013678974080275538, 0.004507128255778742, -0.5777023766717836, -0.22183473821961724, - -0.21097165796140221, 0.0009999999999999998, 0.15247454044839756, -0.016860133890280293, - -0.0010000000000000041, 0.010295000788989577, -0.07634137510346367, 0.001000000000000004, - 0.11059138906917243, 0.11155747833439823, 0.28507741178121776, -0.6568934020522076, - -0.03946451971448205, -0.09344842226998167, 0.3714631386789529, -0.42698118323907186, - -0.1373348981250351, 0.049478972660510614, -0.3324154204258573, 0.5891247913616844, - 0.9755926229965247, 0.93087927370236, -0.11877177700693828, 0.2623524830708099, - -0.0176808722514485, -0.2939702075205401, -0.08171290365380174, 0.30927376735827705, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19421006020173046, 0.6856754272286727, 0.729520686477116, 0.8159282080146236, - -0.01473836467907394, 0.009294992827191201, -0.5778905978478746, -0.2234277585119569, - -0.21185146039058017, 0.001, 0.1598113909064985, -0.009163408937914614, -0.000999999999999996, - 0.009595248333462097, -0.07770731366544041, 0.0010000000000000044, 0.09236802940521474, - 0.11545104650546807, 0.2843277897061831, -0.6766201947517424, -0.03254533670082933, - -0.10411323285894712, 0.3777097454505977, -0.40443565861094083, -0.1396005631047366, - 0.04632616190520821, -0.3293669933555331, 0.5875264837125083, 0.974647302735829, - 0.9484556412280154, -0.12188743359129613, 0.2539611130077046, -0.01770060949003049, - -0.290945893933464, -0.08453305751902218, 0.29966710636033694, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.19565415895949112, 0.6808634435011621, 0.730119598099153, 0.8158845499379047, - -0.015513295036856013, 0.01375453154861463, -0.5778430164946199, -0.22520647405704358, - -0.2124834791615246, 0.0009999999999999994, 0.16604196084906267, -0.0026291401175501886, - -0.0009999999999999948, 0.008783357686154982, -0.07908343451031788, 0.0009999999999999907, - 0.07747221579976597, 0.11906744166847712, 0.28353940109081505, -0.6955942835047012, - -0.02754507581012906, -0.11470275031777898, 0.38335448458557986, -0.38217187624355753, - -0.14127245168304295, 0.043224533261960074, -0.3269608217192516, 0.5854024821044959, - 0.9735665340730661, 0.966205661786917, -0.12507144887725746, 0.245395741978384, - -0.017576776513482387, -0.2879734892294703, -0.08744380901368339, 0.29094801524297054, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.196894665339866, 0.676588046560249, 0.7308083136899096, 0.816239723296375, -0.015479180463105452, - 0.017526531364716918, -0.5772399239342082, -0.22727878211434857, -0.2131899129790484, - 0.0009999999999999987, 0.16948887555747144, 0.0010166290384843615, -0.0009999999999999979, - 0.007939135959935797, -0.08067012792323554, 0.001000000000000005, 0.0680019209828164, - 0.12176738780463639, 0.28201073271168464, -0.7132799681798964, -0.02767939775957887, - -0.12408295799815762, 0.3875201112690527, -0.36097295470234564, -0.1407483636212701, - 0.04055307432626964, -0.3259617782729954, 0.5835643113653933, 0.9725963714632893, - 0.9854776865701667, -0.12825112515826045, 0.23643835682249942, -0.01750872657779171, - -0.2854443251762341, -0.09039024265451859, 0.28370811138756125, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.1980897643151346, 0.6725016339900778, 0.7315339187682273, 0.8167545042558273, - -0.015088330393552531, 0.021039658248959925, -0.5764041592880108, -0.2294774631008535, - -0.21392191588110887, 0.0009999999999999972, 0.17175246762002805, 0.0034307860998629533, - -0.0009999999999999933, 0.007085417196629938, -0.08233997526102543, 0.001000000000000002, - 0.06066583672496351, 0.12408216444225487, 0.2801358376650019, -0.7303593418067946, - -0.029876002708383902, -0.13301586860321563, 0.3910619430788804, -0.340258773609289, - -0.13930840065546302, 0.038095043383495335, -0.32555781520531124, 0.5815842350363146, - 0.9716695351152015, 1.0051592154032487, -0.1314299971291595, 0.22737148899924237, - -0.017454864987419893, -0.28309165625051724, -0.09335020443402796, 0.27692542116466784, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.199128065353043, 0.6688130111756883, 0.7323198227610588, 0.8175225891475426, - -0.014321327813366081, 0.0242831197576124, -0.5752060899347264, -0.2317758650991071, - -0.2149623613693619, 0.0009999999999999979, 0.17269658800891677, 0.004424960364538599, - -0.000999999999999996, 0.005633073912707906, -0.08477764763060995, 0.0009999999999999844, - 0.05579918041841443, 0.1258225277912281, 0.27779609833200625, -0.7451336815411709, - -0.033544475828539276, -0.14059246809083284, 0.39406591474386415, -0.3207035993972867, - -0.13803792243906177, 0.03617672531922255, -0.3253913423681294, 0.5794017324439795, - 0.9708139408619635, 1.0250317781417446, -0.1343869396020747, 0.2174580280279015, - -0.0173128554452656, -0.28117187861987974, -0.09557133053707428, 0.2703863924520662, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20012902492780815, 0.6652718259341169, 0.7331284811604325, 0.8183889168770003, - -0.013377307542490412, 0.027433575641099148, -0.5738536636663985, -0.23413928884121735, - -0.2161377620260172, 0.000999999999999999, 0.1730565432751901, 0.004789856292048345, - -0.000999999999999999, 0.003937000681600556, -0.08752641736993767, 0.001000000000000009, - 0.05194076424229541, 0.12731706997316666, 0.2752252445358139, -0.7589342987719223, - -0.03783972881284138, -0.14763924369798445, 0.39683152077244527, -0.3016201081047165, - -0.13679742702120573, 0.03451451515368684, -0.325346107597325, 0.57693533896509, - 0.9699871890497006, 1.0448945665518368, -0.13725398839089095, 0.20722545092909178, - -0.017131962362106446, -0.27939904827180084, -0.09749283456913928, 0.2638216973892375, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20115567565232856, 0.6620779676975875, 0.7336398899650161, 0.8193049224381386, - -0.012245323035684406, 0.030374014013127507, -0.5724219732025633, -0.2364894110596305, - -0.21776206250592617, 0.0010000000000000007, 0.17257428615746018, 0.004203084010232926, - -0.0010000000000000037, 0.0020366938620283675, -0.09031784143214915, 0.00099999999999999, - 0.0492480099161495, 0.12895219048962425, 0.2732428886238543, -0.7711201850915844, - -0.04314307316864984, -0.15464311620342883, 0.3999024123240485, -0.28380237405086434, - -0.1353802581861263, 0.03308664641038696, -0.32559201175856567, 0.5744564742186987, - 0.9681108190197362, 1.065135328769777, -0.14055336877805638, 0.1968571749812777, - -0.01728982466771248, -0.2784471445582962, -0.0992463936723736, 0.25793657975694095, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2022156370501735, 0.6590154915142911, 0.7340271741009307, 0.8202371364967327, - -0.0110137300642935, 0.033241123692705356, -0.5709507556325302, -0.2388570266422415, - -0.21959369391541805, 0.0009999999999999987, 0.17169620396286897, 0.0031715173242534526, - -0.0010000000000000005, 5.2106748471089495e-05, -0.09312484354719802, 0.0009999999999999777, - 0.047051340083520105, 0.13066083678432547, 0.2714720245876891, -0.7826360408086533, - -0.04889523357135763, -0.16165964323711227, 0.4030748880987191, -0.26649563937144544, - -0.1338562311214491, 0.03178658433199619, -0.3259720148032975, 0.5718091071463037, - 0.965797836267349, 1.0854736573752666, -0.14403468174555373, 0.18643122973506332, - -0.017583715296298718, -0.2777814281675986, -0.1009303311189785, 0.25221156785773763, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20339622667539184, 0.6561816531383926, 0.7344874972192025, 0.8212076296667171, - -0.009747559171642474, 0.03606530490267953, -0.5694052228858134, -0.24140453763753364, - -0.221822626932703, 0.000999999999999995, 0.17035884901371567, 0.0016678159143596867, - -0.0009999999999999998, -0.002319097904702145, -0.09617182220844403, 0.001000000000000016, - 0.04549468657540182, 0.132870917608923, 0.26960689631331575, -0.7922648557613204, - -0.05413635561694333, -0.16798577200979414, 0.40637371950236617, -0.25041900707952125, - -0.1325147334082177, 0.0305513171715367, -0.3257257515394712, 0.5693750311560444, - 0.9639233318048086, 1.1053324142348333, -0.1468871373100105, 0.17578716330525906, - -0.01816114722344563, -0.27651991533702613, -0.10258765646939037, 0.2464235717642684, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20465011086074275, 0.6534330049542678, 0.734977768470957, 0.8221894243023221, - -0.00844511298333823, 0.03888373900176646, -0.567821526074308, -0.24404897953430385, - -0.2242482635184041, 0.001000000000000003, 0.16877825796390916, -8.976140974796639e-05, - -0.0010000000000000002, -0.0048530778663374245, -0.09931960014899996, 0.0010000000000000074, - 0.044222789374956564, 0.1353298349914665, 0.2676679320413626, -0.8011202251626903, - -0.059187743462086224, -0.17405753336542473, 0.4097049342130213, -0.23483605706126678, - -0.13122091146421302, 0.02937489632250686, -0.3252293010818088, 0.5669034241568034, - 0.9622363185396604, 1.1249742112446486, -0.14947256411874812, 0.16506282409009665, - -0.018857455661352863, -0.2749503240109235, -0.10423412679871003, 0.24048824595032173, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20597733055129003, 0.6508908663764694, 0.7354357509515472, 0.8232719512531834, - -0.007120495586127183, 0.0416011716157383, -0.5660759095232567, -0.24695636758367548, - -0.22628277026847968, 0.0009999999999999942, 0.16664733580166236, -0.002322449576654922, - 0.0009999999999999994, -0.007925173305445874, -0.10283961040187037, 0.0009999999999999996, - 0.04327068903012268, 0.13699662339180183, 0.26612923124690513, -0.808912391496871, - -0.06378384235748712, -0.1790856464173889, 0.41162067000458574, -0.22073003378569603, - -0.1298240411906247, 0.0286291114053343, -0.3251542216029229, 0.5651571293551931, - 0.9600872460454237, 1.1445071168871246, -0.15192641881635569, 0.15474849413695393, - -0.019593657045083923, -0.2740582337344399, -0.10578937042185338, 0.23452910726301568, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20732716611858784, 0.6482992817794997, 0.7358791614289208, 0.8243113508045797, - -0.005654987780662357, 0.044276041258760286, -0.5643743883437504, -0.24974569950702813, - -0.22874397817316164, 0.0009999999999999929, 0.1642599784716708, -0.004855343997034559, - 0.001000000000000003, -0.010846496036903042, -0.10619214662091125, 0.0009999999999999816, - 0.04224662493597054, 0.1395241875928055, 0.26483452121363876, -0.8154406499469317, - -0.06826078353787471, -0.18445368501307915, 0.4153911129579128, -0.20647012170448742, - -0.12845264522779362, 0.0280209600119484, -0.324267797385487, 0.562059639441782, - 0.9577353543873721, 1.1634720388892483, -0.15432498676554687, 0.1444059212870006, - -0.02034469799830071, -0.2731052585916348, -0.10730457060948793, 0.228482576914979, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20864455577533955, 0.6458134357304238, 0.7361066105188985, 0.8252865641595821, - -0.004109093785678647, 0.04694502248719448, -0.5627444955124912, -0.25283793832102197, - -0.23169807118111774, 0.000999999999999995, 0.1618207909218283, -0.007482197899053049, - 0.0010000000000000007, -0.013542156436107452, -0.10937186075279151, 0.0010000000000000048, - 0.04132651309735529, 0.14250187578115045, 0.2637820203254087, -0.8213041993167647, - -0.07256068322228129, -0.18977097931308642, 0.41971122265642735, -0.19338418585659195, - -0.12723410145558486, 0.02748420461349449, -0.3232302868814103, 0.559306221346326, - 0.9551156853052735, 1.1807468973982367, -0.15680953664728137, 0.134888934195131, - -0.021320753765339637, -0.2716835596085079, -0.10886722847844732, 0.22279376470793272, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.20996655706294506, 0.6433689414389803, 0.7362338084734358, 0.8262279673710099, - -0.002506474531711458, 0.049620952361274735, -0.5611406460114526, -0.2560890920779307, - -0.23490956766874288, 0.0010000000000000046, 0.15930744930072985, -0.010208492369215597, - 0.0010000000000000009, -0.016133120572823262, -0.11247076152243023, 0.0010000000000000096, - 0.04046232776483788, 0.14572501761527082, 0.2628150474358456, -0.8268927635366081, - -0.0767999787091973, -0.19510007618512826, 0.4242540721007888, -0.18080743045889125, - -0.12605647549058033, 0.027009679897280668, -0.32214233142390636, 0.5566298185549726, - 0.9523753553475525, 1.1972467905180033, -0.15933634612236638, 0.12575724800618146, - -0.02239485655279727, -0.27001333629096574, -0.11045194919263988, 0.21718534280787832, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21125528396248527, 0.6410064290146547, 0.7364013116217001, 0.8272230870215482, - -0.0009292009292025535, 0.05222604297234434, -0.5594403822746647, -0.2594325755649186, - -0.23811735213351445, 0.0009999999999999994, 0.15667485618125204, -0.013064312204933983, - 0.001000000000000006, -0.019013995944725387, -0.11607743362969337, 0.0010000000000000154, - 0.03959658038169399, 0.14919949324395768, 0.2619937667262847, -0.8313863585929929, - -0.07993553156601112, -0.1998872847647498, 0.42897335247588414, -0.16882810172670576, - -0.12540047992037187, 0.027059778212938444, -0.3210174529147508, 0.553169891528173, - 0.9497706067736232, 1.2132643096085334, -0.16113185044225306, 0.11652337096998312, - -0.023433537521428124, -0.2690783261074362, -0.11178083203936734, 0.2102073958970532, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2125467238631639, 0.6386779939567461, 0.7365846212213132, 0.828237910137404, - 0.000655284604575382, 0.05480703737076679, -0.557689630052371, -0.26283798972823974, - -0.24135171613220777, 0.0010000000000000104, 0.15393888307523865, -0.01603686877072039, - 0.0010000000000000018, -0.02202455824199721, -0.1199173102251217, 0.0009999999999999955, - 0.03873806867698209, 0.15282880731433096, 0.2612166965930834, -0.8353924514861683, - -0.0825762021111135, -0.2044541333357091, 0.4337545695039057, -0.1571265617372522, - -0.12495831035312255, 0.02738291899245031, -0.3198917995710695, 0.5492884878384128, - 0.9472282762516486, 1.2290223571889232, -0.1625919359175677, 0.1072562034507656, - -0.024454556610726253, -0.26841147376883834, -0.11299216006992212, 0.20253249639918605, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.213836605219866, 0.6365454222790132, 0.736393811613909, 0.8292336043533765, 0.0020967234346805814, - 0.05723513187984261, -0.5559598662139599, -0.2660931811775194, -0.24430238078887653, - 0.000999999999999999, 0.1512625716563769, -0.019003966003871272, 0.0009999999999999968, - -0.02487346085240238, -0.1237436468524072, 0.0009999999999999825, 0.03776731689593597, - 0.15617290677636014, 0.26137412448353886, -0.8379615921270789, -0.08511023381925394, - -0.2090167615209641, 0.43903029947504635, -0.14748560248088005, -0.12457627361778484, - 0.02763751513328394, -0.3194037216732343, 0.5451657326286543, 0.9439611911273673, - 1.2420049369223092, -0.16415047774911512, 0.09980384851065735, -0.02549437503253537, - -0.2682325669496915, -0.11445897928460831, 0.19482707046858222, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2151365802133613, 0.6345026874463879, 0.7360244785326101, 0.8302160735045164, - 0.0034821527542607607, 0.05959479198019078, -0.5542360568167479, -0.2692869692772911, - -0.2471325541062957, 0.0010000000000000037, 0.14858464782013361, -0.022004523249579803, - 0.000999999999999998, -0.027642636967007424, -0.12756229654244347, 0.0010000000000000193, - 0.03675012369491179, 0.1594045944868015, 0.2619687504249426, -0.8398690191256204, - -0.08760482680491015, -0.21359679805764373, 0.4445168571910282, -0.1388237717892958, - -0.12420585514352525, 0.02787860027092194, -0.3192180986530777, 0.5409130734135136, - 0.940349419134713, 1.253642032421489, -0.1657601856311687, 0.09321260734586748, - -0.026540103403696506, -0.2682367873621528, -0.11604845804196151, 0.18708752988393942, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.2163953063566126, 0.6326170222138046, 0.735694958059418, 0.8311120527203728, - 0.0048564524291806865, 0.061866254389194755, -0.5526316469951602, -0.2725278618912268, - -0.25010281008723334, 0.0010000000000000189, 0.14592153807520558, -0.024986845328854753, - 0.0009999999999999987, -0.030027559955915033, -0.13104579355843324, 0.0010000000000000028, - 0.035910428280554914, 0.16264628826276525, 0.2627789683730397, -0.8407919180677144, - -0.08957436271188775, -0.21738917558082207, 0.4496722680355233, -0.13176617437627874, - -0.12401900811676131, 0.02812655154284081, -0.31942416948053437, 0.5374888961649368, - 0.9371509335735311, 1.26367616153349, -0.1668600572061329, 0.08824143315484495, - -0.02751255735859917, -0.26849605946557925, -0.11760358460939609, 0.1792398448299603, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21764195248799892, 0.6308076571422152, 0.7353828658826912, 0.8319618759955361, - 0.006234857571294122, 0.06409944174004338, -0.5510824121759553, -0.2758044863113748, - -0.253160832099018, 0.0010000000000000148, 0.14324217127720978, -0.027989806522362458, - 0.0010000000000000018, -0.032222576991935016, -0.1343634658154229, 0.0009999999999999933, - 0.035158964090672616, 0.1659166008354749, 0.2636842697576748, -0.8412456259622517, - -0.09129129507080548, -0.22081222786712623, 0.45465122891627224, -0.12548553495384374, - -0.12390947386472222, 0.02839103072415236, -0.319824077375581, 0.534456481772678, - 0.9341533118040762, 1.2729144379615136, -0.16771445971225615, 0.0840623003210226, - -0.0284479801991491, -0.26884991200522307, -0.11914335370089367, 0.17132567146903466, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.21887839559493294, 0.6291518057055644, 0.7352487788919464, 0.8327313550606041, - 0.00759312099261215, 0.06630200538993587, -0.5496406816219087, -0.2790228279816035, - -0.2563816208800374, 0.0009999999999999961, 0.14050705830120677, -0.031004516607638387, - 0.0009999999999999994, -0.0342855920013789, -0.13743230343633653, 0.0010000000000000124, - 0.03426065348126216, 0.1691602077623771, 0.26489978909918965, -0.8406464609073511, - -0.09235595369811818, -0.22357783723162017, 0.4589360171877148, -0.12066893775151659, - -0.12355825361496603, 0.02861025589159157, -0.320159105195846, 0.5318486220587599, - 0.9317109997153404, 1.2807998111185452, -0.1679866535744723, 0.07992770147465408, - -0.02934556607359464, -0.26907210924468883, -0.12034000123908355, 0.1632088150860557, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [0.22011748706515896, 0.6275703970407572, 0.7352027666863867, 0.8334574192300506, - 0.008950594952701755, 0.06849352973950515, -0.5482492622559548, -0.28222453644863066, - -0.25970215993986634, 0.0009999999999999948, 0.13772068300627832, -0.03405303639618982, - 0.0010000000000000052, -0.03628020580460698, -0.140374789575737, 0.0009999999999999944, - 0.0332926700154549, 0.1724119938509405, 0.2662562753503555, -0.8395356534015066, - -0.0930998872331441, -0.22602990796339834, 0.46285856315840695, -0.11657478269043448, - -0.12307436045460478, 0.028818416523663872, -0.32046093040893797, 0.5294488454372008, - 0.9295461093496074, 1.2879992154938789, -0.16796861866620316, 0.07581148511418766, - -0.030224849658937337, -0.2691792783869285, -0.12136535515376601, 0.1549712719589458, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.44317999482154846, 0.3182365596294403, 0.0366680808365345, - 0.23186944425106049, 0.20954547822475433, -0.1814018040895462, -0.13917584717273712] -- [0.22106063160056594, 0.6260471392775484, 0.7351228198776739, 0.8341158523651925, - 0.01035537218937828, 0.07067412516175828, -0.5469448593163, -0.2856520579616282, - -0.2631646036471439, 0.0010000000000000005, 0.13518497685393877, -0.03686069863365323, - 0.0009999999999999966, -0.038148193009237705, -0.14329616194745537, 0.000999999999999988, - 0.032361939881080805, 0.17525310337877306, 0.2681091877356367, -0.8379655854486094, - -0.09381942874417502, -0.22835088234062964, 0.4669555955050452, -0.11315119602981939, - -0.12270553499126471, 0.029150410096909416, -0.32071222165280633, 0.527911230672461, - 0.9269389523665598, 1.29455488622702, -0.16809127684055197, 0.07265927722934122, - -0.03095657846526154, -0.2693254345141402, -0.12219278981438504, 0.14619112524218056, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015641260892152786, 0.45374879240989685, 0.7020354270935059, - 0.027047906070947647, -0.045656319707632065, -0.20394200086593628, -0.24307167530059814] -- [0.22185832711684803, 0.6245524549985269, 0.7350237541504263, 0.8347359531178558, - 0.011792071882578527, 0.07285541742058453, -0.5456820720580917, -0.2892086906223337, - -0.26671507420444396, 0.0009999999999999883, 0.1327613165105301, -0.03956302897209926, - 0.0010000000000000063, -0.0399501235816861, -0.1462059888669721, 0.0009999999999999966, - 0.031452056763915964, 0.17789130425457325, 0.2702041139516095, -0.8361776718892526, - -0.0945305176897876, -0.23061076309731626, 0.4711311449026522, -0.11005374449154678, - -0.12238705231562273, 0.02955549138393998, -0.32093752800486025, 0.5268067591942526, - 0.9241045964173762, 1.3007807268724902, -0.1682874632153916, 0.06999615008284635, - -0.03161198960498227, -0.26945294511423373, -0.12291979067999807, 0.13711198754474802, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2969687581062317, 0.2321910262107849, 0.0799890086054802, - 0.011026418767869473, 0.03245752677321434, -0.2803012430667877, -0.21059319376945496] -- [0.22271719363993855, 0.6230137590475967, 0.7350650674770429, 0.8353112130624184, - 0.013272798446683469, 0.07516187699313868, -0.544453581492905, -0.2928752186650082, - -0.2704635780918621, 0.0009999999999999994, 0.1302146495118795, -0.042393028788175954, - 0.000999999999999995, -0.041723446448639304, -0.14919847562393096, 0.0010000000000000289, - 0.030564189876358738, 0.18058516298033336, 0.2719658105804438, -0.8338422775977737, - -0.09460218591287126, -0.2328002024211337, 0.47487182245420584, -0.10765879981872124, - -0.12216248201678032, 0.03013597747031939, -0.32043328718117825, 0.5262131804473724, - 0.9220933505126305, 1.3069567954330634, -0.1679684586089834, 0.06707288971311759, - -0.032268488076468586, -0.2688630859565845, -0.12344982656486359, 0.1276942060082415, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5068114995956421, 0.0870138481259346, -0.11762022972106934, - 0.00906062126159668, 0.029213707894086838, -0.3155962824821472, -0.1444578319787979] -- [0.22361878256731, 0.6214486333560582, 0.7351767970591019, 0.8358588809957646, 0.014788703605026389, - 0.07754250938850654, -0.5432387914572416, -0.29661892079255203, -0.27434957907549623, - 0.0009999999999999768, 0.12757551935460676, -0.04532387127037273, 0.0009999999999999918, - -0.043478909841147155, -0.15223144651524675, 0.0010000000000000024, 0.02969277622612932, - 0.18334663173410135, 0.273527749039284, -0.8312358070360237, -0.0943472238433282, - -0.23497812426143855, 0.47837333833143747, -0.10560941328903989, -0.121973419582127, - 0.030826583999258982, -0.31954096573669016, 0.5258843023141372, 0.9205149417985036, - 1.313109761228276, -0.16737833330656715, 0.06402046135737617, -0.03292593558078803, - -0.26784748600518776, -0.12387631649111332, 0.11806459369323227, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.11406882107257843, 0.05570409446954727, -0.06862004101276398, - 0.052874136716127396, 0.05513794347643852, -0.347802996635437, -0.17412054538726807] -- [0.2243656377087936, 0.6198610109317527, 0.7350414211091505, 0.8363796185310339, - 0.016300668656721532, 0.07981823501191802, -0.5420631616950272, -0.30013962736593197, - -0.2779638062766914, 0.0010000000000000044, 0.12491582068609634, -0.04826791478548613, - 0.0010000000000000115, -0.04513341521671362, -0.15509726756632744, 0.0009999999999999803, - 0.028776860213912344, 0.18578914187646048, 0.27529714642963726, -0.8297822654979237, - -0.09434022701732993, -0.23732536781105934, 0.48193808004602734, -0.10327672274603303, - -0.12155995340368039, 0.031420452105570845, -0.31837404918065276, 0.5251672360689295, - 0.9185553619247875, 1.3206002652185473, -0.16666098615702074, 0.061425420527571495, - -0.033583058733187496, -0.2651853840866323, -0.12452449743301218, 0.1092201379063625, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4270801842212677, 0.032583773136138916, -0.08298105001449585, - 0.19581831991672516, 0.01512994710355997, -0.17289260029792786, -0.17895960807800293] -- [0.2250406722536846, 0.6182576590754647, 0.7347702981620532, 0.8368831482393543, - 0.017822735564357733, 0.08204684170634263, -0.5409041153993984, -0.3035604209389176, - -0.2814561373078444, 0.0010000000000000115, 0.12221602184658356, -0.051250465870243136, - 0.0009999999999999994, -0.046732763206860754, -0.15787337139848093, 0.0010000000000000096, - 0.027840769697340145, 0.18807588006477316, 0.27715598005114966, -0.8289678059762869, - -0.09447624958659855, -0.23977820380061476, 0.4855268669287313, -0.10077233522788916, - -0.121009627881038, 0.03198089012539352, -0.31705980992932126, 0.524214909031536, - 0.9163900798419128, 1.3287983348788106, -0.16587623472744498, 0.05910514085533354, - -0.03424024883043768, -0.26159489279151593, -0.12529270552029137, 0.10078195167801292, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.22153756022453308, 0.4472527503967285, -0.04789876937866211, - 0.006490321829915047, 0.29276320338249207, -0.29508787393569946, -0.29259562492370605] -- [0.22536729449310092, 0.6168693831953493, 0.7344432482147848, 0.8372047177116207, - 0.019189126304286562, 0.0839541940220447, -0.5400664138596225, -0.30658760650458067, - -0.2846956032923626, 0.001000000000000013, 0.11988166574727285, -0.05382358090991564, - 0.0010000000000000143, -0.04791723459353995, -0.1600493357556147, 0.001000000000000009, - 0.02722225498530091, 0.19007018282224789, 0.27890911138951063, -0.827225155203914, - -0.09418920078728482, -0.24107848297402856, 0.48849857495726334, -0.09902348785165047, - -0.12035390593832311, 0.032492380388219874, -0.31599580366281105, 0.5236689839282945, - 0.9147937786092388, 1.3339949423990363, -0.1649414416563628, 0.05743987977475655, - -0.03475427175237663, -0.26004030762270786, -0.1255698621505921, 0.09335192133876054, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4949386715888977, 0.49358177185058594, -0.10180144011974335, - 0.3329377770423889, 0.5584545731544495, -0.12355132400989532, 0.021222764626145363] -- [0.22549985775473266, 0.6156009966490632, 0.7340849715862455, 0.8374247348322026, - 0.02046922629603475, 0.08568368767230049, -0.5394062753926137, -0.3093991912948266, - -0.2878013689509354, 0.001000000000000003, 0.11775065683202135, -0.05616937541823016, - 0.0010000000000000018, -0.04887044675089735, -0.1618909573652199, 0.0010000000000000143, - 0.02677855599934968, 0.19190879282371984, 0.28060475512053384, -0.8249673669243907, - -0.09366529397581097, -0.2417420168299475, 0.4911293040416467, -0.0976991625304856, - -0.11964064162044989, 0.03297444571182779, -0.3150684666547067, 0.5233559882984611, - 0.9135119452216546, 1.3375314020747497, -0.16392312006994714, 0.05612259218938545, - -0.03518903318834209, -0.259607467928535, -0.12557544731574713, 0.08648068684805005, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13531799614429474, 0.12883351743221283, 0.020396830514073372, - 0.035042665898799896, 0.0, -0.1429031491279602, 0.0] -- [0.2253179983303412, 0.6144910896330793, 0.7333740737811543, 0.837527985155037, - 0.02145513319748245, 0.08706629554054313, -0.5389860958526378, -0.3115260741362204, - -0.29008313947043396, 0.00100000000000001, 0.1162301648852215, -0.05790330453022671, - 0.0009999999999999994, -0.04943320888827879, -0.16342508316323992, 0.0010000000000000035, - 0.026374419197419084, 0.1929891784877655, 0.2821997139704455, -0.8233376066447577, - -0.0931693128255858, -0.24227363672247734, 0.493702585153991, -0.09630940845484337, - -0.11913152060017375, 0.033287111480249, -0.31468009471629543, 0.5232388891422028, - 0.9123103440725471, 1.339292495873405, -0.1633002929866277, 0.05607304385342981, - -0.035331126660431124, -0.25981924135239104, -0.1256312597541463, 0.0812167921611829, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11978581547737122, 0.4101407825946808, -0.04309163987636566, - 0.02792394533753395, 0.0333845429122448, -0.211534321308136, -0.4446060061454773] -- [0.22495685582975475, 0.6134710727368374, 0.7324641512565201, 0.8375653870578685, - 0.02227264301020346, 0.0882515558699999, -0.5387019720239268, -0.3132625731201223, - -0.29189217151224117, 0.0010000000000000052, 0.11505916636027368, -0.059284807781489604, - 0.0010000000000000057, -0.049775314884246025, -0.16478676779583887, 0.000999999999999982, - 0.025992487163601502, 0.19363163204866696, 0.283742058384475, -0.8220643548307157, - -0.0926887884031834, -0.2427260752459145, 0.49624357807096847, -0.09488344827344396, - -0.1187396499891534, 0.0335018274303939, -0.31459563848455285, 0.523234810131217, - 0.9111541728023319, 1.340047781430585, -0.1629014772182803, 0.056733465507163724, - -0.035307417284090606, -0.2604013337301925, -0.1257157456118184, 0.07686784765307486, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.14860768616199493, 0.11546307802200317, -0.4126807749271393, - 0.0472538061439991, 0.0, -0.47667229175567627, -0.08503668755292892] -- [0.2244912858114404, 0.6126057063253981, 0.7313821546097073, 0.8374780256300591, - 0.022897565406184484, 0.08917104726358084, -0.5386601734072821, -0.31448404277604164, - -0.293211947842108, 0.0010000000000000044, 0.11420850983901742, -0.06034545183303246, - 0.0009999999999999894, -0.04968017253302825, -0.1657812906158506, 0.0009999999999999968, - 0.02567302129781942, 0.1938173721613923, 0.28526049368302026, -0.8210032371802461, - -0.09201535080617035, -0.24269381208696864, 0.4982172708040792, -0.0935191746079405, - -0.11838877579797452, 0.033755782401044, -0.3144372899221966, 0.5230454469103497, - 0.9101400602736852, 1.3391874431845034, -0.1624596541349819, 0.058062859817749043, - -0.03525213909876698, -0.26158960191544384, -0.1257960942187072, 0.07400732026627434, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.38155078887939453, 0.5811251401901245, -0.015938900411128998, - 0.4061576724052429, 0.7378761172294617, -0.1826898157596588, -0.16060273349285126] -- [0.22396473540551637, 0.6118299840628398, 0.730200831738107, 0.8373188716711345, - 0.023410670121296266, 0.08993580066691922, -0.538758386873234, -0.315405195917091, - -0.29424398862178475, 0.0010000000000000009, 0.11354314883872334, -0.061219020184934005, - 0.0009999999999999963, -0.04933118499297571, -0.16656357522254153, 0.0009999999999999998, - 0.025389491121125516, 0.19373249923518662, 0.2867646644946486, -0.8200650965874766, - -0.09122978935497005, -0.24237762010569258, 0.499862274996437, -0.09219006948178077, - -0.11806168494952325, 0.03403200213608812, -0.3142362660284127, 0.5227514080862141, - 0.9092084159835921, 1.3373890805864843, -0.16199268046829227, 0.059772079564224645, - -0.03517853107795456, -0.26312677455291333, -0.12587385694646544, 0.0720091638635141, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4149182140827179, 0.4638213515281677, -0.009657265618443489, - 0.63747239112854, 0.5448353886604309, -0.11947324126958847, -0.254296213388443] -- [0.2234533566065829, 0.6110844437217692, 0.7291056272803496, 0.8370827629631136, - 0.02387597552904159, 0.09055562457612108, -0.5390009875688587, -0.3160535814725547, - -0.29511548078776617, 0.0010000000000000182, 0.11299622347202364, -0.061946142387794445, - 0.001000000000000001, -0.048790672787239295, -0.16704110121012275, 0.0010000000000000198, - 0.0253278277383512, 0.19367881186647773, 0.2877535982182335, -0.819731894517773, - -0.0906702570648928, -0.2418390283597319, 0.5009554857350058, -0.09063729895156698, - -0.11751755855755276, 0.034107727823209574, -0.31395920127483057, 0.5236545088966386, - 0.9086177443292882, 1.334723350393099, -0.16149454090933887, 0.06141364151977086, - -0.03505568344471377, -0.2642138512758949, -0.12579074587726133, 0.07107145136713447, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.48455721139907837, 0.8098799586296082, 0.12209618836641312, - 0.580166757106781, 0.6528713703155518, -0.14287050068378448, -0.06669650971889496] -- [0.2229518820572289, 0.6103566156865199, 0.7280616930104069, 0.8368012236605048, - 0.02431389815547912, 0.09109004598739791, -0.5393284249497531, -0.3165403534947822, - -0.29589088800789165, 0.0009999999999999953, 0.11251735860258048, -0.06258897423863913, - 0.001000000000000006, -0.04813667012219482, -0.1673382937377852, 0.0009999999999999725, - 0.02539830318947174, 0.19364234403703218, 0.28843571661458295, -0.819756103219788, - -0.09024610536170523, -0.24116818207095367, 0.5017200219567902, -0.0889508673289562, - -0.11684296162646494, 0.034065848346211736, -0.3136375714297159, 0.5252727244373497, - 0.9082297496056604, 1.3315453887027795, -0.16097789612522162, 0.06301506253584287, - -0.034903311108329636, -0.26502981501524836, -0.12561198658961276, 0.07076159951379145, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46120041608810425, 0.46544772386550903, 0.05338919907808304, - 0.48362088203430176, 0.4387131631374359, -0.07902846485376358, -0.20560088753700256] -- [0.2226667242470824, 0.6096848667507775, 0.7272112392617671, 0.8363812373490017, - 0.02480906120482892, 0.0915536231422238, -0.5398785700342438, -0.31680314621782607, - -0.29677120718622074, 0.00099999999999999, 0.111923293897921, -0.06333044281498203, - 0.0009999999999999985, -0.04696545070948354, -0.16705229850491068, 0.0010000000000000135, - 0.025556509914786724, 0.1936892801806461, 0.28882598613027693, -0.8205063694947973, - -0.0900883930594438, -0.24034833198022001, 0.5019128680798961, -0.08695300745378215, - -0.11570759062134492, 0.03423565710674943, -0.3133638814321503, 0.5261109227947017, - 0.9080326911496742, 1.3285199760555335, -0.16097359692155291, 0.06413441201238419, - -0.03460234228358975, -0.2652605847649939, -0.12483685263949768, 0.07019829357436483, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4814380705356598, 0.6903944611549377, 0.06837189942598343, - 0.6457006931304932, 0.47940388321876526, -0.0807262510061264, -0.0028687429148703814] -- [0.22251496671559748, 0.6090477043519166, 0.7264786204203778, 0.8358768828240415, - 0.025341297445855497, 0.09197515682543303, -0.5405628787949289, -0.3169291483867157, - -0.2977126693091188, 0.00100000000000002, 0.11125500045157045, -0.06413624339662348, - 0.0009999999999999968, -0.04548003753065572, -0.16641132324638036, 0.0010000000000000206, - 0.025768542590732812, 0.19378209151815592, 0.289037378068898, -0.8217016413990533, - -0.09009649487179608, -0.23943627525807443, 0.5017564708271272, -0.08476540734497785, - -0.11428899590221649, 0.03453773912770184, -0.3131213724764041, 0.5264755343790755, - 0.9079518586264277, 1.3255933672593483, -0.16128203501481733, 0.06495774222711145, - -0.034211181632553364, -0.2651326893787329, -0.12369850910280562, 0.06948085969531567, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46498337388038635, 0.5595044493675232, 0.1459413319826126, - 0.37785807251930237, 0.7390245795249939, -0.09862460941076279, -0.19026923179626465] -- [0.22246930467033418, 0.6084973160943814, 0.7258238133855877, 0.8352476409123711, - 0.0258414571125131, 0.09230094079308215, -0.5414555695284076, -0.3168745966856469, - -0.2985954216138159, 0.0010000000000000033, 0.11074863580143422, -0.06479718324726802, - 0.0010000000000000072, -0.043686164889314934, -0.1654460742725074, 0.0009999999999999926, - 0.026128800247084655, 0.1937663142318075, 0.2892801894499926, -0.823299105339426, - -0.09056895563436784, -0.23868396291064317, 0.5014110803049372, -0.0828738594897955, - -0.11268304019109981, 0.03480513106830768, -0.3127384084496434, 0.5275134853113663, - 0.9076866310880508, 1.3228544159733406, -0.16150119294272228, 0.06623853253442122, - -0.03419883094187919, -0.2646908812048907, -0.12248814756884398, 0.06980486602227068, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5243017673492432, 0.5329734086990356, -0.12350758165121078, - 0.41920971870422363, 0.571799099445343, -0.12007209658622742, -0.17542436718940735] -- [0.22249054956865225, 0.6080014248183502, 0.7252179051498145, 0.8345403507464575, - 0.026325188863240836, 0.09256818577300792, -0.5424762837120056, -0.3167069188212672, - -0.29943774097952564, 0.0010000000000000013, 0.11033849737592762, -0.06537266407632014, - 0.0009999999999999957, -0.04170197543475334, -0.16427899592946127, 0.0009999999999999742, - 0.026583117492369072, 0.19367754314904168, 0.2895429049024099, -0.8251441568695943, - -0.09133549981455556, -0.2380303803283844, 0.500943714999067, -0.08116638023224376, - -0.11095934788318225, 0.035052436293918784, -0.3122701390575967, 0.5289741571585357, - 0.9073063076920925, 1.3202336181242196, -0.16166405957144064, 0.06780490027918211, - -0.034422558207750226, -0.26404972706612817, -0.12123270223582389, 0.0707738095742551, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5052318572998047, 0.6215460896492004, -0.22236479818820953, - 0.38139891624450684, 0.8451887965202332, -0.15141116082668304, -0.10900069028139114] -- [0.2226420368510856, 0.607642819702811, 0.7249668032039064, 0.8336863381555423, - 0.026798405665220598, 0.09272384142686704, -0.5437381945915253, -0.3163545250490734, - -0.30034234369649804, 0.0010000000000000141, 0.1100079217055801, -0.06588593410676206, - 0.0010000000000000091, -0.03913648760390501, -0.16268141410162956, 0.0009999999999999898, - 0.027247810671145244, 0.1935814753946322, 0.28935204903893214, -0.8263212328996349, - -0.09225824510749248, -0.2367093732454488, 0.4997887654572077, -0.0806254295609812, - -0.10904217902532477, 0.035541565912839364, -0.3120973649630771, 0.5309345475645916, - 0.9074440421481283, 1.3160683728275486, -0.1619087341946024, 0.0693843509479354, - -0.03436403087882229, -0.2636995810214164, -0.119279333993367, 0.0727287637241591, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4361809194087982, 0.47083941102027893, -0.009173264726996422, - 0.4578735828399658, 0.3517833650112152, -0.10645413398742676, -0.2753402292728424] -- [0.222878441587141, 0.6073726807216887, 0.7249435968942506, 0.832738198178694, 0.02726939822362829, - 0.09280863852699324, -0.5451513824887456, -0.31588099404254816, -0.3012772721533023, - 0.0009999999999999976, 0.10971992309260625, -0.06636713394794956, 0.0010000000000000122, - -0.03620238455328779, -0.16080943124813618, 0.0009999999999999918, 0.02804994112801405, - 0.19347022307124484, 0.2888712657059529, -0.8270737935365855, -0.09328763266686078, - -0.2349596424852711, 0.4981893452848779, -0.08082754211445999, -0.10699868251480685, - 0.03618935923703046, -0.312118481778507, 0.5332158756627456, 0.9079125369751785, - 1.3109218050724671, -0.16220727179886116, 0.07097363820767709, -0.034125078875961314, - -0.2635283069663375, -0.11688042047171975, 0.07530886747329124, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.5689274072647095, 0.5158655047416687, -0.029728008434176445, 0.7138344049453735, - 0.4468128979206085, -0.09716890007257462, -0.18339762091636658] -- [0.22323772911747972, 0.6072597719128673, 0.7251200728922139, 0.8315816822841611, - 0.027732706333081395, 0.09278166185828654, -0.5468952056031414, -0.3152348775966425, - -0.3024804609243874, 0.0009999999999999953, 0.1095503298869134, -0.06673354523673634, - 0.0009999999999999922, -0.03248503354648964, -0.1580742435982786, 0.0010000000000000167, - 0.029051585590187088, 0.19352760022882953, 0.2879674878777867, -0.8282357745340101, - -0.0947517416867625, -0.2329013153296269, 0.49612601740978984, -0.08191012412102094, - -0.10453758063319947, 0.03656765118199068, -0.3128027220473349, 0.535177353671145, - 0.9086500191163936, 1.3045082265150747, -0.16251905076194778, 0.07459081499023264, - -0.03401003273062337, -0.26323245653714666, -0.1140647723438873, 0.07805859618599109, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3799096643924713, 0.46251943707466125, -0.46385127305984497, - 0.4045693874359131, 0.2911392152309418, -0.2918880581855774, -0.18554256856441498] -- [0.22368505732800448, 0.6072478924559757, 0.7254284485418243, 0.8302871419903753, - 0.02819733733650842, 0.09268465335667318, -0.5488512795327519, -0.31446953488214335, - -0.3038396771205611, 0.0010000000000000028, 0.10944609436320128, -0.06703534921097495, - 0.0010000000000000009, -0.02826119101361387, -0.15477605020467086, 0.000999999999999992, - 0.03019135025700549, 0.19367579883692593, 0.2867871521989901, -0.8296815310883545, - -0.09651881764111102, -0.23063969098124765, 0.49375494197397723, -0.08355525689067386, - -0.10179564806228092, 0.036782976421817694, -0.31392275045956125, 0.5369298847332702, - 0.9095630357947101, 1.2972805691785403, -0.16284038682724852, 0.07952237812977986, - -0.033975290703213354, -0.26282216002857905, -0.11097691233029065, 0.08090231653835533, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4137186110019684, 0.4246889352798462, 0.14514727890491486, - 0.4272441565990448, 0.6340752243995667, -0.1725335419178009, -0.17544367909431458] -- [0.22404289643228384, 0.6074537437871804, 0.7259021846178366, 0.8287835111079883, - 0.02852978838976877, 0.09236018860828799, -0.5511565462281944, -0.31339651327186946, - -0.3052341741081154, 0.0010000000000000083, 0.10976628239745091, -0.06689535414041439, - 0.0010000000000000024, -0.023325803532406823, -0.15072413009929017, 0.0009999999999999848, - 0.03152910978768397, 0.19381841987140236, 0.28548475393586953, -0.8301071935869814, - -0.09830688733707685, -0.22795527589783202, 0.49100866451246156, -0.08595218084374279, - -0.0989284119235169, 0.037110380658220476, -0.3157564294523135, 0.5391204135479809, - 0.9106115191688442, 1.2877349273726162, -0.16352075261463064, 0.08510257193870203, - -0.0339924718506045, -0.26370306402567595, -0.10755929019932467, 0.08394681049930888, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46440568566322327, 0.5203483700752258, 0.004747138824313879, - 0.5412392020225525, 0.6212663650512695, -0.18633414804935455, -0.09978023916482925] -- [0.22435396569287913, 0.6078027062237716, 0.7264887630379877, 0.8271367645850313, - 0.028785084809626788, 0.09189125856827991, -0.5536896135589395, -0.312112170265492, - -0.30662923405187803, 0.0009999999999999818, 0.11035065847340296, -0.0664795076668713, - 0.0009999999999999966, -0.01792000423683128, -0.14617113402579077, 0.0009999999999999968, - 0.033008913713612, 0.1939335948540673, 0.28409310999599086, -0.8298572111755192, - -0.1001346180770267, -0.22499054367569502, 0.4880017682507157, -0.08882625349566511, - -0.0959627767729405, 0.03753420704238384, -0.31807327566111765, 0.5415922376591042, - 0.9117500658639616, 1.2766364198825082, -0.16444269725399677, 0.09110813641252934, - -0.03404579888761628, -0.26540779550809096, -0.10392147746953942, 0.08709701160949279, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4699329137802124, 0.5838613510131836, -0.006981401238590479, - 0.44777774810791016, 0.3369303345680237, -0.17061325907707214, -0.15956591069698334] -- [0.22453937636972038, 0.6083243369120577, 0.7273615743138717, 0.8253481543800364, - 0.028817693741774904, 0.09119786592659576, -0.5564646564152196, -0.3103171670049651, - -0.3077822004053161, 0.0009999999999999887, 0.11142695509962625, -0.06548687722943447, - 0.0010000000000000098, -0.012122145143952046, -0.14118768612008323, 0.0010000000000000009, - 0.03453036861396098, 0.19403601220283972, 0.2825209072575648, -0.8276520642030768, - -0.10159354125565778, -0.22147660926461865, 0.4843963928089244, -0.0922179605346699, - -0.09306340148046334, 0.037652530188478354, -0.32039157596299794, 0.5450092794728039, - 0.9136206312742001, 1.2632071630113981, -0.16593459018972287, 0.0982306004620408, - -0.03386078492590337, -0.2674849668449774, -0.09986266699682886, 0.09137073101556009, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4470249116420746, 0.36737069487571716, -0.02712886966764927, - 0.7421159744262695, 0.7081635594367981, -0.2732573449611664, -0.10461148619651794] -- [0.22465693701682946, 0.6089607734302197, 0.7284325290812661, 0.8234578437741115, - 0.028712231024500504, 0.09036264106184556, -0.5593991244333648, -0.3081709803334635, - -0.30875193781644517, 0.0010000000000000041, 0.11281393641084843, -0.06412922831219872, - 0.000999999999999997, -0.0060587483523725566, -0.13591314074106314, 0.0010000000000000041, - 0.03608769718169521, 0.1941098174317726, 0.2808176305899522, -0.8240996092684593, - -0.10282658637097301, -0.21759146669581853, 0.4803625184345473, -0.09594999928972178, - -0.09018906225125617, 0.03759124986230954, -0.3227094042777418, 0.5490445778030355, - 0.9159926544047465, 1.248127838287038, -0.16782188289967367, 0.10612108293175829, - -0.033517831111874015, -0.2697617859988913, -0.09551617822755126, 0.096375284098969, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.10824940353631973, 0.40894266963005066, -0.10374201834201813, - 0.5201700925827026, -0.009407657198607922, -0.452745646238327, -0.3196319341659546] -- [0.22451719256691705, 0.6097399971387304, 0.7294607336573906, 0.8214571045177654, - 0.02843890099004887, 0.08919629176827602, -0.5625330887001732, -0.3054472978269994, - -0.3092723314725733, 0.0010000000000000035, 0.11452848056882058, -0.06236725088907082, - 0.0009999999999999946, 0.0002976682681149236, -0.13021716011965825, 0.0010000000000000187, - 0.03769472523690146, 0.19395280143449015, 0.279188107514294, -0.817677216717726, - -0.10411233133892335, -0.21325320788566535, 0.4756650208738681, -0.10128825858944207, - -0.08703795292893078, 0.03764193860592495, -0.3248578829876653, 0.555348894821046, - 0.9188264181426083, 1.2287230839285899, -0.16949462595892675, 0.11544788781896259, - -0.03318309767503563, -0.2729854711081209, -0.09079221610559497, 0.10335685828643079, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5150296092033386, 0.6253513693809509, -0.2695063054561615, - 0.36105796694755554, 0.6598750352859497, -0.1794372946023941, -0.09744507074356079] -- [0.2242281666231239, 0.610608828232382, 0.7304610667735936, 0.8193748429561519, - 0.028069277363271496, 0.08781609588823397, -0.5657961785853927, -0.3023257961883465, - -0.30946108631458874, 0.0009999999999999994, 0.11642001624879562, -0.060377797346116605, - 0.0009999999999999953, 0.006851395841519304, -0.12423225880992916, 0.001000000000000017, - 0.039348604341532, 0.19360875181628157, 0.2775837931650164, -0.8092122804783374, - -0.10547956773979304, -0.20861251074442638, 0.47047547254851946, -0.10778727121819665, - -0.08365759466766859, 0.037821235644589994, -0.3268863023512704, 0.5631675817048245, - 0.9219832122253053, 1.2061281983784093, -0.17102199149357827, 0.12582999655652904, - -0.03285487464749133, -0.2767817598528345, -0.08580478463679077, 0.11168320642252236, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40284815430641174, 0.543919563293457, -0.07159546762704849, - 0.5789600014686584, 0.5007007122039795, -0.1694386750459671, -0.24349939823150635] -- [0.22372476822664197, 0.6116517302828739, 0.7312539042961853, 0.8171475961677882, - 0.027554569044772787, 0.08616706481041064, -0.5692853315727715, -0.2988056967991032, - -0.3094017590206324, 0.0010000000000000135, 0.11855896054672571, -0.058076408745052883, - 0.000999999999999999, 0.013874604131665684, -0.11787065100840313, 0.0009999999999999942, - 0.04112836003246941, 0.19304001199368223, 0.27652137819128975, -0.7990593851842723, - -0.10704140579196571, -0.20339313665860353, 0.4648231821962852, -0.11535094545308341, - -0.08032809056091576, 0.03819404240403759, -0.32940871340179934, 0.5710374177911676, - 0.9250838050513988, 1.182013300223315, -0.17320995369256584, 0.13792598822761568, - -0.03281443252050146, -0.2814113715756965, -0.0803258938900287, 0.12067882223807806, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4820256531238556, 0.6963319778442383, -0.3346819281578064, - 0.5179240107536316, 0.45890283584594727, -0.12358913570642471, -0.17460885643959045] -- [0.22310739386879633, 0.6128026121376141, 0.7318997779836504, 0.8148104558019263, - 0.02696194908636719, 0.08434691477914244, -0.572924578268869, -0.2950018971681864, - -0.3091392493778079, 0.0010000000000000078, 0.12081326229476903, -0.055620092142481876, - 0.0009999999999999987, 0.021227465489305, -0.11124311909321603, 0.0010000000000000054, - 0.04301840475349004, 0.19228776847692708, 0.27581263657572014, -0.7876490897793416, - -0.10880504837267103, -0.19778990556150366, 0.45881575441045475, -0.12368695561405191, - -0.07699275845824195, 0.03876722999011762, -0.3322923137924999, 0.5788651986382385, - 0.928142250210827, 1.1566276078935618, -0.17587386653944911, 0.15125951980949756, - -0.03298575595956485, -0.28653681085995664, -0.07449744150502555, 0.13006578841618358, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4091947376728058, 0.752021849155426, -0.2806150019168854, - 0.6175856590270996, 0.4376969635486603, -0.11668689548969269, -0.36852607131004333] -- [0.22246127569153137, 0.6140301055279528, 0.7326504301111032, 0.8123811186415301, - 0.026276720121311426, 0.0823996170876032, -0.5766773405971636, -0.2908908859451629, - -0.30874732801158467, 0.0010000000000000035, 0.12301070536494907, -0.05307069524369212, - 0.0009999999999999992, 0.028838314514325294, -0.10458553844513412, 0.0010000000000000057, - 0.04504160304910349, 0.1916843873652412, 0.27516906986621387, -0.7744857847474114, - -0.11013820513914627, -0.1919821449836897, 0.4521686307742726, -0.13355774512686164, - -0.07357142803251399, 0.03976940745103347, -0.33473706041402695, 0.5871672515914017, - 0.9316741251729008, 1.1300540589598091, -0.1781020026593066, 0.1654848454423362, - -0.033188793159401396, -0.2917754655534969, -0.0681875801892233, 0.13945665901606577, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4221351146697998, 0.5134783983230591, -0.09474010765552521, - 0.38676926493644714, 0.5300495028495789, -0.19312722980976105, -0.17674721777439117] -- [0.22178967394961038, 0.6152365711223396, 0.7334692659680138, 0.8098050354383814, - 0.025645816078576612, 0.08036396995004098, -0.5806029013277273, -0.2864441123142046, - -0.3088138196605693, 0.000999999999999992, 0.12510507989478617, -0.050563610387975445, - -0.0009999999999999998, 0.036946200355578086, -0.0975990429431791, 0.000999999999999991, - 0.04702403132348591, 0.1921703213026727, 0.27464633427518625, -0.7596193891994142, - -0.11121016552320828, -0.18661796366804098, 0.4466909468751708, -0.14402500631017712, - -0.07012001200690797, 0.04107838617509313, -0.3361184475128034, 0.5947449504945469, - 0.935552877732289, 1.1020990255541325, -0.18001362848789662, 0.18033247518664755, - -0.03341283054980348, -0.296842503124196, -0.06152607241829706, 0.14885605086476875, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5087016820907593, 0.6663009524345398, 0.03587309271097183, - 0.6826770901679993, 0.38113078474998474, 0.0819728747010231, -0.22638611495494843] -- [0.2209783882317561, 0.6164550336486798, 0.7342112545736295, 0.8072108288897523, - 0.024929107366790902, 0.0782652363822153, -0.5845201195023564, -0.28197937001755197, - -0.30820652148962774, 0.0010000000000000028, 0.1272617262583313, -0.04776263933332352, - -0.000999999999999999, 0.04498219721694331, -0.09072137891329377, 0.0009999999999999955, - 0.04935725501557115, 0.19162268726418266, 0.2739602736843366, -0.744971470182014, - -0.11220657854354191, -0.1808675495697338, 0.43951715127509444, -0.1561847251290268, - -0.06638296513099502, 0.04298572831234249, -0.3385371517278248, 0.6038717162400896, - 0.9394497311523182, 1.073639675387836, -0.18293519495905353, 0.19508420862651127, - -0.03381247215054433, -0.3023316465277971, -0.05468141484726862, 0.1574289620979467, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2592923939228058, 0.49204087257385254, -0.4393715560436249, - 0.42359328269958496, 0.37411487102508545, -0.2580447494983673, -0.4044051766395569] -- [0.22006691235493195, 0.6176128527456579, 0.7348920612530471, 0.8045407253814097, - 0.024242525537041055, 0.07612763316766792, -0.5884990268697383, -0.27741426719138695, - -0.30750601691310026, 0.0009999999999999894, 0.1294166248178344, -0.044832843113059996, - -0.0010000000000000013, 0.05320003488031471, -0.08368601085444241, 0.0009999999999999844, - 0.05187415619407168, 0.19106092260246219, 0.27321030176356065, -0.7301715066052947, - -0.11322189783726903, -0.17530912963566697, 0.4322983367851532, -0.16923418379942687, - -0.062448200821748026, 0.04535039436437097, -0.3411668069309332, 0.613338035114146, - 0.943359134293695, 1.0444062415828368, -0.18661799832087378, 0.20972573484329526, - -0.03435761471013949, -0.30794691401264257, -0.04770285950846875, 0.16534318411884677, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4079096019268036, 0.28512030839920044, 0.14320553839206696, - 0.4054725468158722, 0.35675135254859924, -0.2538098990917206, -0.15494422614574432] -- [0.21920053320428445, 0.6187095153361544, 0.7353986293873288, 0.8017782503964788, - 0.02357725860156286, 0.07397106305755932, -0.5925571971532634, -0.2726644221513676, - -0.3068139217630901, 0.000999999999999998, 0.13129661838847875, -0.04211107438871287, - -0.0010000000000000035, 0.0616116734962725, -0.07669403718240608, 0.000999999999999984, - 0.05443375596214136, 0.19088831502375314, 0.2727125387380753, -0.7157735846137704, - -0.11435419885042437, -0.17020122750423738, 0.425312373662739, -0.18361942511216872, - -0.058495753760769834, 0.048470807777725104, -0.34366316124203483, 0.6219497269048634, - 0.9465470560797185, 1.0150821249675142, -0.19015612121904038, 0.2251083297406114, - -0.035103640689185575, -0.31309217579703197, -0.04094713801253234, 0.17267056684022275, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.18900077044963837, 0.34942781925201416, -0.273305743932724, - 0.3934656083583832, 0.29941096901893616, -0.23621059954166412, -0.1791064590215683] -- [0.21840332445807625, 0.6197423155545865, 0.7357772539030564, 0.7989341238818637, - 0.022933696048596585, 0.0718203195116599, -0.5966742436102902, -0.26776592716677855, - -0.3060919455185269, 0.0009999999999999987, 0.13295357258693716, -0.039562982232951474, - -0.0010000000000000002, 0.07017819396103897, -0.06972840590530198, 0.001000000000000009, - 0.057055797792921754, 0.19098007912069534, 0.2724042995847775, -0.7016877901401138, - -0.11564104254187034, -0.16546713225629225, 0.4185095921765013, -0.1990155899510625, - -0.05450061679319165, 0.052220475861603236, -0.34603960300435127, 0.629806185758438, - 0.9491791387803659, 0.9856236001447868, -0.19358022589497165, 0.2410056977413546, - -0.036007557585070994, -0.3177719853894114, -0.0343636261070985, 0.17945234043941782, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4740794599056244, 0.7743831276893616, -0.4287719428539276, - 0.32947492599487305, 0.6880061626434326, -0.22365984320640564, -0.15167562663555145] -- [0.21762932794659454, 0.6206769958061188, 0.7361941446424941, 0.7960384040380961, - 0.022191169845079947, 0.06978737414818424, -0.600799578634081, -0.2628630488506548, - -0.30537507010308296, 0.0010000000000000002, 0.13470412703350382, -0.037219871458034896, - -0.0010000000000000005, 0.07849484554635301, -0.06396835548624553, 0.0009999999999999957, - 0.05981387949510613, 0.19181806402832446, 0.272176665507159, -0.6880052674510869, - -0.11653171869839533, -0.1613501257038635, 0.41239209354872264, -0.21472774996632307, - -0.05081593576798651, 0.05633493976224345, -0.3482495101332574, 0.6356811323121471, - 0.9511618582123685, 0.9574193430864006, -0.19691278413040206, 0.25560075105711305, - -0.03714880469275045, -0.32150244413553264, -0.028188340949597965, 0.1844948468000547, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4387381672859192, 0.6486315727233887, -0.16383063793182373, - 0.35874706506729126, 0.319232702255249, -0.16828736662864685, -0.13027994334697723] -- [0.21688986348810724, 0.6215241632307571, 0.7366475260543667, 0.7930910173671992, - 0.0213720988737896, 0.06787130602919426, -0.6049325229966407, -0.25796130899999037, - -0.30465649928442273, 0.0009999999999999957, 0.13655286562826524, -0.035011827048157584, - -0.0009999999999999929, 0.0866238545246492, -0.05915441122385539, 0.00100000000000001, - 0.062693857798932, 0.19321297766169462, 0.27200593473321827, -0.6746305473251758, - -0.1171391767631837, -0.15772087997762135, 0.40682782362697706, -0.23063059926890161, - -0.04736549315122354, 0.06074259867095711, -0.35030165978106587, 0.639924810769595, - 0.9526329243306958, 0.9302304355217996, -0.2001690696548693, 0.26911917442496147, - -0.038479001497933665, -0.3244960275692931, -0.022335121547662377, 0.18810769927264978, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.28922373056411743, 0.37548327445983887, 0.4609956443309784, - 0.3597395420074463, 0.5408120155334473, -0.22080783545970917, -0.17144297063350677] -- [0.21602195097108548, 0.6223215401182893, 0.7370873675784467, 0.7900370205540516, - 0.020508664294647008, 0.0662093642544466, -0.6091282467001086, -0.2534997821650691, - -0.304251759230496, 0.001000000000000003, 0.13889895074879954, -0.033634509419317636, - -0.0009999999999999953, 0.0946515229165963, -0.05791303329120058, 0.0009999999999999903, - 0.06575553858845537, 0.1954957564786064, 0.2717092313158881, -0.6624672094941644, - -0.1176958592828701, -0.1549943552606772, 0.402108894218892, -0.24621513486493424, - -0.044306184101506815, 0.06501952486451752, -0.3520039720538113, 0.6424002138013709, - 0.953556237344391, 0.9056023793601147, -0.20375837557544785, 0.2819564911613547, - -0.04013858815720781, -0.3273317492554313, -0.016485625985763173, 0.19007696821789577, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5649714469909668, 0.6105594038963318, -0.29412156343460083, - 0.39026495814323425, 0.4475876986980438, -0.09149907529354095, -0.16568654775619507] -- [0.2150593192930063, 0.6230751087166391, 0.737522393050035, 0.7868884903387534, - 0.019605979065993973, 0.06477211563249667, -0.6133731999311293, -0.24939707181880374, - -0.30412170673551175, 0.001000000000000001, 0.14168172217200545, -0.03289192503398095, - -0.0009999999999999948, 0.10259870238077082, -0.05954914928181847, 0.0009999999999999753, - 0.06897142284927361, 0.19849221426312388, 0.2713044668118309, -0.6513061894490826, - -0.1182207331880354, -0.15296675053767142, 0.3980934957139017, -0.26145562307359205, - -0.041559533541445856, 0.06915120454587859, -0.3534019095247472, 0.643409157000251, - 0.9540379554361127, 0.8831404297902734, -0.20761111500853588, 0.2941969771910401, - -0.04206979180707827, -0.33004170089180607, -0.010641296005099901, 0.19072753533748066, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4833468794822693, 0.6266980171203613, -0.3013025224208832, - 0.6054883003234863, 0.38195669651031494, -0.21396134793758392, -0.1076045036315918] -- [0.21376622697662412, 0.6236956416896468, 0.7379400314949005, 0.7837576421675072, - 0.018394850118404082, 0.06384951252985499, -0.6175020871080935, -0.2458755131618178, - -0.30433592106362567, 0.0010000000000000018, 0.14607753628620898, -0.03305076537235889, - -0.0010000000000000037, 0.1098575870633546, -0.06691768740974201, 0.0009999999999999987, - 0.07223720693441917, 0.20294165273163967, 0.2709718161678957, -0.6406535096122095, - -0.11877843235621574, -0.15262236628686474, 0.3960012823747548, -0.2761751180626101, - -0.03975988398515677, 0.07088416028993258, -0.35352203091415246, 0.6431157818997485, - 0.9531213818006622, 0.8627563010825265, -0.21057183641029412, 0.3056270224159497, - -0.0440541597919347, -0.33213584149798864, -0.00559733329479402, 0.18999013227523556, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.28697818517684937, 0.40789955854415894, 0.19523970782756805, - 0.43688517808914185, 0.44167596101760864, -0.09170684218406677, -0.21673914790153503] -- [0.21218938621841088, 0.6242214490280362, 0.7383475651020744, 0.7806316219507747, - 0.016933084117829778, 0.06338080131470951, -0.6215387481866821, -0.24287035945427163, - -0.3048852672657394, 0.0010000000000000052, 0.1518665168744659, -0.03393401838670072, - -0.0009999999999999896, 0.11654892517396893, -0.07899365785851228, 0.001000000000000003, - 0.07549998208621019, 0.2086743528327449, 0.2706760457711133, -0.6304642947990721, - -0.11929800374161449, -0.15365735990354443, 0.39552363131841467, -0.29036760290676994, - -0.038756249749644214, 0.07054477229899456, -0.3525627511367673, 0.6417068167730904, - 0.9510523985408544, 0.8442311230838732, -0.21279489463393134, 0.31637068184520933, - -0.0460774155776256, -0.3337507654063343, -0.0012109593463653946, 0.1881189257629271, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3676673471927643, 0.6668657660484314, 0.01729598455131054, - 0.42364102602005005, 0.48405754566192627, -0.014953378587961197, -0.27096059918403625] -- [0.2104044547804245, 0.6246806324814458, 0.738731139465114, 0.7775575713894144, - 0.01547204893505291, 0.06300988398379159, -0.6254555087271171, -0.24011160897583864, - -0.30557164719676677, 0.0009999999999999985, 0.15796312381303476, -0.03635475349390769, - -0.0009999999999999968, 0.12283319343599686, -0.08973389920934909, 0.0010000000000000076, - 0.07884784776397488, 0.21474693751139246, 0.2706079785000168, -0.620997295768719, - -0.12006868431235886, -0.15419574966230762, 0.39575388409240475, -0.3038739122642277, - -0.037994688891719385, 0.07163636024579105, -0.35110159990367845, 0.6387160870695165, - 0.9479789822980871, 0.8288270984115947, -0.21452404401840797, 0.3260751180824776, - -0.048152826275275167, -0.33535503878232936, 0.0029613762028700627, 0.18506014623509548, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.16827240586280823, 0.8036423325538635, -0.6063616871833801, - 0.4236948788166046, 0.31813845038414, -0.24425630271434784, -0.3720184564590454] -- [0.20844067078824738, 0.6250826353274442, 0.7390928308485535, 0.7745277986668593, - 0.014021084511668966, 0.06271609787800367, -0.6292668665584908, -0.23755753184910025, - -0.3063712844480495, 0.001, 0.16430346424676864, -0.04006698365242452, -0.0009999999999999894, - 0.12877178950542217, -0.09935565379072972, 0.0009999999999999994, 0.08227203960558953, - 0.22108303630645482, 0.27073438667535127, -0.6121459143170347, -0.12105053399098195, - -0.15428954094655956, 0.39656453055738644, -0.3167610662882496, -0.037440742803078346, - 0.07391387658795222, -0.34923449667252954, 0.6343934390942083, 0.94405813573729, - 0.8161014431315836, -0.21583773292500258, 0.3348931590086608, -0.05027118660219332, - -0.33696280255197764, 0.006951523759832289, 0.1810197036098323, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.605850875377655, 0.570005476474762, -0.13033127784729004, 0.7410562634468079, - 0.3672734200954437, 0.06113952025771141, -0.24468080699443817] -- [0.2061546461364615, 0.6253554284150937, 0.7393875615000893, 0.7716330990753554, - 0.012528155499335114, 0.06244438496823554, -0.63287131750229, -0.23505349473344225, - -0.3071516032882603, 0.0009999999999999987, 0.17109145829416464, -0.04495603113464528, - -0.0009999999999999907, 0.13401171776010573, -0.1052789183530472, 0.001000000000000001, - 0.08570835338536943, 0.22727833216562568, 0.2711330157663844, -0.6037572195827112, - -0.12181177723921578, -0.15323421269423348, 0.39796435060539553, -0.3292745637684491, - -0.03705837701651835, 0.07750648843574509, -0.3475229688904405, 0.629156679591306, - 0.9394412464784854, 0.805429990121289, -0.21784734525214808, 0.3425706029674983, - -0.05238674144385219, -0.3381523396117187, 0.010525473417832627, 0.17645757072906007, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2610284686088562, 0.45623430609703064, -0.2220088392496109, - 0.39080992341041565, 0.48014354705810547, -0.10364539921283722, -0.46485063433647156] -- [0.2035799348774323, 0.6255177556413984, 0.7396202616810124, 0.7688589916790746, - 0.01100701774019613, 0.06218068547929012, -0.6362925890091976, -0.23259475826258502, - -0.3079035279174807, 0.0010000000000000048, 0.1782451738348328, -0.05084843574843895, - -0.0010000000000000009, 0.13864400843671695, -0.10801484034950946, 0.0009999999999999955, - 0.08916435030418272, 0.23331653702871574, 0.27176844515416154, -0.5957535782618946, - -0.12237930548086773, -0.1511907730195155, 0.3998483563909928, -0.3414492618691059, - -0.0368312539939797, 0.0822521154800511, -0.34596276625588757, 0.6231431960008748, - 0.934222425972036, 0.7965380101424517, -0.22045851252963986, 0.3492659250284479, - -0.05450338125504699, -0.33899871109129254, 0.013732676276231655, 0.17144749232716947, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5003201365470886, 0.6574303507804871, -0.13275472819805145, - 0.6121410131454468, 0.53969806432724, -0.02937616966664791, -0.10522118210792542] -- [0.20064701602024523, 0.6256864296757582, 0.7396305222479104, 0.766193805974537, - 0.009303478036022633, 0.06202002818513882, -0.6395420338700191, -0.23034220176949727, - -0.3086315726961889, 0.000999999999999999, 0.18634073107852767, -0.05835353046249413, - -0.000999999999999995, 0.1425404682850456, -0.11075689301629986, 0.0010000000000000263, - 0.09267146572033032, 0.23917600507976822, 0.2729755584487469, -0.5873814597711033, - -0.12304827714292006, -0.1485076923154955, 0.4025912783052835, -0.3537051100049781, - -0.03755276377481459, 0.08706790518727435, -0.3438711907526834, 0.6157088282141151, - 0.9277804539461628, 0.7887679869064178, -0.22298815957806764, 0.35484340595188646, - -0.05659861986997663, -0.3399879638209978, 0.015794884014277564, 0.16623432144048114, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.546917200088501, 0.3726036846637726, 0.07682618498802185, - 0.6687809228897095, 0.3767825663089752, -0.1748247891664505, -0.1746160089969635] -- [0.1974073108006229, 0.625870993487988, 0.7394395075030906, 0.763625242636487, 0.0074458270850077295, - 0.06194784015670944, -0.6426379334954522, -0.22827778885626174, -0.3093443340493689, - 0.001000000000000003, 0.195235651705982, -0.06730797684947756, -0.0009999999999999983, - 0.14578712312011402, -0.11348613118324476, 0.0009999999999999942, 0.09620648597078976, - 0.2448801212182048, 0.27468570792329244, -0.5786608717067858, -0.12378583149177311, - -0.14525291542110638, 0.4060759814199079, -0.3660332915186824, -0.039092548609536974, - 0.09194471795509951, -0.3413150045724259, 0.6069999602752195, 0.9202572040082955, - 0.7820026262842281, -0.22545390416104696, 0.3594482125297074, -0.05866786349873834, - -0.34110291526759523, 0.016841506976662028, 0.16084435492327934, 0.0, 0.0, 0.0, - 1.0, 1.0, 0.0, 0.0, 0.6492553353309631, 0.274284303188324, 0.1300743818283081, 0.6511502265930176, - 0.4283010959625244, -0.14607518911361694, -0.26570793986320496] -- [0.19381653353752049, 0.6261199635708569, 0.7392980071149468, 0.761233039358696, - 0.005387114284498692, 0.061965524741936645, -0.6454885843544129, -0.22640984526293337, - -0.30994905560719127, 0.0009999999999999985, 0.20498196725166345, -0.07793794538161489, - -0.0010000000000000054, 0.1482505371177263, -0.11728820126379973, 0.000999999999999985, - 0.09967907334727948, 0.2502685344432972, 0.2765418338205961, -0.5691814209635017, - -0.1243529189062726, -0.14116081420409626, 0.40999580750036474, -0.37927732787388335, - -0.041120387171281386, 0.0959945034467657, -0.3388668890574481, 0.5974422764063114, - 0.9124566084636158, 0.7764167712860007, -0.22891633402025727, 0.3634915665979695, - -0.06028274685372923, -0.3425301392763879, 0.01722261113771719, 0.15618731948486145, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3774341642856598, 0.5093996524810791, 0.018886800855398178, - 0.3662133514881134, 0.37616240978240967, -0.17935554683208466, -0.17091697454452515] -- [0.18993136367791952, 0.6264343422650321, 0.7391969928783757, 0.7589990810445786, - 0.00314702956556713, 0.062064438937795945, -0.6481191993744168, -0.22472120655349964, - -0.3104735421637187, 0.0009999999999999961, 0.21546115468871133, -0.09011350237125339, - -0.0009999999999999929, 0.1500095777060531, -0.12203321583843837, 0.0009999999999999979, - 0.1030707292850378, 0.25537945778783366, 0.2785209248746548, -0.5589659807126582, - -0.1247545754402675, -0.13630219994974863, 0.41431443494054826, -0.3933653175766478, - -0.04355446609458514, 0.09929078682618367, -0.33650669617144835, 0.5870810097998942, - 0.9044035649958155, 0.7719170606106983, -0.23328174656872513, 0.36705080713897065, - -0.06148053273621691, -0.34423703255467003, 0.01699767891603651, 0.15219660926620357, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4955638647079468, 0.6388664245605469, -0.012293422594666481, - 0.7181238532066345, 0.4337459206581116, -0.154298797249794, -0.24174955487251282] -- [0.18569494723990404, 0.6266991522783333, 0.7394022869563851, 0.7568672559299594, - 0.0008812026520227824, 0.062206847608254304, -0.6506008672708504, -0.22304329987126392, - -0.31100676323871973, 0.0010000000000000005, 0.22649911404161563, -0.10364419085441577, - -0.0009999999999999968, 0.15144422656371292, -0.1271928254075881, 0.001000000000000004, - 0.10626214551615698, 0.2598584663996066, 0.279966366450929, -0.5492450709981865, - -0.12458487511727324, -0.12989937839717855, 0.41850744036890763, -0.40738205170703795, - -0.04669767457627389, 0.10227565927469022, -0.33452681280436974, 0.5764153875203867, - 0.8972284213076184, 0.7698428020528033, -0.2389754045650763, 0.3697684932810831, - -0.061868871223413655, -0.3453567662924029, 0.016879185945167182, 0.149274026792686, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4420473873615265, 0.5654123425483704, -0.0796637311577797, - 0.3657461702823639, 0.6950138211250305, -0.1295912265777588, -0.25665056705474854] -- [0.18114816992671393, 0.6269255087950791, 0.7398866414697531, 0.7548300459815529, - -0.0014048651729223853, 0.06238405067027416, -0.652945524725685, -0.22137967035313122, - -0.31154175793870165, 0.0009999999999999966, 0.23801381024938456, -0.11845861751056032, - -0.000999999999999999, 0.1525831914357647, -0.13271080582306738, 0.0009999999999999992, - 0.1092616546436398, 0.2637375647428371, 0.28091657720690016, -0.5399612097177807, - -0.12387857778052447, -0.1220501796636925, 0.42257690586747987, -0.42137202801413653, - -0.05046578709117929, 0.10497679454728129, -0.3329091969295712, 0.5654959940612684, - 0.8908636345311105, 0.7700077286677192, -0.24589536481617733, 0.37172860044337686, - -0.06150775138422431, -0.3459312337989765, 0.016867447455656638, 0.1473516310565556, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.26317283511161804, 0.7056712508201599, -0.13156840205192566, - 0.7290399074554443, 0.06328712403774261, -0.25165295600891113, -0.25558918714523315] -- [0.17623124678678148, 0.6270794335723197, 0.7402697813443881, 0.7530450957639165, - -0.003609940118833781, 0.06242457859248689, -0.6549910106755684, -0.2199140454224445, - -0.3118581526532056, 0.0009999999999999985, 0.24947161516730773, -0.13440296289538603, - -0.000999999999999994, 0.152878182824683, -0.13764143209552862, 0.001000000000000001, - 0.1125611578019943, 0.2666049466961079, 0.2818587077387564, -0.5311804840113865, - -0.1239671772284355, -0.11353592333655642, 0.426686852219977, -0.4361330089023534, - -0.05414622885323046, 0.10747514903842692, -0.33193173919927227, 0.5550759648585476, - 0.8845267213578443, 0.7706361001891455, -0.253335312726471, 0.3736183503806157, - -0.060922003605854286, -0.34609827721063213, 0.01657672158683631, 0.14631508561902393, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.37116941809654236, 0.6069095134735107, -0.008125899359583855, - 0.46871763467788696, 0.5387622117996216, -0.18947884440422058, -0.2943613827228546] -- [0.1709646680525483, 0.6271641389807359, 0.7405543606279332, 0.7514998999306218, - -0.00574629704595081, 0.06233541324963037, -0.6567565581930053, -0.21863255900170486, - -0.31198149127282543, 0.0009999999999999972, 0.2608824957185043, -0.15139967349034875, - -0.0010000000000000035, 0.15237518324661234, -0.14201651437477336, 0.0009999999999999926, - 0.11613056078062592, 0.2685066984668814, 0.2827917048068979, -0.5228468679066136, - -0.12480608541107027, -0.10439094732441671, 0.4308499405023873, -0.45164344830358416, - -0.057749315710164055, 0.1097873214722825, -0.3315477924898504, 0.5451318832321539, - 0.8782159562079455, 0.7717066628083123, -0.26126480982635003, 0.3754386013787156, - -0.06012063512831881, -0.34588016402283467, 0.016025835614577592, 0.14611920855053043, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3625740706920624, 0.5629050731658936, 0.022296320647001266, - 0.48337945342063904, 0.5216774344444275, -0.15869095921516418, -0.27434539794921875] -- [0.1653764059864881, 0.6271618918674787, 0.7406910660312498, 0.7502102845577148, - -0.007836460145494429, 0.062101656664233694, -0.6582298254225323, -0.2172372348792796, - -0.31188844873507143, 0.0009999999999999968, 0.2721503459648721, -0.1687429895121177, - -0.000999999999999994, 0.15121126593152243, -0.14531325430091926, 0.0010000000000000172, - 0.11941584306933031, 0.26926677297280216, 0.28361720735477786, -0.51516324939894, - -0.12549821054162094, -0.09431122937225755, 0.4352695357233587, -0.4671362142057124, - -0.06181952340703226, 0.11246371447103395, -0.33160760291003805, 0.5349986397414555, - 0.8720919814749984, 0.7742938354662647, -0.26837267016375016, 0.37697011111482814, - -0.05932960528970608, -0.3453519900849175, 0.014852370633406609, 0.14701321714599447, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3700600266456604, 0.4666571617126465, 0.24863265454769135, - 0.37386780977249146, 0.46164241433143616, 0.09952529519796371, -0.2303110510110855] -- [0.15947359721561194, 0.6270710231627119, 0.7406806828061341, 0.7491696066458029, - -0.009893612424464097, 0.061728593937503504, -0.659421411240085, -0.21572245819830327, - -0.31159392763048915, 0.0009999999999999994, 0.28330409489001523, -0.1864000075142629, - -0.0009999999999999942, 0.14940373865787865, -0.14756181366102247, 0.001000000000000015, - 0.12242049180458882, 0.26891045871993097, 0.2843459351649652, -0.5080775050773337, - -0.12605280590038104, -0.08332217920259732, 0.43996069306792135, -0.48261528485322663, - -0.06635435522407387, 0.1155036609877196, -0.33209294950882295, 0.5246991197837294, - 0.8661476851679715, 0.7783306921511183, -0.2746880467893954, 0.3782148683638304, - -0.05853913415442902, -0.3445257285105014, 0.013076804846447191, 0.1489772827166889, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4301838278770447, 0.33239808678627014, 0.035870034247636795, - 0.3985308110713959, 0.7003883123397827, -0.033358383923769, -0.22143910825252533] -- [0.1531773994323091, 0.626824907532288, 0.7406118804944954, 0.7483452881080834, - -0.011847027110598833, 0.06116407556000417, -0.6603771146670365, -0.2141860644885628, - -0.3109318651794893, 0.001000000000000003, 0.29431262109338, -0.2037615752924195, - -0.0009999999999999896, 0.1470270962686738, -0.1487975518388914, 0.0009999999999999916, - 0.12554607153047143, 0.2671104634040152, 0.2848639796837539, -0.5010378426410365, - -0.1262360861944024, -0.07116482371323352, 0.4443158546361136, -0.4983439967113086, - -0.0714369760769011, 0.11948711033555971, -0.3331956251162771, 0.5148141733087687, - 0.8609312817642066, 0.7823805518487843, -0.28136328089907553, 0.3796765209199266, - -0.05801078199683612, -0.343722302379483, 0.011698105431159508, 0.15085395289697892, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5488494634628296, 0.5726338624954224, -0.0959063395857811, - 0.6932507753372192, 0.44043561816215515, -0.1800856739282608, -0.1989917755126953] -- [0.146459677097813, 0.6264143198035689, 0.7404823019279885, 0.7477307302701149, - -0.013725829063098358, 0.06042507053810552, -0.6611045057150309, -0.21261914172818283, - -0.30991597808262844, 0.0010000000000000063, 0.3053002628284735, -0.22071721269034394, - -0.0009999999999999961, 0.14406126181603318, -0.14904786385824847, 0.001000000000000012, - 0.1287855675962817, 0.2639378544205637, 0.2851889010106072, -0.4940092989258916, - -0.12604927858587114, -0.05789541453987761, 0.4483892987082901, -0.5142601359211698, - -0.07712844597435713, 0.1243745599225218, -0.33488381842910664, 0.5053236327925466, - 0.8564362561745522, 0.7863914542943067, -0.2883926002141795, 0.38135647354656005, - -0.057748606152896155, -0.3429589515852098, 0.0107114609886104, 0.1526518822679629, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.370313435792923, 0.579965353012085, -0.2486954778432846, - 0.5870153307914734, 0.42572712898254395, -0.24812519550323486, -0.35605403780937195] -- [0.13940283927403396, 0.6257941035046052, 0.7402838261010952, 0.7472048670261217, - -0.015642487054940618, 0.059690905813980455, -0.661722898994979, -0.21101067589521186, - -0.30894011833870605, 0.0010000000000000005, 0.3166926574379378, -0.23620552046481416, - -0.0009999999999999877, 0.1402801554014867, -0.14796312868160189, 0.0009999999999999974, - 0.1319918611605031, 0.2600438584149981, 0.28529109214091014, -0.48663050471966296, - -0.12445985703798716, -0.04407072826557302, 0.45294460845717155, -0.5302249310083872, - -0.08370699818289515, 0.12924537264611582, -0.3383766269679202, 0.49625127138021763, - 0.8526087306102497, 0.79011331714935, -0.29631878469312617, 0.38405546298695875, - -0.05826255741007062, -0.3424637747128321, 0.011417259748658518, 0.15430058776678343, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2384396195411682, 0.5373000502586365, -0.046213943511247635, - 0.37241387367248535, 0.6158249378204346, -0.11448492109775543, -0.39463120698928833] -- [0.1319997482373771, 0.6249562570759214, 0.7400126024362217, 0.7467712988221689, - -0.01759365042471705, 0.058961723148149285, -0.6622285148825786, -0.20936310435364122, - -0.308017137387598, 0.000999999999999997, 0.32848071317616745, -0.250231677603481, - -0.0009999999999999953, 0.13565149577121258, -0.14551450565391139, 0.0009999999999999887, - 0.13516661533763508, 0.25545505434364607, 0.2851477131510716, -0.4789610505174026, - -0.12147136249512126, -0.029712966904431478, 0.4579727465332278, -0.5462865525375578, - -0.09119613379527385, 0.1341268918040858, -0.34370955554459387, 0.4876218896215036, - 0.8494572895407762, 0.7935905768806234, -0.30514898455146006, 0.38780727277940596, - -0.05958871325209873, -0.342243909792466, 0.013822537887033736, 0.15580727206122136, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4724765717983246, 0.4799695909023285, -0.21030882000923157, - 0.6896644234657288, 0.6195845007896423, -0.13829578459262848, -0.22312995791435242] -- [0.12388468947617783, 0.6240014046336932, 0.7394887197710917, 0.7467755292021478, - -0.01917946282696508, 0.05756069510389333, -0.6623029696216658, -0.20786123179592683, - -0.30548399835025675, 0.0009999999999999998, 0.3394446619919095, -0.2636281962819842, - -0.0009999999999999966, 0.13078345931370908, -0.14121527967174471, 0.0010000000000000143, - 0.13945473780320286, 0.248302646729147, 0.2849567830545926, -0.471848740383914, - -0.12194286783523224, -0.014362458754069756, 0.46182779891434117, -0.5623695821622613, - -0.09790161616146667, 0.13940101306703445, -0.34701528555034067, 0.4784923944939802, - 0.8467752185312419, 0.7960429299619849, -0.30878560309262987, 0.38820366657894195, - -0.06047642129978315, -0.34164651589959555, 0.013498266703437982, 0.15765256793845575, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3725571632385254, 0.3930448889732361, 0.11997926235198975, - 0.4106133282184601, 0.7423286437988281, -0.016824007034301758, -0.629560112953186] -- [0.11500148021653313, 0.6229153302182961, 0.7387028456296124, 0.7472089246457604, - -0.020460021442266572, 0.0555079561797973, -0.6619509628764435, -0.20648336580132354, - -0.30139609672458123, 0.000999999999999997, 0.34984145911328285, -0.2761554238561865, - -0.000999999999999994, 0.12567384159222753, -0.13504862286836747, 0.0009999999999999896, - 0.14488339099125963, 0.23879672118841747, 0.28478867568593147, -0.4651062625718345, - -0.1259747207731113, 0.0018287210838222176, 0.46462933586867244, -0.5783214737093914, - -0.10388585890961448, 0.1450208014257762, -0.34812088682419934, 0.46874548882567474, - 0.8445782082147106, 0.7972442810986397, -0.3070542168525612, 0.3851534278229348, - -0.060896660365973965, -0.34071015794547294, 0.010361719510732966, 0.1598920674984317, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6516837477684021, 0.3801821172237396, -0.29905056953430176, - 0.48101648688316345, 0.3537856936454773, 0.12943299114704132, -0.455414742231369] -- [0.10604192366083873, 0.6216078564891023, 0.7378720600532492, 0.74764140251446, - -0.02171688794493896, 0.053613527356287244, -0.6615786421194506, -0.20488764862085848, - -0.2978884524324297, 0.0010000000000000015, 0.3604643657727112, -0.2867673870161536, - -0.0009999999999999976, 0.12032484236831051, -0.12778212375991382, 0.0010000000000000044, - 0.14942404579865637, 0.2302436298072348, 0.28423093869553206, -0.45926189791709604, - -0.12836547640552795, 0.01701692301477333, 0.46764382690224815, -0.594186614031953, - -0.11032938200575079, 0.1505928433624563, -0.34996314681393803, 0.46013892089879604, - 0.8431098449515494, 0.7984343291383168, -0.3060885418029419, 0.3831213235892779, - -0.06166354204555864, -0.3394101464072572, 0.00808997265122651, 0.16228801949702493, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.37822917103767395, 0.33603790402412415, 0.11980678886175156, - 0.3989201784133911, 0.44176462292671204, 0.007534881588071585, -0.2795381247997284] -- [0.09702421283359462, 0.6200725827973467, 0.736996785484987, 0.7480862814814709, - -0.02290357105760949, 0.05185586932484372, -0.6611757033562257, -0.2030646271269307, - -0.2949209719490547, 0.0009999999999999992, 0.37114686406035785, -0.2955048305991716, - -0.0010000000000000026, 0.11474002045992933, -0.11932037119959493, 0.001000000000000006, - 0.1530231416190171, 0.2225479635392089, 0.28322919849299083, -0.4545190419285826, - -0.1290291970384153, 0.0312289739462377, 0.4707803147670315, -0.6100946738594278, - -0.1171970822581168, 0.15616704428434344, -0.352645410116462, 0.4528370599064019, - 0.8424131171062914, 0.7997113894589934, -0.30593076620101395, 0.38214376608915857, - -0.06279468497442779, -0.3376875733877855, 0.0067344533076408915, 0.16481098719684872, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.36326202750205994, 0.281648188829422, 0.12085757404565811, - 0.3614368140697479, 0.4267965853214264, -0.18954798579216003, -0.4246158301830292] -- [0.08781088763630543, 0.6184231465924138, 0.7358091962053398, 0.7486045921229267, - -0.02392673190423759, 0.05004067471839657, -0.6606925207890408, -0.20128441029233252, - -0.2920920613962652, 0.001, 0.3815970501895027, -0.30276149551128306, -0.001000000000000014, - 0.10884299095443274, -0.11013652770206044, 0.000999999999999992, 0.15627716057486174, - 0.21575723547640918, 0.28272728180903856, -0.45108798034480063, -0.1297887897116032, - 0.04307186705036062, 0.4733747622691132, -0.625432772778861, -0.12376831442960821, - 0.16228756736327932, -0.35432013175031213, 0.4456898271074516, 0.8412712545194352, - 0.8007105354303133, -0.3050296577356503, 0.38092892130071854, -0.0641951945939808, - -0.3358170498662245, 0.005296267589224704, 0.16749729070887645, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.3620782196521759, 0.5029668807983398, -0.007622112985700369, 0.35090434551239014, - 0.563234806060791, -0.19966338574886322, -0.24577493965625763] -- [0.07838888550426448, 0.6166587751255311, 0.7342876016498128, 0.7492063681257702, - -0.02475056375761022, 0.04814953621837736, -0.6601203297243834, -0.19956189013956077, - -0.289378534506224, 0.0009999999999999976, 0.39172984205329575, -0.30847236802744205, - -0.0010000000000000195, 0.10262478873923303, -0.10018462947528656, 0.0010000000000000059, - 0.1591789196751668, 0.20988378381343464, 0.2827580646046079, -0.44919820103199354, - -0.1306574932566784, 0.05238708485315206, 0.4753416907723464, -0.6401908314540262, - -0.12999808339123506, 0.16900401673275023, -0.3549244062929734, 0.438771836074398, - 0.8396489162128244, 0.8014622365423232, -0.3033236376821452, 0.37943769305334896, - -0.06589028560205279, -0.3337674718357551, 0.003770796909343157, 0.17033397298020767, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6412706971168518, 0.38754546642303467, -0.4069325923919678, - 0.6771702170372009, 0.3931654393672943, -0.019464965909719467, -0.2984432578086853] -- [0.0688898636945503, 0.6147545587148261, 0.7327165108156248, 0.749896382316633, - -0.025306945893630008, 0.04627968407352427, -0.6594491376292929, -0.19795581206631363, - -0.2867524049817903, 0.0010000000000000028, 0.4013039409865862, -0.3127979161302358, - -0.0010000000000000067, 0.09616023144724561, -0.09004219990871169, 0.001000000000000011, - 0.1615105929493209, 0.20513033608113096, 0.28255998764642265, -0.44907904490232886, - -0.1306865553436088, 0.05992755807519392, 0.4762925440154194, -0.6544854068485456, - -0.1362010089594967, 0.17546648156769518, -0.3541994266388254, 0.4332580255215805, - 0.8385437710442225, 0.8030406685741854, -0.3018225977261312, 0.3779164216548239, - -0.06754720024702417, -0.33173258309078146, 0.0033156851441539337, 0.1726578792146879, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3967585563659668, 0.6773607730865479, -0.012067417614161968, - 0.6135303974151611, 0.580924928188324, -0.13522927463054657, -0.2923312485218048] -- [0.059301392450444045, 0.6127010097519522, 0.7311015278311862, 0.750692660023773, - -0.025539501715208895, 0.044412391162073385, -0.658662131558994, -0.19649313902621035, - -0.28416828609877204, 0.000999999999999996, 0.4101805999664205, -0.3156567936300866, - -0.0009999999999999896, 0.08942734054441545, -0.07972362783721616, 0.0010000000000000165, - 0.16324604221083547, 0.2015198258051262, 0.2821007909510606, -0.45103047973076793, - -0.12979623124600836, 0.0655499169752952, 0.4760504251082105, -0.6682797336666033, - -0.14235974318848854, 0.18164299586861155, -0.3520372712988401, 0.42938639320550626, - 0.8380120443270958, 0.805534587506724, -0.300544241410011, 0.3763149143776105, -0.06916073000628548, - -0.32968524316504627, 0.0040471678649811665, 0.174364468935249, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.31252920627593994, 0.4129663407802582, -0.03449169173836708, 0.3412918448448181, - 0.39469847083091736, -0.045778222382068634, -0.3096914291381836] -- [0.04988279173329014, 0.6105730145867866, 0.7292364127513182, 0.7515365128067525, - -0.02536679586012225, 0.04267172128874158, -0.6578210393323638, -0.19535100679607767, - -0.2822135590247233, 0.0010000000000000022, 0.41815030181216306, -0.31787089126101786, - -0.001000000000000001, 0.08292190286485836, -0.06991118509209374, 0.0010000000000000076, - 0.16428164791545266, 0.19962261404148654, 0.28162212549127763, -0.45386530825154364, - -0.129080284979108, 0.06773470838405006, 0.47587719432488057, -0.682834395711522, - -0.1482886089338815, 0.18751076380843773, -0.349203393616494, 0.426437373027849, - 0.8368406617080748, 0.8074867652722261, -0.2995636989274709, 0.37501037807695964, - -0.07042476657669572, -0.327638615773605, 0.004658129170247854, 0.17574144194451422, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5988530516624451, 0.03368779644370079, 0.03090924583375454, - 0.3060753345489502, 0.4573369324207306, -0.2688685655593872, -0.2976873517036438] -- [0.04064834817878536, 0.6083620543215513, 0.7271022575942038, 0.7524434773296854, - -0.024714123879500793, 0.04105458678871172, -0.6569113687617895, -0.19457081289043288, - -0.28092845028636315, 0.001000000000000002, 0.42503327539990005, -0.3194105790015289, - -0.0009999999999999968, 0.0766884127647454, -0.06071926772239631, 0.000999999999999997, - 0.16456074890512573, 0.19960126410278392, 0.2811141916113133, -0.4577090900342763, - -0.12856794299107377, 0.06605357402176103, 0.47572718320273144, -0.6982414022502024, - -0.1539455591270859, 0.19301956605621015, -0.34565008328309094, 0.4245514628241901, - 0.8349450645324124, 0.8088017989392876, -0.2989207047434201, 0.3739986579024523, - -0.07129319844574795, -0.32557820389225295, 0.005132362793419245, 0.17669463190975543, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3032926321029663, 0.45686185359954834, 0.17331162095069885, - 0.45193493366241455, 0.42581266164779663, -0.018009234219789505, -0.31981900334358215] -- [0.031623673188066644, 0.6062017623665595, 0.7253847921230275, 0.7532622042091466, - -0.023825291026343655, 0.03961127357289222, -0.6560940132507084, -0.19376379858962034, - -0.28016508995164907, 0.0009999999999999987, 0.4311099852744244, -0.31991948364658673, - -0.0009999999999999872, 0.0705890040298802, -0.053180095609136405, 0.0010000000000000005, - 0.16379545297183223, 0.2010696921443414, 0.28064496590421545, -0.46251265129218977, - -0.12641805321065783, 0.06113119885256214, 0.47496101956424225, -0.7126408301331654, - -0.15881109414360917, 0.19688896098955214, -0.3415978090833583, 0.4237017950980429, - 0.8336289576648117, 0.8115634734158443, -0.298506113093734, 0.3716953490708866, - -0.07164104805754754, -0.322718796361337, 0.00677626112496406, 0.1764241135158274, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3586740791797638, 0.7250373363494873, 0.02569057233631611, - 0.6047858595848083, 0.36947473883628845, 0.03339049220085144, -0.25038185715675354] -- [0.022838422737549827, 0.6040973769452949, 0.7241637553887077, 0.7539837718904782, - -0.022663684591324534, 0.038368476616573784, -0.6553798052493492, -0.1929420322044729, - -0.28001498791132223, 0.0010000000000000002, 0.4362525487252462, -0.3192369033134217, - -0.0010000000000000243, 0.06462864722737306, -0.04756896394504235, 0.0009999999999999998, - 0.16188235865143738, 0.20428921368203026, 0.28021423720398186, -0.46845576773415626, - -0.12239821640758453, 0.05245118883311877, 0.47348157235088717, -0.7258368844667512, - -0.1627570378852042, 0.19886156210902048, -0.3369711496351024, 0.4240825693630274, - 0.8329839354706391, 0.8159415835642246, -0.2983580753209354, 0.36781864871827313, - -0.07138432092956601, -0.31886417139308265, 0.009774649551525419, 0.17469673622207252, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4535175859928131, 0.5703408718109131, 0.027391286566853523, - 0.3703688383102417, 0.5999191999435425, -0.12312120199203491, -0.244378000497818] -- [0.01424804160749109, 0.60198700354633, 0.7236279936129147, 0.7546575480304172, - -0.021449883733914304, 0.03741212233351574, -0.6547000998862158, -0.19187233078382673, - -0.2800584653348294, 0.0010000000000000005, 0.44091589991558044, -0.31749119829671896, - -0.0009999999999999835, 0.058712945698437165, -0.04487369198093338, 0.0009999999999999935, - 0.15836461153865256, 0.20860432070402832, 0.2796622480947702, -0.47338039400958853, - -0.11416091080236836, 0.04108075713883086, 0.47173093355579604, -0.7375572600625653, - -0.16780435816776296, 0.19841823323078378, -0.33136352006511177, 0.4239257081221559, - 0.8337132747407389, 0.821392445196689, -0.2982819948045359, 0.3629054806598907, - -0.0707241813820987, -0.31433279376370166, 0.013199193417730596, 0.17192004782122772, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.43215709924697876, 0.6066935658454895, -0.22154633700847626, - 0.785950779914856, 0.42167404294013977, -0.19804657995700836, -0.1534649133682251] -- [0.005883971959550342, 0.5998671378051782, 0.7239264818274521, 0.7552751052984017, - -0.020177476612542943, 0.036810904955045916, -0.6540621851402428, -0.19055276152477738, - -0.28038356038346807, 0.0010000000000000018, 0.44504813923292874, -0.31446336260652163, - -0.0009999999999999972, 0.052798756618994684, -0.04559458817291071, 0.0010000000000000052, - 0.15302393444639506, 0.21432589465502427, 0.2789641902848987, -0.4770481751486112, - -0.10096745283055175, 0.026452684029852222, 0.469664995866323, -0.7474867924938182, - -0.17417104561473656, 0.19513282256676134, -0.324578259367329, 0.42307966957458304, - 0.8360706789175406, 0.8280457594948062, -0.2983000510571235, 0.35672926533594446, - -0.06958660336645989, -0.30892356976425855, 0.017127114725981735, 0.167861550730929, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3364584147930145, 0.443343847990036, 0.008958623744547367, - 0.4263250529766083, 0.4252053499221802, 0.05307133123278618, -0.2838016152381897] -- [-0.002135456135368429, 0.5978992888754375, 0.7231745831951683, 0.7558001027175235, - -0.01854162271883226, 0.03619039950808302, -0.6535385741810337, -0.1905127825291528, - -0.28312670738651563, 0.0009999999999999998, 0.4485078690887216, -0.3122756528029152, - -0.0010000000000000074, 0.0489997935342191, -0.04501615151552436, 0.000999999999999991, - 0.14988047129512053, 0.22329270383469177, 0.27849704477907505, -0.4799731557015636, - -0.09214470959413738, 0.00815213831087032, 0.4670981625711263, -0.7598703470572066, - -0.17792620933318842, 0.19268693866987796, -0.3175517756158253, 0.4221618026660547, - 0.8369560694469153, 0.8331350473120829, -0.29772235296388266, 0.3533054324308925, - -0.06960131576062709, -0.3055540634112382, 0.020905246584763556, 0.16662143088015655, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.31445571780204773, 0.5410348773002625, -0.1407734602689743, - 0.8388282060623169, 0.3774949014186859, -0.18047447502613068, -0.08239496499300003] -- [-0.009744244970624902, 0.5961222634298311, 0.7211256143506489, 0.7562127647371493, - -0.016483101574103383, 0.03552405726942275, -0.6531528176209696, -0.19191841767546947, - -0.2889298005444699, 0.0010000000000000009, 0.4511337528804202, -0.3110726695217926, - -0.0009999999999999987, 0.04785544810589405, -0.04281567999937012, 0.0010000000000000033, - 0.14925244247233968, 0.2364250746777778, 0.27831562559134515, -0.4819933194372865, - -0.08852684587067165, -0.014664234842882588, 0.4639454722379881, -0.7752692402965009, - -0.17851049044978684, 0.19122337945921938, -0.31023580835255815, 0.4211236809546237, - 0.8360554362497463, 0.8365012615336326, -0.2964036404472547, 0.35337907794990814, - -0.0710053557260216, -0.30485533999069464, 0.02450947090460909, 0.1688923539855249, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4498426616191864, 0.6566728949546814, -0.19464494287967682, - 0.771453320980072, 0.43005090951919556, 0.10662239044904709, -0.2589794993400574] -- [-0.017360199285731695, 0.5945043016672729, 0.720023099773463, 0.7567005514085217, - -0.014588876460101263, 0.035045570952456445, -0.6526585999879928, -0.19324822478001594, - -0.2943955291666253, 0.0009999999999999996, 0.45377977730842217, -0.3091576392754791, - -0.0009999999999999877, 0.050084610374592095, -0.04395656972426436, 0.0009999999999999944, - 0.14736383168559175, 0.24905185541225858, 0.27795520764652826, -0.4817049980874706, - -0.0779600150277471, -0.03548489428983042, 0.46154211600097844, -0.7878792281313769, - -0.17905162448338782, 0.18752498911444979, -0.30406528167008934, 0.4187832522082813, - 0.836374960404926, 0.8408076503467313, -0.2944965924664312, 0.3511056174096168, - -0.07125302457233967, -0.3029527264133405, 0.027559036786748377, 0.1697794272011836, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.7331083416938782, 0.6579992175102234, 0.027102844789624214, - 0.4018152952194214, 0.38251203298568726, -0.08675927668809891, -0.22494246065616608] -- [-0.024988593041431216, 0.5930827140637521, 0.7200848625321024, 0.7572674077770061, - -0.01292179989392873, 0.0348314719194757, -0.6520474436497063, -0.19452011911888537, - -0.29937638177089554, 0.0009999999999999996, 0.4565734563017154, -0.30630454394085604, - -0.000999999999999995, 0.05653682612892788, -0.04922697462910059, 0.0010000000000000152, - 0.1439703792462199, 0.2609379463717343, 0.27739188974026474, -0.47857239325007794, - -0.058851932048374585, -0.053777530592303745, 0.460127915221392, -0.7970143812458673, - -0.1795584771927233, 0.181040500089381, -0.29926886445046413, 0.4147821478307003, - 0.8382096829226702, 0.846219802842907, -0.29187386352397116, 0.3459516104496484, - -0.07006845739272995, -0.29950518375402785, 0.02990891427237245, 0.16900151617417744, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3836170732975006, 0.4338797926902771, -0.0006578384782187641, - 0.416094571352005, 0.5391724109649658, -0.07823365926742554, -0.2760413885116577] -- [-0.03284649160258474, 0.5917965193240372, 0.7204997699865362, 0.757722934280647, - -0.01114862403770413, 0.034706779830461144, -0.6515574437307814, -0.19583834249778748, - -0.3046288061090788, 0.0010000000000000009, 0.45976322335116365, -0.3027173167498267, - -0.000999999999999996, 0.06961723443007845, -0.055356409942917795, 0.0009999999999999894, - 0.14024724840561678, 0.2719409958694982, 0.2779768446596665, -0.4741839962727439, - -0.035287100046031396, -0.07065775428656447, 0.45897301151357717, -0.8052180314203901, - -0.1761821446314966, 0.1740765492844484, -0.294941985200008, 0.4118168047499389, - 0.8389210217657562, 0.8508123379779022, -0.28803557275021646, 0.3416364745486469, - -0.06901638676793297, -0.29706689363417516, 0.03168799317916927, 0.16853983457515828, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4518938660621643, 0.6399761438369751, 0.008895039558410645, - 0.367667019367218, 0.33950936794281006, -0.145926371216774, -0.28083524107933044] -- [-0.04103843936724243, 0.5906741785507659, 0.7213528537519014, 0.7580262495420553, - -0.009285441472329988, 0.03470951238757633, -0.6512336257685761, -0.1971044674039625, - -0.31002815866673833, 0.0009999999999999985, 0.46357066569518507, -0.2980043312849579, - -0.001000000000000002, 0.0913045235011351, -0.06270100686297668, 0.001, 0.13603796327612444, - 0.281475880207281, 0.28007083469747546, -0.46818726202622757, -0.006156903837558083, - -0.08557522056759602, 0.45821633923375166, -0.8121839014293101, -0.16761180119385255, - 0.16640145936633768, -0.29115415254943533, 0.41016741800866996, 0.8382086099703236, - 0.8543764909603199, -0.28264568813653945, 0.3385513392158956, -0.06813755056535781, - -0.29608173929678966, 0.03274642760784793, 0.1686440041021169, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.3247949481010437, 0.19082234799861908, -0.17665883898735046, 0.36627084016799927, - 0.648851215839386, -0.22097329795360565, -0.28713345527648926] -- [-0.049216401227002, 0.5894917664125026, 0.7222968778602479, 0.7582749520678775, - -0.007671292345595571, 0.03478726592124361, -0.6509609008764892, -0.1989840397367838, - -0.3149836565488517, 0.0009999999999999996, 0.46598295486722663, -0.29207347488647684, - -0.0009999999999999953, 0.10835578519490559, -0.06950215108570273, 0.0009999999999999994, - 0.13296653199350997, 0.29033994671142166, 0.2830277338281065, -0.45946759251256397, - 0.021018642408466123, -0.0978108753250221, 0.45814781728403897, -0.818517125784847, - -0.161621988514382, 0.15874347474688988, -0.28793011933351287, 0.4078227620629838, - 0.8367977568055831, 0.8562821089370669, -0.27785807642209603, 0.3359967617963245, - -0.06705787143329399, -0.2957544714788803, 0.03322438690770379, 0.16926381024264084, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5081801414489746, 0.6425208449363708, -0.0050923568196594715, - 0.6179983019828796, 0.4072761833667755, 0.06243659555912018, -0.31194180250167847] -- [-0.05729879632073907, 0.5882518639511405, 0.7233252292069398, 0.758461709479996, - -0.006350157655405676, 0.03493291632857493, -0.6507497230942281, -0.2018123326591722, - -0.3193009319464243, 0.0010000000000000002, 0.4664272741826162, -0.28475809339380953, - -0.000999999999999996, 0.11908470974171827, -0.07561648971877098, 0.0010000000000000054, - 0.1313464954818591, 0.2980796680526943, 0.2870900534440912, -0.44721414690708466, - 0.04591626845419739, -0.10636465073996632, 0.4589058842544061, -0.8242835927320986, - -0.15926464308826763, 0.15121747851578118, -0.2854755995839971, 0.4045591374226469, - 0.8344804597837605, 0.8561102627369626, -0.2738500555212438, 0.33430427757790654, - -0.06572830148393471, -0.2963983919082282, 0.03294775758498589, 0.17068134120119452, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3002541959285736, 0.6964874267578125, -0.04884830489754677, - 0.4417031407356262, 0.4116038978099823, -0.20954298973083496, -0.25367051362991333] -- [-0.06487591519567881, 0.5869986060859519, 0.724304702605871, 0.7585560798315695, - -0.0052023049637933225, 0.03509598501136523, -0.6506411312003773, -0.2052400795005199, - -0.3233291437479045, 0.0010000000000000005, 0.4639427122519689, -0.2771110733959807, - -0.0010000000000000076, 0.12161299956591975, -0.08085769396967542, 0.0009999999999999933, - 0.1311512681514418, 0.30532531325847706, 0.2917088877158404, -0.43323754816194543, - 0.06502465147551424, -0.11407093022648417, 0.4604612190446228, -0.8295517438058839, - -0.1615670582415788, 0.14377835107725417, -0.28408360261090243, 0.4008395378799749, - 0.8317911192421098, 0.8559475313312962, -0.27006157449084717, 0.3336449948756483, - -0.06426353300065084, -0.29726219001038456, 0.032314899600623816, 0.17119517030558618, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5090030431747437, 0.30536893010139465, 0.1315968632698059, - 0.49033835530281067, 0.7920419573783875, -0.012338093481957912, -0.29448291659355164] -- [-0.07174472306247001, 0.5857481183246442, 0.7251974462505658, 0.7585364422807807, - -0.00426200178055112, 0.03528230925501594, -0.6506607869900191, -0.20952154997719502, - -0.3270062209164536, 0.0010000000000000009, 0.4573942816754809, -0.26911630036659245, - -0.0010000000000000076, 0.11306683248656461, -0.08496446906915778, 0.0010000000000000015, - 0.13275605604537186, 0.31183568961647473, 0.2970362036286573, -0.4170382181780045, - 0.07648891900752743, -0.12055458617696078, 0.4630533340564372, -0.8342604382231149, - -0.17029626964080635, 0.13651551544509244, -0.28411644962817195, 0.3965434874877551, - 0.8286071195800983, 0.8558627544037306, -0.2665659800339081, 0.334440343220978, - -0.06262439880464608, -0.2984521741032063, 0.031207713310936896, 0.17053761145138294, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4263420104980469, 0.553597629070282, -0.12025768309831619, - 0.3989822566509247, 0.581350564956665, -0.1435377150774002, -0.31804266571998596] -- [-0.07883486706472057, 0.5846430515956733, 0.7259200631403694, 0.7584931703380684, - -0.0032887381514862123, 0.03534171899926269, -0.6507136525769643, -0.21396848551658879, - -0.32994051228677534, 0.000999999999999999, 0.4498455712543633, -0.260586246124337, - -0.0010000000000000018, 0.10444266031945806, -0.08904346833123, 0.001000000000000002, - 0.13567587780338258, 0.31631612179497703, 0.3029697685695305, -0.3990528847694828, - 0.08226876209552494, -0.12551303878560005, 0.4664869590381824, -0.8380958770096844, - -0.17787534336344202, 0.1292013061947511, -0.2860895107276633, 0.3911247397334066, - 0.8242912438928701, 0.8536488668184666, -0.2626465924768337, 0.33464421817198003, - -0.06113569583566646, -0.30066771908443396, 0.029534295439623073, 0.16923980265469116, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3686196208000183, 0.20634181797504425, 0.04161209985613823, - 0.5527176856994629, 0.384952187538147, -0.33943238854408264, -0.1995936632156372] -- [-0.0862167459655231, 0.583730236365702, 0.7264098932791382, 0.7584208231469899, - -0.002264839175652069, 0.03519427720945512, -0.650810332103183, -0.21866104980647708, - -0.331660500939462, 0.0010000000000000005, 0.44087104511890285, -0.25137034245301537, - -0.0010000000000000005, 0.09576422855803601, -0.09308731989777719, 0.0009999999999999926, - 0.14046324467393972, 0.31766516085356095, 0.3097368788742184, -0.37843218206403345, - 0.08037427590570806, -0.12820351574086625, 0.47105197860695236, -0.8408480765707846, - -0.18361951968154314, 0.12184202668420932, -0.2907387328529098, 0.3841032371967053, - 0.8184343710414537, 0.848353817727884, -0.2581545494836939, 0.3341554944325686, - -0.05985480191486466, -0.3044334015897167, 0.027088200638252388, 0.1671655998332598, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.48366954922676086, 0.6286837458610535, 0.01868809387087822, - 0.6518809795379639, 0.4518691897392273, -0.012481511570513248, -0.28445562720298767] -- [-0.09382975277399772, 0.5829525605785844, 0.7271226194621274, 0.7584513179872782, - -0.0011356247496932886, 0.0349228553793023, -0.6507923653300983, -0.2227765546813827, - -0.3324652362409995, 0.0009999999999999994, 0.43164384422204993, -0.24196908060501815, - -0.0010000000000000018, 0.09260811905944573, -0.0974300721078388, 0.0009999999999999846, - 0.14541024865344182, 0.31687587534911316, 0.3162924052324055, -0.35771238610597506, - 0.07811944668657063, -0.12895816232224488, 0.47547839563870103, -0.8424302621565213, - -0.18426675110622082, 0.11405532068544223, -0.29609821881485465, 0.37739740550022133, - 0.8126715393024155, 0.8429103031258292, -0.2527797277453025, 0.33366945084006355, - -0.058897656221780476, -0.3088726941607707, 0.023952739342331518, 0.16495740701574724, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4063132703304291, 0.631901741027832, -0.08417074382305145, - 0.5511011481285095, 0.1827193945646286, -0.2993633449077606, -0.18052272498607635] -- [-0.1017338387681333, 0.5823680193981078, 0.7281426918401431, 0.7586618128866464, - 0.00019396273294850435, 0.03442167560297565, -0.6505746416013215, -0.22621803503136143, - -0.33189598505773077, 0.000735351599977351, 0.4219030614787104, -0.23256257770130748, - -0.0010000000000000074, 0.09696466707458823, -0.10218765492882512, 0.0009999999999999963, - 0.1507853786751008, 0.31290839143136856, 0.32261151629685825, -0.33703837322426244, - 0.07534953808434464, -0.12690057216508796, 0.47952199135395024, -0.8425466096013893, - -0.17769070081228938, 0.10576009932351896, -0.30238817135805307, 0.3713571865953285, - 0.8070353356744668, 0.8371836941525957, -0.24617630126839649, 0.33324688602063723, - -0.0583972244843825, -0.31435055275462576, 0.019859164279249247, 0.16263521870696127, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40541282296180725, 0.718976616859436, -0.0691426545381546, - 0.3847941756248474, 0.415260910987854, -0.15061026811599731, -0.12379642575979233] -- [-0.10983746030804778, 0.5817357709690367, 0.7292798376970726, 0.7591021471504605, - 0.001714156798743998, 0.03393025062864513, -0.6500844021742909, -0.22991144669505892, - -0.3306883492329635, -0.0009999999999999996, 0.41122317901016264, -0.22345380800544226, - -0.001000000000000002, 0.10219892596297571, -0.10787057257101847, 0.0010000000000000083, - 0.1568969186028995, 0.30767007277544156, 0.328996249181327, -0.31841821207810067, - 0.07069568059615747, -0.1244277882138993, 0.4827418381578933, -0.8412020471870714, - -0.1701113589438373, 0.09787207874302761, -0.3074787858717611, 0.36704350356778637, - 0.8016108820146718, 0.8315833867666236, -0.23935201933172442, 0.33336792272546584, - -0.058204789298748186, -0.31998712749287334, 0.015812744862204554, 0.16070880966646045, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.29529792070388794, 0.3684244155883789, 0.09898333996534348, - 0.42284727096557617, 0.368879109621048, 0.05465427041053772, -0.3656337857246399] -- [-0.11828992803410311, 0.5811949945703797, 0.7305764749136276, 0.7595733688657769, - 0.003278849843859019, 0.033267989306445055, -0.6495619965338975, -0.23326491071458524, - -0.32836518538387655, -0.0010000000000000009, 0.3997930459376797, -0.21406215164543524, - -0.0009999999999999907, 0.10942298629367463, -0.11408459216786515, 0.0010000000000000074, - 0.1644420891530908, 0.3002032108027345, 0.334633598229625, -0.2994454299242412, - 0.06317783954607803, -0.12081075992769379, 0.4857995638405911, -0.8387159049950285, - -0.1612373434556025, 0.09036928205442299, -0.31368637624059276, 0.36203500302184716, - 0.7964796148585543, 0.8258535138485731, -0.23219808472691605, 0.3344513576457422, - -0.05844146367832216, -0.3261230783673625, 0.011843495790290298, 0.15954984623438084, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5878175497055054, 0.36784014105796814, -0.06602686643600464, - 0.41867774724960327, 0.36921966075897217, 0.12810005247592926, -0.25259676575660706] -- [-0.12668620856771567, 0.5805866920972359, 0.7319462193244183, 0.7600488668058291, - 0.004987782629357284, 0.03267278595231271, -0.6490249079578789, -0.2365472815942159, - -0.3263037760400103, -0.0009999999999999992, 0.3872249100638452, -0.20490631056406913, - -0.0009999999999999946, 0.11593800923806184, -0.1204672930640045, 0.0010000000000000085, - 0.1724161418761933, 0.29287984033671294, 0.3394269737828785, -0.282162909863206, - 0.05394805621437142, -0.11831375014467563, 0.4888088988709766, -0.8351390905928262, - -0.15250316592092278, 0.08267971172841257, -0.3213204863460773, 0.35812177088979086, - 0.792048773085682, 0.8213070829370007, -0.22705769312378465, 0.3357781820012979, - -0.05973689076972814, -0.33290376019654216, 0.009553529068630946, 0.15792192420524462, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.35594525933265686, 0.48310691118240356, -0.00907235685735941, - 0.39650535583496094, 0.5359171032905579, -0.14977356791496277, -0.292618066072464] -- [-0.13499480440858877, 0.5798980279258068, 0.7334274357497212, 0.7605082662746907, - 0.0068536457402511445, 0.03221919567721978, -0.6484921957108233, -0.2396735708027759, - -0.3247419851862151, -0.0010000000000000002, 0.37313789592569124, -0.19593892635277171, - -0.0010000000000000054, 0.12146589538817391, -0.12710835865158493, 0.0009999999999999987, - 0.18083206647792677, 0.28594818812903106, 0.34300225700977294, -0.26763201196046826, - 0.04236642350935909, -0.11751020237303568, 0.49183805242640793, -0.8298526086676695, - -0.1440051498266399, 0.07469218875490673, -0.33094848497022566, 0.3558897234620158, - 0.7886428597593433, 0.8187625766052473, -0.22484171713881668, 0.3373869321593882, - -0.06254336724235948, -0.34046213104023915, 0.009722837769629152, 0.15555341071639534, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3177318572998047, 0.46394744515419006, 0.17770054936408997, - 0.6041872501373291, 0.47767576575279236, -0.16600222885608673, -0.3421078026294708] -- [-0.14289020890840923, 0.5792700044551793, 0.7345507415071165, 0.7611087590493189, - 0.00883966115775008, 0.03133295496987086, -0.6478067329241638, -0.24231249887872747, - -0.32259611722106996, -0.0010000000000000007, 0.3573859549141354, -0.18823127246996363, - -0.001000000000000005, 0.1279380027463904, -0.13210985659212474, 0.0009999999999999918, - 0.19006708195123187, 0.2783517158456312, 0.34665814451279703, -0.254332522143531, - 0.029526913158676588, -0.11445451947175972, 0.4938832605515298, -0.8253929983162106, - -0.13532981712528336, 0.06744547971453393, -0.3376460158601236, 0.3529918177385284, - 0.7847628538915232, 0.8151162684559209, -0.2176697264354656, 0.33725509678539134, - -0.06406864823983958, -0.3462551096464787, 0.006549875990126951, 0.15347228193475626, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.46061256527900696, 0.4071749746799469, 0.01990673691034317, - 0.3936096131801605, 0.5827261805534363, -0.09907320141792297, -0.2726839780807495] -- [-0.15016888367472747, 0.5787563881867103, 0.7351549116447345, 0.7619039998403461, - 0.010969852043349177, 0.02983393110902721, -0.646909494386975, -0.24418853097359153, - -0.3196992660753868, -0.0009999999999999998, 0.3392673255282055, -0.18233022314222347, - -0.0010000000000000037, 0.13582589871356235, -0.13472755251645224, 0.000999999999999995, - 0.20042403709240805, 0.2699646608755582, 0.3504843566095415, -0.2429636510416222, - 0.014903239336860209, -0.10805095134815441, 0.49453766321745907, -0.8220248341633006, - -0.12638089554415044, 0.0612449056713249, -0.3398707889456836, 0.3490951201825552, - 0.7801722882874329, 0.8099926741734476, -0.20310885721954075, 0.3346277877083337, - -0.06377921002526017, -0.34934289778466543, -0.0016273334144454304, 0.15187974105520693, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.13265928626060486, 0.3997211158275604, -0.07170630991458893, - 0.4036933481693268, 0.5363414883613586, -0.30203843116760254, -0.32340526580810547] -- [-0.1571827021509813, 0.5782061032085557, 0.7358530959209086, 0.7626407704628809, - 0.013103313849957469, 0.02844468160666354, -0.6460636644187854, -0.245736945946515, - -0.3172039140168231, -0.0010000000000000002, 0.32078215506942653, -0.17666191660759084, - -0.0009999999999999937, 0.14292478739728842, -0.13738653675281687, 0.0009999999999999953, - 0.20999009858116155, 0.26217291321277697, 0.35344892267019185, -0.23274745842603825, - 0.0007386534214256509, -0.10293542841744914, 0.4951537843861673, -0.8182225935645068, - -0.1173471205977677, 0.05523000555873908, -0.34365133329313474, 0.3458642737730574, - 0.7764582003826715, 0.8057726440212951, -0.19078105323232772, 0.3321373008614891, - -0.06435740861023265, -0.352746053672492, -0.0080836962414347, 0.149893706746499, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3906507194042206, 0.5431658625602722, 0.023600518703460693, - 0.4637158215045929, 0.5729851722717285, -0.14668700098991394, -0.3038277328014374] -- [-0.16384988483900104, 0.5776101401485066, 0.7366976085781171, 0.7632515091118919, - 0.015113445983246354, 0.02736080040490381, -0.6453449497672802, -0.24661950432538146, - -0.3154695199772899, -0.0010000000000000015, 0.30218372961150763, -0.1708052242377359, - -0.0009999999999999957, 0.1489058539844738, -0.14021728803424388, 0.0009999999999999957, - 0.2176964908534513, 0.2554867279086216, 0.35515182062957973, -0.22462375605607018, - -0.012290618509967348, -0.09990012531409283, 0.4959985675469613, -0.8134153066793649, - -0.1083192448081529, 0.04931912354090699, -0.34956936621472373, 0.34374279445374006, - 0.7740988687997948, 0.8032424373729055, -0.18184592066476196, 0.3298004516185074, - -0.0661222567757614, -0.35647898232831665, -0.0118633083392225, 0.14726487013828837, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5958952903747559, 0.40038105845451355, -0.48244714736938477, - 0.371391624212265, 0.3613201379776001, -0.4986889958381653, -0.5326074361801147] -- [-0.1701175096876121, 0.5770509777018996, 0.7373976967391644, 0.7638279866103251, - 0.01703803781689006, 0.026333047586145113, -0.6446573374615326, -0.24710260803628747, - -0.31394094593544225, -0.0010000000000000007, 0.2837344081147113, -0.16538696546052598, - -0.0010000000000000052, 0.15475871267463998, -0.14292700735806002, 0.0009999999999999803, - 0.2245616318415895, 0.24932875185569867, 0.35677039533476484, -0.21661104582475016, - -0.02546602752686233, -0.09834409756593074, 0.4971619050257683, -0.80885717889555, - -0.0996408746695473, 0.04389667545750374, -0.35549471292078405, 0.3411552404183947, - 0.7716505748599778, 0.8008717356729397, -0.17379444116624243, 0.32849902889000615, - -0.06811565912525142, -0.3602425881516367, -0.015299238295343937, 0.14498904859955927, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.41918694972991943, 0.7504457235336304, -0.08163828402757645, - 0.6894639730453491, 0.4145931303501129, -0.24311895668506622, -0.2031557261943817] -- [-0.17585526648111466, 0.5765533938133998, 0.7378850143026386, 0.7643287733456979, - 0.018730358536073525, 0.025500966264869473, -0.6440499985440009, -0.2468639440474682, - -0.31280755485756606, -0.0009999999999999998, 0.2659386528867595, -0.16016930127030737, - -0.001000000000000013, 0.16040106666306936, -0.14557712315094806, 0.0010000000000000104, - 0.2296628129641909, 0.24408834130691942, 0.35828924624089953, -0.20893530516645048, - -0.03853539228872919, -0.09916233701925949, 0.49904854558084333, -0.8043237651341719, - -0.09166632773586833, 0.03902349354844371, -0.36123658040126855, 0.3379007622999586, - 0.7690656188730682, 0.7988286002612912, -0.1671172593285542, 0.328780333185377, - -0.07043098505382574, -0.36396093896127185, -0.018197737244944022, 0.14325865840595467, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.15809349715709686, 0.5055763721466064, 0.03637063130736351, - 0.40229710936546326, 0.4572201073169708, -0.11728719621896744, -0.3346266448497772] -- [-0.1811663702120102, 0.5761184482485308, 0.7384451161313542, 0.7647725155806493, - 0.02034418699512441, 0.02472910475615839, -0.6435041451660669, -0.2462705441663041, - -0.31195340258448934, -0.001000000000000001, 0.24873337824202124, -0.1553622306435752, - -0.0010000000000000046, 0.16613620963104356, -0.1480829417382816, 0.0010000000000000013, - 0.23378355441792562, 0.23941856176010354, 0.3594189635240247, -0.20215186635872948, - -0.0505354095763333, -0.10040887182782039, 0.5009064495223694, -0.7997717696963935, - -0.08378100390372348, 0.034593093772316875, -0.36680403559631386, 0.3350522520417689, - 0.7668713201633949, 0.7980590455493213, -0.16095141676287486, 0.32911338533318424, - -0.07246491094824373, -0.36753999915140917, -0.021070833372757933, 0.14209834232165944, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3100137412548065, 0.679489016532898, -0.18350957334041595, - 0.3797090947628021, 0.5833578109741211, -0.08306606858968735, -0.2723802626132965] -- [-0.18589825841130556, 0.5757875998261812, 0.7391368141595678, 0.7651126695255158, - 0.02175851979747362, 0.024127697270355273, -0.6430762194111924, -0.2450598050673936, - -0.31156865689414726, -0.0010000000000000022, 0.2328360811747235, -0.15082498733243907, - -0.0010000000000000028, 0.17199998995984764, -0.15050477727438002, 0.0009999999999999968, - 0.23606462527110836, 0.23567256747097481, 0.35996146675708807, -0.19693923063394597, - -0.060589651492168814, -0.10235347871616253, 0.5028745217885763, -0.79487888545744, - -0.07620109069009687, 0.030653865411124578, -0.37196408882102816, 0.3329183522876397, - 0.7653074415061435, 0.7994119787057111, -0.15558890650549398, 0.32953053360415585, - -0.07402675956330147, -0.37087199030248785, -0.023905579238452542, 0.1418695959302794, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.2211790233850479, 0.19619564712047577, -0.3043927550315857, - 0.4075949192047119, 0.5865529775619507, -0.1876332014799118, -0.26631397008895874] -- [-0.1902780505955726, 0.575511639837221, 0.7397071207077222, 0.7654791508772528, - 0.023126098727240866, 0.02358476890484457, -0.6426123339974134, -0.2439679850658075, - -0.3114356575589139, -0.0010000000000000013, 0.2179525169431804, -0.14674797639349899, - -0.0010000000000000148, 0.17758243169761326, -0.15293731413839204, 0.0010000000000000007, - 0.23805438860632439, 0.2324780617702876, 0.3603567678094762, -0.19139417682349205, - -0.07012329317566994, -0.10476686538469913, 0.504878331180581, -0.7903089638367625, - -0.06878547399488204, 0.02708228206990804, -0.3772191044350401, 0.3305196470060946, - 0.7636246657409056, 0.800277629974362, -0.1504180838435688, 0.33031091319849626, - -0.07568787327197879, -0.3744634694562959, -0.026051316313645972, 0.14123333615017775, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.41232386231422424, 0.42181286215782166, 0.02131127007305622, - 0.5782487392425537, 0.45704585313796997, -0.13939537107944489, -0.23560035228729248] -- [-0.19416088855642324, 0.5753223379434326, 0.7400920007421034, 0.7658838948219165, - 0.024373643270812304, 0.02318192622986043, -0.6420984219435448, -0.2430840515001319, - -0.31171194514588757, -0.0009999999999999972, 0.20499044605206404, -0.14317238768633786, - -0.0009999999999999972, 0.18270016933411293, -0.15549571294965214, 0.0010000000000000074, - 0.23948563750270055, 0.230173294057934, 0.36053131868358407, -0.18536007646885796, - -0.0786801776621705, -0.10795305751958174, 0.5070371672629228, -0.7860472361448725, - -0.061750219335914285, 0.023951885548519528, -0.38255519394089177, 0.3277373632332511, - 0.7617498349778731, 0.8003193211169226, -0.14555113240365, 0.33162875402348213, - -0.07749159486476641, -0.37844846092823164, -0.027063894017931057, 0.13989890359933696, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.539772093296051, 0.4557092487812042, -0.29145991802215576, - 0.7874041199684143, 0.4224141538143158, -0.05090416595339775, -0.4481375217437744] -- [-0.19773524066186102, 0.5751673650280626, 0.7404473191265577, 0.7663208607187061, - 0.02561819349212374, 0.022818525942443147, -0.6415413949725746, -0.2423919695875704, - -0.3123504221242015, -0.0009999999999999963, 0.1931417557910558, -0.14004332430079738, - -0.0010000000000000022, 0.18728760856507412, -0.15799645556772804, 0.0010000000000000256, - 0.24086595984688114, 0.2286492230425301, 0.3605318840849515, -0.1803721663507595, - -0.08657946901150777, -0.11126926012709995, 0.509214664759157, -0.7823658140739616, - -0.054845982792452154, 0.020936280053192508, -0.387553366854675, 0.32597097599263336, - 0.7597954624907984, 0.8012236015830255, -0.14115595152121294, 0.3333975537803413, - -0.07922813122727713, -0.3823036083777099, -0.02809336563271955, 0.13966245732466886, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.30232828855514526, 0.34920549392700195, -0.1163557693362236, - 0.4989991784095764, 0.5989475846290588, -0.03074485808610916, -0.2469678372144699] -- [-0.20085968370173035, 0.5750692561576752, 0.7407654868843071, 0.7668125246338611, - 0.026834564838018826, 0.022536350051210182, -0.6409138562404215, -0.24204733261440925, - -0.31356813092297603, -0.0010000000000000002, 0.18333835805619716, -0.13750930093632485, - -0.000999999999999991, 0.19098299382706685, -0.16047988497618026, 0.000999999999999995, - 0.24217693398335594, 0.22838103554138825, 0.3602620655958433, -0.17717952889672542, - -0.09331399728113345, -0.11477650837528232, 0.5114697777194588, -0.7795202263029121, - -0.04824773709859678, 0.017984804363095387, -0.3919569856695318, 0.32593991488064616, - 0.757707807270682, 0.8035794407386853, -0.13754976556444795, 0.3359367313136381, - -0.08084272480325772, -0.38594748932661427, -0.029156492185347126, 0.14130127122250105, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5204397439956665, 0.6062929034233093, -0.00892680324614048, - 0.7321615815162659, 0.2768024504184723, -0.0932355597615242, -0.1536238044500351] -- [-0.2036931477686343, 0.574972598122437, 0.7412212897638547, 0.7673513201994069, - 0.028093955060190533, 0.022286078875373896, -0.6402234076989565, -0.24190156147860606, - -0.3149422654839981, -0.0009999999999999987, 0.1745161570785527, -0.1354963941806758, - -0.0010000000000000098, 0.19405828365826086, -0.16333783698668475, 0.0010000000000000013, - 0.24358908147712377, 0.22860332522649238, 0.359428979431277, -0.1749498966084256, - -0.099177419890951, -0.11858269929747267, 0.5134971968960215, -0.7767754104726128, - -0.04204494545863534, 0.015153691018999984, -0.3964400486911288, 0.32658317255511665, - 0.7562308714328881, 0.8071513952374358, -0.13440042499338442, 0.3384938991392255, - -0.08254172441849356, -0.3890586491444944, -0.0299457512388924, 0.1431042857596736, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4390285909175873, 0.7105002403259277, -0.2822950482368469, - 0.958102285861969, 0.16429848968982697, -0.041812434792518616, -0.24175573885440826] -- [-0.20606373290437194, 0.5748804119012412, 0.7419221806635841, 0.7679734843941295, - 0.02941780877792386, 0.022093581499487272, -0.6394241107831105, -0.2421132299834917, - -0.3165713733013506, -0.0009999999999999998, 0.1674743866867572, -0.13430749548392332, - -0.0009999999999999913, 0.19608267566530338, -0.16688289368020615, 0.001, 0.2452012516618235, - 0.22964510700932653, 0.35764416199033333, -0.17438464137623602, -0.1035092221948357, - -0.12288908372608567, 0.5151461017820215, -0.7741346804711726, -0.03656504042194431, - 0.01246180578578883, -0.40105236510252257, 0.32840022073561326, 0.7558072131659865, - 0.8127860245044752, -0.13203206454992306, 0.3410687289858847, -0.0843779005092828, - -0.391247192130668, -0.030265594353168345, 0.14517346648185453, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.4814125597476959, 0.4380142092704773, -0.09557973593473434, 0.6075800657272339, - 0.4002794325351715, -0.20563678443431854, -0.11841390281915665] -- [-0.2081180943878777, 0.5747909924905584, 0.7425272509586868, 0.7686000694359423, - 0.03073190475927352, 0.022151289029282144, -0.638606924239999, -0.24286538137151503, - -0.31889216246746677, -0.0009999999999999979, 0.16142985608686336, -0.1333463213529464, - -0.0010000000000000145, 0.19672503140392109, -0.17107072502740406, 0.0009999999999999799, - 0.24696377074243167, 0.23169881031518696, 0.35565335463785785, -0.17403405319216217, - -0.10680237874530539, -0.12919309657347325, 0.5171722258903646, -0.7718268141394299, - -0.03218295940527388, 0.009347502875616363, -0.40647472212967894, 0.3305765203647245, - 0.7550899105224589, 0.8190918104057822, -0.13117492918466334, 0.3445503480146021, - -0.0871211936851015, -0.3936143347869044, -0.029580248241838067, 0.146945165277096, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.40386244654655457, 0.45788657665252686, 0.1381475329399109, - 0.40199989080429077, 0.8155123591423035, -0.1780775934457779, -0.2799922823905945] -- [-0.20964912050471768, 0.5747036147699419, 0.7429670311373783, 0.7692398348626815, - 0.03203743284278164, 0.02263981011907818, -0.6377547478106692, -0.24459864918266894, - -0.32242492654279054, -0.0009999999999999998, 0.1571718668643382, -0.13277304176633137, - -0.0009999999999999907, 0.19493334887883534, -0.17640717786452764, 0.0009999999999999974, - 0.24902934609215477, 0.2355330531450561, 0.35329829858159273, -0.17407507839811698, - -0.10824470210557403, -0.13902801536531784, 0.5198490515467565, -0.7700659868747632, - -0.029777071142873788, 0.005433141058518878, -0.413346998995438, 0.3334165860804574, - 0.7538543808039805, 0.8264812375798485, -0.1329764545303895, 0.3495957068935927, - -0.09146577512312255, -0.3962785462339973, -0.027126459743127412, 0.1481685922900809, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5188000202178955, 0.3649124503135681, 0.2421163022518158, - 0.7672780752182007, 0.7620994448661804, -0.2150706946849823, -0.23622603714466095] -- [-0.21106390615160422, 0.574700807258152, 0.7433795946741214, 0.7699774745174506, - 0.03336025320964048, 0.02273744419740044, -0.6367925807300892, -0.24624682816674023, - -0.3250325863854919, -0.0010000000000000009, 0.15304049036683237, -0.13286553891936023, - -0.000999999999999995, 0.19279311741303506, -0.18113755472717413, 0.0010000000000000122, - 0.2521370607031587, 0.23831794480100765, 0.35087256487078616, -0.17482691748802973, - -0.11032100894662276, -0.14679026766682324, 0.5222582137299614, -0.768295553552877, - -0.027634199642080907, 0.002205952677652072, -0.4196902313482333, 0.3359762871221483, - 0.7526846446856806, 0.8343730546290161, -0.1329923489998158, 0.3533917788929362, - -0.09507486488316666, -0.39858978566515413, -0.02548710364200107, 0.14909660029136582, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.42548075318336487, 0.49406227469444275, -0.08828715234994888, - 0.43875652551651, 0.3628089427947998, -0.030956679955124855, -0.25648054480552673] -- [-0.21224553977731034, 0.5748526998421861, 0.7437444070321024, 0.7708926691413036, - 0.034700796781637705, 0.022126044556909023, -0.6356341601260755, -0.2477091747520527, - -0.3259363522459431, -0.0009999999999999998, 0.149125529716888, -0.1341715705138161, - -0.0009999999999999885, 0.19004614691149224, -0.1848165773518931, 0.000999999999999994, - 0.2571396890797023, 0.23915506231129693, 0.3483567292891906, -0.17675348658458664, - -0.11356739112402608, -0.15075252953298246, 0.5242298551953514, -0.7665729950318575, - -0.025933166533969723, 0.00026527489106255775, -0.4250564364315563, 0.3379872481451268, - 0.7516329947904781, 0.8431909868360551, -0.12979520307093367, 0.35497823352589125, - -0.09734992760187468, -0.4003335447513552, -0.025314411266541707, 0.14951077615449399, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.6193217039108276, 0.5021198987960815, -0.13339169323444366, - 0.5469133257865906, 0.5027331709861755, -0.1859203726053238, -0.17823302745819092] -- [-0.21333580909017358, 0.575028224860998, 0.7440537967296825, 0.7718092166318927, - 0.03600564024775353, 0.02147701626118551, -0.6344705389259055, -0.24923525021755627, - -0.32680374395752376, -0.0009999999999999996, 0.14565387365590238, -0.13557858026378228, - -0.0010000000000000009, 0.18682655778312227, -0.18822345283557154, 0.0010000000000000007, - 0.2625338754445692, 0.23992429839852958, 0.3456227482560996, -0.17944115181122341, - -0.11664706861487616, -0.15408339972308221, 0.5260811842264376, -0.7648891321520843, - -0.02495672437258239, -0.001558249530942731, -0.430236682363676, 0.34035659440314137, - 0.7509348038015533, 0.8518434340781235, -0.12616277012292312, 0.35582781186344875, - -0.09971938353712353, -0.4016124386162636, -0.02528942634210906, 0.14977800546540063, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4840320348739624, 0.5863711833953857, -0.02744484879076481, - 0.699508011341095, 0.3803307116031647, -0.19361820816993713, -0.25176194310188293] -- [-0.2142637223962827, 0.5752453687491016, 0.7442593908293544, 0.7727324921397679, - 0.03724080991845818, 0.02074912314189139, -0.6332985801001969, -0.25086786047067944, - -0.3276053810285823, -0.0009999999999999994, 0.14300503583453586, -0.13717342919346084, - -0.001000000000000004, 0.18273067578463728, -0.19114015304347123, 0.0010000000000000015, - 0.268630606730675, 0.24056552907064932, 0.342501061664294, -0.18351535784457307, - -0.11940736336383524, -0.1562347109536099, 0.5277178111218841, -0.763279201149969, - -0.025333601684270384, -0.003175649402487863, -0.4350718071173225, 0.34339867646998423, - 0.7508942584663993, 0.8601371894102191, -0.12172040307202199, 0.35530197168707706, - -0.10226390540029975, -0.40202903384892047, -0.025537498800979334, 0.14976169153437674, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3503835201263428, 0.5573931336402893, 0.010655591264367104, - 0.4510197639465332, 0.501114547252655, -0.30450883507728577, -0.3189675807952881] -- [-0.21509458360777164, 0.5754821994897805, 0.7444479690219812, 0.7736235818774609, - 0.03847593238777783, 0.01998619456189058, -0.6321603500828828, -0.2525244832068224, - -0.32835739297057753, -0.001, 0.1405085150194936, -0.13896028366560176, -0.0009999999999999983, - 0.1784260655133623, -0.19386814504488006, 0.0009999999999999961, 0.27498118450110204, - 0.24103626724357524, 0.3392583195738976, -0.18724137463321042, -0.12180256934301167, - -0.15789098412658903, 0.5292098793220072, -0.7617744771010583, -0.02594046091623506, - -0.004602589549454714, -0.4400357245608576, 0.34633948465366726, 0.7512000813340511, - 0.867922014510388, -0.11782349731818881, 0.35491775812405485, -0.10473730681972457, - -0.40239305080499166, -0.02589805688219375, 0.1501308015324478, 0.0, 0.0, 0.0, 1.0, - 1.0, 0.0, 0.0, 0.5584325790405273, 0.5525352954864502, 0.000600503699388355, 0.5607250332832336, - 0.42111414670944214, -0.05365103483200073, -0.16674216091632843] -- [-0.21574090925962205, 0.5757555903281656, 0.7446011635557638, 0.7744564494231919, - 0.03970386520415416, 0.019152035810847453, -0.6310895424256154, -0.25421826655012214, - -0.32902105337282994, -0.0010000000000000024, 0.13831244488387928, -0.14110773485399664, - -0.000999999999999984, 0.17371879031337537, -0.19624432726695162, 0.0009999999999999957, - 0.28179206252354855, 0.24118780696220393, 0.33579591286584215, -0.19030471920019168, - -0.12349383000134519, -0.15860195081055894, 0.5304427097736041, -0.7604888796582773, - -0.02698023597645895, -0.005667790280954896, -0.4452331972665971, 0.3490978978678788, - 0.752165689514104, 0.8747193307223177, -0.11496050999396426, 0.3548038253488175, - -0.10706836650988459, -0.4026563658097435, -0.026481729098996383, 0.15124822081994083, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.43242794275283813, 0.41363590955734253, -0.10878012329339981, - 0.7220767736434937, 0.5476739406585693, -0.2414834201335907, -0.21665377914905548] -- [-0.21629271389902185, 0.5760528761316446, 0.744696215101907, 0.7752239632488479, - 0.040886585482445455, 0.018277439996418045, -0.6300968410645399, -0.2559277582670307, - -0.3294866893167583, -0.0009999999999999983, 0.1362212065084387, -0.1433391283945664, - -0.0010000000000000085, 0.1689925159709063, -0.1983477432545796, 0.0009999999999999907, - 0.2890128961549334, 0.2409869841719389, 0.3326890138490473, -0.1920628208148085, - -0.125162125124028, -0.15886717339311512, 0.5315713061189272, -0.7593208373009864, - -0.02811124786506067, -0.006688127883934938, -0.4503281198123194, 0.3513767045373966, - 0.7530464985130344, 0.8804623105176402, -0.11220728096611984, 0.354990400373925, - -0.10951704020882631, -0.4029505513659528, -0.027096386185843705, 0.15274625800544758, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.35833898186683655, 0.34658682346343994, 0.013208452612161636, - 0.5826573967933655, 0.375459223985672, -0.17363813519477844, -0.3088071346282959] -- [-0.21665939529022915, 0.5763944768763155, 0.7446758457507622, 0.7758666268654374, - 0.041970910315621134, 0.01732221294497814, -0.6292609641018705, -0.2576611049187851, - -0.3295732819765361, -0.0009999999999999992, 0.13436008014654932, -0.14571985852685151, - -0.0010000000000000132, 0.1642246983693721, -0.19992361875974132, 0.00100000000000001, - 0.297012890390707, 0.24011272222263905, 0.33028636283651785, -0.19126993180805077, - -0.12677397683528152, -0.15826800811748723, 0.5325226960708225, -0.7583960898706226, - -0.029410285285040312, -0.00762202993979148, -0.45520485826606816, 0.35272840156237856, - 0.7537627733466539, 0.8841296184285788, -0.10966691270215279, 0.35576475290138243, - -0.11219207391401895, -0.4033030905866908, -0.027775264778573495, 0.15500841505710633, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.24362793564796448, 0.7208894491195679, 0.011458830907940865, - 0.4334750771522522, 0.21711081266403198, -0.019946862012147903, -0.3684675991535187] -- [-0.2169857424013847, 0.5767512255972742, 0.744603357428156, 0.7764550288877176, - 0.04300803110247475, 0.016334247851266602, -0.6284911214351376, -0.2594704834237788, - -0.32936520172580913, -0.0010000000000000013, 0.1325650789248982, -0.14812153238179845, - -0.0009999999999999972, 0.15958521719519186, -0.20137174298140406, 0.000999999999999974, - 0.305346963098281, 0.23875564269489777, 0.32810474257741, -0.18957266789985436, - -0.12819100321562737, -0.15735190131027124, 0.5334962749657081, -0.7576001153182014, - -0.030814395940899297, -0.00851835943642751, -0.46015670662250097, 0.35382586821275464, - 0.7543062149438132, 0.8869222103528148, -0.10737195533768708, 0.3566275365799428, - -0.11472290038875449, -0.4040109072807846, -0.02845422485674232, 0.15751981392369208, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.444545716047287, 0.4979526996612549, -0.14794960618019104, - 0.4177856147289276, 0.40302062034606934, -0.21358707547187805, -0.16222582757472992] -- [-0.21723202933404565, 0.5771388453775136, 0.7444246946068066, 0.7769375622016167, - 0.043947002327771566, 0.015280567139000419, -0.6278560262468794, -0.2614205314013243, - -0.3285884203634506, -0.0009999999999999972, 0.13092252555835884, -0.15056177501283508, - -0.000999999999999997, 0.15519669713287154, -0.2025652213892072, 0.000999999999999987, - 0.31429637896897344, 0.2364523283951439, 0.32636271473261735, -0.1860653855807954, - -0.12918477171987566, -0.15580791261734675, 0.5345308167580112, -0.757058131686128, - -0.032411422319132605, -0.00934159573458021, -0.46524793734802605, 0.3544176568912723, - 0.754504196742079, 0.8879496043430544, -0.10556470195325893, 0.35766111279322155, - -0.11696112417064804, -0.4054266033174117, -0.02914006100936868, 0.16052302162352913, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3616107106208801, 0.03560412675142288, 0.20368057489395142, - 0.3988410234451294, 0.41832461953163147, -0.19966475665569305, -0.4273531436920166] -- [-0.21748064784652438, 0.5775145765061559, 0.7442286162431456, 0.7773823038359804, - 0.044837570148108616, 0.01422537367857932, -0.6272670760765169, -0.2635733272465873, - -0.32765292858293815, -0.0009999999999999983, 0.1292722802470387, -0.1529375925606037, - -0.000999999999999986, 0.15077699835258, -0.20367346424014773, 0.0010000000000000005, - 0.32347437036871707, 0.23400688555809357, 0.3247737418763245, -0.18239255154821168, - -0.12999909231838555, -0.1541894265842262, 0.535523627732501, -0.7565100534296042, - -0.03387353054821366, -0.010285046285998292, -0.4701363354481244, 0.3549529274413199, - 0.7546901754361931, 0.888516832628437, -0.10366437028021624, 0.3587347995103926, - -0.11919833746465595, -0.4069346625991211, -0.029910383048295807, 0.163462986380733, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5872864127159119, 0.3633207380771637, -0.1769903153181076, - 0.6201866865158081, 0.8556017279624939, -0.14121177792549133, -0.14533530175685883] -- [-0.2177326010129763, 0.5778745232418419, 0.7439975590185335, 0.7777531357767837, - 0.04562924773136625, 0.013162849837730377, -0.626773301062673, -0.266112901527407, - -0.3264020321461675, -0.0010000000000000002, 0.12760043813098232, -0.15519301710013778, - -0.0010000000000000024, 0.14629348816115806, -0.2046021111985003, 0.000999999999999995, - 0.3330132225085994, 0.2312515870435808, 0.3235004961676446, -0.178416060526828, - -0.13037204934377913, -0.15239418423616627, 0.5364395927881848, -0.7559427943549022, - -0.03503276326668562, -0.011473415581686915, -0.4746068191478589, 0.35538402196510893, - 0.7548519844645032, 0.8881580388282035, -0.10157009773892942, 0.35987280120304443, - -0.1214316324727822, -0.40860835428139813, -0.030855911180139323, 0.16624039222836484, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.33714550733566284, 0.5900288820266724, -0.21839116513729095, - 0.367006778717041, 0.3734999895095825, -0.20869676768779755, -0.31603556871414185] -- [-0.21797073100061143, 0.5781840240059654, 0.7437589856761546, 0.7780563185513978, - 0.0463610065006755, 0.012117246474430979, -0.62636426668223, -0.2688134685657199, - -0.32502681294024993, -0.000999999999999998, 0.12581957893364026, -0.15730974260532402, - -0.0010000000000000059, 0.1418136326379577, -0.20538209101458796, 0.0010000000000000306, - 0.3425366123826188, 0.22850737725352813, 0.3223201000134687, -0.17474845344113615, - -0.13085882942630114, -0.1505798665295559, 0.5372959494293862, -0.755440865564815, - -0.03618433315756936, -0.012623869887764769, -0.47890394677992393, 0.3557848842066435, - 0.75506808840802, 0.8878622977175978, -0.09968470284731044, 0.3610996285483464, - -0.12349473324383384, -0.4100809380531008, -0.03177436280001653, 0.16913122987246787, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.4595901370048523, 0.38019272685050964, 0.03064659982919693, - 0.6417443752288818, 0.5888263583183289, -0.23084397614002228, -0.14400310814380646] -- [-0.21817546277600205, 0.5783977157117023, 0.7435075881354042, 0.7782205697101403, - 0.04696607317423085, 0.011099514763137322, -0.6261339582091359, -0.2718099278779424, - -0.32338147450327315, -0.0010000000000000009, 0.12380368138206281, -0.15915156739684144, - -0.0009999999999999937, 0.13735043057170657, -0.20584143157711557, 0.0010000000000000057, - 0.351907311921056, 0.22574536331309647, 0.32134024909713177, -0.171773511484571, - -0.1315056658729816, -0.14868599393607346, 0.5380290827432437, -0.7550821543747719, - -0.03727789651825648, -0.013689240071585592, -0.482839513360918, 0.3561328331233319, - 0.7553991354064371, 0.8877341513253555, -0.09823440750786926, 0.3625110827009732, - -0.12519508934302065, -0.4111135155363799, -0.0326405512842191, 0.17224635282776748, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.378512978553772, 0.2701737582683563, -0.08694326877593994, - 0.8666203618049622, 0.4128510355949402, -0.051204681396484375, -0.2299373596906662] -- [-0.2183852716971848, 0.5785533866375949, 0.7432120781296262, 0.7783317386436378, - 0.047493741519569845, 0.010134153515317278, -0.6259723221269222, -0.27508784194722957, - -0.32164942334934965, -0.0009999999999999955, 0.12154688368163045, -0.16076238492823638, - -0.0009999999999999885, 0.13259010030831336, -0.20621918153475696, 0.000999999999999983, - 0.36099519555196585, 0.22304512695014836, 0.3204488949171518, -0.169459461563922, - -0.13253747163746407, -0.1469725518475664, 0.538824839114682, -0.754635588972117, - -0.03843850231318673, -0.014876658429609094, -0.48653059971389745, 0.35639253582471087, - 0.7557336447284054, 0.8877944961027934, -0.09682067461434371, 0.36369254137883933, - -0.1268874958789195, -0.4119857564581815, -0.03335905750258215, 0.17455420699254726, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.5062863826751709, 0.621429443359375, -0.19734619557857513, - 0.5185531973838806, 0.28047168254852295, -0.07218588888645172, -0.32234299182891846] -- [-0.21860560687153266, 0.5785900774196714, 0.7428243784632728, 0.7783315599076492, - 0.04785612859291068, 0.009272509843284395, -0.6259583008228492, -0.27891699015502414, - -0.3197048982811534, -0.0009999999999999966, 0.1187506903798963, -0.1618896186814254, - -0.0009999999999999944, 0.12719953447601715, -0.2064139816985596, 0.0009999999999999861, - 0.369342991298936, 0.22042278838222298, 0.319754990407937, -0.1685839852169784, - -0.13432404110086635, -0.14561014731532676, 0.5397511561550676, -0.754005456566241, - -0.039711726242509404, -0.01632605043935744, -0.4897038222366825, 0.3564677886444919, - 0.7560760437661096, 0.8882699648667799, -0.09548269389572442, 0.3643710566206781, - -0.12855965983427253, -0.4125170937343078, -0.03376080962242072, 0.17509768035353812, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.642845630645752, 0.5591713190078735, 0.28550776839256287, - 0.6246072053909302, 0.5615995526313782, 0.16002260148525238, -0.2485101819038391] -- [-0.21881102372359515, 0.5785267524946678, 0.7424499548706439, 0.7782959397366208, - 0.0481299375588453, 0.008502639171824198, -0.6259925274531533, -0.28294657948587815, - -0.3176413926601351, -0.0009999999999999957, 0.11557988222447478, -0.16273561983737284, - -0.0010000000000000013, 0.12149473462094586, -0.2066145411630481, 0.00099999999999998, - 0.3768932935514092, 0.217853690521084, 0.31924225312233856, -0.16828966341908946, - -0.13640403563525905, -0.14433249290529068, 0.5406099920074314, -0.753218590183, - -0.04114616420472301, -0.017605989575548587, -0.4922606331585482, 0.3565903900737757, - 0.7564140505754785, 0.8889811841580567, -0.09395543496775094, 0.36509064399408847, - -0.1300230602004703, -0.41254960109608474, -0.03394428795211071, 0.17513346198340105, - 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.3875086307525635, 0.42708468437194824, 0.017038723453879356, - 0.3905268609523773, 0.5853323936462402, -0.1523910015821457, -0.2534206211566925] -- [-0.2189838197762376, 0.5782383735870888, 0.7421076868076594, 0.778183378669266, - 0.0482085711385252, 0.00792838835942928, -0.6261339341464326, -0.28737017975130325, - -0.31527769659914634, -0.0010000000000000022, 0.11159292656839656, -0.16296081146847508, - -0.0009999999999999963, 0.1150863992065569, -0.206822740927185, 0.0009999999999999918, - 0.3825844805285141, 0.21535325223343144, 0.31913923429285856, -0.16926925622526748, - -0.13909056428602856, -0.14322377994520755, 0.5413127523698918, -0.7520927746154262, - -0.04289782295048948, -0.01851077276028733, -0.49345704400375856, 0.35681724871320414, - 0.7567425679344099, 0.8902029033946635, -0.0920057362674218, 0.36590448158143196, - -0.13102168431693556, -0.41148412139565843, -0.03364527522463056, 0.17404700639616655, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.28610482811927795, 0.36359062790870667, 0.05500851571559906, - 0.013300135731697083, 0.457319974899292, -0.2914963960647583, -0.20339499413967133] -- [-0.21916277024739597, 0.5778525585321417, 0.7417540466983896, 0.7780244055723368, - 0.04815420997975294, 0.00748639124575435, -0.6263410814733069, -0.29180169104125103, - -0.3128459132336272, -0.001000000000000001, 0.10740413589566727, -0.1626953263722671, - -0.0010000000000000002, 0.1086494253723083, -0.2070007381669225, 0.0010000000000000035, - 0.38688267368941764, 0.21294314077637552, 0.31930668802302214, -0.1702053084779517, - -0.14142246084131208, -0.142408337005444, 0.5420388006947223, -0.7508745811488206, - -0.044817365416599045, -0.019401168614326364, -0.4942169693746209, 0.3572364448898778, - 0.7570186144209158, 0.8911908026238607, -0.09012005239777587, 0.3670260900998905, - -0.1319538461159923, -0.41070112208318416, -0.03305042754531987, 0.17289750953264543, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3580537736415863, -0.026889625936746597, 0.34946128726005554, - 0.5317650437355042, 0.06241375952959061, -0.4075752794742584, -0.10208185017108917] -- [-0.219356141326489, 0.57723488621266, 0.7413745935694932, 0.77775770280354, 0.04779349534673868, - 0.007341699087675443, -0.6267015533623701, -0.2962316244300087, -0.3102235986823106, - -0.0009999999999999944, 0.1028054988789203, -0.16129890325186375, -0.001, 0.10211157310824039, - -0.20710018732369595, 0.000999999999999993, 0.3879846219967296, 0.210711039463024, - 0.3201014374882786, -0.1710142333352557, -0.1429424209353017, -0.14226035896115807, - 0.5428148704325235, -0.7494505461788289, -0.04709138373147462, -0.02026200131539447, - -0.4939772361931361, 0.3580995262150809, 0.7571756765609691, 0.8916188732092877, - -0.08837968782628594, 0.36884503420518394, -0.13273231876990096, -0.41056813746126425, - -0.03178357762997455, 0.17161205777435032, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3194640576839447, - -0.1330048143863678, -0.027054402977228165, 0.4875822961330414, 0.01712772063910961, - -0.32797905802726746, -0.22415509819984436] -- [-0.2195547640466666, 0.5765048350684775, 0.7410412378157305, 0.7774176468746313, - 0.04727411747385332, 0.007347001816766302, -0.6271626437450528, -0.3004893618057809, - -0.3075391522336185, -0.0009999999999999972, 0.0981975651469069, -0.15927659555487203, - -0.0010000000000000078, 0.09579362219469688, -0.20705679476918157, 0.001000000000000008, - 0.387494280145058, 0.2085631641200681, 0.32133783523886117, -0.1708465622731165, - -0.14380946710899092, -0.14220900507485829, 0.54360616166794, -0.7481332582330321, - -0.04935630559593361, -0.021148718523943642, -0.4930361760309619, 0.35912310014631904, - 0.7572116101141996, 0.89081759575332, -0.08663132190778436, 0.3713501946553633, - -0.13342883383795637, -0.410890752623198, -0.030322022336912537, 0.17065901736573372, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2438219040632248, 0.1463942974805832, -0.1048196479678154, - 0.07426423579454422, 0.06569000333547592, -0.13036715984344482, -0.032515089958906174] -- [-0.2197621780592169, 0.5755082414805246, 0.7408101763092789, 0.7769022183241292, - 0.046379150092723864, 0.007705479420930072, -0.6278634749582256, -0.30438661125663835, - -0.3047102330861587, -0.0010000000000000028, 0.09360442324362371, -0.15577628563898932, - -0.0010000000000000046, 0.08995381175901658, -0.20667235859154334, 0.0009999999999999994, - 0.38332596340033936, 0.20662402392178514, 0.32361297889331697, -0.16838962500568294, - -0.14316076800299718, -0.14239259661318293, 0.5444356575165583, -0.7470819773535149, - -0.05158352744442337, -0.022096102251770965, -0.49044126309708935, 0.3605369333359035, - 0.756964398158186, 0.8871336031035251, -0.08486554981372772, 0.3754715015372638, - -0.13393499968757586, -0.4122887600665675, -0.028407119759496488, 0.17048987013365177, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.17834128439426422, 0.15755265951156616, -0.08150579035282135, - 0.0, 0.0, -0.2606666684150696, -0.12234604358673096] -- [-0.21995708655521648, 0.5744020417708572, 0.7406103040560791, 0.7763142600224732, - 0.04529656751700465, 0.008227552301710293, -0.6286626265653632, -0.30791867100319104, - -0.3018600708343353, -0.0009999999999999957, 0.08909725837432651, -0.1516259812206979, - -0.0010000000000000013, 0.08412200236023333, -0.20619206844859364, 0.0010000000000000015, - 0.3773988947508264, 0.20489027780338762, 0.3262234116254022, -0.16459675630165096, - -0.14184224595354122, -0.14276690867479774, 0.5452614663996257, -0.745856275450267, - -0.05399853789550406, -0.023212994855043745, -0.4872898050050432, 0.36131844663074, - 0.7567904588699572, 0.8820251679985495, -0.08349005117843485, 0.3801850382451727, - -0.1344395564811651, -0.4138722936315112, -0.026301403324303585, 0.17000652698858187, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.21810923516750336, -0.11061020940542221, 0.12776292860507965, - 0.04663792997598648, 0.0, -0.30800101161003113, -0.31837114691734314] -- [-0.22011595596553085, 0.5730403096187302, 0.7404775038140469, 0.7755503452360512, - 0.04376050188231084, 0.00914866023624799, -0.6297007086665327, -0.31064008398682497, - -0.29899476291958255, -0.001000000000000002, 0.0848044885767683, -0.1459150424187117, - -0.0009999999999999972, 0.07829706775261323, -0.20548377273222618, 0.001000000000000004, - 0.36729402171279396, 0.20368935523931497, 0.3296342075629603, -0.1576420167065732, - -0.1389157393538128, -0.14360528582911664, 0.5460905110258268, -0.744200139637086, - -0.0568652022633151, -0.0247269372440628, -0.4827893365431065, 0.36059485210057224, - 0.7567924778315169, 0.8735414547710111, -0.08305373906949474, 0.3863426413974255, - -0.13494482809695324, -0.4159067128188649, -0.02373887862175609, 0.1687825954725205, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.08967452496290207, 0.42029324173927307, -0.2026807963848114, - 0.0, 0.32021912932395935, -0.05994643643498421, -0.027339909225702286] -- [-0.22028843989744473, 0.5715385991438648, 0.7403779494275946, 0.7747020463854795, - 0.04203597572341275, 0.010200906092661092, -0.6308451930434446, -0.3127341468433803, - -0.2960447363545417, -0.0010000000000000013, 0.08076024420469924, -0.139545292934704, - -0.0009999999999999905, 0.0723632023844327, -0.20470137691947157, 0.000999999999999978, - 0.3554913055948884, 0.20275447960303045, 0.3332924242742613, -0.14991447703002422, - -0.1352228277716374, -0.1447160978212318, 0.546933958112271, -0.7420421935168755, - -0.0601195669426902, -0.02621044020191076, -0.4777336165845342, 0.35949132939272743, - 0.7569735051849384, 0.8642396418057271, -0.08300083120606523, 0.39274080964283653, - -0.13535122848002562, -0.417885138077753, -0.02093338600008966, 0.1673960920091696, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.012466778978705406, 0.29738566279411316, 0.0496203675866127, - 0.016210773959755898, 0.3461754024028778, -0.2420661747455597, -0.007943837903439999] -- [-0.22046970139770727, 0.5697344389397909, 0.7403545576998731, 0.7736552402722887, - 0.03986162479720228, 0.011575507024413526, -0.6322457020058598, -0.31336978809659055, - -0.2929844491324231, -0.001000000000000001, 0.07721494366419904, -0.13164115920597733, - -0.001000000000000004, 0.06620112450614543, -0.20374232630000713, 0.0009999999999999992, - 0.33945612446709633, 0.2025161182627662, 0.33752556203501677, -0.14040583926131275, - -0.12954623147101105, -0.146448120849336, 0.5478112954064364, -0.738683088270416, - -0.0643110964506457, -0.02757171434050515, -0.47133681345837997, 0.35742553925935155, - 0.7575968200815655, 0.8530529154221608, -0.08389779219021662, 0.39980050394334044, - -0.13551936281204516, -0.41971637803544193, -0.017530042760227574, 0.1656530708092699, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.26821479201316833, 0.3935849070549011, 0.061330538243055344, - 0.5341399908065796, 0.016164736822247505, -0.4154812693595886, -0.3059277832508087] -- [-0.22065086354973626, 0.5677998576119808, 0.7403086236268202, 0.7725200639248493, - 0.037438116452505124, 0.013054105474482871, -0.633751314476189, -0.31328302922095924, - -0.2896655152252214, -0.0010000000000000009, 0.07422708445047276, -0.12299636281535538, - -0.0010000000000000076, 0.0599034995140995, -0.2026187020602957, 0.000999999999999999, - 0.32237873620106144, 0.20245974903893305, 0.34212319026920074, -0.13040553978756053, - -0.12352247998356757, -0.14854605707398183, 0.5486244319401861, -0.7348196292674936, - -0.06866049093052909, -0.029171448678115136, -0.46419427926604506, 0.3552601940048667, - 0.7581648598545879, 0.8412071540250827, -0.0850809433552269, 0.40723109428025206, - -0.13553729170412526, -0.4215554420967501, -0.013959887710160435, 0.16397602372871203, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.004438521806150675, 0.38535425066947937, 0.045771561563014984, - 0.04724656045436859, 0.026866035535931587, -0.23526744544506073, -0.15503624081611633] -- [-0.22077378250259885, 0.5656384309942958, 0.7402061350334533, 0.7711912134040243, - 0.03444198801633786, 0.014785306834349431, -0.6354992183565032, -0.3113870466936308, - -0.2859079553555732, -0.0009999999999999983, 0.07231246315272438, -0.11274301258986348, - -0.0010000000000000117, 0.05339132394785411, -0.2010888360187299, 0.0009999999999999864, - 0.3021885215081139, 0.20288062322136569, 0.34753951747774814, -0.1193995518791201, - -0.11620568618438214, -0.1514086129368577, 0.549225094843611, -0.7298251732669319, - -0.07335095561848465, -0.03125971152500368, -0.45526295492464214, 0.3527060720622348, - 0.7585936307986993, 0.8279175152490954, -0.08699162152398257, 0.41568707977536623, - -0.13517591452131136, -0.4233390623638149, -0.009965168831967547, 0.1624978521489289, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03984205052256584, 0.13271503150463104, -0.36997494101524353, - 0.34222131967544556, 0.05212089791893959, -0.17622950673103333, -0.0051981015130877495] -- [-0.2208978599451343, 0.5633962125347566, 0.7401580428535428, 0.7697643891465452, - 0.03117115751964274, 0.016549293392864584, -0.6373517592577346, -0.3087915239923566, - -0.28179961238990336, -0.0010000000000000007, 0.0712025834825897, -0.1017881276831847, - -0.0009999999999999963, 0.04692179954360282, -0.19929624144914446, 0.0009999999999999812, - 0.2819257836024548, 0.20352850733689767, 0.3528757350900492, -0.1076976117780991, - -0.10881621973144309, -0.15442361058158274, 0.5493416340990076, -0.7246107128714468, - -0.07813260859615763, -0.03362886923034614, -0.44600542925404757, 0.35012345264655476, - 0.7597350751333483, 0.8139355595270824, -0.08940363865866804, 0.4246603982996134, - -0.13483904695784762, -0.42507900814356564, -0.005720584744124744, 0.1616942691249834, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2340935468673706, 0.2838091254234314, 0.14383907616138458, - 0.29653993248939514, 0.0016301018185913563, -0.2952231764793396, -0.1245737075805664] -- [-0.2209293279507458, 0.5611054958415153, 0.7402547704241029, 0.7681248351952041, - 0.02727519218214428, 0.01839888483106472, -0.6394542849060758, -0.30420222130909197, - -0.2771601237104795, -0.0010000000000000013, 0.07167838731985123, -0.08939658662823959, - -0.0010000000000000128, 0.04075138371101375, -0.1968312774544735, 0.0009999999999999937, - 0.2602769481505375, 0.20479325831084538, 0.35783879073036334, -0.09459854374746435, - -0.1003530903753659, -0.1575745625389876, 0.5481687756759864, -0.7190368786929713, - -0.08307954295591649, -0.036549480916756916, -0.4360249936781271, 0.34723148311906266, - 0.7627389321572778, 0.79854341297888, -0.09312412903083454, 0.43506631712174026, - -0.13456880916149122, -0.4265222306334401, -0.0008265392393854793, 0.16264402445828133, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.11833060532808304, 0.28058937191963196, 0.24955600500106812, - 0.1883169412612915, -0.007465505041182041, -0.3314324915409088, -0.0314331129193306] -- [-0.22091315379651702, 0.5588690382233034, 0.7403323985555817, 0.7664028445453461, - 0.02312458646556084, 0.020152167062627093, -0.6416274803468741, -0.2991155337372257, - -0.27183658831832114, -0.00044540169349374203, 0.07285996631916444, -0.07665342253384687, - -0.000999999999999997, 0.03481762975449989, -0.19423589579830172, 0.0009999999999999918, - 0.23961355728458006, 0.20568419574669208, 0.3622878105581732, -0.07935799849009549, - -0.09224679905400315, -0.16086602551652415, 0.5468069180773958, -0.714010059020241, - -0.08796870252600149, -0.03955136346031226, -0.42673472518995675, 0.34321242585301903, - 0.7661914663490322, 0.781944926660928, -0.09718622822940585, 0.44636070501671715, - -0.13427697232709754, -0.4288974230166474, 0.0040120323899502045, 0.16425326247425515, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3352317810058594, 0.13051696121692657, 0.16685238480567932, - 0.6061336994171143, 0.0, -0.4661073088645935, -0.07021920382976532] -- [-0.22063110864688598, 0.5569429025553353, 0.7403604003061482, 0.7645401322564853, - 0.01838194834229763, 0.02172851348639126, -0.6439474837640481, -0.29232077809406964, - -0.2652435087962121, 0.0009999999999999987, 0.07543435536672613, -0.0633436882582239, - -0.0009999999999999946, 0.02959811745869483, -0.19142744378522145, 0.0009999999999999803, - 0.21961342032189143, 0.2056907882103655, 0.3654277872398724, -0.05950715756408199, - -0.0837804932235514, -0.1643298502871139, 0.5448297312606285, -0.7104504424819325, - -0.09261939324599738, -0.042533137371262957, -0.4188274160831172, 0.33634426824381525, - 0.7708743445093542, 0.7628108734650291, -0.10216028460813037, 0.4600391266819735, - -0.13391971788581755, -0.43336836753663033, 0.008718618990975594, 0.16757591146822956, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.22024481075985902, 0.5551301441444577, 0.7404469882117641, 0.7627310257004455, - 0.013614838019530943, 0.023199879640270822, -0.6461561608499349, -0.285463186984868, - -0.25821455147119654, 0.0009999999999999994, 0.07820442812821089, -0.05037327450568, - -0.0010000000000000057, 0.02472135105114694, -0.18862755039457915, 0.0009999999999999905, - 0.20095917655079634, 0.20518549824054177, 0.36854391460702207, -0.038666501246219516, - -0.07570126959479705, -0.1676064039218873, 0.5418697291047576, -0.7076551167287213, - -0.0964517361741405, -0.04536711466769012, -0.41084409257167864, 0.3296173573867082, - 0.775980816545132, 0.7424971880042541, -0.10726163207420347, 0.47483956613153316, - -0.13355962612211378, -0.4385766788285575, 0.013474102360003704, 0.17188603205913347, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2194361691160154, 0.5538455770967503, 0.7406666884957317, 0.7609190193734579, - 0.00866838823386265, 0.02437068302485457, -0.6483310688298569, -0.27764070422981924, - -0.250333716071023, -0.0010000000000000018, 0.08164426356521978, -0.03814694917967515, - -0.0009999999999999894, 0.021394386837500147, -0.18545036711357205, 0.001000000000000021, - 0.18435459984669864, 0.20331146383238158, 0.3711025092143305, -0.014068299955338237, - -0.0676623123311297, -0.16998404433406006, 0.5368716112067351, -0.7074469517703252, - -0.09802018665698961, -0.047658214900796185, -0.40389030323945374, 0.3213536089003633, - 0.7822823858957938, 0.7194678304296156, -0.11270148388359194, 0.49300794424620553, - -0.13317423385904725, -0.4457562544100027, 0.018391970096237692, 0.17919436857066215, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2185068777929622, 0.552859209648923, 0.7409984883360076, 0.7589036943213358, - 0.0038131610087127803, 0.025245895967947428, -0.6507021494396075, -0.2696553761895126, - -0.24221062688728906, -0.0010000000000000009, 0.08525907666485502, -0.026411518787522507, - -0.0010000000000000098, 0.0197954563553315, -0.18119388150292356, 0.0010000000000000009, - 0.16934493580638318, 0.2005793397169377, 0.3725995791595514, 0.015690539794069126, - -0.06056908389790847, -0.17125511566000565, 0.531790773009127, -0.7078210718782576, - -0.09831689674666212, -0.04968804575721268, -0.3997018309878347, 0.3096896464302922, - 0.7888934468612786, 0.693982875534781, -0.1182267310507387, 0.5112018652431249, - -0.13273839419956474, -0.45471549172916054, 0.02339689681569468, 0.18657407944445306, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2171522717198078, 0.5525894343827754, 0.7416159230156514, 0.7567211022552303, - -0.0007450295414929539, 0.02563226369021236, -0.6532347245751151, -0.2612890735815916, - -0.2340455518087036, -0.0009999999999999998, 0.08916686151827331, -0.0162708964330284, - -0.0010000000000000026, 0.02192952212067967, -0.1752448678994605, 0.0009999999999999918, - 0.15733666571138552, 0.19621807889217183, 0.37296221833909265, 0.055121788134626884, - -0.05530393456780958, -0.17015094655585195, 0.5252354225499938, -0.7097487471265058, - -0.09507808814574312, -0.050840457004049856, -0.39876501195997105, 0.2928559246720182, - 0.7964245794291293, 0.6631339548414452, -0.12395632594076997, 0.5297285078942058, - -0.13214240234304114, -0.46866278023718794, 0.02870426938571235, 0.19458646501184992, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2155476486599784, 0.5524826828792984, 0.742309534086467, 0.7547097523567228, - -0.004936866313695027, 0.025679709219399487, -0.6555374661932462, -0.2529537937900153, - -0.22556707359581346, -0.0010000000000000005, 0.09198999150556252, -0.007804674407132062, - -0.000999999999999998, 0.025111672746633396, -0.16889019418322354, 0.0010000000000000022, - 0.14653490957786677, 0.19103263111345734, 0.37282739569312623, 0.09548418691534596, - -0.05204976039288984, -0.16794097565906607, 0.5179797341717075, -0.7112044065355726, - -0.08999523759768983, -0.05120752552262522, -0.398012554971959, 0.275347888941055, - 0.8038477456616825, 0.6323759762949369, -0.12863976224982468, 0.5462488040246101, - -0.13084412809592644, -0.4807315036664265, 0.033435895768025546, 0.20202663719056038, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2131827861745805, 0.5529542575165005, 0.7431816532984447, 0.7531864217809923, - -0.008327970469150221, 0.02467723295334042, -0.6572913304816794, -0.24432390532765594, - -0.21590899292291663, -0.0010000000000000041, 0.09210379869233053, -0.0037608802950666424, - -0.0009999999999999985, 0.03126966834566311, -0.1614573454971535, 0.000999999999999994, - 0.13913843272251505, 0.18323041305985394, 0.371693601662695, 0.13915101807496863, - -0.054624911615865, -0.1622660255733934, 0.5087931982673626, -0.7137982424305962, - -0.07954120047696686, -0.049313258878313115, -0.39755153702873575, 0.25569177294448847, - 0.8110076848805005, 0.6025543245948417, -0.13019031656609642, 0.5586063889174138, - -0.1274473370602021, -0.4888330313715529, 0.036557634009328314, 0.20952821901039814, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2113002619503391, 0.5532726432484383, 0.7444526043170515, 0.7516809388372359, - -0.011372050374556247, 0.023987049938511182, -0.658992461333434, -0.23613601647646462, - -0.20863133225183916, -0.0009999999999999957, 0.0927643658608331, 0.0009885554653857162, - -0.0010000000000000072, 0.03451307562259659, -0.15126783103062963, 0.0009999999999999905, - 0.13141857562869055, 0.17851639599723082, 0.3698001612859197, 0.1800227531504713, - -0.05482008915784352, -0.15571126925392628, 0.4996968519023078, -0.7103120567796332, - -0.07027574644052427, -0.04823144443951188, -0.3969487320050766, 0.2399676439794758, - 0.8181598059844509, 0.5799381386752833, -0.13274464305737066, 0.5643887198218809, - -0.1247710743527233, -0.4927538996532073, 0.040028760889420506, 0.21291762459974653, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20965925796352392, 0.5535287264138935, 0.7460156359773251, 0.7504577484041172, - -0.01398915823662829, 0.023724380504632107, -0.6603443231237195, -0.22864945175464133, - -0.20349010624837205, -0.0009999999999999987, 0.09441363379591794, 0.005398183282976504, - -0.0010000000000000028, 0.035833567293952935, -0.1421865679492265, 0.0010000000000000165, - 0.12472293389137278, 0.17627144748551568, 0.366360329574578, 0.21960975386960307, - -0.05567413200152465, -0.15116057667611157, 0.49088236374026917, -0.7027391050471368, - -0.06202874766598919, -0.048185395394226616, -0.3970091006380315, 0.2291589444666766, - 0.8258765455393748, 0.5667459815566224, -0.13803778761314528, 0.5684602965609454, - -0.12434408777568769, -0.49488615814231895, 0.04503058460504657, 0.21774827033327163, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20833842870619992, 0.5538320112434267, 0.7477949280599373, 0.7492354363242537, - -0.01645411182247295, 0.023547379585043913, -0.6616804697698256, -0.2214447970691923, - -0.19956975898690313, -0.000999999999999997, 0.09651182103997526, 0.010138679848355382, - -0.0009999999999999948, 0.036215723759319675, -0.1326252054242976, 0.000999999999999996, - 0.11809101293628833, 0.17560824374741968, 0.3625929338360891, 0.255514237978073, - -0.05548982586827653, -0.14642860255595888, 0.4818149297727052, -0.6905881942697597, - -0.05464693224518349, -0.04867464640990376, -0.39654001344394524, 0.2224270829536129, - 0.833860004484461, 0.5600647527138117, -0.14402109202956068, 0.5675772964267642, - -0.12391909998722266, -0.49407752159825374, 0.05012399117182826, 0.21951655205541573, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20726333509102246, 0.5541806042715466, 0.7499843399205031, 0.7481775946530257, - -0.018547109522854918, 0.023423238372148204, -0.6628255000312836, -0.21488413124629474, - -0.1969390421576327, -0.0010000000000000041, 0.09886284921633558, 0.014491246468195917, - -0.0009999999999999894, 0.035308172122009034, -0.12347155421963497, 0.0009999999999999955, - 0.11262321234568587, 0.17643489186834413, 0.3579835041704425, 0.2876962017431096, - -0.054144185318762784, -0.14190848273068857, 0.47213391244925673, -0.6736833800100878, - -0.048689928443187334, -0.04970984323855598, -0.39498885654101434, 0.22193604720842386, - 0.8425699206894623, 0.5628309914944926, -0.15214021860560067, 0.5624972512480053, - -0.12349458248804132, -0.4902353776696715, 0.05548728623168657, 0.21939022868330293, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20647679934488633, 0.5546461338954771, 0.752193968492561, 0.7471259035272787, - -0.02047925062279276, 0.02325965847259558, -0.6639596921954022, -0.20865799379129932, - -0.19524708060277035, -0.0009999999999999953, 0.10135348962389468, 0.018925820630325683, - -0.0009999999999999972, 0.03407260207303246, -0.11423286658148106, 0.0010000000000000005, - 0.10722124061499624, 0.178482499220961, 0.3532667599671168, 0.3152583318132887, - -0.05197498155491675, -0.138066858611742, 0.462271259316894, -0.6524969452280612, - -0.04331303637943392, -0.05106310682857493, -0.3929892560780039, 0.2253211042003651, - 0.8511373032774082, 0.5720369832635516, -0.16023176024672228, 0.5520664684700686, - -0.1231391745595685, -0.48323494149213747, 0.060629060511502304, 0.21481038734828625, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20596620721463058, 0.5551735325920747, 0.7543391109076105, 0.7461867823923833, - -0.022098753608010453, 0.02305263499208514, -0.6649703052706903, -0.20301683602262766, - -0.1945272376794983, -0.001000000000000006, 0.1038388083329596, 0.02309248204561906, - -0.0009999999999999976, 0.032064109520200744, -0.10545296710479146, 0.0010000000000000046, - 0.10231858876626262, 0.18170299049865646, 0.3481747442919664, 0.3387726579751482, - -0.048493360190509774, -0.13560073878229098, 0.45236281734869344, -0.6263775282474808, - -0.0390327936505443, -0.05255372821144014, -0.38979018244969715, 0.23367483883785953, - 0.8592423645819932, 0.5884626634248822, -0.16815865676680336, 0.5367105071657481, - -0.1229390107632594, -0.4738033315672749, 0.06513562537766215, 0.20569012198420938, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20557277493474968, 0.5557633636447233, 0.7565072255690073, 0.7452374159634486, - -0.02344414816051005, 0.022753763211451047, -0.6659983723890519, -0.1978234275685038, - -0.19458527812191528, -0.001000000000000005, 0.1059548411949095, 0.026876674436767094, - -0.0010000000000000154, 0.03039615144235104, -0.09661655256070982, 0.000999999999999983, - 0.09774688957888675, 0.18582292640636522, 0.3425906699200499, 0.3566671853942329, - -0.04488140822103488, -0.13372509394120913, 0.44188659381726386, -0.596857545174644, - -0.035014546152111095, -0.05386579071052005, -0.3867242520924239, 0.24479458184468272, - 0.8677132956017729, 0.6109137600703408, -0.1764066147762922, 0.5170286662176496, - -0.12280460962205024, -0.4596806435875706, 0.06964563345360417, 0.19213429143178154, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20522121654522035, 0.5563122353177634, 0.7587257587475595, 0.7442369701571603, - -0.02436749208805393, 0.02237934830563388, -0.6670957370198397, -0.19331541801773935, - -0.1956831284201988, -0.0010000000000000063, 0.1075775726247608, 0.030105668474057497, - -0.0010000000000000059, 0.02935037306709543, -0.0876798449463008, 0.0009999999999999953, - 0.09397086133546631, 0.19097635509414834, 0.33569354344068375, 0.3685391758487981, - -0.04094712569443212, -0.13150701346528929, 0.43052947002240805, -0.563782732513963, - -0.0317510088872456, -0.054218857775805665, -0.3839105204931432, 0.259085805882211, - 0.87742997109238, 0.6400967199382002, -0.18566864705084943, 0.49411353600086677, - -0.12286534910632117, -0.44048042131808035, 0.07426118643320716, 0.17604436730010228, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20483201016975927, 0.5566812962436117, 0.7608481721199999, 0.7432943710784171, - -0.024916730230344224, 0.022047926957736826, -0.6681366053395325, -0.18927365473566635, - -0.197397955428451, -0.0009999999999999827, 0.10856441282342578, 0.03274657096573882, - -0.000999999999999985, 0.028662715946170564, -0.07895274180859167, 0.0010000000000000028, - 0.09040186964682742, 0.19680737740567836, 0.3281627611159946, 0.37493208127849936, - -0.037758890471065974, -0.13072989191787196, 0.418894009843215, -0.5282281401264632, - -0.028524164779582757, -0.05440898844703069, -0.38106671596740416, 0.27406075595919654, - 0.8869252397446434, 0.6736892188227929, -0.19460988664669748, 0.4690977066479564, - -0.12287416627734754, -0.41588987101711017, 0.07828464647812682, 0.1563951342428731, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20431508511256952, 0.5567847218448676, 0.7626690244365042, 0.7424363380314419, - -0.025013382474502136, 0.02178018259105927, -0.669095089142004, -0.18592789913020133, - -0.19989308398753833, -0.0009999999999999992, 0.10873552356812147, 0.034692690150065014, - -0.0009999999999999979, 0.028553499846456405, -0.0704200435104179, 0.0009999999999999929, - 0.08721466172521505, 0.20341253465388415, 0.31971367097031367, 0.37619076661075945, - -0.035341356775851505, -0.1311430284118018, 0.4073756107344628, -0.4907153392778419, - -0.025723607566330232, -0.054163260456151475, -0.3780158267268166, 0.28909102819267196, - 0.8957203811219745, 0.7107444519126616, -0.20237943692852936, 0.4433462684793357, - -0.12265272387836242, -0.3860808347438825, 0.08042881635342132, 0.13596041422112654, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20364725065188874, 0.5564798756503627, 0.7644356627647593, 0.7416333537233348, - -0.024601910248271942, 0.021654714128187317, -0.6700043193989436, -0.18301057444216284, - -0.20292380886042238, -0.0009999999999999957, 0.10804884295849679, 0.03586656147263653, - -0.0009999999999999937, 0.028901630788268975, -0.06209843775325045, 0.0009999999999999972, - 0.08413166288204815, 0.21048102255202192, 0.31032820136337325, 0.3720504335716717, - -0.034168070333689106, -0.1333205449971919, 0.395496873776916, -0.45175558768465685, - -0.022824514163391804, -0.053695734136545506, -0.3752266304231971, 0.3028557452649758, - 0.9043370107569239, 0.7512649956661318, -0.21019592996962053, 0.41876774628593955, - -0.12225251052741147, -0.3520534848653132, 0.08223418355840162, 0.11514395639596221, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2027134105379033, 0.5557733428320114, 0.7661217775637815, 0.7408104843360216, - -0.02363042634833775, 0.02158374524586916, -0.6709512435263739, -0.1805433708795464, - -0.20645985962272836, -0.0010000000000000115, 0.10619678297029346, 0.036148716271570805, - -0.0009999999999999742, 0.030096421017217236, -0.05363248725349937, 0.0009999999999999788, - 0.08118616650235538, 0.21771082304239775, 0.30001733581981027, 0.36269783743946893, - -0.033932519739379365, -0.1366274662887202, 0.3835188232976298, -0.4123505865369606, - -0.020220400798845767, -0.05284230821798119, -0.3732614316568285, 0.31450040491806436, - 0.9123768612271685, 0.7946903739295391, -0.21812564318151648, 0.3966247469119409, - -0.12120945088349694, -0.31526951865474606, 0.08296729339338917, 0.0967760314990305, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20163396824858887, 0.5546701273481087, 0.767650736633307, 0.7400866665262168, - -0.022206380295235462, 0.021649971614606946, -0.6717960117724604, -0.17847067540133532, - -0.21049109416391415, -0.0010000000000000224, 0.10349900173404333, 0.03570367417546081, - -0.0009999999999999907, 0.03151745970560209, -0.04542488663952687, 0.0009999999999999963, - 0.07827421583707198, 0.22542998315807186, 0.28886866346971, 0.3503670556331891, - -0.03508685266631532, -0.14164114565495972, 0.37138041237072805, -0.3737338259716792, - -0.01740664336946555, -0.05182073561822691, -0.37139526571364795, 0.3232290563327973, - 0.9201687073242566, 0.8384823696856675, -0.22571664406393835, 0.37806509751942363, - -0.11999073753467558, -0.2779266438614017, 0.0831953156850013, 0.08181244337678618, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.2003854401786844, 0.5533072016001073, 0.7688950092363613, 0.7394864638166309, - -0.020411987711708376, 0.021739053867832498, -0.6725106200845921, -0.17692852835510453, - -0.21507082713346362, -0.000999999999999998, 0.09977265941054489, 0.0345885837762092, - -0.0009999999999999935, 0.033077229793400345, -0.03719307704386692, 0.0009999999999999703, - 0.07550075739037981, 0.2335730354944907, 0.2771264580465322, 0.3360119883363313, - -0.03733920414259713, -0.14758410608800027, 0.35986615832889723, -0.3379762491864764, - -0.014599691185525494, -0.050617608668167426, -0.36980982636588794, 0.3283187901275481, - 0.9271108458694634, 0.8806769692002212, -0.23203036628406165, 0.3627027919276486, - -0.11813710642580812, -0.24196215804951562, 0.08169110107287045, 0.07220747814364331, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19897636593792886, 0.5517532909306415, 0.7698865958613365, 0.7389479769050512, - -0.018417837313784966, 0.021848512681322055, -0.6731562323786564, -0.1754902304009297, - -0.21965019431973773, -0.0009999999999999926, 0.09539932184515368, 0.03292827279169527, - -0.0009999999999999911, 0.03479038867268538, -0.029262779217670214, 0.0010000000000000024, - 0.07266834824385626, 0.24159109498491224, 0.26517252548010817, 0.3221632860628715, - -0.04052513460921061, -0.15434869118095265, 0.3485380858272685, -0.3052499720223723, - -0.011552800430102659, -0.048742369677107864, -0.36832654822507455, 0.33102890138105323, - 0.9337243636427612, 0.9195119870248993, -0.23789413890851704, 0.35136479642043617, - -0.11642907162706935, -0.20959643948814613, 0.07993893507336924, 0.06685164220908904, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1974812706406378, 0.5500880292749699, 0.7703348933878672, 0.7385132457471993, - -0.016218838600042636, 0.021833198269084652, -0.6736901710606631, -0.17437041650910987, - -0.22402605427936623, -0.0010000000000000048, 0.09007799333170412, 0.030537326325108437, - -0.0009999999999999883, 0.036502723042106736, -0.02162885915711226, 0.001000000000000004, - 0.06989849979011331, 0.24878390715280416, 0.25346192478446505, 0.3099817469502888, - -0.044198233792925576, -0.16129867078335133, 0.3384289886162625, -0.2785986352080959, - -0.00856928474065532, -0.0449028575639915, -0.36735418992586816, 0.33231614142373944, - 0.9391341704745535, 0.9521862157044153, -0.24204273135595764, 0.34348697451314825, - -0.11529779297026499, -0.1823221153708845, 0.07732231523306002, 0.06594401062650353, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1959122344809913, 0.548389775610371, 0.7705543690975044, 0.7380655271336171, - -0.014184887025205468, 0.021896023489633536, -0.6742244661775478, -0.17317998909465723, - -0.22804104280262807, -0.00099999999999998, 0.08500740035579323, 0.02850753809640321, - -0.0009999999999999924, 0.03822134616707189, -0.014176477518276562, 0.0010000000000000145, - 0.06715331609556577, 0.2554612002866795, 0.24218488864762633, 0.29973359562756574, - -0.04817503236152198, -0.1684196259124599, 0.3293364077757777, -0.25528011316222193, - -0.005601641080761083, -0.04129050559146482, -0.3672188665838426, 0.33307394547154057, - 0.9439634828528821, 0.9803811361833759, -0.24662842586689146, 0.3383124588603611, - -0.11478477369493627, -0.15959531258555318, 0.07480819144658359, 0.06786876242482573, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19472911258914058, 0.546642437751157, 0.7701857265561999, 0.737603766310501, - -0.0123773108929045, 0.02218798737243001, -0.6747556441527446, -0.17272130173570044, - -0.23203767157880495, -0.000999999999999993, 0.08109973672693693, 0.027926506461138173, - -0.0010000000000000028, 0.03947495641530847, -0.006593023634964299, 0.0009999999999999861, - 0.0651254898952498, 0.2615241109281503, 0.23207776048642947, 0.29112131629561144, - -0.05189185531223061, -0.17648688887167593, 0.3235499624116556, -0.23826444771924615, - -0.003684499210909675, -0.03924431893274884, -0.3708697599149621, 0.33621783806022676, - 0.9465336131893641, 1.0016240388288242, -0.2529408086422832, 0.3351846901496329, - -0.11671814609700736, -0.1423112456771152, 0.07271564320114667, 0.07219599348700022, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19323782239735673, 0.5449332993622932, 0.7698451686692691, 0.7371243570496804, - -0.010741229567022626, 0.02236108663230829, -0.6753016289304402, -0.17172850299283643, - -0.23494654504381213, -0.0009999999999999753, 0.07681935811246121, 0.02701498326051624, - -0.0009999999999999985, 0.04117965717510056, 0.000931669416153436, 0.001000000000000013, - 0.06308441127017693, 0.26602484193370657, 0.22235749175339115, 0.2839132944041771, - -0.05651702526877793, -0.18299717630752002, 0.3178312665306756, -0.2235542708180769, - -0.001050319514892506, -0.03660178432507827, -0.373535535138408, 0.3374241843330121, - 0.9490927545962419, 1.020415856877785, -0.2571490211649623, 0.3315662470671189, - -0.11746930555014937, -0.1281664207642667, 0.07025382795710353, 0.07759798149773886, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19101176495470137, 0.5433410223114986, 0.7697333643162197, 0.7366785146099741, - -0.008928483674978237, 0.02191290192522951, -0.67582902647084, -0.16998003029816613, - -0.2357367874993966, -0.000999999999999991, 0.07001464891992065, 0.023597400018687226, - -0.0010000000000000013, 0.04425780666946663, 0.009087593822674505, 0.000999999999999985, - 0.0615154738117557, 0.2667772229593292, 0.21314340767440637, 0.27546879959028464, - -0.06429856849151364, -0.18549027661841733, 0.3110498645760367, -0.21330084822640677, - 0.00393820576837449, -0.03183261439017736, -0.37275977300909013, 0.3324795075728457, - 0.9516108926647115, 1.0382747441146696, -0.2527916969673411, 0.3208578598280866, - -0.11348249234529151, -0.11705936036463037, 0.06632567569979603, 0.08267660842822405, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18868657158323088, 0.5417762368737056, 0.7695478384881754, 0.7361216416421773, - -0.0076768803354769505, 0.021864778477892352, -0.676452456331155, -0.1680956749528084, - -0.23607533092546457, -0.0009999999999999848, 0.06466040594923331, 0.02164231679119984, - 0.0010000000000000002, 0.04713523639297173, 0.0167983753564575, 0.0009999999999999957, - 0.05989454658949674, 0.26677804782791525, 0.20455838968870343, 0.2683785525359588, - -0.0720657321094437, -0.1872307025285079, 0.3042204999410971, -0.20620891142558082, - 0.008896511197028851, -0.027393436783348976, -0.37270482917186315, 0.32813918303319795, - 0.9535933685075798, 1.0529792981776989, -0.2487348098394144, 0.31100979911745896, - -0.11013796384270894, -0.1079194893875464, 0.062240597371140814, 0.0885509692058863, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1870261552298916, 0.5401436326102126, 0.7690434572447842, 0.7354511849497505, - -0.006381284733697126, 0.021965252868129125, -0.6771915249230417, -0.16658573128878748, - -0.2372614817733454, -0.0009999999999999764, 0.0603807766505818, 0.020834491983743268, - 0.0009999999999999996, 0.04992724978222172, 0.024571505375805228, 0.0010000000000000106, - 0.058177161672957715, 0.26769244068107034, 0.19740587437062365, 0.2610193981714226, - -0.07896583855568619, -0.1902837162362176, 0.30211421405443056, -0.20321660675761455, - 0.013114325685553882, -0.02478162436211972, -0.37556295038534443, 0.3275357981016853, - 0.9533195970668102, 1.0620180128449297, -0.24600553378460155, 0.30207865556905017, - -0.10943673542019887, -0.09988808806136759, 0.0575328977776089, 0.0955129695365202, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18538729153246236, 0.538547801473734, 0.7685963272118286, 0.7347535649043918, - -0.005340827920765172, 0.02208689992551507, -0.6779534226398504, -0.16481274418739347, - -0.2378068731347231, -0.0009999999999999979, 0.05655003621019218, 0.020485159417231398, - 0.001000000000000001, 0.05262881966921234, 0.03203543698281148, 0.0009999999999999822, - 0.056485867198989424, 0.26788859751666644, 0.19068968740710826, 0.25463344867159426, - -0.08548022263601578, -0.19270879600624832, 0.3005770881516244, -0.2012171713802332, - 0.017309939503888434, -0.02231942512295506, -0.3784947608908538, 0.3270825899463779, - 0.9530207759706106, 1.0696118718033771, -0.2434428120767932, 0.29349742973867704, - -0.10884815192159704, -0.0932600921241344, 0.0530783090634474, 0.10243866629167801, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1840021915320049, 0.5370207889397566, 0.7683653871245698, 0.7340991897154524, - -0.004424418358556362, 0.022174671249815196, -0.6786656674212037, -0.16322939034014836, - -0.23774838246381122, -0.0010000000000000234, 0.052991925082741045, 0.020315019008610054, - 0.0009999999999999998, 0.054858472686152834, 0.039018336052134096, 0.0010000000000000271, - 0.05537718774097437, 0.2671690020709843, 0.18465025339202112, 0.24729672302602398, - -0.0910132650001367, -0.1946837333122668, 0.29998744072342504, -0.20194789749331946, - 0.021418055968864584, -0.020256846505441635, -0.3823064214321062, 0.3263966676802752, - 0.9526151371251359, 1.075615485565695, -0.24161558499074595, 0.2848378598868695, - -0.10873096830132357, -0.08836982222267865, 0.04972857195264209, 0.10806563058723416, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18264707830384083, 0.5355491410742139, 0.768140517276758, 0.7334350088483131, - -0.0037186720654197912, 0.022236796076034623, -0.6793855931457616, -0.16137710031160324, - -0.23718212927717167, -0.0010000000000000252, 0.04973688753348782, 0.020430988810640073, - 0.0009999999999999905, 0.05698575460333065, 0.04567157208501491, 0.0009999999999999562, - 0.05421794822559696, 0.2660005826931941, 0.17905140020789723, 0.24093028523734775, - -0.0962223242499641, -0.1962618836478598, 0.29981835439960697, -0.20349839079407428, - 0.025428761930639348, -0.01835177963142318, -0.38602452133531634, 0.3258435040533224, - 0.9520595620223901, 1.0802911581607768, -0.23983236798605068, 0.2763118468995647, - -0.10865611865206248, -0.08448346747613203, 0.046470291455527654, 0.11349001289799264, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18145536663508116, 0.5341571327157361, 0.7678818245121849, 0.7328738969931252, - -0.0030591578875713604, 0.02213383559966261, -0.6799974896871143, -0.15942378758482656, - -0.23619083357695092, -0.0009999999999999755, 0.046237947861058885, 0.020132255908018995, - 0.0010000000000000009, 0.05855461711875406, 0.05156265972552759, 0.0009999999999999799, - 0.05312743780163067, 0.26460682034334554, 0.17425289846707864, 0.2344489939704339, - -0.10088461843634128, -0.19811899552620976, 0.3002469094614653, -0.207702793210002, - 0.029164952948003753, -0.01676891602278684, -0.38976377599051504, 0.324790533766905, - 0.9508288757599941, 1.0823982955716251, -0.23824942048868172, 0.26745044662836326, - -0.10877743087469167, -0.08249494118406818, 0.04362480625044286, 0.11735246278433632, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18029431876010238, 0.5328235901423182, 0.7676340139999539, 0.7322949303000217, - -0.0025670051942453228, 0.022021430360596134, -0.680626624623291, -0.15732688187443591, - -0.2348293420132382, -0.0009999999999999827, 0.04302361997906334, 0.020083020591335054, - 0.0010000000000000102, 0.060059230384818925, 0.057178779889496725, 0.0009999999999999935, - 0.05204809436718262, 0.2628143312849181, 0.16983317470487502, 0.2287165076463315, - -0.10521998934993759, -0.19954600464577374, 0.3009297551524905, -0.21259095986148335, - 0.03269466951503505, -0.015287307856275205, -0.3933531531430248, 0.32383071967533994, - 0.9495223481098045, 1.0833620725264763, -0.23672723723496208, 0.25863279902498426, - -0.10885560581411625, -0.08108773701133029, 0.040929898074525746, 0.12095893539434234, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17925021958588455, 0.5315807134741416, 0.7673622934600542, 0.7317525616820869, - -0.002114520552946362, 0.02187063183807061, -0.6812161130930021, -0.15545081043712314, - -0.23322896502248472, -0.0009999999999999844, 0.03981688864994382, 0.01981045393611739, - 0.0009999999999999907, 0.061151490366746934, 0.062086685457589885, 0.0009999999999999764, - 0.05122697576692663, 0.2605445337480117, 0.1661519986648763, 0.22287256778861061, - -0.10892889178999138, -0.20072065950398663, 0.301788366538782, -0.21995542564135415, - 0.035445524773803375, -0.013898836143577974, -0.3965525069324173, 0.3222685134641821, - 0.9478640825849779, 1.0813362775920654, -0.23548671615754022, 0.24948960126960038, - -0.10873659846451546, -0.08095758301872649, 0.03895037939990811, 0.1229241587485577, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1782635371485177, 0.530380473971231, 0.7670883293579052, 0.7312026020625277, - -0.0018021490085276455, 0.021723597962459304, -0.6818119918918306, -0.15350776090534207, - -0.2314277111122976, -0.000999999999999991, 0.03691710177152118, 0.019830386922977127, - 0.0009999999999999924, 0.062245752326857494, 0.06684819335327519, 0.0010000000000000195, - 0.050487996916792274, 0.25813638005739736, 0.16282043321210973, 0.21743171995417812, - -0.11261192472943773, -0.20167707428689152, 0.30293458793152683, -0.22768282228355902, - 0.03819241355889103, -0.012683242431594074, -0.3997246059470817, 0.32079227692827605, - 0.9461047109751688, 1.0786572363635512, -0.23453659541041671, 0.24067704332507553, - -0.10875846475085778, -0.08101467138717404, 0.0370137508515339, 0.12467794096529358, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17749686033579756, 0.5292151531897373, 0.7667191279292879, 0.7307098968708327, - -0.0015864878063361306, 0.021629741713272237, -0.6823435234138853, -0.15191071032926715, - -0.2298179675144107, -0.0010000000000000505, 0.03445401264098772, 0.020193703030856328, - 0.0009999999999999799, 0.06323898863313676, 0.0713564590616633, 0.001000000000000017, - 0.0502758480745967, 0.2561389408783265, 0.160332616170856, 0.2110732165969258, -0.11699345557277586, - -0.20304611520481955, 0.3047153765159343, -0.23661866725382835, 0.04113105156041495, - -0.01196909269869221, -0.4029835064396237, 0.31886707676912107, 0.9438546343168761, - 1.0744161286623681, -0.23500692272327603, 0.23320328681976182, -0.10947567411039101, - -0.08122630722991167, 0.03529533447497315, 0.12504405277234779, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17676936535255838, 0.5281091659433343, 0.7664874246850999, 0.730201067438846, - -0.0015104545453557556, 0.021475507789999782, -0.6828930532692493, -0.15015011891874047, - -0.2279977935680385, -0.0010000000000000501, 0.032189831916122486, 0.020764597847086688, - 0.0010000000000000085, 0.06429737654801775, 0.0758669556066154, 0.0009999999999999799, - 0.05015704481147363, 0.2541267034433516, 0.15811455711810307, 0.20481182036874745, - -0.12109826249173547, -0.20391094991861336, 0.30637071064709553, -0.24571341389721285, - 0.04422102066247287, -0.011506676702111803, -0.40594498876422835, 0.3170658432319749, - 0.9419427352565198, 1.0702226583118915, -0.23578587650333924, 0.22599931075483293, - -0.11009566949418588, -0.08143815772228531, 0.033801664234326435, 0.12528582657628595, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17618057132834988, 0.5271407594539881, 0.7668774146933715, 0.7296892988739747, - -0.0016660735722809606, 0.02103729000562475, -0.6834531320704278, -0.1480376556081607, - -0.22596956631958615, -0.000999999999999986, 0.030030197690649445, 0.0214944726756689, - 0.0010000000000000063, 0.06557102536405643, 0.08076587258766126, 0.0010000000000000098, - 0.05058174159018751, 0.25278324046427814, 0.1565962011569952, 0.1967598840106306, - -0.1245314270791337, -0.20332295752717036, 0.3067227509387693, -0.25520783444602985, - 0.04827695911380018, -0.012042428829575208, -0.40757830481819124, 0.31511598867079105, - 0.9417614933217183, 1.0674445712843053, -0.23813504038066924, 0.22004087863227478, - -0.11023958113521222, -0.08125703580202227, 0.03347262903988211, 0.12459757858506124, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17564309458070254, 0.5262144783756683, 0.7672182087853536, 0.7292151250683787, - -0.00191618168203656, 0.02057856806094792, -0.6839723328876907, -0.14584498686560546, - -0.22378334979416642, -0.0010000000000000065, 0.028031855947709954, 0.022325552649854342, - 0.0009999999999999861, 0.06675457303297638, 0.08539393881077545, 0.0010000000000000035, - 0.050933014214193995, 0.2513710614720774, 0.15533002173704233, 0.189063292031587, - -0.12792427747088903, -0.20282837046920618, 0.3071622899020742, -0.26479835470605034, - 0.05211222605125377, -0.01251814103920273, -0.40891102417286246, 0.31333364711969774, - 0.9414644844068574, 1.0642013738805236, -0.24044142400896001, 0.2141397560742733, - -0.1103209662802325, -0.08129158968295438, 0.03320904460298776, 0.12365327524615945, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17531253474401023, 0.525297889533647, 0.7672142371616827, 0.7290174786864374, - -0.002224050365697699, 0.020062491782676304, -0.6841973880344104, -0.1436213997475002, - -0.22146953815773368, -0.0009999999999999948, 0.02604416837109983, 0.022943994065892696, - 0.0009999999999999907, 0.06732237939244935, 0.08888784946485201, 0.0009999999999999935, - 0.050916622093665646, 0.25011553431736927, 0.15470005303444612, 0.18123301584927914, - -0.13184228624208108, -0.20378064484668576, 0.30750377705352, -0.27453731234831463, - 0.05499652208421069, -0.01247331764242579, -0.40876993941558165, 0.3116539609860746, - 0.9405535071213345, 1.0595017829858704, -0.24252103662985278, 0.2084640673330893, - -0.11006350505052565, -0.08223705624624267, 0.0332986064692465, 0.12116380965006063, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17500579147258305, 0.5244196767340448, 0.767217586044189, 0.7288350416483915, - -0.0026008494016108593, 0.019507519349738215, -0.684406439432444, -0.14130553033159432, - -0.21903372244263025, -0.0010000000000000159, 0.024135824993683067, 0.023593492645390436, - 0.0010000000000000122, 0.06782988726849663, 0.09224304810440585, 0.0009999999999999736, - 0.05083418844634258, 0.2488135828417293, 0.15426111606854015, 0.173678630350492, - -0.13547160920785276, -0.2046239547071338, 0.3077733277678641, -0.2842286017738494, - 0.05769820358777154, -0.01241845253099843, -0.40841413472945853, 0.3101007818842704, - 0.9397327130801494, 1.0545614401679348, -0.2446094761142717, 0.20276118213834857, - -0.10981213301648486, -0.08335096867484439, 0.03351758163274345, 0.11836720369600691, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17476431954046717, 0.5235810470687424, 0.7671769097311896, 0.7287929398716859, - -0.0030295314927628954, 0.018786062588271726, -0.6844696900408003, -0.13877894240303018, - -0.21643556871816655, -0.0010000000000000033, 0.02199338391189545, 0.02387831565930867, - 0.0010000000000000063, 0.06789306007103056, 0.09507038085990295, 0.001000000000000012, - 0.05040782512939597, 0.2476358509117019, 0.15433094078164436, 0.1661501604298583, - -0.13809567609595358, -0.20560549316468923, 0.307242551270951, -0.2933280621524018, - 0.059602328770752916, -0.012163220109085856, -0.40695456420084897, 0.3086374397406597, - 0.9394138716814017, 1.049019990858029, -0.2467493077386716, 0.19689286417798035, - -0.10958796483771067, -0.08532600415910149, 0.034456156990433175, 0.11369289761311935, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.174586790853865, 0.5227737457230193, 0.7671628645450639, 0.7287827780962026, - -0.003504729580626051, 0.01803816348310684, -0.6844983592961376, -0.1362255189378204, - -0.2137690898271413, -0.0010000000000000195, 0.01996210761475756, 0.024236272163579688, - 0.0010000000000000083, 0.06788207265393846, 0.09781164704134375, 0.001000000000000015, - 0.049999328136651755, 0.2464791500254526, 0.15448315678406588, 0.1588493244319452, - -0.1405570615607634, -0.2064733025829617, 0.3067436602159194, -0.3022738869318948, - 0.061473015908184415, -0.012073404705020106, -0.40543910825148965, 0.3072878554253357, - 0.9392083407846696, 1.0433359619905294, -0.24884613063729857, 0.19129964695880103, - -0.10934205058698789, -0.08723549023414466, 0.035420132343252995, 0.10893247747316652, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1747085557658181, 0.5219967784443335, 0.76721702588889, 0.729000983684152, -0.004005328019940349, - 0.017199356859819374, -0.6842848129679623, -0.1337571756138336, -0.21110010115543712, - -0.0009999999999999794, 0.01802923751860342, 0.024686317845382545, 0.0009999999999999764, - 0.06733871624907778, 0.10024382384689387, 0.0010000000000000052, 0.04970931858099854, - 0.2457534131698619, 0.154672323277207, 0.15174701939383917, -0.14255808470190295, - -0.2073210216616614, 0.3061806935292953, -0.3100184902395552, 0.06338686747000384, - -0.012865654480597323, -0.4036181083284113, 0.3061832266132921, 0.939662006173545, - 1.037389488756943, -0.25069182127294704, 0.18735799891327282, -0.10895220597152154, - -0.0888257300896403, 0.03653261433767182, 0.1035539930627344, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17488652783117095, 0.5212457718662111, 0.7672690807623839, 0.7292174801780565, - -0.00452937968602059, 0.01635608665349724, -0.6840715092384769, -0.13129018586459895, - -0.20841141492632648, -0.0009999999999999963, 0.016227451762503557, 0.025208699205065646, - 0.0010000000000000083, 0.06682521707512837, 0.10262748565611333, 0.0009999999999999994, - 0.04944514382890923, 0.2450186377307413, 0.1549968406764871, 0.14485811377170704, - -0.14455263869926302, -0.20812432437320053, 0.30568380851676513, -0.3175237353502604, - 0.06523368397200159, -0.013669592449944194, -0.401802142211959, 0.30512254255112503, - 0.9401032870699675, 1.0313117880424072, -0.2526143766072498, 0.18349794526902005, - -0.10852192665785668, -0.09048173275393596, 0.037689181768446635, 0.09810234219677762, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1753172502602698, 0.5205179320691475, 0.7672205609235337, 0.7294549892389756, - -0.005016752549373633, 0.015556942065105711, -0.683833482963365, -0.12903484752034614, - -0.20588919974079645, -0.000999999999999953, 0.01475344000901027, 0.025963498894756656, - 0.0010000000000000085, 0.06639326006991636, 0.1048616499324288, 0.0009999999999999929, - 0.049346271861506176, 0.24446003184688167, 0.15578921209040028, 0.1384103387843926, - -0.14690919647965647, -0.20919660339651627, 0.30543713534289785, -0.3232573024043045, - 0.06693647047594935, -0.014500325102292905, -0.400004924406641, 0.3040603736223479, - 0.9404704455933324, 1.024886041067178, -0.2549960163215018, 0.1801744924838537, - -0.10782906258199874, -0.0925724147940285, 0.03911516357584446, 0.09208283753342186, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1757737129831205, 0.5198182138211735, 0.7671935607347697, 0.7296990282816702, - -0.0055068673022517205, 0.014759176802869974, -0.6835869873230495, -0.12676437710909758, - -0.20339237376600167, -0.000999999999999996, 0.013332430846973874, 0.026707198488961057, - 0.0010000000000000028, 0.06596011493461151, 0.10701519166167424, 0.001000000000000054, - 0.0491702394451027, 0.24392613763662668, 0.15657940569465512, 0.13212286895639075, - -0.14914509107877327, -0.2102086889334236, 0.3051633949212578, -0.32880088377370204, - 0.06849286783406246, -0.015314182190752165, -0.3981500532838317, 0.30304954669269085, - 0.940966643757774, 1.018419152576139, -0.25723783011069346, 0.17679677834086413, - -0.10715080779953559, -0.09464777855168728, 0.040527749507826547, 0.08613565289274323, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17630171462491306, 0.519189855980517, 0.767219144209355, 0.7300079134683205, - -0.005881429426540734, 0.014028404058304042, -0.6832693897293409, -0.12455286429515403, - -0.20126186005519223, -0.0010000000000000087, 0.011821492484088848, 0.027254332558896067, - 0.0009999999999999848, 0.06542891594496328, 0.10876524487542219, 0.0009999999999999681, - 0.04851309322481135, 0.24371382565124555, 0.15708359615132636, 0.12623580836721962, - -0.15095289056241779, -0.2112451517287239, 0.30461228898970943, -0.33288466026362884, - 0.06939314526738728, -0.016009410841284516, -0.3959235197234641, 0.3022053892992822, - 0.9422880658560491, 1.0120383184734192, -0.2585748677195257, 0.17314910354498683, - -0.10653616555987463, -0.09668251582740164, 0.04185387632008253, 0.08057135088711422, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.176853870847108, 0.5185866783142395, 0.7672634419869881, 0.7303455227241831, - -0.006264240115420585, 0.013296655446618344, -0.6829197432249635, -0.12234139155975254, - -0.19914837328372917, -0.0010000000000000334, 0.010364113822057559, 0.027782086619049077, - 0.0009999999999999788, 0.0648549590448998, 0.11039913814861281, 0.0010000000000000228, - 0.04784319807950621, 0.24357215401594476, 0.15756993308895403, 0.1205222349186952, - -0.15265128759469096, -0.21219533880614821, 0.3040311485129487, -0.3368257385293517, - 0.07029636510420495, -0.01672388095745934, -0.39369251065576105, 0.3014860366736683, - 0.9436788097664138, 1.0057281087752055, -0.2597099804729828, 0.16967551416739926, - -0.10592302817114359, -0.09864593841677459, 0.04315753324896108, 0.07511814555174931, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17747666118380753, 0.5180568699409362, 0.7673527887701099, 0.7308914860204934, - -0.006609937026346323, 0.012616438268002236, -0.6823450519207225, -0.12025895378557899, - -0.1972893765073837, -0.0010000000000000302, 0.008894891170528899, 0.028153964781320227, - 0.0010000000000000048, 0.06391853730398836, 0.11134388758754202, 0.0010000000000000228, - 0.047096996664781426, 0.2440329904893331, 0.15773095799291623, 0.11548586296070153, - -0.15388757051033913, -0.21292105975114398, 0.30320967089054013, -0.33957929642547174, - 0.07150021862982402, -0.01759986483959672, -0.39140111347857137, 0.30154981853483076, - 0.9455366851800242, 1.0001652427537298, -0.25947441515293884, 0.1674410512537613, - -0.1053054058350652, -0.10021730932150447, 0.0443149861719044, 0.07037674696300025, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1781144396421987, 0.5175459916492422, 0.7674267668655756, 0.7314391276299985, - -0.006946947821280826, 0.011955356402846258, -0.6817665377100262, -0.11820964952858734, - -0.19547174326608424, -0.0010000000000000492, 0.007501317278191215, 0.028523897639623354, - 0.0009999999999999955, 0.06298605988905655, 0.11222036258795925, 0.0009999999999999998, - 0.04632476196239715, 0.2444837901233153, 0.15784159529367356, 0.11058096343633254, - -0.15506784915255478, -0.21366522705167615, 0.3024204572069423, -0.3421656710922132, - 0.07260644861314122, -0.01850540890330651, -0.38916245002696304, 0.30161400506985536, - 0.9474276828101627, 0.9946387396276336, -0.259258805636288, 0.16510210855785606, - -0.10468769558368676, -0.10179492996717716, 0.04537283587381281, 0.0658039340508194, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17876637752985752, 0.5170839452649293, 0.7673207736703727, 0.7320101092739676, - -0.0071582575274325095, 0.011478541776628777, -0.6811594544220665, -0.11651546659091762, - -0.19402674998707647, -0.0009999999999999887, 0.006291698314467222, 0.028917912976196683, - 0.0009999999999999987, 0.06200699753730639, 0.11269131999091932, 0.0009999999999999658, - 0.045382525135658255, 0.24495584524253575, 0.1574317996703434, 0.10623580615598631, - -0.15608161763023104, -0.21483092806497028, 0.3018357296218128, -0.34338143594766046, - 0.07331532380308255, -0.01969321284498161, -0.3872570270088992, 0.301687250743096, - 0.9495562137436194, 0.9895946333990278, -0.2591592090640264, 0.16206529887768262, - -0.10407957650857112, -0.1034275056328968, 0.045739431891763556, 0.062357480757100466, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.17942302148916642, 0.5166410008391925, 0.7672440580433406, 0.7325891109722849, - -0.007372424863438163, 0.010995796692097756, -0.6805423824359323, -0.11479332867931541, - -0.19257558210506065, -0.0009999999999999885, 0.005130919963734816, 0.029287889930757784, - 0.0009999999999999916, 0.06104745594663927, 0.11312450552731582, 0.0009999999999999935, - 0.044408619521079294, 0.24543260057903657, 0.15698758048989248, 0.10198148122013805, - -0.1569860405100179, -0.21591207954242017, 0.3012027731640274, -0.34449945714545477, - 0.07394506433119089, -0.020893370296901668, -0.38538414798219456, 0.30197295948029174, - 0.9518028725695384, 0.9847355891113506, -0.25907519247693234, 0.15916628954362708, - -0.10347163902318302, -0.10494239591760565, 0.04604116084739883, 0.05901552079755927, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18003177467020692, 0.5162716191150991, 0.7673176485046354, 0.7332354324802653, - -0.007564314064117901, 0.010510816531015568, -0.679851531177262, -0.11296701086791319, - -0.19112366678107776, -0.0009999999999999612, 0.004012343032362023, 0.02956610535409222, - 0.0010000000000000206, 0.060162030713232006, 0.11333532351228173, 0.001000000000000003, - 0.04321307651048397, 0.246017083358717, 0.15617130097555057, 0.0981079540199569, - -0.15725896012591517, -0.21660540829480165, 0.3002009663098353, -0.34472655087998344, - 0.07426984111541453, -0.02228408228849082, -0.3837177526902489, 0.30386542270099304, - 0.9549301420724423, 0.9814255274507871, -0.2590854526200416, 0.15731680116025418, - -0.10286983536278005, -0.1056218165931879, 0.045871166804891346, 0.056434539830813236, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18064173812723078, 0.515915182241635, 0.7674001162628649, 0.7338992006472125, - -0.007752555520845002, 0.010034920351043383, -0.6791400161570699, -0.11115821963758604, - -0.18967983121277837, -0.001000000000000015, 0.002938901423381746, 0.029814165228253094, - 0.0009999999999999933, 0.05924236286652439, 0.1134560390772382, 0.0009999999999999907, - 0.04199078600601042, 0.24657349632333964, 0.15530566556530345, 0.09438084780607157, - -0.1574444186451349, -0.2173047051132851, 0.299186392199382, -0.34480554988524886, - 0.07453524603051072, -0.023661785929884313, -0.3820792336510296, 0.3057257341389744, - 0.9581015060249081, 0.9782197116875163, -0.2590068807125365, 0.1553274448065115, - -0.10223209564835414, -0.10626948263695464, 0.045633500915567234, 0.05400764021990716, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1811841030405124, 0.5155998590224009, 0.7674937918606412, 0.7347060571307115, - -0.007884694434938798, 0.009668019025373427, -0.6782708681767458, -0.10958174244771807, - -0.1883206503000901, -0.0009999999999999716, 0.0019172837682813078, 0.029960066484399597, - 0.0010000000000000284, 0.05798385387832976, 0.11291659459309679, 0.000999999999999991, - 0.040572347626466256, 0.246968714593822, 0.15396053212589603, 0.09157022904664476, - -0.15708858267393386, -0.21825893024246595, 0.2980610301246181, -0.34358336733837713, - 0.07461450562952385, -0.025065403520471943, -0.3806260677632573, 0.307382648616933, - 0.9616242998995417, 0.975977901587612, -0.2582031135830116, 0.15220880261620115, - -0.10131685438001782, -0.10665085000916022, 0.044885143104589784, 0.052743046033421725, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1817296985175675, 0.515293793416575, 0.7675865861180697, 0.735521479856293, - -0.008017876633837445, 0.00930229576534996, -0.6773900896955805, -0.10800548625776851, - -0.18695405864447256, -0.0010000000000000382, 0.0009500661994088477, 0.030089061148674188, - 0.0010000000000000024, 0.05669941676815381, 0.11233810081123591, 0.001000000000000015, - 0.0391321205665375, 0.24732511213905425, 0.1525673748404353, 0.08886453269838425, - -0.15662765554383434, -0.2191914280213364, 0.29688075181727797, -0.34224298247671864, - 0.07461591306427827, -0.02644882493932543, -0.37922267831166145, 0.3091382163127391, - 0.9652462789789376, 0.9738873509036783, -0.25733714379619826, 0.14910168102850968, - -0.1004286266930394, -0.10702243454022829, 0.04410489503501608, 0.05159551058274382, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1822356394338519, 0.5150144223218565, 0.7676211383413638, 0.7364154525878925, - -0.008147802003469927, 0.008973635521153146, -0.6764209993619462, -0.1065208219742823, - -0.1855243392109465, -0.0010000000000000816, 0.00015444744009101614, 0.030236339895413973, - 0.0009999999999999957, 0.05513233054338274, 0.11146925227010925, 0.000999999999999978, - 0.037525390900523654, 0.24740822687672845, 0.15070374867792097, 0.08681965103562422, - -0.15540197541634618, -0.22011800405268173, 0.2952583857853494, -0.33978647779910703, - 0.07422938740796524, -0.027805654570238394, -0.37819891469426314, 0.3117721467542121, - 0.9696954056853699, 0.9731670894325254, -0.255936464427771, 0.14604566541999792, - -0.09976600709065458, -0.10731085566076178, 0.043077607063909854, 0.05139770080483018, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1827521332237755, 0.5147398050939628, 0.7676419483922201, 0.7373167026613306, - -0.008272218432400742, 0.008650355923337778, -0.6754412052290184, -0.10504996161319971, - -0.18411638690250093, -0.000999999999999996, -0.0005876366929290143, 0.0303696632412319, - 0.0010000000000000126, 0.05355546706364224, 0.11055486574599603, 0.001000000000000014, - 0.03590645473246777, 0.24749291810424753, 0.14884197843081245, 0.08487753509787425, - -0.15418187882585424, -0.2210767202092616, 0.29363792205005973, -0.3372460349850296, - 0.07376307119743322, -0.029140667108607767, -0.37723940541250317, 0.3144418236046779, - 0.9741482380546138, 0.972521561358905, -0.2544413067989286, 0.1430066139604924, - -0.09914593954221657, -0.10758437436404007, 0.04199379071995017, 0.051318904796346015, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18330646440178117, 0.5144598842717636, 0.7674997674083768, 0.738291687807888, - -0.008346358276587607, 0.0083920890719743, -0.6743777093426999, -0.10378372429635217, - -0.182890414084663, -0.000999999999999981, -0.001123556420881751, 0.030551172108054216, - 0.0009999999999999879, 0.05183454767735995, 0.10925967206906644, 0.000999999999999984, - 0.034194897513063004, 0.24763358746679093, 0.14694625769285602, 0.08362055717375923, - -0.15309112954072354, -0.22246469252455625, 0.2920395495145955, -0.3338561300528361, - 0.07283173757282667, -0.030441405900578436, -0.376834195571584, 0.3174626767922441, - 0.9786373990283982, 0.9726231733577495, -0.2520951425971959, 0.14006881764579313, - -0.09889866724180928, -0.107701596478717, 0.04043687808209477, 0.05228333406963799, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18386478295298148, 0.5141879969547903, 0.767386993343667, 0.7392662613965837, - -0.008422787664329376, 0.008120628767575598, -0.6733115971057521, -0.10247423182189577, - -0.1816433431986535, -0.0009999999999999662, -0.001638385681860188, 0.030702090808326303, - 0.0009999999999999996, 0.05011095045505476, 0.10796310368911384, 0.0009999999999999868, - 0.032456456256901134, 0.24776945381777274, 0.14507881416788138, 0.08242874722246668, - -0.15193770653036354, -0.22377820977197405, 0.2903488781487858, -0.3303341795384478, - 0.07186345281194169, -0.03172852935981314, -0.3764536289524456, 0.32051388518264856, - 0.9832074260277469, 0.9729061622196331, -0.24968756570045425, 0.1371300520974399, - -0.09865957649303604, -0.10778134036346726, 0.03884994698551588, 0.053317646673255545, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18441289661479632, 0.5139531887008747, 0.7675155520125224, 0.7402499639940963, - -0.00853176810809764, 0.007734297569192474, -0.6722331294876802, -0.10083592348617307, - -0.18017943186005805, -0.000999999999999995, -0.0022103044250853516, 0.03074946247627904, - 0.0009999999999999998, 0.048311211577588616, 0.1066643385661429, 0.0009999999999999972, - 0.030485378671865997, 0.24789913066950595, 0.1434383079709286, 0.08164792947290962, - -0.15024218594020142, -0.22451333382689262, 0.28782026321886756, -0.32547184490942555, - 0.07076959700484847, -0.033053413007946515, -0.3762801549381473, 0.32390190766155197, - 0.9885378981350624, 0.9749699528808734, -0.24670329287822246, 0.13413694735698115, - -0.09846938829893989, -0.10746173968093954, 0.03698414363799626, 0.05500267337572557, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18496760644654708, 0.5137244076758524, 0.7676448117012938, 0.7412367521205901, - -0.008637857153612004, 0.007343188838538496, -0.6711479287811256, -0.09919576526759027, - -0.1787148180767763, -0.0010000000000000087, -0.00276823208168738, 0.030754825252981347, - 0.0009999999999999898, 0.046485055947393, 0.10531455692436803, 0.0010000000000000106, - 0.028531912678412888, 0.2480418340744275, 0.14183745380504356, 0.0809293916952272, - -0.14856695980844722, -0.22526251466148953, 0.28526794140014594, -0.3205489838604651, - 0.06965491492622337, -0.03430686740441182, -0.37613780305637967, 0.3273247414824077, - 0.9938473961919657, 0.9771337970902513, -0.24371379244460545, 0.13112343154203304, - -0.0982940028133201, -0.107151064265997, 0.035069667387784144, 0.05677825467064475, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1855471220996078, 0.5135226981990318, 0.7677511346309925, 0.7422686042137222, - -0.008736982650567061, 0.006908488938725975, -0.6700098932951366, -0.09760937472599085, - -0.17722157596111612, -0.0010000000000000204, -0.003423002708144407, 0.030538077021338695, - 0.0010000000000000072, 0.04435695907623541, 0.10346581709961412, 0.000999999999999996, - 0.026774727157243744, 0.24834978006412622, 0.14059198535271827, 0.0806068192780306, - -0.1471220604174235, -0.22626122480338265, 0.28251215333379276, -0.31493395324298196, - 0.06852745630957566, -0.03501731891995627, -0.37629399708020517, 0.331117060885288, - 0.9989465700314942, 0.9803603244718376, -0.24066542115482037, 0.127877133417341, - -0.09822249140984766, -0.10687122675658818, 0.0326774700405643, 0.0594711409557404, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18613562162379074, 0.5133226836378091, 0.7678463398332162, 0.7432954908138522, - -0.008830378377984985, 0.006480814422904412, -0.6688735581542397, -0.09602249992341037, - -0.17574673103422064, -0.0010000000000000167, -0.004042390904572897, 0.030311418409728935, - 0.001, 0.042254490074497435, 0.1016193088569284, 0.0009999999999999983, 0.025012137752352958, - 0.24866538487468343, 0.13940918186660614, 0.08032573704899319, -0.1457332813336379, - -0.22726236265073615, 0.2797613126745573, -0.3092803910742829, 0.06741816331101756, - -0.03572273750983461, -0.37642410877153837, 0.3349717633498078, 1.003938564469858, - 0.9836246636814944, -0.23760999580713468, 0.12467384298578259, -0.09817247338483012, - -0.10657160479182823, 0.030297099128141436, 0.06222043513109742, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18678144158401766, 0.5131116499583288, 0.7677946241005865, 0.7442875206815623, - -0.008899279811567363, 0.006124569236780867, -0.6677719513637812, -0.09450673186297935, - -0.17444788534214103, -0.0009999999999999946, -0.004499191569596423, 0.030180715454277956, - 0.0009999999999999868, 0.04040051423107621, 0.09981047325295513, 0.0010000000000000154, - 0.023218645392085326, 0.2490955627042483, 0.1388616603465934, 0.08027474382409007, - -0.1449627634078797, -0.22838234949670577, 0.2771125562702199, -0.3031590620642639, - 0.06668619742345654, -0.03654568256504568, -0.37625621210099436, 0.3394899557657015, - 1.0077717589802184, 0.9873452899936447, -0.2344835874913381, 0.12193605229602195, - -0.09828397807799633, -0.10595565616481004, 0.028037988020538925, 0.06559919435596487, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18743426947297884, 0.5129054110125397, 0.7677611418828202, 0.7452854499517806, - -0.008963767915189267, 0.005764908379451629, -0.6666603443930893, -0.09298678801631285, - -0.17314999490377017, -0.0010000000000000187, -0.004953050397592636, 0.030012529341858246, - 0.0010000000000000113, 0.03852957267144224, 0.09798069602802256, 0.0010000000000000128, - 0.02143254233471331, 0.24953457248089778, 0.13831339726531816, 0.08027728561477179, - -0.14415224221768747, -0.2294614305800198, 0.2744593042521009, -0.2970287265846481, - 0.06591620555389517, -0.03733725419541543, -0.37603624770336885, 0.34405667537779433, - 1.0115768502975633, 0.9911371642190541, -0.23134060078386104, 0.11919286511564052, - -0.09834444674087714, -0.10535189775724987, 0.025778510237798556, 0.06901222365457942, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18813436174902667, 0.5127293220791435, 0.7679182543901302, 0.746370872155722, - -0.009009302142794709, 0.0053572194879725876, -0.6654477093444416, -0.09148949712855016, - -0.1718386795167552, -0.0009999999999999807, -0.005598148055907428, 0.029600463972470056, - 0.0009999999999999966, 0.03643329325617877, 0.09591946681865594, 0.0009999999999999731, - 0.019765761310359783, 0.2500978176024638, 0.1377484593470301, 0.08060934137288427, - -0.14289351887484206, -0.23013693082322248, 0.2717848864396541, -0.2907856757306077, - 0.06487705170434482, -0.03793260912374469, -0.3751981168306832, 0.3491874348782332, - 1.015053940014427, 0.9957987597849218, -0.22800665246283927, 0.11642607055609772, - -0.09773242402715186, -0.10478642053248756, 0.023507668170898183, 0.07285376834770148, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18884121806360796, 0.5125547868260169, 0.7680759180613448, 0.7474557161466329, - -0.009050490753530349, 0.004956969554075819, -0.6642314878637453, -0.09000546548310467, - -0.17053407994010153, -0.000999999999999999, -0.0062201714637137515, 0.029174635066643587, - 0.0010000000000000076, 0.03432165087365869, 0.09384524661352768, 0.0009999999999999666, - 0.01812027727263984, 0.2506511659232004, 0.1371917874231929, 0.08099791590954548, - -0.14165845528465876, -0.23079635508747076, 0.2691356431908391, -0.2845554094774437, - 0.06382541443109734, -0.03849923884196376, -0.3743162851725088, 0.35435314291778836, - 1.018471700972419, 1.0004675629689586, -0.2246796621843775, 0.11370599394556655, - -0.09715633666929115, -0.10423108606625082, 0.021243544772805847, 0.07667930962241253, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.18960401961063345, 0.512375362043669, 0.7682233478883677, 0.7485593934408437, - -0.009080790507312359, 0.004633680289589877, -0.6629893684986312, -0.08875575463299688, - -0.16928818990356784, -0.0009999999999999879, -0.006762046703098097, 0.028765620908514776, - 0.0010000000000000002, 0.03200047026590702, 0.09162468022158408, 0.0010000000000000007, - 0.016770936257151828, 0.2511013264094563, 0.13677412175955442, 0.0817454688865305, - -0.14071820130145976, -0.23132225247442906, 0.2668352839068026, -0.27850121660049515, - 0.06277331474163851, -0.038871715017243735, -0.3728828367633381, 0.35989427467037927, - 1.0211403821360385, 1.0052617263876837, -0.2214685325576677, 0.11164783673850122, - -0.09694277118259548, -0.1036761993994432, 0.019070471354208283, 0.0803505227109729, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19037174246623076, 0.5121971485662887, 0.7683588015919978, 0.7496611370679459, - -0.009098153240785563, 0.004316190585219199, -0.6617452483217654, -0.08753036024650063, - -0.1680601927217572, -0.0009999999999999731, -0.007305456426831244, 0.02833203805080327, - 0.0009999999999999929, 0.029674172968492084, 0.08940537299801106, 0.0010000000000000137, - 0.015442201003863607, 0.2515493940553332, 0.13635799647468755, 0.08254876786643169, - -0.1398228872358618, -0.23188749271077336, 0.26456913604114185, -0.27252307933557457, - 0.06172277181862622, -0.03925099954809947, -0.37145786274278886, 0.3654232186273497, - 1.0237256186017938, 1.0099921574820288, -0.2183204884323369, 0.10957744349944448, - -0.09670412115431658, -0.10316650143354211, 0.016948523667108556, 0.08400176395072088, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19117954413552926, 0.5120095592392284, 0.7683158360085812, 0.7507644739239829, - -0.008975157780225281, 0.004053317308411628, -0.6604965721752563, -0.08670199538117411, - -0.16704780816460865, -0.0010000000000000143, -0.00808604195732124, 0.027726292273024564, - 0.00100000000000001, 0.027261550490580204, 0.08722150722779967, 0.0010000000000000026, - 0.014425765457165453, 0.2519790309491976, 0.13602062106183135, 0.0836248019305461, - -0.13954256830299724, -0.23305276744430636, 0.262768928504972, -0.2675811669736552, - 0.06084858260605227, -0.03989057533872491, -0.37016321289157, 0.3706767604077585, - 1.0251630095880073, 1.0138720657363642, -0.21604851010441298, 0.10741654211668444, - -0.09603295731539771, -0.10318877393818732, 0.015525873374412085, 0.08743239188336031, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19199211057711385, 0.5118237638580463, 0.7682802382710103, 0.7518620664341794, - -0.008846001484631157, 0.0037925135955588973, -0.6592501787301139, -0.08587966481642871, - -0.16603375858433778, -0.001000000000000027, -0.00885375497297244, 0.02710648352242271, - 0.001000000000000001, 0.02486388444796281, 0.08504000601808634, 0.0009999999999999987, - 0.013420512437401697, 0.2523865068752495, 0.1356735431218641, 0.08473267747292092, - -0.13923690562662655, -0.2341876126032648, 0.2609994187233827, -0.26270935376690124, - 0.05995599000111347, -0.04051124100527909, -0.36888680888479447, 0.3758979675381238, - 1.0265884428032381, 1.0177258909550015, -0.21372961709453, 0.10525199730049692, - -0.09538058922160338, -0.10322115851024602, 0.014127324363631665, 0.09083807533338725, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19285094185912507, 0.51164200247161, 0.7683496314577781, 0.7529083816069612, - -0.008669381110360029, 0.003526412087642296, -0.658058793083823, -0.08520522850942483, - -0.16496629735926785, -0.0009999999999999523, -0.009629257668183216, 0.026422891911182654, - 0.0009999999999999907, 0.02267035924425589, 0.08290322413080686, 0.0009999999999999987, - 0.012608762425678067, 0.2524691215289304, 0.13524427091816726, 0.08588776280770544, - -0.1385445514153639, -0.23491101614851317, 0.2596915357291359, -0.25886273656126996, - 0.058938491900302904, -0.04099436876364738, -0.36789216100011013, 0.3805485389226929, - 1.0278338466442625, 1.021209892034458, -0.21072383618484464, 0.103077610926179, - -0.09496521254652363, -0.10330679462903852, 0.013099555848635973, 0.09391801406978537, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19370894000171687, 0.511460801602774, 0.7684149586655442, 0.7539433886479695, - -0.008488125927744603, 0.0032654278038909412, -0.656876438467318, -0.08453916107370806, - -0.16391271902939417, -0.0010000000000000141, -0.010392967913133325, 0.025737100360254898, - 0.0010000000000000148, 0.0204957559546527, 0.08079680664737746, 0.0010000000000000063, - 0.01180497311812469, 0.25255229318697847, 0.13483134124642374, 0.08707858746872991, - -0.1378749802488842, -0.235657451567021, 0.2584353565806709, -0.2551010440181452, - 0.05793480115170105, -0.041475577916996534, -0.3669177345789707, 0.38515046009515386, - 1.0290312879889083, 1.0246038751105384, -0.20774937533110366, 0.10091244724073613, - -0.0945630792857414, -0.1034085275338959, 0.012054491060033085, 0.09697026488553759, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1945323091643652, 0.5112725531494472, 0.7684085952170798, 0.754825341472199, - -0.008273928218499649, 0.0030536692666821874, -0.655866542131264, -0.08406113777162302, - -0.1630768597327867, -0.0010000000000000245, -0.011143707171809448, 0.025162458225630433, - 0.000999999999999992, 0.018617864469779564, 0.07920741060493405, 0.0010000000000000208, - 0.011150891116494414, 0.2526615427491366, 0.13473335925616156, 0.08856925427624847, - -0.137571291143881, -0.2368292645373341, 0.25803418917716936, -0.25274385796969484, - 0.05730511459040007, -0.04207816680734948, -0.3662855472822033, 0.38883051137573704, - 1.0294279913939008, 1.0265118270459255, -0.20530927267890914, 0.09893592513688221, - -0.09433285194354467, -0.1036934399806433, 0.010725046934283116, 0.09962466574589562, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19536189528101727, 0.5110843046341189, 0.7684072790124856, 0.7556985595961477, - -0.008058583657723854, 0.0028470660670869698, -0.6548638335322453, -0.08359981769068181, - -0.16224031507951509, -0.0010000000000000072, -0.011869521524217254, 0.024594068227929554, - 0.0010000000000000113, 0.016759968935021283, 0.07763847092749226, 0.0009999999999999916, - 0.010519119279775448, 0.2527394245645334, 0.13464861001021955, 0.09007491247020136, - -0.13726816609090212, -0.23797336667078967, 0.2576459547216719, -0.2504377763544663, - 0.05663077485677915, -0.04264814553302183, -0.3656752569585254, 0.39243428622248094, - 1.0297971503182537, 1.0283847442722844, -0.20293585426054767, 0.09697666246221646, - -0.09408697309662087, -0.10398457564620542, 0.009439470860707252, 0.1022448092348342, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19628918519439978, 0.5108858540932449, 0.7684994919282799, 0.7564388949135072, - -0.007859934848438064, 0.002701673219790567, -0.6540115600262913, -0.08348403243604816, - -0.16138004570078407, -0.0010000000000000232, -0.012305841402029904, 0.0242871333339454, - 0.0009999999999999976, 0.015251439545753268, 0.0764536403714998, 0.0010000000000000198, - 0.01031343177368834, 0.2522444473961259, 0.1348214386090185, 0.0916926639895263, - -0.13697866785376753, -0.238627600959817, 0.25750442741411755, -0.24905574824838225, - 0.05526728208131527, -0.04275021514287114, -0.3654700791576492, 0.3945746820074247, - 1.0296598858125021, 1.029621435763068, -0.20181328339360222, 0.09534212098660091, - -0.09349577930780463, -0.1043117520331512, 0.008978034575765695, 0.10425774413960959, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19722019534712076, 0.5106869592526748, 0.7685913191110944, 0.7571718665982025, - -0.007658538110199698, 0.0025609956630579596, -0.6531657925269986, -0.08338098464274028, - -0.16053048933688782, -0.001000000000000012, -0.012730595978309027, 0.023977628513691458, - 0.0010000000000000141, 0.013757671929256358, 0.07528356070048822, 0.001000000000000008, - 0.010115879377974316, 0.25174087436904347, 0.13500978532496394, 0.0933070566906727, - -0.13670378618800164, -0.2392734438575684, 0.25738955236764804, -0.24773221131220868, - 0.05392361140004916, -0.042861644637245136, -0.3652661934934501, 0.3966805597605873, - 1.0294871961376537, 1.0308152867266944, -0.20072505018319994, 0.0937355569298272, - -0.09291031185694118, -0.10462654306662407, 0.00850996055644662, 0.10626514712322471, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.19821479433722858, 0.5104644136155552, 0.7686699438288928, 0.7577738966678292, - -0.0074316264248644, 0.0024889683595483852, -0.6524701506536836, -0.0835673214337315, - -0.15990557479429374, -0.0009999999999999816, -0.01307608908712473, 0.023756235419370483, - 0.0009999999999999916, 0.012554218436216675, 0.07442872043977881, 0.0009999999999999707, - 0.010098318403177256, 0.2510723514986098, 0.13552119442933028, 0.09478511140129915, - -0.13673663511647993, -0.2397673400601243, 0.25784492202420967, -0.24763063421398607, - 0.05314864889355635, -0.04332218301486211, -0.3650910988818905, 0.39802203126169666, - 1.0285696437645444, 1.0311195986066832, -0.20036947952849682, 0.09271234831789088, - -0.09242996648575007, -0.10461463686850049, 0.007913835258404603, 0.10817334147056401, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.1992111280840634, 0.5102426802953228, 0.7687499353418045, 0.7583676603271136, - -0.00720700664472302, 0.002419378255845913, -0.6517827072223215, -0.08375277132343663, - -0.1592876030174607, -0.0009999999999999488, -0.0134088310187247, 0.023536579773193348, - 0.000999999999999992, 0.011358687497986002, 0.07358650577830719, 0.0009999999999999814, - 0.010081584364603309, 0.25040803097124015, 0.1360405020355054, 0.09624896947546309, - -0.13675609500355965, -0.24024975292306625, 0.2583033600074274, -0.24756873260432533, - 0.05236922428422705, -0.04377782008901178, -0.36490873718057965, 0.39935093867689014, - 1.0276517330891843, 1.0314098385802892, -0.20002776638915198, 0.09169032541675913, - -0.0919514397039148, -0.10458233670244184, 0.0073185044068582415, 0.11007311217494593, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20023949526479168, 0.5100260891363475, 0.7688626337602765, 0.758781427321579, - -0.007063619319521479, 0.002374849362825509, -0.6513027029918124, -0.08393160804586818, - -0.1588352787224454, -0.0010000000000000176, -0.013609245694833565, 0.023503998427383096, - 0.0009999999999999996, 0.010329660560685908, 0.0730500309768851, 0.0010000000000000024, - 0.010100586365308542, 0.24985583935373062, 0.13673800886548246, 0.09734058053139294, - -0.13646530206912957, -0.24046758565454784, 0.2588369975069497, -0.248470779097163, - 0.051640515972468375, -0.044270927913201384, -0.36456328570335317, 0.40039485489335364, - 1.0267324229074708, 1.0313730863017112, -0.200021180541134, 0.09069284955354451, - -0.0915226587992695, -0.10402011744992166, 0.006753254457226376, 0.11176869926546996, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20127006681126533, 0.5098088834838097, 0.768971631655965, 0.759195332961207, - -0.006918895701526999, 0.002332180458542552, -0.6508218928604838, -0.08411874723413709, - -0.15838193515464757, -0.0009999999999999731, -0.013807885667590104, 0.02345842347045487, - 0.0009999999999999987, 0.009301769571572778, 0.07250534584178424, 0.0009999999999999916, - 0.010125365960746658, 0.24929137619777103, 0.13743500081658655, 0.09844356742563756, - -0.1361728236426209, -0.24068798358796534, 0.25937577893129427, -0.24940394852749304, - 0.050901455755012884, -0.0447459722024624, -0.36422387660353767, 0.40140891198518563, - 1.0258006786935663, 1.03130972985249, -0.20002088431545387, 0.08968867960542481, - -0.09109509286276911, -0.10348186092978424, 0.00618496655299503, 0.11343881197690181, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20235527534179143, 0.5095606654510741, 0.7689776609677971, 0.7596241784656967, - -0.006749302728673337, 0.0023147904005019673, -0.6503231474800767, -0.08454935139934153, - -0.15790121727037068, -0.001000000000000006, -0.01414115171546798, 0.023187929145510232, - 0.0009999999999999833, 0.008292967368537935, 0.07172985157713141, 0.0010000000000000102, - 0.010329407802043332, 0.24838536613655157, 0.13809481190956524, 0.09984965407780277, - -0.1358668960048009, -0.24098581449375683, 0.26005862820797443, -0.25121636688886234, - 0.05002807361088598, -0.04486114576654113, -0.3640737543395793, 0.4016063174309742, - 1.0245183827050537, 1.0305071347703567, -0.20020122152563025, 0.08852031859061975, - -0.09071001137144302, -0.10358504975652141, 0.005534367246311922, 0.1143741534253707, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20344321652273434, 0.5093110524530543, 0.7689779870560021, 0.7600506854593273, - -0.006579373417662582, 0.002302421121839768, -0.6498264123865806, -0.08498774339176908, - -0.15742830260143983, -0.0009999999999999532, -0.014460519912350691, 0.022923156818629106, - 0.0010000000000000024, 0.00729136141597277, 0.0709661647610199, 0.000999999999999958, - 0.01053482719283122, 0.2474770897759357, 0.13875310621742357, 0.10124486471204833, - -0.13557467951377647, -0.24128595643622122, 0.2607392372340527, -0.2530397064716575, - 0.049133740061392386, -0.044969713027549124, -0.36392403455866645, 0.40180989477315243, - 1.023227274663559, 1.029696596784041, -0.2003905415568018, 0.08735008199481713, - -0.0903223562304559, -0.10369447624194565, 0.004899262650710826, 0.11529761894463492, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20461769817542805, 0.5090018449458379, 0.768786009069252, 0.7604108395545797, - -0.006415525492751035, 0.002431126569703795, -0.6494061023305471, -0.08570088528514794, - -0.1572255801708415, -0.0010000000000000178, -0.014489072693972777, 0.023005473191830893, - 0.0010000000000000022, 0.006524408103616169, 0.07061323773862119, 0.0009999999999999903, - 0.010802988955809363, 0.24649924743451476, 0.13932399138930812, 0.10222642995649199, - -0.13577979603872165, -0.24167274519337387, 0.2613436868207782, -0.25525426360426884, - 0.04769049931084059, -0.04502493140663765, -0.36382480758452224, 0.4023421790417689, - 1.0216425140057734, 1.0286342402546038, -0.20087549649621922, 0.08607717885057668, - -0.08986878098023528, -0.1039559510980665, 0.004807456395389673, 0.11579780367090285, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20579085280941936, 0.5086939087588083, 0.7685981173406753, 0.7607676744891214, - -0.00625315166599844, 0.0025599990724873667, -0.6489891293014112, -0.08641671701342304, - -0.1570269924852304, -0.0009999999999999384, -0.014512888299300112, 0.023083535982443695, - 0.0010000000000000189, 0.0057562909777843195, 0.07026047419782967, 0.0010000000000000005, - 0.011078520873881515, 0.24552586351722538, 0.13989050231979813, 0.10318887650779977, - -0.13597083359254128, -0.24203958769128522, 0.2619279969619184, -0.2574667392031413, - 0.046266339601863875, -0.04507686022765422, -0.3637220101728986, 0.4028749695749833, - 1.0200750431898955, 1.027597997029422, -0.20135878707736046, 0.08482119258177491, - -0.08941841062369973, -0.10421749578630925, 0.004739724860544541, 0.11628295971222771, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20690515307982316, 0.5084334834183949, 0.7685920230768588, 0.7609945249571003, - -0.006158316546450964, 0.0026763318145216876, -0.6487235508062349, -0.08725747788068496, - -0.1570161285471651, -0.0009999999999999874, -0.014527522963898297, 0.023143760043257404, - 0.0009999999999999842, 0.004916534102208615, 0.06991137891837423, 0.000999999999999965, - 0.0116859166348002, 0.2447560210902825, 0.1402486177250728, 0.10336278705827036, - -0.13557973625021186, -0.2415449864067459, 0.26162567647514556, -0.2595530158345076, - 0.04585475611170447, -0.04516762589487419, -0.36350284712876696, 0.40354510937680954, - 1.0192646346814371, 1.0277072381589931, -0.20175089692869, 0.0842442400081791, -0.08912572205579335, - -0.10441199983253502, 0.0057226416023994945, 0.11611865222167457, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20801792328632346, 0.5081722198767642, 0.7685817066065732, 0.7612230502102123, - -0.006062013985911222, 0.002796240129882101, -0.6484557817278146, -0.08810289381910863, - -0.15701157216300196, -0.0010000000000000332, -0.014539830816495843, 0.023198592074418583, - 0.0009999999999999983, 0.004076871366552716, 0.06955888995767745, 0.000999999999999986, - 0.0122856683509064, 0.2439857207754333, 0.14060285526911687, 0.10353995845444387, - -0.1351940782300779, -0.24106131632436875, 0.26132790320148225, -0.2616450544693995, - 0.04542564531123872, -0.04525607613888716, -0.3632958205603084, 0.40420815631446605, - 1.0184345233520669, 1.02779617782172, -0.2021360473805433, 0.0836582953051283, -0.08883513025322785, - -0.10461481802276364, 0.006696399847085326, 0.11594106984836243, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.20903838361223612, 0.5078406950492332, 0.7683254976377869, 0.761553174936814, - -0.005873524638618575, 0.0031102132539951874, -0.6480683528961219, -0.0892366470981203, - -0.15740687019410887, -0.0009999999999999944, -0.014658468211495584, 0.023158054497649707, - 0.001000000000000014, 0.003250342337620416, 0.0690266668508732, 0.0009999999999999898, - 0.0124550043005642, 0.24319508767814135, 0.14072344762223493, 0.1040124424027635, - -0.13518709096825646, -0.24120945154711726, 0.26130716681321764, -0.2640550209454933, - 0.04421347079542957, -0.04542407246195826, -0.36383072384441817, 0.4044351887778706, - 1.0164605780089369, 1.0267199773976006, -0.20209595316500914, 0.08255009938765238, - -0.08863836930150833, -0.10528770739259562, 0.007138424112139696, 0.1149959056328099, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.21005696413603278, 0.5075099197704603, 0.7680714650085205, 0.7618829340135006, - -0.005685656097129359, 0.0034253135226918544, -0.6476807357031689, -0.09037327422255702, - -0.1578063907134372, -0.001000000000000019, -0.014774102404211828, 0.023113184394142693, - 0.0009999999999999853, 0.0024245389492468473, 0.0684899865567933, 0.000999999999999984, - 0.012626148277197135, 0.24240777746668787, 0.14083878001229042, 0.10447171423890583, - -0.13517288615912904, -0.24134784600181916, 0.2612760789700727, -0.2664608195506908, - 0.04300454163713772, -0.04558597363621705, -0.36435871792133034, 0.4046717233995956, - 1.0144930460130022, 1.0256627907670444, -0.2020575271202782, 0.08146083594699705, - -0.0884373060976762, -0.10594782315060139, 0.007582026501620829, 0.11404491953733861, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.21091412141523955, 0.5072502819041463, 0.7680300712598468, 0.7622027923122165, - -0.005557762001962837, 0.003830577007386299, -0.6473031294167982, -0.0917911479609728, - -0.15860392879548585, -0.0009999999999999846, -0.014829774797172757, 0.022994344843086482, - 0.0009999999999999983, 0.0016458162304244124, 0.06758514416491046, 0.000999999999999979, - 0.012961516789920632, 0.24193927592553832, 0.14052087569009886, 0.10395883744729986, - -0.13454426705573663, -0.24063919514042179, 0.2602690486968137, -0.2683964155178928, - 0.042356903455890355, -0.04550087623032995, -0.3643499718793841, 0.4057464858270893, - 1.0131143314619806, 1.0262762906999596, -0.20207367510153165, 0.08202235607186764, - -0.08786061416897285, -0.10552799223904767, 0.008247433181354678, 0.1126478423141877, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.21177005178192634, 0.5069909844649988, 0.767989003679091, 0.7625223933496509, - -0.005429293388011812, 0.004236807334061215, -0.6469251671385512, -0.09321317437088368, - -0.15940338453512914, -0.0010000000000000083, -0.014884591917285422, 0.022869892073079334, - 0.001000000000000005, 0.000868762144520856, 0.06667808788207602, 0.0010000000000000085, - 0.013300862041238226, 0.24147076792522107, 0.14019873533856964, 0.10343812029014046, - -0.13391385924446153, -0.23992989114496413, 0.2592604417292902, -0.2703305801303151, - 0.04170505826887161, -0.0454105834688773, -0.36434117916528885, 0.4068257138417118, - 1.011736357023813, 1.0268921647161666, -0.20209311928859006, 0.08258541991272303, - -0.08728372835799283, -0.1051069316079119, 0.008909299321792892, 0.11124739911301158, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.21241535085546306, 0.5067734806613466, 0.7680269570506808, 0.7628570734159785, - -0.0052175325472780905, 0.004791965412653088, -0.6465283442826082, -0.09542736101573038, - -0.16065583031764663, -0.000999999999999936, -0.015145500744851772, 0.022380605413461957, - 0.0009999999999999955, 0.00043279940441265483, 0.06545661667279254, 0.0009999999999999796, - 0.014358919816786789, 0.24100735232160123, 0.13931212194223566, 0.10254046860052475, - -0.13310967402700144, -0.23905187993832158, 0.2580054338765616, -0.2723161475318489, - 0.04083610580906401, -0.044907337744123615, -0.3647168084461133, 0.40834522838961457, - 1.0105229614709528, 1.0279622526405687, -0.20259670780784966, 0.08333215893778785, - -0.08665294934539827, -0.10449784339195278, 0.008993391589914045, 0.1091786129034864, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -- [-0.21306068346433843, 0.5065562912786956, 0.7680645571501519, 0.763191289805969, - -0.005005485640432194, 0.005346795236192113, -0.6461311105793518, -0.09764084753545176, - -0.16190757833712074, -0.000999999999999961, -0.01540659652311303, 0.021886870168284715, - 0.001, -3.243436196080112e-06, 0.06423545816131196, 0.0009999999999999861, 0.015416953490236284, - 0.24054454631093553, 0.13842254643462612, 0.10163341309443061, -0.13230538090925692, - -0.23817434517836694, 0.2567496252247842, -0.2742987550721412, 0.039964738595664934, - -0.044401496218164416, -0.3650880653598781, 0.4098661435088383, 1.0093091895328938, - 1.0290319312162102, -0.2031004727986001, 0.08408063421066587, -0.08602202692772555, - -0.10388869280661762, 0.00907741809163619, 0.10711084824811719, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -right_hand_palm_link_position: -- [-0.10141434520483017, 1.2709695100784302, 0.6379595398902893] -- [-0.10057274997234344, 1.2664413452148438, 0.6378775238990784] -- [-0.10031461715698242, 1.2617640495300293, 0.6380867958068848] -- [-0.09959238022565842, 1.256491780281067, 0.6377752423286438] -- [-0.09896448254585266, 1.2511093616485596, 0.6377146244049072] -- [-0.09827042371034622, 1.2447824478149414, 0.637209951877594] -- [-0.09752756357192993, 1.2382420301437378, 0.6370075345039368] -- [-0.09629107266664505, 1.2308322191238403, 0.6365101933479309] -- [-0.09494487941265106, 1.2232797145843506, 0.6357883810997009] -- [-0.09390145540237427, 1.2144253253936768, 0.635219931602478] -- [-0.09298756718635559, 1.2054744958877563, 0.6345850229263306] -- [-0.09170828759670258, 1.1957597732543945, 0.6343365907669067] -- [-0.0903535932302475, 1.1860928535461426, 0.6341777443885803] -- [-0.08865892887115479, 1.1761348247528076, 0.6335933208465576] -- [-0.08680878579616547, 1.1660102605819702, 0.6325970888137817] -- [-0.08461335301399231, 1.1564596891403198, 0.6322282552719116] -- [-0.08277514576911926, 1.1466659307479858, 0.6330022215843201] -- [-0.08080200850963593, 1.1373460292816162, 0.6331613063812256] -- [-0.07810938358306885, 1.1290440559387207, 0.6320158243179321] -- [-0.0756622850894928, 1.1206105947494507, 0.6319509744644165] -- [-0.07302039116621017, 1.112473964691162, 0.631949245929718] -- [-0.07035577297210693, 1.104833722114563, 0.6316689252853394] -- [-0.06786350905895233, 1.0970466136932373, 0.63144850730896] -- [-0.0651538148522377, 1.0897493362426758, 0.6308924555778503] -- [-0.0624116025865078, 1.0825965404510498, 0.6302537322044373] -- [-0.05913819372653961, 1.0757488012313843, 0.6295928955078125] -- [-0.05578115954995155, 1.0691601037979126, 0.6300455927848816] -- [-0.052496008574962616, 1.062697410583496, 0.6302716732025146] -- [-0.048902932554483414, 1.0563849210739136, 0.6302628517150879] -- [-0.044873930513858795, 1.0502969026565552, 0.6302675604820251] -- [-0.043485697358846664, 1.0398523807525635, 0.6299722790718079] -- [-0.05088702589273453, 1.031569480895996, 0.6353959441184998] -- [-0.06376069784164429, 1.0255324840545654, 0.64427250623703] -- [-0.07310865074396133, 1.0237953662872314, 0.6550662517547607] -- [-0.07797826081514359, 1.0246223211288452, 0.6667462587356567] -- [-0.0773138478398323, 1.0262943506240845, 0.6790915727615356] -- [-0.08231724053621292, 1.023036003112793, 0.6959434747695923] -- [-0.08522047102451324, 1.0182734727859497, 0.7134275436401367] -- [-0.08636945486068726, 1.0115786790847778, 0.7296351790428162] -- [-0.08633895218372345, 1.0037521123886108, 0.7442911863327026] -- [-0.0851380005478859, 0.9949053525924683, 0.7571527361869812] -- [-0.08346148580312729, 0.9852608442306519, 0.768481433391571] -- [-0.08107783645391464, 0.9753844738006592, 0.7785106301307678] -- [-0.07881231606006622, 0.9652860164642334, 0.7874190211296082] -- [-0.0763360932469368, 0.9551657438278198, 0.7953522205352783] -- [-0.07455434650182724, 0.9451812505722046, 0.8019341826438904] -- [-0.07271231710910797, 0.9352693557739258, 0.8078484535217285] -- [-0.07132074236869812, 0.9254670143127441, 0.8131359219551086] -- [-0.06996951252222061, 0.9156714081764221, 0.8179826736450195] -- [-0.06890358775854111, 0.9060363173484802, 0.8223192691802979] -- [-0.06792913377285004, 0.8963309526443481, 0.8263149261474609] -- [-0.06743612140417099, 0.886585533618927, 0.8290566205978394] -- [-0.06686482578516006, 0.8768752217292786, 0.8317685723304749] -- [-0.06667459011077881, 0.86712247133255, 0.8329557180404663] -- [-0.06675245612859726, 0.8572239279747009, 0.8335623741149902] -- [-0.06701663136482239, 0.8467738032341003, 0.8325316905975342] -- [-0.06728500127792358, 0.8359705805778503, 0.8311040997505188] -- [-0.06718313694000244, 0.8243567943572998, 0.8284909725189209] -- [-0.06684708595275879, 0.812280535697937, 0.8259477615356445] -- [-0.06567038595676422, 0.7998719215393066, 0.822767972946167] -- [-0.0639742985367775, 0.78739994764328, 0.8198058605194092] -- [-0.061767008155584335, 0.7751303315162659, 0.8170442581176758] -- [-0.05916576087474823, 0.7630069255828857, 0.8145400285720825] -- [-0.05646178498864174, 0.7510465383529663, 0.8121601939201355] -- [-0.05366581678390503, 0.7391737103462219, 0.8097432255744934] -- [-0.05103355646133423, 0.7273852229118347, 0.8072738647460938] -- [-0.048322685062885284, 0.7156355977058411, 0.8048338890075684] -- [-0.04589598998427391, 0.7038389444351196, 0.8023239374160767] -- [-0.04345070943236351, 0.6921164989471436, 0.7997321486473083] -- [-0.04113578051328659, 0.680159330368042, 0.7970927953720093] -- [-0.03873768076300621, 0.6682501435279846, 0.7945197820663452] -- [-0.036391183733940125, 0.6561338305473328, 0.7923862934112549] -- [-0.03390566632151604, 0.643919050693512, 0.7903189063072205] -- [-0.031260550022125244, 0.6314161419868469, 0.7889034152030945] -- [-0.028425272554159164, 0.6187424659729004, 0.7877160906791687] -- [-0.025332067161798477, 0.6055024862289429, 0.7869094610214233] -- [-0.021945349872112274, 0.5922197103500366, 0.7864052057266235] -- [-0.01805889792740345, 0.5783839821815491, 0.7862034440040588] -- [-0.01392650417983532, 0.5644848346710205, 0.7861812114715576] -- [-0.009218730032444, 0.5501918792724609, 0.7862496376037598] -- [-0.004165910184383392, 0.5358991622924805, 0.7864493131637573] -- [0.0013929717242717743, 0.5214430689811707, 0.7868214249610901] -- [0.0074004679918289185, 0.5070580840110779, 0.787368655204773] -- [0.013916760683059692, 0.49273204803466797, 0.7882994413375854] -- [0.020913567394018173, 0.4786031246185303, 0.7894764542579651] -- [0.028367996215820312, 0.46481087803840637, 0.7910247445106506] -- [0.036269064992666245, 0.4513009488582611, 0.792811393737793] -- [0.04462851583957672, 0.4384375512599945, 0.7951101064682007] -- [0.053409144282341, 0.42585647106170654, 0.7976638078689575] -- [0.062435444444417953, 0.4141024947166443, 0.8007608652114868] -- [0.07175882160663605, 0.4026613235473633, 0.8043174147605896] -- [0.08117815852165222, 0.39206618070602417, 0.8077620267868042] -- [0.09087718278169632, 0.38187575340270996, 0.8115716576576233] -- [0.10045196115970612, 0.3726251721382141, 0.8151925802230835] -- [0.11026732623577118, 0.3636731207370758, 0.8187916278839111] -- [0.11996321380138397, 0.35534724593162537, 0.8218891620635986] -- [0.12966908514499664, 0.34753262996673584, 0.8249314427375793] -- [0.1393737494945526, 0.3399142920970917, 0.8271085023880005] -- [0.1486579328775406, 0.33323681354522705, 0.8294651508331299] -- [0.1581507921218872, 0.32645219564437866, 0.8305400013923645] -- [0.16707034409046173, 0.3206835389137268, 0.831783652305603] -- [0.17562927305698395, 0.31546005606651306, 0.8321595191955566] -- [0.18398380279541016, 0.31057095527648926, 0.8322476148605347] -- [0.19168132543563843, 0.30644166469573975, 0.8316254019737244] -- [0.19950367510318756, 0.3017737865447998, 0.8302327990531921] -- [0.20632927119731903, 0.2984720766544342, 0.8284890055656433] -- [0.21300432085990906, 0.29540038108825684, 0.8264129757881165] -- [0.2191995233297348, 0.2927817106246948, 0.8239404559135437] -- [0.22528119385242462, 0.29038006067276, 0.8212355375289917] -- [0.23080642521381378, 0.2882818877696991, 0.8181393146514893] -- [0.23622095584869385, 0.28646913170814514, 0.814918041229248] -- [0.24129080772399902, 0.28474941849708557, 0.811535656452179] -- [0.2462528944015503, 0.28312602639198303, 0.8080540299415588] -- [0.2509128451347351, 0.2816177010536194, 0.8045523166656494] -- [0.25543543696403503, 0.2802574634552002, 0.8010269999504089] -- [0.25940656661987305, 0.2790539562702179, 0.7974570393562317] -- [0.2631811499595642, 0.27785947918891907, 0.7938774824142456] -- [0.2663458287715912, 0.27684280276298523, 0.7904092669487] -- [0.26930418610572815, 0.2757507860660553, 0.7869446873664856] -- [0.2716611325740814, 0.2747957706451416, 0.7836620211601257] -- [0.27377986907958984, 0.2737753093242645, 0.7803530693054199] -- [0.27528512477874756, 0.2729821503162384, 0.7770589590072632] -- [0.27651703357696533, 0.272173136472702, 0.7738650441169739] -- [0.2773808240890503, 0.27116715908050537, 0.770435094833374] -- [0.2780908942222595, 0.2703793942928314, 0.767021894454956] -- [0.2786494195461273, 0.26931673288345337, 0.7633390426635742] -- [0.2790943384170532, 0.2682449221611023, 0.7597276568412781] -- [0.27913618087768555, 0.2673775255680084, 0.756640613079071] -- [0.27887314558029175, 0.2667608857154846, 0.7535474300384521] -- [0.27865463495254517, 0.26618868112564087, 0.7511559128761292] -- [0.2785607576370239, 0.26538699865341187, 0.748755156993866] -- [0.2781188189983368, 0.2651544511318207, 0.7466234564781189] -- [0.27809056639671326, 0.26467204093933105, 0.7448304891586304] -- [0.2782244086265564, 0.26421472430229187, 0.7433680295944214] -- [0.27854156494140625, 0.2638219892978668, 0.7421815991401672] -- [0.2790864408016205, 0.2633160948753357, 0.7415885925292969] -- [0.27951857447624207, 0.2626091539859772, 0.740902841091156] -- [0.2801651954650879, 0.2620188593864441, 0.7404586672782898] -- [0.2805904150009155, 0.26131248474121094, 0.7403193712234497] -- [0.28087523579597473, 0.2609284520149231, 0.7403609156608582] -- [0.28133532404899597, 0.26014477014541626, 0.7406724691390991] -- [0.2810153663158417, 0.2594035863876343, 0.7419133186340332] -- [0.28074946999549866, 0.2585931122303009, 0.7436476945877075] -- [0.28011801838874817, 0.257904052734375, 0.7456651329994202] -- [0.2795678675174713, 0.25718387961387634, 0.7477803826332092] -- [0.2781382203102112, 0.2572154998779297, 0.7507317662239075] -- [0.27651214599609375, 0.2575826942920685, 0.7535876631736755] -- [0.27456387877464294, 0.2582298219203949, 0.7564859390258789] -- [0.27198782563209534, 0.2596883177757263, 0.759495735168457] -- [0.2685161530971527, 0.2614031434059143, 0.7625513076782227] -- [0.2646973133087158, 0.2634183168411255, 0.7652137279510498] -- [0.26023292541503906, 0.2657460570335388, 0.767979621887207] -- [0.2552969753742218, 0.2683616578578949, 0.7706876397132874] -- [0.24992497265338898, 0.2708023190498352, 0.7732629179954529] -- [0.2442609965801239, 0.27350494265556335, 0.7759613394737244] -- [0.23796945810317993, 0.27568337321281433, 0.7786951065063477] -- [0.23166586458683014, 0.277482807636261, 0.7812166810035706] -- [0.22495025396347046, 0.279486745595932, 0.7838919162750244] -- [0.21793891489505768, 0.28121691942214966, 0.7866615056991577] -- [0.21053899824619293, 0.28243204951286316, 0.7893106341362] -- [0.20285147428512573, 0.28356531262397766, 0.7919924259185791] -- [0.1947515457868576, 0.28468057513237, 0.7945845723152161] -- [0.18648913502693176, 0.2853260934352875, 0.7968718409538269] -- [0.17758767306804657, 0.2860771417617798, 0.7993139028549194] -- [0.16844964027404785, 0.28666290640830994, 0.8014214634895325] -- [0.15898512303829193, 0.287022203207016, 0.8034844398498535] -- [0.1493614912033081, 0.28724756836891174, 0.8052060008049011] -- [0.13891175389289856, 0.28763899207115173, 0.806974470615387] -- [0.12810908257961273, 0.2881529927253723, 0.8084959983825684] -- [0.11665543913841248, 0.2887098789215088, 0.8099605441093445] -- [0.10443828254938126, 0.2895662784576416, 0.8115196228027344] -- [0.09172944724559784, 0.29038164019584656, 0.8131954669952393] -- [0.07858042418956757, 0.2915624976158142, 0.8152356743812561] -- [0.06518753618001938, 0.29242226481437683, 0.8169879913330078] -- [0.051489539444446564, 0.29321280121803284, 0.8190116286277771] -- [0.037614718079566956, 0.29381322860717773, 0.8208104968070984] -- [0.023423973470926285, 0.29436737298965454, 0.8226813673973083] -- [0.00907808355987072, 0.29474762082099915, 0.8244803547859192] -- [-0.005568530410528183, 0.294900119304657, 0.8262995481491089] -- [-0.020409954711794853, 0.29522621631622314, 0.8282095193862915] -- [-0.03554810583591461, 0.2951231598854065, 0.8299689888954163] -- [-0.050716932862997055, 0.2949322462081909, 0.831810712814331] -- [-0.06598278880119324, 0.29438117146492004, 0.8336057662963867] -- [-0.08105369657278061, 0.29362860321998596, 0.8354274034500122] -- [-0.09589362144470215, 0.29248112440109253, 0.8373797535896301] -- [-0.11047253012657166, 0.290998637676239, 0.8392931222915649] -- [-0.12474577128887177, 0.28905749320983887, 0.8409973382949829] -- [-0.13859006762504578, 0.2867949604988098, 0.842847466468811] -- [-0.15215465426445007, 0.2834058105945587, 0.8441548347473145] -- [-0.16535189747810364, 0.28019312024116516, 0.8456244468688965] -- [-0.17825405299663544, 0.2764437198638916, 0.8466653227806091] -- [-0.19069978594779968, 0.27310651540756226, 0.8479359149932861] -- [-0.2028948962688446, 0.2693914771080017, 0.8490527868270874] -- [-0.2148326188325882, 0.26608380675315857, 0.8503056764602661] -- [-0.22662605345249176, 0.26299241185188293, 0.8514350652694702] -- [-0.23844166100025177, 0.26088282465934753, 0.8521440029144287] -- [-0.25060343742370605, 0.25854212045669556, 0.8516914248466492] -- [-0.2625211179256439, 0.256492018699646, 0.8510589003562927] -- [-0.27428460121154785, 0.2549589276313782, 0.8503918051719666] -- [-0.28585028648376465, 0.2545863091945648, 0.8495928049087524] -- [-0.2974930703639984, 0.25399744510650635, 0.8483951687812805] -- [-0.30895566940307617, 0.25513535737991333, 0.8472144603729248] -- [-0.32027557492256165, 0.255748450756073, 0.8455321192741394] -- [-0.33142855763435364, 0.2569587826728821, 0.8435985445976257] -- [-0.34191206097602844, 0.25917160511016846, 0.8414203524589539] -- [-0.3523576855659485, 0.2616434693336487, 0.8387304544448853] -- [-0.36268043518066406, 0.2649157643318176, 0.8358083367347717] -- [-0.3727889657020569, 0.26815932989120483, 0.8327561020851135] -- [-0.38252246379852295, 0.2718099355697632, 0.8297606110572815] -- [-0.39201080799102783, 0.27495089173316956, 0.8265608549118042] -- [-0.4011717140674591, 0.2777627110481262, 0.823357880115509] -- [-0.40985193848609924, 0.28036659955978394, 0.8202283978462219] -- [-0.41787296533584595, 0.28243356943130493, 0.8170065879821777] -- [-0.425390362739563, 0.2840593755245209, 0.8138341307640076] -- [-0.43215450644493103, 0.2852163314819336, 0.8107446432113647] -- [-0.43877673149108887, 0.28583723306655884, 0.8075750470161438] -- [-0.4446789026260376, 0.28603947162628174, 0.8047422170639038] -- [-0.4499448835849762, 0.28762200474739075, 0.8025100231170654] -- [-0.45511147379875183, 0.28804248571395874, 0.7999012470245361] -- [-0.4599188268184662, 0.28847795724868774, 0.7974880933761597] -- [-0.4640343487262726, 0.2898196578025818, 0.7957350015640259] -- [-0.468104749917984, 0.2903556227684021, 0.7934062480926514] -- [-0.4714279770851135, 0.29151201248168945, 0.7916399240493774] -- [-0.4746738374233246, 0.2920856177806854, 0.7896437644958496] -- [-0.4771155118942261, 0.29273030161857605, 0.7879251837730408] -- [-0.4796146750450134, 0.29303959012031555, 0.7862089276313782] -- [-0.48135867714881897, 0.2929922342300415, 0.7845171093940735] -- [-0.4831859767436981, 0.2934742569923401, 0.7831963300704956] -- [-0.48447853326797485, 0.2935093641281128, 0.7815034985542297] -- [-0.4856353998184204, 0.2933959364891052, 0.7797327041625977] -- [-0.4867061376571655, 0.2931042015552521, 0.7781370878219604] -- [-0.4872318208217621, 0.29269009828567505, 0.7765479683876038] -- [-0.4875900149345398, 0.29207468032836914, 0.7751628756523132] -- [-0.4877919554710388, 0.2914544641971588, 0.7736573219299316] -- [-0.4878193140029907, 0.29093295335769653, 0.7722364068031311] -- [-0.48789089918136597, 0.29092493653297424, 0.7706350684165955] -- [-0.48780959844589233, 0.29140204191207886, 0.7689222693443298] -- [-0.4880731701850891, 0.2920733094215393, 0.7671209573745728] -- [-0.488126665353775, 0.29326438903808594, 0.7652924656867981] -- [-0.48864391446113586, 0.29443827271461487, 0.7637317180633545] -- [-0.48821303248405457, 0.29557156562805176, 0.7618025541305542] -- [-0.488309383392334, 0.29672884941101074, 0.7602331638336182] -- [-0.48845767974853516, 0.2977009415626526, 0.7588441371917725] -- [-0.48857712745666504, 0.29843229055404663, 0.7575200200080872] -- [-0.4888210594654083, 0.298872709274292, 0.7565441131591797] -- [-0.4890133738517761, 0.2990192472934723, 0.7556697130203247] -- [-0.4889402389526367, 0.2983556091785431, 0.7554000020027161] -- [-0.4889666438102722, 0.2980460822582245, 0.7547829747200012] -- [-0.4889993667602539, 0.29758232831954956, 0.754546582698822] -- [-0.48906856775283813, 0.297215074300766, 0.7544641494750977] -- [-0.48897311091423035, 0.2971222698688507, 0.7546897530555725] -- [-0.48884931206703186, 0.2972303628921509, 0.7550439834594727] -- [-0.4889207184314728, 0.2976701557636261, 0.7558175325393677] -- [-0.48872482776641846, 0.29824134707450867, 0.7566284537315369] -- [-0.4884195625782013, 0.2989581823348999, 0.7576245069503784] -- [-0.4880857467651367, 0.29972612857818604, 0.758793830871582] -- [-0.48761188983917236, 0.3006223440170288, 0.760211169719696] -- [-0.4867628812789917, 0.3017045855522156, 0.7617197036743164] -- [-0.48600825667381287, 0.3032051920890808, 0.7637635469436646] -- [-0.4852518141269684, 0.3051320016384125, 0.7661428451538086] -- [-0.48505544662475586, 0.3078674077987671, 0.768830418586731] -- [-0.4849843382835388, 0.31123086810112, 0.771869421005249] -- [-0.4856519401073456, 0.3162280321121216, 0.7756166458129883] -- [-0.4864213466644287, 0.32213860750198364, 0.7795515656471252] -- [-0.48795220255851746, 0.33066001534461975, 0.7839245796203613] -- [-0.48924490809440613, 0.33933714032173157, 0.7877248525619507] -- [-0.4903525412082672, 0.3492908179759979, 0.7909348607063293] -- [-0.49089330434799194, 0.3596729338169098, 0.7925248146057129] -- [-0.49107110500335693, 0.37096187472343445, 0.7921850681304932] -- [-0.49061697721481323, 0.3825812339782715, 0.7905772924423218] -- [-0.4894094467163086, 0.3949708640575409, 0.7871315479278564] -- [-0.4873935878276825, 0.4076235592365265, 0.7824718356132507] -- [-0.48423629999160767, 0.42085567116737366, 0.776118814945221] -- [-0.48014527559280396, 0.43395063281059265, 0.7688783407211304] -- [-0.47484877705574036, 0.4468953311443329, 0.7606746554374695] -- [-0.4684849679470062, 0.4591173231601715, 0.7521279454231262] -- [-0.4610217213630676, 0.4703497886657715, 0.7432039380073547] -- [-0.4527328908443451, 0.4801473617553711, 0.7344893217086792] -- [-0.45081833004951477, 0.48489972949028015, 0.7297484874725342] -- [-0.4428899586200714, 0.4904618263244629, 0.7226051092147827] -- [-0.43218937516212463, 0.4954775869846344, 0.7151786684989929] -- [-0.42904695868492126, 0.5014001131057739, 0.7092013359069824] -- [-0.4262720048427582, 0.506574273109436, 0.7034902572631836] -- [-0.4208698570728302, 0.5095542073249817, 0.6988241076469421] -- [-0.41468265652656555, 0.5113769769668579, 0.6952596306800842] -- [-0.4088192582130432, 0.5130374431610107, 0.6924583911895752] -- [-0.40418586134910583, 0.5132187604904175, 0.6903457045555115] -- [-0.40032100677490234, 0.5130776762962341, 0.6886259317398071] -- [-0.39719122648239136, 0.5123794078826904, 0.6873169541358948] -- [-0.3949522078037262, 0.5109833478927612, 0.6862869262695312] -- [-0.3939712345600128, 0.5082621574401855, 0.685758113861084] -- [-0.393247127532959, 0.5055971145629883, 0.6854274272918701] -- [-0.3935808539390564, 0.5017871856689453, 0.6853936910629272] -- [-0.3940325975418091, 0.4983765482902527, 0.6850961446762085] -- [-0.39513272047042847, 0.49418172240257263, 0.6852896809577942] -- [-0.3964577317237854, 0.490079402923584, 0.6857054233551025] -- [-0.39818069338798523, 0.4851304590702057, 0.6862099170684814] -- [-0.40004318952560425, 0.48146533966064453, 0.6872615814208984] -- [-0.40153631567955017, 0.47609394788742065, 0.6886813044548035] -- [-0.40184393525123596, 0.470689594745636, 0.690146267414093] -- [-0.40163370966911316, 0.4645293354988098, 0.6913278102874756] -- [-0.3997243046760559, 0.45864301919937134, 0.6923038363456726] -- [-0.39856553077697754, 0.45294082164764404, 0.6933780908584595] -- [-0.40295854210853577, 0.44710859656333923, 0.6953770518302917] -- [-0.4052374064922333, 0.4418557584285736, 0.6969305872917175] -- [-0.40770596265792847, 0.43663716316223145, 0.6986045241355896] -- [-0.4092685282230377, 0.43164360523223877, 0.700035810470581] -- [-0.41063785552978516, 0.4267227351665497, 0.7014824151992798] -- [-0.41075944900512695, 0.422495573759079, 0.7025098204612732] -- [-0.41061165928840637, 0.4187426269054413, 0.7034077644348145] -- [-0.4103783071041107, 0.41532549262046814, 0.7041680216789246] -- [-0.4099714756011963, 0.4122224748134613, 0.704791784286499] -- [-0.40890073776245117, 0.40967947244644165, 0.7049784660339355] -- [-0.40787604451179504, 0.40730950236320496, 0.7051420211791992] -- [-0.407622367143631, 0.40444573760032654, 0.7057455778121948] -- [-0.40733644366264343, 0.40165388584136963, 0.70634526014328] -- [-0.4066123962402344, 0.39939436316490173, 0.7066690325737] -- [-0.4058428704738617, 0.39722150564193726, 0.7069661021232605] -- [-0.4046531617641449, 0.39561131596565247, 0.706923246383667] -- [-0.4034460484981537, 0.3940563201904297, 0.7068474292755127] -- [-0.4019637703895569, 0.3929750323295593, 0.7064115405082703] -- [-0.4004739820957184, 0.3919389843940735, 0.7059751152992249] -- [-0.39867284893989563, 0.39145225286483765, 0.705406904220581] -- [-0.3968595862388611, 0.3910256326198578, 0.7048301100730896] -- [-0.3948385417461395, 0.39100468158721924, 0.7040141224861145] -- [-0.393099844455719, 0.3905000388622284, 0.7034729719161987] -- [-0.3912900984287262, 0.39010050892829895, 0.70279860496521] -- [-0.3893454074859619, 0.38977617025375366, 0.7020881772041321] -- [-0.3873244822025299, 0.39000949263572693, 0.7012761831283569] -- [-0.38545119762420654, 0.3900022804737091, 0.7006144523620605] -- [-0.38378793001174927, 0.39015012979507446, 0.6999301910400391] -- [-0.3822499215602875, 0.39041462540626526, 0.6992162466049194] -- [-0.3808917999267578, 0.39055415987968445, 0.6984821557998657] -- [-0.37950658798217773, 0.3908042311668396, 0.6977352499961853] -- [-0.3783383071422577, 0.3917495310306549, 0.696914792060852] -- [-0.3794834017753601, 0.390392005443573, 0.696964681148529] -- [-0.37863799929618835, 0.390714168548584, 0.6963681578636169] -- [-0.37975549697875977, 0.3908237814903259, 0.6958967447280884] -- [-0.38045886158943176, 0.3907140791416168, 0.695747435092926] -- [-0.3808164894580841, 0.3909127712249756, 0.69545578956604] -- [-0.3818940222263336, 0.39077961444854736, 0.6953238844871521] -- [-0.38301488757133484, 0.39073604345321655, 0.6951779723167419] -- [-0.3845188319683075, 0.39070969820022583, 0.6951384544372559] -- [-0.38602402806282043, 0.39066532254219055, 0.6951114535331726] -- [-0.3878728151321411, 0.39049792289733887, 0.6951330304145813] -- [-0.3890393078327179, 0.3906010091304779, 0.6947397589683533] -- [-0.3887094259262085, 0.3910994827747345, 0.6943438053131104] -- [-0.3919970393180847, 0.39009952545166016, 0.6945514678955078] -- [-0.39375007152557373, 0.38981181383132935, 0.6945829391479492] -- [-0.3942084312438965, 0.39010804891586304, 0.6944094300270081] -- [-0.39596500992774963, 0.3896307945251465, 0.6943216323852539] -- [-0.39848434925079346, 0.3887869417667389, 0.6943473219871521] -- [-0.39974743127822876, 0.38850921392440796, 0.6942914724349976] -- [-0.4011179208755493, 0.3881068229675293, 0.6942812204360962] -- [-0.402616947889328, 0.38759350776672363, 0.6943721771240234] -- [-0.4040394127368927, 0.38711753487586975, 0.6944320201873779] -right_hand_palm_link_wxyz: -- [0.6572909951210022, 0.15838629007339478, 0.5863845348358154, -0.44613388180732727] -- [0.658167839050293, 0.1582193523645401, 0.5846661925315857, -0.44715428352355957] -- [0.6585543155670166, 0.157085120677948, 0.5838945508003235, -0.4479926824569702] -- [0.6590890884399414, 0.1574157327413559, 0.5822980999946594, -0.44916656613349915] -- [0.6593254208564758, 0.15775978565216064, 0.5808987021446228, -0.4505090117454529] -- [0.6598548889160156, 0.15787453949451447, 0.5795257687568665, -0.45146089792251587] -- [0.6604468822479248, 0.15723587572574615, 0.5784593820571899, -0.45218512415885925] -- [0.6617881655693054, 0.15564602613449097, 0.5781121253967285, -0.45121729373931885] -- [0.6634416580200195, 0.154957115650177, 0.5766739249229431, -0.45086637139320374] -- [0.6639248132705688, 0.1556037813425064, 0.5738799571990967, -0.45348966121673584] -- [0.6646625399589539, 0.15598031878471375, 0.5710947513580322, -0.4557900130748749] -- [0.6652584671974182, 0.15647539496421814, 0.5678176283836365, -0.4588349759578705] -- [0.6660304069519043, 0.1569107621908188, 0.5645031332969666, -0.4616475999355316] -- [0.6671608686447144, 0.15719448029994965, 0.5607331395149231, -0.464504599571228] -- [0.6682720184326172, 0.1585676372051239, 0.5552520751953125, -0.4690028429031372] -- [0.6693702340126038, 0.15831606090068817, 0.5522160530090332, -0.4711017310619354] -- [0.6701717376708984, 0.15619029104709625, 0.5517841577529907, -0.47117793560028076] -- [0.6714868545532227, 0.15434107184410095, 0.5506402254104614, -0.471252977848053] -- [0.6742986440658569, 0.1527588963508606, 0.5490478277206421, -0.46960869431495667] -- [0.6746382117271423, 0.15469299256801605, 0.5457352995872498, -0.4723413288593292] -- [0.6752762198448181, 0.15792328119277954, 0.5415316820144653, -0.47519001364707947] -- [0.6760603189468384, 0.15889869630336761, 0.5398263931274414, -0.47568991780281067] -- [0.6766087412834167, 0.15954013168811798, 0.538364827632904, -0.47635161876678467] -- [0.6771950721740723, 0.16162841022014618, 0.5360726714134216, -0.4773981273174286] -- [0.6776626706123352, 0.16418440639972687, 0.5335735082626343, -0.4786606729030609] -- [0.6775984168052673, 0.1693013459444046, 0.5287070274353027, -0.4823548197746277] -- [0.677569568157196, 0.1732037216424942, 0.5266712307929993, -0.48323628306388855] -- [0.6774634122848511, 0.17697633802890778, 0.5248834490776062, -0.483962744474411] -- [0.6769973635673523, 0.18141429126262665, 0.5229867696762085, -0.48502397537231445] -- [0.6762934923171997, 0.18773046135902405, 0.5190903544425964, -0.48778006434440613] -- [0.6745707392692566, 0.1909506469964981, 0.5158189535140991, -0.49236440658569336] -- [0.6716489791870117, 0.18604473769664764, 0.5183972120285034, -0.49551916122436523] -- [0.6683029532432556, 0.17404454946517944, 0.523550808429718, -0.4989730715751648] -- [0.6681546568870544, 0.165397047996521, 0.5225703120231628, -0.5031234622001648] -- [0.6724007725715637, 0.1608150154352188, 0.5141579508781433, -0.507599413394928] -- [0.6847891211509705, 0.15985240042209625, 0.49620649218559265, -0.5092054009437561] -- [0.693059504032135, 0.14948002994060516, 0.47821053862571716, -0.5183039307594299] -- [0.702752411365509, 0.14029952883720398, 0.4595843255519867, -0.5246305465698242] -- [0.7133334279060364, 0.13218870759010315, 0.44113120436668396, -0.5282846093177795] -- [0.7223021984100342, 0.12540709972381592, 0.4245583713054657, -0.53132164478302] -- [0.729783296585083, 0.11961442232131958, 0.41064393520355225, -0.5333669185638428] -- [0.7355897426605225, 0.11379999667406082, 0.39943909645080566, -0.5351688265800476] -- [0.7396748065948486, 0.10904346406459808, 0.39049002528190613, -0.5371296405792236] -- [0.742419958114624, 0.10351843386888504, 0.38375622034072876, -0.5392842888832092] -- [0.7442139983177185, 0.09802799671888351, 0.3784269690513611, -0.541598379611969] -- [0.7463803887367249, 0.091312475502491, 0.3729497492313385, -0.5435869097709656] -- [0.7480179667472839, 0.08468986302614212, 0.36821794509887695, -0.5456300377845764] -- [0.7500623464584351, 0.07758763432502747, 0.36413368582725525, -0.5466197729110718] -- [0.7519930601119995, 0.07046091556549072, 0.3605089485645294, -0.5473343729972839] -- [0.7546265721321106, 0.06386946886777878, 0.357352077960968, -0.5465883016586304] -- [0.757538914680481, 0.05727415904402733, 0.35424649715423584, -0.5453106760978699] -- [0.7622277736663818, 0.05013144016265869, 0.3511834144592285, -0.5414477586746216] -- [0.7672103643417358, 0.04390360414981842, 0.3469286561012268, -0.5376812219619751] -- [0.7744206190109253, 0.03763451427221298, 0.34050804376602173, -0.531893253326416] -- [0.7827262878417969, 0.0316731333732605, 0.33273449540138245, -0.5249990820884705] -- [0.7936159372329712, 0.02714463323354721, 0.3222481310367584, -0.5153570771217346] -- [0.8049728274345398, 0.02421070635318756, 0.3118354380130768, -0.5041738748550415] -- [0.816807746887207, 0.023326275870203972, 0.3020083010196686, -0.49099090695381165] -- [0.8283704519271851, 0.022156046703457832, 0.2946056127548218, -0.47594010829925537] -- [0.8390362858772278, 0.023086586967110634, 0.2895795702934265, -0.46003127098083496] -- [0.848422646522522, 0.025463635101914406, 0.2870480716228485, -0.44399771094322205] -- [0.8562093377113342, 0.02873224765062332, 0.2862178087234497, -0.42913761734962463] -- [0.8629917502403259, 0.03254815563559532, 0.2866066098213196, -0.41478005051612854] -- [0.8687893152236938, 0.03652706742286682, 0.2867294251918793, -0.40206608176231384] -- [0.8740487098693848, 0.039931535720825195, 0.28747108578681946, -0.38962119817733765] -- [0.8783978819847107, 0.042129307985305786, 0.28791290521621704, -0.37914159893989563] -- [0.8825632929801941, 0.043926794081926346, 0.28821778297424316, -0.3688940405845642] -- [0.8862009644508362, 0.043787695467472076, 0.28748786449432373, -0.3606676757335663] -- [0.8895304799079895, 0.04374208301305771, 0.2866613566875458, -0.3530541956424713] -- [0.8925060033798218, 0.042343102395534515, 0.2847713828086853, -0.3471962511539459] -- [0.8953192234039307, 0.040533117949962616, 0.28257355093955994, -0.341924786567688] -- [0.897612452507019, 0.036912936717271805, 0.28020361065864563, -0.3382532596588135] -- [0.8999772667884827, 0.03311895206570625, 0.2776179909706116, -0.33447304368019104] -- [0.9017991423606873, 0.028822844848036766, 0.2752860188484192, -0.33188101649284363] -- [0.9035541415214539, 0.024601653218269348, 0.2730780839920044, -0.3292611241340637] -- [0.9052698612213135, 0.018869755789637566, 0.2707454562187195, -0.32684412598609924] -- [0.9070568084716797, 0.013365412130951881, 0.2681702971458435, -0.3242743909358978] -- [0.9086905717849731, 0.006890959106385708, 0.265569269657135, -0.32203561067581177] -- [0.9103500247001648, 0.0002677194424904883, 0.2629817724227905, -0.31953608989715576] -- [0.9117857813835144, -0.007724156137555838, 0.2600456476211548, -0.31774717569351196] -- [0.913171648979187, -0.01592506282031536, 0.2571266293525696, -0.3158317506313324] -- [0.9143684506416321, -0.02523317001760006, 0.2536756098270416, -0.31455081701278687] -- [0.9153834581375122, -0.03465217724442482, 0.25038421154022217, -0.31333690881729126] -- [0.9162818789482117, -0.04375113919377327, 0.24657823145389557, -0.31258973479270935] -- [0.916943371295929, -0.05239957943558693, 0.2430548369884491, -0.31207892298698425] -- [0.917356550693512, -0.0608055405318737, 0.2388511747121811, -0.3125852942466736] -- [0.9175605177879333, -0.06890919804573059, 0.23477403819561005, -0.31339311599731445] -- [0.9173110723495483, -0.07653991878032684, 0.23070518672466278, -0.315368115901947] -- [0.9168115258216858, -0.08427026122808456, 0.2268448919057846, -0.3176420331001282] -- [0.9159467220306396, -0.08998292684555054, 0.2237333506345749, -0.320761501789093] -- [0.9149779677391052, -0.09525737911462784, 0.2206610143184662, -0.3241141736507416] -- [0.9139554500579834, -0.0997859388589859, 0.21891577541828156, -0.3268088102340698] -- [0.9126811623573303, -0.10387858748435974, 0.2172553390264511, -0.33018529415130615] -- [0.9112447500228882, -0.10722284764051437, 0.21629571914672852, -0.33369502425193787] -- [0.9097787737846375, -0.1107921153306961, 0.21577507257461548, -0.33685117959976196] -- [0.9086207747459412, -0.11406941711902618, 0.21551741659641266, -0.33904072642326355] -- [0.9078044295310974, -0.11624309420585632, 0.21549081802368164, -0.3405028283596039] -- [0.9071676731109619, -0.11941707134246826, 0.21544642746448517, -0.3411293029785156] -- [0.906873881816864, -0.119843028485775, 0.21663953363895416, -0.34100520610809326] -- [0.9068844318389893, -0.12296316027641296, 0.2158801406621933, -0.3403475880622864] -- [0.9071948528289795, -0.12270376831293106, 0.21706707775592804, -0.33885547518730164] -- [0.9078854322433472, -0.12230227887630463, 0.21850331127643585, -0.33621782064437866] -- [0.9086393117904663, -0.12141643464565277, 0.21997268497943878, -0.33353352546691895] -- [0.9093805551528931, -0.1199401468038559, 0.22224891185760498, -0.33052483201026917] -- [0.9099087119102478, -0.12091828882694244, 0.22257448732852936, -0.3284895420074463] -- [0.9105611443519592, -0.11906690150499344, 0.2255232185125351, -0.3253316581249237] -- [0.9111599922180176, -0.11729702353477478, 0.22873130440711975, -0.32204148173332214] -- [0.911409318447113, -0.11520034819841385, 0.23294435441493988, -0.3190590441226959] -- [0.9115867018699646, -0.11308805644512177, 0.23753511905670166, -0.31590786576271057] -- [0.911794126033783, -0.11041691154241562, 0.24180418252944946, -0.3130016624927521] -- [0.9118268489837646, -0.10757292062044144, 0.24673525989055634, -0.31003472208976746] -- [0.911758303642273, -0.10472475737333298, 0.25164157152175903, -0.30725568532943726] -- [0.9116901755332947, -0.1018221378326416, 0.25647270679473877, -0.3044256269931793] -- [0.9114020466804504, -0.09912773221731186, 0.2610420286655426, -0.3022865355014801] -- [0.9110155701637268, -0.09633148461580276, 0.26550939679145813, -0.3004588782787323] -- [0.9106068015098572, -0.09357121586799622, 0.26932546496391296, -0.29917117953300476] -- [0.9101129174232483, -0.09080749750137329, 0.27267950773239136, -0.29848670959472656] -- [0.9095611572265625, -0.08794917911291122, 0.2758902907371521, -0.29807332158088684] -- [0.9088705778121948, -0.08519551903009415, 0.27897295355796814, -0.29811084270477295] -- [0.9079383611679077, -0.08279851078987122, 0.28245124220848083, -0.2983510494232178] -- [0.9069661498069763, -0.08082718402147293, 0.28597593307495117, -0.29849138855934143] -- [0.9056282043457031, -0.07871987670660019, 0.29000911116600037, -0.2992244362831116] -- [0.9043750762939453, -0.0761958658695221, 0.29342299699783325, -0.300337553024292] -- [0.9026708006858826, -0.07368983328342438, 0.2975527048110962, -0.3020223379135132] -- [0.9008972644805908, -0.07175258547067642, 0.3027536869049072, -0.30261486768722534] -- [0.8988531231880188, -0.06916987150907516, 0.3081413805484772, -0.3038542866706848] -- [0.8967358469963074, -0.06647034734487534, 0.31372371315956116, -0.3049983084201813] -- [0.8952301740646362, -0.06369823962450027, 0.3174419105052948, -0.3061631917953491] -- [0.8935648798942566, -0.06191261485219002, 0.321146696805954, -0.30752789974212646] -- [0.892746090888977, -0.06026063859462738, 0.3229256272315979, -0.3083702623844147] -- [0.8924022316932678, -0.0585491843521595, 0.32301270961761475, -0.30960142612457275] -- [0.8925387859344482, -0.059040773659944534, 0.32457831501960754, -0.3074696660041809] -- [0.8927673697471619, -0.05805302411317825, 0.3237396776676178, -0.30787765979766846] -- [0.8934855461120605, -0.056960511952638626, 0.3223068118095398, -0.3075016140937805] -- [0.8942326903343201, -0.05656078830361366, 0.32098865509033203, -0.30678167939186096] -- [0.894990861415863, -0.056003935635089874, 0.31917524337768555, -0.30656465888023376] -- [0.8956466317176819, -0.05490050092339516, 0.3171427547931671, -0.3069581687450409] -- [0.8962281346321106, -0.05430443957448006, 0.31553152203559875, -0.30702757835388184] -- [0.8972912430763245, -0.05401545763015747, 0.31392136216163635, -0.3056209683418274] -- [0.8975605368614197, -0.05354105308651924, 0.31252366304397583, -0.306345134973526] -- [0.898563027381897, -0.05373213812708855, 0.3100660443305969, -0.3058696985244751] -- [0.8989108204841614, -0.052772585302591324, 0.3076811134815216, -0.307419091463089] -- [0.8997994065284729, -0.05208856984972954, 0.30435416102409363, -0.3082469701766968] -- [0.9007178544998169, -0.05168807506561279, 0.29999831318855286, -0.30989763140678406] -- [0.9016689658164978, -0.051878876984119415, 0.2949830889701843, -0.31190767884254456] -- [0.9029423594474792, -0.05108252912759781, 0.28943461179733276, -0.313549280166626] -- [0.9038193225860596, -0.050615642219781876, 0.2845412790775299, -0.3155707120895386] -- [0.9059656858444214, -0.05013120919466019, 0.2773888111114502, -0.31586143374443054] -- [0.9080661535263062, -0.04994771629571915, 0.27095407247543335, -0.3154439330101013] -- [0.9099313020706177, -0.04890017956495285, 0.2637876570224762, -0.31630635261535645] -- [0.9120900630950928, -0.04783089458942413, 0.25614237785339355, -0.3165358304977417] -- [0.9140172600746155, -0.046676747500896454, 0.24883458018302917, -0.3169780671596527] -- [0.9154302477836609, -0.04544529691338539, 0.2422628551721573, -0.31816786527633667] -- [0.9168515801429749, -0.04407874494791031, 0.23492808640003204, -0.31976404786109924] -- [0.9186362028121948, -0.043576739728450775, 0.22781267762184143, -0.3198591470718384] -- [0.9194199442863464, -0.04300845414400101, 0.22069156169891357, -0.3226644992828369] -- [0.9200552701950073, -0.043508902192115784, 0.21360230445861816, -0.32554468512535095] -- [0.920143187046051, -0.04339532554149628, 0.20767541229724884, -0.3291262686252594] -- [0.9199451804161072, -0.043043117970228195, 0.20160940289497375, -0.3334691822528839] -- [0.9185281991958618, -0.043822113424539566, 0.19672928750514984, -0.34012213349342346] -- [0.9167934060096741, -0.04429489001631737, 0.1921214461326599, -0.34729960560798645] -- [0.9145870208740234, -0.04416745901107788, 0.1885729730129242, -0.3549927771091461] -- [0.9117220640182495, -0.04604221507906914, 0.18639349937438965, -0.36318108439445496] -- [0.9075732231140137, -0.04656669497489929, 0.18450647592544556, -0.3742990493774414] -- [0.9037050604820251, -0.04627696052193642, 0.18304310739040375, -0.3842794895172119] -- [0.8994547128677368, -0.046795785427093506, 0.18207967281341553, -0.3945102393627167] -- [0.8944553732872009, -0.04860334098339081, 0.18260648846626282, -0.40526774525642395] -- [0.889384388923645, -0.0488784983754158, 0.1823527216911316, -0.41635772585868835] -- [0.8835604190826416, -0.050229333341121674, 0.1833135336637497, -0.42801180481910706] -- [0.8775226473808289, -0.05193817615509033, 0.18462613224983215, -0.43951067328453064] -- [0.8708046674728394, -0.05474099889397621, 0.18779999017715454, -0.4510364234447479] -- [0.8641817569732666, -0.0565960519015789, 0.1912655383348465, -0.4619568884372711] -- [0.8562623858451843, -0.057758912444114685, 0.1967335194349289, -0.47410398721694946] -- [0.8500633835792542, -0.05973074585199356, 0.20111458003520966, -0.4830913841724396] -- [0.8434017300605774, -0.06196321174502373, 0.20580363273620605, -0.49242129921913147] -- [0.8369020223617554, -0.06454374641180038, 0.2108505517244339, -0.5009703040122986] -- [0.8305230736732483, -0.06685967743396759, 0.21629999577999115, -0.5088962316513062] -- [0.824000895023346, -0.06986121088266373, 0.22145432233810425, -0.5168168544769287] -- [0.817845344543457, -0.0727018192410469, 0.2259995937347412, -0.5241827964782715] -- [0.8120629191398621, -0.07573533803224564, 0.23093128204345703, -0.530555009841919] -- [0.8064777255058289, -0.07851408421993256, 0.23469102382659912, -0.5369815826416016] -- [0.8014026880264282, -0.0822637602686882, 0.23887242376804352, -0.542149543762207] -- [0.7965261340141296, -0.08659424632787704, 0.2423258125782013, -0.5471067428588867] -- [0.7923089861869812, -0.09109923988580704, 0.24634955823421478, -0.550689697265625] -- [0.7880143523216248, -0.09506796300411224, 0.24964793026447296, -0.5546813011169434] -- [0.7838902473449707, -0.09932735562324524, 0.25266340374946594, -0.5584006905555725] -- [0.7798376083374023, -0.10362131893634796, 0.25509074330329895, -0.5621784329414368] -- [0.7762832641601562, -0.10918843001127243, 0.2577253580093384, -0.5648359060287476] -- [0.7737047672271729, -0.11438468843698502, 0.2590329945087433, -0.5667440891265869] -- [0.7702687978744507, -0.1197158694267273, 0.26123908162117004, -0.5693050026893616] -- [0.7671209573745728, -0.12479867786169052, 0.26302045583724976, -0.5716387033462524] -- [0.7629976868629456, -0.1292032152414322, 0.26615110039711, -0.5747213363647461] -- [0.7591348886489868, -0.1333893984556198, 0.26955753564834595, -0.5772868394851685] -- [0.7545205950737, -0.13629740476608276, 0.27364397048950195, -0.58072429895401] -- [0.7498507499694824, -0.1381605714559555, 0.2788521945476532, -0.5838465094566345] -- [0.7456695437431335, -0.13639722764492035, 0.28560084104537964, -0.5863487720489502] -- [0.7415623068809509, -0.13860531151294708, 0.28904852271080017, -0.5893426537513733] -- [0.7376042604446411, -0.14029720425605774, 0.2931819558143616, -0.5918622612953186] -- [0.7334928512573242, -0.1410040557384491, 0.29832151532173157, -0.5942308306694031] -- [0.7299903631210327, -0.13833458721637726, 0.30498263239860535, -0.595787763595581] -- [0.7276346683502197, -0.13733772933483124, 0.3097711503505707, -0.5964294075965881] -- [0.7253385782241821, -0.13156872987747192, 0.3174549341201782, -0.5964862704277039] -- [0.723284125328064, -0.1301877498626709, 0.3216427266597748, -0.5970401167869568] -- [0.7214926481246948, -0.12660890817642212, 0.32679757475852966, -0.5971783399581909] -- [0.7182309627532959, -0.12160373479127884, 0.33214083313941956, -0.5991988182067871] -- [0.716387927532196, -0.11704369634389877, 0.3364515006542206, -0.5999077558517456] -- [0.7143950462341309, -0.11256622523069382, 0.34027501940727234, -0.6009836792945862] -- [0.713111400604248, -0.10777659714221954, 0.34300532937049866, -0.6018336415290833] -- [0.7130866646766663, -0.10168994218111038, 0.3451525866985321, -0.6016942262649536] -- [0.7138434052467346, -0.09589335322380066, 0.3462715148925781, -0.6011055707931519] -- [0.7150437235832214, -0.09080520272254944, 0.3462526500225067, -0.6004797220230103] -- [0.7165428400039673, -0.08530312776565552, 0.34588393568992615, -0.5997114777565002] -- [0.7182580232620239, -0.07964590936899185, 0.3450312912464142, -0.5989283919334412] -- [0.7203622460365295, -0.07488330453634262, 0.3438757658004761, -0.5976787805557251] -- [0.7222976684570312, -0.07093000411987305, 0.3426927626132965, -0.5965035557746887] -- [0.7240464687347412, -0.06983781605958939, 0.34138524532318115, -0.5952607989311218] -- [0.725572407245636, -0.06778091937303543, 0.33921509981155396, -0.5948810577392578] -- [0.7271319627761841, -0.06100894883275032, 0.339324414730072, -0.5936462879180908] -- [0.728774905204773, -0.05895191431045532, 0.33630451560020447, -0.5935577750205994] -- [0.7300754189491272, -0.05670491233468056, 0.33482328057289124, -0.5930159091949463] -- [0.7308931946754456, -0.04929887130856514, 0.33335888385772705, -0.5934951901435852] -- [0.7319919466972351, -0.047704704105854034, 0.33204373717308044, -0.5930084586143494] -- [0.7322710156440735, -0.04283644258975983, 0.33052805066108704, -0.5938815474510193] -- [0.7333852648735046, -0.04055261239409447, 0.3287576735019684, -0.593649685382843] -- [0.7346557974815369, -0.03762933611869812, 0.32839295268058777, -0.5924718976020813] -- [0.7355274558067322, -0.03453221917152405, 0.3278406262397766, -0.5918844938278198] -- [0.7374001145362854, -0.03155815601348877, 0.3275541663169861, -0.5898756384849548] -- [0.7377411127090454, -0.02612421289086342, 0.32811832427978516, -0.5894012451171875] -- [0.7380795478820801, -0.02221544273197651, 0.328605592250824, -0.5888661742210388] -- [0.7382684946060181, -0.01933535374701023, 0.3294721841812134, -0.5882463455200195] -- [0.7383667230606079, -0.016761580482125282, 0.3298538625240326, -0.5879880785942078] -- [0.7391423583030701, -0.014948088675737381, 0.3308390974998474, -0.5865071415901184] -- [0.7397060394287109, -0.012641731649637222, 0.33134305477142334, -0.585565447807312] -- [0.7404924631118774, -0.011175423860549927, 0.33216458559036255, -0.584134042263031] -- [0.7415671944618225, -0.009308744221925735, 0.3323201537132263, -0.5827131271362305] -- [0.7425248026847839, -0.007113324478268623, 0.3328856825828552, -0.5811998248100281] -- [0.743347704410553, -0.0051559945568442345, 0.33364471793174744, -0.5797316431999207] -- [0.744597852230072, -0.0023683723993599415, 0.3330908417701721, -0.5784624218940735] -- [0.745966374874115, -7.637462840648368e-05, 0.33316949009895325, -0.5766559839248657] -- [0.7478466033935547, 0.001968830358237028, 0.33285942673683167, -0.5743920207023621] -- [0.7486780881881714, 0.0043198163621127605, 0.3321777582168579, -0.5736900568008423] -- [0.7503407001495361, 0.006463085766881704, 0.3314177691936493, -0.5719345211982727] -- [0.7513091564178467, 0.008396987803280354, 0.33089306950569153, -0.5709410905838013] -- [0.7520270943641663, 0.009738669730722904, 0.33033931255340576, -0.5702948570251465] -- [0.7525929808616638, 0.01063610427081585, 0.3296549320220947, -0.5699281692504883] -- [0.7528637647628784, 0.011731070466339588, 0.32838529348373413, -0.5702820420265198] -- [0.7522872686386108, 0.012377731502056122, 0.3260805308818817, -0.5723478198051453] -- [0.7516252398490906, 0.012774723581969738, 0.325910359621048, -0.573305070400238] -- [0.7510993480682373, 0.013153640553355217, 0.32497119903564453, -0.574517548084259] -- [0.7509331107139587, 0.01386245246976614, 0.32333531975746155, -0.5756400227546692] -- [0.7510393857955933, 0.01423290092498064, 0.3220216929912567, -0.5762283802032471] -- [0.7509834170341492, 0.014556034468114376, 0.32093772292137146, -0.5768975615501404] -- [0.7507300972938538, 0.0151903685182333, 0.31850504875183105, -0.5785567760467529] -- [0.7505369186401367, 0.01609707437455654, 0.31629765033721924, -0.5799922347068787] -- [0.749675989151001, 0.017399117350578308, 0.31369176506996155, -0.5824779272079468] -- [0.7490375638008118, 0.01843990385532379, 0.31082385778427124, -0.5848000645637512] -- [0.7477737069129944, 0.020274996757507324, 0.30700546503067017, -0.588362991809845] -- [0.7475360035896301, 0.021765513345599174, 0.30395588278770447, -0.5901922583580017] -- [0.7468639612197876, 0.02378120832145214, 0.2999465763568878, -0.5930097699165344] -- [0.7460038065910339, 0.026575172320008278, 0.29551249742507935, -0.5961915850639343] -- [0.7451954483985901, 0.02734292671084404, 0.29022306203842163, -0.5997554659843445] -- [0.7442280054092407, 0.028861669823527336, 0.2843395173549652, -0.6036908626556396] -- [0.7428016066551208, 0.030390143394470215, 0.2773946523666382, -0.6085838079452515] -- [0.7411573529243469, 0.032059647142887115, 0.2698315680027008, -0.6138801574707031] -- [0.7389147877693176, 0.03323376551270485, 0.2611021399497986, -0.6202629208564758] -- [0.7366891503334045, 0.03301289677619934, 0.25338101387023926, -0.6260967254638672] -- [0.7338076233863831, 0.03489486500620842, 0.24835066497325897, -0.6313719749450684] -- [0.7298246026039124, 0.03975973650813103, 0.24690160155296326, -0.6362504959106445] -- [0.7253450155258179, 0.04930359125137329, 0.24940627813339233, -0.639718770980835] -- [0.7201142907142639, 0.06089402362704277, 0.2543147802352905, -0.6426905989646912] -- [0.7141510844230652, 0.07643263787031174, 0.2628139555454254, -0.6442630290985107] -- [0.7072986364364624, 0.09362732619047165, 0.2732742130756378, -0.6452004909515381] -- [0.6994314193725586, 0.11362095177173615, 0.2861659824848175, -0.644976794719696] -- [0.690523087978363, 0.13482937216758728, 0.3006637990474701, -0.6438945531845093] -- [0.6809259653091431, 0.1579866111278534, 0.31685057282447815, -0.6410816311836243] -- [0.6701269149780273, 0.18213219940662384, 0.3340015411376953, -0.6373388171195984] -- [0.6590444445610046, 0.2066958248615265, 0.35197263956069946, -0.6317059993743896] -- [0.6474249362945557, 0.2313455194234848, 0.36937040090560913, -0.6252083778381348] -- [0.6252220869064331, 0.25582537055015564, 0.3687787353992462, -0.6384770274162292] -- [0.6097207069396973, 0.286493182182312, 0.37171316146850586, -0.6387421488761902] -- [0.6020559668540955, 0.3100954294204712, 0.3830428719520569, -0.62820965051651] -- [0.5861101150512695, 0.32990697026252747, 0.40202611684799194, -0.6212980151176453] -- [0.5789212584495544, 0.33847495913505554, 0.43302595615386963, -0.6023067235946655] -- [0.5754155516624451, 0.3452213406562805, 0.45308107137680054, -0.5868870615959167] -- [0.577199399471283, 0.35216033458709717, 0.46549758315086365, -0.5710830092430115] -- [0.5740240216255188, 0.3665386438369751, 0.46534308791160583, -0.5653332471847534] -- [0.5723431706428528, 0.37524014711380005, 0.46631911396980286, -0.5605037808418274] -- [0.5723447203636169, 0.3811420500278473, 0.46824556589126587, -0.5548858642578125] -- [0.5907651782035828, 0.3542425334453583, 0.5033835172653198, -0.5216451287269592] -- [0.5919498801231384, 0.35610532760620117, 0.5064854621887207, -0.5160007476806641] -- [0.5931715369224548, 0.3556358516216278, 0.5083593726158142, -0.5130704045295715] -- [0.5946571826934814, 0.35436752438545227, 0.5103164911270142, -0.5102777481079102] -- [0.5969510674476624, 0.34987375140190125, 0.5124016404151917, -0.5086082220077515] -- [0.5796699523925781, 0.38025131821632385, 0.4813324511051178, -0.5363866090774536] -- [0.5831136107444763, 0.3714934289455414, 0.4865248501300812, -0.5341016054153442] -- [0.585644543170929, 0.3648395836353302, 0.48889750242233276, -0.5337525010108948] -- [0.5871235132217407, 0.35797393321990967, 0.48965802788734436, -0.5360741019248962] -- [0.5674103498458862, 0.380475252866745, 0.4573705196380615, -0.5692944526672363] -- [0.5721796154975891, 0.37270668148994446, 0.4567797780036926, -0.5701335668563843] -- [0.577875018119812, 0.36423900723457336, 0.455659419298172, -0.5707581639289856] -- [0.5858176350593567, 0.3543575406074524, 0.45451995730400085, -0.5697892308235168] -- [0.5937471389770508, 0.34741079807281494, 0.44997870922088623, -0.569463849067688] -- [0.5992497205734253, 0.34061506390571594, 0.4450361132621765, -0.5716851949691772] -- [0.6003798246383667, 0.32667964696884155, 0.44889387488365173, -0.5756028294563293] -- [0.6017261743545532, 0.3177112936973572, 0.4483995735645294, -0.5795885920524597] -- [0.6030813455581665, 0.3085647225379944, 0.4481382668018341, -0.5833117961883545] -- [0.604817807674408, 0.30061909556388855, 0.4461105167865753, -0.5872042775154114] -- [0.6066032648086548, 0.29298049211502075, 0.4439484775066376, -0.5908507108688354] -- [0.6116457581520081, 0.28245407342910767, 0.4466768205165863, -0.5887179970741272] -- [0.6176849007606506, 0.2717817723751068, 0.450827419757843, -0.584255576133728] -- [0.6222812533378601, 0.26440244913101196, 0.4523277282714844, -0.5815985798835754] -- [0.6268923282623291, 0.25755107402801514, 0.4540153443813324, -0.578397274017334] -- [0.6318932771682739, 0.25194233655929565, 0.45568326115608215, -0.5740981101989746] -- [0.6364468336105347, 0.24681340157985687, 0.45705312490463257, -0.5701938271522522] -- [0.6389845609664917, 0.2429831176996231, 0.4557558000087738, -0.570038914680481] -- [0.6415276527404785, 0.23922212421894073, 0.4544799029827118, -0.5697920322418213] -- [0.6442569494247437, 0.23633968830108643, 0.4536820948123932, -0.5685498714447021] -- [0.6469889283180237, 0.23357316851615906, 0.4529207944869995, -0.5671961307525635] -- [0.6498657464981079, 0.2318069338798523, 0.4525589048862457, -0.5649162530899048] -- [0.6526985764503479, 0.23022770881652832, 0.45206087827682495, -0.5626905560493469] -- [0.6558135151863098, 0.2292550951242447, 0.4524427354335785, -0.5591476559638977] -- [0.6587756872177124, 0.22859716415405273, 0.4523778557777405, -0.555978536605835] -- [0.6617470979690552, 0.22891920804977417, 0.452544629573822, -0.5521685481071472] -- [0.6647186875343323, 0.22929240763187408, 0.45273011922836304, -0.5482786297798157] -- [0.668017566204071, 0.23009902238845825, 0.4535982608795166, -0.543190062046051] -- [0.6705873012542725, 0.2311381995677948, 0.45258644223213196, -0.5404193997383118] -- [0.6726135611534119, 0.23361192643642426, 0.450297087430954, -0.5387474894523621] -- [0.6747739315032959, 0.23645348846912384, 0.4481925964355469, -0.5365568399429321] -- [0.6777024269104004, 0.23846016824245453, 0.44796207547187805, -0.5321523547172546] -- [0.6799083948135376, 0.24156345427036285, 0.4460318088531494, -0.5295537114143372] -- [0.6817966103553772, 0.24492041766643524, 0.44435176253318787, -0.5269902348518372] -- [0.683933436870575, 0.24728533625602722, 0.4439108073711395, -0.523476779460907] -- [0.6856308579444885, 0.250881165266037, 0.44196438789367676, -0.521187424659729] -- [0.6875566244125366, 0.2544518709182739, 0.44029614329338074, -0.5183237195014954] -- [0.6917392015457153, 0.2547907829284668, 0.4429968595504761, -0.5102276802062988] -- [0.6901311278343201, 0.25873124599456787, 0.43921414017677307, -0.5136808156967163] -- [0.6918575763702393, 0.26113831996917725, 0.438361793756485, -0.510860800743103] -- [0.6934598684310913, 0.25818297266960144, 0.4432234764099121, -0.5059720277786255] -- [0.6954488158226013, 0.25702276825904846, 0.44498878717422485, -0.502269983291626] -- [0.6966142654418945, 0.25753065943717957, 0.44563689827919006, -0.499813973903656] -- [0.697323739528656, 0.2569231688976288, 0.44662895798683167, -0.4982494115829468] -- [0.6981726288795471, 0.25654295086860657, 0.44796451926231384, -0.4960528612136841] -- [0.6984555721282959, 0.25604796409606934, 0.44973108172416687, -0.4943086802959442] -- [0.6987599730491638, 0.2554562985897064, 0.4514324367046356, -0.4926309585571289] -- [0.6989600658416748, 0.25417235493659973, 0.45315268635749817, -0.491430401802063] -- [0.697912871837616, 0.2565286457538605, 0.4529339075088501, -0.49189573526382446] -- [0.6990689635276794, 0.2593589425086975, 0.4508664011955261, -0.49066758155822754] -- [0.6985029578208923, 0.25572702288627625, 0.45413991808891296, -0.4903612434864044] -- [0.6984465718269348, 0.25450167059898376, 0.4557385742664337, -0.4895954132080078] -- [0.6991413235664368, 0.25562775135040283, 0.45545440912246704, -0.4882796108722687] -- [0.6987333297729492, 0.2546812891960144, 0.45678359270095825, -0.48811662197113037] -- [0.6977765560150146, 0.2522386312484741, 0.4589393436908722, -0.4887312352657318] -- [0.697260856628418, 0.2523416578769684, 0.46000152826309204, -0.48841533064842224] -- [0.6967946887016296, 0.2516184449195862, 0.46154025197029114, -0.48800182342529297] -- [0.6959221959114075, 0.250093549489975, 0.46400707960128784, -0.48769137263298035] -- [0.6949986219406128, 0.2489582896232605, 0.46609148383140564, -0.48760154843330383] -table_position: -- [0.0, 0.15, 0.35] -table_wxyz: -- [0.707106783, 0.707106783, 0.0, 0.0] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/object_registry.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/object_registry.py deleted file mode 100644 index c6668580..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/object_registry.py +++ /dev/null @@ -1,80 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from dataclasses import dataclass - -from robotic_grounding.assets import ASSET_DIR - - -@dataclass -class ObjectSpec: - """Specification for a loadable object.""" - - usd_path: str | None = None - urdf_path: str | None = None - rigid_urdf_path: str | None = None - scale: tuple[float, float, float] = (1.0, 1.0, 1.0) - articulated: bool = False - - -_ARCTIC_URDF_DIR = f"{ASSET_DIR}/urdfs/arctic" - -OBJECT_REGISTRY: dict[str, ObjectSpec] = { - # Arctic objects (articulated URDFs, base joints removed) - "box": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/box_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/box_rigid.urdf", - articulated=True, - ), - "capsulemachine": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/capsulemachine_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/capsulemachine_rigid.urdf", - articulated=True, - ), - "espressomachine": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/espressomachine_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/espressomachine_rigid.urdf", - articulated=True, - ), - "ketchup": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/ketchup_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/ketchup_rigid.urdf", - articulated=True, - ), - "laptop": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/laptop_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/laptop_rigid.urdf", - articulated=True, - ), - "microwave": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/microwave_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/microwave_rigid.urdf", - articulated=True, - ), - "mixer": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/mixer_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/mixer_rigid.urdf", - articulated=True, - ), - "notebook": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/notebook_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/notebook_rigid.urdf", - articulated=True, - ), - "phone": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/phone_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/phone_rigid.urdf", - articulated=True, - ), - "waffleiron": ObjectSpec( - urdf_path=f"{_ARCTIC_URDF_DIR}/waffleiron_art.urdf", - rigid_urdf_path=f"{_ARCTIC_URDF_DIR}/waffleiron_rigid.urdf", - articulated=True, - ), -} - - -def get_object_spec(name: str) -> ObjectSpec | None: - """Return the object spec for the given name, or None if not found.""" - return OBJECT_REGISTRY.get(name) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/__init__.py deleted file mode 100644 index 9199bc14..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Policy assets and implementations (e.g. sonic, grasp).""" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/__init__.py deleted file mode 100644 index b8e92c61..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/__init__.py +++ /dev/null @@ -1,10 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Grasp policy for G1 hands.""" - -from .grasp import G1GraspPolicy, GraspPolicyCfg - -__all__ = [ - "G1GraspPolicy", - "GraspPolicyCfg", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/grasp.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/grasp.py deleted file mode 100644 index de46916f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/grasp/grasp.py +++ /dev/null @@ -1,173 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from dataclasses import MISSING - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.utils import configclass - - -@configclass -class GraspPolicyCfg: - """Configuration for the G1 grasp policy.""" - - asset_name: str = "robot" - """Name of the robot asset in the scene.""" - - grasp_start_threshold: float = 0.20 - """Distance at which hand starts closing. (meters)""" - - grasp_end_threshold: float = 0.05 - """Distance at which hand is fully closed. (meters)""" - - grasp_open_threshold: float = 0.10 - """Distance to end of object trajectory at which hands are opened. (meters)""" - - joint_names: list[str] = MISSING # type: ignore[assignment] - """List of joint names to control.""" - - -class G1GraspPolicy: - """Grasp policy that closes hands based on distance to object.""" - - right_hand_joints_ids: tuple[int, ...] - right_hand_joints_names: tuple[str, ...] - left_hand_joints_ids: tuple[int, ...] - left_hand_joints_names: tuple[str, ...] - - def __init__(self, cfg: GraspPolicyCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the grasp policy with config and environment.""" - self.cfg = cfg - self.device = env.device - - # get joint limits for the joints in joint_names - self.joint_ids, self.joint_names = env.scene[self.cfg.asset_name].find_joints( - self.cfg.joint_names - ) - - # get right hand joints - right_hand_pairs = [ - (joint_id, joint_name) - for joint_id, joint_name in zip( - self.joint_ids, self.joint_names, strict=True - ) - if "right" in joint_name - ] - self.right_hand_joints_ids, self.right_hand_joints_names = ( - zip(*right_hand_pairs, strict=True) if right_hand_pairs else ((), ()) - ) - self.right_hand_joint_limits = env.scene[ - self.cfg.asset_name - ].data.joint_pos_limits[:, self.right_hand_joints_ids, :] - - # get left hand joints - left_hand_pairs = [ - (joint_id, joint_name) - for joint_id, joint_name in zip( - self.joint_ids, self.joint_names, strict=True - ) - if "left" in joint_name - ] - self.left_hand_joints_ids, self.left_hand_joints_names = ( - zip(*left_hand_pairs, strict=True) if left_hand_pairs else ((), ()) - ) - self.left_hand_joint_limits = env.scene[ - self.cfg.asset_name - ].data.joint_pos_limits[:, self.left_hand_joints_ids, :] - - # Create masks for joints that need inverted close direction (all thumb joints go to lower limit when closing) - self.left_hand_invert_mask = torch.tensor( - ["thumb" in name for name in self.left_hand_joints_names], - dtype=torch.bool, - device=self.device, - ) - self.right_hand_invert_mask = torch.tensor( - ["thumb" in name for name in self.right_hand_joints_names], - dtype=torch.bool, - device=self.device, - ) - - def _compute_close_amount(self, distance: torch.Tensor) -> torch.Tensor: - """Compute close amount [0, 1] based on distance. - - - distance >= grasp_start_threshold: 0.0 (fully open) - - distance <= grasp_end_threshold: 1.0 (fully closed) - - in between: linear interpolation - """ - range_size = self.cfg.grasp_start_threshold - self.cfg.grasp_end_threshold - close_amount = 1.0 - (distance - self.cfg.grasp_end_threshold) / range_size - return torch.clamp(close_amount, 0.0, 1.0) - - def __call__( - self, obs: dict, env: ManagerBasedRLEnv, command_name: str = "motion" - ) -> dict: - """Compute hand actions from hand_policy observations.""" - obs = obs["hand_policy"] - - # Compute left hand close amount based on distance (0.0 = open, 1.0 = closed) - left_hand_object_transform = obs[:, :7] - left_hand_distance = torch.norm(left_hand_object_transform[:, :3], dim=-1) - left_hand_close_amount = self._compute_close_amount(left_hand_distance) - - # Compute right hand close amount based on distance - right_hand_object_transform = obs[:, 7:] - right_hand_distance = torch.norm(right_hand_object_transform[:, :3], dim=-1) - right_hand_close_amount = self._compute_close_amount(right_hand_distance) - - # Expand to all joints - left_hand_actions = ( - left_hand_close_amount.unsqueeze(-1) - .expand(-1, len(self.left_hand_joints_ids)) - .clone() - ) - right_hand_actions = ( - right_hand_close_amount.unsqueeze(-1) - .expand(-1, len(self.right_hand_joints_ids)) - .clone() - ) - - # If command is at end of object trajectory, override to open hands - command = env.command_manager.get_term(command_name) - end_object_pos = command._object_pos_w[-1, :3] + env.scene.env_origins - end_open_hands = ( - torch.norm(command.object_pos_w - end_object_pos, dim=-1) - < self.cfg.grasp_open_threshold - ) - end_open_hands = end_open_hands.unsqueeze(-1) - left_hand_actions = torch.where( - end_open_hands, torch.zeros_like(left_hand_actions), left_hand_actions - ) - right_hand_actions = torch.where( - end_open_hands, torch.zeros_like(right_hand_actions), right_hand_actions - ) - - # Thumbs are inverted: they open at lower limit, close at upper limit - left_hand_actions[:, self.left_hand_invert_mask] = ( - 1.0 - left_hand_actions[:, self.left_hand_invert_mask] - ) - right_hand_actions[:, self.right_hand_invert_mask] = ( - 1.0 - right_hand_actions[:, self.right_hand_invert_mask] - ) - - # Scale to joint limits: 0.0 -> lower limit (open), 1.0 -> upper limit (closed) - left_hand_actions = ( - left_hand_actions - * ( - self.left_hand_joint_limits[:, :, 0] - - self.left_hand_joint_limits[:, :, 1] - ) - + self.left_hand_joint_limits[:, :, 1] - ) - right_hand_actions = ( - right_hand_actions - * ( - self.right_hand_joint_limits[:, :, 1] - - self.right_hand_joint_limits[:, :, 0] - ) - + self.right_hand_joint_limits[:, :, 0] - ) - - return { - "left_hand_actions": left_hand_actions, - "right_hand_actions": right_hand_actions, - } diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/LICENSE deleted file mode 100644 index 20cd33bf..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/LICENSE +++ /dev/null @@ -1,186 +0,0 @@ -================================================================================ -DUAL LICENSE NOTICE -================================================================================ - -This repository is dual-licensed. Different components are under different terms: - -1. SOURCE CODE - Apache License 2.0 - All source code, scripts, and software components - -2. MODEL WEIGHTS - NVIDIA Open Model License - All trained model checkpoints and weights - -See below for the full text of each license. - -================================================================================ -PART 1: SOURCE CODE LICENSE (Apache License 2.0) -================================================================================ - -Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. - - -================================================================================ -PART 2: MODEL WEIGHTS LICENSE (NVIDIA Open Model License) -================================================================================ - -NVIDIA OPEN MODEL LICENSE AGREEMENT - -Last Modified: October 24, 2025 - -NVIDIA Corporation and its affiliates ("NVIDIA") grants permission to use machine -learning models under specific conditions. Key permissions include creating -derivative models and distributing them, with NVIDIA retaining no ownership claims -over outputs generated by users. - -SECTION 1: DEFINITIONS - -1.1 "Derivative Model" means any modification of, or works based on or derived -from, the Model, excluding outputs. - -1.2 "Legal Entity" means the union of the acting entity and all other entities -that control, are controlled by, or are under common control with that entity. - -1.3 "Model" means the machine learning model, software, and any checkpoints, -weights, algorithms, parameters, configuration files, and documentation that NVIDIA -makes available under this Agreement. - -1.4 "NVIDIA Cosmos Model" means a multimodal Model that is covered by this Agreement. - -1.5 "Special-Purpose Model" means a Model that is limited to narrow, -purpose-specific tasks. - -1.6 "You" or "Your" means an individual or Legal Entity exercising permissions -granted by this Agreement. - -SECTION 2: CONDITIONS FOR USE, LICENSE GRANT, AI ETHICS AND IP OWNERSHIP - -2.1 Conditions for Use. You must comply with all terms and conditions of this -Agreement. If You initiate copyright or patent litigation against any entity -(including a cross-claim or counterclaim in a lawsuit) alleging that the Model -constitutes direct or contributory infringement, then Your licenses under this -Agreement shall terminate. If You circumvent any safety guardrails or safety -measures built in to the Model without providing comparable alternatives, Your -rights under this Agreement shall terminate. NVIDIA may update this Agreement at -any time to comply with applicable law; Your continued use constitutes Your -acceptance of the updated terms. - -2.2 License Grant. Subject to the terms and conditions of this Agreement, NVIDIA -hereby grants You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, -revocable license to publicly perform, publicly display, reproduce, use, create -derivative works of, make, have made, sell, offer for sale, distribute and import -the Model. - -2.3 AI Ethics. Your use of the Model must be in accordance with NVIDIA's -Trustworthy AI terms, which can be found at -https://www.nvidia.com/en-us/agreements/trustworthy-ai/terms/. - -2.4 IP Ownership. NVIDIA owns the original Model and NVIDIA's Derivative Models. -You own Your Derivative Models. NVIDIA makes no claim of ownership to outputs. You -are responsible for outputs and their subsequent uses. - -SECTION 3: REDISTRIBUTION - -You may reproduce and distribute copies of the Model or Derivative Models thereof, -with or without modifications, provided that You meet the following conditions: - -a. You must include a copy of this Agreement. - -b. You must include the following attribution notice, which can appear in the same -location as other third-party notices or license information: "Licensed by NVIDIA -Corporation under the NVIDIA Open Model License." - -c. If You are distributing a NVIDIA Cosmos Model, You must also include the phrase -"Built on NVIDIA Cosmos" on the applicable website, in the user interface, in a -blog, in an "about" page, or in product documentation. - -d. You may add Your own copyright statement to Your modifications and may provide -additional or different license terms and conditions for use, reproduction, or -distribution of Your modifications or for any Derivative Models as a whole, -provided Your use, reproduction, and distribution otherwise complies with this -Agreement. - -SECTION 4: SEPARATE COMPONENTS - -The Model may contain components that are subject to separate legal notices or -governed by separate licenses (including Open Source Software Licenses), as may be -described in any files made available with the Model. Your use of those separate -components is subject to the applicable license. This Agreement shall control over -the separate licenses for third-party Open Source Software to the extent that the -separate license imposes additional restrictions. "Open Source Software License" -means any software license approved by the Open Source Initiative, Free Software -Foundation, or similar recognized organization, or a license identified by SPDX. - -SECTION 5: TRADEMARKS - -This Agreement does not grant permission to use the trade names, trademarks, -service marks, or product names of NVIDIA, except as required for reasonable and -customary use in describing the origin of the Model and reproducing the content of -the notice. - -SECTION 6: DISCLAIMER OF WARRANTY - -NVIDIA provides the Model on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF -ANY KIND, either express or implied, including, without limitation, any warranties -or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A -PARTICULAR PURPOSE. You are solely responsible for reviewing the documentation -accompanying the Model and determining the appropriateness of using the Model, and -You understand that Special-Purpose Models are limited to narrow, purpose-specific -tasks and must not be deployed for uses that are beyond such tasks. - -SECTION 7: LIMITATION OF LIABILITY - -In no event and under no legal theory, whether in tort (including negligence), -contract, or otherwise, unless required by applicable law (such as deliberate and -grossly negligent acts) or agreed to in writing, will NVIDIA be liable to You for -damages, including any direct, indirect, special, incidental, or consequential -damages of any character arising as a result of this Agreement or out of the use -or inability to use the Model or Derivative Models or outputs (including but not -limited to damages for loss of goodwill, work stoppage, computer failure or -malfunction, or any and all other commercial damages or losses), even if NVIDIA has -been advised of the possibility of such damages. - -SECTION 8: INDEMNITY - -You will defend, indemnify and hold harmless NVIDIA and its affiliates, and their -respective employees, contractors, directors, officers and agents, from and against -any and all claims, damages, obligations, losses, liabilities, costs or debt, and -expenses (including but not limited to attorney's fees) arising from Your use or -distribution of the Model or Derivative Models or outputs. - -SECTION 9: FEEDBACK - -NVIDIA may use feedback You provide without restriction and without any -compensation to You. - -SECTION 10: GOVERNING LAW - -This Agreement will be governed in all respects by the laws of the United States -and of the State of Delaware, without regard to conflict of laws provisions. The -federal and state courts residing in Santa Clara County, California shall have -exclusive jurisdiction over any dispute arising out of this Agreement, and You -hereby consent to the personal jurisdiction of such courts. However, NVIDIA shall -have the right to seek injunctive relief in any court of competent jurisdiction. - -SECTION 11: TRADE AND COMPLIANCE - -You shall comply with all applicable import, export, trade, and economic sanctions -laws, including without limitation the Export Administration Regulations and -economic sanctions laws implemented by the Office of Foreign Assets Control, that -restrict or govern the destination, end-user and end-use of NVIDIA products, -technology, software, and services. - ---- - -Version Release Date: October 24, 2025 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/NOTICE deleted file mode 100644 index be2bb9df..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/NOTICE +++ /dev/null @@ -1,11 +0,0 @@ -This directory contains SONIC ONNX model weights: - -- encoder_batched.onnx -- decoder_batched.onnx - -These model weights are licensed by NVIDIA Corporation under the NVIDIA Open -Model License. The full license text is in the LICENSE file alongside this -notice. - -Source code in this repository remains licensed under Apache-2.0 unless -otherwise marked. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/decoder_batched.onnx b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/decoder_batched.onnx deleted file mode 100644 index 3fce526d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/decoder_batched.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:de629511bc3a110a26827a54389e82d35a722ab9b7cc4232aab865b8451d2a18 -size 40903373 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/encoder_batched.onnx b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/encoder_batched.onnx deleted file mode 100644 index a0999992..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/policies/sonic/encoder_batched.onnx +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6071c943492cb6c91e99b392200a288e829f5fea9491b530b34451fcf9e8d02a -size 50112213 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/rigid_object.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/rigid_object.py deleted file mode 100644 index 99c8247e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/rigid_object.py +++ /dev/null @@ -1,72 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import math - -import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObjectCfg - -from robotic_grounding.assets import ASSET_DIR - -object_name = "arctic/box_rigid" -object_no_collision_name = "arctic/box_no_collision" - -RIGID_OBJECT_CFG = RigidObjectCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - asset_path=f"{ASSET_DIR}/urdfs/{object_name}.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - enable_gyroscopic_forces=False, - linear_damping=0.01, - angular_damping=0.01, - max_linear_velocity=1000.0, - max_angular_velocity=64 / math.pi * 180.0, - max_depenetration_velocity=1.0, - max_contact_impulse=1e3, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0.0, damping=0.0 - ) - ), - collision_props=sim_utils.CollisionPropertiesCfg( - contact_offset=0.0, - rest_offset=0.0, - ), - collider_type="convex_decomposition", - ), - init_state=RigidObjectCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - rot=[1, 0, 0, 0], - ), -) - -RIGID_OBJECT_NO_COLLISION_CFG = RigidObjectCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - asset_path=f"{ASSET_DIR}/urdfs/{object_no_collision_name}.urdf", - activate_contact_sensors=True, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - enable_gyroscopic_forces=False, - linear_damping=0.01, - angular_damping=0.01, - max_linear_velocity=1000.0, - max_angular_velocity=64 / math.pi * 180.0, - max_depenetration_velocity=1.0, - max_contact_impulse=1e3, - ), - joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg( - stiffness=0.0, damping=0.0 - ) - ), - ), - init_state=RigidObjectCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - rot=[1, 0, 0, 0], - ), -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/robot_registry.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/robot_registry.py deleted file mode 100644 index c9607199..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/robot_registry.py +++ /dev/null @@ -1,82 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from dataclasses import dataclass, field - -from isaaclab.assets.articulation import ArticulationCfg - -from robotic_grounding.assets.g1 import G1_CYLINDER_MODEL_12_HANDS_DEX_DELAYED_CFG -from robotic_grounding.assets.sharpa_wave import ( - FINGER_JOINTS, - FINGERTIP_BODY_NAME, - HAND_CONTACT_BODIES, - LEFT_SHARPA_WAVE_CFG, - LEFT_SHARPA_WAVE_PRIMITIVE_CFG, - RIGHT_SHARPA_WAVE_CFG, - RIGHT_SHARPA_WAVE_PRIMITIVE_CFG, - WRIST_BODY_NAME, - WRIST_JOINTS, -) - - -@dataclass -class RobotSpec: - """Everything needed to place a robot into a scene and wire up commands. - - Supports two layouts: - - Dual floating hands: set left_cfg + right_cfg - - Single robot (humanoid, mobile manip): set robot_cfg - """ - - # Single robot articulation (whole-body, mobile manip, etc.) - robot_cfg: ArticulationCfg | None = None - - # Dual floating hands - left_cfg: ArticulationCfg | None = None - right_cfg: ArticulationCfg | None = None - - # Dual floating hands with primitive URDFs - left_primitive_cfg: ArticulationCfg | None = None - right_primitive_cfg: ArticulationCfg | None = None - - # Hand joint/body names for command wiring - wrist_joint_names: list[str] = field(default_factory=list) - finger_joint_names: list[str] = field(default_factory=list) - wrist_body_name: str = "" - fingertip_body_name: str = "" - hand_contact_bodies: list[str] = field(default_factory=list) - - @property - def is_dual_hand(self) -> bool: - """Whether the robot has separate left and right hand configs.""" - return self.left_cfg is not None and self.right_cfg is not None - - -ROBOT_REGISTRY: dict[str, RobotSpec] = { - "sharpa_wave": RobotSpec( - left_cfg=LEFT_SHARPA_WAVE_CFG, - right_cfg=RIGHT_SHARPA_WAVE_CFG, - left_primitive_cfg=LEFT_SHARPA_WAVE_PRIMITIVE_CFG, - right_primitive_cfg=RIGHT_SHARPA_WAVE_PRIMITIVE_CFG, - wrist_joint_names=WRIST_JOINTS, - finger_joint_names=FINGER_JOINTS, - wrist_body_name=WRIST_BODY_NAME, - fingertip_body_name=FINGERTIP_BODY_NAME, - hand_contact_bodies=HAND_CONTACT_BODIES, - ), - # Whole-body retarget (soma_to_g1.py) uses main_with_hand.urdf — keep asset in sync. - "g1": RobotSpec( - robot_cfg=G1_CYLINDER_MODEL_12_HANDS_DEX_DELAYED_CFG, - wrist_joint_names=[], - finger_joint_names=[], - wrist_body_name="", - fingertip_body_name="", - hand_contact_bodies=[], - ), -} - - -def get_robot_spec(name: str) -> RobotSpec | None: - """Return the robot spec for the given name, or None if not found.""" - return ROBOT_REGISTRY.get(name) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/sharpa_wave.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/sharpa_wave.py deleted file mode 100644 index 24366169..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/sharpa_wave.py +++ /dev/null @@ -1,366 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import math - -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets.articulation import ArticulationCfg - -from robotic_grounding.assets import ASSET_DIR - -################################################# -# Control Parameters -################################################# - -WRIST_ARMATURE = 0.001 -WRIST_FRICTION = 0.001 -WRIST_EFFORT_LIMIT = 80.0 -WRIST_VELOCITY_LIMIT = 16.0 - -WRIST_SLIDE_STIFFNESS = 60.0 -WRIST_SLIDE_DAMPING = 3.0 - -WRIST_REVOLUTE_STIFFNESS = 60.0 -WRIST_REVOLUTE_DAMPING = 3.0 - -FINGER_STIFFNESS = 1.74533 -FINGER_DAMPING = 0.01745 -FINGER_VELOCITY_LIMIT = 11.62 - -MCP_ARMATURE = 0.00265 -MCP_FRICTION = 0.07456 -MCP_EFFORT_LIMIT = 1.864 - -PIP_ARMATURE = 0.0006 -PIP_FRICTION = 0.01276 -PIP_EFFORT_LIMIT = 0.638 - -DIP_ARMATURE = 0.00042 -DIP_FRICTION = 0.00378738 -DIP_EFFORT_LIMIT = 0.18937 - -CMC_ARMATURE = 0.0032 -CMC_FRICTION = 0.132 -CMC_EFFORT_LIMIT = 3.3 - -PCMC_ARMATURE = 0.00012 -PCMC_FRICTION = 0.012 -PCMC_EFFORT_LIMIT = 0.5285 - -sharpa_wave_urdf_dir = f"{ASSET_DIR}/urdfs/sharpawave" -dual_sharpa_wave_urdf_path = f"{sharpa_wave_urdf_dir}/dual_sharpa_wave.urdf" -right_sharpa_wave_urdf_path = f"{sharpa_wave_urdf_dir}/right_sharpa_wave.urdf" -left_sharpa_wave_urdf_path = f"{sharpa_wave_urdf_dir}/left_sharpa_wave.urdf" -right_sharpa_wave_primitive_urdf_path = ( - f"{sharpa_wave_urdf_dir}/right_sharpa_wave_primitive.urdf" -) -left_sharpa_wave_primitive_urdf_path = ( - f"{sharpa_wave_urdf_dir}/left_sharpa_wave_primitive.urdf" -) - -rigid_props = sim_utils.RigidBodyPropertiesCfg( - disable_gravity=False, - retain_accelerations=False, - enable_gyroscopic_forces=False, - linear_damping=0.01, - angular_damping=0.01, - max_linear_velocity=1000.0, - max_angular_velocity=64 / math.pi * 180.0, - max_depenetration_velocity=1.0, - max_contact_impulse=1e3, -) - -articulation_props = sim_utils.ArticulationRootPropertiesCfg( - enabled_self_collisions=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, - sleep_threshold=0.005, - stabilization_threshold=0.0005, -) - -none_joint_drive = sim_utils.UrdfConverterCfg.JointDriveCfg( - gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=0, damping=0) -) - -finger_actuators = ImplicitActuatorCfg( - joint_names_expr=[ - ".*_CMC_.*", # thumb CMC - ".*_pinky_CMC", # pinky CMC - ".*_MCP_.*", - ".*_IP", - ".*_PIP", - ".*_DIP", - ], - effort_limit_sim={ - ".*_CMC_.*": CMC_EFFORT_LIMIT, - ".*_pinky_CMC": PCMC_EFFORT_LIMIT, - ".*_MCP_.*": MCP_EFFORT_LIMIT, - ".*_IP": PIP_EFFORT_LIMIT, - ".*_PIP": PIP_EFFORT_LIMIT, - ".*_DIP": DIP_EFFORT_LIMIT, - }, - velocity_limit_sim={ - ".*_CMC_.*": FINGER_VELOCITY_LIMIT, - ".*_pinky_CMC": FINGER_VELOCITY_LIMIT, - ".*_MCP_.*": FINGER_VELOCITY_LIMIT, - ".*_IP": FINGER_VELOCITY_LIMIT, - ".*_PIP": FINGER_VELOCITY_LIMIT, - ".*_DIP": FINGER_VELOCITY_LIMIT, - }, - stiffness={ - ".*_CMC_.*": FINGER_STIFFNESS, - ".*_pinky_CMC": FINGER_STIFFNESS, - ".*_MCP_.*": FINGER_STIFFNESS, - ".*_IP": FINGER_STIFFNESS, - ".*_PIP": FINGER_STIFFNESS, - ".*_DIP": FINGER_STIFFNESS, - }, - damping={ - ".*_CMC_.*": FINGER_DAMPING, - ".*_pinky_CMC": FINGER_DAMPING, - ".*_MCP_.*": FINGER_DAMPING, - ".*_IP": FINGER_DAMPING, - ".*_PIP": FINGER_DAMPING, - ".*_DIP": FINGER_DAMPING, - }, - armature={ - ".*_CMC_.*": CMC_ARMATURE, - ".*_pinky_CMC": PCMC_ARMATURE, - ".*_MCP_.*": MCP_ARMATURE, - ".*_IP": PIP_ARMATURE, - ".*_PIP": PIP_ARMATURE, - ".*_DIP": DIP_ARMATURE, - }, - friction={ - ".*_CMC_.*": CMC_FRICTION, - ".*_pinky_CMC": PCMC_FRICTION, - ".*_MCP_.*": MCP_FRICTION, - ".*_IP": PIP_FRICTION, - ".*_PIP": PIP_FRICTION, - ".*_DIP": DIP_FRICTION, - }, -) - -################################################# -# Dual Sharpa Wave with shared base link -################################################# - -DUAL_SHARPA_WAVE_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=True, - asset_path=dual_sharpa_wave_urdf_path, - activate_contact_sensors=True, - rigid_props=rigid_props, - articulation_props=articulation_props, - joint_drive=none_joint_drive, - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos={ - "right_wrist_y": 0.2, - "left_wrist_y": -0.2, - }, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=1.0, - actuators={ - "wrists": ImplicitActuatorCfg( - joint_names_expr=[ - ".*_wrist_x", - ".*_wrist_y", - ".*_wrist_z", - ".*_wrist_roll", - ".*_wrist_pitch", - ".*_wrist_yaw", - ], - effort_limit_sim={ - ".*_wrist_x": WRIST_EFFORT_LIMIT, - ".*_wrist_y": WRIST_EFFORT_LIMIT, - ".*_wrist_z": WRIST_EFFORT_LIMIT, - ".*_wrist_roll": WRIST_EFFORT_LIMIT, - ".*_wrist_pitch": WRIST_EFFORT_LIMIT, - ".*_wrist_yaw": WRIST_EFFORT_LIMIT, - }, - velocity_limit_sim={ - ".*_wrist_x": WRIST_VELOCITY_LIMIT, - ".*_wrist_y": WRIST_VELOCITY_LIMIT, - ".*_wrist_z": WRIST_VELOCITY_LIMIT, - ".*_wrist_roll": WRIST_VELOCITY_LIMIT, - ".*_wrist_pitch": WRIST_VELOCITY_LIMIT, - ".*_wrist_yaw": WRIST_VELOCITY_LIMIT, - }, - stiffness={ - ".*_wrist_x": WRIST_SLIDE_STIFFNESS, - ".*_wrist_y": WRIST_SLIDE_STIFFNESS, - ".*_wrist_z": WRIST_SLIDE_STIFFNESS, - ".*_wrist_roll": WRIST_REVOLUTE_STIFFNESS, - ".*_wrist_pitch": WRIST_REVOLUTE_STIFFNESS, - ".*_wrist_yaw": WRIST_REVOLUTE_STIFFNESS, - }, - damping={ - ".*_wrist_x": WRIST_SLIDE_DAMPING, - ".*_wrist_y": WRIST_SLIDE_DAMPING, - ".*_wrist_z": WRIST_SLIDE_DAMPING, - ".*_wrist_roll": WRIST_REVOLUTE_DAMPING, - ".*_wrist_pitch": WRIST_REVOLUTE_DAMPING, - ".*_wrist_yaw": WRIST_REVOLUTE_DAMPING, - }, - armature={ - ".*_wrist_x": WRIST_ARMATURE, - ".*_wrist_y": WRIST_ARMATURE, - ".*_wrist_z": WRIST_ARMATURE, - ".*_wrist_roll": WRIST_ARMATURE, - ".*_wrist_pitch": WRIST_ARMATURE, - ".*_wrist_yaw": WRIST_ARMATURE, - }, - friction={ - ".*_wrist_x": WRIST_FRICTION, - ".*_wrist_y": WRIST_FRICTION, - ".*_wrist_z": WRIST_FRICTION, - ".*_wrist_roll": WRIST_FRICTION, - ".*_wrist_pitch": WRIST_FRICTION, - ".*_wrist_yaw": WRIST_FRICTION, - }, - ), - "fingers": finger_actuators, - }, -) - -DUAL_SHARPA_WAVE_ACTION_SCALE = {} -for a in DUAL_SHARPA_WAVE_CFG.actuators.values(): - e = a.effort_limit_sim - s = a.stiffness - names = a.joint_names_expr - if not isinstance(e, dict): - e = {n: e for n in names} - if not isinstance(s, dict): - s = {n: s for n in names} - for n in names: - if n in e and n in s and s[n]: - DUAL_SHARPA_WAVE_ACTION_SCALE[n] = 0.25 * e[n] / s[n] - -################################################# -# Single Sharpa Wave -################################################# - -RIGHT_SHARPA_WAVE_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - asset_path=right_sharpa_wave_urdf_path, - activate_contact_sensors=True, - rigid_props=rigid_props, - articulation_props=articulation_props, - joint_drive=none_joint_drive, - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos={".*": 0.0}, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=1.0, - actuators={ - "fingers": finger_actuators, - }, -) - -RIGHT_SHARPA_WAVE_PRIMITIVE_CFG = RIGHT_SHARPA_WAVE_CFG.replace( - spawn=RIGHT_SHARPA_WAVE_CFG.spawn.replace( - asset_path=right_sharpa_wave_primitive_urdf_path, - ), -) - -LEFT_SHARPA_WAVE_CFG = ArticulationCfg( - spawn=sim_utils.UrdfFileCfg( - fix_base=False, - asset_path=left_sharpa_wave_urdf_path, - activate_contact_sensors=True, - rigid_props=rigid_props, - articulation_props=articulation_props, - joint_drive=none_joint_drive, - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.0), - joint_pos={".*": 0.0}, - joint_vel={".*": 0.0}, - ), - soft_joint_pos_limit_factor=1.0, - actuators={ - "fingers": finger_actuators, - }, -) - -LEFT_SHARPA_WAVE_PRIMITIVE_CFG = LEFT_SHARPA_WAVE_CFG.replace( - spawn=LEFT_SHARPA_WAVE_CFG.spawn.replace( - asset_path=left_sharpa_wave_primitive_urdf_path, - ), -) - -################################################# -# Parameters -################################################# - -WRIST_JOINTS = [ - ".*_wrist_x", - ".*_wrist_y", - ".*_wrist_z", - ".*_wrist_roll", - ".*_wrist_pitch", - ".*_wrist_yaw", -] - -FINGER_JOINTS = [ - ".*_index_MCP_FE", - ".*_middle_MCP_FE", - ".*_pinky_CMC", - ".*_ring_MCP_FE", - ".*_thumb_CMC_FE", - ".*_index_MCP_AA", - ".*_middle_MCP_AA", - ".*_pinky_MCP_FE", - ".*_ring_MCP_AA", - ".*_thumb_CMC_AA", - ".*_index_PIP", - ".*_middle_PIP", - ".*_pinky_MCP_AA", - ".*_ring_PIP", - ".*_thumb_MCP_FE", - ".*_index_DIP", - ".*_middle_DIP", - ".*_pinky_PIP", - ".*_ring_DIP", - ".*_thumb_MCP_AA", - ".*_pinky_DIP", - ".*_thumb_IP", -] - -WRIST_BODY_NAME = ".*_hand_C_MC" -FINGERTIP_BODY_NAME = ".*_DP" - -# All links in the hand with collision geometry (palm, phalanges). -# Excludes *_fingertip: no collision in URDF, so not created as rigid bodies. -# Excludes *_elastomer: URDF importer merges them into parent _DP bodies. -HAND_CONTACT_BODIES = [ - # Palm - ".*_hand_C_MC", # "C_MC" stands for "Central Metacarpal"; palm/base link - # Thumb - ".*_thumb_MC", - ".*_thumb_PP", - ".*_thumb_DP", - # Index - ".*_index_PP", - ".*_index_MP", - ".*_index_DP", - # Middle - ".*_middle_PP", - ".*_middle_MP", - ".*_middle_DP", - # Ring - ".*_ring_PP", - ".*_ring_MP", - ".*_ring_DP", - # Pinky - ".*_pinky_MC", - ".*_pinky_PP", - ".*_pinky_MP", - ".*_pinky_DP", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/arctic_table.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/arctic_table.urdf deleted file mode 100644 index 1f2b16b9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/arctic_table.urdf +++ /dev/null @@ -1,31 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/LICENSE deleted file mode 100644 index e472bbd9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/NOTICE deleted file mode 100644 index 16269b40..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/NOTICE +++ /dev/null @@ -1,8 +0,0 @@ -This directory contains Unitree G1 and Dex3 URDF robot-description assets. - -The assets are from or derived from Unitree G1 Description assets developed by -Unitree Robotics and are licensed under the BSD 3-Clause License. Local changes -include repository path normalization and reduced variants for the G1 workflows -used by this project. - -The full license text is in the LICENSE file alongside this notice. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_left.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_left.urdf deleted file mode 100644 index 825a6543..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_left.urdf +++ /dev/null @@ -1,282 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_right.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_right.urdf deleted file mode 100644 index b1b444f3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/dex3_right.urdf +++ /dev/null @@ -1,282 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main.urdf deleted file mode 100644 index 1d0d4f8e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main.urdf +++ /dev/null @@ -1,1494 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_nodex.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_nodex.urdf deleted file mode 100644 index a9499a69..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_nodex.urdf +++ /dev/null @@ -1,1036 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_with_hand.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_with_hand.urdf deleted file mode 100644 index 492bd6a4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/g1/main_with_hand.urdf +++ /dev/null @@ -1,1493 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/meshes b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/meshes deleted file mode 120000 index 4b198861..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/meshes +++ /dev/null @@ -1 +0,0 @@ -../meshes \ No newline at end of file diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/LICENSE deleted file mode 100644 index b3ccbb77..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/LICENSE +++ /dev/null @@ -1,6 +0,0 @@ -Copyright 2025 Sharpa Group -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. -You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/NOTICE deleted file mode 100644 index e33ef383..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/NOTICE +++ /dev/null @@ -1,7 +0,0 @@ -Portions of this codebase include software, Mujoco, developed by Google-Deepmind (https://github.com/google-deepmind/mujoco): -Copyright 2021 DeepMind Technologies Limited -Box collision code (engine_collision_box.c) is Copyright 2016 Svetoslav Kolev. -Source code is licensed under the Apache License, Version 2.0: -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/README.md deleted file mode 100644 index 91d40818..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/README.md +++ /dev/null @@ -1,9 +0,0 @@ -## Sharpa Wave Hand Assets - -The Sharpa Wave hand-related meshes, URDFs, and XML files are derived from the original files provided in the [Sharpa Wave hand repository](https://github.com/sharpa-robotics/sharpa-urdf-usd-xml). - -For our use case, we adapt the URDF and XML assets as follows: - -1. Add sites to the XML files for retargeting. -2. Replace collision meshes with primitive capsule geometries to reduce simulation time. -3. Modify the thumb and palm meshes to avoid self-collisions. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/dual_sharpa_wave.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/dual_sharpa_wave.urdf deleted file mode 100644 index b6e75b81..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/dual_sharpa_wave.urdf +++ /dev/null @@ -1,3027 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave.urdf deleted file mode 100644 index 60c10b10..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave.urdf +++ /dev/null @@ -1,1445 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave_primitive.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave_primitive.urdf deleted file mode 100644 index 91f6a531..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/left_sharpa_wave_primitive.urdf +++ /dev/null @@ -1,1426 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave.urdf deleted file mode 100644 index 894b83d8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave.urdf +++ /dev/null @@ -1,1444 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave_primitive.urdf b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave_primitive.urdf deleted file mode 100644 index 1ae94631..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/urdfs/sharpawave/right_sharpa_wave_primitive.urdf +++ /dev/null @@ -1,1426 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1.xml deleted file mode 100644 index 6996584e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1.xml +++ /dev/null @@ -1,446 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof.xml deleted file mode 100644 index 8861049c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof.xml +++ /dev/null @@ -1,478 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof_rev_1_0.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof_rev_1_0.xml deleted file mode 100644 index 62b0d4cd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/g1/g1_29dof_rev_1_0.xml +++ /dev/null @@ -1,472 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/LICENSE deleted file mode 100644 index b3ccbb77..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/LICENSE +++ /dev/null @@ -1,6 +0,0 @@ -Copyright 2025 Sharpa Group -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. -You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/NOTICE deleted file mode 100644 index e33ef383..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/NOTICE +++ /dev/null @@ -1,7 +0,0 @@ -Portions of this codebase include software, Mujoco, developed by Google-Deepmind (https://github.com/google-deepmind/mujoco): -Copyright 2021 DeepMind Technologies Limited -Box collision code (engine_collision_box.c) is Copyright 2016 Svetoslav Kolev. -Source code is licensed under the Apache License, Version 2.0: -Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at - http://www.apache.org/licenses/LICENSE-2.0 -Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/README.md deleted file mode 100644 index 91d40818..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/README.md +++ /dev/null @@ -1,9 +0,0 @@ -## Sharpa Wave Hand Assets - -The Sharpa Wave hand-related meshes, URDFs, and XML files are derived from the original files provided in the [Sharpa Wave hand repository](https://github.com/sharpa-robotics/sharpa-urdf-usd-xml). - -For our use case, we adapt the URDF and XML assets as follows: - -1. Add sites to the XML files for retargeting. -2. Replace collision meshes with primitive capsule geometries to reduce simulation time. -3. Modify the thumb and palm meshes to avoid self-collisions. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/left_sharpawave.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/left_sharpawave.xml deleted file mode 100644 index 3e3da51c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/left_sharpawave.xml +++ /dev/null @@ -1,272 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/right_sharpawave.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/right_sharpawave.xml deleted file mode 100644 index 5d37a9ff..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/xmls/sharpawave/right_sharpawave.xml +++ /dev/null @@ -1,272 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/README.md deleted file mode 100644 index f5b1be68..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/README.md +++ /dev/null @@ -1,158 +0,0 @@ -# `motion_schema` — unified motion parquet (version `motion_v1`) - -Single schema shared by: - -- **Producers**: `scripts/retarget/soma_to_g1.py`, - `source/robotic_grounding/robotic_grounding/planner/g1_planner.py::save_planner_parquet`. -- **Consumers**: training loader at - `source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_utils.py`, - `SceneConfig.from_motion_file()`, `scripts/rsl_rl/dummy_agent.py::_autoframe_viewer`, - `scripts/reconstruct_support_surfaces.py`, `tasks/scene_utils/replay_data.py`. - -Design context lives alongside this doc in -[motion_schema.md](motion_schema.md). - -## Public API - -```python -from robotic_grounding.motion_schema import ( - MotionData, # in-memory dataclass - SCHEMA_VERSION, # "motion_v1" - SchemaVersionMismatch, # raised by reader on mismatch - MissingRequiredField, # raised by reader on missing required field - SINGLE_ROBOT, # motion_kind value: whole-body - DUAL_HAND, # motion_kind value: floating dual hands - KNOWN_MOTION_KINDS, # frozenset of accepted motion_kind values - save_motion_parquet, # writer - load_motion_data_parquet,# reader - build_schema, # pyarrow schema factory - resolve_motion_kind, # resolves and validates `motion_kind` - required_fields_for, # required fields for a given motion_kind -) -``` - -Reader: `md = load_motion_data_parquet(path, device="cuda")`. Accepts a file -path, Hive partition directory, or dataset root; resolves to the first parquet -file and backfills partition columns (`sequence_id`, `robot_name`) from the -directory names. Fills both the on-disk view (`robot_root_position`, `ee_pose_w`, -`hand_sides`-indexed lists, etc.) and the flattened view (`left_wrist_position`, -`right_hand_frames`, etc.) so `tracking_command.py` consumes `MotionData` without -changes. - -Writer: `save_motion_parquet(md, root_path, partition_cols=["sequence_id", "robot_name"])`. -Runs required-field checks (fail-fast when any training-eligible column for the -file's `motion_kind` is missing) and a lightweight `wxyz`-convention guard on -`robot_root_wxyz`. - -## Motion kinds - -Every file carries an explicit `motion_kind` discriminator. Required fields -branch on this value, so producers must tag the file at write time. - -- **`single_robot`** — whole-body humanoid trajectories (e.g. G1, G1+Dex3 - planner output). Requires `robot_joint_names`, `robot_root_position`, - `robot_root_wxyz`, `robot_joint_positions` in addition to the common fields. -- **`dual_hand`** — floating dual-wrist trajectories (e.g. Dex3 from - MANO source). Requires `hand_sides`, `hand_frame_names`, `hand_frames_w`, - `hand_finger_joint_names`, `hand_finger_joints` in addition to the common - fields. Whole-body joint state may be left empty. - -Files predating `motion_kind` will fail to load with `MissingRequiredField`; -no migrator is shipped, so producers must be re-run. - -## Common required fields (every motion_kind) - -The writer / reader will reject a file missing any of: - -| Field | Shape | -|---|---| -| `schema_version` | string (`"motion_v1"`) | -| `sequence_id` | string | -| `robot_name` | string | -| `motion_kind` | string (`"single_robot"` or `"dual_hand"`) | -| `fps` | float32 (> 0) | -| `ee_link_names` | list[str] (length `E`) | -| `ee_pose_w` | (T, E, 7), each entry `[x, y, z, qw, qx, qy, qz]` | -| `object_body_names` | list[str] (length `B`) | -| `object_body_position` | (T, B, 3) | -| `object_body_wxyz` | (T, B, 4), wxyz | - -### Additional required fields for `motion_kind="single_robot"` - -| Field | Shape | -|---|---| -| `robot_joint_names` | list[str] (length `J`) | -| `robot_root_position` | (T, 3) | -| `robot_root_wxyz` | (T, 4), wxyz | -| `robot_joint_positions` | (T, J) aligned with `robot_joint_names` | - -### Additional required fields for `motion_kind="dual_hand"` - -| Field | Shape | -|---|---| -| `hand_sides` | list[str] (length `S`) | -| `hand_frame_names` | list[list[str]] aligned with `hand_sides` | -| `hand_frames_w` | per-side `(T, K, 7)` aligned with `hand_sides` | -| `hand_finger_joint_names` | list[list[str]] aligned with `hand_sides` | -| `hand_finger_joints` | per-side `(T, J_f)` aligned with `hand_sides` | - -The writer additionally enforces that the outer length of every per-side -field equals `len(hand_sides)`, catching producers that set `hand_sides` but -forget to populate one side's payload. - -## Optional groups - -Left empty when the producer does not have the data; training guards each -field. - -- **Contacts** — `hand_contact_link_names`, `hand_link_contact_positions`, - `hand_link_contact_normals`, `hand_object_contact_positions`, - `hand_object_contact_normals`, `hand_object_contact_part_ids`, - `hand_contact_active`. Per-side, aligned by `hand_sides`. -- **Object metadata** — `object_name`, `safe_object_name`, `safe_object_body_names`, - `object_mesh_paths`, `object_urdf_paths`, `object_mesh_radius`, - `object_articulation`, `object_root_axis_angle`, `object_root_position`. -- **Source raw motion** — `source_kind`, `source_payload` (pickled bytes), - `source_joint_names`. -- **Retarget diagnostics** — `ik_error_per_frame`, `ik_num_iterations`, - `frame_task_errors`. - -## Conventions (do not break) - -- Quaternions are **wxyz** everywhere. No xyzw, no Euler angles in the on-disk - schema. -- Pose entries combine position + orientation as `[x, y, z, qw, qx, qy, qz]` - (length 7). -- Time series are `(T, ...)` with T as the leading axis. Per-timestep entries - only — no transposed layouts. -- World frame for wrist and object body poses. If a producer uses a different - convention it must record that in `coord_frame`. - -## Versioning - -- On-disk files carry `schema_version` as a string; the reader raises - `SchemaVersionMismatch` if it does not match `SCHEMA_VERSION`. -- Breaking changes bump the version (e.g. `motion_v2`). Additive changes - inside a version are backward-compatible. -- No migrator is shipped. Files written before the `motion_kind` discriminator - was added must be regenerated by re-running the producing retarget or - planner script. - -## Tests - -- `tests/test_motion_schema.py` — U1–U6 unit tests (round-trip, minimal-file, - version enforcement, `hand_sides` alignment, quaternion guard, variable E) - and K1–K5 `motion_kind` tests (dual-hand round-trip, single/dual required- - field enforcement, per-side alignment, missing/unknown kind rejection). -- `tests/test_motion_schema_parquet_integration.py` — round-trip via the - reader + `SceneConfig.from_motion_file()`. -- `tests/test_replay_data.py` — `load_replay_trajectory` on `motion_v1` - single-robot and dual-hand inputs plus `ManoSharpaData` legacy dual-hand. - -Run: - -```bash -python tests/test_motion_schema.py -python tests/test_motion_schema_parquet_integration.py -python tests/test_replay_data.py -``` diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/__init__.py deleted file mode 100644 index b938b161..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/__init__.py +++ /dev/null @@ -1,66 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unified motion parquet schema (`motion_v1`). - -Shared format used by all whole-body retargeting scripts, the planner, and the -training loader. See `motion_schema.md` for design motivation. -""" - -from .reader import load_motion_data_parquet -from .schema import ( - ALL_FIELDS, - COMMON_REQUIRED_FIELDS, - CONTACT_FIELDS, - DIAGNOSTICS_FIELDS, - DUAL_HAND, - DUAL_HAND_PER_SIDE_FIELDS, - DUAL_HAND_REQUIRED_FIELDS, - EE_FIELDS, - HAND_FIELDS, - KNOWN_MOTION_KINDS, - METADATA_FIELDS, - OBJECT_FIELDS, - REQUIRED_TRAINING_FIELDS, - ROBOT_FIELDS, - SCHEMA_VERSION, - SINGLE_ROBOT, - SINGLE_ROBOT_REQUIRED_FIELDS, - SOURCE_FIELDS, - MissingRequiredField, - MotionData, - SchemaVersionMismatch, - build_schema, - required_fields_for, - resolve_motion_kind, -) -from .writer import DEFAULT_PARTITION_COLS, save_motion_parquet - -__all__ = [ - "ALL_FIELDS", - "COMMON_REQUIRED_FIELDS", - "CONTACT_FIELDS", - "DEFAULT_PARTITION_COLS", - "DIAGNOSTICS_FIELDS", - "DUAL_HAND", - "DUAL_HAND_PER_SIDE_FIELDS", - "DUAL_HAND_REQUIRED_FIELDS", - "EE_FIELDS", - "HAND_FIELDS", - "KNOWN_MOTION_KINDS", - "METADATA_FIELDS", - "MissingRequiredField", - "MotionData", - "OBJECT_FIELDS", - "REQUIRED_TRAINING_FIELDS", - "ROBOT_FIELDS", - "SCHEMA_VERSION", - "SINGLE_ROBOT", - "SINGLE_ROBOT_REQUIRED_FIELDS", - "SOURCE_FIELDS", - "SchemaVersionMismatch", - "build_schema", - "load_motion_data_parquet", - "required_fields_for", - "resolve_motion_kind", - "save_motion_parquet", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/reader.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/reader.py deleted file mode 100644 index 289ef83c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/reader.py +++ /dev/null @@ -1,416 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reader for `motion_v1` parquets. - -Training and downstream tools call `load_motion_data_parquet(path)` to get a -populated `MotionData`. The reader handles: - -- Directory-or-file path resolution (matches the legacy behaviour of - `tracking_utils.load_motion_data` and `SceneConfig.from_motion_file`). -- Schema version enforcement with actionable error messages. -- On-disk → in-memory mapping: `hand_sides`-indexed lists are also exposed as - flattened `left_*` / `right_*` attributes for consumers that predate the - unified schema. -- Convenience derived fields: `ee_pos_w`, `ee_quat_w`, `object_pos_w`, - `object_quat_w`. -""" - -from __future__ import annotations - -from pathlib import Path -from typing import Any - -import numpy as np -import pyarrow as pa -import pyarrow.parquet as pq - -from .schema import ( - DUAL_HAND, - DUAL_HAND_PER_SIDE_FIELDS, - SCHEMA_VERSION, - MissingRequiredField, - MotionData, - SchemaVersionMismatch, - required_fields_for, - resolve_motion_kind, -) - -# Optional dependency: the reader only needs torch when producing tensor -# outputs. Import at module level for ruff PLC0415 but keep the module -# importable in thin environments that don't ship torch. -try: - import torch - - _TORCH_AVAILABLE = True -except ImportError: - _TORCH_AVAILABLE = False - -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- - - -def _resolve_parquet_file(path: str | Path) -> Path: - """Resolve a path to a concrete parquet file. - - Accepts either a direct parquet file, a Hive partition directory, or a - dataset root that contains exactly one partition. - """ - p = Path(path) - if p.is_file(): - return p - if p.is_dir(): - matches = list(p.rglob("*.parquet")) - if not matches: - raise FileNotFoundError(f"No parquet files under {p}") - return matches[0] - raise FileNotFoundError(f"Motion parquet path does not exist: {p}") - - -def _read_single_file(path: Path) -> pa.Table: - """Read a single parquet file, bypassing Hive partition auto-inference. - - `pq.read_table(path)` on a partitioned path tree can infer partition - columns as dictionary types and then clash with the explicit string - columns in the file body. We side-step that by using `ParquetFile`. - """ - pf = pq.ParquetFile(str(path)) - return pf.read() - - -def _partition_values_from_path(path: Path) -> dict[str, str]: - """Extract Hive partition values (`col=val`) from the path components.""" - values: dict[str, str] = {} - for parent in path.parents: - if "=" in parent.name: - col, val = parent.name.split("=", 1) - values[col] = val - return values - - -def _is_empty(val: Any) -> bool: - if val is None: - return True - if isinstance(val, list | tuple | str | bytes): - return len(val) == 0 - return False - - -def _as_tensor( - value: Any, - device: str | None = None, - dtype: Any = None, -) -> Any: - """Coerce a nested-list value into a torch tensor, or return None. - - Returns `Any` because torch is an optional dependency at the module level; - callers that care about the exact type should narrow themselves. - """ - if value is None: - return None - if not _TORCH_AVAILABLE: - raise RuntimeError( - "torch is required to materialize motion_v1 tensors but is not installed in this environment." - ) - - if isinstance(value, torch.Tensor): - out = value - else: - arr = np.asarray(value) - if arr.dtype == object or arr.size == 0: - return None - if dtype is None: - # Preserve int dtype when the input is integer (e.g. part_ids). - if np.issubdtype(arr.dtype, np.integer): - dtype = torch.long - else: - dtype = torch.float32 - out = torch.as_tensor(arr, dtype=dtype) - if device is not None: - out = out.to(device) - return out - - -# --------------------------------------------------------------------------- -# Core reader -# --------------------------------------------------------------------------- - - -def _flatten_per_side( - data: dict[str, Any], - disk_column: str, - side_index: int, -) -> Any: - """Return the side-indexed entry from a per-side list, or None if absent/empty.""" - col = data.get(disk_column) - if not col or col[0] is None: - return None - per_side_list = col[0] - if side_index >= len(per_side_list): - return None - value = per_side_list[side_index] - if _is_empty(value): - return None - return value - - -def _validate_version(data: dict[str, Any], path: Path) -> None: - version = data.get("schema_version", [None]) - if not version or not version[0]: - raise SchemaVersionMismatch( - got="", expected=SCHEMA_VERSION, path=str(path) - ) - got = version[0] - if got != SCHEMA_VERSION: - raise SchemaVersionMismatch(got=got, expected=SCHEMA_VERSION, path=str(path)) - - -def _validate_required(data: dict[str, Any], path: Path) -> None: - """Branch required-field validation on the file's `motion_kind`. - - The reader refuses any file that lacks an explicit `motion_kind`. - Producers must regenerate files that predate the discriminator instead - of relying on inference from neighboring columns. - """ - raw_kind = (data.get("motion_kind") or [None])[0] - if not raw_kind: - raise MissingRequiredField(missing=["motion_kind"], path=str(path)) - try: - motion_kind = resolve_motion_kind({"motion_kind": [raw_kind]}) - except ValueError as exc: - raise MissingRequiredField(missing=["motion_kind"], path=str(path)) from exc - - required = required_fields_for(motion_kind) - missing: list[str] = [] - for name in required: - if name not in data: - missing.append(name) - continue - val = data[name] - if not val or val[0] is None: - missing.append(name) - continue - if name == "fps": - try: - if float(val[0]) <= 0.0: - missing.append(name) - except (TypeError, ValueError): - missing.append(name) - continue - if hasattr(val[0], "__len__") and len(val[0]) == 0: - missing.append(name) - if missing: - raise MissingRequiredField(missing=missing, path=str(path)) - - if motion_kind == DUAL_HAND: - n_sides = len(data.get("hand_sides", [[]])[0] or []) - misaligned: list[tuple[str, int]] = [] - for name in DUAL_HAND_PER_SIDE_FIELDS: - per_side = data.get(name, [[]])[0] or [] - if len(per_side) != n_sides: - misaligned.append((name, len(per_side))) - if misaligned: - details = ", ".join( - f"{name} has {actual} entries" for name, actual in misaligned - ) - raise MissingRequiredField( - missing=[name for name, _ in misaligned], path=str(path) - ) from ValueError( - f"motion_kind=dual_hand per-side fields must align with hand_sides (len={n_sides}); {details}." - ) - - -def load_motion_data_parquet( - path: str | Path, - device: str | None = None, -) -> MotionData: - """Load a `motion_v1` parquet and return a populated `MotionData`. - - Args: - path: File or directory containing the parquet. - device: Optional torch device string to place tensors on. - - Returns: - `MotionData` with both on-disk and flattened attribute views populated. - - Raises: - SchemaVersionMismatch: If the file's `schema_version` is not `motion_v1`. - MissingRequiredField: If required training fields are absent. - FileNotFoundError: If the path doesn't resolve. - """ - parquet_path = _resolve_parquet_file(path) - table = _read_single_file(parquet_path) - data = table.to_pydict() - - # `pq.write_to_dataset` strips partition columns from the file body. When - # we bypass Hive auto-inference above, we must backfill them from the - # partition directory names ourselves. - for col, val in _partition_values_from_path(parquet_path).items(): - if col not in data or not data[col] or data[col][0] is None: - data[col] = [val] - - _validate_version(data, parquet_path) - _validate_required(data, parquet_path) - - md = MotionData() - - # ---- Metadata --------------------------------------------------------- - md.schema_version = data["schema_version"][0] - md.sequence_id = data.get("sequence_id", [""])[0] or "" - md.robot_name = data.get("robot_name", [""])[0] or "" - md.motion_kind = data.get("motion_kind", [""])[0] or "" - md.source_dataset = data.get("source_dataset", [""])[0] or "" - md.raw_motion_file = data.get("raw_motion_file", [""])[0] or "" - md.fps = float(data.get("fps", [0.0])[0] or 0.0) - md.coord_frame = data.get("coord_frame", [""])[0] or "" - - # ---- Robot state ------------------------------------------------------ - # Robot fields are only required for `single_robot`; for `dual_hand` they - # may be absent or empty, and the loaded tensors are correspondingly None. - md.robot_joint_names = list(data.get("robot_joint_names", [[]])[0] or []) - md.robot_root_position = _as_tensor( - data.get("robot_root_position", [None])[0], device=device - ) - md.robot_root_wxyz = _as_tensor( - data.get("robot_root_wxyz", [None])[0], device=device - ) - md.robot_joint_positions = _as_tensor( - data.get("robot_joint_positions", [None])[0], device=device - ) - - # `file_joint_names` is the legacy attribute used by tracking_command.py - # for joint reordering; we alias it to robot_joint_names. - md.file_joint_names = md.robot_joint_names or None - - # ---- End-effector frames --------------------------------------------- - ee_link_names = list(data.get("ee_link_names", [[]])[0] or []) - md.ee_link_names = ee_link_names or None - ee_pose = _as_tensor(data["ee_pose_w"][0], device=device) - md.ee_pose_w = ee_pose - if ee_pose is not None and ee_pose.ndim == 3 and ee_pose.shape[-1] == 7: - md.ee_pos_w = ee_pose[..., 0:3].contiguous() - md.ee_quat_w = ee_pose[..., 3:7].contiguous() - - # ---- Object ----------------------------------------------------------- - md.object_name = data.get("object_name", [""])[0] or "" - md.safe_object_name = data.get("safe_object_name", [""])[0] or "" - md.object_body_names = list(data.get("object_body_names", [[]])[0] or []) - md.safe_object_body_names = list(data.get("safe_object_body_names", [[]])[0] or []) - md.object_mesh_paths = list(data.get("object_mesh_paths", [[]])[0] or []) - md.object_urdf_paths = list(data.get("object_urdf_paths", [[]])[0] or []) - md.object_mesh_radius = list(data.get("object_mesh_radius", [[]])[0] or []) or None - md.object_articulation = _as_tensor( - data.get("object_articulation", [None])[0], device=device - ) - md.object_root_axis_angle = _as_tensor( - data.get("object_root_axis_angle", [None])[0], device=device - ) - md.object_root_position = _as_tensor( - data.get("object_root_position", [None])[0], device=device - ) - obj_pos = _as_tensor(data["object_body_position"][0], device=device) - obj_quat = _as_tensor(data["object_body_wxyz"][0], device=device) - md.object_body_position = obj_pos - md.object_body_wxyz = obj_quat - # Primary body (index 0). - if obj_pos is not None and obj_pos.ndim == 3: - md.object_pos_w = obj_pos[:, 0].contiguous() - elif obj_pos is not None: - md.object_pos_w = obj_pos - if obj_quat is not None and obj_quat.ndim == 3: - md.object_quat_w = obj_quat[:, 0].contiguous() - elif obj_quat is not None: - md.object_quat_w = obj_quat - - # ---- Hands (on-disk + flattened) -------------------------------------- - hand_sides = list(data.get("hand_sides", [[]])[0] or []) - md.hand_sides = hand_sides - md.hand_frame_names = list(data.get("hand_frame_names", [[]])[0] or []) - md.hand_finger_joint_names = list( - data.get("hand_finger_joint_names", [[]])[0] or [] - ) - # Convert per-side tensor lists. - per_side_frames = data.get("hand_frames_w", [[]])[0] or [] - md.hand_frames_w = [_as_tensor(x, device=device) for x in per_side_frames] - per_side_fj = data.get("hand_finger_joints", [[]])[0] or [] - md.hand_finger_joints = [_as_tensor(x, device=device) for x in per_side_fj] - - for side in ("left", "right"): - if side not in hand_sides: - continue - idx = hand_sides.index(side) - # Frame names, frames. - frame_names = ( - md.hand_frame_names[idx] if idx < len(md.hand_frame_names) else None - ) - frames = md.hand_frames_w[idx] if idx < len(md.hand_frames_w) else None - setattr(md, f"{side}_hand_frame_names", list(frame_names or []) or None) - setattr(md, f"{side}_hand_frames", frames) - - # Finger joints / names. - fj_names = ( - md.hand_finger_joint_names[idx] - if idx < len(md.hand_finger_joint_names) - else None - ) - fj = md.hand_finger_joints[idx] if idx < len(md.hand_finger_joints) else None - setattr(md, f"{side}_finger_joint_names", list(fj_names or []) or None) - setattr(md, f"{side}_finger_joints", fj) - - # Wrist pose is derived from the corresponding ee slot when we can. - # ee_link_names convention: produce `[left_*, right_*]` in order; the - # side->ee mapping is left-first when both sides are present. - if md.ee_pose_w is not None and md.ee_link_names: - # Match side to the first ee link whose name contains the side. - matches = [ - i for i, n in enumerate(md.ee_link_names) if side in (n or "").lower() - ] - if matches: - ee_idx = matches[0] - pose = md.ee_pose_w[:, ee_idx, :] - setattr(md, f"{side}_wrist_position", pose[:, 0:3].contiguous()) - setattr(md, f"{side}_wrist_wxyz", pose[:, 3:7].contiguous()) - - # ---- Contacts --------------------------------------------------------- - md.hand_contact_link_names = list( - data.get("hand_contact_link_names", [[]])[0] or [] - ) - for disk_col, attr in ( - ("hand_link_contact_positions", "link_contact_positions"), - ("hand_link_contact_normals", "link_contact_normals"), - ("hand_object_contact_positions", "object_contact_positions"), - ("hand_object_contact_normals", "object_contact_normals"), - ("hand_object_contact_part_ids", "object_contact_part_ids"), - ("hand_contact_active", "hand_contact_active"), - ): - per_side = data.get(disk_col, [[]])[0] or [] - tensors = [_as_tensor(x, device=device) for x in per_side] - setattr(md, disk_col, tensors) - for side in ("left", "right"): - if side in hand_sides: - idx = hand_sides.index(side) - if idx < len(tensors): - # Flattened attribute names follow the legacy tracking_command.py - # convention: `left_link_contact_positions` etc. - if disk_col == "hand_contact_active": - setattr(md, f"{side}_hand_contact_active", tensors[idx]) - else: - setattr(md, f"{side}_{attr}", tensors[idx]) - - # ---- Source raw ------------------------------------------------------- - md.source_kind = data.get("source_kind", [""])[0] or "" - md.source_payload = data.get("source_payload", [b""])[0] or b"" - md.source_joint_names = list(data.get("source_joint_names", [[]])[0] or []) - - # ---- Diagnostics ----------------------------------------------------- - md.ik_error_per_frame = _as_tensor( - data.get("ik_error_per_frame", [None])[0], device=device - ) - md.ik_num_iterations = _as_tensor( - data.get("ik_num_iterations", [None])[0], device=device - ) - md.frame_task_errors = _as_tensor( - data.get("frame_task_errors", [None])[0], device=device - ) - - return md diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/schema.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/schema.py deleted file mode 100644 index a028ad76..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/schema.py +++ /dev/null @@ -1,537 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unified `MotionData` parquet schema (version `motion_v1`). - -This module is the single source of truth for the on-disk layout used by all -whole-body retargeting scripts, the planner, and the training loader. - -- `SCHEMA_VERSION` is the on-disk version tag written to every file. -- `build_schema()` returns the flat pyarrow schema used by writer/reader. -- `MotionData` is the in-memory dataclass carried across the pipeline; its - attribute names are a stable contract with consumers (notably - `tasks/v2p_whole_body/mdp/commands/tracking_command.py`). - -Design notes: - -- All per-side data (hands, contacts) is stored on disk as `hand_sides`-indexed - lists. The in-memory dataclass exposes flattened `left_*` / `right_*` - attributes for consumer convenience. -- Quaternions are `wxyz` everywhere, both on disk and in memory. -- Poses that combine position and orientation are packed as `[x, y, z, qw, qx, - qy, qz]` (length 7) per frame. -""" - -from __future__ import annotations - -from dataclasses import dataclass, field, replace -from typing import Any - -import pyarrow as pa - -SCHEMA_VERSION: str = "motion_v1" -"""Current schema version. Bumped on breaking changes.""" - -# --------------------------------------------------------------------------- -# Field inventory -# --------------------------------------------------------------------------- - - -def _ts(dtype: pa.DataType) -> pa.DataType: - """Wrap a per-timestep value type as a time-series list.""" - return pa.list_(dtype) - - -_POSE7 = pa.list_(pa.float32(), 7) # [x, y, z, qw, qx, qy, qz] -_VEC3 = pa.list_(pa.float32(), 3) -_VEC4 = pa.list_(pa.float32(), 4) - - -METADATA_FIELDS: list[tuple[str, pa.DataType]] = [ - ("schema_version", pa.string()), - ("sequence_id", pa.string()), - ("robot_name", pa.string()), - ("motion_kind", pa.string()), - ("source_dataset", pa.string()), - ("raw_motion_file", pa.string()), - ("fps", pa.float32()), - ("coord_frame", pa.string()), -] - -ROBOT_FIELDS: list[tuple[str, pa.DataType]] = [ - ("robot_joint_names", pa.list_(pa.string())), - ("robot_root_position", _ts(_VEC3)), - ("robot_root_wxyz", _ts(_VEC4)), - ("robot_joint_positions", _ts(pa.list_(pa.float32()))), -] - -EE_FIELDS: list[tuple[str, pa.DataType]] = [ - ("ee_link_names", pa.list_(pa.string())), - ("ee_pose_w", _ts(pa.list_(_POSE7))), # (T, E, 7) -] - -HAND_FIELDS: list[tuple[str, pa.DataType]] = [ - ("hand_sides", pa.list_(pa.string())), - ("hand_frame_names", pa.list_(pa.list_(pa.string()))), - # Per-side (T, K, 7) stored as list of list of list of float. pyarrow can't - # express "ragged inner list with fixed inner pose length," so the inner - # `pose7` dimension uses a fixed-size list. - ("hand_frames_w", pa.list_(_ts(pa.list_(_POSE7)))), - ("hand_finger_joint_names", pa.list_(pa.list_(pa.string()))), - ("hand_finger_joints", pa.list_(_ts(pa.list_(pa.float32())))), -] - -OBJECT_FIELDS: list[tuple[str, pa.DataType]] = [ - ("object_name", pa.string()), - ("safe_object_name", pa.string()), - ("object_body_names", pa.list_(pa.string())), - ("safe_object_body_names", pa.list_(pa.string())), - ("object_mesh_paths", pa.list_(pa.string())), - ("object_urdf_paths", pa.list_(pa.string())), - ("object_mesh_radius", pa.list_(pa.float32())), - ("object_articulation", _ts(pa.float32())), - ("object_root_axis_angle", _ts(_VEC3)), - ("object_root_position", _ts(_VEC3)), - # (T, B, 3) — ragged B, inner vec3 fixed-size - ("object_body_position", _ts(pa.list_(_VEC3))), - ("object_body_wxyz", _ts(pa.list_(_VEC4))), -] - -CONTACT_FIELDS: list[tuple[str, pa.DataType]] = [ - # All per-side, aligned with `hand_sides` index. - ("hand_contact_link_names", pa.list_(pa.list_(pa.string()))), - # Per-side (T, N, 3) with ragged N. - ("hand_link_contact_positions", pa.list_(_ts(pa.list_(_VEC3)))), - ("hand_link_contact_normals", pa.list_(_ts(pa.list_(_VEC3)))), - ("hand_object_contact_positions", pa.list_(_ts(pa.list_(_VEC3)))), - ("hand_object_contact_normals", pa.list_(_ts(pa.list_(_VEC3)))), - ("hand_object_contact_part_ids", pa.list_(_ts(pa.list_(pa.int32())))), - ("hand_contact_active", pa.list_(_ts(pa.float32()))), -] - -SOURCE_FIELDS: list[tuple[str, pa.DataType]] = [ - ("source_kind", pa.string()), - ("source_payload", pa.binary()), # pickled dict; opaque to training - ("source_joint_names", pa.list_(pa.string())), -] - -DIAGNOSTICS_FIELDS: list[tuple[str, pa.DataType]] = [ - ("ik_error_per_frame", _ts(pa.float32())), - ("ik_num_iterations", _ts(pa.int32())), - ("frame_task_errors", _ts(pa.list_(pa.float32()))), -] - - -ALL_FIELDS: list[tuple[str, pa.DataType]] = ( - METADATA_FIELDS - + ROBOT_FIELDS - + EE_FIELDS - + HAND_FIELDS - + OBJECT_FIELDS - + CONTACT_FIELDS - + SOURCE_FIELDS - + DIAGNOSTICS_FIELDS -) - - -# Recognized values for the `motion_kind` discriminator. The schema branches -# its required-field check on this value: `single_robot` files carry -# whole-body joint state, while `dual_hand` files carry only floating-wrist -# end-effector state plus per-side hand frames. -SINGLE_ROBOT: str = "single_robot" -DUAL_HAND: str = "dual_hand" -KNOWN_MOTION_KINDS: frozenset[str] = frozenset({SINGLE_ROBOT, DUAL_HAND}) - - -# Fields required for any training-eligible file regardless of motion kind. -# `motion_kind` is included so loaders fail fast on legacy files that predate -# the discriminator instead of silently inferring a kind. -COMMON_REQUIRED_FIELDS: tuple[str, ...] = ( - "schema_version", - "sequence_id", - "robot_name", - "motion_kind", - "fps", - "ee_link_names", - "ee_pose_w", - "object_body_names", - "object_body_position", - "object_body_wxyz", -) - - -# Required for `motion_kind == "single_robot"`: whole-body joint trajectories. -SINGLE_ROBOT_REQUIRED_FIELDS: tuple[str, ...] = ( - "robot_joint_names", - "robot_root_position", - "robot_root_wxyz", - "robot_joint_positions", -) - - -# Required for `motion_kind == "dual_hand"`: floating-wrist trajectories -# carry per-side hand frames and finger joints aligned by `hand_sides`. -DUAL_HAND_REQUIRED_FIELDS: tuple[str, ...] = ( - "hand_sides", - "hand_frame_names", - "hand_frames_w", - "hand_finger_joint_names", - "hand_finger_joints", -) - - -# Per-side fields that must have an outer length equal to `len(hand_sides)`. -# This catches producers that set `hand_sides` but forgot to populate one -# side's payload, which would otherwise silently produce ragged data. -DUAL_HAND_PER_SIDE_FIELDS: tuple[str, ...] = ( - "hand_frame_names", - "hand_frames_w", - "hand_finger_joint_names", - "hand_finger_joints", -) - - -# Backwards-compatible alias. New code should consult the per-kind tuples -# above. This union is the conservative super-set used only by callers that -# do not yet know the file's `motion_kind`. -REQUIRED_TRAINING_FIELDS: tuple[str, ...] = tuple( - dict.fromkeys( - COMMON_REQUIRED_FIELDS - + SINGLE_ROBOT_REQUIRED_FIELDS - + DUAL_HAND_REQUIRED_FIELDS - ) -) - - -def resolve_motion_kind(source: Any) -> str: - """Return the explicit `motion_kind` from a `MotionData` or pyarrow row dict. - - Raises: - ValueError: If `motion_kind` is missing, empty, or not one of the - values in `KNOWN_MOTION_KINDS`. Inference from `robot_joint_names` - or `hand_sides` is intentionally not performed; producers must - tag every file explicitly. - """ - if isinstance(source, dict): - raw = source.get("motion_kind") - if isinstance(raw, list): - raw = raw[0] if raw else None - else: - raw = getattr(source, "motion_kind", None) - if raw is None or raw == "": - raise ValueError( - "Motion file is missing `motion_kind`. Producers must set " - f"motion_kind to one of {sorted(KNOWN_MOTION_KINDS)}; legacy " - "files predating this field are not supported and must be " - "regenerated." - ) - if raw not in KNOWN_MOTION_KINDS: - raise ValueError( - f"Unknown motion_kind={raw!r}. Expected one of {sorted(KNOWN_MOTION_KINDS)}." - ) - return raw - - -def required_fields_for(motion_kind: str) -> tuple[str, ...]: - """Return the required field tuple for a given resolved `motion_kind`.""" - if motion_kind == SINGLE_ROBOT: - return COMMON_REQUIRED_FIELDS + SINGLE_ROBOT_REQUIRED_FIELDS - if motion_kind == DUAL_HAND: - return COMMON_REQUIRED_FIELDS + DUAL_HAND_REQUIRED_FIELDS - raise ValueError( - f"Unknown motion_kind={motion_kind!r}. Expected one of {sorted(KNOWN_MOTION_KINDS)}." - ) - - -# --------------------------------------------------------------------------- -# Schema construction -# --------------------------------------------------------------------------- - - -def build_schema() -> pa.Schema: - """Return the unified `motion_v1` pyarrow schema.""" - return pa.schema([(name, dtype) for name, dtype in ALL_FIELDS]) - - -# --------------------------------------------------------------------------- -# In-memory dataclass -# --------------------------------------------------------------------------- - - -@dataclass -class MotionData: - """In-memory container for motion data. - - Field naming preserves the contract consumed by - `tasks/v2p_whole_body/mdp/commands/tracking_command.py`: `left_*` / - `right_*` attributes for hand and contact data, even though on disk these - live under `hand_sides`-indexed columns. The reader is responsible for - populating both the on-disk view (useful for debugging / re-serialization) - and the flattened view (consumed by training). - - Tensors are `None` when the corresponding group was not written by the - producer. Training code already guards each optional field. - """ - - # ---- Metadata ---------------------------------------------------------- - schema_version: str = SCHEMA_VERSION - sequence_id: str = "" - robot_name: str = "" - # Discriminator between layout shapes. Required to be non-empty before - # writing or after loading; see `KNOWN_MOTION_KINDS` for accepted values. - motion_kind: str = "" - source_dataset: str = "" - raw_motion_file: str = "" - fps: float = 0.0 - coord_frame: str = "" - - # ---- Robot state (required for training) ------------------------------- - robot_joint_names: list[str] = field(default_factory=list) - robot_root_position: Any = None # torch.Tensor (T, 3), world frame - robot_root_wxyz: Any = None # torch.Tensor (T, 4), wxyz - robot_joint_positions: Any = None # torch.Tensor (T, J) - - # ---- End-effector frames ---------------------------------------------- - ee_link_names: list[str] | None = None - ee_pose_w: Any = None # torch.Tensor (T, E, 7); on-disk form - # Flattened for training: ee_pos_w + ee_quat_w split from ee_pose_w. - ee_pos_w: Any = None # torch.Tensor (T, E, 3) - ee_quat_w: Any = None # torch.Tensor (T, E, 4) - ee_link_ids: list[int] | None = None - - # ---- Object ------------------------------------------------------------ - object_name: str = "" - safe_object_name: str = "" - object_body_names: list[str] = field(default_factory=list) - safe_object_body_names: list[str] = field(default_factory=list) - object_mesh_paths: list[str] = field(default_factory=list) - object_urdf_paths: list[str] = field(default_factory=list) - object_mesh_radius: list[float] | None = None - object_articulation: Any = None - object_root_axis_angle: Any = None - object_root_position: Any = None - object_body_position: Any = None # torch.Tensor (T, B, 3) - object_body_wxyz: Any = None # torch.Tensor (T, B, 4) - - # Convenience aliases used by training: primary object body (index 0). - object_pos_w: Any = None # torch.Tensor (T, 3) - object_quat_w: Any = None # torch.Tensor (T, 4) - - # ---- Hands (on-disk view) --------------------------------------------- - hand_sides: list[str] = field(default_factory=list) - hand_frame_names: list[list[str]] = field(default_factory=list) - hand_frames_w: list[Any] = field(default_factory=list) - hand_finger_joint_names: list[list[str]] = field(default_factory=list) - hand_finger_joints: list[Any] = field(default_factory=list) - - # ---- Hands (flattened view for training) ------------------------------- - left_wrist_position: Any = None - left_wrist_wxyz: Any = None - right_wrist_position: Any = None - right_wrist_wxyz: Any = None - left_finger_joints: Any = None - right_finger_joints: Any = None - left_hand_frames: Any = None - right_hand_frames: Any = None - left_hand_frame_names: list[str] | None = None - right_hand_frame_names: list[str] | None = None - left_finger_joint_names: list[str] | None = None - right_finger_joint_names: list[str] | None = None - - # ---- Contacts (on-disk view) ------------------------------------------ - hand_contact_link_names: list[list[str]] = field(default_factory=list) - hand_link_contact_positions: list[Any] = field(default_factory=list) - hand_link_contact_normals: list[Any] = field(default_factory=list) - hand_object_contact_positions: list[Any] = field(default_factory=list) - hand_object_contact_normals: list[Any] = field(default_factory=list) - hand_object_contact_part_ids: list[Any] = field(default_factory=list) - hand_contact_active: list[Any] = field(default_factory=list) - - # ---- Contacts (flattened view) ---------------------------------------- - left_link_contact_positions: Any = None - left_object_contact_positions: Any = None - right_link_contact_positions: Any = None - right_object_contact_positions: Any = None - left_link_contact_normals: Any = None - left_object_contact_normals: Any = None - right_link_contact_normals: Any = None - right_object_contact_normals: Any = None - left_object_contact_part_ids: Any = None - right_object_contact_part_ids: Any = None - left_hand_contact_active: Any = None - right_hand_contact_active: Any = None - - # ---- Source raw motion ------------------------------------------------- - source_kind: str = "" - source_payload: bytes = b"" - source_joint_names: list[str] = field(default_factory=list) - - # ---- Diagnostics ------------------------------------------------------- - ik_error_per_frame: Any = None - ik_num_iterations: Any = None - frame_task_errors: Any = None - - # ---- Consumer contract ------------------------------------------------ - # Kept so the training loader preserves the legacy `file_joint_names` - # semantic (alias of `robot_joint_names` for joint reordering). - file_joint_names: list[str] | None = None - - # ------------------------------------------------------------------ - # Time-axis subsetting - # ------------------------------------------------------------------ - - # Field names whose first axis is the time axis T. Used by `trim()` so - # the schema stays the single source of truth for time-axis layout. - # Not annotated → class attribute only, not a dataclass field. - _TIME_AXIS_TENSOR_FIELDS = ( - "robot_root_position", - "robot_root_wxyz", - "robot_joint_positions", - "ee_pose_w", - "ee_pos_w", - "ee_quat_w", - "object_articulation", - "object_root_axis_angle", - "object_root_position", - "object_body_position", - "object_body_wxyz", - "object_pos_w", - "object_quat_w", - "left_wrist_position", - "left_wrist_wxyz", - "right_wrist_position", - "right_wrist_wxyz", - "left_finger_joints", - "right_finger_joints", - "left_hand_frames", - "right_hand_frames", - "left_link_contact_positions", - "left_object_contact_positions", - "right_link_contact_positions", - "right_object_contact_positions", - "left_link_contact_normals", - "left_object_contact_normals", - "right_link_contact_normals", - "right_object_contact_normals", - "left_object_contact_part_ids", - "right_object_contact_part_ids", - "left_hand_contact_active", - "right_hand_contact_active", - "ik_error_per_frame", - "ik_num_iterations", - "frame_task_errors", - ) - - # List-of-tensor fields where each element is `(T, ...)` and should be - # sliced per-element. - _TIME_AXIS_TENSOR_LIST_FIELDS = ( - "hand_frames_w", - "hand_finger_joints", - "hand_link_contact_positions", - "hand_link_contact_normals", - "hand_object_contact_positions", - "hand_object_contact_normals", - "hand_object_contact_part_ids", - "hand_contact_active", - ) - - def num_frames(self) -> int: - """Return motion length T inferred from a required time-axis field.""" - ref = self.robot_root_position - if ref is None: - return 0 - return int(ref.shape[0]) - - def trim(self, start_frame: int = 0, end_frame: int | None = None) -> "MotionData": - """Return a copy of this MotionData restricted to ``[start, end)`` frames. - - Slices every time-axis tensor (and per-element of every time-axis tensor - list) along axis 0. Non-tensor metadata is shared with the source. - - Args: - start_frame: First frame to keep (inclusive). Must be ``>= 0``. - end_frame: One past the last frame to keep. ``None`` means the - end of the sequence. - - Returns: - A new ``MotionData`` whose time-axis fields contain frames - ``[start_frame, end_frame)`` of the original. - - Raises: - ValueError: If the range is empty or out of bounds. - """ - total = self.num_frames() - if total == 0: - # Nothing to trim; return self to avoid masking a missing-field bug - # behind a silent no-op clone. - if start_frame == 0 and end_frame in (None, 0): - return self - raise ValueError( - f"Cannot trim MotionData with no time-axis tensors: requested " - f"[{start_frame}, {end_frame})." - ) - - end = total if end_frame is None else end_frame - if start_frame < 0 or end > total or start_frame >= end: - raise ValueError( - f"Invalid trim range [{start_frame}, {end}) for motion of length " - f"{total}. Require 0 <= start_frame < end_frame <= num_frames." - ) - - if start_frame == 0 and end == total: - return self - - updates: dict[str, Any] = {} - for name in self._TIME_AXIS_TENSOR_FIELDS: - value = getattr(self, name) - if value is None: - continue - updates[name] = value[start_frame:end] - for name in self._TIME_AXIS_TENSOR_LIST_FIELDS: - value = getattr(self, name) - if not value: - continue - updates[name] = [None if v is None else v[start_frame:end] for v in value] - return replace(self, **updates) - - -# --------------------------------------------------------------------------- -# Errors -# --------------------------------------------------------------------------- - - -class SchemaVersionMismatch(RuntimeError): # noqa: N818 - """Raised when a parquet file's `schema_version` does not match this module. - - Kept without the "Error" suffix to preserve the public API name; the - corresponding N818 ruff warning is intentionally suppressed. - """ - - def __init__(self, got: str, expected: str, path: str) -> None: - """Build a readable error pointing producers at the right action.""" - super().__init__( - f"Motion parquet at {path} has schema_version={got!r} but this " - f"codebase expects {expected!r}. Re-run the producing retarget " - f"or planner script on the latest source tree to regenerate this " - f"file; no migrator is shipped." - ) - self.got = got - self.expected = expected - self.path = path - - -class MissingRequiredField(RuntimeError): # noqa: N818 - """Raised when a training-eligible parquet lacks a required column. - - Kept without the "Error" suffix to preserve the public API name; the - corresponding N818 ruff warning is intentionally suppressed. - """ - - def __init__(self, missing: list[str], path: str) -> None: - """Build a readable error listing missing fields.""" - super().__init__( - f"Motion parquet at {path} is missing required fields: {missing}. " - f"This file claims schema_version={SCHEMA_VERSION!r} but is not " - f"training-eligible. Check the producer." - ) - self.missing = missing - self.path = path diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/writer.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/writer.py deleted file mode 100644 index 6393d056..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/motion_schema/writer.py +++ /dev/null @@ -1,285 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Writer for `motion_v1` parquets. - -Producers populate a `MotionData` dataclass and call `save_motion_parquet(...)`. -The writer serializes a single row per parquet file and writes it under a -Hive partition (`sequence_id=/robot_name=/`). -""" - -from __future__ import annotations - -import shutil -from collections.abc import Sized -from pathlib import Path -from typing import Any, cast - -import numpy as np -import pyarrow as pa -import pyarrow.parquet as pq - -from .schema import ( - ALL_FIELDS, - DUAL_HAND, - DUAL_HAND_PER_SIDE_FIELDS, - SCHEMA_VERSION, - MotionData, - build_schema, - required_fields_for, - resolve_motion_kind, -) - -# Optional dependency: torch is only needed when producers pass tensors to the -# coerce path. Import at module level so ruff's PLC0415 stays happy, but keep -# the module importable in thin environments that don't ship torch. -try: - import torch - - _TORCH_AVAILABLE = True -except ImportError: - _TORCH_AVAILABLE = False - -# --------------------------------------------------------------------------- -# Validation -# --------------------------------------------------------------------------- - - -def _validate_wxyz(tag: str, wxyz: Any) -> None: - """Cheap sanity check that a quaternion series is plausibly `wxyz`. - - Guards against producers accidentally packing `xyzw`. Two heuristics: - - - `|w|` must exceed 0.3 at least once (identity or near-identity poses - are near-universal in any long trajectory; if the first component is - never the scalar, the convention is likely swapped). - - `|w|` must not exceed 1.01 (quaternions must be unit; a value > 1.01 - indicates raw axis-angle or some other mis-stashed representation). - """ - if wxyz is None: - return - arr = np.asarray(wxyz, dtype=np.float32) - if arr.size == 0: - return - if arr.ndim < 2 or arr.shape[-1] != 4: - return - flat = arr.reshape(-1, 4) - w = np.abs(flat[:, 0]) - last = np.abs(flat[:, 3]) - w_max = float(w.max()) - last_max = float(last.max()) - if w_max < 0.3 and last_max > 0.9: - # Classic xyzw layout: w stays small, last component hugs 1. - raise ValueError( - f"[{tag}] quaternion series looks like xyzw, not wxyz " - f"(max |first|={w_max:.3f}, max |last|={last_max:.3f}). " - f"Producer must swap conventions before writing." - ) - if w_max > 1.01: - raise ValueError( - f"[{tag}] quaternion w component exceeds 1.01 (max={w_max:.3f}); not unit quaternions." - ) - - -def _coerce(value: Any) -> Any: - """Coerce torch tensors / numpy arrays to nested python lists for pyarrow.""" - if value is None: - return None - if _TORCH_AVAILABLE and isinstance(value, torch.Tensor): - return value.detach().cpu().numpy().tolist() - if isinstance(value, np.ndarray): - return value.tolist() - if isinstance(value, list | tuple): - return [_coerce(v) for v in value] - if isinstance(value, bytes | str | int | float | bool): - return value - return value - - -def _row_dict(md: MotionData) -> dict[str, Any]: - """Build a single-row pyarrow-friendly dict from a `MotionData`. - - Each field is wrapped in a one-element list because each parquet file - stores a single trajectory as a single row; wrapping matches the legacy - `ManoSharpaData` convention and plays nicely with partitioned writes. - """ - md.schema_version = SCHEMA_VERSION - - per_side_count = len(md.hand_sides or []) - - def _per_side(seq: list[Any] | None) -> list[Any]: - if not seq: - return [[] for _ in range(per_side_count)] - return [_coerce(v) for v in seq] - - row = { - # Metadata - "schema_version": [md.schema_version], - "sequence_id": [md.sequence_id], - "robot_name": [md.robot_name], - "motion_kind": [md.motion_kind], - "source_dataset": [md.source_dataset], - "raw_motion_file": [md.raw_motion_file], - "fps": [float(md.fps) if md.fps is not None else 0.0], - "coord_frame": [md.coord_frame], - # Robot - "robot_joint_names": [list(md.robot_joint_names or [])], - "robot_root_position": [_coerce(md.robot_root_position)], - "robot_root_wxyz": [_coerce(md.robot_root_wxyz)], - "robot_joint_positions": [_coerce(md.robot_joint_positions)], - # EE - "ee_link_names": [list(md.ee_link_names or [])], - "ee_pose_w": [_coerce(md.ee_pose_w)], - # Hands - "hand_sides": [list(md.hand_sides or [])], - "hand_frame_names": [list(md.hand_frame_names or [])], - "hand_frames_w": [_per_side(md.hand_frames_w)], - "hand_finger_joint_names": [list(md.hand_finger_joint_names or [])], - "hand_finger_joints": [_per_side(md.hand_finger_joints)], - # Object - "object_name": [md.object_name], - "safe_object_name": [md.safe_object_name], - "object_body_names": [list(md.object_body_names or [])], - "safe_object_body_names": [list(md.safe_object_body_names or [])], - "object_mesh_paths": [list(md.object_mesh_paths or [])], - "object_urdf_paths": [list(md.object_urdf_paths or [])], - "object_mesh_radius": [list(md.object_mesh_radius or [])], - "object_articulation": [_coerce(md.object_articulation)], - "object_root_axis_angle": [_coerce(md.object_root_axis_angle)], - "object_root_position": [_coerce(md.object_root_position)], - "object_body_position": [_coerce(md.object_body_position)], - "object_body_wxyz": [_coerce(md.object_body_wxyz)], - # Contacts - "hand_contact_link_names": [list(md.hand_contact_link_names or [])], - "hand_link_contact_positions": [_per_side(md.hand_link_contact_positions)], - "hand_link_contact_normals": [_per_side(md.hand_link_contact_normals)], - "hand_object_contact_positions": [_per_side(md.hand_object_contact_positions)], - "hand_object_contact_normals": [_per_side(md.hand_object_contact_normals)], - "hand_object_contact_part_ids": [_per_side(md.hand_object_contact_part_ids)], - "hand_contact_active": [_per_side(md.hand_contact_active)], - # Source raw - "source_kind": [md.source_kind], - "source_payload": [md.source_payload or b""], - "source_joint_names": [list(md.source_joint_names or [])], - # Diagnostics - "ik_error_per_frame": [_coerce(md.ik_error_per_frame)], - "ik_num_iterations": [_coerce(md.ik_num_iterations)], - "frame_task_errors": [_coerce(md.frame_task_errors)], - } - - # Pyarrow rejects column counts mismatching the schema; keep them aligned. - assert set(row.keys()) == { - name for name, _ in ALL_FIELDS - }, f"writer row dict out of sync with schema; diff={set(row.keys()) ^ {name for name, _ in ALL_FIELDS}}" - return row - - -def _validate_required(md: MotionData) -> None: - """Fail fast if required training fields are empty or missing. - - Validation is branched on `md.motion_kind`: whole-body files require - `robot_*` joint state, while dual-hand files require per-side hand frames - and finger joints. The discriminator must be set explicitly; producers - that omit `motion_kind` raise here. - """ - motion_kind = resolve_motion_kind(md) - required = required_fields_for(motion_kind) - missing: list[str] = [] - for name in required: - val = getattr(md, name, None) - if name == "fps": - if val is None or float(val) <= 0.0: - missing.append(name) - elif val is None or ( - hasattr(val, "__len__") and len(val) == 0 and name != "schema_version" - ): - missing.append(name) - if missing: - raise ValueError( - f"Cannot write motion parquet (motion_kind={motion_kind!r}): " - f"required fields are empty: {missing}. Producer must populate at " - f"least: {list(required)}." - ) - - if motion_kind == DUAL_HAND: - n_sides = len(md.hand_sides or []) - misaligned: list[tuple[str, int]] = [] - for name in DUAL_HAND_PER_SIDE_FIELDS: - val = getattr(md, name, None) - actual = len(cast(Sized, val)) if hasattr(val, "__len__") else 0 - if actual != n_sides: - misaligned.append((name, actual)) - if misaligned: - details = ", ".join( - f"{name} has {actual} entries" for name, actual in misaligned - ) - raise ValueError( - f"Cannot write motion parquet (motion_kind=dual_hand): per-side " - f"fields must align with hand_sides (len={n_sides}); {details}." - ) - - -# --------------------------------------------------------------------------- -# Public writer -# --------------------------------------------------------------------------- - - -DEFAULT_PARTITION_COLS: list[str] = ["sequence_id", "robot_name"] - - -def save_motion_parquet( - md: MotionData, - root_path: str | Path, - partition_cols: list[str] | None = None, - validate: bool = True, - file_name: str | None = None, -) -> Path: - """Write a `MotionData` row to a Hive-partitioned parquet dataset. - - Args: - md: Populated `MotionData`. - root_path: Dataset root (e.g. `.../whole_body/soma`). - partition_cols: Hive partition keys. Defaults to `sequence_id`, `robot_name`. - validate: If True, run required-fields and wxyz sanity checks before writing. - file_name: Optional stable basename for the single parquet file (e.g. - `"data.parquet"`). When ``None`` (default), pyarrow's auto-generated - UUID-prefixed name is kept. Since ``pq.write_to_dataset`` doesn't - expose a basename, we rename after the fact; safe because the writer - clears the partition dir above so exactly one file is produced. - - Returns: - The partition directory that was written. - """ - if validate: - _validate_required(md) - _validate_wxyz("robot_root_wxyz", md.robot_root_wxyz) - - partition_cols = partition_cols or DEFAULT_PARTITION_COLS - schema = build_schema() - table = pa.Table.from_pydict(_row_dict(md), schema=schema) - - root = Path(root_path) - partition_dir = root - row = table.to_pydict() - for col in partition_cols: - val = row[col][0] - partition_dir = partition_dir / f"{col}={val}" - if partition_dir.is_dir(): - shutil.rmtree(partition_dir) - - pq.write_to_dataset( - table, - root_path=str(root), - partition_cols=partition_cols, - compression="zstd", - ) - - if file_name is not None: - written = list(partition_dir.glob("*.parquet")) - if len(written) != 1: - raise RuntimeError( - f"Expected exactly one parquet under {partition_dir} after " - f"write_to_dataset, found {len(written)}: {written}" - ) - written[0].rename(partition_dir / file_name) - - return partition_dir diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/LICENSE deleted file mode 100644 index 20cd33bf..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/LICENSE +++ /dev/null @@ -1,186 +0,0 @@ -================================================================================ -DUAL LICENSE NOTICE -================================================================================ - -This repository is dual-licensed. Different components are under different terms: - -1. SOURCE CODE - Apache License 2.0 - All source code, scripts, and software components - -2. MODEL WEIGHTS - NVIDIA Open Model License - All trained model checkpoints and weights - -See below for the full text of each license. - -================================================================================ -PART 1: SOURCE CODE LICENSE (Apache License 2.0) -================================================================================ - -Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. - -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. - - -================================================================================ -PART 2: MODEL WEIGHTS LICENSE (NVIDIA Open Model License) -================================================================================ - -NVIDIA OPEN MODEL LICENSE AGREEMENT - -Last Modified: October 24, 2025 - -NVIDIA Corporation and its affiliates ("NVIDIA") grants permission to use machine -learning models under specific conditions. Key permissions include creating -derivative models and distributing them, with NVIDIA retaining no ownership claims -over outputs generated by users. - -SECTION 1: DEFINITIONS - -1.1 "Derivative Model" means any modification of, or works based on or derived -from, the Model, excluding outputs. - -1.2 "Legal Entity" means the union of the acting entity and all other entities -that control, are controlled by, or are under common control with that entity. - -1.3 "Model" means the machine learning model, software, and any checkpoints, -weights, algorithms, parameters, configuration files, and documentation that NVIDIA -makes available under this Agreement. - -1.4 "NVIDIA Cosmos Model" means a multimodal Model that is covered by this Agreement. - -1.5 "Special-Purpose Model" means a Model that is limited to narrow, -purpose-specific tasks. - -1.6 "You" or "Your" means an individual or Legal Entity exercising permissions -granted by this Agreement. - -SECTION 2: CONDITIONS FOR USE, LICENSE GRANT, AI ETHICS AND IP OWNERSHIP - -2.1 Conditions for Use. You must comply with all terms and conditions of this -Agreement. If You initiate copyright or patent litigation against any entity -(including a cross-claim or counterclaim in a lawsuit) alleging that the Model -constitutes direct or contributory infringement, then Your licenses under this -Agreement shall terminate. If You circumvent any safety guardrails or safety -measures built in to the Model without providing comparable alternatives, Your -rights under this Agreement shall terminate. NVIDIA may update this Agreement at -any time to comply with applicable law; Your continued use constitutes Your -acceptance of the updated terms. - -2.2 License Grant. Subject to the terms and conditions of this Agreement, NVIDIA -hereby grants You a perpetual, worldwide, non-exclusive, no-charge, royalty-free, -revocable license to publicly perform, publicly display, reproduce, use, create -derivative works of, make, have made, sell, offer for sale, distribute and import -the Model. - -2.3 AI Ethics. Your use of the Model must be in accordance with NVIDIA's -Trustworthy AI terms, which can be found at -https://www.nvidia.com/en-us/agreements/trustworthy-ai/terms/. - -2.4 IP Ownership. NVIDIA owns the original Model and NVIDIA's Derivative Models. -You own Your Derivative Models. NVIDIA makes no claim of ownership to outputs. You -are responsible for outputs and their subsequent uses. - -SECTION 3: REDISTRIBUTION - -You may reproduce and distribute copies of the Model or Derivative Models thereof, -with or without modifications, provided that You meet the following conditions: - -a. You must include a copy of this Agreement. - -b. You must include the following attribution notice, which can appear in the same -location as other third-party notices or license information: "Licensed by NVIDIA -Corporation under the NVIDIA Open Model License." - -c. If You are distributing a NVIDIA Cosmos Model, You must also include the phrase -"Built on NVIDIA Cosmos" on the applicable website, in the user interface, in a -blog, in an "about" page, or in product documentation. - -d. You may add Your own copyright statement to Your modifications and may provide -additional or different license terms and conditions for use, reproduction, or -distribution of Your modifications or for any Derivative Models as a whole, -provided Your use, reproduction, and distribution otherwise complies with this -Agreement. - -SECTION 4: SEPARATE COMPONENTS - -The Model may contain components that are subject to separate legal notices or -governed by separate licenses (including Open Source Software Licenses), as may be -described in any files made available with the Model. Your use of those separate -components is subject to the applicable license. This Agreement shall control over -the separate licenses for third-party Open Source Software to the extent that the -separate license imposes additional restrictions. "Open Source Software License" -means any software license approved by the Open Source Initiative, Free Software -Foundation, or similar recognized organization, or a license identified by SPDX. - -SECTION 5: TRADEMARKS - -This Agreement does not grant permission to use the trade names, trademarks, -service marks, or product names of NVIDIA, except as required for reasonable and -customary use in describing the origin of the Model and reproducing the content of -the notice. - -SECTION 6: DISCLAIMER OF WARRANTY - -NVIDIA provides the Model on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF -ANY KIND, either express or implied, including, without limitation, any warranties -or conditions of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A -PARTICULAR PURPOSE. You are solely responsible for reviewing the documentation -accompanying the Model and determining the appropriateness of using the Model, and -You understand that Special-Purpose Models are limited to narrow, purpose-specific -tasks and must not be deployed for uses that are beyond such tasks. - -SECTION 7: LIMITATION OF LIABILITY - -In no event and under no legal theory, whether in tort (including negligence), -contract, or otherwise, unless required by applicable law (such as deliberate and -grossly negligent acts) or agreed to in writing, will NVIDIA be liable to You for -damages, including any direct, indirect, special, incidental, or consequential -damages of any character arising as a result of this Agreement or out of the use -or inability to use the Model or Derivative Models or outputs (including but not -limited to damages for loss of goodwill, work stoppage, computer failure or -malfunction, or any and all other commercial damages or losses), even if NVIDIA has -been advised of the possibility of such damages. - -SECTION 8: INDEMNITY - -You will defend, indemnify and hold harmless NVIDIA and its affiliates, and their -respective employees, contractors, directors, officers and agents, from and against -any and all claims, damages, obligations, losses, liabilities, costs or debt, and -expenses (including but not limited to attorney's fees) arising from Your use or -distribution of the Model or Derivative Models or outputs. - -SECTION 9: FEEDBACK - -NVIDIA may use feedback You provide without restriction and without any -compensation to You. - -SECTION 10: GOVERNING LAW - -This Agreement will be governed in all respects by the laws of the United States -and of the State of Delaware, without regard to conflict of laws provisions. The -federal and state courts residing in Santa Clara County, California shall have -exclusive jurisdiction over any dispute arising out of this Agreement, and You -hereby consent to the personal jurisdiction of such courts. However, NVIDIA shall -have the right to seek injunctive relief in any court of competent jurisdiction. - -SECTION 11: TRADE AND COMPLIANCE - -You shall comply with all applicable import, export, trade, and economic sanctions -laws, including without limitation the Export Administration Regulations and -economic sanctions laws implemented by the Office of Foreign Assets Control, that -restrict or govern the destination, end-user and end-use of NVIDIA products, -technology, software, and services. - ---- - -Version Release Date: October 24, 2025 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/NOTICE deleted file mode 100644 index 2489e679..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/NOTICE +++ /dev/null @@ -1,14 +0,0 @@ -This directory contains the G1 whole-body planner source and its bundled -MotionBricks planner model package. - -Source code in this directory is licensed under Apache-2.0. - -The bundled model package is licensed by NVIDIA Corporation under the NVIDIA -Open Model License: - -- motionbricks/assets/models/planner.pkg - -Planner MuJoCo assets include third-party Unitree G1 robot-description content; -see assets/mujoco/NOTICE and assets/mujoco/LICENSE. - -The full license text is in the LICENSE file alongside this notice. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/README.md deleted file mode 100644 index 3e38dc58..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/README.md +++ /dev/null @@ -1,146 +0,0 @@ -# Whole-Body Planner - -Generates planned whole-body motion from V2P retargeted hand/object trajectories. Takes EE targets as input, runs a learned motion model to produce full-body joint trajectories, and outputs a single Hive-partitioned parquet containing everything needed for RL training. - -## Quick Start - -```bash -cd /path/to/video_to_data -PYTHONPATH=robotic_grounding/source/robotic_grounding:$PYTHONPATH \ -python -m robotic_grounding.planner.g1_planner \ - --v2p_parquet robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic/arctic_processed \ - --v2p_sequence box_grab \ - --robot dex3 \ - --workspace_offset -0.30 0.0 0.07 \ - --output planner_processed -``` - -This opens a MuJoCo viewer showing the planned motion with EE axes, object mesh, and support surface. - -## Pipeline - -``` -V2P Retargeted Parquet (arctic_processed/) -│ -├── Step 1: Nominal FK -│ Compute G1 nominal wrist positions from standing pose -│ -├── Step 2: Load V2P Reference -│ Load hand/object trajectories, interpolate to target FPS -│ -├── Step 3-4: Transform to G1 Frame -│ Reference frame alignment → yaw correction → position offset -│ -├── Step 5: Build Trajectory -│ Hold nominal (5s) → interpolate (5s) → hold start (5s) → reference -│ -├── Step 6: Inference -│ MotionInferenceAgent: EE targets → chunked autoregressive → full-body qpos -│ -├── Step 7: Build Full Qpos -│ Combine: planner body (29 DOF) + reference fingers + static legs -│ -├── Step 8: Save Parquet -│ Hive-partitioned: planner_processed/sequence_id=.../robot_name=.../*.parquet -│ Post-write invariants (utils/validation.py) hard-fail on any contract -│ break before the parquet leaves planning. -│ -├── Step 8b: Reconstruct Support Surface -│ support_recon writes /reconstructed_stage/_support.usda. -│ Disks that sit on another body's trajectory (a tool resting on the -│ target) are filtered so they don't spawn intersecting the target body. -│ -└── Step 9: Viewer - MuJoCo playback with EE axes, object mesh, support surface -``` - -Pre-plan, `utils/validation.warn_reference_issues` runs over the loaded V2P -motion and prints warnings for reference-owned gaps (missing required fields, -unresolvable asset paths, missing URDF mesh dependencies, off-FPS source). -These are informational — the upstream retargeter / asset pipeline owns them. - -## CLI Arguments - -| Argument | Default | Description | -|----------|---------|-------------| -| `--v2p_parquet` | required | Path to V2P retargeted parquet folder | -| `--v2p_sequence` | `box_grab` | Sequence ID substring filter | -| `--v2p_robot_name` | `dex3` | Robot name filter | -| `--v2p_trajectory_id` | `0` | Trajectory index within filtered results | -| `--robot` | `dex3` | Robot type. Only `dex3` is supported. | -| `--workspace_offset` | `-0.10 0.0 -0.15` | XYZ offset for EE targets | -| `--target_fps` | `150.0` | Resample V2P data to this FPS | -| `--ref_seconds` | `-1` | Seconds of reference to include (-1 = all) | -| `--output` | `planner_processed/` | Output parquet directory | -| `--no_viewer` | false | Skip MuJoCo visualization | -| `--ik_verify` | false | Run IK reachability check | -| `--ik_plan` | false | Use IK solution instead of learned model | - -## Output Parquet Schema - -The planner writes the unified `motion_v1` schema (see -[../motion_schema/README.md](../motion_schema/README.md) and -[../motion_schema/motion_schema.md](../motion_schema/motion_schema.md)). Hive -layout: `/sequence_id=/robot_name=/data.parquet`. - -The planner populates: - -- Robot state (`robot_root_position`, `robot_root_wxyz`, `robot_joint_positions`, - `robot_joint_names`) decomposed from the planner's mujoco qpos. -- `ee_link_names` set to `["left_hand_palm_link", "right_hand_palm_link"]` - for dex3, where the palm is the free-flyer URDF root. `ee_pose_w (T, 2, 7)` - is built from the reference wrist trajectories. -- Object metadata + trajectory (`object_body_position`, `object_body_wxyz`, - `object_body_names`, `object_articulation`, mesh/URDF paths copied from the - upstream ManoSharpaData retarget file). -- `object_root_position` / `object_root_axis_angle` derived from body 0 of the - planner-frame object pose so the env's articulated scene init lands where the - trajectory starts. -- `robot_joint_names` / `robot_joint_positions` cover every actuated joint - (body + fingers) in MuJoCo joint order; the per-side `hand_finger_joints` / - `hand_finger_joint_names` lists stay populated for callers that want the - side-segregated view. -- Per-side hand frames + contact groups are transformed by the same per-frame - rigid transform applied to `ee_pose_w` / `object_body_position`, so every - field of the output parquet lives in a single coherent planner frame. - -Support surfaces are discovered by `SceneConfig.from_motion_file()` from the -sibling `reconstructed_stage/` directory; they are not embedded in the parquet -(previously stored as `support_position` / `support_size`). - -## Module Layout - -``` -planner/ -├── g1_planner.py CLI orchestration; sole consumer of utils/ -├── trajectory.py warmup builder: hold nominal → interp → hold start -│ → reference -├── visualization.py MuJoCo viewer with EE axes, object mesh, support -├── motionbricks/ MotionBricks model backend (current planner) -│ ├── inference.py loads planner_agent.pkg, runs chunked AR inference -│ └── qpos.py qpos assembly helpers -└── utils/ Pure helpers, no planner state - ├── transforms.py Quaternion conversions, low-level rigid - │ primitives (quat_*, transform_primary_*, - │ transform_contact_*_by_part), and the high-level - │ transform_reference pipeline (local frame fix → - │ heading → position offset → workspace shift) - ├── loader.py Resample V2P motion fields to target FPS - │ (linear for positions, SLERP for quats, masked - │ interp for contacts) - └── validation.py Pre-plan warn_reference_issues + post-plan - assert_motion_parquet_invariants. The asserts - catch every contract task 48 had to patch by - hand; running them at planning time means - regressions surface before training sees them. -``` - -Support-surface reconstruction (still-frame detection → mesh-projected -disks → USDA output, with a phantom-tool cross-body filter) lives in -`retarget/support_recon.py` — shared with the standalone CLI -`scripts/reconstruct_support_surfaces.py` and retargeting viz tools. - -The active model backend is `motionbricks/`. `MotionInferenceAgent` in -`motionbricks/inference.py` loads weights from a self-contained `torch.package` -archive bundled under `assets/models/`, so the planner runs without any -training-codebase dependency. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/__init__.py deleted file mode 100644 index 0e123aea..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# Self-contained whole-body planner module. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/LICENSE b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/LICENSE deleted file mode 100644 index e472bbd9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/LICENSE +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/NOTICE b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/NOTICE deleted file mode 100644 index 67266ea8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/NOTICE +++ /dev/null @@ -1,6 +0,0 @@ -This directory contains MuJoCo XML assets used by the G1 whole-body planner. - -The G1 and Dex3 robot-description assets are from or derived from Unitree G1 -Description assets developed by Unitree Robotics and are licensed under the BSD -3-Clause License. The full license text is in the LICENSE file alongside this -notice. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_29dof.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_29dof.xml deleted file mode 100644 index ea37cf37..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_29dof.xml +++ /dev/null @@ -1,521 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_dex3_hands.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_dex3_hands.xml deleted file mode 100644 index 3fa2e220..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/g1_dex3_hands.xml +++ /dev/null @@ -1,393 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/scene_29dof.xml b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/scene_29dof.xml deleted file mode 100644 index 838de41a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/assets/mujoco/scene_29dof.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/cli.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/cli.py deleted file mode 100644 index 4eff5a91..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/cli.py +++ /dev/null @@ -1,151 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""CLI surface for the G1 whole-body planner. - -`parse_args` builds the argparse namespace consumed by `g1_planner.main()`. -It lives in its own module so the orchestration script stays focused on -the pipeline rather than CLI plumbing. -""" - -from __future__ import annotations - -import argparse - -# CLI defaults for the planner's nominal-hold + interp + hold-start -# approach segment. Mirror the constants `g1_planner.main` uses at runtime; -# duplicated here as literals so this module has no dependency on -# `g1_planner.py` and stays free of circular-import gymnastics. -_HOLD_START_S = 5.0 -_INTERP_DURATION_S = 5.0 -_HOLD_END_S = 5.0 -_ROOT_FIX_COMPONENTS = ("x", "y", "z", "roll", "pitch", "yaw") - - -def parse_args() -> argparse.Namespace: - """Build the planner CLI parser and return the parsed namespace.""" - parser = argparse.ArgumentParser(description="G1 whole-body planner") - parser.add_argument("--robot", choices=["dex3"], default="dex3") - parser.add_argument("--v2p_parquet", required=True) - parser.add_argument("--v2p_robot_name", default="dex3") - parser.add_argument("--v2p_sequence", default="box_grab") - parser.add_argument("--v2p_trajectory_id", type=int, default=0) - parser.add_argument( - "--v2p_start_frame", - type=int, - default=0, - help=( - "Drop this many frames from the interpolated V2P reference before " - "building the planner warmup/interp trajectory. Useful for skipping " - "dataset-specific T-pose/approach lead-ins." - ), - ) - parser.add_argument( - "--v2p_start_at_first_contact", - action="store_true", - help=( - "Start the reference at the first detected hand-object contact " - "minus --v2p_pre_contact_frames." - ), - ) - parser.add_argument( - "--v2p_pre_contact_frames", - type=int, - default=10, - help="Number of interpolated V2P frames to keep before first contact.", - ) - parser.add_argument( - "--v2p_end_after_last_contact_frames", - type=int, - default=-1, - help=( - "If >= 0, truncate the interpolated V2P reference after the last " - "detected hand-object contact plus this many frames. A value of 0 " - "keeps through the last contact frame." - ), - ) - parser.add_argument("--target_fps", type=float, default=150.0) - parser.add_argument("--hold_start_s", type=float, default=_HOLD_START_S) - parser.add_argument("--interp_s", type=float, default=_INTERP_DURATION_S) - parser.add_argument("--hold_end_s", type=float, default=_HOLD_END_S) - parser.add_argument( - "--no_approach", - action="store_true", - help=( - "Disable the planner's nominal hold/interp/hold approach segment. " - "The generated trajectory starts directly at the V2P reference." - ), - ) - parser.add_argument( - "--workspace_offset", type=float, nargs=3, default=[-0.10, 0.0, -0.15] - ) - parser.add_argument("--ref_seconds", type=float, default=-1) - parser.add_argument("--output", default=None) - parser.add_argument("--no_viewer", action="store_true") - parser.add_argument("--ik_verify", action="store_true") - parser.add_argument("--ik_plan", action="store_true") - parser.add_argument( - "--fix_lower_body", - action="store_true", - help=( - "Override the model's lower-body (hip/knee/ankle) predictions " - "with a static crouch and run the AR-aware loop that pins those " - "bodies in the model's chunk seeds." - ), - ) - parser.add_argument( - "--fix_root", - nargs="+", - choices=_ROOT_FIX_COMPONENTS, - default=(), - help=( - "Pin selected root components. Components are x y z roll pitch yaw; " - "e.g. '--fix_root z roll pitch' clamps height and roll/pitch while " - "leaving root XY translation and yaw free." - ), - ) - parser.add_argument( - "--fix_root_pos", - action="store_true", - help="Legacy alias for '--fix_root x y z'.", - ) - parser.add_argument( - "--fix_root_z", - action="store_true", - help="Legacy alias for '--fix_root z'.", - ) - parser.add_argument( - "--fix_root_rot", - action="store_true", - help="Legacy alias for '--fix_root roll pitch yaw'.", - ) - parser.add_argument( - "--fix_root_rp", - action="store_true", - help="Legacy alias for '--fix_root roll pitch'.", - ) - parser.add_argument( - "--no_smooth_qpos", - action="store_true", - help="Disable post-inference qpos smoothing (global Hamming + boundary blend).", - ) - parser.add_argument( - "--search_heading_deg", - type=float, - default=0.0, - help=( - "If > 0, run inference at heading offsets [-N, -N/2, 0, +N/2, +N] " - "degrees around the heading-toward-object correction and pick the " - "candidate with the lowest mean wrist tracking error." - ), - ) - parser.add_argument( - "--heading_align_frame", - choices=("start", "first_contact"), - default="start", - help=( - "Frame used for the heading-toward-object correction. 'start' " - "keeps the legacy behavior; 'first_contact' uses the detected " - "first contact frame within the trimmed reference." - ), - ) - return parser.parse_args() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/g1_planner.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/g1_planner.py deleted file mode 100644 index c8a28f2a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/g1_planner.py +++ /dev/null @@ -1,787 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""G1 whole-body planner: EE targets → full-body motion via learned model. - -Generates planned whole-body motion from V2P retargeted hand/object trajectories. -Outputs a Hive-partitioned parquet with body qpos, EE targets, hand keypoints, -contacts, and scene metadata. - -Usage: - MUJOCO_GL=egl python -m robotic_grounding.planner.g1_planner \ - --v2p_parquet /path/to/arctic_processed \ - --v2p_sequence box_grab \ - --output /path/to/planner_processed -""" - -from __future__ import annotations - -import os -from pathlib import Path -from typing import Any - -import mujoco -import numpy as np -from scipy.interpolate import interp1d -from scipy.spatial.transform import Rotation, Slerp - -from robotic_grounding.assets import ASSET_DIR -from robotic_grounding.motion_schema import MotionData, save_motion_parquet -from robotic_grounding.planner.cli import parse_args -from robotic_grounding.planner.motionbricks.inference import MotionInferenceAgent -from robotic_grounding.planner.trajectory import build_interp_trajectory -from robotic_grounding.planner.utils.motion import ( - assemble_hand_contact_fields, - assemble_object_fields, - interpolate_robot_motion_data, -) -from robotic_grounding.planner.utils.qpos import ( - LEFT_WRIST, - RIGHT_WRIST, - ROOT_HEIGHT, - build_full_qpos, - wrist_ee_error_from_qpos, -) -from robotic_grounding.planner.utils.transforms import ( - apply_local_frame_fix, - transform_reference, -) -from robotic_grounding.planner.utils.validation import ( - assert_motion_parquet_invariants, - warn_missing_urdf_mesh_deps, - warn_reference_issues, -) -from robotic_grounding.planner.visualization import visualize -from robotic_grounding.retarget.data_logger import ManoDex3Data -from robotic_grounding.retarget.support_recon import ( - reconstruct_support_for_sequence, -) - -# -- Constants ------------------------------------------------------------------ - -FPS = 30 -HOLD_START_S = 5.0 -INTERP_DURATION_S = 5.0 -HOLD_END_S = 5.0 - -NOMINAL_JOINTS = { - 22: -0.5, # left_shoulder_pitch - 23: 0.2, # left_shoulder_roll - 25: 0.0, # left_elbow - 29: -0.5, # right_shoulder_pitch - 30: -0.2, # right_shoulder_roll - 32: 0.0, # right_elbow -} - -_ASSETS_DIR = Path(__file__).parent / "assets" - - -# -- Helpers ------------------------------------------------------------------- - - -def _resolve_asset_path_for_output(path: str | None) -> str | None: - """Re-root stored asset paths under this checkout when possible. - - Returns the original path if it can't be resolved locally; callers should - warn the user (a downstream consumer will fail on the missing file). - """ - if not path: - return path - marker = "assets/" - if marker in path: - suffix = path.rsplit(marker, maxsplit=1)[-1] - local = Path(ASSET_DIR) / suffix - if local.exists(): - return str(local) - if Path(path).exists(): - return path - print( - f" WARNING: asset path {path!r} could not be resolved against the " - "current workspace; downstream scene-spawn will fail on the missing file." - ) - return path - - -def get_nominal_ee(xml_path: str) -> dict: - """FK at nominal arm pose to get wrist positions/quats.""" - model = mujoco.MjModel.from_xml_path(xml_path) - data = mujoco.MjData(model) - data.qpos[2] = ROOT_HEIGHT - data.qpos[3] = 1.0 # wxyz identity quaternion w component - for idx, val in NOMINAL_JOINTS.items(): - data.qpos[idx] = val - mujoco.mj_forward(model, data) - left_id = model.body(LEFT_WRIST).id - right_id = model.body(RIGHT_WRIST).id - return { - "left_pos": data.xpos[left_id].copy().astype(np.float32), - "left_quat": data.xquat[left_id].copy().astype(np.float32), - "right_pos": data.xpos[right_id].copy().astype(np.float32), - "right_quat": data.xquat[right_id].copy().astype(np.float32), - } - - -def _trim_motion_data_range( - motion: Any, start_frame: int, end_frame: int | None = None -) -> Any: - """Keep a frame range from every frame-major motion field.""" - if start_frame <= 0 and end_frame is None: - return motion - n_frames = len(motion.robot_right_wrist_position) - start_frame = max(0, int(start_frame)) - end_frame = n_frames if end_frame is None else min(n_frames, int(end_frame)) - if start_frame >= end_frame: - raise ValueError( - f"V2P trim range [{start_frame}, {end_frame}) is empty for reference " - f"length {n_frames}" - ) - for attr, value in vars(motion).items(): - if isinstance(value, (str, bytes)) or value is None: - continue - try: - value_len = len(value) - except TypeError: - continue - if value_len != n_frames: - continue - if isinstance(value, tuple): - setattr(motion, attr, value[start_frame:end_frame]) - else: - setattr(motion, attr, value[start_frame:end_frame]) - return motion - - -def _hand_object_contact_frame_bounds( - motion: Any, threshold: float = 1e-5 -) -> tuple[int | None, int | None]: - """Return first/last frames with any nonzero hand-object contact point.""" - first: int | None = None - last: int | None = None - for side in ("left", "right"): - contacts = getattr(motion, f"mano_{side}_object_contact_positions", None) - if not contacts: - continue - arr = np.asarray(contacts, dtype=np.float32) - if arr.ndim < 3 or arr.shape[0] == 0: - continue - xyz = arr[..., :3] - active = np.abs(xyz).sum(axis=-1) > threshold - frame_ids = np.flatnonzero(active.any(axis=1)) - if frame_ids.size == 0: - continue - side_first = int(frame_ids[0]) - side_last = int(frame_ids[-1]) - first = side_first if first is None else min(first, side_first) - last = side_last if last is None else max(last, side_last) - return first, last - - -def _first_hand_object_contact_frame( - motion: Any, threshold: float = 1e-5 -) -> int | None: - """Return the first frame with any nonzero hand-object contact point.""" - first, _ = _hand_object_contact_frame_bounds(motion, threshold) - return first - - -def load_v2p_reference( - parquet_folder: str, - filters: list, - trajectory_id: int = 0, - target_fps: float = 100.0, - robot_type: str = "dex3", - start_frame: int = 0, - start_at_first_contact: bool = False, - pre_contact_frames: int = 0, - end_after_last_contact_frames: int = -1, -) -> dict[str, Any]: - """Load and interpolate V2P retargeted reference data.""" - if robot_type != "dex3": - raise ValueError(f"Unsupported robot_type={robot_type!r}; expected 'dex3'.") - data_class = ManoDex3Data - motion = data_class.from_parquet( - root_path=parquet_folder, filters=filters, trajectory_id=trajectory_id - ) - motion = interpolate_robot_motion_data(motion, target_fps) - n_frames = len(motion.robot_right_wrist_position) - effective_start_frame = int(start_frame) - effective_end_frame = None - first_contact_frame = None - last_contact_frame = None - if start_at_first_contact: - first_contact_frame, last_contact_frame = _hand_object_contact_frame_bounds( - motion - ) - if first_contact_frame is None: - print(" WARNING: no hand-object contact detected; using v2p_start_frame") - else: - contact_start = max(0, first_contact_frame - int(pre_contact_frames)) - effective_start_frame = max(effective_start_frame, contact_start) - elif int(end_after_last_contact_frames) >= 0: - first_contact_frame, last_contact_frame = _hand_object_contact_frame_bounds( - motion - ) - if int(end_after_last_contact_frames) >= 0: - if last_contact_frame is None: - print(" WARNING: no hand-object contact detected; not trimming V2P end") - else: - effective_end_frame = min( - n_frames, last_contact_frame + int(end_after_last_contact_frames) + 1 - ) - motion = _trim_motion_data_range(motion, effective_start_frame, effective_end_frame) - first_contact_frame_in_reference = None - if first_contact_frame is not None: - rel_contact = int(first_contact_frame) - int(effective_start_frame) - n_trimmed = len(motion.robot_right_wrist_position) - if 0 <= rel_contact < n_trimmed: - first_contact_frame_in_reference = rel_contact - - obj_pos_all = np.array(motion.object_body_position, dtype=np.float32) # (T, B, 3) - obj_quat_all = np.array(motion.object_body_wxyz, dtype=np.float32) # (T, B, 4) - # Primary body (body 0) for heading/transform computation - if obj_pos_all.ndim == 3: - obj_pos = obj_pos_all[:, 0] # (T, 3) - obj_quat = obj_quat_all[:, 0] - else: - obj_pos = obj_pos_all - obj_quat = obj_quat_all - - return { - "left_pos": np.array(motion.robot_left_wrist_position, dtype=np.float32), - "left_quat": np.array(motion.robot_left_wrist_wxyz, dtype=np.float32), - "right_pos": np.array(motion.robot_right_wrist_position, dtype=np.float32), - "right_quat": np.array(motion.robot_right_wrist_wxyz, dtype=np.float32), - "left_finger_joints": np.array( - motion.robot_left_finger_joints, dtype=np.float32 - ), - "right_finger_joints": np.array( - motion.robot_right_finger_joints, dtype=np.float32 - ), - "left_joint_names": list(motion.left_robot_finger_joint_names or []), - "right_joint_names": list(motion.right_robot_finger_joint_names or []), - "object_pos": obj_pos, - "object_quat": obj_quat, - "object_pos_all": obj_pos_all, - "object_quat_all": obj_quat_all, - "object_name": getattr(motion, "object_name", None), - "object_mesh_paths": getattr(motion, "object_mesh_paths", None), - "fps": target_fps, - "start_frame": effective_start_frame, - "end_frame": effective_end_frame, - "first_contact_frame": first_contact_frame, - "first_contact_frame_in_reference": first_contact_frame_in_reference, - "last_contact_frame": last_contact_frame, - "_motion_data": motion, - } - - -def save_planner_parquet( - output_dir: str, - full_qpos: np.ndarray, - ref_data: dict[str, Any], - model: mujoco.MjModel, - ref_raw: dict[str, Any], - robot_type: str, - sequence_id: str, -) -> int: - """Save planner output as a `motion_v1` Hive-partitioned parquet. - - Embeds: body qpos (from planner), EE/object/finger/contact data (from V2P). - """ - T = full_qpos.shape[0] - T_ref = ref_data["left_pos"].shape[0] - T_use = min(T, T_ref) - - # Decompose full_qpos into root + body + finger slices. Body joints are - # all non-hand actuated joints regardless of where they sit in the qpos - # layout — for dex3 the right arm joints come AFTER the left finger - # joints, so a single contiguous slice would drop them. - qpos_slice = full_qpos[:T_use] - robot_root_position = qpos_slice[:, 0:3].tolist() - robot_root_wxyz = qpos_slice[:, 3:7].tolist() - - # robot_joint_names / robot_joint_positions cover every actuated joint - # (body + fingers) in MuJoCo joint order. The env's tracking_command - # resolves cfg.joint_names against this list by name, so fingers must be - # present or it raises "Motion joint reference is missing tracked robot - # joints". The per-side `hand_finger_joints` / `hand_finger_joint_names` - # lists below stay populated for callers that want the side-segregated view. - body_qpos_idx: list[int] = [] - robot_joint_names: list[str] = [] - for i in range(model.njnt): - name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) - if not name or "free" in name.lower(): - continue - body_qpos_idx.append(int(model.jnt_qposadr[i])) - robot_joint_names.append(name) - robot_joint_positions = qpos_slice[:, body_qpos_idx].tolist() - - # EE pose built from per-side wrist position + wxyz reference. - left_pos = np.asarray(ref_data["left_pos"][:T_use], dtype=np.float32) - left_quat = np.asarray(ref_data["left_quat"][:T_use], dtype=np.float32) - right_pos = np.asarray(ref_data["right_pos"][:T_use], dtype=np.float32) - right_quat = np.asarray(ref_data["right_quat"][:T_use], dtype=np.float32) - ee_pose_w = np.stack( - [ - np.concatenate([left_pos, left_quat], axis=-1), - np.concatenate([right_pos, right_quat], axis=-1), - ], - axis=1, - ) # (T, 2, 7) - - # Object body position / wxyz — support single- or multi-body. - obj_body_pos = ref_data.get("object_pos_all", ref_data["object_pos"][:, None]) - obj_body_wxyz = ref_data.get("object_quat_all", ref_data["object_quat"][:, None]) - obj_body_pos_arr = np.asarray(obj_body_pos, dtype=np.float32)[:T_use] - obj_body_wxyz_arr = np.asarray(obj_body_wxyz, dtype=np.float32)[:T_use] - object_body_position = obj_body_pos_arr.tolist() - object_body_wxyz = obj_body_wxyz_arr.tolist() - - motion = ref_raw.get("_motion_data") - object_fields = assemble_object_fields( - motion=motion, - obj_body_pos_arr=obj_body_pos_arr, - obj_body_wxyz_arr=obj_body_wxyz_arr, - T_use=T_use, - fallback_object_name=str(ref_data.get("object_name", "box")), - resolve_asset_path=_resolve_asset_path_for_output, - warn_missing_deps=warn_missing_urdf_mesh_deps, - ) - - hand_fields = assemble_hand_contact_fields( - motion=motion, - obj_body_pos_arr=obj_body_pos_arr, - obj_body_wxyz_arr=obj_body_wxyz_arr, - T_use=T_use, - ) - - robot_name = "g1_dex3" - # ee_link_names tells the env which body the EE pose was recorded from. - # For dex3 the per-side wrist-position fields actually hold the palm-link - # pose (the free-flyer URDF root), so a `wrist_yaw_link` label would put - # the env's reward target on the wrong body with a ~4 cm systematic offset. - ee_link_names = ["left_hand_palm_link", "right_hand_palm_link"] - md = MotionData( - sequence_id=sequence_id, - robot_name=robot_name, - motion_kind="single_robot", - source_dataset="planner", - raw_motion_file="", - fps=float(FPS), - coord_frame="robot_base_z_up", - robot_joint_names=robot_joint_names, - robot_root_position=robot_root_position, - robot_root_wxyz=robot_root_wxyz, - robot_joint_positions=robot_joint_positions, - ee_link_names=ee_link_names, - ee_pose_w=ee_pose_w.tolist(), - object_body_position=object_body_position, - object_body_wxyz=object_body_wxyz, - **object_fields, - **hand_fields, - ) - # Layout: `/planner_processed/sequence_id=…/robot_name=…/*.parquet` - # with the support USD as a sibling at `/reconstructed_stage/`. - # SceneConfig._discover_support_surface walks - # `.parent.parent.parent / reconstructed_stage`, so the - # extra `planner_processed/` layer is what lets it find the support file. - partition_root = Path(output_dir) / "planner_processed" - partition_dir = save_motion_parquet(md, root_path=str(partition_root)) - print(f" Saved {partition_dir} ({T_use} frames)") - # Hard-fail before training ever sees the parquet so silent data - # corruption can't make it past planning. - assert_motion_parquet_invariants(partition_dir, robot_type=robot_type) - return T_use - - -def main() -> None: - """Run the full planner pipeline end-to-end from the CLI args.""" - args = parse_args() - hold_start_s = 0.0 if args.no_approach else args.hold_start_s - interp_s = 0.0 if args.no_approach else args.interp_s - hold_end_s = 0.0 if args.no_approach else args.hold_end_s - - scene_xml = str(_ASSETS_DIR / "mujoco" / "scene_29dof.xml") - hand_xml = str(_ASSETS_DIR / "mujoco" / "g1_dex3_hands.xml") - - # Step 1: Nominal EE FK - print(f"Step 1: Nominal FK (robot={args.robot})") - nom = get_nominal_ee(scene_xml) - print( - f" Left: {np.round(nom['left_pos'], 3)} Right: {np.round(nom['right_pos'], 3)}" - ) - - # Step 2: Load V2P reference - print("\nStep 2: Load V2P reference") - filters = [ - ("robot_name", "=", args.v2p_robot_name), - ("sequence_id", "contains", args.v2p_sequence), - ] - ref_raw = load_v2p_reference( - args.v2p_parquet, - filters, - trajectory_id=args.v2p_trajectory_id, - target_fps=args.target_fps, - robot_type=args.robot, - start_frame=args.v2p_start_frame, - start_at_first_contact=args.v2p_start_at_first_contact, - pre_contact_frames=args.v2p_pre_contact_frames, - end_after_last_contact_frames=args.v2p_end_after_last_contact_frames, - ) - if ref_raw.get("first_contact_frame") is not None: - print( - " first hand-object contact: " - f"frame {ref_raw['first_contact_frame']} " - f"(keeping {args.v2p_pre_contact_frames} pre-contact frames)" - ) - if ref_raw.get("last_contact_frame") is not None: - print(f" last hand-object contact: frame {ref_raw['last_contact_frame']}") - if ref_raw.get("start_frame", 0): - print(f" dropped leading {ref_raw['start_frame']} interpolated V2P frames") - if ref_raw.get("end_frame") is not None: - print( - " trimmed V2P end at interpolated frame " - f"{ref_raw['end_frame']} " - f"(keeping {args.v2p_end_after_last_contact_frames} post-contact frames)" - ) - print(f" {ref_raw['left_pos'].shape[0]} frames at {ref_raw['fps']}fps") - # Reference-owned checks: warn if input motion or workspace assets are - # missing fields the planner can't produce on its own. Issues here belong - # to the upstream retargeting / asset pipeline, not this script. - warn_reference_issues(ref_raw.get("_motion_data"), ref_raw, args.robot) - heading_frame = 0 - if args.heading_align_frame == "first_contact": - first_contact_ref = ref_raw.get("first_contact_frame_in_reference") - if first_contact_ref is None: - print( - " WARNING: first-contact heading requested but no contact frame is available; using frame 0" - ) - else: - heading_frame = int(first_contact_ref) - print( - " heading alignment frame: first contact " - f"(reference frame {heading_frame})" - ) - else: - print(" heading alignment frame: start (reference frame 0)") - - # Step 6: Inference (initial agent build) - if args.ik_verify or args.ik_plan: - print("IK mode — not implemented in this script yet") - return - - print("\nStep 6: Inference") - agent = MotionInferenceAgent(device="cuda") - vis_xml = hand_xml if os.path.exists(hand_xml) else scene_xml - model = mujoco.MjModel.from_xml_path(vis_xml) - - def _plan_one_offset( - delta_yaw_offset_rad: float, - ) -> tuple[ - dict[str, Any], - tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray], - np.ndarray, - np.ndarray, - np.ndarray, - int, - float, - ]: - """Run Steps 3-7 for a single heading offset and return outputs + score. - - Returns ``(ref_data, traj_tuple, qpos_full, mfm_ref, full_qpos, - ref_start, score)`` where ``score`` is the mean wrist tracking - error in metres. - """ - ref_data_local = transform_reference( - ref_raw, - nom, - workspace_offset=tuple(args.workspace_offset), - robot_type=args.robot, - delta_yaw_offset=delta_yaw_offset_rad, - heading_frame=heading_frame, - ) - T_raw_local = ref_data_local["left_pos"].shape[0] - N_ref_local = ( - T_raw_local if args.ref_seconds < 0 else int(FPS * args.ref_seconds) - ) - plan_n_ref_local = min(N_ref_local, T_raw_local) - traj = build_interp_trajectory( - nom, - ref_data_local["left_pos"], - ref_data_local["left_quat"], - ref_data_local["right_pos"], - ref_data_local["right_quat"], - fps=FPS, - hold_start_s=hold_start_s, - interp_s=interp_s, - hold_end_s=hold_end_s, - n_ref=plan_n_ref_local, - ) - traj_lp_l, traj_lq_l, traj_rp_l, traj_rq_l, seg_local = traj - T_total_local = len(traj_lp_l) - ref_start_local = seg_local["ref_start"] - root_pos_local = np.zeros((T_total_local, 3), dtype=np.float32) - root_pos_local[:, 2] = ROOT_HEIGHT - root_wxyz_local = np.tile([1.0, 0.0, 0.0, 0.0], (T_total_local, 1)).astype( - np.float32 - ) - result_local = agent.infer_from_ee_positions( - root_pos_local, - root_wxyz_local, - traj_lp_l, - traj_lq_l, - traj_rp_l, - traj_rq_l, - root_height_override=ROOT_HEIGHT, - max_chunk_tokens=6, - modes=("autoregressive",), - smooth=not args.no_smooth_qpos, - half_stride_blend=True, - fix_lower_body=args.fix_lower_body, - ) - qpos_full_local = result_local["autoregressive"]["qpos"] - T_mfm_local = qpos_full_local.shape[0] - T_ref_mfm_local = T_mfm_local - ref_start_local - mfm_ref_local = qpos_full_local[ - ref_start_local : ref_start_local + T_ref_mfm_local - ] - T_target_local = plan_n_ref_local - if T_ref_mfm_local != T_target_local and T_ref_mfm_local > 1: - t_src = np.linspace(0, 1, T_ref_mfm_local) - t_dst = np.linspace(0, 1, T_target_local) - pos_joints = np.concatenate( - [mfm_ref_local[:, :3], mfm_ref_local[:, 7:]], axis=1 - ) - pos_joints_interp = interp1d(t_src, pos_joints, axis=0, kind="linear")( - t_dst - ) - root_quats = Rotation.from_quat(mfm_ref_local[:, 3:7]) - root_quats_interp = Slerp(t_src, root_quats)(t_dst).as_quat() - mfm_ref_new = np.zeros( - (T_target_local, mfm_ref_local.shape[1]), - dtype=mfm_ref_local.dtype, - ) - mfm_ref_new[:, :3] = pos_joints_interp[:, :3] - mfm_ref_new[:, 3:7] = root_quats_interp - mfm_ref_new[:, 7:] = pos_joints_interp[:, 3:] - mfm_ref_local = mfm_ref_new - else: - mfm_ref_local = mfm_ref_local[:T_target_local] - T_save_local = min(T_target_local, mfm_ref_local.shape[0]) - full_qpos_local, _, _, _ = build_full_qpos( - mfm_ref_local, - ref_data_local, - model, - T_save_local, - fix_lower_body=args.fix_lower_body, - fix_root_pos=args.fix_root_pos, - fix_root_rot=args.fix_root_rot, - fix_root_z=args.fix_root_z, - fix_root_rp=args.fix_root_rp, - fix_root_components=args.fix_root, - ) - score = wrist_ee_error_from_qpos(full_qpos_local, ref_data_local, model) - return ( - ref_data_local, - (traj_lp_l, traj_lq_l, traj_rp_l, traj_rq_l), - qpos_full_local, - mfm_ref_local, - full_qpos_local, - ref_start_local, - score, - ) - - if args.search_heading_deg > 0.0: - n = float(args.search_heading_deg) - candidates_deg = [-n, -n / 2.0, 0.0, n / 2.0, n] - print( - f"\nLocal heading search across {candidates_deg} deg " - "(picking lowest wrist EE error):" - ) - scored = [] - for d_deg in candidates_deg: - res = _plan_one_offset(np.radians(d_deg)) - scored.append((d_deg, res)) - print(f" Δ={d_deg:+5.1f}°: wrist EE = {res[-1] * 1000:.1f}mm") - best_deg, best_res = min(scored, key=lambda kv: kv[1][-1]) - print(f" → best Δ={best_deg:+.1f}° (EE = {best_res[-1] * 1000:.1f}mm)") - ref_data, traj_tuple, qpos_full, mfm_ref, full_qpos, ref_start, _ = best_res - else: - ref_data, traj_tuple, qpos_full, mfm_ref, full_qpos, ref_start, score = ( - _plan_one_offset(0.0) - ) - print(f" Wrist EE error: {score * 1000:.1f}mm") - traj_lp, traj_lq, traj_rp, traj_rq = traj_tuple - print(f" Yaw correction: {np.degrees(ref_data['delta_yaw']):.1f} deg") - print(f" {full_qpos.shape}") - - # Step 8: Save parquet - print("\nStep 8: Save parquet") - sequence_id = args.v2p_sequence - # Try to extract full sequence ID from the parquet data - motion = ref_raw.get("_motion_data") - if motion and hasattr(motion, "sequence_id") and motion.sequence_id: - sequence_id = motion.sequence_id - - # `output_dir` is the dataset root; the planner writes parquet under - # `/planner_processed/…` and the support USD under - # `/reconstructed_stage/…`. SceneConfig discovers the support - # USD by walking up from the parquet's `sequence_id=…` dir. - output_dir = args.output or str(Path.cwd() / "planner_output") - save_planner_parquet( - output_dir, - full_qpos, - ref_data, - model, - ref_raw, - args.robot, - sequence_id, - ) - - # Step 8b: Reconstruct support surface USD for the transformed object - # positions. SceneConfig.from_motion_file walks parquet -> ../../../ - # /reconstructed_stage/_support.usda, so we drop the USD next to - # the planner output rather than relying on the upstream retarget's - # un-transformed surface. - print("\nStep 8b: Reconstruct support surface") - support_dir = Path(output_dir) / "reconstructed_stage" - support_dir.mkdir(parents=True, exist_ok=True) - support_usda = support_dir / f"{sequence_id}_support.usda" - reconstruct_support_for_sequence( - input_dir=Path(output_dir) / "planner_processed", - sequence_id=sequence_id, - output_override=str(support_usda), - schema="motion_v1", - ) - - # Step 9: Viewer — show the FULL trajectory (warmup + reference) - if not args.no_viewer: - print("\nStep 9: Viewer | Space=pause") - # Build full qpos for the entire planner output (warmup + reference) - vis_full_qpos, _, _, _ = build_full_qpos( - qpos_full, - ref_data, - model, - qpos_full.shape[0], - fix_lower_body=args.fix_lower_body, - fix_root_pos=args.fix_root_pos, - fix_root_rot=args.fix_root_rot, - fix_root_z=args.fix_root_z, - fix_root_rp=args.fix_root_rp, - fix_root_components=args.fix_root, - ) - - # Discover object mesh and support surface from V2P parquet - object_mesh_path = None - support_usda_path = None - resolved_mesh_paths: list[str] = [] - motion = ref_raw.get("_motion_data") - if motion: - # Resolve every per-body mesh path declared in the parquet, - # not just the first that exists locally — multi-body objects - # (e.g. taco spoon + pan) need all of them rendered. - mesh_paths = getattr(motion, "object_mesh_paths", None) - obj_name = getattr(motion, "object_name", None) - if mesh_paths: - iterable = mesh_paths if isinstance(mesh_paths, list) else [mesh_paths] - for mp in iterable: - if not mp: - continue - if os.path.exists(mp): - resolved_mesh_paths.append(mp) - continue - suffix = ( - mp.split("assets/meshes/")[-1] - if "assets/meshes/" in mp - else None - ) - if not suffix: - continue - local = os.path.join(ASSET_DIR, "meshes", suffix) - if os.path.exists(local): - resolved_mesh_paths.append(local) - continue - tex = os.path.join(os.path.dirname(local), "mesh_tex.obj") - if os.path.exists(tex): - resolved_mesh_paths.append(tex) - if resolved_mesh_paths: - object_mesh_path = resolved_mesh_paths[0] - - # Fallback: look up from object registry - if object_mesh_path is None and obj_name: - mesh_dir = os.path.join(ASSET_DIR, "meshes", "arctic", obj_name) - tex = os.path.join(mesh_dir, "mesh_tex.obj") - if os.path.exists(tex): - object_mesh_path = tex - - # Discover support USDA — walk up from parquet to find reconstructed_stage/ - v2p_path = Path(args.v2p_parquet).resolve() - if sequence_id: - for parent in [v2p_path] + list(v2p_path.parents): - candidate = ( - parent / "reconstructed_stage" / f"{sequence_id}_support.usda" - ) - if candidate.exists(): - support_usda_path = str(candidate) - break - - if resolved_mesh_paths: - print(f" Object meshes ({len(resolved_mesh_paths)}):") - for mp in resolved_mesh_paths: - print(f" {mp}") - elif object_mesh_path: - print(f" Object mesh: {object_mesh_path}") - if support_usda_path: - print(f" Support: {support_usda_path}") - - # Build support transform: same yaw + offset as applied to EE/object - support_xform = None - if support_usda_path: - source = apply_local_frame_fix(ref_raw, robot_type=args.robot) - src_midpoint = 0.5 * (source["left_pos"][0] + source["right_pos"][0]) - support_xform = { - "delta_yaw": ref_data["delta_yaw"], - "offset": ref_data["offset"], - "src_midpoint": src_midpoint, - } - - obj_pos_vis = ref_data.get("object_pos_all", ref_data.get("object_pos")) - obj_quat_vis = ref_data.get("object_quat_all", ref_data.get("object_quat")) - - # Per-body mesh list for the viewer. Prefer the resolved per-body - # mesh_paths from the parquet (one entry per object body); fall back - # to the arctic registry layout, then to the singular mesh. - object_mesh_paths_list = list(resolved_mesh_paths) - if not object_mesh_paths_list: - body_names = getattr(motion, "object_body_names", None) or [] - obj_name = ref_data.get("object_name", "box") - mesh_base = os.path.join(ASSET_DIR, "meshes", "arctic", obj_name) - for bname in body_names: - bp = os.path.join(mesh_base, f"{bname}.obj") - if os.path.exists(bp): - object_mesh_paths_list.append(bp) - if not object_mesh_paths_list and object_mesh_path: - object_mesh_paths_list = [object_mesh_path] - - visualize( - vis_xml, - vis_full_qpos, - traj_lp, - traj_lq, - traj_rp, - traj_rq, - left_finger_joints=ref_data["left_finger_joints"], - right_finger_joints=ref_data["right_finger_joints"], - recorded_left_names=ref_data["left_joint_names"], - recorded_right_names=ref_data["right_joint_names"], - object_pos=obj_pos_vis, - object_quat=obj_quat_vis, - fps=FPS, - ref_start=ref_start, - object_mesh_paths=object_mesh_paths_list, - support_usda_path=support_usda_path, - support_transform=support_xform, - ) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/__init__.py deleted file mode 100644 index cd6b6a87..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MotionBricks (gr00t) planner adapter.""" - -from robotic_grounding.planner.motionbricks.inference import MotionInferenceAgent - -__all__ = ["MotionInferenceAgent"] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/assets/models/planner.pkg b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/assets/models/planner.pkg deleted file mode 100644 index 7625f706..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/assets/models/planner.pkg +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bccc22c3d1834bebe729ec229507d7118d702b1746475d76839d81cc36bb6fd3 -size 796514994 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/inference.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/inference.py deleted file mode 100644 index 6d2383c2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/inference.py +++ /dev/null @@ -1,677 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""v2d-side adapter for the gr00t MotionBricks planner bundle. - -Loads the torch.package archive at -``planner/assets/models/motionbricks_planner.pkg`` and exposes -``MotionInferenceAgent`` with a contract identical to the legacy MFM-based -agent so ``g1_planner.py`` only changes its import line. All -``torch.package`` workarounds (dataclass exec ordering, bundled ``groot.*`` -namespace strings) are contained in this file. -""" - -from __future__ import annotations - -import contextlib -import dataclasses as _dc -import sys -from collections.abc import Iterator -from pathlib import Path -from typing import Any - -import numpy as np -from torch.package import PackageImporter - -from robotic_grounding.planner.motionbricks.qpos import ( - DEFAULT_SEED_XML, - HAND_ROOT_TO_WRIST_OFFSET_LOCAL_LEFT, - HAND_ROOT_TO_WRIST_OFFSET_LOCAL_RIGHT, - LOWER_BODY_BODY_INDICES_ISAACLAB, - apply_hand_root_to_wrist_offset, - build_seed_qpos, - chunk_boundary_centers, - features_to_qpos, - qpos_to_body_world, - smooth_qpos_at_boundaries, - smooth_qpos_global, -) - -# --------------------------------------------------------------------------- -# Section 1: torch.package compatibility shim -# --------------------------------------------------------------------------- -# `@dataclass`-defined types inside a torch.package archive crash on unpickle -# because the package importer exec's the module body before registering it -# in ``sys.modules``. Python's ``dataclasses._is_type`` does -# ``sys.modules.get(cls.__module__).__dict__`` to detect the KW_ONLY sentinel -# and raises ``AttributeError: NoneType``. Returning ``False`` from the -# patched ``_is_type`` is the correct fallback (the annotation is not -# ``KW_ONLY``). Fixed upstream in CPython 3.12. - - -@contextlib.contextmanager -def _torch_package_compat() -> Iterator[None]: - """Patch ``dataclasses._is_type`` for one torch.package load. - - ``@dataclass``-defined types inside a torch.package'd module crash on - unpickle without this; the patch is reverted on context exit. - """ - orig = _dc._is_type # type: ignore[attr-defined] - - def patched( - annotation: Any, - cls: type, - a_module: Any, - a_type: Any, - is_type_predicate: Any, - ) -> bool: - if sys.modules.get(cls.__module__) is None: - return False - return orig(annotation, cls, a_module, a_type, is_type_predicate) - - _dc._is_type = patched # type: ignore[attr-defined] - try: - yield - finally: - _dc._is_type = orig # type: ignore[attr-defined] - - -# --------------------------------------------------------------------------- -# Section 2: Bundle loader -# --------------------------------------------------------------------------- -# The bundled module namespace appears here only; callers receive a clean -# dict and never see torch.package internals. - -_BUNDLE_NAMESPACE = "groot.rl.trl.inference.motionbricks_e2e" -_SUPPORTED_BUNDLE_SCHEMA = 1 - -_PKG_PATH = Path(__file__).parent / "assets" / "models" / "planner.pkg" - - -def _load_bundle( - device: str, -) -> tuple[PackageImporter, dict[str, Any], dict[str, Any]]: - """Open the .pkg and import the bundled pipeline + transforms helpers. - - Returns the importer (kept alive so module imports stay valid), the - unpickled bundle dict, and a dict of helper modules. - """ - if not _PKG_PATH.exists(): - raise FileNotFoundError( - f"MotionBricks planner bundle not found at {_PKG_PATH}. " - "Run `git lfs pull` to fetch the binary asset." - ) - - with _torch_package_compat(): - importer = PackageImporter(str(_PKG_PATH)) - bundle = importer.load_pickle("bundle", "bundle.pkl") - - schema = int(bundle.get("schema_version", -1)) - if schema != _SUPPORTED_BUNDLE_SCHEMA: - raise RuntimeError( - f"Bundle at {_PKG_PATH} has schema_version={schema}, " - f"but this adapter expects {_SUPPORTED_BUNDLE_SCHEMA}. " - "Re-export the bundle from gr00t with the matching exporter." - ) - - # Hide the bundled-namespace import strings behind a private dict; the - # public class never references them again. - bundle_modules = { - "pipeline": importer.import_module(f"{_BUNDLE_NAMESPACE}.pipeline"), - "transforms": importer.import_module(f"{_BUNDLE_NAMESPACE}.transforms"), - } - - bundle["root_model"].to(device).eval() - bundle["pose_model"].to(device).eval() - - return importer, bundle, bundle_modules - - -# --------------------------------------------------------------------------- -# Section 3: Lower-body-fixed chunked AR loop -# --------------------------------------------------------------------------- -# Vendored from the bundled ``run_chunked_autoregressive_inference`` so we -# can override the lower-body body transforms at each chunk's pose seed and -# prediction tail. With this in place, the model sees a static lower body -# in its autoregressive context and produces upper-body motion consistent -# with stationary legs. - - -def _override_lower_body_in_place( - transforms: np.ndarray, static_pose: np.ndarray -) -> None: - """Pin lower-body body slots in ``transforms`` to ``static_pose``. - - Both arrays are ``(F, num_bodies * 9)`` packed; only the bodies in - ``LOWER_BODY_BODY_INDICES_ISAACLAB`` are overwritten in place. - """ - num_bodies = transforms.shape[-1] // 9 - F = transforms.shape[0] - view = transforms.reshape(F, num_bodies, 9) - static_view = static_pose[:F].reshape(F, num_bodies, 9) - for idx in LOWER_BODY_BODY_INDICES_ISAACLAB: - view[:, idx] = static_view[:, idx] - - -def _run_chunked_lower_body_fixed( - pipeline_module: Any, - transforms_module: Any, - root_model: Any, - pose_model: Any, - *, - root_ee_input: np.ndarray, - pose_ee_input: np.ndarray | None, - derive_pose_ee_from_pred_root: bool, - seed_root: np.ndarray, - seed_pose: np.ndarray, - static_pose_shared: np.ndarray, - chunk_tokens: int, - overlap_tokens: int, - device: str, -) -> dict[str, Any]: - """Chunked AR with the lower-body bodies pinned across chunks. - - Mirrors the bundled chunked AR loop but, after each per-chunk inference, - overrides the lower-body slots in the predicted body transforms with - the static-seed values before the next chunk reads its seed. The model - therefore sees stationary legs in its context window every chunk. - """ - nfpt = root_model.num_frames_per_token - max_tokens = min( - root_model._max_tokens, - getattr(pose_model, "_max_tokens", root_model._max_tokens), - ) - chunk_tokens = min(max(chunk_tokens, 2), max_tokens) - overlap_tokens = max(1, min(overlap_tokens, chunk_tokens - 1)) - overlap_frames = overlap_tokens * nfpt - chunk_frames = chunk_tokens * nfpt - stride_frames = chunk_frames - overlap_frames - - total_frames = (root_ee_input.shape[0] // nfpt) * nfpt - if total_frames < nfpt: - raise ValueError(f"Need at least {nfpt} frames, got {root_ee_input.shape[0]}") - - root_dim = root_model.root_dim - pose_dim = pose_model.pose_dim - root_accum = np.zeros((total_frames, root_dim), dtype=np.float32) - pose_accum = np.zeros((total_frames, pose_dim), dtype=np.float32) - weight_accum = np.zeros((total_frames, 1), dtype=np.float32) - - previous_result: dict[str, np.ndarray] | None = None - previous_range: tuple[int, int] | None = None - chunk_infos: list[dict[str, Any]] = [] - root_ee_key = pipeline_module._model_ee_key(root_model) - pose_ee_key = pipeline_module._model_ee_key(pose_model) - use_chunk_local_frame = ( - pipeline_module._model_root_key(root_model) - == pipeline_module.HEADING_ROOT_XY_KEY - ) - - start = 0 - while start < total_frames: - end = min(start + chunk_frames, total_frames) - end = start + ((end - start) // nfpt) * nfpt - if end - start < nfpt: - break - - window_num_tokens = (end - start) // nfpt - root_ee_window = root_ee_input[start:end] - pose_ee_window = pose_ee_input[start:end] if pose_ee_input is not None else None - - if previous_result is None: - start_root_shared = seed_root[:nfpt].copy() - start_pose_shared = seed_pose[:nfpt].copy() - else: - assert previous_range is not None - prev_start, _ = previous_range - local_start = start - prev_start - local_end = local_start + nfpt - if local_start < 0 or local_end > previous_result["pred_frames"]: - raise RuntimeError( - f"Chunk {start}:{end} outside previous range {previous_range}" - ) - start_root_shared = previous_result["pred_root"][ - local_start:local_end - ].copy() - start_pose_shared = previous_result["pred_joints"][ - local_start:local_end - ].copy() - - # Pin the lower body in the AR seed so the model sees stationary legs. - _override_lower_body_in_place( - start_pose_shared, static_pose_shared[start : start + nfpt] - ) - - if use_chunk_local_frame: - chunk_origin_xy = np.asarray(start_root_shared[0, :2], dtype=np.float32) - chunk_heading_mat = transforms_module._heading_matrix_from_pose_frame( - start_pose_shared[0] - ).astype(np.float32) - start_root = transforms_module._root_xy_shared_to_local( - start_root_shared, chunk_origin_xy, chunk_heading_mat - ) - start_pose = transforms_module._packed_transforms_change_frame( - start_pose_shared, - chunk_heading_mat, - inverse_heading=True, - positions_are_root_relative=True, - ) - root_ee_window_model = transforms_module._packed_transforms_change_frame( - root_ee_window, - chunk_heading_mat, - origin_xy=( - chunk_origin_xy - if root_ee_key == pipeline_module.ROOT_TARGET_EE_KEY - else None - ), - inverse_heading=True, - positions_are_root_relative=root_ee_key - != pipeline_module.ROOT_TARGET_EE_KEY, - ) - pose_ee_window_model = ( - transforms_module._packed_transforms_change_frame( - pose_ee_window, - chunk_heading_mat, - origin_xy=( - chunk_origin_xy - if pose_ee_key == pipeline_module.ROOT_TARGET_EE_KEY - else None - ), - inverse_heading=True, - positions_are_root_relative=pose_ee_key - != pipeline_module.ROOT_TARGET_EE_KEY, - ) - if pose_ee_window is not None - else None - ) - else: - chunk_origin_xy = np.zeros(2, dtype=np.float32) - chunk_heading_mat = np.eye(3, dtype=np.float32) - start_root = start_root_shared - start_pose = start_pose_shared - root_ee_window_model = root_ee_window - pose_ee_window_model = pose_ee_window - - result = pipeline_module.run_inference( - root_model, - pose_model, - root_ee_input=root_ee_window_model, - pose_ee_input=pose_ee_window_model, - derive_pose_ee_from_pred_root=derive_pose_ee_from_pred_root, - num_tokens=window_num_tokens, - start_root=start_root, - start_pose=start_pose, - device=device, - ) - - local_n = min(result["pred_frames"], end - start) - pred_root_shared = ( - transforms_module._root_xy_local_to_shared( - result["pred_root"][:local_n], chunk_origin_xy, chunk_heading_mat - ) - if use_chunk_local_frame - else result["pred_root"][:local_n] - ) - pred_joints_shared = ( - transforms_module._packed_transforms_change_frame( - result["pred_joints"][:local_n], - chunk_heading_mat, - inverse_heading=False, - positions_are_root_relative=True, - ) - if use_chunk_local_frame - else result["pred_joints"][:local_n] - ) - - # Overwrite lower-body bodies in the prediction so the next chunk's - # AR seed (read from previous_result) is also lower-body-static. - _override_lower_body_in_place( - pred_joints_shared, static_pose_shared[start : start + local_n] - ) - - weights = pipeline_module._triangular_chunk_weights(local_n)[:, None] - root_accum[start : start + local_n] += pred_root_shared * weights - pose_accum[start : start + local_n] += pred_joints_shared * weights - weight_accum[start : start + local_n] += weights - - chunk_infos.append( - { - "start_frame": start, - "end_frame": start + local_n, - "num_tokens": int(np.ceil(local_n / nfpt)), - "window_num_tokens": int(window_num_tokens), - "predicted_num_tokens": int(result["pred_num_tokens"]), - "chunk_num_tokens_mode": "forced", - "chunk_origin_xy": chunk_origin_xy.tolist(), - "chunk_local_frame_enabled": bool(use_chunk_local_frame), - } - ) - - previous_result = { - "pred_root": pred_root_shared, - "pred_joints": pred_joints_shared, - "pred_frames": local_n, - } - previous_range = (start, start + local_n) - if start + local_n >= total_frames: - break - start += stride_frames - - valid = weight_accum[:, 0] > 0 - if not np.all(valid): - last_valid = int(np.nonzero(valid)[0][-1]) + 1 - root_accum = root_accum[:last_valid] - pose_accum = pose_accum[:last_valid] - weight_accum = weight_accum[:last_valid] - - pred_root = root_accum / np.clip(weight_accum, 1e-8, None) - pred_joints = pose_accum / np.clip(weight_accum, 1e-8, None) - # Final pass: enforce lower-body-static in the blended output. - _override_lower_body_in_place( - pred_joints, static_pose_shared[: pred_joints.shape[0]] - ) - - return { - "pred_root": pred_root, - "pred_joints": pred_joints, - "pred_num_tokens": int(np.ceil(pred_root.shape[0] / nfpt)), - "pred_frames": int(pred_root.shape[0]), - "chunk_infos": chunk_infos, - } - - -# --------------------------------------------------------------------------- -# Section 4: Half-stride second pass -# --------------------------------------------------------------------------- - - -def _blend_two_passes( - pred_a: dict[str, Any], - pred_b: dict[str, Any], - *, - offset_b: int, - total_frames: int, -) -> dict[str, Any]: - """Average two AR passes that share the same shared-frame indexing. - - Pass A covers ``[0, len_a)``; pass B covers ``[offset_b, offset_b + len_b)`` - in the same first-frame heading frame. Each pass is already a smooth - chunked-AR output; the half-stride offset ensures that frames near a - chunk boundary in one pass sit mid-chunk in the other, so a per-frame - mean smooths out residual seam artifacts. - """ - F = min( - total_frames, - max(pred_a["pred_frames"], offset_b + pred_b["pred_frames"]), - ) - accum_root = np.zeros((F, pred_a["pred_root"].shape[-1]), dtype=np.float32) - accum_pose = np.zeros((F, pred_a["pred_joints"].shape[-1]), dtype=np.float32) - count = np.zeros((F, 1), dtype=np.float32) - - n_a = min(pred_a["pred_frames"], F) - accum_root[:n_a] += pred_a["pred_root"][:n_a] - accum_pose[:n_a] += pred_a["pred_joints"][:n_a] - count[:n_a] += 1.0 - - n_b = min(pred_b["pred_frames"], F - offset_b) - if n_b > 0: - accum_root[offset_b : offset_b + n_b] += pred_b["pred_root"][:n_b] - accum_pose[offset_b : offset_b + n_b] += pred_b["pred_joints"][:n_b] - count[offset_b : offset_b + n_b] += 1.0 - - count = np.clip(count, 1e-8, None) - return { - "pred_root": accum_root / count, - "pred_joints": accum_pose / count, - "pred_num_tokens": pred_a.get("pred_num_tokens", 0), - "pred_frames": int(F), - "chunk_infos": (pred_a.get("chunk_infos") or []) - + (pred_b.get("chunk_infos") or []), - } - - -# --------------------------------------------------------------------------- -# Section 5: Public agent -# --------------------------------------------------------------------------- - - -class MotionInferenceAgent: - """Whole-body motion inference: EE targets → full-body qpos. - - Drop-in replacement for the legacy MFM-based agent. Public method - ``infer_from_ee_positions`` carries the same signature so - ``g1_planner.py`` only needs to swap its import line. - - Inputs are MuJoCo z-up world frame; output qpos layout is - ``[pos(3), xyzw_quat(4), joints(29)]`` — same as legacy. - """ - - def __init__(self, device: str = "cuda") -> None: - """Load the bundle and place all weights on ``device``.""" - self._device = device - self._importer, self._bundle, self._mods = _load_bundle(device) - self._root_model = self._bundle["root_model"] - self._pose_model = self._bundle["pose_model"] - self._kin = self._bundle["humanoid_kinematics"] - self._body_reorder = self._bundle["body_reorder"] - - @property - def nfpt(self) -> int: - """Number of frames per MotionBricks token.""" - return int(self._bundle["nfpt"]) - - @property - def model_fps(self) -> int: - """Training-time frame rate, returned for legacy parity only. - - MotionBricks training data is at 25 fps. v2d's planner orchestrator - resamples upstream and trims downstream, so this is for logging only. - """ - return 25 - - @property - def max_tokens(self) -> int: - """Maximum tokens in a single chunked-AR window.""" - return int(self._bundle["max_tokens"]) - - def infer_from_ee_positions( - self, - root_pos: np.ndarray, - root_wxyz: np.ndarray, - left_ee_pos: np.ndarray, - left_ee_quat_wxyz: np.ndarray, - right_ee_pos: np.ndarray, - right_ee_quat_wxyz: np.ndarray, - root_height_override: float | None = None, - z_offset: float = 0.0, - src_fps: float | None = None, - max_chunk_tokens: int = 6, - overlap_tokens: int = 3, - modes: tuple[str, ...] = ("autoregressive",), - smooth: bool = True, - half_stride_blend: bool = True, - left_hand_root_offset_local: tuple[float, float, float] | None = ( - HAND_ROOT_TO_WRIST_OFFSET_LOCAL_LEFT - ), - right_hand_root_offset_local: tuple[float, float, float] | None = ( - HAND_ROOT_TO_WRIST_OFFSET_LOCAL_RIGHT - ), - fix_lower_body: bool = False, - ) -> dict[str, dict[str, Any]]: - """Run whole-body inference from end-effector targets. - - Inputs are MuJoCo z-up world-frame trajectories. Output qpos layout - matches the legacy contract: ``[pos(3), xyzw_quat(4), joints(29)]``. - - Args: - root_pos: ``(T, 3)`` desired root position trajectory. - root_wxyz: ``(T, 4)`` desired root quaternion (wxyz). - left_ee_pos: ``(T, 3)`` desired left wrist position. - left_ee_quat_wxyz: ``(T, 4)`` desired left wrist quaternion (wxyz). - right_ee_pos: ``(T, 3)`` desired right wrist position. - right_ee_quat_wxyz: ``(T, 4)`` desired right wrist quaternion (wxyz). - root_height_override: Optional fixed Z to apply to ``root_pos``. - z_offset: Optional additive Z offset on top of the input root Z. - src_fps: Source FPS, accepted for legacy parity but unused — - v2d orchestrator handles resampling upstream. - max_chunk_tokens: Chunk length in MotionBricks tokens. - overlap_tokens: Overlap between consecutive chunks, used for - triangular-weight blending and AR seeding of the next chunk. - modes: Inference modes; currently only ``"autoregressive"`` is - produced. Kept as a tuple for legacy parity. - smooth: Accepted for legacy parity; the bundled pipeline applies - its own boundary blending so this flag is a no-op. - half_stride_blend: Accepted for legacy parity; chunked AR - already overlap-blends so this flag is a no-op. - left_hand_root_offset_local: Local-frame offset from the V2P - hand-root body (palm_link) to wrist_yaw_link, applied to - ``left_ee_pos`` before canonicalization. Pass ``None`` to - skip when the inputs are already at wrist_yaw_link. - right_hand_root_offset_local: Right-side counterpart. - fix_lower_body: When ``True``, run a custom AR loop that pins - the lower-body bodies (hips/knees/ankles) to the static - seed pose at every chunk's AR context. The model sees - stationary legs in its history and produces upper-body - motion consistent with that constraint. - - Returns: - ``{"autoregressive": {"qpos": (T, 36), "chunk_kf_info": [...]}}`` - with qpos in MuJoCo xyzw-quaternion layout. - """ - del src_fps, half_stride_blend # parity-only kwargs - - if root_height_override is not None: - root_pos = root_pos.copy() - root_pos[:, 2] = root_height_override - if z_offset != 0.0: - root_pos = root_pos.copy() - root_pos[:, 2] += z_offset - - # V2P wrist positions track the hand-root body (palm_link / hand_C_MC). - # The model expects wrist_yaw_link positions, which are offset along the - # forearm. Convert before canonicalization. - if left_hand_root_offset_local is not None: - left_ee_pos = apply_hand_root_to_wrist_offset( - left_ee_pos, left_ee_quat_wxyz, left_hand_root_offset_local - ) - if right_hand_root_offset_local is not None: - right_ee_pos = apply_hand_root_to_wrist_offset( - right_ee_pos, right_ee_quat_wxyz, right_hand_root_offset_local - ) - - T = root_pos.shape[0] - - # Heading canonicalization expects a body trajectory derived from - # MuJoCo FK on a qpos. Synthesize a static-pose qpos and FK it so - # the bundled helpers see the same coordinate convention used at - # training time. - seed_qpos, seed_joint_names = build_seed_qpos( - num_frames=T, root_height=float(root_pos[0, 2]) - ) - body_pos_w, body_wxyz_w = qpos_to_body_world( - seed_qpos, seed_joint_names, DEFAULT_SEED_XML - ) - - transforms = self._mods["transforms"] - gt_joint_transforms, gt_root_xy = transforms._canonicalize_heading_transforms( - body_pos_w=body_pos_w, - body_wxyz_w=body_wxyz_w, - root_pos_w=seed_qpos[:, :3], - root_wxyz_w=seed_qpos[:, 3:7], - ) - root_ee_transforms, pose_ee_transforms, _ee_marker_pos = ( - transforms._canonicalize_ee_targets( - left_pos_w=left_ee_pos, - left_wxyz_w=left_ee_quat_wxyz, - right_pos_w=right_ee_pos, - right_wxyz_w=right_ee_quat_wxyz, - root_pos_w=seed_qpos[:, :3], - root_wxyz_w=seed_qpos[:, 3:7], - ) - ) - - root_target_ee_key = self._bundle["root_ee_key"].endswith( - "hand_ee_target_transforms_nonflat" - ) - root_ee_input = root_ee_transforms if root_target_ee_key else pose_ee_transforms - derive_pose = self._bundle["derive_pose_ee_from_pred_root"] - pose_ee_input = None if derive_pose else pose_ee_transforms - - pipeline = self._mods["pipeline"] - nfpt = int(self._bundle["nfpt"]) - chunk_frames = int(max_chunk_tokens) * nfpt - half_stride = chunk_frames // 2 - - def _run_pass(start_offset: int) -> dict[str, Any]: - ee_in = root_ee_input[start_offset:] - pose_ee_in = ( - pose_ee_input[start_offset:] if pose_ee_input is not None else None - ) - if fix_lower_body: - static_pass = gt_joint_transforms[start_offset:] - return _run_chunked_lower_body_fixed( - pipeline, - self._mods["transforms"], - self._root_model, - self._pose_model, - root_ee_input=ee_in, - pose_ee_input=pose_ee_in, - derive_pose_ee_from_pred_root=derive_pose, - seed_root=gt_root_xy[start_offset:], - seed_pose=gt_joint_transforms[start_offset:], - static_pose_shared=static_pass, - chunk_tokens=int(max_chunk_tokens), - overlap_tokens=int(overlap_tokens), - device=self._device, - ) - return pipeline.run_chunked_autoregressive_inference( - self._root_model, - self._pose_model, - root_ee_input=ee_in, - seed_root=gt_root_xy[start_offset:], - seed_pose=gt_joint_transforms[start_offset:], - pose_ee_input=pose_ee_in, - derive_pose_ee_from_pred_root=derive_pose, - chunk_tokens=int(max_chunk_tokens), - overlap_tokens=int(overlap_tokens), - chunk_num_tokens_mode="forced", - device=self._device, - ) - - result_a = _run_pass(0) - if half_stride >= nfpt and root_ee_input.shape[0] - half_stride >= nfpt: - result_b = _run_pass(half_stride) - result = _blend_two_passes( - result_a, - result_b, - offset_b=half_stride, - total_frames=root_ee_input.shape[0], - ) - else: - result = result_a - - qpos_36 = features_to_qpos( - result["pred_joints"], - result["pred_root"], - self._kin, - self._body_reorder, - ) - - # Damp chunk-overlap oscillation: a mild global Hamming pass first, - # then targeted boundary smoothing with a wider window. - if smooth: - qpos_36 = smooth_qpos_global(qpos_36, nfpt=nfpt) - boundaries = chunk_boundary_centers(result.get("chunk_infos") or []) - if boundaries: - qpos_36 = smooth_qpos_at_boundaries(qpos_36, boundaries, nfpt=nfpt) - - # Repack root quat from wxyz → xyzw to match the legacy contract. - qpos_xyzw = qpos_36.copy() - qpos_xyzw[:, 3:7] = qpos_36[:, [4, 5, 6, 3]] - - chunk_kf_info = result.get("chunk_infos", []) - return { - "autoregressive": { - "qpos": qpos_xyzw.astype(np.float32), - "chunk_kf_info": chunk_kf_info, - } - } diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/qpos.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/qpos.py deleted file mode 100644 index c3c39af7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/motionbricks/qpos.py +++ /dev/null @@ -1,508 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Kinematics helpers for the MotionBricks planner adapter. - -Pure math + MuJoCo FK; no torch.package or bundle awareness. The agent -(``motionbricks_inference.MotionInferenceAgent``) calls into this module to -build a canonicalization seed and to convert decoded body transforms back -into MuJoCo qpos. -""" - -from __future__ import annotations - -from pathlib import Path -from typing import Any - -import mujoco -import numpy as np -import torch -from scipy.spatial.transform import Rotation - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- - -# G1 body order expected by the planner. Must match the model's -# training-time body order; do not reorder. -G1_BODY_NAMES_ISAACLAB: list[str] = [ - "pelvis", - "left_hip_pitch_link", - "right_hip_pitch_link", - "waist_yaw_link", - "left_hip_roll_link", - "right_hip_roll_link", - "waist_roll_link", - "left_hip_yaw_link", - "right_hip_yaw_link", - "torso_link", - "left_knee_link", - "right_knee_link", - "left_shoulder_pitch_link", - "right_shoulder_pitch_link", - "left_ankle_pitch_link", - "right_ankle_pitch_link", - "left_shoulder_roll_link", - "right_shoulder_roll_link", - "left_ankle_roll_link", - "right_ankle_roll_link", - "left_shoulder_yaw_link", - "right_shoulder_yaw_link", - "left_elbow_link", - "right_elbow_link", - "left_wrist_roll_link", - "right_wrist_roll_link", - "left_wrist_pitch_link", - "right_wrist_pitch_link", - "left_wrist_yaw_link", - "right_wrist_yaw_link", -] - -DEFAULT_SEED_XML: Path = ( - Path(__file__).parent.parent / "assets" / "mujoco" / "g1_29dof.xml" -) - -# Joint name ordering for qpos[7:] when FK'd against ``DEFAULT_SEED_XML``. -G1_BODY_JOINT_NAMES: list[str] = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", -] - -# Indices into ``G1_BODY_NAMES_ISAACLAB`` for the lower-body bodies that -# get pinned when fix_lower_body is on (hips, knees, ankles — both sides). -LOWER_BODY_BODY_INDICES_ISAACLAB: tuple[int, ...] = ( - 1, # left_hip_pitch_link - 2, # right_hip_pitch_link - 4, # left_hip_roll_link - 5, # right_hip_roll_link - 7, # left_hip_yaw_link - 8, # right_hip_yaw_link - 10, # left_knee_link - 11, # right_knee_link - 14, # left_ankle_pitch_link - 15, # right_ankle_pitch_link - 18, # left_ankle_roll_link - 19, # right_ankle_roll_link -) - - -# V2P retargeting reports wrist positions at the HAND ROOT body -# (palm_link/hand_C_MC), but the planner is trained on the wrist_yaw_link -# body. These local-frame offsets convert hand-root world positions to -# wrist_yaw_link world positions: -# wrist_yaw_link_world = hand_root_world + R(hand_root_world_quat) @ offset -# Values are read from the dex3 hand MJCF (palm_link is a fixed-joint child -# of wrist_yaw_link with identity body_quat, so palm_link's world quat -# equals wrist_yaw_link's). -HAND_ROOT_TO_WRIST_OFFSET_LOCAL_LEFT: tuple[float, float, float] = ( - -0.0415, - -0.003, - 0.0, -) -HAND_ROOT_TO_WRIST_OFFSET_LOCAL_RIGHT: tuple[float, float, float] = ( - -0.0415, - 0.003, - 0.0, -) - - -def apply_hand_root_to_wrist_offset( - pos_w: np.ndarray, - quat_wxyz: np.ndarray, - offset_local: tuple[float, float, float], -) -> np.ndarray: - """Shift hand-root world positions to wrist_yaw_link world positions. - - Args: - pos_w: ``(T, 3)`` hand-root world positions. - quat_wxyz: ``(T, 4)`` hand-root world quaternions in wxyz order. - offset_local: ``(3,)`` constant offset in the hand-root local frame. - - Returns: - ``(T, 3)`` wrist_yaw_link world positions. - """ - quat_xyzw = quat_wxyz[:, [1, 2, 3, 0]] - rot = Rotation.from_quat(quat_xyzw) - offset_w = rot.apply(np.asarray(offset_local, dtype=np.float64)) - return (pos_w + offset_w).astype(np.float32) - - -# Nominal upper-body + leg pose for the canonicalization seed. Values are -# joint angles in radians; arms slightly tucked, legs in a soft crouch. -NOMINAL_BODY_JOINTS: dict[str, float] = { - "left_shoulder_pitch_joint": -0.5, - "left_shoulder_roll_joint": 0.2, - "left_elbow_joint": 0.0, - "right_shoulder_pitch_joint": -0.5, - "right_shoulder_roll_joint": -0.2, - "right_elbow_joint": 0.0, - "left_hip_pitch_joint": -0.1, - "left_knee_joint": 0.4, - "left_ankle_pitch_joint": -0.2, - "right_hip_pitch_joint": -0.1, - "right_knee_joint": 0.4, - "right_ankle_pitch_joint": -0.2, -} - - -# --------------------------------------------------------------------------- -# Seed qpos + MuJoCo FK -# --------------------------------------------------------------------------- - - -def build_seed_qpos( - num_frames: int, root_height: float -) -> tuple[np.ndarray, list[str]]: - """Build a static-pose seed qpos for canonicalization FK. - - Args: - num_frames: Number of frames in the trajectory. - root_height: World-frame Z height of the root body. - - Returns: - ``(qpos, joint_names)`` where ``qpos`` is ``(num_frames, 36)`` with - layout ``[root_pos(3), root_wxyz(4), body_joints(29)]`` and - ``joint_names`` matches the qpos[7:] ordering. - """ - qpos = np.zeros((num_frames, 36), dtype=np.float32) - qpos[:, 2] = root_height - qpos[:, 3] = 1.0 # wxyz identity (w=1) - for jname, val in NOMINAL_BODY_JOINTS.items(): - idx = G1_BODY_JOINT_NAMES.index(jname) - qpos[:, 7 + idx] = val - return qpos, list(G1_BODY_JOINT_NAMES) - - -def qpos_to_body_world( - qpos: np.ndarray, joint_names: list[str], xml_path: Path -) -> tuple[np.ndarray, np.ndarray]: - """Run MuJoCo FK and return G1 body world poses in IsaacLab body order. - - Args: - qpos: ``(T, model.nq)`` qpos with root pos+wxyz at qpos[:7]. - joint_names: Joint name ordering of qpos[7:]. - xml_path: Path to the MuJoCo XML used for FK. - - Returns: - ``(body_pos, body_wxyz)`` with shapes ``(T, 30, 3)`` and - ``(T, 30, 4)`` in IsaacLab body order. - """ - model = mujoco.MjModel.from_xml_path(str(xml_path)) - data = mujoco.MjData(model) - - if qpos.shape[-1] != model.nq: - raise ValueError( - f"seed qpos dim {qpos.shape[-1]} != MuJoCo nq={model.nq}; " - f"check {xml_path}" - ) - - src_idx = {n: i for i, n in enumerate(joint_names)} - model_joint_names = [ - mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j) - for j in range(model.njnt) - if int(model.jnt_qposadr[j]) >= 7 - ] - qpos_perm = [src_idx[n] for n in model_joint_names] - - body_ids = [] - for name in G1_BODY_NAMES_ISAACLAB: - bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if bid < 0: - raise ValueError(f"MuJoCo XML missing G1 body {name!r} in {xml_path}") - body_ids.append(bid) - - T = qpos.shape[0] - body_pos = np.zeros((T, len(body_ids), 3), dtype=np.float32) - body_wxyz = np.zeros((T, len(body_ids), 4), dtype=np.float32) - for t in range(T): - data.qpos[:7] = qpos[t, :7] - data.qpos[7:] = qpos[t, 7:][qpos_perm] - mujoco.mj_forward(model, data) - for j, bid in enumerate(body_ids): - body_pos[t, j] = data.xpos[bid] - xyzw = Rotation.from_matrix(data.xmat[bid].reshape(3, 3)).as_quat() - body_wxyz[t, j] = xyzw[[3, 0, 1, 2]] - - return body_pos, body_wxyz - - -# --------------------------------------------------------------------------- -# Qpos reconstruction -# --------------------------------------------------------------------------- -# The exporter ships kinematic chain tensors (parents, dof_axis, local -# rotations) as plain data so this module can convert decoded body -# transforms back to MuJoCo qpos without pulling open3d / hydra / lxml into -# v2d's runtime. - - -def _rot6d_to_matrix(rot6d: torch.Tensor) -> torch.Tensor: - """Convert rot6d (first two columns of R) to a rotation matrix. - - Args: - rot6d: ``[..., 6]`` packed as the first two columns of a rotation - matrix flattened to 6 channels. - - Returns: - ``[..., 3, 3]`` rotation matrix recovered via Gram-Schmidt + cross. - """ - cols = rot6d.reshape(*rot6d.shape[:-1], 3, 2) - c0 = cols[..., :, 0] - c0 = c0 / c0.norm(dim=-1, keepdim=True).clamp(min=1e-8) - c1 = cols[..., :, 1] - c1 = c1 - (c0 * c1).sum(dim=-1, keepdim=True) * c0 - c1 = c1 / c1.norm(dim=-1, keepdim=True).clamp(min=1e-8) - c2 = torch.cross(c0, c1, dim=-1) - return torch.stack([c0, c1, c2], dim=-1) - - -def _global_to_local_rotations( - global_rots: torch.Tensor, parents: torch.Tensor -) -> torch.Tensor: - """Convert global body rotations to per-joint local rotations. - - Args: - global_rots: ``[..., J, 3, 3]`` global rotation matrices. - parents: ``[J]`` parent body index per joint, with ``-1`` marking - roots (which pass through unchanged). - - Returns: - ``[..., J, 3, 3]`` local rotations such that - ``parent_rot @ local == global_rot``. - """ - p = parents[: global_rots.shape[-3]].clone() - root_mask = p == -1 - p[root_mask] = 0 # placeholder; overwritten below - parent_rot = global_rots[..., p.long(), :, :] - local = torch.matmul(parent_rot.transpose(-1, -2), global_rots) - if root_mask.any(): - local[..., root_mask, :, :] = global_rots[..., root_mask, :, :] - return local - - -def _rotation_matrices_to_dof( - rot_mats: torch.Tensor, dof_axis: torch.Tensor -) -> torch.Tensor: - """Project per-joint rotation matrices onto their actuated axis. - - Args: - rot_mats: ``[..., N, 3, 3]`` per-joint rotations in the joint's - local frame. - dof_axis: ``[N, 3]`` one-hot-style axis selector per joint. - - Returns: - ``[..., N]`` scalar joint angles around the selected axis. - """ - R = rot_mats - x_angle = torch.atan2(R[..., 2, 1], R[..., 2, 2]) - y_angle = torch.atan2(R[..., 0, 2], R[..., 0, 0]) - z_angle = torch.atan2(R[..., 1, 0], R[..., 1, 1]) - xyz = torch.stack([x_angle, y_angle, z_angle], dim=-1) - axis = dof_axis.to(R.device, R.dtype) - for _ in range(xyz.dim() - 2): - axis = axis.unsqueeze(0) - axis = axis.expand(*xyz.shape[:-1], 3) - return (xyz * axis).sum(dim=-1) - - -def _hamming_smooth(signal: np.ndarray, half_width: int) -> np.ndarray: - """Edge-padded Hamming smoothing along the first axis.""" - if half_width <= 0 or signal.shape[0] <= 2: - return signal.copy() - signal = np.asarray(signal, dtype=np.float32) - pad_left = np.repeat(signal[:1], half_width, axis=0) - pad_right = np.repeat(signal[-1:], half_width, axis=0) - padded = np.concatenate([pad_left, signal, pad_right], axis=0) - kernel = np.hamming(2 * half_width + 1).astype(np.float32) - kernel /= kernel.sum() - flat = padded.reshape(padded.shape[0], -1) - out = np.empty((signal.shape[0], flat.shape[1]), dtype=np.float32) - for i in range(signal.shape[0]): - out[i] = (flat[i : i + kernel.shape[0]] * kernel[:, None]).sum(axis=0) - return out.reshape(signal.shape) - - -def _smooth_quat_wxyz(quat: np.ndarray, half_width: int) -> np.ndarray: - """Hamming smoothing for a wxyz quaternion sequence with sign continuity.""" - if half_width <= 0 or quat.shape[0] <= 2: - return quat.copy() - aligned = np.asarray(quat, dtype=np.float32).copy() - for i in range(1, aligned.shape[0]): - if float(np.dot(aligned[i - 1], aligned[i])) < 0.0: - aligned[i] *= -1.0 - smoothed = _hamming_smooth(aligned, half_width) - smoothed /= np.clip(np.linalg.norm(smoothed, axis=-1, keepdims=True), 1e-8, None) - return smoothed.astype(np.float32) - - -def _nlerp_quat_wxyz(q0: np.ndarray, q1: np.ndarray, alpha: np.ndarray) -> np.ndarray: - """Sign-corrected normalized linear quaternion interpolation.""" - q0 = np.asarray(q0, dtype=np.float32) - q1 = np.asarray(q1, dtype=np.float32).copy() - flip = np.sum(q0 * q1, axis=-1, keepdims=True) < 0.0 - q1 = np.where(flip, -q1, q1) - out = q0 * (1.0 - alpha) + q1 * alpha - out /= np.clip(np.linalg.norm(out, axis=-1, keepdims=True), 1e-8, None) - return out.astype(np.float32) - - -def _smooth_g1_qpos_wxyz( - qpos: np.ndarray, - *, - pos_width: int = 3, - quat_width: int = 3, - joint_width: int = 2, -) -> np.ndarray: - """Smooth a G1 qpos trajectory with wxyz root quaternion.""" - out = np.asarray(qpos, dtype=np.float32).copy() - out[:, :3] = _hamming_smooth(out[:, :3], pos_width) - out[:, 3:7] = _smooth_quat_wxyz(out[:, 3:7], quat_width) - out[:, 7:] = _hamming_smooth(out[:, 7:], joint_width) - return out - - -def chunk_boundary_centers(chunk_infos: list[dict[str, Any]] | None) -> list[int]: - """Frame indices at the centre of each chunk-overlap region.""" - if not chunk_infos: - return [] - centers: list[int] = [] - for prev, cur in zip(chunk_infos[:-1], chunk_infos[1:], strict=False): - overlap_start = int(cur["start_frame"]) - overlap_end = min(int(prev["end_frame"]), int(cur["end_frame"])) - if overlap_end > overlap_start: - centers.append(overlap_start + (overlap_end - overlap_start) // 2) - else: - centers.append(overlap_start) - return centers - - -def smooth_qpos_global(qpos: np.ndarray, *, nfpt: int) -> np.ndarray: - """Mild global Hamming smoothing for the entire qpos trajectory. - - Equivalent to the legacy planner's first ``smooth_qpos(...)`` pass - before targeted boundary smoothing. Half-widths are conservative so - natural motion is preserved. - """ - return _smooth_g1_qpos_wxyz( - qpos, pos_width=2, quat_width=2, joint_width=max(1, nfpt // 2) - ) - - -def smooth_qpos_at_boundaries( - qpos: np.ndarray, - boundary_centers: list[int], - *, - nfpt: int, -) -> np.ndarray: - """Apply Hamming smoothing locally around each chunk boundary. - - A Gaussian mask centred on each overlap midpoint blends smoothed - Hamming output with the raw qpos. Quaternions use sign-corrected NLerp; - positions and joints use linear blend. Half-widths match the legacy - planner's ``boundary_radius = 3 * nfpt`` for joints. - """ - if not boundary_centers: - return qpos.copy() - raw = np.asarray(qpos, dtype=np.float32) - boundary_radius = max(3 * nfpt, 6) - smooth = _smooth_g1_qpos_wxyz( - raw, - pos_width=max(boundary_radius // 2, 2), - quat_width=max(boundary_radius // 2, 2), - joint_width=boundary_radius, - ) - T = raw.shape[0] - frames = np.arange(T, dtype=np.float32) - mask = np.zeros((T, 1), dtype=np.float32) - sigma = max(float(nfpt) * 1.5, 1.0) - for center in boundary_centers: - gaussian = np.exp(-0.5 * ((frames - float(center)) / sigma) ** 2) - mask = np.maximum(mask, gaussian.reshape(T, 1).astype(np.float32)) - out = raw.copy() - out[:, :3] = raw[:, :3] * (1.0 - mask) + smooth[:, :3] * mask - out[:, 7:] = raw[:, 7:] * (1.0 - mask) + smooth[:, 7:] * mask - out[:, 3:7] = _nlerp_quat_wxyz(raw[:, 3:7], smooth[:, 3:7], mask) - return out.astype(np.float32) - - -def features_to_qpos( - pred_joints: np.ndarray, - pred_root_xy: np.ndarray, - kin: dict[str, Any], - body_reorder: list[int], -) -> np.ndarray: - """Reconstruct G1 MuJoCo qpos from decoded body transforms + root XY. - - Args: - pred_joints: ``(F, num_bodies * 9)`` packed body transforms in - IsaacLab body order — per body: ``[pos(3), rot6d(6)]``. - pred_root_xy: ``(F, 2)`` predicted root XY in the same heading - frame. - kin: Kinematics dict with ``num_bodies``, ``num_dof``, - ``dof_axis``, ``parents``, ``local_rotation_mat``. - body_reorder: 30-int IsaacLab → MuJoCo body index permutation. - - Returns: - ``(F, 7 + num_dof)`` MuJoCo qpos with root quaternion in wxyz - order. - """ - F = min(pred_joints.shape[0], pred_root_xy.shape[0]) - num_bodies = kin["num_bodies"] - pj = torch.from_numpy(np.asarray(pred_joints[:F], dtype=np.float32)) - pj = pj.reshape(F, num_bodies, 9) - - reorder = body_reorder[:num_bodies] - pj = pj[:, reorder] - - body_pos = pj[..., :3].clone() - body_rot = _rot6d_to_matrix(pj[..., 3:9]) - - rxy = torch.from_numpy(np.asarray(pred_root_xy[:F], dtype=np.float32)) - body_pos[..., 0] += rxy[:, None, 0] - body_pos[..., 1] += rxy[:, None, 1] - - parents = kin["parents"].to(body_rot.dtype) - local_rots = _global_to_local_rotations(body_rot, parents) - - root_mat_np = local_rots[..., 0, :, :].cpu().numpy() - root_quat_xyzw = Rotation.from_matrix(root_mat_np.reshape(-1, 3, 3)).as_quat() - root_quat_wxyz = root_quat_xyzw[:, [3, 0, 1, 2]].reshape(F, 4) - - local_rot_mat = kin["local_rotation_mat"].to(local_rots.device, local_rots.dtype) - joint_rot_mat = torch.matmul( - local_rot_mat[:, 1:num_bodies].transpose(-1, -2), - local_rots[..., 1:num_bodies, :, :], - ) - dof_angles = _rotation_matrices_to_dof(joint_rot_mat, kin["dof_axis"]) - dof_angles = dof_angles.squeeze(0).cpu().numpy() - - qpos = np.zeros((F, 7 + kin["num_dof"]), dtype=np.float32) - qpos[:, :3] = body_pos[:, 0].cpu().numpy() - qpos[:, 3:7] = root_quat_wxyz - qpos[:, 7:] = dof_angles - return qpos diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/trajectory.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/trajectory.py deleted file mode 100644 index 3b1b92c0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/trajectory.py +++ /dev/null @@ -1,144 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Trajectory building for planner warmup and reference playback. - -Builds a 4-part trajectory: hold nominal → interpolate → hold start → reference. -""" - -from __future__ import annotations - -import numpy as np -from scipy.spatial.transform import Rotation, Slerp - - -def build_interp_trajectory( - nominal_ee: dict, - ref_left_pos: np.ndarray, - ref_left_quat: np.ndarray, - ref_right_pos: np.ndarray, - ref_right_quat: np.ndarray, - fps: float, - hold_start_s: float = 5.0, - interp_s: float = 5.0, - hold_end_s: float = 5.0, - n_ref: int = -1, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, dict]: - """Build a warmup + reference trajectory for the planner. - - Segments: - 1. Hold nominal pose for hold_start_s seconds - 2. SLERP/linear interpolate from nominal to reference[0] over interp_s - 3. Hold reference[0] for hold_end_s seconds - 4. Append reference data (first n_ref frames, or all if n_ref=-1) - - Args: - nominal_ee: Dict with left_pos, left_quat, right_pos, right_quat (wxyz). - ref_left_pos: (T, 3) reference left wrist positions. - ref_left_quat: (T, 4) reference left wrist quaternions (wxyz). - ref_right_pos: (T, 3) reference right wrist positions. - ref_right_quat: (T, 4) reference right wrist quaternions (wxyz). - fps: Output frame rate. - hold_start_s: Duration of nominal hold. - interp_s: Duration of interpolation. - hold_end_s: Duration of start-pose hold. - n_ref: Number of reference frames to include (-1 = all). - - Returns: - (left_pos, left_quat, right_pos, right_quat, segment_info) - Each is a concatenated (N_total, ...) array. - segment_info: dict with segment lengths and ref_start index. - """ - n_hold_start = int(hold_start_s * fps) - n_interp = int(interp_s * fps) - n_hold_end = int(hold_end_s * fps) - if n_ref < 0: - n_ref = len(ref_left_pos) - else: - n_ref = min(n_ref, len(ref_left_pos)) - - # Nominal pose (single frame) - nom_lp = np.array(nominal_ee["left_pos"], dtype=np.float32) - nom_lq = np.array(nominal_ee["left_quat"], dtype=np.float32) - nom_rp = np.array(nominal_ee["right_pos"], dtype=np.float32) - nom_rq = np.array(nominal_ee["right_quat"], dtype=np.float32) - - # Reference start (frame 0) - tgt_lp = ref_left_pos[0].astype(np.float32) - tgt_lq = ref_left_quat[0].astype(np.float32) - tgt_rp = ref_right_pos[0].astype(np.float32) - tgt_rq = ref_right_quat[0].astype(np.float32) - - parts_lp, parts_lq, parts_rp, parts_rq = [], [], [], [] - - # Segment 1: hold nominal - if n_hold_start > 0: - parts_lp.append(np.tile(nom_lp, (n_hold_start, 1))) - parts_lq.append(np.tile(nom_lq, (n_hold_start, 1))) - parts_rp.append(np.tile(nom_rp, (n_hold_start, 1))) - parts_rq.append(np.tile(nom_rq, (n_hold_start, 1))) - - # Segment 2: interpolate nominal → reference[0] - if n_interp > 0: - alpha = np.linspace(0.0, 1.0, n_interp, dtype=np.float32) - - # Linear position interpolation - parts_lp.append(nom_lp[None] + alpha[:, None] * (tgt_lp - nom_lp)[None]) - parts_rp.append(nom_rp[None] + alpha[:, None] * (tgt_rp - nom_rp)[None]) - - # SLERP quaternion interpolation - parts_lq.append(_slerp_array(nom_lq, tgt_lq, alpha)) - parts_rq.append(_slerp_array(nom_rq, tgt_rq, alpha)) - - # Segment 3: hold reference start - if n_hold_end > 0: - parts_lp.append(np.tile(tgt_lp, (n_hold_end, 1))) - parts_lq.append(np.tile(tgt_lq, (n_hold_end, 1))) - parts_rp.append(np.tile(tgt_rp, (n_hold_end, 1))) - parts_rq.append(np.tile(tgt_rq, (n_hold_end, 1))) - - # Segment 4: reference data - ref_start = n_hold_start + n_interp + n_hold_end - parts_lp.append(ref_left_pos[:n_ref].astype(np.float32)) - parts_lq.append(ref_left_quat[:n_ref].astype(np.float32)) - parts_rp.append(ref_right_pos[:n_ref].astype(np.float32)) - parts_rq.append(ref_right_quat[:n_ref].astype(np.float32)) - - seg_info = { - "n_hold_start": n_hold_start, - "n_interp": n_interp, - "n_hold_end": n_hold_end, - "n_ref": n_ref, - "ref_start": ref_start, - } - - return ( - np.concatenate(parts_lp), - np.concatenate(parts_lq), - np.concatenate(parts_rp), - np.concatenate(parts_rq), - seg_info, - ) - - -def _slerp_array( - q0_wxyz: np.ndarray, - q1_wxyz: np.ndarray, - alpha: np.ndarray, -) -> np.ndarray: - """SLERP between two wxyz quaternions at multiple alpha values. - - Args: - q0_wxyz: (4,) start quaternion, wxyz. - q1_wxyz: (4,) end quaternion, wxyz. - alpha: (N,) interpolation weights in [0, 1]. - - Returns: - (N, 4) interpolated quaternions, wxyz. - """ - q0_xyzw = q0_wxyz[[1, 2, 3, 0]] - q1_xyzw = q1_wxyz[[1, 2, 3, 0]] - key_rots = Rotation.from_quat([q0_xyzw, q1_xyzw]) - slerp = Slerp([0.0, 1.0], key_rots) - interp_rots = slerp(alpha) - out_xyzw = interp_rots.as_quat() - return out_xyzw[:, [3, 0, 1, 2]].astype(np.float32) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/motion.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/motion.py deleted file mode 100644 index 45750395..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/motion.py +++ /dev/null @@ -1,413 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""V2P retargeted-motion manipulation: input resampling + output assembly. - -`interpolate_robot_motion_data` is the input side: takes a -`ManoDex3Data` / `ManoSharpaData` dataclass and resamples every -time-series field to the planner's target FPS (linear for positions / -joint angles / object articulation, SLERP for quats, contact-aware -masked interp for contact fields). - -`assemble_object_fields` and `assemble_hand_contact_fields` are the -output side: read the same motion dataclass plus the planner-frame -object body arrays, and build the per-field dicts that -`save_planner_parquet` assembles into a `MotionData` row. They apply -the per-frame V2P→planner rigid transform to hand keypoints and -contacts so every field of the output parquet lives in one coherent -frame. -""" - -from __future__ import annotations - -from collections.abc import Callable -from typing import Any - -import numpy as np -from scipy.interpolate import interp1d -from scipy.spatial.transform import Rotation, Slerp - -from robotic_grounding.planner.utils.transforms import ( - quat_conj, - quat_mul, - transform_contact_dir_by_part, - transform_contact_pos_by_part, - transform_primary_pos, - transform_primary_quat, -) - - -def interpolate_robot_motion_data(motion_data: Any, target_fps: float) -> Any: - """Resample a V2P retargeted motion's time-series fields to ``target_fps``. - - Linear interpolation for positions / joint angles / object articulation, - SLERP for wrist + per-body object quaternions, and contact-aware linear - interpolation that zeroes intervals where either neighbour is inactive. - """ - n_frames = len(motion_data.robot_right_wrist_position) - src_times = np.arange(n_frames) / motion_data.fps - tgt_times = np.linspace(0, src_times[-1], int(src_times[-1] * target_fps)) - - def _linear(data: Any) -> Any: - return interp1d(src_times, np.asarray(data), kind="linear", axis=0)( - tgt_times - ).tolist() - - def _slerp(quat_data: Any) -> Any: - quats = np.asarray(quat_data) - return ( - Slerp(src_times, Rotation.from_quat(quats, scalar_first=True))(tgt_times) - .as_quat(scalar_first=True) - .tolist() - ) - - def _slerp_batch(quat_data: Any) -> Any: - arr = np.asarray(quat_data) - results = [_slerp(arr[:, i, :]) for i in range(arr.shape[1])] - return np.array(results).transpose(1, 0, 2).tolist() - - def _frames(frame_data: Any) -> Any: - arr = np.asarray(frame_data) - pos = np.array(_linear(arr[:, :, :3])) - rot = np.array(_slerp_batch(arr[:, :, 3:])) - return np.concatenate([pos, rot], axis=2).tolist() - - def _contact_linear(data: Any, part_ids: Any) -> Any: - if not data: - return [] - H, N = len(data), len(data[0]) - arr = np.concatenate( - [np.asarray(data), np.asarray(part_ids).reshape(H, N, 1)], axis=-1 - ) - interp_result = interp1d(src_times, arr, kind="linear", axis=0)(tgt_times) - nonzero_src = np.abs(arr) > 1e-8 - idx_lo = np.clip( - np.searchsorted(src_times, tgt_times, side="right") - 1, - 0, - len(src_times) - 1, - ) - idx_hi = np.minimum(idx_lo + 1, len(src_times) - 1) - mask = nonzero_src[idx_lo] & nonzero_src[idx_hi] - return np.where(mask, interp_result, 0.0).tolist() - - motion_data.object_articulation = _linear(motion_data.object_articulation) - motion_data.robot_right_finger_joints = _linear( - motion_data.robot_right_finger_joints - ) - motion_data.robot_left_finger_joints = _linear(motion_data.robot_left_finger_joints) - motion_data.robot_right_wrist_position = _linear( - motion_data.robot_right_wrist_position - ) - motion_data.robot_left_wrist_position = _linear( - motion_data.robot_left_wrist_position - ) - motion_data.robot_right_wrist_wxyz = _slerp(motion_data.robot_right_wrist_wxyz) - motion_data.robot_left_wrist_wxyz = _slerp(motion_data.robot_left_wrist_wxyz) - motion_data.object_body_position = _linear(motion_data.object_body_position) - motion_data.object_body_wxyz = _slerp_batch(motion_data.object_body_wxyz) - motion_data.robot_right_frames = _frames(motion_data.robot_right_frames) - motion_data.robot_left_frames = _frames(motion_data.robot_left_frames) - - for side in ("right", "left"): - part_ids = getattr(motion_data, f"mano_{side}_object_contact_part_ids", []) - for field in ( - "link_contact_positions", - "object_contact_positions", - "object_contact_normals", - ): - attr = f"mano_{side}_{field}" - val = getattr(motion_data, attr, []) - if val: - setattr(motion_data, attr, _contact_linear(val, part_ids)) - - return motion_data - - -def assemble_object_fields( - motion: Any | None, - obj_body_pos_arr: np.ndarray, - obj_body_wxyz_arr: np.ndarray, - T_use: int, - fallback_object_name: str, - resolve_asset_path: Callable[[str | None], str | None], - warn_missing_deps: Callable[[list[str]], None], -) -> dict[str, Any]: - """Build object-side fields of the planner's output parquet. - - ``obj_body_pos_arr`` / ``obj_body_wxyz_arr`` are the planner-frame - object body arrays sliced to ``T_use`` (caller has already pulled - them from ref_data and converted). ``motion`` is the upstream - ManoSharpaData/ManoDex3Data dataclass; metadata fields are carried - through, missing ones fall back to sensible defaults. Asset paths - are remapped via ``resolve_asset_path``, and URDF mesh dependencies - surfaced via ``warn_missing_deps``. - - ``object_root_*`` always derives from body 0 of the planner-frame - pose so the env's articulated scene init lands where the trajectory - starts, regardless of whether the upstream motion carried a - separately-resampled root field. - """ - object_root_position: list = obj_body_pos_arr[:, 0, :].astype(np.float32).tolist() - object_root_axis_angle: list = ( - Rotation.from_quat(obj_body_wxyz_arr[:, 0, :], scalar_first=True) - .as_rotvec() - .astype(np.float32) - .tolist() - ) - - object_name = str(fallback_object_name) - object_body_names: list[str] = ["object"] - safe_object_body_names: list[str] = ["object"] - object_mesh_paths: list[str] = [] - object_urdf_paths: list[str] = [] - object_mesh_radius: list[float] | None = None - object_articulation: list[float] = [0.0] * T_use - safe_object_name = object_name - - if motion is not None: - if getattr(motion, "object_body_names", None): - object_body_names = list(motion.object_body_names) - if getattr(motion, "safe_object_body_names", None): - safe_object_body_names = list(motion.safe_object_body_names) - if getattr(motion, "object_mesh_paths", None): - object_mesh_paths = [ - str(resolved) - for p in motion.object_mesh_paths - if (resolved := resolve_asset_path(p)) - ] - if getattr(motion, "object_urdf_paths", None): - object_urdf_paths = [ - str(resolved) - for p in motion.object_urdf_paths - if (resolved := resolve_asset_path(p)) - ] - # The URDF can resolve locally while its visual - # or collision dependencies don't. Surface those gaps now so the - # user fixes the workspace before training hits the import crash. - warn_missing_deps(object_urdf_paths) - if getattr(motion, "object_mesh_radius", None): - object_mesh_radius = [float(r) for r in motion.object_mesh_radius] - if getattr(motion, "safe_object_name", None): - safe_object_name = motion.safe_object_name - obj_art = getattr(motion, "object_articulation", None) - if obj_art is not None: - object_articulation = np.asarray(obj_art, dtype=np.float32)[:T_use].tolist() - - return { - "object_name": object_name, - "safe_object_name": safe_object_name, - "object_body_names": object_body_names, - "safe_object_body_names": safe_object_body_names, - "object_mesh_paths": object_mesh_paths, - "object_urdf_paths": object_urdf_paths, - "object_mesh_radius": object_mesh_radius, - "object_articulation": object_articulation, - "object_root_position": object_root_position, - "object_root_axis_angle": object_root_axis_angle, - } - - -def assemble_hand_contact_fields( - motion: Any | None, - obj_body_pos_arr: np.ndarray, - obj_body_wxyz_arr: np.ndarray, - T_use: int, -) -> dict[str, Any]: - """Build per-side hand-frame + contact fields in the planner frame. - - Builds the per-frame V2P→planner rigid transform anchored on the - primary object body using ``motion.object_body_*`` as the raw source - and the supplied planner-frame arrays as the destination, then - applies it to ``robot_*_frames`` (primary-body transform) and to the - four contact arrays (per-body transform via part_ids). Re-derives - ``hand_contact_active`` from object-contact-position magnitudes. - - Returns empty per-side lists when ``motion`` is ``None`` or a side - has no wrist position; callers should still emit those empty lists - so the output parquet's per-side fields stay length-2. - """ - out: dict[str, Any] = { - "hand_sides": [], - "hand_frame_names": [], - "hand_frames_w": [], - "hand_finger_joint_names": [], - "hand_finger_joints": [], - "hand_contact_link_names": [], - "hand_link_contact_positions": [], - "hand_link_contact_normals": [], - "hand_object_contact_positions": [], - "hand_object_contact_normals": [], - "hand_object_contact_part_ids": [], - "hand_contact_active": [], - } - if motion is None: - return out - - raw_obj_pos_all = np.asarray(motion.object_body_position, dtype=np.float32)[:T_use] - raw_obj_quat_all = np.asarray(motion.object_body_wxyz, dtype=np.float32)[:T_use] - if raw_obj_pos_all.ndim == 2: - raw_obj_pos_all = raw_obj_pos_all[:, None] - raw_obj_quat_all = raw_obj_quat_all[:, None] - - # Length-align in case a per-frame field was skipped during trimming. - common_T = min(raw_obj_pos_all.shape[0], obj_body_pos_arr.shape[0]) - raw_obj_pos_all = raw_obj_pos_all[:common_T] - raw_obj_quat_all = raw_obj_quat_all[:common_T] - dst_obj_pos_all = obj_body_pos_arr[:common_T] - dst_obj_quat_all = obj_body_wxyz_arr[:common_T] - primary_r_rel = quat_mul(dst_obj_quat_all[:, 0], quat_conj(raw_obj_quat_all[:, 0])) - raw_primary_pos = raw_obj_pos_all[:, 0] - dst_primary_pos = dst_obj_pos_all[:, 0] - - for side in ("left", "right"): - wrist_pos = getattr(motion, f"robot_{side}_wrist_position", None) - if wrist_pos is None: - continue - out["hand_sides"].append(side) - frames = getattr(motion, f"robot_{side}_frames", None) or [] - frame_names = getattr(motion, f"{side}_robot_frame_names", None) or [] - finger_joints = getattr(motion, f"robot_{side}_finger_joints", None) or [] - finger_joint_names = ( - getattr(motion, f"{side}_robot_finger_joint_names", None) or [] - ) - link_contacts = ( - getattr(motion, f"mano_{side}_link_contact_positions", None) or [] - ) - link_normals = getattr(motion, f"mano_{side}_link_contact_normals", None) or [] - obj_contacts = ( - getattr(motion, f"mano_{side}_object_contact_positions", None) or [] - ) - obj_normals = getattr(motion, f"mano_{side}_object_contact_normals", None) or [] - part_ids_attr = ( - getattr(motion, f"mano_{side}_object_contact_part_ids", None) or [] - ) - - out["hand_frame_names"].append(list(frame_names)) - # Lift hand_frames_w into the planner frame. The consumer in - # tracking_command._precompute_hand_keypoints_in_object_frame - # combines these keypoint poses with object_body_position to build - # the wrist/fingertip targets; passing through V2P-frame keypoints - # against a planner-frame object pose silently produces targets up - # to a metre off. - if frames: - frames_arr = np.asarray(frames, dtype=np.float32)[:common_T] - frame_pos = transform_primary_pos( - frames_arr[..., :3], - raw_primary_pos, - dst_primary_pos, - primary_r_rel, - ) - frame_quat = transform_primary_quat(frames_arr[..., 3:], primary_r_rel) - out["hand_frames_w"].append( - np.concatenate([frame_pos, frame_quat], axis=-1).tolist() - ) - else: - out["hand_frames_w"].append([]) - - out["hand_finger_joint_names"].append(list(finger_joint_names)) - out["hand_finger_joints"].append( - np.asarray(finger_joints, dtype=np.float32)[:T_use].tolist() - if finger_joints - else [] - ) - out["hand_contact_link_names"].append([]) - - # part_ids are 1-indexed object-body indices that drive the per-body - # contact transform. Some loaders carry them in a dedicated array - # (often left at source fps), others embed them in the 4th column of - # the contact-position arrays (already at planner fps). Probe both - # and nearest-neighbor upsample the dedicated array if it hasn't - # been interpolated. - part_ids_arr: np.ndarray | None = None - if len(part_ids_attr): - src = np.asarray(part_ids_attr, dtype=np.int64) - if src.shape[0] >= common_T: - part_ids_arr = src[:common_T] - elif src.shape[0] > 0: - src_t = np.linspace(0.0, 1.0, src.shape[0]) - dst_t = np.linspace(0.0, 1.0, common_T) - nn_idx = np.clip( - np.searchsorted(src_t, dst_t, side="right") - 1, - 0, - src.shape[0] - 1, - ) - part_ids_arr = src[nn_idx] - if part_ids_arr is None and obj_contacts: - oc_probe = np.asarray(obj_contacts, dtype=np.float32) - if oc_probe.ndim == 3 and oc_probe.shape[-1] >= 4: - part_ids_arr = np.rint(oc_probe[:common_T, :, 3]).astype(np.int64) - inactive = np.linalg.norm(oc_probe[:common_T, :, :3], axis=-1) <= 1e-8 - part_ids_arr = np.where(inactive, 0, part_ids_arr) - - # Contacts ride the same per-body rigid transform as the object - # bodies they reference. Positions translate + rotate, normals - # rotate only. - if obj_contacts: - oc_a = np.asarray(obj_contacts, dtype=np.float32)[:common_T, :, :3] - oc_transformed = transform_contact_pos_by_part( - oc_a, - raw_obj_pos_all, - dst_obj_pos_all, - raw_obj_quat_all, - dst_obj_quat_all, - part_ids_arr, - ) - out["hand_object_contact_positions"].append(oc_transformed.tolist()) - else: - out["hand_object_contact_positions"].append([]) - - if obj_normals: - on_a = np.asarray(obj_normals, dtype=np.float32)[:common_T, :, :3] - on_transformed = transform_contact_dir_by_part( - on_a, - raw_obj_quat_all, - dst_obj_quat_all, - part_ids_arr, - ) - out["hand_object_contact_normals"].append(on_transformed.tolist()) - else: - out["hand_object_contact_normals"].append([]) - - if link_contacts: - lc_a = np.asarray(link_contacts, dtype=np.float32)[:common_T, :, :3] - lc_transformed = transform_contact_pos_by_part( - lc_a, - raw_obj_pos_all, - dst_obj_pos_all, - raw_obj_quat_all, - dst_obj_quat_all, - part_ids_arr, - ) - out["hand_link_contact_positions"].append(lc_transformed.tolist()) - else: - out["hand_link_contact_positions"].append([]) - - if link_normals: - ln_a = np.asarray(link_normals, dtype=np.float32)[:common_T, :, :3] - ln_transformed = transform_contact_dir_by_part( - ln_a, - raw_obj_quat_all, - dst_obj_quat_all, - part_ids_arr, - ) - out["hand_link_contact_normals"].append(ln_transformed.tolist()) - else: - out["hand_link_contact_normals"].append([]) - - out["hand_object_contact_part_ids"].append( - part_ids_arr.tolist() if part_ids_arr is not None else [] - ) - - # Per-frame contact-active mask: 1 when at least one contact point - # is recorded against the object, 0 otherwise. Derived from the - # already-upsampled `obj_contacts` so the mask length matches the - # other per-frame contact arrays. tracking_command refuses to load - # motion files where both sides are absent, so always emit a - # per-side mask (zero-filled in the worst case). - if obj_contacts: - cp = np.asarray(obj_contacts, dtype=np.float32)[:common_T, :, :3] - active = (np.abs(cp).sum(axis=-1) > 1e-5).any(axis=-1).astype(np.float32) - else: - active = np.zeros((common_T,), dtype=np.float32) - out["hand_contact_active"].append(active.tolist()) - - return out diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/qpos.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/qpos.py deleted file mode 100644 index 14d060bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/qpos.py +++ /dev/null @@ -1,232 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MuJoCo-side qpos assembly + scoring for the G1 whole-body planner. - -Owns the constants and helpers that turn the model's chunked-AR output -(`planned_qpos`) plus per-side V2P finger data into a single -`(T, model.nq)` qpos trajectory: body-joint name → qpos-index mapping, -finger-joint resolution, root-component pinning, and an FK-based wrist -tracking error used to score heading-offset candidates. -""" - -from __future__ import annotations - -from typing import Any - -import mujoco -import numpy as np -from scipy.spatial.transform import Rotation - -from robotic_grounding.planner.utils.transforms import xyzw_to_wxyz - -# -- Constants --------------------------------------------------------------- - -ROOT_HEIGHT = 0.793 -ROOT_FIX_COMPONENTS = ("x", "y", "z", "roll", "pitch", "yaw") - -LEG_OVERRIDES = { - "left_hip_pitch_joint": -0.1, - "left_hip_roll_joint": 0.0, - "left_hip_yaw_joint": 0.0, - "left_knee_joint": 0.4, - "left_ankle_pitch_joint": -0.2, - "left_ankle_roll_joint": 0.0, - "right_hip_pitch_joint": -0.1, - "right_hip_roll_joint": 0.0, - "right_hip_yaw_joint": 0.0, - "right_knee_joint": 0.4, - "right_ankle_pitch_joint": -0.2, - "right_ankle_roll_joint": 0.0, -} - -LEFT_WRIST = "left_wrist_yaw_link" -RIGHT_WRIST = "right_wrist_yaw_link" - -G1_BODY_JOINT_NAMES = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", -] - - -# -- Mappings ---------------------------------------------------------------- - - -def build_body_joint_mapping(model: mujoco.MjModel) -> dict[int, int]: - """Map 29 body DOFs to combined model qpos indices.""" - mapping = {} - for dof_idx, jname in enumerate(G1_BODY_JOINT_NAMES): - jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, jname) - if jid >= 0: - mapping[dof_idx] = int(model.jnt_qposadr[jid]) - return mapping - - -def build_finger_mapping(model: mujoco.MjModel, joint_names: list[str]) -> list[int]: - """Map finger joint names to model qpos indices.""" - result = [] - for jname in joint_names: - try: - result.append(int(model.jnt_qposadr[model.joint(jname).id])) - except Exception: - result.append(-1) - return result - - -# -- Wrist tracking error ---------------------------------------------------- - - -def wrist_ee_error_from_qpos( - full_qpos: np.ndarray, - ref_data: dict, - model: mujoco.MjModel, -) -> float: - """Mean L2 distance from FK'd wrist_yaw_link bodies to V2P wrist targets. - - Used to score heading-offset candidates during the local search. - """ - data = mujoco.MjData(model) - li = model.body(LEFT_WRIST).id - ri = model.body(RIGHT_WRIST).id - target_l = np.asarray(ref_data["left_pos"], dtype=np.float64) - target_r = np.asarray(ref_data["right_pos"], dtype=np.float64) - T = min(full_qpos.shape[0], target_l.shape[0], target_r.shape[0]) - err_l = np.empty(T, dtype=np.float64) - err_r = np.empty(T, dtype=np.float64) - for t in range(T): - data.qpos[: full_qpos.shape[1]] = full_qpos[t] - mujoco.mj_forward(model, data) - err_l[t] = np.linalg.norm(data.xpos[li] - target_l[t]) - err_r[t] = np.linalg.norm(data.xpos[ri] - target_r[t]) - return float(((err_l + err_r) / 2.0).mean()) - - -# -- Root pinning ------------------------------------------------------------ - - -def root_fix_component_set( - components: tuple[str, ...] | list[str] = (), - *, - fix_root_pos: bool = False, - fix_root_rot: bool = False, - fix_root_z: bool = False, - fix_root_rp: bool = False, -) -> set[str]: - """Merge the generalized root component list with legacy root flags.""" - result = set(components or ()) - invalid = result.difference(ROOT_FIX_COMPONENTS) - if invalid: - raise ValueError(f"Unknown root fix component(s): {sorted(invalid)}") - if fix_root_pos: - result.update(("x", "y", "z")) - if fix_root_z: - result.add("z") - if fix_root_rot: - result.update(("roll", "pitch", "yaw")) - if fix_root_rp: - result.update(("roll", "pitch")) - return result - - -def root_wxyz_with_fixed_components( - q_xyzw: np.ndarray, fixed_components: set[str] -) -> np.ndarray: - """Apply roll/pitch/yaw root clamps while preserving free components.""" - if not fixed_components.intersection(("roll", "pitch", "yaw")): - return xyzw_to_wxyz(q_xyzw) - - euler_xyz = Rotation.from_quat(q_xyzw).as_euler("xyz", degrees=False) - if "roll" in fixed_components: - euler_xyz[0] = 0.0 - if "pitch" in fixed_components: - euler_xyz[1] = 0.0 - if "yaw" in fixed_components: - euler_xyz[2] = 0.0 - q_fixed_xyzw = Rotation.from_euler("xyz", euler_xyz).as_quat() - return xyzw_to_wxyz(q_fixed_xyzw).astype(np.float32) - - -# -- Full qpos assembly ------------------------------------------------------ - - -def build_full_qpos( - planned_qpos: np.ndarray, - ref_data: dict[str, Any], - model: mujoco.MjModel, - T_save: int, - fix_lower_body: bool = False, - fix_root_pos: bool = False, - fix_root_rot: bool = False, - fix_root_z: bool = False, - fix_root_rp: bool = False, - fix_root_components: tuple[str, ...] | list[str] = (), -) -> tuple[np.ndarray, dict[int, int], list[int], list[int]]: - """Combine planned body + reference fingers + (optionally fixed) parts.""" - nq = model.nq - full_qpos = np.zeros((T_save, nq), dtype=np.float32) - body_map = build_body_joint_mapping(model) - l_finger_map = build_finger_mapping(model, ref_data.get("left_joint_names", [])) - r_finger_map = build_finger_mapping(model, ref_data.get("right_joint_names", [])) - fixed_root = root_fix_component_set( - fix_root_components, - fix_root_pos=fix_root_pos, - fix_root_rot=fix_root_rot, - fix_root_z=fix_root_z, - fix_root_rp=fix_root_rp, - ) - - for t in range(T_save): - t_plan = min(t, planned_qpos.shape[0] - 1) - full_qpos[t, :3] = planned_qpos[t_plan, :3] - if "x" in fixed_root: - full_qpos[t, 0] = 0.0 - if "y" in fixed_root: - full_qpos[t, 1] = 0.0 - if "z" in fixed_root: - full_qpos[t, 2] = ROOT_HEIGHT - full_qpos[t, 3:7] = root_wxyz_with_fixed_components( - planned_qpos[t_plan, 3:7], fixed_root - ) - for dof_idx, qi in body_map.items(): - full_qpos[t, qi] = planned_qpos[t_plan, 7 + dof_idx] - if fix_lower_body: - for jname, val in LEG_OVERRIDES.items(): - try: - full_qpos[t, int(model.jnt_qposadr[model.joint(jname).id])] = val - except Exception: - pass - f_idx = min(t, ref_data["left_finger_joints"].shape[0] - 1) - for j, qi in enumerate(l_finger_map): - if qi >= 0 and j < ref_data["left_finger_joints"].shape[1]: - full_qpos[t, qi] = ref_data["left_finger_joints"][f_idx, j] - for j, qi in enumerate(r_finger_map): - if qi >= 0 and j < ref_data["right_finger_joints"].shape[1]: - full_qpos[t, qi] = ref_data["right_finger_joints"][f_idx, j] - - return full_qpos, body_map, l_finger_map, r_finger_map diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/transforms.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/transforms.py deleted file mode 100644 index 2f710f06..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/transforms.py +++ /dev/null @@ -1,418 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Coordinate transforms for the G1 whole-body planner. - -Two layers coexist here: - -- High-level pipeline (`transform_reference` and the `apply_*` helpers) maps a - V2P retargeted reference into the planner workspace frame: local frame fix, - heading-toward-object yaw, position alignment with the nominal EE midpoint, - and an optional workspace offset. -- Low-level rigid-transform primitives (`quat_*`, `transform_primary_*`, - `transform_contact_*_by_part`) reproduce the same per-frame rigid transform - for arrays the pipeline doesn't itself touch (hand keypoints, per-body - contact positions/normals), so downstream consumers see a single coherent - frame across every field of the output parquet. - -Quaternions use the wxyz convention throughout, matching the on-disk motion -schema. -""" - -from __future__ import annotations - -import numpy as np -from scipy.spatial.transform import Rotation - -# -- Sharpa → G1 frame rotation matrices ------------------------------------------- -# Sharpa hand_C_MC body frame: -# Left: X=right, Y=down, Z=forward -# Right: X=left, Y=up, Z=forward -# G1 wrist_yaw_link frame (both hands): -# X=forward, Y=left, Z=up - -_R_S2G_LEFT = Rotation.from_matrix( - [ - [0, 0, 1], # G1 X (fwd) = Sharpa Z (fwd) - [-1, 0, 0], # G1 Y (left) = -Sharpa X (right) - [0, -1, 0], # G1 Z (up) = -Sharpa Y (down) - ] -) - -_R_S2G_RIGHT = Rotation.from_matrix( - [ - [0, 0, 1], # G1 X (fwd) = Sharpa Z (fwd) - [1, 0, 0], # G1 Y (left) = Sharpa X (left) - [0, 1, 0], # G1 Z (up) = Sharpa Y (up) - ] -) - -# Data conversion: post-multiply by R_s2g.inv() -_R_LOCAL_FIX_LEFT = _R_S2G_LEFT.inv() -_R_LOCAL_FIX_RIGHT = _R_S2G_RIGHT.inv() - - -# -- Quaternion conversions --------------------------------------------------- - - -def xyzw_to_wxyz(q: np.ndarray) -> np.ndarray: - """Permute a single quaternion or quaternion array from xyzw to wxyz.""" - q = np.asarray(q) - return np.concatenate([q[..., 3:4], q[..., :3]], axis=-1) - - -def wxyz_to_xyzw(q: np.ndarray) -> np.ndarray: - """Permute a single quaternion or quaternion array from wxyz to xyzw.""" - q = np.asarray(q) - return np.concatenate([q[..., 1:4], q[..., 0:1]], axis=-1) - - -def fix_quat_wxyz(q_wxyz: np.ndarray, r_fix: Rotation) -> np.ndarray: - """Post-multiply wxyz quaternion(s) by a rotation. - - Args: - q_wxyz: (T, 4) or (4,) quaternions in wxyz format. - r_fix: Rotation to post-multiply. - - Returns: - Corrected quaternions in wxyz format, same shape as input. - """ - squeeze = q_wxyz.ndim == 1 - if squeeze: - q_wxyz = q_wxyz[np.newaxis] - - # wxyz → xyzw for scipy - q_xyzw = q_wxyz[:, [1, 2, 3, 0]] - r_src = Rotation.from_quat(q_xyzw) - r_out = r_src * r_fix - out_xyzw = r_out.as_quat() - out_wxyz = out_xyzw[:, [3, 0, 1, 2]] - - return out_wxyz[0] if squeeze else out_wxyz - - -# -- Low-level rigid-transform primitives ------------------------------------- - - -def quat_conj(q: np.ndarray) -> np.ndarray: - """Return the conjugate of a wxyz quaternion (negate the vector part).""" - return np.stack([q[..., 0], -q[..., 1], -q[..., 2], -q[..., 3]], axis=-1) - - -def quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Hamilton product of two wxyz quaternions, broadcast over batch dims.""" - w1, x1, y1, z1 = q1[..., 0], q1[..., 1], q1[..., 2], q1[..., 3] - w2, x2, y2, z2 = q2[..., 0], q2[..., 1], q2[..., 2], q2[..., 3] - return np.stack( - [ - w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, - w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, - w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, - w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, - ], - axis=-1, - ) - - -def quat_rotate(q: np.ndarray, v: np.ndarray) -> np.ndarray: - """Rotate vector ``v`` by wxyz quaternion ``q`` (q v q*).""" - vq = np.concatenate([np.zeros_like(v[..., :1]), v], axis=-1) - return quat_mul(quat_mul(q, vq), quat_conj(q))[..., 1:] - - -def broadcast_time_axis( - arr: np.ndarray, shape_no_last: tuple[int, ...], last: int -) -> np.ndarray: - """Broadcast a (T, last) array against a leading (T, *inner) target shape.""" - expand = (1,) * (len(shape_no_last) - 1) - return np.broadcast_to( - arr.reshape(arr.shape[0], *expand, last), (*shape_no_last, last) - ) - - -def transform_primary_pos( - arr: np.ndarray, - raw_obj_pos: np.ndarray, - dst_obj_pos: np.ndarray, - r_rel: np.ndarray, -) -> np.ndarray: - """Rotate positions about the primary object body's per-frame frame change. - - ``arr`` has shape ``(T, ..., 3)``; the leading T axis is broadcast against - ``r_rel`` / ``raw_obj_pos`` / ``dst_obj_pos`` (all shape ``(T, 4)`` or - ``(T, 3)``). - """ - arr = np.asarray(arr, dtype=np.float32) - r = broadcast_time_axis(r_rel, arr.shape[:-1], 4) - raw_p = broadcast_time_axis(raw_obj_pos, arr.shape[:-1], 3) - dst_p = broadcast_time_axis(dst_obj_pos, arr.shape[:-1], 3) - return quat_rotate(r, arr - raw_p) + dst_p - - -def transform_primary_quat(arr: np.ndarray, r_rel: np.ndarray) -> np.ndarray: - """Rotate quaternions by the primary object body's per-frame frame change.""" - arr = np.asarray(arr, dtype=np.float32) - r = broadcast_time_axis(r_rel, arr.shape[:-1], 4) - return quat_mul(r, arr) - - -def transform_contact_pos_by_part( - arr: np.ndarray, - raw_obj_pos_all: np.ndarray, - dst_obj_pos_all: np.ndarray, - raw_obj_quat_all: np.ndarray, - dst_obj_quat_all: np.ndarray, - part_ids: np.ndarray | None, -) -> np.ndarray: - """Per-body rigid transform on ``(T, N, 3)`` contact positions. - - Each contact point uses its own body's pose change (rather than the - primary body's), which matters when the hand contacts a non-primary - body of a multi-body or articulated object. ``part_ids`` is 1-indexed - into the object's body list; entries equal to 0 are treated as - inactive and left untouched. - """ - out = np.asarray(arr, dtype=np.float32).copy() - if part_ids is None: - return out - num_bodies = raw_obj_pos_all.shape[1] - xyz = out[..., :3] - for body_idx in range(num_bodies): - mask = part_ids == body_idx + 1 - if not np.any(mask): - continue - t_idx, contact_idx = np.where(mask) - r_rel = quat_mul( - dst_obj_quat_all[t_idx, body_idx], - quat_conj(raw_obj_quat_all[t_idx, body_idx]), - ) - xyz[t_idx, contact_idx] = ( - quat_rotate( - r_rel, xyz[t_idx, contact_idx] - raw_obj_pos_all[t_idx, body_idx] - ) - + dst_obj_pos_all[t_idx, body_idx] - ) - out[..., :3] = xyz - return out - - -def transform_contact_dir_by_part( - arr: np.ndarray, - raw_obj_quat_all: np.ndarray, - dst_obj_quat_all: np.ndarray, - part_ids: np.ndarray | None, -) -> np.ndarray: - """Per-body rotation on ``(T, N, 3)`` contact normals — translation skipped. - - Direction-only counterpart to :func:`transform_contact_pos_by_part`. - Zero-length normals (contact-inactive frames) are left untouched. - """ - out = np.asarray(arr, dtype=np.float32).copy() - if part_ids is None: - return out - num_bodies = raw_obj_quat_all.shape[1] - xyz = out[..., :3] - for body_idx in range(num_bodies): - mask = (part_ids == body_idx + 1) & (np.linalg.norm(xyz, axis=-1) > 1e-8) - if not np.any(mask): - continue - t_idx, contact_idx = np.where(mask) - r_rel = quat_mul( - dst_obj_quat_all[t_idx, body_idx], - quat_conj(raw_obj_quat_all[t_idx, body_idx]), - ) - xyz[t_idx, contact_idx] = quat_rotate(r_rel, xyz[t_idx, contact_idx]) - out[..., :3] = xyz - return out - - -# -- High-level transform pipeline -------------------------------------------- - - -def apply_local_frame_fix( - data: dict, - robot_type: str = "sharpa", -) -> dict: - """Apply Sharpa→G1 local frame correction to wrist quaternions. - - Args: - data: Dict with keys left_quat, right_quat (wxyz), and positions. - robot_type: "sharpa" or "dex3". Dex3 is already aligned. - - Returns: - Copy of data with corrected quaternions. - """ - out = dict(data) - if robot_type == "sharpa": - out["left_quat"] = fix_quat_wxyz(data["left_quat"], _R_LOCAL_FIX_LEFT) - out["right_quat"] = fix_quat_wxyz(data["right_quat"], _R_LOCAL_FIX_RIGHT) - return out - - -def compute_heading_toward_object( - left_pos: np.ndarray, - right_pos: np.ndarray, - obj_pos: np.ndarray | None, - frame_index: int = 0, -) -> float: - """Compute heading angle from wrist midpoint toward the object. - - Falls back to hand-perpendicular heading if no object data. - - Args: - left_pos: (T, 3) left wrist positions. - right_pos: (T, 3) right wrist positions. - obj_pos: (T, 3) object positions, or None. - frame_index: Reference frame to read positions from (clipped to range). - - Returns: - Heading angle in radians. - """ - idx = int(np.clip(frame_index, 0, len(left_pos) - 1)) - mid = 0.5 * (left_pos[idx] + right_pos[idx]) - if obj_pos is not None and len(obj_pos) > 0: - obj_idx = int(np.clip(idx, 0, len(obj_pos) - 1)) - fwd = obj_pos[obj_idx, :2] - mid[:2] - if np.linalg.norm(fwd) > 1e-4: - return float(np.arctan2(fwd[1], fwd[0])) - - # Fallback: perpendicular to L→R vector - lr = right_pos[idx] - left_pos[idx] - return float(np.arctan2(-lr[0], lr[1])) - - -def apply_yaw_correction( - data: dict, - delta_yaw: float, - midpoint: np.ndarray | None = None, -) -> dict: - """Rotate all positions and quaternions by delta_yaw around Z axis. - - Args: - data: Dict with left_pos, left_quat, right_pos, right_quat, - and optionally object_pos, object_quat. - delta_yaw: Rotation angle in radians. - midpoint: Center of rotation in XY. If None, uses frame-0 wrist midpoint. - - Returns: - Copy of data with rotated values. - """ - if midpoint is None: - midpoint = 0.5 * (data["left_pos"][0] + data["right_pos"][0]) - midpoint = np.array(midpoint, dtype=np.float64) - - r_yaw = Rotation.from_euler("z", delta_yaw) - out = dict(data) - - for pos_key in ("left_pos", "right_pos", "object_pos"): - if pos_key in out and out[pos_key] is not None: - p = np.array(out[pos_key], dtype=np.float64) - p = r_yaw.apply(p - midpoint) + midpoint - out[pos_key] = p.astype(np.float32) - - for quat_key in ("left_quat", "right_quat", "object_quat"): - if quat_key in out and out[quat_key] is not None: - q = np.array(out[quat_key], dtype=np.float64) - q_xyzw = q[:, [1, 2, 3, 0]] - r_src = Rotation.from_quat(q_xyzw) - r_out = r_yaw * r_src - q_out = r_out.as_quat()[:, [3, 0, 1, 2]] - out[quat_key] = q_out.astype(np.float32) - - return out - - -def apply_position_offset( - left_pos: np.ndarray, - right_pos: np.ndarray, - nominal_ee: dict, -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Shift trajectory so frame-0 wrist midpoint aligns with nominal EE midpoint. - - Args: - left_pos: (T, 3) left wrist positions. - right_pos: (T, 3) right wrist positions. - nominal_ee: Dict with left_pos, right_pos from FK at nominal pose. - - Returns: - (shifted_left, shifted_right, offset_vector) - """ - nom_mid = 0.5 * (nominal_ee["left_pos"] + nominal_ee["right_pos"]) - traj_mid = 0.5 * (left_pos[0] + right_pos[0]) - offset = nom_mid - traj_mid - return left_pos + offset, right_pos + offset, offset - - -def transform_reference( - ref_raw: dict, - nominal_ee: dict, - workspace_offset: tuple[float, float, float] = (0.0, 0.0, 0.0), - robot_type: str = "sharpa", - delta_yaw_offset: float = 0.0, - heading_frame: int = 0, -) -> dict: - """Full transform pipeline: local frame fix → yaw → position align → offset. - - Args: - ref_raw: Raw reference data from load_v2p_reference(). - nominal_ee: Nominal EE dict from get_nominal_ee(). - workspace_offset: Additional (x, y, z) offset. - robot_type: "sharpa" or "dex3". - delta_yaw_offset: Extra yaw rotation in radians added on top of the - heading-toward-object correction. Used for local search over - starting heading. - heading_frame: Reference frame used to compute heading toward object. - - Returns: - Dict with transformed positions, quaternions, finger joints, and metadata - (delta_yaw, offset vector). - """ - ws = np.array(workspace_offset, dtype=np.float32) - - # Step 1: local frame fix - data = apply_local_frame_fix(ref_raw, robot_type=robot_type) - - # Step 2: yaw correction - heading_src = compute_heading_toward_object( - data["left_pos"], - data["right_pos"], - data.get("object_pos"), - frame_index=heading_frame, - ) - g1_forward = 0.0 # G1 faces +X - delta_yaw = g1_forward - heading_src + float(delta_yaw_offset) - data = apply_yaw_correction(data, delta_yaw) - - # Step 3: position offset - lp, rp, offset = apply_position_offset( - data["left_pos"], - data["right_pos"], - nominal_ee, - ) - data["left_pos"] = lp + ws - data["right_pos"] = rp + ws - if data.get("object_pos") is not None: - data["object_pos"] = data["object_pos"] + offset + ws - - # Transform all object bodies (multi-body objects) - if data.get("object_pos_all") is not None: - obj_all = np.array(data["object_pos_all"], dtype=np.float64) - r_yaw = Rotation.from_euler("z", delta_yaw) - midpoint = 0.5 * (ref_raw["left_pos"][0] + ref_raw["right_pos"][0]) - for b in range(obj_all.shape[1]): - obj_all[:, b] = r_yaw.apply(obj_all[:, b] - midpoint) + midpoint - obj_all[:, b] += offset + ws - data["object_pos_all"] = obj_all.astype(np.float32) - if data.get("object_quat_all") is not None: - quat_all = np.array(data["object_quat_all"], dtype=np.float64) - r_yaw = Rotation.from_euler("z", delta_yaw) - for b in range(quat_all.shape[1]): - q_xyzw = quat_all[:, b, [1, 2, 3, 0]] - r_src = Rotation.from_quat(q_xyzw) - r_out = r_yaw * r_src - quat_all[:, b] = r_out.as_quat()[:, [3, 0, 1, 2]] - data["object_quat_all"] = quat_all.astype(np.float32) - - data["delta_yaw"] = delta_yaw - data["offset"] = offset + ws - data["heading_frame"] = int(heading_frame) - return data diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/validation.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/validation.py deleted file mode 100644 index 82d40c0c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/utils/validation.py +++ /dev/null @@ -1,452 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Pre- and post-planning validation for the G1 whole-body planner. - -`warn_reference_issues` is called before planning starts. It checks reference- -owned data (input motion + on-disk assets) and prints warnings; it never -modifies anything. Issues here are the responsibility of the upstream data -producer (retargeting / asset pipeline). - -`assert_motion_parquet_invariants` is called after the planner writes its -output parquet and hard-fails if any planner-owned invariant is violated, so -silent data corruption can't leak into training. -""" - -from __future__ import annotations - -import re -from pathlib import Path -from typing import Any - -import numpy as np -import pyarrow.parquet as pq - -# ---------------------------------------------------------------------------- -# Pre-plan: reference-owned checks (warnings only) -# ---------------------------------------------------------------------------- - -REQUIRED_MOTION_FIELDS: tuple[str, ...] = ( - "object_body_position", - "object_body_wxyz", - "robot_left_wrist_position", - "robot_left_wrist_wxyz", - "robot_right_wrist_position", - "robot_right_wrist_wxyz", - "robot_left_frames", - "robot_right_frames", - "object_urdf_paths", - "object_mesh_paths", -) - -# Source FPS values seen across the retargeting pipelines we currently support. -# A non-listed value is a warning, not a hard failure. -EXPECTED_SOURCE_FPS: tuple[float, ...] = (30.0, 60.0, 100.0, 150.0) - -# Optional fields — present-but-empty is fine, only warn if the user expects -# contact-aware downstream training. -OPTIONAL_CONTACT_FIELDS: tuple[str, ...] = ( - "mano_left_object_contact_positions", - "mano_right_object_contact_positions", - "mano_left_link_contact_positions", - "mano_right_link_contact_positions", -) - - -def _has_nonempty(motion: Any, attr: str) -> bool: - value = getattr(motion, attr, None) - if value is None: - return False - try: - return len(value) > 0 - except TypeError: - return value is not None - - -def _resolve_path(path: str | None) -> Path | None: - if not path: - return None - p = Path(path) - return p if p.exists() else None - - -def walk_urdf_mesh_deps(urdf_path: Path) -> list[Path]: - """Return absolute paths of every ```` referenced by URDF.""" - try: - text = urdf_path.read_text() - except OSError: - return [] - deps: list[Path] = [] - for raw in re.findall(r'mesh\s+filename="([^"]+)"', text): - if raw.startswith("package://"): - deps.append(Path(raw)) - elif Path(raw).is_absolute(): - deps.append(Path(raw)) - else: - deps.append((urdf_path.parent / raw).resolve()) - return deps - - -def warn_missing_urdf_mesh_deps(urdf_paths: list[str]) -> None: - """Print one warning per missing visual/collision file referenced by URDFs. - - A URDF can resolve locally while its ```` children (visual STL, - collision OBJ) aren't present — the IsaacLab importer would then error at - scene-spawn time. Surfacing the gap now lets the user fix the workspace - before they hit the import crash. - """ - for urdf in urdf_paths: - urdf_p = Path(urdf) - if not urdf_p.exists(): - continue - for dep in walk_urdf_mesh_deps(urdf_p): - if not dep.exists(): - print( - f" WARNING: URDF {urdf_p.name} references missing mesh {dep}; " - "copy this file into the workspace before training." - ) - - -def warn_reference_issues(motion: Any, ref_data: dict, robot_type: str) -> list[str]: - """Print warnings for reference-owned problems before planning. - - Returns the list of warning lines emitted, for downstream logging / tests. - Never raises — these are the upstream data producer's responsibility, and - the planner can still produce output (it just may be incomplete). - """ - warnings: list[str] = [] - - def _warn(msg: str) -> None: - line = f" REFERENCE WARNING: {msg}" - print(line) - warnings.append(line) - - # 1. Required input fields - missing = [f for f in REQUIRED_MOTION_FIELDS if not _has_nonempty(motion, f)] - if missing: - _warn( - f"reference motion is missing required fields {missing}; " - "planner output will have empty / zero values for the corresponding " - "training-side columns." - ) - - # 2. Source FPS sanity - src_fps = float(getattr(motion, "fps", 0.0) or 0.0) - if src_fps <= 0.0: - _warn( - "reference motion has fps <= 0; downstream upsampling cannot infer source rate." - ) - elif src_fps not in EXPECTED_SOURCE_FPS: - _warn( - f"reference motion fps={src_fps} is not in the expected set " - f"{EXPECTED_SOURCE_FPS}; verify the contact upsample assumes the right rate." - ) - - # 3. Asset paths resolve - for attr in ("object_urdf_paths", "object_mesh_paths"): - paths = getattr(motion, attr, None) or [] - for p in paths: - if _resolve_path(p) is None: - _warn( - f"{attr} entry {p!r} does not resolve to an existing file; " - "scene spawn will fail unless the planner's path remapper recovers it." - ) - - # 4. URDF mesh dependencies - for urdf in getattr(motion, "object_urdf_paths", None) or []: - urdf_p = _resolve_path(urdf) - if urdf_p is None: - continue - for dep in walk_urdf_mesh_deps(urdf_p): - if not dep.exists(): - _warn( - f"URDF {urdf_p.name} references missing mesh {dep}; " - "copy this visual/collision file into the workspace before training." - ) - - # 5. Contact-aware downstream training will read these — empty == zero reward signal. - empty_contacts = [ - f for f in OPTIONAL_CONTACT_FIELDS if not _has_nonempty(motion, f) - ] - if len(empty_contacts) == len(OPTIONAL_CONTACT_FIELDS): - _warn( - "reference motion has no MANO contact arrays on either side; " - "any contact-conditioned reward / observation will be all-zero." - ) - elif empty_contacts: - _warn(f"reference motion is missing contact arrays {empty_contacts}.") - - # 6. ee_link_names label on the reference (informational; planner overrides this). - ref_ee = getattr(motion, "ee_link_names", None) - if ref_ee: - expected = ( - ("left_hand_palm_link", "right_hand_palm_link") - if robot_type == "dex3" - else ("left_wrist_yaw_link", "right_wrist_yaw_link") - ) - if tuple(ref_ee) != expected: - _warn( - f"reference ee_link_names={list(ref_ee)} disagrees with the planner's " - f"convention {list(expected)} for robot_type={robot_type!r}; " - "the planner will overwrite this label in the output parquet." - ) - - return warnings - - -# ---------------------------------------------------------------------------- -# Post-plan: planner-owned invariants (hard failures) -# ---------------------------------------------------------------------------- - - -EXPECTED_EE_LINK_NAMES: dict[str, list[str]] = { - "sharpa": ["left_wrist_yaw_link", "right_wrist_yaw_link"], - "dex3": ["left_hand_palm_link", "right_hand_palm_link"], -} - -# Per-robot fingertip body suffixes that the training env expects to find in -# `robot_joint_names` (so `tracking_command._load_and_process_motion` can -# resolve `cfg.joint_names`). These are the joint names, not body names. -EXPECTED_FINGER_JOINT_PATTERNS: dict[str, tuple[re.Pattern[str], ...]] = { - "dex3": (re.compile(r"^(left|right)_hand_(thumb|index|middle)_\d_joint$"),), - # Sharpa has 22+22 finger joints; the planner currently doesn't gate on - # name patterns for sharpa, so leave the check open. - "sharpa": (), -} - -# Palm-vs-ee distance after the planner's V2P→planner transform. For dex3 the -# palm IS the EE (free-flyer URDF root), so they should coincide to numerical -# precision. For sharpa the EE (wrist_yaw_link) and palm (hand_C_MC) differ by -# the URDF fixed-joint offset (~4 cm); we don't check this side strictly. -PALM_EE_TOLERANCE_M: dict[str, float] = { - "dex3": 0.01, - "sharpa": 0.10, -} - - -class ParquetInvariantError(AssertionError): - """Raised when the output parquet violates a planner-owned invariant.""" - - -def _arr(value: Any) -> np.ndarray: - return np.asarray(value, dtype=np.float64) - - -def _load_md(parquet_path: Path) -> dict: - """Load the parquet as a plain dict (one entry per Hive row).""" - table = pq.read_table(str(parquet_path)) - data = table.to_pydict() - # Each top-level column is a 1-row list; unwrap. - return {k: v[0] for k, v in data.items()} - - -def _check_object_root(md: dict) -> list[str]: - errs: list[str] = [] - bp = _arr(md.get("object_body_position", [])) - rp = md.get("object_root_position") - if bp.ndim != 3: - return [f"object_body_position has wrong rank {bp.shape} (expected (T,B,3))"] - if rp is None: - return ["object_root_position is None — required for articulated scene init"] - rp_a = _arr(rp) - if rp_a.shape[0] != bp.shape[0]: - errs.append( - f"object_root_position length {rp_a.shape[0]} != object_body_position length {bp.shape[0]}; " - "they must share a frame count so the scene init pose matches the trajectory start." - ) - if rp_a.shape[0] == bp.shape[0]: - max_d = float(np.max(np.linalg.norm(rp_a - bp[:, 0, :], axis=-1))) - if max_d > 1e-3: - errs.append( - f"object_root_position diverges from object_body_position[:,0] by {max_d:.4f} m; " - "root must mirror body 0 so the env's reset pose lands where the trajectory starts." - ) - - raa = md.get("object_root_axis_angle") - if raa is None: - errs.append("object_root_axis_angle is None") - else: - raa_a = _arr(raa) - if raa_a.shape[0] != bp.shape[0]: - errs.append( - f"object_root_axis_angle length {raa_a.shape[0]} != object_body_position length {bp.shape[0]}" - ) - return errs - - -def _check_ee_link_names(md: dict, robot_type: str) -> list[str]: - expected = EXPECTED_EE_LINK_NAMES.get(robot_type) - if expected is None: - return [f"unknown robot_type={robot_type!r}; cannot validate ee_link_names"] - actual = list(md.get("ee_link_names") or []) - if actual != expected: - return [ - f"ee_link_names={actual} but expected {expected} for robot_type={robot_type!r}; " - "this label tells the env which body the EE pose was recorded from, so a " - "mismatch puts the reward target on the wrong link." - ] - return [] - - -def _check_hand_frames_transform(md: dict, robot_type: str) -> list[str]: - """Verify hand_frames_w[palm] coincides with ee_pose_w. - - For dex3 the palm IS the EE (free-flyer root); they should coincide to - numerical precision. A larger gap means the planner-frame rigid transform - was applied to one of these arrays but not the other. - """ - errs: list[str] = [] - hfw = md.get("hand_frames_w") - hfn = md.get("hand_frame_names") - ee = md.get("ee_pose_w") - sides = md.get("hand_sides") or [] - if hfw is None or not hfw or ee is None: - return errs # nothing to check - hfw_a = _arr(hfw) # (S, T, K, 7) - ee_a = _arr(ee) # (T, 2, 7) - if hfw_a.ndim != 4 or ee_a.ndim != 3: - return errs - palm_name_by_robot = { - "dex3": ("left_hand_palm_link", "right_hand_palm_link"), - "sharpa": ("left_hand_C_MC", "right_hand_C_MC"), - } - palm_names = palm_name_by_robot.get(robot_type, ()) - tol = PALM_EE_TOLERANCE_M.get(robot_type, 0.10) - for s_idx, side in enumerate(sides): - if s_idx >= hfw_a.shape[0] or s_idx >= len(palm_names): - continue - names = list(hfn[s_idx]) if hfn and s_idx < len(hfn) else [] - target = palm_names[s_idx] - if target not in names: - continue - k = names.index(target) - palm_xyz = hfw_a[s_idx, :, k, :3] - side_xyz = ee_a[:, s_idx, :3] if ee_a.shape[1] > s_idx else None - if side_xyz is None or palm_xyz.shape[0] != side_xyz.shape[0]: - continue - max_d = float(np.max(np.linalg.norm(palm_xyz - side_xyz, axis=-1))) - if max_d > tol: - errs.append( - f"hand_frames_w[{side}, palm] vs ee_pose_w[{side}] max distance " - f"{max_d:.4f} m exceeds {tol} m; both columns describe the same body " - "so they should agree — one of them is in the wrong frame." - ) - return errs - - -def _check_fingers_in_joint_names(md: dict, robot_type: str) -> list[str]: - patterns = EXPECTED_FINGER_JOINT_PATTERNS.get(robot_type) or () - if not patterns: - return [] - names = list(md.get("robot_joint_names") or []) - has_finger = any(p.match(n) for p in patterns for n in names) - if not has_finger: - return [ - "robot_joint_names has no finger joints; tracking_command resolves " - "cfg.joint_names against this list and raises if a tracked joint is missing." - ] - return [] - - -def _check_contact_field_shapes(md: dict) -> list[str]: - """Confirm contact arrays have a sane (S, T, N, 3) shape and the right T.""" - errs: list[str] = [] - bp = _arr(md.get("object_body_position", [])) - if bp.ndim != 3: - return errs - T = bp.shape[0] - for field in ( - "hand_link_contact_positions", - "hand_link_contact_normals", - "hand_object_contact_positions", - "hand_object_contact_normals", - ): - value = md.get(field) - if value is None: - continue - for s_idx, side_arr in enumerate(value): - if side_arr is None or len(side_arr) == 0: - continue - side_a = _arr(side_arr) - if side_a.ndim != 3: - errs.append(f"{field}[side={s_idx}] has wrong rank {side_a.shape}") - continue - if side_a.shape[0] != T: - errs.append( - f"{field}[side={s_idx}] frame count {side_a.shape[0]} != T={T}; " - "every per-frame contact array must match the body trajectory length." - ) - if side_a.shape[-1] != 3: - errs.append( - f"{field}[side={s_idx}] last dim {side_a.shape[-1]} != 3; " - "the part-id column is stored separately, only xyz should remain here." - ) - - # hand_contact_active must be per-side, length T, binary-ish. - hca = md.get("hand_contact_active") - if hca is not None: - hca_a = _arr(hca) - if hca_a.ndim == 2 and hca_a.shape[1] != T: - errs.append(f"hand_contact_active shape {hca_a.shape} second dim != T={T}") - return errs - - -def _check_asset_paths(md: dict) -> list[str]: - errs: list[str] = [] - for field in ("object_urdf_paths", "object_mesh_paths"): - for p in md.get(field) or []: - if not p or not Path(p).exists(): - errs.append( - f"{field} entry {p!r} does not exist on disk; " - "the parquet must be self-contained against the current workspace." - ) - # Walk URDF deps; missing visuals/collisions block scene spawn. - for urdf in md.get("object_urdf_paths") or []: - urdf_p = Path(urdf) - if not urdf_p.exists(): - continue - for dep in walk_urdf_mesh_deps(urdf_p): - if not dep.exists(): - errs.append( - f"URDF {urdf_p.name} references missing mesh {dep}; " - "every visual/collision file the URDF lists must be present." - ) - return errs - - -def assert_motion_parquet_invariants( - parquet_path: str | Path, - robot_type: str, -) -> None: - """Hard-fail if the planner output violates any required invariant. - - Call this immediately after `save_motion_parquet` returns. Each check - targets a downstream consumer contract; the error messages spell out the - contract so the fix is obvious. - """ - parquet_path = Path(parquet_path) - # `parquet_path` is the partition directory returned by `save_motion_parquet`; - # the actual parquet sits inside as `.parquet` (or `data.parquet`). - if parquet_path.is_dir(): - candidates = sorted(parquet_path.glob("*.parquet")) - if not candidates: - raise ParquetInvariantError( - f"No .parquet file under {parquet_path} — cannot validate planner output." - ) - parquet_file = candidates[0] - else: - parquet_file = parquet_path - - md = _load_md(parquet_file) - errors: list[str] = [] - errors.extend(_check_object_root(md)) - errors.extend(_check_ee_link_names(md, robot_type)) - errors.extend(_check_hand_frames_transform(md, robot_type)) - errors.extend(_check_fingers_in_joint_names(md, robot_type)) - errors.extend(_check_contact_field_shapes(md)) - errors.extend(_check_asset_paths(md)) - - if errors: - header = f"Planner output {parquet_file} failed {len(errors)} invariant(s):" - raise ParquetInvariantError("\n - ".join([header, *errors])) - - print(f" planner_validation: {parquet_file.name} passed all invariants.") diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/visualization.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/visualization.py deleted file mode 100644 index 48d98da3..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/planner/visualization.py +++ /dev/null @@ -1,386 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MuJoCo viewer for planner output visualization. - -Shows planned body motion with target/achieved EE axes, object trajectory, -and optional finger animation. -""" - -from __future__ import annotations - -import os -import re -import time -import xml.etree.ElementTree as ET - -import mujoco -import mujoco.viewer -import numpy as np -from scipy.spatial.transform import Rotation - - -def add_axes( - scn: mujoco.MjvScene, - pos: np.ndarray, - quat_wxyz: np.ndarray, - length: float = 0.08, - radius: float = 0.004, - alpha: float = 1.0, -) -> None: - """Draw an RGB axis triad in the MuJoCo viewer scene. - - Args: - scn: MuJoCo viewer scene (mujoco.MjvScene). - pos: (3,) position. - quat_wxyz: (4,) orientation in wxyz format. - length: Axis length in meters. - radius: Axis cylinder radius. - alpha: Opacity. - """ - q_xyzw = quat_wxyz[[1, 2, 3, 0]] - rot = Rotation.from_quat(q_xyzw).as_matrix() - - colors = [ - [1, 0, 0, alpha], # X = red - [0, 1, 0, alpha], # Y = green - [0, 0, 1, alpha], # Z = blue - ] - - for axis_idx in range(3): - if scn.ngeom >= scn.maxgeom: - return - tip = pos + rot[:, axis_idx] * length - rgba = np.array(colors[axis_idx], dtype=np.float32) - mujoco.mjv_connector( - scn.geoms[scn.ngeom], - mujoco.mjtGeom.mjGEOM_CAPSULE, - radius, - np.asarray(pos, dtype=np.float64), - np.asarray(tip, dtype=np.float64), - ) - scn.geoms[scn.ngeom].rgba = rgba - scn.ngeom += 1 - - -def _inject_scene_objects( - xml_path: str, - object_mesh_paths: list[str] | None = None, - support_usda_path: str | None = None, - support_transform: dict | None = None, -) -> str: - """Inject object meshes and support surface into MuJoCo XML. - - Args: - xml_path: Base robot XML path. - object_mesh_paths: List of .obj mesh paths (one per object body), or None. - support_usda_path: Path to support surface .usda file, or None. - support_transform: Dict for transforming support positions. - - Returns: - Modified XML string with injected bodies. - """ - tree = ET.parse(xml_path) - root = tree.getroot() - xml_dir = os.path.dirname(os.path.abspath(xml_path)) - - # Resolve any include files by inlining them - # (from_xml_string can't resolve relative includes) - for inc in root.findall(".//include"): - inc_file = inc.get("file") - if inc_file and not os.path.isabs(inc_file): - inc_path = os.path.join(xml_dir, inc_file) - if os.path.exists(inc_path): - inc_tree = ET.parse(inc_path) - inc_root = inc_tree.getroot() - parent = root - for child in list(inc_root): - parent.append(child) - parent.remove(inc) - - # Absolutize meshdir so from_xml_string can find meshes - compiler = root.find("compiler") - if compiler is not None: - meshdir = compiler.get("meshdir", "") - if meshdir and not os.path.isabs(meshdir): - compiler.set("meshdir", os.path.abspath(os.path.join(xml_dir, meshdir))) - - worldbody = root.find("worldbody") - if worldbody is None: - worldbody = ET.SubElement(root, "worldbody") - - # Inject one visual-only free body per object mesh - if object_mesh_paths: - asset = root.find("asset") - if asset is None: - asset = ET.SubElement(root, "asset") - for i, mesh_path in enumerate(object_mesh_paths): - if os.path.exists(mesh_path): - mesh_name = f"object_mesh_{i}" - # Match support_recon.py: TACO `_cm.obj` meshes are in - # centimetres; ARCTIC meshes are already in metres. - scale_value = 0.01 if mesh_path.endswith("_cm.obj") else 1.0 - scale = f"{scale_value} {scale_value} {scale_value}" - ET.SubElement( - asset, - "mesh", - name=mesh_name, - file=mesh_path, - scale=scale, - ) - body_name = f"object_vis_{i}" - obj_body = ET.SubElement(worldbody, "body", name=body_name, pos="0 0 0") - ET.SubElement( - obj_body, - "inertial", - pos="0 0 0", - mass="0.001", - diaginertia="0.000001 0.000001 0.000001", - ) - ET.SubElement(obj_body, "freejoint", name=f"{body_name}_joint") - ET.SubElement( - obj_body, - "geom", - type="mesh", - mesh=mesh_name, - contype="0", - conaffinity="0", - rgba="0.8 0.6 0.4 0.8", - ) - - # Inject support surface from USDA (parse cylinder primitives) - if support_usda_path is not None and os.path.exists(support_usda_path): - with open(support_usda_path) as f: - usda_content = f.read() - - # Parse cylinder definitions from USDA - cylinders = re.findall( - r'def Cylinder "(\w+)".*?height = ([\d.]+).*?radius = ([\d.]+).*?' - r"xformOp:translate = \(([-\d., ]+)\)", - usda_content, - re.DOTALL, - ) - - for name, height, radius, translate in cylinders: - pos = np.array([float(x.strip()) for x in translate.split(",")]) - - # Transform from V2P frame to G1 viewer frame - if support_transform is not None: - src_mid = support_transform["src_midpoint"] - r_yaw = Rotation.from_euler("z", support_transform["delta_yaw"]) - pos = r_yaw.apply(pos - src_mid) + src_mid - pos += support_transform["offset"] - - pos_str = f"{pos[0]} {pos[1]} {pos[2]}" - support_body = ET.SubElement( - worldbody, "body", name=f"support_{name}", pos=pos_str - ) - ET.SubElement( - support_body, - "geom", - type="cylinder", - size=f"{radius} {float(height)/2}", - contype="0", - conaffinity="0", - rgba="0.7 0.6 0.5 0.6", - ) - - return ET.tostring(root, encoding="unicode") - - -def visualize( - xml_path: str, - qpos_traj: np.ndarray, - traj_lp: np.ndarray, - traj_lq: np.ndarray, - traj_rp: np.ndarray, - traj_rq: np.ndarray, - left_finger_joints: np.ndarray | None = None, - right_finger_joints: np.ndarray | None = None, - recorded_left_names: list[str] | None = None, - recorded_right_names: list[str] | None = None, - object_pos: np.ndarray | None = None, - object_quat: np.ndarray | None = None, - fps: float = 30.0, - ref_start: int = 0, - object_mesh_paths: list[str] | None = None, - support_usda_path: str | None = None, - support_transform: dict | None = None, -) -> None: - """Interactive playback of planned motion with target/achieved EE axes. - - Args: - xml_path: MuJoCo XML path. - qpos_traj: (T, nq) full qpos trajectory. - traj_lp: (T, 3) target left wrist positions. - traj_lq: (T, 4) target left wrist quaternions (wxyz). - traj_rp: (T, 3) target right wrist positions. - traj_rq: (T, 4) target right wrist quaternions (wxyz). - left_finger_joints: Optional (T_ref, J_left) finger joint angles. - right_finger_joints: Optional (T_ref, J_right) finger joint angles. - recorded_left_names: Finger joint names for left hand. - recorded_right_names: Finger joint names for right hand. - object_pos: Optional (T_ref, 3) object positions. - object_quat: Optional (T_ref, 4) object quaternions (wxyz). - fps: Playback frame rate. - ref_start: Frame index where reference data begins. - object_mesh_paths: List of per-body object .obj mesh paths for visual overlay. - support_usda_path: Path to support surface USDA file. - support_transform: Optional dict carrying yaw/offset to apply to the support primitive translates. - """ - # Inject scene objects into XML if provided - if object_mesh_paths or support_usda_path: - xml_str = _inject_scene_objects( - xml_path, - object_mesh_paths, - support_usda_path, - support_transform=support_transform, - ) - model = mujoco.MjModel.from_xml_string(xml_str) - else: - model = mujoco.MjModel.from_xml_path(xml_path) - data = mujoco.MjData(model) - - left_id = model.body("left_wrist_yaw_link").id - right_id = model.body("right_wrist_yaw_link").id - - # Find injected object bodies (one per mesh) - obj_vis_jnt_adrs = [] - for i in range(len(object_mesh_paths or [])): - try: - jnt_id = mujoco.mj_name2id( - model, mujoco.mjtObj.mjOBJ_JOINT, f"object_vis_{i}_joint" - ) - if jnt_id >= 0: - obj_vis_jnt_adrs.append(model.jnt_qposadr[jnt_id]) - except Exception: - pass - - # Build finger joint mappings - l_finger_map = _build_finger_map(model, recorded_left_names) - r_finger_map = _build_finger_map(model, recorded_right_names) - - T_play = min(qpos_traj.shape[0], len(traj_lp)) - paused = False - - def key_callback(key: int) -> None: - nonlocal paused - if key == 32: # spacebar - paused = not paused - - with mujoco.viewer.launch_passive(model, data, key_callback=key_callback) as viewer: - viewer.cam.distance = 2.0 - viewer.cam.elevation = -15 - viewer.cam.azimuth = 180 - - frame = 0 - while viewer.is_running() and frame < T_play: - if paused: - time.sleep(0.01) - viewer.sync() - continue - - # Set body qpos - data.qpos[: qpos_traj.shape[1]] = qpos_traj[frame] - - # Update object body positions. Hold the first object pose during - # warmup/interp so the scene is visible before the reference starts. - if obj_vis_jnt_adrs and object_pos is not None: - obj_idx = min(max(frame - ref_start, 0), object_pos.shape[0] - 1) - for i, jnt_adr in enumerate(obj_vis_jnt_adrs): - if object_pos.ndim == 3 and i < object_pos.shape[1]: - data.qpos[jnt_adr : jnt_adr + 3] = object_pos[obj_idx, i] - if object_quat is not None: - data.qpos[jnt_adr + 3 : jnt_adr + 7] = object_quat[ - obj_idx, i - ] - elif object_pos.ndim == 2 and i == 0: - data.qpos[jnt_adr : jnt_adr + 3] = object_pos[obj_idx] - if object_quat is not None: - data.qpos[jnt_adr + 3 : jnt_adr + 7] = object_quat[obj_idx] - - # Set finger joints from reference - if frame >= ref_start: - f_idx = min( - frame - ref_start, - ( - (left_finger_joints.shape[0] - 1) - if left_finger_joints is not None - else 0 - ), - ) - _set_fingers(data, model, l_finger_map, left_finger_joints, f_idx) - _set_fingers(data, model, r_finger_map, right_finger_joints, f_idx) - - mujoco.mj_forward(model, data) - - # Draw axes - viewer.user_scn.ngeom = 0 - - # Target EE (thick) - add_axes( - viewer.user_scn, - traj_lp[frame], - traj_lq[frame], - length=0.12, - radius=0.006, - ) - add_axes( - viewer.user_scn, - traj_rp[frame], - traj_rq[frame], - length=0.12, - radius=0.006, - ) - - # Achieved EE (thin) - add_axes( - viewer.user_scn, - data.xpos[left_id], - data.xquat[left_id], - length=0.08, - radius=0.003, - alpha=0.6, - ) - add_axes( - viewer.user_scn, - data.xpos[right_id], - data.xquat[right_id], - length=0.08, - radius=0.003, - alpha=0.6, - ) - - viewer.sync() - time.sleep(1.0 / fps) - frame += 1 - - -def _build_finger_map( - model: mujoco.MjModel, joint_names: list[str] | None -) -> list[int]: - """Map finger joint names to model qpos indices.""" - if not joint_names: - return [] - result = [] - for name in joint_names: - try: - jid = model.joint(name).id - result.append(int(model.jnt_qposadr[jid])) - except Exception: - result.append(-1) - return result - - -def _set_fingers( - data: mujoco.MjData, - model: mujoco.MjModel, - finger_map: list[int], - finger_joints: np.ndarray | None, - f_idx: int, -) -> None: - """Set finger joint positions from reference data.""" - if finger_joints is None or not finger_map: - return - for j, qi in enumerate(finger_map): - if qi >= 0 and j < finger_joints.shape[1]: - data.qpos[qi] = finger_joints[f_idx, j] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/__init__.py deleted file mode 100644 index 046bc884..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/__init__.py +++ /dev/null @@ -1,14 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import os -from pathlib import Path - -ASSETS_DIR = Path(__file__).parent.parent / "assets" -BODY_MODELS_DIR = ASSETS_DIR / "body_models" -if "HUMAN_MOTION_DATA_DIR" in os.environ: - HUMAN_MOTION_DATA_DIR = Path(os.environ["HUMAN_MOTION_DATA_DIR"]) -else: - HUMAN_MOTION_DATA_DIR = ASSETS_DIR / "human_motion_data" -SHARPA_WAVE_XMLS_DIR = ASSETS_DIR / "xmls" / "sharpawave" -G1_URDF_DIR = ASSETS_DIR / "urdfs" / "g1" -MESHES_DIR = ASSETS_DIR / "meshes" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/README.md deleted file mode 100644 index e1e5c91b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/README.md +++ /dev/null @@ -1,206 +0,0 @@ -# Retarget config bundle - -Per-robot configuration for the SOMA → robot retargeter. One robot = one -sub-directory under `configs/`, containing exactly two files: - -``` -configs/ -└── / - ├── frame_alignment.json # source-to-robot frame change (world + per-bone) - └── retargeter.json # URDF, IK map, foot framing, posture-task -``` - -`load_robot_config()` in -`source/.../retarget/robot_config.py` reads both files together and -returns a fully-materialized `RobotRetargetConfig` whose matrices are -already composed for runtime consumption. Runtime code does not need to -know whether a value was supplied as a 3×3 matrix or an `xyzw` -quaternion; the loader normalizes both forms. - -## Convention rules - -These hold across both files: - -- **Matrices** are row-major 3×3 lists. -- **Quaternions** are `xyzw`. -- **Distances** are meters. -- **Robot world frame**: X = forward, Y = left, Z = up. -- **Rotation composition** is right-multiply, applied as - `target_rot = R_world @ source_rot @ correction`. - -## `frame_alignment.json` - -The frame change from the source skeleton (e.g. SOMA-X) into -the robot world. Two layers: - -- **World layer**: `world_axis_swap_matrix` (or the equivalent - `world_axis_swap_xyzw`) — a single rotation that maps source-world - vectors into robot-world vectors. Applied left-multiplied to every - source position and source-world rotation. -- **Per-bone layer**: `joint_offsets[]` — for each source - joint that drives an IK target, the basis change between the - source-joint local frame and the robot-link local frame. Applied - right-multiplied onto the source bone's global rotation: - `target_rot = R_world @ source_rot @ joint_offsets[joint].q_offset`. - -### Schema - -```jsonc -{ - "schema_version": 1, // bump on incompatible changes - "robot_name": "g1", // must match retargeter.json - "source_model": "soma", // must match retargeter.json - "_description": "...", // optional human-readable summary - "_provenance": "...", // optional: where values came from - "_upstream_reference": "...", // optional: original table URL - - // World-frame axis swap (one of these is required): - "world_axis_swap_matrix": [[...],[...],[...]], - // or "world_axis_swap_xyzw": [x, y, z, w] - - // Per-bone corrections, keyed by source joint name: - "joint_offsets": { - "Hips": { - "q_offset_matrix": [[...],[...],[...]], // or q_offset_xyzw - "t_offset": [0.0, 0.0, 0.01] // optional, default zeros - }, - // ... - }, - - // Optional: SOMA joint names whose q_offset was algorithmically - // derived (rather than hand-tuned). Consumers may use this list to - // assert tight q0 invariants; absent values default to []: - "_auto_derived_joints": ["LeftLeg", "LeftShin", "LeftFoot", ...] -} -``` - -`t_offset` is applied in the corrected effector frame at runtime as -`target_pos += target_rot @ t_offset`. Only the `joint_offsets` whose -key appears in `retargeter.json:ik_map`'s `soma_joint` field are -projected onto a `t_per_link` entry; offsets for other joints are -parsed but unused. - -## `retargeter.json` - -What the IK actually does on the robot. - -### Schema - -```jsonc -{ - "schema_version": 1, - "robot_name": "g1", - "source_model": "soma", - "_description": "...", - "_provenance": "...", - "_upstream_reference": "...", - - "urdf": "../../../assets/urdfs/g1/main_with_hand.urdf", - "package_dirs": ["../../../assets/urdfs/g1"], - "base_source_joint": "Hips", // anchor used for source -> robot scaling - - "ik_map": { - "": { - "soma_joint": "", - "position_cost": 1.0, - "orientation_cost": 0.2 - }, - // ... - }, - - "foot_frames": ["left_ankle_roll_link", "right_ankle_roll_link"], - "ankle_roll_offset": 0.037, // meters, ankle-roll origin -> sole - - "posture_task": { // optional, defaults to all-zero - "q0_cost": 0.05, - "q_prev_cost": 0.5, - "lm_damping": 0.0, - "gain": 1.0 - } -} -``` - -`urdf` and `package_dirs` are resolved relative to the directory the -JSON lives in (i.e. `configs//`). `posture_task` is consumed by -`WholeBodyKinematics`; absent or all-zero costs disable the posture -term entirely (the legacy default). - -## Adding a new robot - -1. **Copy an existing bundle** as a starting point: - ``` - cp -r configs/g1 configs/ - ``` -2. **Update `retargeter.json`**: - - `robot_name`: set to ``. - - `urdf`, `package_dirs`: point at the new robot's URDF. - - `ik_map`: replace each `` key with a frame name - that exists in the new URDF (verify with - `pinocchio.RobotWrapper.BuildFromURDF(...).model.frames`). Tune - `position_cost` / `orientation_cost` per IK target. - - `foot_frames`, `ankle_roll_offset`: set from the URDF's foot - geometry. -3. **Update `frame_alignment.json`**: - - `robot_name`: set to ``. - - `world_axis_swap_matrix`: usually identical to a sibling robot - because both share the source skeleton's world convention. Only - change if the new URDF uses a different world frame. - - `joint_offsets[*].q_offset_matrix`: see "Tuning q_offsets" below. - - `joint_offsets[*].t_offset`: usually zero; set non-zero only for - joints where the URDF link origin sits at a known offset from the - source joint origin (e.g. an ankle link offset to the sole). -4. **Validate the bundle** via `load_robot_config("")` (see - "Verifying a bundle" below). - -## Tuning `q_offsets` - -Three legitimate sources, in order of preference: - -1. **Copy from an upstream reference table.** NVIDIA/soma-retargeter - ships per-bone `q_offset` quaternions for several humanoid URDFs; - the G1 bundle was seeded this way. The `_upstream_reference` field in - each JSON points at the table. -2. **Derive from a known rest pose.** For each `(source_joint, - robot_frame)` pair, set the source skeleton to a reference rest pose - and compute the rotation that aligns the source joint's world - rotation with the URDF link's world rotation: - ``` - q_offset = (R_world @ R_source_world).T @ R_link_world - ``` - This is what a "derive corrections" tool would automate. Constraint - to satisfy: applying `q_offset` makes the IK target match the URDF's - `q0` configuration at the chosen rest pose. -3. **Hand-tune in a visualizer.** Run the retargeter against a single - reference pose with the new `q_offset` matrix and inspect the IK - target frame in `viser_playback`. Iterate until the visual alignment - is correct. - -When values are derived (path 2) rather than hand-tuned (paths 1 or 3), -add the source joint's name to `_auto_derived_joints` so downstream -assertions can be tightened. - -## Verifying a bundle - -Smoke-test the loader: - -```bash -python -c "from robotic_grounding.retarget.robot_config import load_robot_config; \ - c = load_robot_config(''); \ - print(c.robot_name, len(c.r_per_bone), c.posture_task)" -``` - -For a deeper check, run a short retarget on a fixture sequence and -inspect the result: - -```bash -python scripts/retarget/soma_to_g1.py --robot-name --visualize -``` - -Open the printed viser URL and confirm: -- The robot lands near the source skeleton at frame 0 (no axis flip). -- Each IK end-effector tracks its source joint over time (no per-bone - twist). -- Feet sit on the ground after the post-process (no float / sink). - -If a per-bone twist appears, the corresponding `joint_offsets[*]` entry -is wrong — re-derive or hand-tune. diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/frame_alignment.json b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/frame_alignment.json deleted file mode 100644 index 012d6db1..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/frame_alignment.json +++ /dev/null @@ -1,363 +0,0 @@ -{ - "_description": "Source-to-robot frame alignment for G1: world axis swap + per-bone (q_offset, t_offset) corrections.", - "_provenance": "World axis swap is hand-authored (see configs/README.md). Per-bone q_offsets were seeded from NVIDIA/soma-retargeter and refined empirically; values may diverge from upstream by design.", - "_upstream_reference": "https://github.com/NVIDIA/soma-retargeter/blob/main/soma_retargeter/configs/unitree_g1/soma_to_g1_scaler_config.json", - "joint_offsets": { - "Chest": { - "q_offset_matrix": [ - [ - -0.08592688903335513, - 0, - 0.9963014452167824 - ], - [ - 0.9963014452167824, - 0, - 0.08592688903335516 - ], - [ - 0, - 1, - -2.7755575615628914e-17 - ] - ], - "t_offset": [ - 0.005, - 0, - -0.094 - ] - }, - "Hips": { - "q_offset_matrix": [ - [ - 0, - 0, - 1 - ], - [ - 1, - 0, - 0 - ], - [ - 0, - 1, - 0 - ] - ], - "t_offset": [ - 0, - 0, - 0.01 - ] - }, - "LeftArm": { - "q_offset_matrix": [ - [ - 0.07881496459283907, - 0.1684950366927157, - -0.9825464996457673 - ], - [ - 0.9950477040407043, - -0.07317786622442149, - 0.06726861510514176 - ], - [ - -0.06056622853994259, - -0.9829824121034725, - -0.17342811033879832 - ] - ], - "t_offset": [ - 0.012, - 0.01, - -0.034 - ] - }, - "LeftFoot": { - "q_offset_matrix": [ - [ - 0.9343864447777271, - 0, - -0.3562611006209339 - ], - [ - -0.3562611006209339, - 0, - -0.9343864447777271 - ], - [ - 0, - 0.9999999999999999, - 0 - ] - ], - "t_offset": [ - -0.017, - 0.04, - -0.017 - ] - }, - "LeftForeArm": { - "q_offset_matrix": [ - [ - 1.0000000000000002, - 0, - 0 - ], - [ - 0, - 0, - 1.0000000000000002 - ], - [ - 0, - -1.0000000000000002, - 0 - ] - ], - "t_offset": [ - 0.012, - -0.015, - 0.01 - ] - }, - "LeftHand": { - "q_offset_matrix": [ - [ - 1.0000000000000002, - 0, - 0 - ], - [ - 0, - 0, - 1.0000000000000002 - ], - [ - 0, - -1.0000000000000002, - 0 - ] - ], - "t_offset": [ - 0.015, - -0.006, - 0.005 - ] - }, - "LeftLeg": { - "q_offset_matrix": [ - [ - -0.15748662834291427, - 0, - -0.98752111972007 - ], - [ - -0.98752111972007, - 0, - 0.15748662834291427 - ], - [ - 0, - 0.9999999999999999, - 0 - ] - ], - "t_offset": [ - -0.03, - 0.036, - -0.05 - ] - }, - "LeftShin": { - "q_offset_matrix": [ - [ - 0, - 0, - -1 - ], - [ - -1, - 0, - 0 - ], - [ - 0, - 1, - 0 - ] - ], - "t_offset": [ - 0, - 0.04, - -0.005 - ] - }, - "RightArm": { - "q_offset_matrix": [ - [ - -0.07323665317143896, - 0.16842049166912543, - 0.9829908090201946 - ], - [ - -0.9950484472818821, - -0.07874653696702621, - -0.06064297550101494 - ], - [ - 0.06719360233059607, - -0.9825647667719575, - 0.17335368154212039 - ] - ], - "t_offset": [ - 0.012, - -0.01, - -0.034 - ] - }, - "RightFoot": { - "q_offset_matrix": [ - [ - -0.9343864447777274, - 0, - 0.3562611006209341 - ], - [ - 0.3562611006209341, - 3.469446951953614e-18, - 0.9343864447777275 - ], - [ - 0, - 1.0000000000000002, - 3.469446951953614e-18 - ] - ], - "t_offset": [ - -0.017, - -0.04, - -0.017 - ] - }, - "RightForeArm": { - "q_offset_matrix": [ - [ - -1.0000000000000002, - 0, - 0 - ], - [ - 0, - 0, - -1.0000000000000002 - ], - [ - 0, - -1.0000000000000002, - 0 - ] - ], - "t_offset": [ - 0.012, - 0.015, - 0.01 - ] - }, - "RightHand": { - "q_offset_matrix": [ - [ - -1.0000000000000002, - 0, - 0 - ], - [ - 0, - 0, - -1.0000000000000002 - ], - [ - 0, - -1.0000000000000002, - 0 - ] - ], - "t_offset": [ - 0.015, - 0.006, - 0.005 - ] - }, - "RightLeg": { - "q_offset_matrix": [ - [ - 0.15748662834291427, - 0, - 0.98752111972007 - ], - [ - 0.98752111972007, - 0, - -0.15748662834291427 - ], - [ - 0, - 0.9999999999999999, - 0 - ] - ], - "t_offset": [ - -0.03, - -0.036, - -0.05 - ] - }, - "RightShin": { - "q_offset_matrix": [ - [ - 0, - 0, - 1 - ], - [ - 1, - 0, - 0 - ], - [ - 0, - 1, - 0 - ] - ], - "t_offset": [ - 0, - -0.04, - -0.005 - ] - } - }, - "robot_name": "g1", - "schema_version": 1, - "source_model": "soma", - "world_axis_swap_matrix": [ - [ - 0, - 0, - -1 - ], - [ - -1, - 0, - 0 - ], - [ - 0, - 1, - 0 - ] - ] -} diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/retargeter.json b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/retargeter.json deleted file mode 100644 index 5f3a2abd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/configs/g1/retargeter.json +++ /dev/null @@ -1,66 +0,0 @@ -{ - "_description": "IK targets (robot frame -> source joint), foot framing, and posture-task weights for the G1 retargeter.", - "_provenance": "URDF + foot frames + ankle_roll_offset are local to this repo. ik_map weights and posture_task were seeded from NVIDIA/soma-retargeter and tuned for the snack-box / book sequences; values diverge from upstream by design.", - "_upstream_reference": "https://github.com/NVIDIA/soma-retargeter/blob/main/soma_retargeter/configs/unitree_g1/soma_to_g1_retargeter_config.json", - "ankle_roll_offset": 0.037, - "base_source_joint": "Hips", - "foot_frames": [ - "left_ankle_roll_link", - "right_ankle_roll_link" - ], - "ik_map": { - "left_ankle_roll_link": { - "orientation_cost": 0.1, - "position_cost": 5, - "soma_joint": "LeftFoot" - }, - "left_knee_link": { - "orientation_cost": 0.1, - "position_cost": 0.25, - "soma_joint": "LeftShin" - }, - "left_wrist_yaw_link": { - "orientation_cost": 0.2, - "position_cost": 1, - "soma_joint": "LeftHand" - }, - "pelvis": { - "orientation_cost": 0, - "position_cost": 0.5, - "soma_joint": "Hips" - }, - "right_ankle_roll_link": { - "orientation_cost": 0.1, - "position_cost": 5, - "soma_joint": "RightFoot" - }, - "right_knee_link": { - "orientation_cost": 0.1, - "position_cost": 0.25, - "soma_joint": "RightShin" - }, - "right_wrist_yaw_link": { - "orientation_cost": 0.2, - "position_cost": 1, - "soma_joint": "RightHand" - }, - "torso_link": { - "orientation_cost": 0.5, - "position_cost": 0, - "soma_joint": "Chest" - } - }, - "package_dirs": [ - "../../../assets/urdfs/g1" - ], - "posture_task": { - "gain": 1, - "lm_damping": 0, - "q0_cost": 0.05, - "q_prev_cost": 0.5 - }, - "robot_name": "g1", - "schema_version": 1, - "source_model": "soma", - "urdf": "../../../assets/urdfs/g1/main_with_hand.urdf" -} diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/data_logger.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/data_logger.py deleted file mode 100644 index 0cfc4858..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/data_logger.py +++ /dev/null @@ -1,593 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import argparse -import hashlib -import re -import shutil -from dataclasses import MISSING, field, fields, make_dataclass -from pathlib import Path -from typing import Any, Optional -from urllib.parse import unquote - -import pyarrow as pa -import pyarrow.compute as pc -import pyarrow.parquet as pq - -# Type alias for field specification -# Format: (field_name, pyarrow_type, python_type, is_time_series) -FieldSpec = tuple[str, pa.DataType, type, bool] - -############################################################# -# Base fields -############################################################# -BASE_FIELDS: list[FieldSpec] = [ - ("sequence_id", pa.string(), str, False), - ("raw_motion_file", pa.string(), str, False), - ("robot_name", pa.string(), str, False), - ("fps", pa.float32(), float, False), -] - -############################################################# -# Body model fields -############################################################# -MANO_FIELDS: list[FieldSpec] = [ - ("mano_flat_hand_mean", pa.bool_(), bool, False), - ("mano_center_idx", pa.int32(), int, False), - ("mano_to_robot_scale", pa.float32(), float, False), - ("mano_right_betas", pa.list_(pa.float32()), list[float], False), - ("mano_left_betas", pa.list_(pa.float32()), list[float], False), - ("mano_link_names", pa.list_(pa.string()), list[str], False), - # Time series - ("mano_right_trans", pa.list_(pa.list_(pa.float32(), 3)), list[list[float]], True), - ( - "mano_right_global_orient", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "mano_right_finger_pose", - pa.list_(pa.list_(pa.float32(), 45)), - list[list[float]], - True, - ), - ( - "mano_right_joints", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 21)), - list[list[list[float]]], - True, - ), - ( - "mano_right_joints_wxyz", - pa.list_(pa.list_(pa.list_(pa.float32(), 4), 21)), - list[list[list[float]]], - True, - ), - ("mano_right_fitting_err", pa.list_(pa.float32()), list[float], True), - ( - "mano_right_tips_distance", - pa.list_(pa.list_(pa.float32(), 5)), - list[list[float]], - True, - ), - ( - "mano_right_link_contact_positions", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_right_link_contact_normals", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_right_object_contact_positions", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_right_object_contact_normals", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_right_object_contact_part_ids", - pa.list_(pa.list_(pa.int32(), 16)), - list[list[int]], - True, - ), - # MANO left hand (time series) - ("mano_left_trans", pa.list_(pa.list_(pa.float32(), 3)), list[list[float]], True), - ( - "mano_left_global_orient", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "mano_left_finger_pose", - pa.list_(pa.list_(pa.float32(), 45)), - list[list[float]], - True, - ), - ( - "mano_left_joints", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 21)), - list[list[list[float]]], - True, - ), - ( - "mano_left_joints_wxyz", - pa.list_(pa.list_(pa.list_(pa.float32(), 4), 21)), - list[list[list[float]]], - True, - ), - ("mano_left_fitting_err", pa.list_(pa.float32()), list[float], True), - ( - "mano_left_tips_distance", - pa.list_(pa.list_(pa.float32(), 5)), - list[list[float]], - True, - ), - ( - "mano_left_link_contact_positions", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_left_link_contact_normals", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_left_object_contact_positions", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_left_object_contact_normals", - pa.list_(pa.list_(pa.list_(pa.float32(), 3), 16)), - list[list[list[float]]], - True, - ), - ( - "mano_left_object_contact_part_ids", - pa.list_(pa.list_(pa.int32(), 16)), - list[list[int]], - True, - ), -] - -############################################################# -# Robot fields -############################################################# -SHARPA_NUM_FRAMES = 67 -SHARPA_FIELDS: list[FieldSpec] = [ - ("right_robot_finger_joint_names", pa.list_(pa.string()), list[str], False), - ("right_robot_frame_names", pa.list_(pa.string()), list[str], False), - ("right_robot_frame_task_names", pa.list_(pa.string()), list[str], False), - ("left_robot_finger_joint_names", pa.list_(pa.string()), list[str], False), - ("left_robot_frame_names", pa.list_(pa.string()), list[str], False), - ("left_robot_frame_task_names", pa.list_(pa.string()), list[str], False), - # Time series - ( - "robot_right_wrist_position", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "robot_right_wrist_wxyz", - pa.list_(pa.list_(pa.float32(), 4)), - list[list[float]], - True, - ), - ( - "robot_right_finger_joints", - pa.list_(pa.list_(pa.float32(), 22)), - list[list[float]], - True, - ), - ( - "robot_right_frames", - pa.list_(pa.list_(pa.list_(pa.float32(), 7), SHARPA_NUM_FRAMES)), - list[list[list[float]]], - True, - ), - ( - "robot_right_frame_task_errors", - pa.list_(pa.list_(pa.float32(), 11)), - list[list[float]], - True, - ), - ("robot_right_num_optimization_iterations", pa.list_(pa.int32()), list[int], True), - ( - "robot_left_wrist_position", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "robot_left_wrist_wxyz", - pa.list_(pa.list_(pa.float32(), 4)), - list[list[float]], - True, - ), - ( - "robot_left_finger_joints", - pa.list_(pa.list_(pa.float32(), 22)), - list[list[float]], - True, - ), - ( - "robot_left_frames", - pa.list_(pa.list_(pa.list_(pa.float32(), 7), SHARPA_NUM_FRAMES)), - list[list[list[float]]], - True, - ), - ( - "robot_left_frame_task_errors", - pa.list_(pa.list_(pa.float32(), 11)), - list[list[float]], - True, - ), - ("robot_left_num_optimization_iterations", pa.list_(pa.int32()), list[int], True), -] - -DEX3_NUM_FINGER_JOINTS = 7 -DEX3_MANO_NUM_FRAMES = 23 -DEX3_NUM_FRAME_TASKS = 4 -DEX3_MANO_FIELDS: list[FieldSpec] = [ - ("right_robot_finger_joint_names", pa.list_(pa.string()), list[str], False), - ("right_robot_frame_names", pa.list_(pa.string()), list[str], False), - ("right_robot_frame_task_names", pa.list_(pa.string()), list[str], False), - ("left_robot_finger_joint_names", pa.list_(pa.string()), list[str], False), - ("left_robot_frame_names", pa.list_(pa.string()), list[str], False), - ("left_robot_frame_task_names", pa.list_(pa.string()), list[str], False), - # Time series - ( - "robot_right_wrist_position", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "robot_right_wrist_wxyz", - pa.list_(pa.list_(pa.float32(), 4)), - list[list[float]], - True, - ), - ( - "robot_right_finger_joints", - pa.list_(pa.list_(pa.float32(), DEX3_NUM_FINGER_JOINTS)), - list[list[float]], - True, - ), - ( - "robot_right_frames", - pa.list_(pa.list_(pa.list_(pa.float32(), 7), DEX3_MANO_NUM_FRAMES)), - list[list[list[float]]], - True, - ), - ( - "robot_right_frame_task_errors", - pa.list_(pa.list_(pa.float32(), DEX3_NUM_FRAME_TASKS)), - list[list[float]], - True, - ), - ("robot_right_num_optimization_iterations", pa.list_(pa.int32()), list[int], True), - ( - "robot_left_wrist_position", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "robot_left_wrist_wxyz", - pa.list_(pa.list_(pa.float32(), 4)), - list[list[float]], - True, - ), - ( - "robot_left_finger_joints", - pa.list_(pa.list_(pa.float32(), DEX3_NUM_FINGER_JOINTS)), - list[list[float]], - True, - ), - ( - "robot_left_frames", - pa.list_(pa.list_(pa.list_(pa.float32(), 7), DEX3_MANO_NUM_FRAMES)), - list[list[list[float]]], - True, - ), - ( - "robot_left_frame_task_errors", - pa.list_(pa.list_(pa.float32(), DEX3_NUM_FRAME_TASKS)), - list[list[float]], - True, - ), - ("robot_left_num_optimization_iterations", pa.list_(pa.int32()), list[int], True), -] - -############################################################# -# Object fields -############################################################# -OBJECT_FIELDS: list[FieldSpec] = [ - ("object_name", pa.string(), str, False), - ("safe_object_name", pa.string(), str, False), - ("object_body_names", pa.list_(pa.string()), list[str], False), - ("safe_object_body_names", pa.list_(pa.string()), list[str], False), - ("object_mesh_paths", pa.list_(pa.string()), list[str], False), - ("object_urdf_paths", pa.list_(pa.string()), list[str], False), - ("object_mesh_radius", pa.list_(pa.float32()), list[float], False), - # Time series - ("object_articulation", pa.list_(pa.float32()), list[float], True), - ( - "object_root_axis_angle", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "object_root_position", - pa.list_(pa.list_(pa.float32(), 3)), - list[list[float]], - True, - ), - ( - "object_body_position", - pa.list_(pa.list_(pa.list_(pa.float32(), 3))), - list[list[list[float]]], - True, - ), - ( - "object_body_wxyz", - pa.list_(pa.list_(pa.list_(pa.float32(), 4))), - list[list[list[float]]], - True, - ), -] - - -############################################################# -# Data logger class factory -############################################################# -def create_data_logger_class( - name: str, - field_specs: list[FieldSpec], -) -> type: - """Create a data logger class from field specifications. - - Args: - name: Name of the generated class. - field_specs: List of field specifications to include. - - Returns: - A dataclass type with logging and serialization methods. - """ - # Build schema - schema = pa.schema([(name, pa_type) for name, pa_type, _, _ in field_specs]) - - # Sort fields, keep metadata before time-series - sorted_specs = sorted(field_specs, key=lambda x: x[3]) - - # Build dataclass fields - dataclass_fields: list[tuple[str, Any, Any]] = [] - for field_name, _, py_type, is_time_series in sorted_specs: - if is_time_series: - dataclass_fields.append((field_name, py_type, field(default_factory=list))) - else: - dataclass_fields.append((field_name, py_type, MISSING)) - - # Create base dataclass - base_cls = make_dataclass(name, dataclass_fields) - - # Add methods - class DataLoggerMixin: - """Mixin providing logging and serialization methods.""" - - _schema = schema - _field_specs = field_specs - - def log_timestep(self, **kwargs: Any) -> None: - """Log data for a single timestep.""" - time_series_fields = { - fname for fname, _, _, is_ts in self._field_specs if is_ts - } - for key, value in kwargs.items(): - if value is not None: - if key not in time_series_fields: - raise ValueError( - f"'{key}' is not a valid time-series field. " - f"Valid fields: {sorted(time_series_fields)}" - ) - getattr(self, key).append(value) - - def to_dict(self) -> dict: - """Convert to dictionary for serialization.""" - return {f.name: getattr(self, f.name) for f in fields(self)} # type: ignore[arg-type] - - def save_to_parquet( - self, root_path: str, partition_cols: Optional[list[str]] = None - ) -> None: - """Save to Parquet file. - - If partition_cols are specified and the partition directory already - exists, it is removed first so stale data is not left behind. - """ - table = pa.Table.from_pylist([self.to_dict()], schema=self._schema) - - # Remove existing partition directory to avoid duplicate files. - # Read partition values from the table (after pyarrow casting) so - # directory names match exactly what write_to_dataset creates. - if partition_cols: - partition_dir = Path(root_path) - row = table.to_pydict() - for col in partition_cols: - val = row[col][0] - partition_dir = partition_dir / f"{col}={val}" - if partition_dir.is_dir(): - shutil.rmtree(partition_dir) - - pq.write_to_dataset( - table, - root_path=root_path, - partition_cols=partition_cols, - compression="zstd", - ) - - @classmethod - def from_parquet( - cls, - root_path: str, - filters: Optional[list] = None, - trajectory_id: int = 0, - ) -> Any: - """Load from Parquet file.""" - parquet_filters: Optional[list[tuple[str, str, Any]]] = None - contains_filters: list[tuple[str, str]] = [] - if filters: - parquet_filters = [] - for col, op, val in filters: - if op == "contains": - contains_filters.append((col, val)) - else: - parquet_filters.append((col, op, val)) - if not parquet_filters: - parquet_filters = None - - dataset = pq.read_table( - root_path, filters=parquet_filters, schema=cls._schema - ) - for col, substring in contains_filters: - dataset = dataset.filter(pc.match_substring(dataset[col], substring)) - - num_rows = len(dataset) - if num_rows == 0: - raise ValueError(f"No data found in {root_path} with filters {filters}") - if num_rows > 1: - print(f"Multiple rows {num_rows} found. Using row {trajectory_id}.") - dataset = dataset.slice(trajectory_id, trajectory_id + 1) - - # Select columns in schema order and cast so partition columns have correct types - dataset = dataset.select(cls._schema.names) - dataset = dataset.cast(cls._schema) - data_dict = dataset.to_pylist()[0] - return cls(**data_dict) - - # Combine base class with mixin - combined_cls = type(name, (DataLoggerMixin, base_cls), {}) - return combined_cls - - -def list_sequence_ids(root_path: str) -> list[str]: - """Return sorted list of unique sequence_id values in the Parquet dataset. - - Reads partition directory names when ``sequence_id`` is a partition column, - avoiding a full table scan. Falls back to reading the column if the - directory structure doesn't match. - """ - root = Path(root_path) - if not root.is_dir(): - print(f"Input directory not found: {root_path}. Run the loader first.") - return [] - # Partitioned datasets store sequence_id as directory names: sequence_id=/ - partition_dirs = sorted(root.glob("sequence_id=*/")) - if partition_dirs: - return [unquote(d.name.split("=", 1)[1]) for d in partition_dirs] - # Fallback: read the column from Parquet files - table = pq.read_table(root_path, columns=["sequence_id"]) - ids = pc.unique(table["sequence_id"]) - return sorted(ids.to_pylist()) - - -def shard_matches(key: str, shard_id: int, num_shards: int) -> bool: - """Return True iff ``key`` belongs to this shard. - - Uses md5 for a stable, process-independent partition so the same key - always lands in the same shard regardless of ``PYTHONHASHSEED``. - Pass ``num_shards <= 1`` to disable sharding (matches everything). - """ - if num_shards <= 1: - return True - h = int(hashlib.md5(key.encode()).hexdigest(), 16) - return h % num_shards == shard_id - - -def add_sequence_filter_args(parser: argparse.ArgumentParser) -> None: - """Add --sequence_pattern, --sequence_id, --max_sequences, --shard_id, --num_shards.""" - group = parser.add_argument_group("sequence filtering") - group.add_argument( - "--sequence_id", - type=str, - default=None, - help="Process a single sequence by exact ID.", - ) - group.add_argument( - "--sequence_pattern", - type=str, - default=None, - help="Regex pattern to filter sequence IDs (e.g., '.*box.*').", - ) - group.add_argument( - "--max_sequences", - type=int, - default=None, - help="Limit to first N sequences after filtering.", - ) - group.add_argument( - "--shard_id", - type=int, - default=0, - help="Shard index (0-based) for parallel processing.", - ) - group.add_argument( - "--num_shards", - type=int, - default=1, - help="Total number of shards. 1 = no sharding (default).", - ) - - -def filter_sequence_ids(sequence_ids: list[str], args: argparse.Namespace) -> list[str]: - """Apply all sequence filters, including shard partitioning (applied last).""" - if getattr(args, "sequence_id", None): - sequence_ids = [s for s in sequence_ids if s == args.sequence_id] - if getattr(args, "sequence_pattern", None): - pat = re.compile(args.sequence_pattern) - sequence_ids = [s for s in sequence_ids if pat.search(s)] - if getattr(args, "max_sequences", None): - sequence_ids = sequence_ids[: args.max_sequences] - - num_shards = getattr(args, "num_shards", 1) or 1 - shard_id = getattr(args, "shard_id", 0) or 0 - if num_shards > 1: - sequence_ids = [ - s for s in sequence_ids if shard_matches(s, shard_id, num_shards) - ] - return sequence_ids - - -############################################################# -# Logger Classes -############################################################# -# ManoSharpaData remains the logger for the dual-hand V2P pipeline. -# The whole-body G1 and hand-only Dex3 producers write the unified -# ``motion_v1`` schema directly via ``robotic_grounding.motion_schema``; -# no dedicated logger class is needed for them. -ManoSharpaData = create_data_logger_class( - "ManoSharpaData", - BASE_FIELDS + MANO_FIELDS + SHARPA_FIELDS + OBJECT_FIELDS, -) - -ManoDex3Data = create_data_logger_class( - "ManoDex3Data", - BASE_FIELDS + MANO_FIELDS + DEX3_MANO_FIELDS + OBJECT_FIELDS, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/dataset_registry.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/dataset_registry.py deleted file mode 100644 index f4bd392f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/dataset_registry.py +++ /dev/null @@ -1,219 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Central dataset registry — single source of truth for dataset properties. - -Every script that needs to know about datasets (loaders, CSS tools, URDF -generation, training validation) should import from here instead of -maintaining its own hardcoded constants. - -Usage:: - - from robotic_grounding.retarget.dataset_registry import ( - get_dataset_config, - get_all_dataset_names, - ) - - config = get_dataset_config("taco") - print(config.fps) # 30.0 - print(config.mesh_format) # "obj" -""" - -from __future__ import annotations - -from dataclasses import dataclass, field -from typing import Any - -BASE_CSS_PREFIX = "v2d/human_motion_data" - - -@dataclass(frozen=True) -class DatasetConfig: - """Immutable configuration for a single dataset. - - Attributes: - name: Short identifier (e.g. "taco", "arctic"). - fps: Frames per second of the source motion data. - mano_kwargs: Keyword arguments for MANO forward kinematics - (flat_hand_mean, center_idx); consumed by the loader in reconstruction. - mesh_vertex_scale: Scale factor to convert mesh vertices to meters. - 0.01 for TACO (centimeters), 1.0 for datasets already in meters. - mesh_format: Source mesh file format ("obj" or "glb"). - has_articulated_objects: Whether objects have articulated parts - (e.g. Arctic drawers/scissors). Affects URDF generation. - has_contact_data: Whether processed parquets include per-link - contact positions/normals. - has_support_surfaces: Whether support surface reconstruction is - expected to produce output. - link_to_site_quat_wxyz: Quaternion (w, x, y, z) for the MANO - link-to-site transform used during IK retargeting. None if - no transform is needed. - loaded_suffix: Suffix for the loaded data directory name - (e.g. "_loaded" -> "{name}_loaded"). - processed_suffix: Suffix for the processed data directory name - (e.g. "_processed" -> "{name}_processed"). - css_raw_prefix: Subdirectory under the dataset's CSS path for raw - data. Empty string means "dataset/" (the default). - TACO overrides this to "dataset/Hand_Poses/". - loader_script: Deprecated/unused — the Stage-1 loaders moved to - reconstruction's v2d_task_library_loader (MANO/GPL); Stage-1 load now - runs in the reconstruction load workflow. Left empty here. - retarget_scripts: Mapping from robot name (e.g. "sharpa_wave", - "dex3") to retarget script path (relative to repo root). Allows - ``run_retarget.py --dataset arctic --robot dex3`` to dispatch to - the right per-robot retargeter. - """ - - # Identity - name: str - - # Loader metadata - fps: float - mano_kwargs: dict[str, Any] = field(default_factory=dict) - mesh_vertex_scale: float = 1.0 - mesh_format: str = "obj" - - # Capabilities - has_articulated_objects: bool = False - has_contact_data: bool = True - has_support_surfaces: bool = True - - # IK retargeting - link_to_site_quat_wxyz: tuple[float, ...] | None = None - - # Storage path conventions (relative to HUMAN_MOTION_DATA_DIR/{name}/) - loaded_suffix: str = "_loaded" - processed_suffix: str = "_processed" - - # CSS storage - css_raw_prefix: str = "" - - # Script dispatch (relative to repo root). - # NOTE: loader_script (Stage-1 load) is no longer populated — the loaders moved - # to reconstruction's v2d_task_library_loader (MANO/GPL). Kept (empty) for - # back-compat. retarget_scripts (Stage-2 IK) stay in robotic_grounding. - loader_script: str = "" - retarget_scripts: dict[str, str] = field(default_factory=dict) - - -# --------------------------------------------------------------------------- -# Registry -# --------------------------------------------------------------------------- - -DATASET_CONFIGS: dict[str, DatasetConfig] = { - "taco": DatasetConfig( - name="taco", - fps=30.0, - mano_kwargs={"flat_hand_mean": True, "center_idx": 0}, - mesh_vertex_scale=0.01, - mesh_format="obj", - has_articulated_objects=False, - has_contact_data=True, - css_raw_prefix="dataset/Hand_Poses/", - retarget_scripts={ - "sharpa_wave": "scripts/retarget/taco_to_sharpa.py", - "dex3": "scripts/retarget/taco_to_dex3.py", - }, - ), - "arctic": DatasetConfig( - name="arctic", - fps=30.0, - mano_kwargs={"flat_hand_mean": False, "center_idx": None}, - mesh_vertex_scale=1.0, - mesh_format="obj", - has_articulated_objects=True, - has_contact_data=True, - link_to_site_quat_wxyz=(0.5, -0.5, 0.5, 0.5), - retarget_scripts={ - "sharpa_wave": "scripts/retarget/arctic_to_sharpa.py", - "dex3": "scripts/retarget/arctic_to_dex3.py", - }, - ), - "oakink2": DatasetConfig( - name="oakink2", - fps=120.0, - mano_kwargs={"flat_hand_mean": True, "center_idx": 0}, - mesh_vertex_scale=1.0, - mesh_format="obj", - has_articulated_objects=False, - has_contact_data=True, - retarget_scripts={"sharpa_wave": "scripts/retarget/oakink2_to_sharpa.py"}, - ), - "hot3d": DatasetConfig( - name="hot3d", - fps=30.0, - mano_kwargs={"flat_hand_mean": False, "center_idx": None}, - mesh_vertex_scale=1.0, - mesh_format="glb", - has_articulated_objects=False, - has_contact_data=True, - retarget_scripts={"sharpa_wave": "scripts/retarget/hot3d_to_sharpa.py"}, - ), - "h2o": DatasetConfig( - name="h2o", - fps=30.0, - mano_kwargs={"flat_hand_mean": False, "center_idx": None}, - mesh_vertex_scale=1.0, - mesh_format="obj", - has_articulated_objects=False, - has_contact_data=True, - retarget_scripts={"sharpa_wave": "scripts/retarget/h2o_to_sharpa.py"}, - ), - "grab": DatasetConfig( - name="grab", - fps=120.0, - mano_kwargs={"flat_hand_mean": False, "center_idx": None}, - mesh_vertex_scale=1.0, - mesh_format="obj", - has_articulated_objects=False, - has_contact_data=True, - retarget_scripts={"sharpa_wave": "scripts/retarget/grab_to_sharpa.py"}, - ), - "dexycb": DatasetConfig( - name="dexycb", - fps=30.0, - mano_kwargs={"flat_hand_mean": False, "center_idx": None}, - mesh_vertex_scale=1.0, - mesh_format="obj", - has_articulated_objects=False, - has_contact_data=True, - retarget_scripts={"sharpa_wave": "scripts/retarget/dexycb_to_sharpa.py"}, - ), -} - - -# --------------------------------------------------------------------------- -# Public API -# --------------------------------------------------------------------------- - - -def get_dataset_config(name: str) -> DatasetConfig: - """Return the configuration for a dataset. - - Raises: - ValueError: If *name* is not registered. - """ - if name not in DATASET_CONFIGS: - available = ", ".join(sorted(DATASET_CONFIGS)) - raise ValueError(f"Unknown dataset '{name}'. Available datasets: {available}") - return DATASET_CONFIGS[name] - - -def get_all_dataset_names() -> tuple[str, ...]: - """Return all registered dataset names (insertion order).""" - return tuple(DATASET_CONFIGS) - - -def get_css_stage_prefixes(name: str) -> dict[str, str]: - """Derive the CSS S3 stage prefixes for a dataset. - - Returns a dict with keys "raw", "loaded", "processed" mapping to - S3 prefix strings under the ``BASE_CSS_PREFIX``. - """ - config = get_dataset_config(name) - base = f"{BASE_CSS_PREFIX}/{name}" - raw_prefix = config.css_raw_prefix or "dataset/" - return { - "raw": f"{base}/{raw_prefix}", - "loaded": f"{base}/{name}{config.loaded_suffix}/", - "processed": f"{base}/{name}{config.processed_suffix}/", - } diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/ground_alignment.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/ground_alignment.py deleted file mode 100644 index 2a48c36f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/ground_alignment.py +++ /dev/null @@ -1,973 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Post-process modules that correct drift in retargeted whole-body data. - -The retarget loop in ``scripts/retarget/soma_to_g1.py`` anchors the robot to -the ground on frame 0 but has no mechanism to track drift over the rest of the -sequence. The reconstruction's camera-to-world mapping can drift several -centimeters over long sequences, causing the robot to appear to sink or float -and causing objects to misalign with their rest surfaces. - -This module provides a two-pass correction that runs **after** the existing -retarget loop. It consumes the loop's outputs (packaged in -:class:`FirstPassResult`) and produces two corrections: - -1. A per-frame Z-offset for the robot (:func:`compute_plane_alignment_offsets`) - that drags the lowest contact point (e.g. foot sole) to a reference plane - every frame. The plane abstraction (:class:`ReferencePlane`) is - horizontal-only for now (``z = constant``) but the call shape is - intentionally future-proof: a ``contact_xyz`` array of any contact points - the caller decides are relevant, and a plane object that knows its own - signed distance. -2. A per-frame corrected object trajectory (:func:`correct_object_trajectory`) - that follows two simple rules: - - - **Rule 1** (interaction): when the robot is interacting with the object, - preserve the reconstructed hand-to-object relative pose by applying the - same robot Z-delta to the object. - - **Rule 2** (no interaction): the object is stationary in world frame, - anchored to the adjacent contact segment so that release / pickup are - seamless (no pop). Specifically, a no-interaction segment is held at: - * the **last frame of the preceding contact** (release pose) when one - exists, else - * the **first frame of the following contact** (upcoming pickup pose) - when one exists, else - * the segment median (degenerate case: sequence has no contact at - all, so there is no physical "placed" pose to honor). - This mirrors what physically happens: the object is set down at - ``P_release`` and stays there until the next pickup. - -Interaction detection is its own pluggable module -(:func:`compute_interaction_mask`) so future strategies (beyond hand-contact) -plug in without touching object correction. - -All functions are pure NumPy and do **not** require Pinocchio or Isaac. They -can be unit-tested against a synthetic :class:`FirstPassResult` without -rerunning IK. -""" - -from __future__ import annotations - -import json -from dataclasses import dataclass -from pathlib import Path -from typing import Literal - -import numpy as np -from scipy.signal import medfilt, savgol_filter -from scipy.spatial.transform import Rotation as R - -# --------------------------------------------------------------------------- -# Data classes -# --------------------------------------------------------------------------- - - -@dataclass -class FirstPassResult: - """Outputs from the retarget loop (pass 1). - - All arrays are post-pass-1 (i.e. the loop's frame-0 ``ground_z_offset`` is - already applied), so the ``ankle_frame_xyz`` Z values line up with - ``ee_pose_w`` Z values. All positions are in meters, all orientations are - wxyz quaternions unless suffixed with ``axis_angle``. - """ - - fps: float - # Robot world-frame trajectories (T, ...). - robot_root_position: np.ndarray # (T, 3) - robot_root_wxyz: np.ndarray # (T, 4) - robot_joint_positions: np.ndarray # (T, J) - ee_pose_w: np.ndarray # (T, E, 7) -- [x,y,z,qw,qx,qy,qz] - # Object trajectories (T, ...). - object_root_position: np.ndarray # (T, 3) - object_root_axis_angle: np.ndarray # (T, 3) - object_body_position: np.ndarray # (T, B, 3) - object_body_wxyz: np.ndarray # (T, B, 4) - # Per-side per-frame hand-contact mask (0.0/1.0). - hand_contact_active_per_frame: np.ndarray # (T, S) - # Source-raw translations shifted by pass-1 ground offset. - source_head_translation: np.ndarray # (T, 3) - source_root_translation: np.ndarray # (T, 3) - # NEW: cached ankle XYZ per frame (post pass-1 ground offset, pre-clamp). - # Consistent with ee_pose_w's Z convention. - ankle_frame_xyz: np.ndarray # (T, 2, 3) -- index 0 = left, 1 = right - - -@dataclass(frozen=True) -class ReferencePlane: - """Plane defined by a unit normal and a signed offset. - - Plane equation: ``normal . p + offset = 0`` (``p`` in robot world frame). - For a horizontal plane at ``z = z0``, ``normal = (0, 0, 1)`` and - ``offset = -z0`` so the legacy ``ReferencePlane(z=0.0)`` becomes - ``ReferencePlane.horizontal(z=0.0)``. - - The plane is oriented so ``normal[2] > 0`` in robot frame; a transformed - plane whose vertical component is below ``1e-6`` raises because a - near-vertical "ground" plane is unusable for foot anchoring. - - Attributes: - normal: Unit-normal triple. Stored as a tuple so the dataclass stays - hashable; normalized at construction. - offset: Signed offset such that ``n . p + offset = 0`` lies on the - plane. - """ - - normal: tuple[float, float, float] = (0.0, 0.0, 1.0) - offset: float = 0.0 - - def __post_init__(self) -> None: - """Normalize ``normal`` (preserving direction) and validate vertical.""" - n = np.asarray(self.normal, dtype=np.float64) - norm = float(np.linalg.norm(n)) - if norm < 1e-12: - raise ValueError(f"ReferencePlane normal has zero length: {self.normal}") - n = n / norm - if abs(float(n[2])) < 1e-6: - raise ValueError( - "ReferencePlane normal is near-horizontal " - f"(normal_z={float(n[2]):+.3e}); cannot ground feet on a vertical " - "plane. Reorient the plane before constructing it." - ) - if float(n[2]) < 0.0: - n = -n - object.__setattr__(self, "offset", -float(self.offset)) - object.__setattr__(self, "normal", (float(n[0]), float(n[1]), float(n[2]))) - - @classmethod - def horizontal(cls, z: float = 0.0) -> "ReferencePlane": - """Return a horizontal plane at world Z = ``z`` (normal = +Z).""" - return cls(normal=(0.0, 0.0, 1.0), offset=-float(z)) - - @property - def normal_z(self) -> float: - """Vertical component of the (already-oriented) unit normal.""" - return float(self.normal[2]) - - def signed_distance(self, points: np.ndarray) -> np.ndarray: - """Signed distance ``n . p + offset`` for points ``(..., 3)``. - - Positive = above the plane along the unit normal (must-stay-above - convention). - """ - arr = np.asarray(points, dtype=np.float64) - if arr.ndim == 0 or arr.shape[-1] != 3: - raise ValueError(f"points last axis must be 3; got shape {arr.shape}") - n = np.asarray(self.normal, dtype=np.float64) - return np.einsum("...i,i->...", arr, n) + float(self.offset) - - def vertical_offset_to_plane(self, points: np.ndarray) -> np.ndarray: - """Signed Z shift that drives each point's signed-distance to zero. - - For a horizontal plane this is just ``-signed_distance``. For a - tilted plane with vertical component ``n_z``, vertical shift - ``dz`` reduces signed distance by ``n_z * dz``, so the shift that - zeros the distance is ``dz = -signed_distance / n_z``. - """ - return -self.signed_distance(points) / self.normal_z - - -@dataclass -class PlaneAlignmentConfig: - """Configuration for :func:`compute_plane_alignment_offsets`. - - The algorithm is deliberately simple: per frame, offset = negative of the - minimum signed distance across contact points, then smooth with a - rolling median (robust to brief swing-phase foot lifts and single-frame - reconstruction outliers). An optional Savitzky-Golay pass after the - median can produce gentler trajectory transitions. - - Attributes: - median_window: Rolling-median window size (frames). Should be wider - than the longest expected "both feet off the ground" gap and - narrower than the timescale of real drift. Default ``11`` frames - covers typical walking swing phases at 30 fps (~0.37 s). - savgol_window: Optional Savitzky-Golay post-smoothing window length - (odd frames). Set ``0`` to disable. Defaults to disabled because - the rolling median alone is already a strong attenuator of - single-frame noise, and extra smoothing lags real drift onsets. - savgol_polyorder: Savitzky-Golay polynomial order. Only used when - ``savgol_window > 0``. - """ - - median_window: int = 11 - savgol_window: int = 0 - savgol_polyorder: int = 3 - - -@dataclass -class InteractionMaskConfig: - """Configuration for :func:`compute_interaction_mask`. - - Attributes: - strategy: Detection strategy. - - - ``hand_contact`` (default): OR the per-side - ``hand_contact_active_per_frame`` signal from pass 1. This - is the CORRECT-BY-DEFAULT choice because: - - 1. ``hand_contact_active_per_frame`` now runs against the - object mesh at its raw (ungrounded) reconstruction Z, so - finger/palm contact fires accurately on true grips (e.g. - on ``skinny_wood_chair`` it reports ~1200 any-side frames - of real grip, vs. the noise-corrupted velocity signal). - 2. Rule 2 in ``correct_object_trajectory`` anchors the - object to the adjacent contact pose during no-contact - runs, which is what a physically-stationary placed / - pre-pickup object should look like. That anchoring only - triggers for no-contact frames, so an overly-eager - interaction mask (e.g. the old velocity default) forces - the object to track its noisy raw trajectory everywhere - and the "rest" segments visibly jitter. - - - ``hand_contact_or_velocity``: OR of hand_contact with a - simple object-motion detector. A frame is interaction if - its object root XY speed exceeds ``v_interact_mps``. - Intended for sequences where the object is carried by - non-hand body parts (e.g. a box hugged to the torso) so the - palm/fingertip contact detector does not fire. NOT the - default because the velocity signal is easily tripped by - reconstruction noise during rest, which eliminates all - no-contact frames and disables Rule 2 (the anchoring that - keeps placed objects still). - - v_interact_mps: Object-root XY speed threshold (meters per second) - used only by ``hand_contact_or_velocity``. Defaults to 5 cm/s. - pad_frames: Number of frames to dilate the velocity-based mask on - both sides so brief slow moments inside a carry don't chop - the segment into tiny pieces. - """ - - strategy: Literal["hand_contact", "hand_contact_or_velocity"] = "hand_contact" - v_interact_mps: float = 0.05 - pad_frames: int = 3 - - -@dataclass -class ObjectCorrectionConfig: - """Configuration for :func:`correct_object_trajectory`. - - Attributes: - min_seg_frames: Segments shorter than this are absorbed into the - adjacent segment to suppress single-frame contact dropouts. - boundary_blend_frames: Number of frames over which to blend the - correction across a segment boundary. Symmetric cosine S-curve - on position; slerp on orientation. - """ - - min_seg_frames: int = 3 - boundary_blend_frames: int = 6 - - -# --------------------------------------------------------------------------- -# Module 2: robot plane alignment -# --------------------------------------------------------------------------- - - -def compute_plane_alignment_offsets( - contact_xyz: np.ndarray, - plane: ReferencePlane, - cfg: PlaneAlignmentConfig, -) -> np.ndarray: - """Per-frame offset (along +Z) that puts the lowest contact on the plane. - - This is the "drag lowest contact to the plane" strategy, which replaces - the prior stance-gated contact-aware drift estimator. The new approach - is more robust to time-varying ground drift (the chair-sequence regime) - because it does not depend on the cascade - stance-detect -> NaN -> interp -> savgol -> pin-frame-0 that was - silently losing amplitude when the real ground level moved mid-sequence. - - Algorithm: - - 1. Compute signed distance to the plane for every contact point per - frame. - 2. Take the minimum per frame (the foot closest to / deepest below the - plane). The offset that would place that point exactly on the plane - is ``-min(signed_distance)``. - 3. Apply a rolling median to absorb short excursions where every - contact is briefly above the plane (e.g. a single swing-phase frame - where both feet happen to be off the ground in reconstruction noise) - or single-frame reconstruction spikes. - 4. Optionally apply Savitzky-Golay smoothing on top for gentler - transitions (disabled by default). - - Args: - contact_xyz: ``(T, K, 3)`` world-frame contact points per frame - (e.g. per-side sole XYZ, or palm XYZ, or any point set the - caller decides is the relevant grounding contact). - plane: Reference plane. Horizontal-only for now. - cfg: Smoothing parameters. - - Returns: - ``(T,)`` offsets. For a horizontal plane, callers apply as - ``position[..., 2] += offsets[...]`` to any world-frame Z - coordinate they want grounded to the plane. - """ - arr = np.asarray(contact_xyz, dtype=np.float64) - if arr.ndim != 3 or arr.shape[-1] != 3: - raise ValueError("contact_xyz must have shape (T, K, 3); " f"got {arr.shape}") - T, K, _ = arr.shape - if T == 0: - return np.zeros(0, dtype=np.float64) - if K == 0: - return np.zeros(T, dtype=np.float64) - - # Per-frame raw offset: vertical shift that drives the lowest contact - # point's signed distance to zero (``dz = -signed_distance / normal_z``; - # for a horizontal plane this collapses to the legacy ``-signed_distance``). - dz = plane.vertical_offset_to_plane(arr) # (T, K) - raw = dz.max(axis=1) # (T,) -- lift by the largest correction needed - - # Rolling median smoothing. - win = max(1, int(cfg.median_window)) - if win > 1 and T >= 3: - if win % 2 == 0: - win += 1 - # medfilt requires kernel_size <= T and odd; clamp safely. - win = min(win, T if T % 2 == 1 else T - 1) - if win >= 3: - smoothed = medfilt(raw, kernel_size=win) - else: - smoothed = raw.copy() - else: - smoothed = raw.copy() - - # Optional Savgol smoothing (off by default). - sw = int(cfg.savgol_window) - if sw > 0 and T > sw: - if sw % 2 == 0: - sw += 1 - poly = max(1, min(int(cfg.savgol_polyorder), sw - 1, 5)) - smoothed = savgol_filter(smoothed, sw, poly, mode="nearest") - - return smoothed.astype(np.float64) - - -# --------------------------------------------------------------------------- -# Module 3: interaction mask -# --------------------------------------------------------------------------- - - -def compute_interaction_mask( - first_pass: FirstPassResult, - cfg: InteractionMaskConfig, -) -> np.ndarray: - """Per-frame boolean mask for robot-object interaction. - - See :class:`InteractionMaskConfig` for the available strategies. The - returned mask is consumed by :func:`correct_object_trajectory`: true - frames get Rule 1 (object follows the robot), false frames get Rule 2 - (object is stationary, anchored to the adjacent contact). - - Args: - first_pass: Pass-1 outputs. Uses - ``hand_contact_active_per_frame`` and, for the velocity-based - strategy, ``object_root_position`` and ``fps``. - cfg: Strategy selection. - - Returns: - ``(T,)`` boolean array where ``True`` means the robot is interacting - with the object on that frame. - """ - hca = np.asarray(first_pass.hand_contact_active_per_frame, dtype=np.float64) - if hca.ndim == 0: - T = 0 - elif hca.ndim == 1: - T = hca.shape[0] - hca = hca[:, None] - else: - T = hca.shape[0] - - if T == 0: - return np.zeros(0, dtype=bool) - - # Hand-contact component: OR across sides. - if hca.shape[1] == 0: - hand_mask = np.zeros(T, dtype=bool) - else: - hand_mask = np.any(hca > 0.5, axis=1) - - if cfg.strategy == "hand_contact": - return hand_mask - if cfg.strategy != "hand_contact_or_velocity": - raise NotImplementedError( - f"Unknown interaction-mask strategy: {cfg.strategy!r}" - ) - - # Velocity-based component: frames where the object root is moving - # faster than the threshold (in XY plane — vertical motion alone is a - # weaker signal and can be caused by gravity/settling). - obj_pos = np.asarray(first_pass.object_root_position, dtype=np.float64) - if obj_pos.ndim != 2 or obj_pos.shape[-1] != 3 or obj_pos.shape[0] != T: - return hand_mask - fps = float(first_pass.fps) - if fps <= 0 or T < 2: - return hand_mask - dt = 1.0 / fps - # Finite-difference speed; pad the first frame with 0. - diff = np.diff(obj_pos[:, :2], axis=0) - speed = np.concatenate( - [np.zeros(1, dtype=np.float64), np.linalg.norm(diff, axis=-1) / dt] - ) # (T,) - vel_mask = speed > float(cfg.v_interact_mps) - - # Dilate the velocity mask by `pad_frames` on each side so brief slow - # moments within a carry don't fragment the interaction segment. - pad = max(0, int(cfg.pad_frames)) - if pad > 0 and vel_mask.any(): - kernel = np.ones(2 * pad + 1, dtype=bool) - padded = np.convolve( - vel_mask.astype(np.int32), kernel.astype(np.int32), mode="same" - ) - vel_mask = padded > 0 - - return hand_mask | vel_mask - - -# --------------------------------------------------------------------------- -# Module 4: object trajectory correction -# --------------------------------------------------------------------------- - - -def correct_object_trajectory( - first_pass: FirstPassResult, - interaction_mask: np.ndarray, - robot_delta_z: np.ndarray, - cfg: ObjectCorrectionConfig, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Corrected object trajectory from rule-1 / rule-2 / boundary blend. - - Args: - first_pass: Pass-1 outputs. Uses object arrays and their shapes. - interaction_mask: ``(T,)`` bool from :func:`compute_interaction_mask`. - robot_delta_z: ``(T,)`` from :func:`compute_plane_alignment_offsets`. - cfg: Segmentation / blending parameters. - - Returns: - Tuple of ``(root_position, root_axis_angle, body_position, - body_wxyz)`` as arrays with the same shape as their ``first_pass`` - counterparts. - """ - raw_root_pos = np.asarray(first_pass.object_root_position, dtype=np.float64) - raw_root_aa = np.asarray(first_pass.object_root_axis_angle, dtype=np.float64) - raw_body_pos = np.asarray(first_pass.object_body_position, dtype=np.float64) - raw_body_wxyz = np.asarray(first_pass.object_body_wxyz, dtype=np.float64) - mask = np.asarray(interaction_mask, dtype=bool) - delta_z = np.asarray(robot_delta_z, dtype=np.float64) - - T = raw_root_pos.shape[0] - if mask.shape[0] != T or delta_z.shape[0] != T: - raise ValueError( - f"Length mismatch: T={T}, mask={mask.shape[0]}, " - f"delta_z={delta_z.shape[0]}" - ) - - corr_root_pos = raw_root_pos.copy() - corr_root_aa = raw_root_aa.copy() - corr_body_pos = raw_body_pos.copy() - corr_body_wxyz = raw_body_wxyz.copy() - - if T == 0: - return corr_root_pos, corr_root_aa, corr_body_pos, corr_body_wxyz - - # Derive raw root quats from axis-angle once. - raw_root_quat = R.from_rotvec(raw_root_aa).as_quat(scalar_first=True) # (T, 4) - - # Enumerate and merge short segments. - segments = _enumerate_segments(mask) - segments = _absorb_short_segments(segments, cfg.min_seg_frames) - - # Pre-compute a constant anchor pose for each no-contact segment. The - # anchor is the adjacent contact segment's corrected boundary pose - # (preceding contact's last frame preferred; following contact's first - # frame as the fallback). If no contact exists anywhere in the sequence - # we degrade to the segment median (there is no physical "placed" pose - # to honor in that case). - no_contact_anchors: dict[ - int, tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] - ] = {} - for idx, seg in enumerate(segments): - if not seg[2]: - no_contact_anchors[idx] = _compute_no_contact_anchor( - seg_idx=idx, - segments=segments, - raw_root_pos=raw_root_pos, - raw_root_quat=raw_root_quat, - raw_body_pos=raw_body_pos, - raw_body_wxyz=raw_body_wxyz, - delta_z=delta_z, - ) - - # Phase A: per-segment native correction (no blending). - for idx, seg in enumerate(segments): - _write_segment( - seg, - no_contact_anchor=no_contact_anchors.get(idx), - raw_root_pos=raw_root_pos, - raw_root_quat=raw_root_quat, - raw_body_pos=raw_body_pos, - raw_body_wxyz=raw_body_wxyz, - delta_z=delta_z, - corr_root_pos=corr_root_pos, - corr_root_aa=corr_root_aa, - corr_body_pos=corr_body_pos, - corr_body_wxyz=corr_body_wxyz, - ) - - # Phase B: boundary blends. - blend = max(0, int(cfg.boundary_blend_frames)) - if blend > 0 and len(segments) > 1: - for i in range(len(segments) - 1): - _blend_boundary( - left_seg=segments[i], - right_seg=segments[i + 1], - left_anchor=no_contact_anchors.get(i), - right_anchor=no_contact_anchors.get(i + 1), - blend_frames=blend, - raw_root_pos=raw_root_pos, - raw_root_quat=raw_root_quat, - raw_body_pos=raw_body_pos, - raw_body_wxyz=raw_body_wxyz, - delta_z=delta_z, - corr_root_pos=corr_root_pos, - corr_root_aa=corr_root_aa, - corr_body_pos=corr_body_pos, - corr_body_wxyz=corr_body_wxyz, - ) - - return corr_root_pos, corr_root_aa, corr_body_pos, corr_body_wxyz - - -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- - - -def _quaternion_mean(quats_wxyz: np.ndarray) -> np.ndarray: - """Markley quaternion mean of ``(N, 4)`` wxyz quats. Returns ``(4,)``. - - Handles the quaternion double-cover via the largest-eigenvalue eigenvector - of the (4x4) outer-product-sum matrix, so sign flips across ``q`` and - ``-q`` do not bias the mean. - """ - q = np.asarray(quats_wxyz, dtype=np.float64) - if q.ndim != 2 or q.shape[1] != 4: - raise ValueError(f"Expected (N, 4); got {q.shape}") - if q.shape[0] == 0: - return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64) - # Normalize rows before accumulating; guards against degenerate inputs. - norms = np.linalg.norm(q, axis=1, keepdims=True) - norms = np.where(norms > 1e-12, norms, 1.0) - qn = q / norms - M = qn.T @ qn / q.shape[0] - eigvals, eigvecs = np.linalg.eigh(M) - mean = eigvecs[:, np.argmax(eigvals)] - # Canonical sign: scalar component non-negative. - if mean[0] < 0: - mean = -mean - # Renormalize (eigh vectors are already unit-norm, but keep for safety). - mean = mean / max(np.linalg.norm(mean), 1e-12) - return mean - - -def _slerp_wxyz(q0: np.ndarray, q1: np.ndarray, t: float) -> np.ndarray: - """Slerp between two wxyz quats at blend ``t`` in ``[0, 1]``. Returns ``(4,)``.""" - q0 = np.asarray(q0, dtype=np.float64) - q1 = np.asarray(q1, dtype=np.float64) - # Ensure same hemisphere. - if np.dot(q0, q1) < 0: - q1 = -q1 - dot = float(np.clip(np.dot(q0, q1), -1.0, 1.0)) - if dot > 0.9995: - out = (1.0 - t) * q0 + t * q1 - return out / max(np.linalg.norm(out), 1e-12) - theta = np.arccos(dot) - s = np.sin(theta) - out = (np.sin((1.0 - t) * theta) * q0 + np.sin(t * theta) * q1) / s - return out / max(np.linalg.norm(out), 1e-12) - - -def _enumerate_segments(mask: np.ndarray) -> list[tuple[int, int, bool]]: - """Return list of ``(start, end, in_contact)`` covering all frames.""" - T = mask.shape[0] - if T == 0: - return [] - segments: list[tuple[int, int, bool]] = [] - cur = bool(mask[0]) - start = 0 - for t in range(1, T): - v = bool(mask[t]) - if v != cur: - segments.append((start, t, cur)) - start = t - cur = v - segments.append((start, T, cur)) - return segments - - -def _absorb_short_segments( - segments: list[tuple[int, int, bool]], min_seg_frames: int -) -> list[tuple[int, int, bool]]: - """Greedy: merge any segment shorter than ``min_seg_frames`` with its neighbor.""" - if not segments or min_seg_frames <= 1: - return list(segments) - merged: list[tuple[int, int, bool]] = [] - for seg in segments: - length = seg[1] - seg[0] - if merged and length < min_seg_frames: - ps, _, pv = merged[-1] - merged[-1] = (ps, seg[1], pv) - else: - merged.append(seg) - # Also handle a leading short segment: if it exists and we have a successor, - # merge forward instead of leaving it orphaned. - if len(merged) >= 2 and (merged[0][1] - merged[0][0]) < min_seg_frames: - _, end0, _ = merged[0] - s1, e1, v1 = merged[1] - merged[0:2] = [(merged[0][0], e1, v1)] - return merged - - -def _contact_pose_at( - t: int, - raw_root_pos: np.ndarray, - raw_root_quat: np.ndarray, - raw_body_pos: np.ndarray, - raw_body_wxyz: np.ndarray, - delta_z: np.ndarray, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Rule-1 pose at frame ``t``: raw pose with only the robot Z-delta added.""" - root_pos = raw_root_pos[t].copy() - root_pos[2] += delta_z[t] - root_quat = raw_root_quat[t].copy() - body_pos = raw_body_pos[t].copy() - body_pos[..., 2] += delta_z[t] - body_wxyz = raw_body_wxyz[t].copy() - return root_pos, root_quat, body_pos, body_wxyz - - -def _compute_no_contact_anchor( - *, - seg_idx: int, - segments: list[tuple[int, int, bool]], - raw_root_pos: np.ndarray, - raw_root_quat: np.ndarray, - raw_body_pos: np.ndarray, - raw_body_wxyz: np.ndarray, - delta_z: np.ndarray, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Return the constant anchor pose for a no-contact segment. - - Priority: - - 1. **Preceding-contact anchor** (seamless release): the corrected pose - on the last frame of the nearest preceding contact segment. This is - the physically "placed" pose. - 2. **Following-contact anchor** (seamless pickup): the corrected pose - on the first frame of the nearest following contact segment. Used - only when there is no preceding contact at all in the sequence so - far. - 3. **Segment-median fallback**: used only when the whole sequence has - no contact (there is no physical anchor to honor). Degenerate case. - """ - start, end, in_contact = segments[seg_idx] - assert not in_contact - - # 1. Preceding contact segment (closest, searching backward). - for i in range(seg_idx - 1, -1, -1): - prev_start, prev_end, prev_in = segments[i] - if prev_in: - return _contact_pose_at( - prev_end - 1, - raw_root_pos, - raw_root_quat, - raw_body_pos, - raw_body_wxyz, - delta_z, - ) - - # 2. Following contact segment (closest, searching forward). - for i in range(seg_idx + 1, len(segments)): - next_start, next_end, next_in = segments[i] - if next_in: - return _contact_pose_at( - next_start, - raw_root_pos, - raw_root_quat, - raw_body_pos, - raw_body_wxyz, - delta_z, - ) - - # 3. Degenerate fallback: no contact anywhere in the sequence. - seg_slice_root_pos = raw_root_pos[start:end] - seg_slice_root_quat = raw_root_quat[start:end] - seg_slice_body_pos = raw_body_pos[start:end] - seg_slice_body_wxyz = raw_body_wxyz[start:end] - root_pos = np.median(seg_slice_root_pos, axis=0) - root_quat = _quaternion_mean(seg_slice_root_quat) - body_pos = np.median(seg_slice_body_pos, axis=0) - B = seg_slice_body_wxyz.shape[1] if seg_slice_body_wxyz.ndim >= 2 else 0 - body_wxyz = np.empty((B, 4), dtype=np.float64) - for b in range(B): - body_wxyz[b] = _quaternion_mean(seg_slice_body_wxyz[:, b]) - return root_pos, root_quat, body_pos, body_wxyz - - -def _segment_pose_at( - t: int, - seg: tuple[int, int, bool], - no_contact_anchor: tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] | None, - raw_root_pos: np.ndarray, - raw_root_quat: np.ndarray, - raw_body_pos: np.ndarray, - raw_body_wxyz: np.ndarray, - delta_z: np.ndarray, -) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Return ``seg``'s native correction at time ``t``. - - Returns ``(root_pos, root_quat, body_pos, body_wxyz)``. - - ``t`` may lie outside ``[seg.start, seg.end)`` for boundary-blending - purposes. No-contact (rule-2) poses are constants so they extend trivially. - Contact (rule-1) poses are clamped to the segment's nearest boundary frame - when ``t`` is outside the segment: a contact segment's "extrapolation" - outside its own range is constant at its boundary value, which avoids - pulling blended values toward potentially unrelated ``raw[t]`` samples - from a neighboring reconstruction regime. - - ``no_contact_anchor`` must be supplied for non-contact segments (computed - by :func:`_compute_no_contact_anchor`) and is ignored for contact segments. - """ - start, end, in_contact = seg - if in_contact: - t_eff = max(start, min(end - 1, t)) - root_pos, root_quat, body_pos, body_wxyz = _contact_pose_at( - t_eff, raw_root_pos, raw_root_quat, raw_body_pos, raw_body_wxyz, delta_z - ) - else: - if no_contact_anchor is None: - raise ValueError("no_contact_anchor required for non-contact segment") - # Fresh copies so callers cannot mutate the cached anchor. - root_pos, root_quat, body_pos, body_wxyz = no_contact_anchor - root_pos = root_pos.copy() - root_quat = root_quat.copy() - body_pos = body_pos.copy() - body_wxyz = body_wxyz.copy() - return root_pos, root_quat, body_pos, body_wxyz - - -def _write_segment( - seg: tuple[int, int, bool], - *, - no_contact_anchor: tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] | None, - raw_root_pos: np.ndarray, - raw_root_quat: np.ndarray, - raw_body_pos: np.ndarray, - raw_body_wxyz: np.ndarray, - delta_z: np.ndarray, - corr_root_pos: np.ndarray, - corr_root_aa: np.ndarray, - corr_body_pos: np.ndarray, - corr_body_wxyz: np.ndarray, -) -> None: - """Write ``seg``'s native correction into the corrected arrays.""" - start, end, in_contact = seg - if in_contact: - corr_root_pos[start:end] = raw_root_pos[start:end] - corr_root_pos[start:end, 2] += delta_z[start:end] - # orientation unchanged (rule 1) - corr_root_aa[start:end] = R.from_quat( - raw_root_quat[start:end], scalar_first=True - ).as_rotvec() - if corr_body_pos.ndim == 3: - corr_body_pos[start:end] = raw_body_pos[start:end] - corr_body_pos[start:end, :, 2] += delta_z[start:end, None] - if corr_body_wxyz.ndim == 3: - corr_body_wxyz[start:end] = raw_body_wxyz[start:end] - return - - # Rule 2: broadcast the pre-computed anchor pose across the segment. - if no_contact_anchor is None: - raise ValueError("no_contact_anchor required for non-contact segment") - root_pos, root_quat, body_pos, body_wxyz = no_contact_anchor - corr_root_pos[start:end] = root_pos[None, :] - corr_root_aa[start:end] = R.from_quat(root_quat, scalar_first=True).as_rotvec()[ - None, : - ] - if corr_body_pos.ndim == 3: - corr_body_pos[start:end] = body_pos[None, :, :] - if corr_body_wxyz.ndim == 3: - corr_body_wxyz[start:end] = body_wxyz[None, :, :] - - -def _blend_boundary( - *, - left_seg: tuple[int, int, bool], - right_seg: tuple[int, int, bool], - left_anchor: tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] | None, - right_anchor: tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] | None, - blend_frames: int, - raw_root_pos: np.ndarray, - raw_root_quat: np.ndarray, - raw_body_pos: np.ndarray, - raw_body_wxyz: np.ndarray, - delta_z: np.ndarray, - corr_root_pos: np.ndarray, - corr_root_aa: np.ndarray, - corr_body_pos: np.ndarray, - corr_body_wxyz: np.ndarray, -) -> None: - """Cosine S-curve blend across a ``blend_frames``-wide boundary window. - - Overwrites the window centered at the boundary with a blend of the two - adjoining segments' native corrections. - - With the updated Rule 2 (no-contact anchored to adjacent contact), the - blend is often a no-op at contact->no-contact boundaries because both - sides produce the same pose at the boundary frame. It still handles the - no-contact->contact (pickup) transition, where the anchored pose may - differ slightly from the first contact frame's raw + delta, and any - degenerate no-contact-only sequence. - """ - t_b = right_seg[0] - half = max(1, blend_frames // 2) - blend_lo = max(t_b - half, left_seg[0]) - blend_hi = min(t_b + half, right_seg[1]) - n = blend_hi - blend_lo - if n <= 1: - return - - # Cosine ramp: 0 at blend_lo, 1 at blend_hi - 1. - ramp = 0.5 * (1.0 - np.cos(np.linspace(0.0, np.pi, n))) - - for idx, t in enumerate(range(blend_lo, blend_hi)): - w = float(ramp[idx]) - l_pos, l_quat, l_bpos, l_bwxyz = _segment_pose_at( - t, - left_seg, - left_anchor, - raw_root_pos, - raw_root_quat, - raw_body_pos, - raw_body_wxyz, - delta_z, - ) - r_pos, r_quat, r_bpos, r_bwxyz = _segment_pose_at( - t, - right_seg, - right_anchor, - raw_root_pos, - raw_root_quat, - raw_body_pos, - raw_body_wxyz, - delta_z, - ) - corr_root_pos[t] = (1.0 - w) * l_pos + w * r_pos - corr_root_aa[t] = R.from_quat( - _slerp_wxyz(l_quat, r_quat, w), scalar_first=True - ).as_rotvec() - if corr_body_pos.ndim == 3: - corr_body_pos[t] = (1.0 - w) * l_bpos + w * r_bpos - if corr_body_wxyz.ndim == 3: - B = corr_body_wxyz.shape[1] - for b in range(B): - corr_body_wxyz[t, b] = _slerp_wxyz(l_bwxyz[b], r_bwxyz[b], w) - - -# --------------------------------------------------------------------------- -# Module 5: ground_plane.json loader (reconstruction-side static plane) -# --------------------------------------------------------------------------- - - -def _transform_plane_under_rigid( - plane: np.ndarray, rotation: np.ndarray, translation: np.ndarray -) -> np.ndarray: - """Transform a plane ``(a, b, c, d)`` under ``x_new = R x_old + t``. - - Plane equation transforms as ``n_new = R @ n_old`` and - ``d_new = d_old - n_new^T @ t`` so points satisfying ``n_old.p_old + d = 0`` - in the source frame still satisfy ``n_new.p_new + d_new = 0`` after the - rigid mapping. - """ - n_old = np.asarray(plane[:3], dtype=np.float64) - d_old = float(plane[3]) - R_arr = np.asarray(rotation, dtype=np.float64) - t_arr = np.asarray(translation, dtype=np.float64).reshape(3) - n_new = R_arr @ n_old - d_new = d_old - float(n_new @ t_arr) - return np.concatenate([n_new, [d_new]]) - - -def load_ground_plane_robot_frame( - path: Path | str, - *, - cv_to_source: np.ndarray, - first_frame_anchor: np.ndarray, - source_to_robot: np.ndarray, -) -> ReferencePlane | None: - """Load ``ground_plane.json`` and transform it into the robot frame. - - The reconstruction stores the plane in OpenCV camera coordinates - (X=right, Y=down, Z=forward). The retargeter then composes three - transforms before the robot world frame is reached: - - 1. ``cv_to_source`` — homogeneous (4x4) flip from CV to the source - skeleton's world frame (see ``_convert_object_poses_cv_to_soma``). - 2. ``first_frame_anchor`` — homogeneous (4x4) anchor that places the - source skeleton's frame-0 pelvis at the origin (see - ``SOMA._first_frame_transform``). - 3. ``source_to_robot`` — 3x3 rotation that swaps source axes into - robot axes (``config.r_world`` in the new SOMA pipeline). - - The plane is transformed under each rigid mapping and finally returned - as a :class:`ReferencePlane` in robot frame, or ``None`` when the JSON - file is absent so the caller can fall back to the legacy heuristics. - - Args: - path: Path to ``ground_plane.json``. - cv_to_source: ``(4, 4)`` homogeneous transform from CV to source frame. - first_frame_anchor: ``(4, 4)`` homogeneous transform that anchors the - source skeleton on its frame-0 pelvis. - source_to_robot: ``(3, 3)`` rotation from source world to robot world. - - Returns: - :class:`ReferencePlane` in robot frame, or ``None`` if the JSON does - not exist on disk. Raises :class:`ValueError` for malformed payloads. - """ - path = Path(path) - if not path.is_file(): - return None - payload = json.loads(path.read_text(encoding="utf-8")) - if "plane" not in payload or len(payload["plane"]) != 4: - raise ValueError(f"{path}: expected a 4-element 'plane' list (ax+by+cz+d=0)") - plane = np.asarray(payload["plane"], dtype=np.float64) - - # 1. CV -> source frame (rigid). - cv_to_source_arr = np.asarray(cv_to_source, dtype=np.float64) - if cv_to_source_arr.shape != (4, 4): - raise ValueError(f"cv_to_source must be (4, 4); got {cv_to_source_arr.shape}") - plane = _transform_plane_under_rigid( - plane, cv_to_source_arr[:3, :3], cv_to_source_arr[:3, 3] - ) - - # 2. First-frame anchor (rigid). - anchor_arr = np.asarray(first_frame_anchor, dtype=np.float64) - if anchor_arr.shape != (4, 4): - raise ValueError(f"first_frame_anchor must be (4, 4); got {anchor_arr.shape}") - plane = _transform_plane_under_rigid(plane, anchor_arr[:3, :3], anchor_arr[:3, 3]) - - # 3. Source -> robot rotation (no translation). - src_to_robot_arr = np.asarray(source_to_robot, dtype=np.float64) - if src_to_robot_arr.shape != (3, 3): - raise ValueError( - f"source_to_robot must be (3, 3); got {src_to_robot_arr.shape}" - ) - plane = _transform_plane_under_rigid( - plane, src_to_robot_arr, np.zeros(3, dtype=np.float64) - ) - - n = plane[:3] - d = float(plane[3]) - return ReferencePlane(normal=(float(n[0]), float(n[1]), float(n[2])), offset=d) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/hand_kinematics.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/hand_kinematics.py deleted file mode 100644 index a360c7b0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/hand_kinematics.py +++ /dev/null @@ -1,550 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from typing import Any, Literal, Optional - -import numpy as np -import pink -import pinocchio as pin -import torch -import viser -from loop_rate_limiters import RateLimiter -from pink import solve_ik -from pink.limits import ConfigurationLimit, VelocityLimit -from pink.tasks import FrameTask, RelativeFrameTask -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.retarget.params import ( - DEX3_TO_MANO_MAPPING, - MANO_JOINTS_ORDER, - SHARPA_RELATIVE_FRAMES, - SHARPA_TO_MANO_MAPPING, - SHARPA_TO_MANO_ROTATION_OFFSET, -) -from robotic_grounding.retarget.pinocchio_viser_visualizer import ViserVisualizer -from robotic_grounding.retarget.utils import subtract_frame_transforms - - -class HandKinematics: - """Base class for hand kinematics.""" - - def __init__( - self, - side: Literal["right", "left"], - robot_asset_path: str, - source_model: Literal["mano"], - use_relative_frames: bool = False, - solver: str = "daqp", - max_iter: int = 200, - frequency: float = 200.0, - frame_tasks_converged_threshold: float = 1e-6, - ) -> None: - """ - Initialize the hand kinematics. - - Args: - side: Hand side ("left" or "right"). - robot_asset_path: Path to the robot URDF file. - source_model: Source motion model. Only "mano" is supported. - use_relative_frames: Whether to use relative frame tasks. - solver: IK solver to use. - max_iter: Maximum IK iterations. - frequency: IK solve frequency. - frame_tasks_converged_threshold: Convergence threshold. - """ - self.side = side - self.robot_asset_path = robot_asset_path - self.source_model = source_model - self.use_relative_frames = use_relative_frames - self.solver = solver - self.max_iter = max_iter - self.frequency = frequency - self.frame_tasks_converged_threshold = frame_tasks_converged_threshold - - # Load robot model - self.robot = self.load_robot_model() - - # Record robot info, free layer creates 7 joints - self.robot_finger_joint_names = {} - # Record the rest of the joints - for i in range(2, self.robot.model.nq - 7 + 2): - joint_name = self.robot.model.names[i] - self.robot_finger_joint_names[i - 2] = joint_name - - self.robot_frame_names = {} - # include body, joint, and site frames - for i in range(len(self.robot.model.frames)): - frame_name = self.robot.model.frames[i].name - self.robot_frame_names[i] = frame_name - - self.configuration_limits = [ - ConfigurationLimit(self.robot.model), - VelocityLimit(self.robot.model), - ] - - # Setup pink solver - self.configuration = pink.Configuration( - self.robot.model, - self.robot.data, - self.robot.q0, - ) - - # Setup rate limiter - rate = RateLimiter(frequency=frequency, warn=False) - self.dt = rate.period - - # Get source joint order based on source model - self.source_joint_order = self.get_source_joint_order() - - # Get source to target mapping - self.target_to_source = self.get_target_to_source_mapping() - self.target_to_source_rel = self.get_target_to_source_rel() - - # Setup IK frame tasks - self.frame_tasks = self.setup_frame_tasks() - - # Setup relative frame tasks - if self.use_relative_frames and self.target_to_source_rel is not None: - self.relative_frame_tasks = self.setup_relative_frame_tasks() - - def load_robot_model(self) -> pin.RobotWrapper: - """Load the robot model. - - Returns: - pin.RobotWrapper: The robot model. - """ - raise NotImplementedError( - f"{self.__class__.__name__} must implement this method." - ) - - def get_source_joint_order(self) -> list[str]: - """Get the source joint order. ``source_model`` is MANO-only.""" - return MANO_JOINTS_ORDER - - def get_target_to_source_mapping(self) -> dict[str, tuple[str, float, float]]: - """Get the source to target mapping. - - Expected to return a dictionary of the form {target_site_name: (source_joint_name, position_cost, orientation_cost)}. - """ - raise NotImplementedError( - f"{self.__class__.__name__} must implement this method." - ) - - def get_target_to_source_rel(self) -> list[tuple[str, str, float, float]]: - """Get the source to target relative mapping. - - Returns: - List of tuples of the form (target_site_name, root_site_name, position_cost, orientation_cost). - """ - return [] - - def get_base_source_joint(self) -> str: - """Base source joint name for scaling. ``source_model`` is MANO-only.""" - return "wrist" - - def transform_source_position(self, position: np.ndarray) -> np.ndarray: - """Transform source position to robot convention. - - Override in subclass if coordinate system transformation is needed. - Default implementation returns the position unchanged. - - Args: - position: Position array of shape (3,) - - Returns: - Transformed position with same shape as input. - """ - return position - - def transform_source_rotation(self, rotation: np.ndarray) -> np.ndarray: - """Transform source rotation matrix to robot convention. - - Override in subclass if coordinate system transformation is needed. - Default implementation returns the rotation unchanged. - - Args: - rotation: Rotation matrix of shape (3, 3). - - Returns: - Transformed rotation matrix of shape (3, 3). - """ - return rotation - - def get_frame_rotation_correction(self, frame_name: str) -> np.ndarray: - """Get rotation correction for a specific robot frame. - - Override in subclass if per-frame rotation corrections are needed - Default implementation returns identity (no correction). - - Args: - frame_name: Name of the robot frame. - - Returns: - Rotation correction matrix of shape (3, 3). - """ - return np.eye(3) - - def visualize( - self, - viser_server: viser.ViserServer, - qpos: np.ndarray, - ) -> None: - """Visualize the robot in viser.""" - if not hasattr(self, "robot_viser_model"): - self.robot_viser_model = ViserVisualizer( - viser_server=viser_server, - model=self.robot.model, - visual_model=self.robot.visual_model, - collision_model=self.robot.collision_model, - ) - - self.robot_viser_model.display(qpos) - - def set_frame_tasks_target( - self, - source_joints: np.ndarray, - source_joints_wxyz: np.ndarray, - base_source_joint: Optional[str] = None, - source_to_robot_scale: float = 1.0, - ) -> None: - """Set the target for the IK frame tasks. - - Args: - source_joints: Array of shape (num_joints, 3) with joint positions. - source_joints_wxyz: Array of shape (num_joints, 4) with joint orientations as wxyz quaternions. - base_source_joint: Name of the base joint for scaling. If None, uses get_base_source_joint(). - source_to_robot_scale: Scale factor for positions relative to base joint. - """ - if base_source_joint is None: - base_source_joint = self.get_base_source_joint() - - # Get base position, apply coordinate transform if needed - base_joint_idx = self.source_joint_order.index(base_source_joint) - base_pos = self.transform_source_position(source_joints[base_joint_idx]) - - for robot_frame_name, ( - source_joint_name, - _, - _, - ) in self.target_to_source.items(): - # 1.1 Get source joint position, apply coordinate transform if needed - source_joint_idx = self.source_joint_order.index(source_joint_name) - target_pos = self.transform_source_position(source_joints[source_joint_idx]) - - # 1.2 Apply scale factor relative to base joint - target_pos = base_pos + (target_pos - base_pos) * source_to_robot_scale - - # 1.3 Get source joint rotation, apply coordinate transform if needed - target_wxyz = source_joints_wxyz[source_joint_idx] - target_rot = R.from_quat(target_wxyz, scalar_first=True).as_matrix() - target_rot = self.transform_source_rotation(target_rot) - - # 1.4 Apply per-frame rotation corrections - frame_correction = self.get_frame_rotation_correction(robot_frame_name) - target_rot = target_rot @ frame_correction - - # 1.5 Set the target for the IK frame task - self.frame_tasks[robot_frame_name].transform_target_to_world.translation = ( - target_pos.copy() - ) - self.frame_tasks[robot_frame_name].transform_target_to_world.rotation = ( - target_rot.copy() - ) - - def set_relative_frame_tasks_target( - self, - source_joints: np.ndarray, - source_joints_wxyz: np.ndarray, - source_to_robot_scale: float = 1.0, - ) -> None: - """Set the target for the IK relative frame tasks.""" - for ( - robot_target_site_name, - robot_root_site_name, - _, - _, - ) in self.target_to_source_rel: - # 2.1 Extract the target position - task_name = f"relative_{robot_target_site_name}_to_{robot_root_site_name}" - source_target_joint_name = self.target_to_source[robot_target_site_name][0] - source_target_joint_idx = self.source_joint_order.index( - source_target_joint_name - ) - source_target_joint_pos = source_joints[source_target_joint_idx] - source_target_joint_wxyz = source_joints_wxyz[source_target_joint_idx] - source_root_joint_name = self.target_to_source[robot_root_site_name][0] - source_root_joint_idx = self.source_joint_order.index( - source_root_joint_name - ) - source_root_joint_pos = source_joints[source_root_joint_idx] - source_root_joint_wxyz = source_joints_wxyz[source_root_joint_idx] - # 2.2 Compute the target position with scale factor - source_root_p_target, source_root_q_target = subtract_frame_transforms( - source_root_joint_pos, - source_root_joint_wxyz, - source_target_joint_pos, - source_target_joint_wxyz, - ) - self.relative_frame_tasks[ - task_name - ].transform_target_to_world.translation = source_root_p_target.copy() - self.relative_frame_tasks[task_name].transform_target_to_world.rotation = ( - source_root_q_target.copy() - ) - raise NotImplementedError("Relative frame tasks are not debugged yet.") - - def setup_frame_tasks(self) -> dict[str, FrameTask]: - """Setup the IK frame tasks.""" - frame_tasks = {} - for robot_site_name, ( - _, - position_cost, - orientation_cost, - ) in self.target_to_source.items(): - frame_tasks[robot_site_name] = FrameTask( - robot_site_name, - position_cost=position_cost, - orientation_cost=orientation_cost, - lm_damping=1.0, - ) - frame_tasks[robot_site_name].set_target_from_configuration( - self.configuration - ) - return frame_tasks - - def setup_relative_frame_tasks(self) -> dict[str, RelativeFrameTask]: - """Setup the relative frame tasks.""" - relative_frame_tasks = {} - for ( - robot_target_site_name, - robot_root_site_name, - position_cost, - orientation_cost, - ) in self.target_to_source_rel: - task_name = f"relative_{robot_target_site_name}_to_{robot_root_site_name}" - relative_frame_tasks[task_name] = RelativeFrameTask( - frame=robot_target_site_name, - root=robot_root_site_name, - position_cost=position_cost, - orientation_cost=orientation_cost, - lm_damping=1.0, - ) - return relative_frame_tasks - - def compute( - self, - source_joints: torch.Tensor | np.ndarray, - source_joints_wxyz: torch.Tensor | np.ndarray, - source_to_robot_scale: float = 1.0, - qpos: Optional[np.ndarray] = None, - ) -> dict[str, Any]: - """Compute the hand kinematics.""" - # 0. Move to numpy - if isinstance(source_joints, torch.Tensor): - source_joints = source_joints.detach().cpu().numpy() - if isinstance(source_joints_wxyz, torch.Tensor): - source_joints_wxyz = source_joints_wxyz.detach().cpu().numpy() - - # 1. Set the target for the IK frame tasks - self.set_frame_tasks_target( - source_joints=source_joints, - source_joints_wxyz=source_joints_wxyz, - source_to_robot_scale=source_to_robot_scale, - ) - tasks = [*self.frame_tasks.values()] - - # 2. Set the target for the relative frame tasks - if self.use_relative_frames: - self.set_relative_frame_tasks_target( - source_joints, source_joints_wxyz, source_to_robot_scale - ) - tasks = [*tasks, *self.relative_frame_tasks.values()] - - # 3. Solve the IK tasks - self.configuration.q = self.robot.q0.copy() if qpos is None else qpos.copy() - - frame_tasks_pos_error = { - task_name: float(np.inf) for task_name in self.frame_tasks.keys() - } - frame_tasks_converged = { - task_name: False for task_name in self.frame_tasks.keys() - } - - num_optimization_iterations = 0 - for _ in range(self.max_iter): - vel = solve_ik( - configuration=self.configuration, - tasks=tasks, - dt=self.dt, - solver=self.solver, - safety_break=False, - limits=self.configuration_limits, - ) - self.configuration.integrate_inplace(vel, self.dt) - num_optimization_iterations += 1 - - # Check if the solution is converged, terminate if it is - for task_name, task in self.frame_tasks.items(): - # See ``WholeBodyKinematics.compute``: cast through - # ``np.asarray`` so we always slice a plain ndarray, not an - # Eigen-backed view from pinocchio whose ``__getitem__`` can - # fail on slice objects on some eigenpy builds. - err_vec = np.asarray(task.compute_error(self.configuration)) - pos_error = float(np.linalg.norm(err_vec[:3])) - last_pos_error = frame_tasks_pos_error[task_name] - if ( - abs(pos_error - last_pos_error) - < self.frame_tasks_converged_threshold - ): - frame_tasks_converged[task_name] = True - frame_tasks_pos_error[task_name] = pos_error - - if all(frame_tasks_converged.values()): - break - - # 3. Compute frame poses - frame_pose = [] - for _, frame_name in self.robot_frame_names.items(): - pos = self.configuration.get_transform_frame_to_world( - frame_name - ).translation - rot_matrix = self.configuration.get_transform_frame_to_world( - frame_name - ).rotation - wxyz = R.from_matrix(rot_matrix).as_quat(scalar_first=True) - frame_pose.append(np.hstack([pos, wxyz]).tolist()) - frame_pose = np.asarray(frame_pose) - - return { - "q": self.configuration.q.copy(), - "frame_pose": frame_pose, - "frame_task_errors": [ - frame_tasks_pos_error[k] for k in self.frame_tasks.keys() - ], - "num_optimization_iterations": num_optimization_iterations, - } - - -class SharpaHandKinematics(HandKinematics): - """Sharpa hand kinematics class.""" - - def __init__( - self, - side: Literal["right", "left"], - robot_asset_path: str, - source_model: Literal["mano"], - use_relative_frames: bool = False, - solver: str = "daqp", - max_iter: int = 200, - frequency: float = 200.0, - frame_tasks_converged_threshold: float = 1e-6, - ) -> None: - """Initialize the Sharpa hand kinematics.""" - super().__init__( - side=side, - robot_asset_path=robot_asset_path, - source_model=source_model, - use_relative_frames=use_relative_frames, - solver=solver, - max_iter=max_iter, - frequency=frequency, - frame_tasks_converged_threshold=frame_tasks_converged_threshold, - ) - self._sharpa_rotation_corrections: dict[str, np.ndarray] = {} - for frame_pattern, offset_wxyz in SHARPA_TO_MANO_ROTATION_OFFSET.items(): - frame_name = frame_pattern.replace(".*", self.side) - self._sharpa_rotation_corrections[frame_name] = ( - R.from_quat(offset_wxyz, scalar_first=True).inv().as_matrix() - ) - - def load_robot_model(self) -> pin.RobotWrapper: - """Load the robot model.""" - return pin.RobotWrapper.BuildFromMJCF( - filename=self.robot_asset_path, - root_joint=pin.JointModelFreeFlyer(), - ) - - def get_target_to_source_mapping(self) -> dict[str, tuple[str, float, float]]: - """Source-to-target mapping. ``source_model`` is MANO-only.""" - return { - k.replace(".*", self.side): v for k, v in SHARPA_TO_MANO_MAPPING.items() - } - - def get_target_to_source_rel(self) -> list[tuple[str, str, float, float]]: - """Source-to-target relative mapping. ``source_model`` is MANO-only.""" - return [ - ( - entry[0].replace(".*", self.side), - entry[1].replace(".*", self.side), - entry[2], - entry[3], - ) - for entry in SHARPA_RELATIVE_FRAMES - ] - - def get_frame_rotation_correction(self, frame_name: str) -> np.ndarray: - """Apply SHARPA_TO_MANO_ROTATION_OFFSET for wrist frame alignment.""" - if frame_name in self._sharpa_rotation_corrections: - return self._sharpa_rotation_corrections[frame_name] - return np.eye(3) - - -class Dex3HandKinematics(HandKinematics): - """Dex3 hand kinematics class for MANO to Dex3 retargeting.""" - - def __init__( - self, - side: Literal["right", "left"], - robot_asset_path: str, - package_dirs: Optional[list[str]] = None, - source_model: Literal["mano"] = "mano", - use_relative_frames: bool = False, - solver: str = "daqp", - max_iter: int = 100, - frequency: float = 100.0, - frame_tasks_converged_threshold: float = 1e-6, - ) -> None: - """Initialize the Dex3 hand kinematics.""" - self.package_dirs = package_dirs or [] - - super().__init__( - side=side, - robot_asset_path=robot_asset_path, - source_model=source_model, - use_relative_frames=use_relative_frames, - solver=solver, - max_iter=max_iter, - frequency=frequency, - frame_tasks_converged_threshold=frame_tasks_converged_threshold, - ) - - def load_robot_model(self) -> pin.RobotWrapper: - """Load the robot model from URDF with free-flyer root joint.""" - return pin.RobotWrapper.BuildFromURDF( - filename=self.robot_asset_path, - package_dirs=self.package_dirs, - root_joint=pin.JointModelFreeFlyer(), - ) - - def get_target_to_source_mapping(self) -> dict[str, tuple[str, float, float]]: - """Dex3 source-to-target mapping. ``source_model`` is MANO-only.""" - return {k.replace(".*", self.side): v for k, v in DEX3_TO_MANO_MAPPING.items()} - - # No transform_source_position / transform_source_rotation overrides: - # MANO is in the same convention as the Dex3 robot, so the base-class - # identity defaults (HandKinematics.transform_source_*) are correct. - - # MANO→Dex3 palm frame corrections (180° rotations) - _R_MANO_PALM_RIGHT = np.array( - [[-1, 0, 0], [0, -1, 0], [0, 0, 1]], dtype=np.float64 - ) # 180° about Z - _R_MANO_PALM_LEFT = np.array( - [[-1, 0, 0], [0, 1, 0], [0, 0, -1]], dtype=np.float64 - ) # 180° about Y - - def get_frame_rotation_correction(self, frame_name: str) -> np.ndarray: - """Get rotation correction for palm frames to align with human hand.""" - if "palm" in frame_name: - if self.side == "right": - return self._R_MANO_PALM_RIGHT - return self._R_MANO_PALM_LEFT - return np.eye(3) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/naming.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/naming.py deleted file mode 100644 index ae8a00b2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/naming.py +++ /dev/null @@ -1,16 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Naming helpers shared between dataset loading and URDF/asset generation. - -Kept dependency-free (pure string ops) so consumers like -``scripts/generate_rigid_urdfs.py`` can import it without pulling in the -hand-FK / dataset-loading stack (which now lives in reconstruction). -""" - - -def make_usd_safe(name: str) -> str: - """Make a name safe for USD prim paths (no leading digits, no @ etc.).""" - safe = name.replace("@", "_") - if safe and (safe[0].isdigit() or not (safe[0].isalpha() or safe[0] == "_")): - safe = f"obj_{safe}" - return safe diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/params.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/params.py deleted file mode 100644 index bb11af7d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/params.py +++ /dev/null @@ -1,272 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -############################################################# -# MANO parameters -############################################################# - -MANO_JOINTS_ORDER = [ - "wrist", - "thumb1", - "thumb2", - "thumb3", - "thumb4", - "index1", - "index2", - "index3", - "index4", - "middle1", - "middle2", - "middle3", - "middle4", - "ring1", - "ring2", - "ring3", - "ring4", - "pinky1", - "pinky2", - "pinky3", - "pinky4", -] - -MANO_TRANSFORMS_ORDER = [ - "wrist", - "index1", - "index2", - "index3", - "middle1", - "middle2", - "middle3", - "pinky1", - "pinky2", - "pinky3", - "ring1", - "ring2", - "ring3", - "thumb1", - "thumb2", - "thumb3", -] - -TRANSFORMS_TO_JOINTS = [ - 0, - 13, - 14, - 15, - 15, # tip - 1, - 2, - 3, - 3, # tip - 4, - 5, - 6, - 6, # tip - 10, - 11, - 12, - 12, # tip - 7, - 8, - 9, - 9, # tip -] - -MANO_JOINTS_PARENTS = [ - -1, - 0, - 1, - 2, - 3, - 0, - 5, - 6, - 7, - 0, - 9, - 10, - 11, - 0, - 13, - 14, - 15, - 0, - 17, - 18, - 19, -] - -MANO_FINGERTIP_INDICES = [4, 8, 12, 16, 20] - -############################################################# -# SOMA parameters (NVlabs SOMA-X, MHR identity) -############################################################# - -# Ordered SOMA rig joint names exported by ``save_soma_npz``. Index -# positions are stable for any SOMA-X export because ``save_soma_npz`` -# writes joint_names verbatim from ``soma.rig_data["joint_names"]``. -SOMA_JOINTS_ORDER = [ - "Hips", # 0 - "Spine1", # 1 - "Spine2", # 2 - "Chest", # 3 - "Neck1", # 4 - "Neck2", # 5 - "Head", # 6 - "HeadEnd", # 7 - "Jaw", # 8 - "LeftEye", # 9 - "RightEye", # 10 - "LeftShoulder", # 11 - "LeftArm", # 12 - "LeftForeArm", # 13 - "LeftHand", # 14 - "LeftHandThumb1", # 15 - "LeftHandThumb2", # 16 - "LeftHandThumb3", # 17 - "LeftHandThumbEnd", # 18 - "LeftHandIndex1", # 19 - "LeftHandIndex2", # 20 - "LeftHandIndex3", # 21 - "LeftHandIndex4", # 22 - "LeftHandIndexEnd", # 23 - "LeftHandMiddle1", # 24 - "LeftHandMiddle2", # 25 - "LeftHandMiddle3", # 26 - "LeftHandMiddle4", # 27 - "LeftHandMiddleEnd", # 28 - "LeftHandRing1", # 29 - "LeftHandRing2", # 30 - "LeftHandRing3", # 31 - "LeftHandRing4", # 32 - "LeftHandRingEnd", # 33 - "LeftHandPinky1", # 34 - "LeftHandPinky2", # 35 - "LeftHandPinky3", # 36 - "LeftHandPinky4", # 37 - "LeftHandPinkyEnd", # 38 - "RightShoulder", # 39 - "RightArm", # 40 - "RightForeArm", # 41 - "RightHand", # 42 - "RightHandThumb1", # 43 - "RightHandThumb2", # 44 - "RightHandThumb3", # 45 - "RightHandThumbEnd", # 46 - "RightHandIndex1", # 47 - "RightHandIndex2", # 48 - "RightHandIndex3", # 49 - "RightHandIndex4", # 50 - "RightHandIndexEnd", # 51 - "RightHandMiddle1", # 52 - "RightHandMiddle2", # 53 - "RightHandMiddle3", # 54 - "RightHandMiddle4", # 55 - "RightHandMiddleEnd", # 56 - "RightHandRing1", # 57 - "RightHandRing2", # 58 - "RightHandRing3", # 59 - "RightHandRing4", # 60 - "RightHandRingEnd", # 61 - "RightHandPinky1", # 62 - "RightHandPinky2", # 63 - "RightHandPinky3", # 64 - "RightHandPinky4", # 65 - "RightHandPinkyEnd", # 66 - "LeftLeg", # 67 - "LeftShin", # 68 - "LeftFoot", # 69 - "LeftToeBase", # 70 - "LeftToeEnd", # 71 - "RightLeg", # 72 - "RightShin", # 73 - "RightFoot", # 74 - "RightToeBase", # 75 - "RightToeEnd", # 76 -] - -############################################################# -# IK parameters for Sharpa hand -############################################################# - -SHARPA_TO_MANO_ROTATION_OFFSET = { - # Sharpa hand frame: orientation wxyz offset - ".*_hand_C_MC": (0.5, -0.5, 0.5, 0.5), -} - -SHARPA_TO_MANO_MAPPING = { - # Sharpa body frame: (target MANO joint, position cost, orientation cost) - ".*_hand_C_MC": ("wrist", 0.2, 0.2), - # ".*_thumb_CMC_VL_site": ("thumb1", 0.0, 0.0), - ".*_thumb_MCP_VL_site": ("thumb2", 0.1, 0.0), - # ".*_thumb_DP_site": ("thumb3", 0.0, 0.0), - ".*_thumb_tip_site": ("thumb4", 1.0, 0.05), - # ".*_index_MCP_VL_site": ("index1", 0.0, 0.0), - ".*_index_MP_site": ("index1", 0.1, 0.0), - # ".*_index_DP_site": ("index3", 0.0, 0.0), - ".*_index_tip_site": ("index4", 1.0, 0.1), - # ".*_middle_MCP_VL_site": ("middle1", 0.0, 0.0), - ".*_middle_MP_site": ("middle1", 0.1, 0.0), - # ".*_middle_DP_site": ("middle3", 0.0, 0.0), - ".*_middle_tip_site": ("middle4", 1.0, 0.1), - # ".*_ring_MCP_VL_site": ("ring1", 0.0, 0.0), - ".*_ring_MP_site": ("ring1", 0.1, 0.0), - # ".*_ring_DP_site": ("ring3", 0.0, 0.0), - ".*_ring_tip_site": ("ring4", 1.0, 0.1), - # ".*_pinky_MC_site": ("pinky1", 0.0, 0.0), - ".*_pinky_MP_site": ("pinky1", 0.1, 0.0), - # ".*_pinky_DP_site": ("pinky3", 0.0, 0.0), - ".*_pinky_tip_site": ("pinky4", 0.5, 0.1), -} - -SHARPA_RELATIVE_FRAMES = [ - # Target site: (root site, position cost, orientation cost) - (".*_thumb_tip_site", ".*_index_tip_site", 0.1, 0.0), - (".*_thumb_tip_site", ".*_middle_tip_site", 0.1, 0.0), - (".*_thumb_tip_site", ".*_ring_tip_site", 0.1, 0.0), - (".*_thumb_tip_site", ".*_pinky_tip_site", 0.1, 0.0), - (".*_index_tip_site", ".*_middle_tip_site", 0.1, 0.0), - (".*_index_tip_site", ".*_ring_tip_site", 0.1, 0.0), - (".*_index_tip_site", ".*_pinky_tip_site", 0.1, 0.0), - (".*_middle_tip_site", ".*_ring_tip_site", 0.1, 0.0), - (".*_middle_tip_site", ".*_pinky_tip_site", 0.1, 0.0), - (".*_ring_tip_site", ".*_pinky_tip_site", 0.1, 0.0), -] - -############################################################# -# IK parameters for Dex3 hand -############################################################# - -DEX3_TO_MANO_MAPPING = { - # Dex3 .* hand sites: (target MANO joint, position cost, orientation cost) - ".*_hand_palm_link": ("wrist", 1.0, 0.1), - ".*_thumb_tip": ("thumb4", 1.0, 0.0), - ".*_index_tip": ("index4", 1.0, 0.0), - ".*_middle_tip": ("middle4", 1.0, 0.0), -} - -############################################################# -# MANO hand link definitions -############################################################# - -# MANO hand link definitions: (link_name, list of joint indices). -# Used to assign contact points to the closest link via joint distances. -MANO_HAND_LINKS = { - "link_palm": [0, 1, 5, 9, 13, 17], - "link_thumb1": [1, 2], - "link_thumb2": [2, 3], - "link_thumb3": [3, 4], - "link_index1": [5, 6], - "link_index2": [6, 7], - "link_index3": [7, 8], - "link_middle1": [9, 10], - "link_middle2": [10, 11], - "link_middle3": [11, 12], - "link_ring1": [13, 14], - "link_ring2": [14, 15], - "link_ring3": [15, 16], - "link_pinky1": [17, 18], - "link_pinky2": [18, 19], - "link_pinky3": [19, 20], -} - -NUM_MANO_LINKS = len(MANO_HAND_LINKS) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/pinocchio_viser_visualizer.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/pinocchio_viser_visualizer.py deleted file mode 100644 index e685bdbd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/pinocchio_viser_visualizer.py +++ /dev/null @@ -1,327 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Code adapted from https://github.com/stack-of-tasks/pinocchio/blob/devel/bindings/python/pinocchio/visualize/viser_visualizer.py""" - -# ruff: noqa -# mypy: ignore-errors - -from pathlib import Path - -import hppfcl -import numpy as np - -import pinocchio as pin -from pinocchio.visualize.base_visualizer import BaseVisualizer - -import trimesh -import viser - -MESH_TYPES = (hppfcl.BVHModelBase, hppfcl.HeightFieldOBBRSS, hppfcl.HeightFieldAABB) - - -class ViserVisualizer(BaseVisualizer): - """A Pinocchio visualizer using Viser.""" - - def __init__( - self, - viser_server: viser.ViserServer, - model=pin.Model(), - collision_model=None, - visual_model=None, - copy_models=False, - data=None, - collision_data=None, - visual_data=None, - ): - super().__init__( - model, - collision_model, - visual_model, - copy_models, - data, - collision_data, - visual_data, - ) - self.static_objects = [] - self.viser_server = viser_server - self.frames = {} - self.loadViewerModel() - - def loadViewerModel( - self, - rootNodeName="pinocchio", - collision_color=None, - visual_color=None, - frame_axis_length=0.018, - frame_axis_radius=0.0008, - ): - """Load the robot in a Viser viewer. - - Parameters: - rootNodeName: name to give to the robot in the viewer - collision_color: optional, color to give to the collision model of - the robot. Format is a list of four RGBA floating-point numbers - (between 0 and 1) - visual_color: optional, color to give to the visual model of - the robot. Format is a list of four RGBA floating-point numbers - (between 0 and 1) - frame_axis_length: optional, length of frame axes if displaying frames. - frame_axis_radius: optional, radius of frame axes if displaying frames. - """ - self.viewerRootNodeName = rootNodeName - - # Create root frames to help toggle visibility in the Viser UI. - self.visualRootNodeName = rootNodeName + "/visual" - self.visualRootFrame = self.viser_server.scene.add_frame( - self.visualRootNodeName, show_axes=False - ) - self.collisionRootNodeName = rootNodeName + "/collision" - self.collisionRootFrame = self.viser_server.scene.add_frame( - self.collisionRootNodeName, show_axes=False - ) - self.framesRootNodeName = rootNodeName + "/frames" - self.framesRootFrame = self.viser_server.scene.add_frame( - self.framesRootNodeName, show_axes=False - ) - - # Load visual model - if (visual_color is not None) and (len(visual_color) != 4): - raise RuntimeError("visual_color must have 4 elements for RGBA.") - if self.visual_model is not None: - for visual in self.visual_model.geometryObjects: - self.loadViewerGeometryObject( - visual, self.visualRootNodeName, visual_color - ) - self.displayVisuals(True) - - # Load collision model - if (collision_color is not None) and (len(collision_color) != 4): - raise RuntimeError("collision_color must have 4 elements for RGBA.") - if self.collision_model is not None: - for collision in self.collision_model.geometryObjects: - self.loadViewerGeometryObject( - collision, self.collisionRootNodeName, collision_color - ) - self.displayCollisions(False) - - # Load frames - for frame in self.model.frames: - frame_name = self.framesRootNodeName + "/" + frame.name - self.frames[frame_name] = self.viser_server.scene.add_frame( - frame_name, - show_axes=True if "site" in frame_name else False, - axes_length=frame_axis_length, - axes_radius=frame_axis_radius, - ) - self.displayFrames(True) - - def loadViewerGeometryObject(self, geometry_object, prefix="", color=None): - """Loads a single geometry object.""" - name = geometry_object.name - if prefix: - name = prefix + "/" + name - geom = geometry_object.geometry - color_override = color or geometry_object.meshColor - - if isinstance(geom, hppfcl.Box): - frame = self.viser_server.scene.add_box( - name, - dimensions=geom.halfSide * 2.0, - color=color_override[:3], - opacity=color_override[3], - ) - elif isinstance(geom, hppfcl.Sphere): - frame = self.viser_server.scene.add_icosphere( - name, - radius=geom.radius, - color=color_override[:3], - opacity=color_override[3], - ) - elif isinstance(geom, hppfcl.Cylinder): - mesh = trimesh.creation.cylinder( - radius=geom.radius, - height=geom.halfLength * 2.0, - ) - frame = self.viser_server.scene.add_mesh_simple( - name, - mesh.vertices, - mesh.faces, - color=color_override[:3], - opacity=color_override[3], - ) - elif isinstance(geom, MESH_TYPES): - frame = self._add_mesh_from_path( - name, geometry_object.meshPath, color_override - ) - elif isinstance(geom, hppfcl.Convex): - if len(geometry_object.meshPath) > 0: - frame = self._add_mesh_from_path( - name, geometry_object.meshPath, color_override - ) - else: - frame = self._add_mesh_from_convex(name, geom, color_override) - else: - raise RuntimeError(f"Unsupported geometry type for {name}: {type(geom)}") - - self.frames[name] = frame - - def _add_mesh_from_path(self, name, mesh_path, color): - """ - Load a mesh from a file. - """ - extension = Path(mesh_path).suffix.lower() - if extension == ".dae" or color is None: - mesh = trimesh.load_scene(mesh_path) - return self.viser_server.scene.add_mesh_trimesh(name, mesh) - else: - mesh = trimesh.load_mesh(mesh_path) - return self.viser_server.scene.add_mesh_simple( - name, - mesh.vertices, - mesh.faces, - color=color[:3], - opacity=color[3], - ) - - def _add_mesh_from_convex(self, name, geom, color): - """ - Load a mesh from triangles stored inside a hppfcl.Convex. - """ - num_tris = geom.num_polygons - call_triangles = geom.polygons - call_vertices = geom.points - - vertices = call_vertices() - vertices = vertices.astype(np.float32) - faces = np.empty((num_tris, 3), dtype=int) - for k in range(num_tris): - tri = call_triangles(k) - faces[k] = [tri[i] for i in range(3)] - - return self.viser_server.scene.add_mesh_simple( - name, - vertices, - faces, - color=color[:3], - opacity=color[3], - ) - - def display(self, q=None): - """ - Display the robot at configuration q in the viewer by placing all the bodies - """ - if q is not None: - pin.forwardKinematics(self.model, self.data, q) - - if self.collisionRootFrame.visible: - self.updatePlacements(pin.GeometryType.COLLISION) - - if self.visualRootFrame.visible: - self.updatePlacements(pin.GeometryType.VISUAL) - - if self.framesRootFrame.visible: - self.updateFrames() - - def displayCollisions(self, visibility): - self.collisionRootFrame.visible = visibility - self.updatePlacements(pin.GeometryType.COLLISION) - - def displayVisuals(self, visibility): - self.visualRootFrame.visible = visibility - self.updatePlacements(pin.GeometryType.VISUAL) - - def displayFrames(self, visibility): - self.framesRootFrame.visible = visibility - self.updateFrames() - - def drawFrameVelocities(self, *args, **kwargs): - raise NotImplementedError("drawFrameVelocities is not yet implemented.") - - def updatePlacements(self, geometry_type): - if geometry_type == pin.GeometryType.VISUAL: - geom_model = self.visual_model - geom_data = self.visual_data - prefix = self.viewerRootNodeName + "/visual" - else: - geom_model = self.collision_model - geom_data = self.collision_data - prefix = self.viewerRootNodeName + "/collision" - - pin.updateGeometryPlacements(self.model, self.data, geom_model, geom_data) - for geom_id, geometry_object in enumerate(geom_model.geometryObjects): - # Get mesh pose. - M = geom_data.oMg[geom_id] - - # Update viewer configuration. - frame_name = prefix + "/" + geometry_object.name - frame = self.frames[frame_name] - frame.position = M.translation * geometry_object.meshScale - frame.wxyz = pin.Quaternion(M.rotation).coeffs()[ - [3, 0, 1, 2] - ] # Pinocchio uses xyzw - - def updateFrames(self): - pin.updateFramePlacements(self.model, self.data) - for frame_id, frame in enumerate(self.model.frames): - # Get frame pose. - M = self.data.oMf[frame_id] - - # Update viewer configuration. - viser_frame_name = self.framesRootNodeName + "/" + frame.name - viser_frame = self.frames[viser_frame_name] - viser_frame.position = M.translation - viser_frame.wxyz = pin.Quaternion(M.rotation).coeffs()[ - [3, 0, 1, 2] - ] # Pinocchio uses xyzw - - def captureImage(self, w=None, h=None, client_id=None, transport_format="jpeg"): - """ - Capture an image from the Viser viewer and return an RGB array. - - Parameters: - w: The width of the captured image. If None, uses the actual camera width. - h: The height of the captured image. If None, uses the actual camera height. - client_id: The ID of the Viser client handle. - If None, uses the first available client. - transport_format: The transport format to use for the captured image. - Can be "jpeg" (default) or "png". - """ - clients = self.viewer.get_clients() - if len(clients) == 0: - raise RuntimeError("Viser server has no attached clients!") - - if client_id is None: - cli = next(iter(clients.values())) - elif client_id not in clients: - raise RuntimeError( - f"Viser server does not have a client with ID '{client_id}'" - ) - else: - cli = clients[client_id] - - height = h or cli.camera.image_height - width = w or cli.camera.image_width - return cli.get_render( - height=height, width=width, transport_format=transport_format - ) - - def setBackgroundColor(self, preset_name: str = "gray", col_top=None, col_bot=None): - raise NotImplementedError("setBackgroundColor is not yet implemented.") - - def setCameraTarget(self, target: np.ndarray): - raise NotImplementedError("setCameraTarget is not yet implemented.") - - def setCameraPosition(self, position: np.ndarray): - raise NotImplementedError("setCameraPosition is not yet implemented.") - - def setCameraZoom(self, zoom: float): - raise NotImplementedError("setCameraZoom is not yet implemented.") - - def setCameraPose(self, pose: np.ndarray = np.eye(4)): - raise NotImplementedError("setcameraPose is not yet implemented.") - - def disableCameraControl(self): - raise NotImplementedError("disableCameraControl is not yet implemented.") - - def enableCameraControl(self): - raise NotImplementedError("enableCameraControl is not yet implemented.") diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/read_soma.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/read_soma.py deleted file mode 100644 index ccce2304..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/read_soma.py +++ /dev/null @@ -1,657 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""SOMA-X parametric body model wrapper for the SOMA-to-G1 retargeter. - -Reads ``soma_params.npz`` saved by the SOMA exporter and reconstructs the -source-space joint positions, global joint orientations (wxyz), and (optional) -mesh vertices that ``WholeBodyKinematics.compute()`` consumes. - -Save schema (from the SOMA exporter ``save_soma_npz``): - - poses (T, J, 3) per-joint local rotation vectors (rad) - transl (T, 3) root translation in meters - joint_names (J,) ordered SOMA rig joint names - identity_model_type "mhr" | "soma" | ... - identity_coeffs (T, K_id) identity (shape) coeffs; constant in time - scale_params (T, K_sc) identity scale params; constant in time - joint_orient (J + 1, 3, 3) rest-pose joint orientation matrices - unit "meters" - keep_root bool when False, ``poses[:, 0]`` is the root - rotation_repr "rotvec" - absolute_pose bool when False, ``poses`` are local rotations - -The ``SOMA`` class in this file mirrors the public surface of -the SOMA exporter so ``soma_to_g1.py`` can consume it directly. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.retarget import BODY_MODELS_DIR - -# --------------------------------------------------------------------------- -# Lightweight container so consumers do not import torch internals. -# --------------------------------------------------------------------------- - - -@dataclass -class SOMAMotion: - """Reconstructed SOMA motion in source coordinates. - - Attributes: - joints: ``(T, J, 3)`` joint positions in meters, source frame. - joints_wxyz: ``(T, J, 4)`` global joint rotations, wxyz quaternions. - vertices: ``(T, V, 3)`` body mesh vertices in source frame. - num_frames: ``T``. - joint_names: List of SOMA rig joint names of length ``J``. - identity_model_type: ``"mhr"``, ``"soma"`` etc. - unit: Unit of ``transl`` from the export (always normalized to meters here). - """ - - joints: np.ndarray - joints_wxyz: np.ndarray - vertices: np.ndarray - num_frames: int - joint_names: list[str] - identity_model_type: str - unit: str - - -# --------------------------------------------------------------------------- -# SOMALayer-backed reader. -# --------------------------------------------------------------------------- - - -def _unit_to_meters_factor(unit: str) -> float: - """Convert SOMA ``unit`` field to a meters scale factor.""" - u = (unit or "meters").lower() - if u in ("m", "meter", "meters"): - return 1.0 - if u in ("cm", "centimeter", "centimeters"): - return 0.01 - if u in ("mm", "millimeter", "millimeters"): - return 0.001 - raise ValueError(f"Unsupported SOMA unit {unit!r}") - - -_SOMA_REQUIRED_ASSETS: tuple[str, ...] = ( - "SOMA_neutral.npz", - "correctives_model.pt", -) -_SOMA_REQUIRED_BY_IDENTITY: dict[str, tuple[str, ...]] = { - "mhr": ( - "MHR/mhr_model_lod1.pt", - "MHR/base_body_lod1.obj", - "MHR/SOMA_wrap_lod1.obj", - ), -} - - -def _missing_assets(root: Path, identity_model_type: str) -> list[str]: - """Return the list of expected SOMA asset files missing under ``root``.""" - needed = list(_SOMA_REQUIRED_ASSETS) + list( - _SOMA_REQUIRED_BY_IDENTITY.get(identity_model_type.lower(), ()) - ) - return [name for name in needed if not (root / name).is_file()] - - -_SETUP_HINT_PRINTED: set[tuple[str, str]] = set() - - -def _emit_setup_hint( - target_root: Path, - identity_model_type: str, - missing: list[str], -) -> None: - """Print a one-shot 'how to populate the SOMA cache' warning. - - Centralized so every code path that detects a missing asset (the - explicit-data_root branch, the repo-local default branch, and any - future ``SOMA``-style wrapper for a different robot) prints the - *same* actionable instruction. Deduplicated per - ``(target_root, identity_model_type)`` per process so scripts that - construct ``SOMA`` repeatedly (e.g. tests, batch loops) do not spam - the log. Update here only when the bootstrap contract (script name, - default cache location) changes. - """ - key = (str(target_root), identity_model_type) - if key in _SETUP_HINT_PRINTED: - return - _SETUP_HINT_PRINTED.add(key) - identity_flag = ( - f" --identity-model-type {identity_model_type}" - if identity_model_type != "mhr" - else "" - ) - print( - f"[read_soma] WARNING: SOMA body-model cache at {target_root} is " - f"missing assets for identity_model_type={identity_model_type!r}: " - f"{missing}.\n" - f" -> Run `python scripts/setup_soma_assets.py{identity_flag}` to " - "download them (~822 MB from HuggingFace).\n" - " Falling back to SOMA-X's built-in HuggingFace cache for this " - "run; subsequent runs will keep re-downloading until the cache " - "above is populated." - ) - - -def _resolve_data_root( - data_root: str | Path | None, - identity_model_type: str, -) -> Path | None: - """Resolve the SOMA assets root for ``SOMALayer``. - - Strategy (match the MANO pattern of repo-local assets, but - never force-create an empty directory that blocks SOMA's built-in - HuggingFace download): - - * If ``data_root`` is an explicit path and contains the full bundle - (``SOMA_neutral.npz``, ``correctives_model.pt``, plus the - identity-model files for ``identity_model_type``), use it. - * If ``BODY_MODELS_DIR / "soma"`` is fully populated, use it (drop-in - local cache alongside MANO). - * Otherwise return ``None`` so ``SOMALayer`` auto-downloads to the - HuggingFace cache; this keeps the first-run bootstrap working in a - fresh Docker image without any manual asset staging. We additionally - print a one-shot setup-script reminder so users do not silently pay - the HuggingFace download cost on every run. - - Never create an empty ``BODY_MODELS_DIR / "soma"`` directory: SOMA-X - treats an existing-but-incomplete directory as "assets are supposed to - be here" and refuses to fall back to HuggingFace, which surfaces as a - cryptic ``FileNotFoundError`` for ``SOMA_neutral.npz``. - """ - if data_root is not None: - path = Path(data_root).expanduser() - missing = _missing_assets(path, identity_model_type) - if not missing: - return path - _emit_setup_hint(path, identity_model_type, missing) - return None - - repo_local = BODY_MODELS_DIR / "soma" - if repo_local.is_dir(): - missing = _missing_assets(repo_local, identity_model_type) - if not missing: - return repo_local - _emit_setup_hint(repo_local, identity_model_type, missing) - else: - # Fresh checkout: directory does not exist yet. Same actionable - # guidance, treating the canonical default path as "missing - # everything" so the user sees one consistent message. - _emit_setup_hint( - repo_local, - identity_model_type, - _missing_assets(repo_local, identity_model_type), - ) - return None - - -def _matrix_to_wxyz(matrix: np.ndarray) -> np.ndarray: - """Convert a stack of ``(..., 3, 3)`` matrices to wxyz quaternions.""" - flat = matrix.reshape(-1, 3, 3) - quats_xyzw = R.from_matrix(flat).as_quat() # scipy returns xyzw by default - quats_wxyz = np.empty_like(quats_xyzw) - quats_wxyz[:, 0] = quats_xyzw[:, 3] - quats_wxyz[:, 1] = quats_xyzw[:, 0] - quats_wxyz[:, 2] = quats_xyzw[:, 1] - quats_wxyz[:, 3] = quats_xyzw[:, 2] - out_shape = matrix.shape[:-2] + (4,) - return quats_wxyz.reshape(out_shape) - - -class SOMA: - """SOMA model wrapper. - - Wraps ``soma.SOMALayer`` and provides ``load_motion(...)`` returning the - dict shape: ``joints``, ``joints_wxyz``, - ``vertices``, ``num_frames``. - """ - - def __init__( - self, - data_root: str | Path | None = None, - identity_model_type: str = "mhr", - device: torch.device | None = None, - ) -> None: - """Construct a ``SOMALayer`` over the local asset root. - - Args: - data_root: Local SOMA asset directory. Defaults to - ``assets/body_models/soma`` so SOMA-X reuses the repo asset - layout (matching the MANO convention). The SOMA-X - package will populate this directory on first use if empty. - identity_model_type: SOMA identity model. ``mhr`` is the default - used by the exporter referenced by this codebase. - device: Torch device. Defaults to CUDA when available. - """ - try: - from soma import SOMALayer # noqa: PLC0415 (lazy: heavy optional dep) - except ImportError as exc: # pragma: no cover - surfaced at runtime - raise ImportError( - "py-soma-x is required for read_soma. Install via " - "`pip install py-soma-x` (already added to the retarget Docker image)." - ) from exc - - self.device = ( - device - if device is not None - else torch.device("cuda" if torch.cuda.is_available() else "cpu") - ) - self.identity_model_type = identity_model_type - self.data_root = _resolve_data_root(data_root, identity_model_type) - self.layer = SOMALayer( - data_root=None if self.data_root is None else str(self.data_root), - identity_model_type=identity_model_type, - device=str(self.device), - ) - # ``rig_data["joint_names"]`` includes a "Root" parent entry at index - # 0. The retargeter's joint indexing aligns with the SOMA exporter's - # ``save_soma_npz`` convention which drops the root, so expose - # ``rig_joint_names`` of length J without "Root". - rig_joint_names_full = [str(n) for n in self.layer.rig_data["joint_names"]] - self.rig_joint_names: list[str] = rig_joint_names_full[1:] - # ``rig_data["parents"]`` does not exist; the SOMA package builds its - # parent list from ``joint_parent_ids`` by dropping the root and - # shifting indices, so ``self.layer.parents`` (length J) is the - # right source. Cache as numpy for downstream FK code. - self.parents: np.ndarray = np.asarray(self.layer.parents, dtype=np.int64) - # Faces exposed for the SOMA visualize() method. - # ``rig_data`` is a numpy archive (NpzFile), not a dict, so we use - # ``.files`` to check membership rather than .get on a dict. - rig_data = self.layer.rig_data - rig_data_files = ( - set(rig_data.files) if hasattr(rig_data, "files") else set(rig_data) - ) - if "triangles" in rig_data_files: - self.faces: np.ndarray | None = np.asarray( - rig_data["triangles"], dtype=np.int64 - ) - elif "faces" in rig_data_files: - self.faces = np.asarray(rig_data["faces"], dtype=np.int64) - else: - self.faces = None - - # ------------------------------------------------------------------ - # Public API - # ------------------------------------------------------------------ - - def load_motion( - self, - params_path: str | Path, - *, - normalize: bool = False, - ) -> dict[str, Any]: - """Load SOMA params and return joints/orientations/vertices. - - The returned dict has the shape the retargeter - can swap source-body wrappers without changing the per-frame loop. - - Args: - params_path: Path to ``soma_params.npz``. - normalize: When False (default), return the raw SOMA world-frame - trajectory. The G1 retargeter expects raw world data because - ``WholeBodyKinematics`` applies its own source-to-robot - rotation and ground anchoring, mirroring the SOMA-X - ``soma-retargeter`` reference. When True, also undo the - frame-0 root rotation/translation so Hips sits at the - origin; only useful for visualization / debugging. - - Returns: - dict with keys: - - * ``joints``: ``(T, J, 3)`` joint world positions. - * ``joints_wxyz``: ``(T, J, 4)`` global joint rotations (wxyz). - * ``vertices``: ``(T, V, 3)`` mesh vertices. - * ``num_frames``: ``T``. - * ``joint_names``: SOMA rig joint name list. - * ``identity_model_type``: ``"mhr"`` etc. - * ``unit``: Always ``"meters"`` after normalization. - * ``first_frame_transl``: (3,) frame-0 translation used to anchor - the body. Always populated, even when ``normalize=False``, so - callers can apply the same anchor to side data later. - * ``first_frame_R_inv``: (3, 3) inverse of frame-0 root rotation. - * ``normalized``: bool reflecting the ``normalize`` arg. - """ - params = self._load_npz(params_path) - meters = _unit_to_meters_factor(params["unit"]) - - poses_rotvec = params["poses"].astype(np.float32) # (T, J, 3) - transl = params["transl"].astype(np.float32) * meters # (T, 3) - identity = params["identity_coeffs"][0].astype(np.float32) # (K_id,) - scale = params["scale_params"][0].astype(np.float32) # (K_sc,) - joint_orient = params["joint_orient"].astype(np.float32) # (J + 1, 3, 3) - keep_root = bool(np.asarray(params["keep_root"]).item()) - absolute_pose = bool(np.asarray(params.get("absolute_pose", False)).item()) - if absolute_pose: - raise ValueError( - "SOMA export with absolute_pose=True is not yet supported by read_soma." - ) - - T_frames = int(poses_rotvec.shape[0]) - - joints, joints_wxyz, vertices = self._reconstruct( - poses_rotvec=poses_rotvec, - transl=transl, - identity=identity, - scale=scale, - joint_orient=joint_orient, - keep_root=keep_root, - ) - - # Compute the first-frame normalization transform so callers (e.g. - # the object-trajectory loader in ``soma_to_g1.py``) can apply the - # *same* transform to other world-frame data such as ``poses.npy``. - # Use the exported root pose, not the reconstructed Hips joint frame: - # the latter includes SOMA rig/rest-frame orientation and can tilt the - # gravity axis, which makes rigid objects appear sideways after anchoring. - first_frame_transform = _first_frame_transform(poses_rotvec[0, 0], transl[0]) - - if normalize: - joints, joints_wxyz, vertices = _normalize_to_first_frame( - joints=joints, - joints_wxyz=joints_wxyz, - vertices=vertices, - transform=first_frame_transform, - ) - - return { - "joints": joints.astype(np.float64), - "joints_wxyz": joints_wxyz.astype(np.float64), - "vertices": vertices.astype(np.float64), - "num_frames": T_frames, - "joint_names": list(self.rig_joint_names), - "identity_model_type": self.identity_model_type, - "unit": "meters", - # Frame-0 anchoring transform that ``normalize=True`` applied - # (or would apply). Caller can use these to apply the same - # anchor to side data (e.g. object trajectory): - # ``p_anchored = (p_world - transl_first) @ R_first_inv.T``. - "first_frame_transl": first_frame_transform["transl_first"], - "first_frame_R_inv": first_frame_transform["R_first_inv"], - "normalized": bool(normalize), - } - - # ------------------------------------------------------------------ - # Reconstruction - # ------------------------------------------------------------------ - - def _reconstruct( - self, - *, - poses_rotvec: np.ndarray, - transl: np.ndarray, - identity: np.ndarray, - scale: np.ndarray, - joint_orient: np.ndarray, - keep_root: bool, - ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Run SOMALayer and return ``(joints, joints_wxyz, vertices)`` in source frame. - - Calls ``SOMALayer.forward`` to get vertices + joint world positions - in one pass (passing ``transl`` so they land in world coords), then - derives global joint rotations via the SOMA package's own - ``apply_joint_orient_local`` + ``joint_local_to_world`` utilities. - """ - del ( - joint_orient, - keep_root, - ) # info captured by ``self.layer.t_pose_world`` already - T_frames = poses_rotvec.shape[0] - J = poses_rotvec.shape[1] - device = self.device - - with torch.no_grad(): - poses_t = torch.from_numpy(poses_rotvec).to(device) - identity_t = ( - torch.from_numpy(identity).to(device).unsqueeze(0).expand(T_frames, -1) - ) - scale_t = ( - torch.from_numpy(scale).to(device).unsqueeze(0).expand(T_frames, -1) - ) - transl_t = torch.from_numpy(transl).to(device) - - output = self.layer( - poses_t, - identity_t, - scale_params=scale_t, - transl=transl_t, - ) - verts_t = output["vertices"] - joints_t = output["joints"] - - global_rot_t = self._compute_global_rotations( - poses_t, T_frames=T_frames, J=J - ) - - vertices = verts_t.detach().cpu().numpy() - joints = joints_t.detach().cpu().numpy() - global_rot = global_rot_t.detach().cpu().numpy() - - joints_wxyz = _matrix_to_wxyz(global_rot) - return joints, joints_wxyz, vertices - - def _compute_global_rotations( - self, - poses_t: torch.Tensor, - *, - T_frames: int, - J: int, - ) -> torch.Tensor: - """Compute per-joint global rotation matrices from local rotvec poses. - - Mirrors ``SOMALayer.pose``'s rotation handling: - - * convert local rotvec poses to rotation matrices (Rodrigues), - * pad a root identity rotation at index 0 so the array length matches - ``rig_data["joint_names"]``, - * apply the saved ``t_pose_world`` rest orientations via - ``apply_joint_orient_local``, - * propagate to world frame with ``joint_local_to_world_levelorder``. - - Returns: - ``(T, J, 3, 3)`` global rotations in the SOMA source frame, with - the leading root joint removed so the indexing matches the rig - joint names exposed by ``self.rig_joint_names``. - """ - from soma.geometry.lbs import batch_rodrigues # noqa: PLC0415 - from soma.geometry.rig_utils import ( # noqa: PLC0415 - apply_joint_orient_local, - compute_skeleton_levels, - joint_local_to_world_levelorder, - precompute_joint_orient, - ) - - device = self.device - joint_parent_ids = self.layer.joint_parent_ids - # ``t_pose_world`` is (J + 1, 4, 4). Use the upper 3x3 as joint_orient. - joint_orient_world = self.layer.t_pose_world[..., :3, :3] - orient, orient_parent_T = precompute_joint_orient( - joint_orient_world.to(device), joint_parent_ids.to(device) - ) - - # poses_t: (T, J, 3) -> rotation matrices (T, J, 3, 3). - local_rot = batch_rodrigues(poses_t.reshape(-1, 3)).reshape(T_frames, J, 3, 3) - # Pad root identity to make length J + 1, matching rig_data layout. - eye = ( - torch.eye(3, device=device, dtype=local_rot.dtype) - .unsqueeze(0) - .unsqueeze(0) - .expand(T_frames, 1, 3, 3) - ) - padded_local_rot = torch.cat([eye, local_rot], dim=1) - oriented_local = apply_joint_orient_local( - padded_local_rot, orient, orient_parent_T - ) - levels = compute_skeleton_levels(joint_parent_ids, device=device) - world_rot = joint_local_to_world_levelorder(oriented_local, levels) - # Drop the synthetic root index 0 to match self.rig_joint_names. - return world_rot[:, 1:, :, :] - - # ------------------------------------------------------------------ - # Internals - # ------------------------------------------------------------------ - - def visualize( - self, - viser_server: Any, - vertices: torch.Tensor | np.ndarray, - root_path: str = "/soma", - rgba: np.ndarray | None = None, - ) -> None: - """Visualize SOMA mesh in viser. - - Args: - viser_server: Viser server instance. - vertices: Mesh vertices, shape (V, 3). - root_path: Root path in viser scene tree. - rgba: RGBA color array, shape (4,). Defaults to skin tone. - """ - try: - from judo.visualizers.model import add_mesh # noqa: PLC0415 - except ImportError: # pragma: no cover - judo is optional in some envs - return - if self.faces is None: - return - - if isinstance(vertices, torch.Tensor): - vertices = vertices.detach().cpu().numpy() - if rgba is None: - rgba = np.array([255, 219, 172, 180]) - - add_mesh( - viser_server, - f"{root_path}/mesh", - vertices=np.asarray(vertices, dtype=np.float64), - faces=np.asarray(self.faces, dtype=np.int64), - pos=np.array([0, 0, 0]), - quat=np.array([1, 0, 0, 0]), - rgba=rgba, - ) - - def _load_npz(self, params_path: str | Path) -> dict[str, Any]: - """Load and lightly validate ``soma_params.npz`` keys.""" - path = Path(params_path) - if not path.is_file(): - raise FileNotFoundError(f"SOMA params not found: {path}") - archive = np.load(path, allow_pickle=True) - keys = set(archive.files) - required = { - "poses", - "transl", - "joint_names", - "identity_model_type", - "identity_coeffs", - "scale_params", - "joint_orient", - "unit", - "keep_root", - } - missing = required - keys - if missing: - raise ValueError( - f"SOMA params at {path} is missing fields: {sorted(missing)}; present={sorted(keys)}" - ) - - params = {k: archive[k] for k in archive.files} - params["unit"] = ( - str(params["unit"].item()) - if params["unit"].dtype.kind == "U" - else str(params["unit"]) - ) - identity_model_type = params["identity_model_type"] - params["identity_model_type"] = ( - str(identity_model_type.item()) - if identity_model_type.dtype.kind == "U" - else str(identity_model_type) - ) - if params["identity_model_type"] != self.identity_model_type: - raise ValueError( - f"SOMA params identity_model_type={params['identity_model_type']!r} " - f"does not match SOMALayer={self.identity_model_type!r}. " - "Pass identity_model_type= when constructing SOMA(...)." - ) - params["joint_names"] = [str(n) for n in params["joint_names"]] - if params["joint_names"] != self.rig_joint_names: - raise ValueError( - "SOMA params joint_names disagree with SOMALayer.rig_data joint_names. " - "Re-export the sequence with the same SOMA-X version." - ) - return params - - -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- - - -def _first_frame_transform( - root_rotvec_first: np.ndarray, - transl_first: np.ndarray, -) -> dict[str, np.ndarray]: - """Build the first-frame anchoring transform. - - The returned transform satisfies:: - - p_anchored = (p_world - transl_first) @ R_first_inv.T - - for any world-frame point ``p_world``. ``root_rotvec_first`` is the - exporter root pose at frame 0 (``poses[0, 0]`` for ``keep_root=False``). - Do not use the reconstructed Hips joint-world orientation here: it is a - rig joint frame, not the gravity-preserving world/root transform. - """ - R_first = R.from_rotvec(np.asarray(root_rotvec_first, dtype=np.float64)).as_matrix() - return { - "transl_first": np.asarray(transl_first, dtype=np.float64), - "R_first": R_first, - "R_first_inv": R_first.T, - } - - -def _normalize_to_first_frame( - *, - joints: np.ndarray, - joints_wxyz: np.ndarray, - vertices: np.ndarray, - transform: dict[str, np.ndarray], -) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Normalize so frame 0 is centered at the origin with canonical orientation. - - Post-processing: subtract frame-0 root - translation and undo frame-0 root rotation across all frames so the - downstream G1 retargeter sees a sequence anchored at the origin. This - keeps the existing first-frame ground-anchoring logic in - ``soma_to_g1.py``. - """ - transl_first = transform["transl_first"] - R_first_inv = transform["R_first_inv"] - - T_frames = joints.shape[0] - joints_out = joints.astype(np.float64).copy() - vertices_out = vertices.astype(np.float64).copy() - joints_wxyz_out = joints_wxyz.astype(np.float64).copy() - - for t in range(T_frames): - joints_out[t] = (joints_out[t] - transl_first) @ R_first_inv.T - vertices_out[t] = (vertices_out[t] - transl_first) @ R_first_inv.T - for j in range(joints_wxyz_out.shape[1]): - q = joints_wxyz_out[t, j] - mat = R.from_quat(np.array([q[1], q[2], q[3], q[0]])).as_matrix() - mat_corrected = R_first_inv @ mat - xyzw = R.from_matrix(mat_corrected).as_quat() - joints_wxyz_out[t, j] = np.array([xyzw[3], xyzw[0], xyzw[1], xyzw[2]]) - - return joints_out, joints_wxyz_out, vertices_out - - -__all__ = [ - "SOMA", - "SOMAMotion", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/retarget_utils.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/retarget_utils.py deleted file mode 100644 index 56284730..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/retarget_utils.py +++ /dev/null @@ -1,191 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Shared utilities for converting hand-object datasets to ManoSharpaData schema. - -Use this module from dataset-specific conversion scripts (e.g. ARCTIC, TACO) -to avoid duplicating IK setup, per-frame IK, and saving logic. -""" - -from typing import Any, Literal, Optional - -import numpy as np -import torch -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.retarget import G1_URDF_DIR, MESHES_DIR, SHARPA_WAVE_XMLS_DIR -from robotic_grounding.retarget.hand_kinematics import ( - Dex3HandKinematics, - HandKinematics, - SharpaHandKinematics, -) - -# Default partition columns when saving to Parquet (shared across datasets) -DEFAULT_PARTITION_COLS = ["sequence_id", "robot_name"] - - -def setup_sharpa_kinematics( - side: Literal["right", "left"], - frequency: float = 200.0, - max_iter: int = 100, - frame_tasks_converged_threshold: float = 1e-6, -) -> HandKinematics: - """Create HandKinematics for the Sharpa hand. - - Args: - side: "right" or "left". - frequency: Solver frequency (Hz). - max_iter: Maximum number of IK iterations. - frame_tasks_converged_threshold: Convergence threshold for IK. - - Returns: - HandKinematics instance for the given side. - """ - robot_asset_path = str(SHARPA_WAVE_XMLS_DIR / f"{side}_sharpawave.xml") - return SharpaHandKinematics( - side=side, - robot_asset_path=robot_asset_path, - source_model="mano", - use_relative_frames=False, - max_iter=max_iter, - frequency=frequency, - frame_tasks_converged_threshold=frame_tasks_converged_threshold, - ) - - -def setup_dex3_kinematics( - side: Literal["right", "left"], - frequency: float = 100.0, - max_iter: int = 100, - frame_tasks_converged_threshold: float = 1e-6, -) -> HandKinematics: - """Create HandKinematics for the Dex3 hand with MANO source. - - Args: - side: "right" or "left". - frequency: Solver frequency (Hz). - max_iter: Maximum number of IK iterations. - frame_tasks_converged_threshold: Convergence threshold for IK. - - Returns: - HandKinematics instance for the given side. - """ - robot_asset_path = str(G1_URDF_DIR / f"dex3_{side}.urdf") - package_dirs = [str(MESHES_DIR)] - return Dex3HandKinematics( - side=side, - robot_asset_path=robot_asset_path, - package_dirs=package_dirs, - source_model="mano", - use_relative_frames=False, - max_iter=max_iter, - frequency=frequency, - frame_tasks_converged_threshold=frame_tasks_converged_threshold, - ) - - -def run_frame_ik( - right_kinematics: HandKinematics, - left_kinematics: HandKinematics, - right_mano_joints: torch.Tensor, - right_mano_joints_wxyz: torch.Tensor, - left_mano_joints: torch.Tensor, - left_mano_joints_wxyz: torch.Tensor, - mano_to_robot_scale: float, - right_qpos_prev: Optional[np.ndarray] = None, - left_qpos_prev: Optional[np.ndarray] = None, - right_wrist_position: Optional[np.ndarray] = None, - right_wrist_quat_xyzw: Optional[np.ndarray] = None, - left_wrist_position: Optional[np.ndarray] = None, - left_wrist_quat_xyzw: Optional[np.ndarray] = None, -) -> tuple[np.ndarray, np.ndarray, dict[str, Any], dict[str, Any]]: - """Run IK for one frame for both hands. - - When right_qpos_prev (or left_qpos_prev) is None, it is initialized from - right_wrist_position and right_wrist_quat_xyzw (and left equivalents). - Quat must be in Pinocchio/qpos order (xyzw). After the first frame, pass - the returned right_qpos/left_qpos as right_qpos_prev/left_qpos_prev. - - Args: - right_kinematics: Right-hand kinematics. - left_kinematics: Left-hand kinematics. - right_mano_joints: Right MANO joints (21, 3). - right_mano_joints_wxyz: Right MANO joint quats wxyz (21, 4). - left_mano_joints: Left MANO joints (21, 3). - left_mano_joints_wxyz: Left MANO joint quats wxyz (21, 4). - mano_to_robot_scale: Scale from MANO to robot. - right_qpos_prev: Previous right qpos (None to initialize from wrist). - left_qpos_prev: Previous left qpos (None to initialize from wrist). - right_wrist_position: Used when right_qpos_prev is None (3,). - right_wrist_quat_xyzw: Used when right_qpos_prev is None (4,) xyzw. - left_wrist_position: Used when left_qpos_prev is None (3,). - left_wrist_quat_xyzw: Used when left_qpos_prev is None (4,) xyzw. - - Returns: - (right_qpos, left_qpos, right_kinematics_results, left_kinematics_results). - """ - if right_qpos_prev is None: - right_qpos = right_kinematics.robot.q0.copy() - right_qpos[:3] = right_wrist_position - right_qpos[3:7] = right_wrist_quat_xyzw - else: - right_qpos = right_qpos_prev.copy() - - if left_qpos_prev is None: - left_qpos = left_kinematics.robot.q0.copy() - left_qpos[:3] = left_wrist_position - left_qpos[3:7] = left_wrist_quat_xyzw - else: - left_qpos = left_qpos_prev.copy() - - right_results = right_kinematics.compute( - right_mano_joints, - right_mano_joints_wxyz, - source_to_robot_scale=mano_to_robot_scale, - qpos=right_qpos, - ) - right_qpos = right_results["q"] - - left_results = left_kinematics.compute( - left_mano_joints, - left_mano_joints_wxyz, - source_to_robot_scale=mano_to_robot_scale, - qpos=left_qpos, - ) - left_qpos = left_results["q"] - - return right_qpos, left_qpos, right_results, left_results - - -def wrist_pose_from_mano_joint0( - joint0_position: np.ndarray, - joint0_wxyz: np.ndarray, - link_to_site_quat_xyzw: Optional[np.ndarray] = None, -) -> tuple[np.ndarray, np.ndarray]: - """Compute robot wrist position and quat (xyzw) from MANO wrist (joint 0). - - Optionally applies a rotation offset (e.g. from link frame to site). - Default offset used in ARCTIC conversion: [0.5, -0.5, 0.5, 0.5] in wxyz. - - Args: - joint0_position: MANO wrist position (3,). - joint0_wxyz: MANO wrist quaternion wxyz (4,). - link_to_site_quat_xyzw: If given, rotation offset in xyzw applied - to the right (e.g. link to site). Default None = no offset. - - Returns: - (position, quat_xyzw) for use as wrist_position and wrist_quat_xyzw - in run_frame_ik(). - """ - pos = np.asarray(joint0_position, dtype=np.float64) - q_mano = R.from_quat( - np.asarray(joint0_wxyz, dtype=np.float64).reshape(4), - scalar_first=True, - ) - if link_to_site_quat_xyzw is not None: - q_offset = R.from_quat( - np.asarray(link_to_site_quat_xyzw, dtype=np.float64).reshape(4), - scalar_first=False, - ) - q_mano = q_mano * q_offset.inv() - quat_xyzw = q_mano.as_quat(scalar_first=False) - return pos, quat_xyzw diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/robot_config.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/robot_config.py deleted file mode 100644 index 22ad1622..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/robot_config.py +++ /dev/null @@ -1,581 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Per-robot retarget config loader. - -Reads a two-file bundle per robot: - -* ``configs//frame_alignment.json`` -> world axis swap + per-bone - rotation/translation corrections that map the source skeleton frame - into the robot frame. -* ``configs//retargeter.json`` -> URDF, IK map, foot framing, - optional posture-task weights. - -Files of either shape can be supplied to :func:`load_robot_config`, which -returns a :class:`RobotRetargetConfig` whose ``r_per_bone`` and -``r_per_link`` dicts already hold the final composed numpy 3x3 matrices the -IK runtime expects. Runtime code does not need to know whether a value was -supplied as ``q_offset_xyzw``, ``q_offset_matrix``, or composed with a -``wrist_tweaks`` entry. - -Convention rules (must match anything that consumes a -``RobotRetargetConfig`` value): - -* Matrices are row-major 3x3 lists. -* Quaternions are ``xyzw``. -* Distances are meters. -* Robot world convention: X = forward, Y = left, Z = up. -* Rotation composition is right-multiply: - ``target_rot = R_world @ source_rot @ correction``. -* ``joint_offsets`` represent source-joint-local -> robot-link-local basis - changes (right-multiplied onto the SOMA bone's global rotation). -* ``wrist_tweaks`` are post-correction right-multiplies on individual - robot frames; rename to ``per_link_tweaks`` if you need them on - non-wrist links. - -Schema is intentionally minimal. Bump ``schema_version`` for changes -that are not backward-compatible (renamed/removed fields, or fields -whose defaults would silently alter existing IK behaviour). Optional -fields whose missing-value default is a strict no-op (e.g. -``posture_task`` defaulting to all-zero costs) do not need a bump. -""" - -from __future__ import annotations - -import json -from dataclasses import dataclass, field -from pathlib import Path -from typing import Any - -import numpy as np -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.retarget import ASSETS_DIR - -CONFIGS_DIR = Path(__file__).resolve().parent / "configs" -"""Default location for ``configs//{frame_alignment,retargeter}.json``.""" - -SCHEMA_VERSION = 1 - - -@dataclass(frozen=True) -class IkMapEntry: - """One row of the IK end-effector map.""" - - soma_joint: str - position_cost: float - orientation_cost: float - - -@dataclass(frozen=True) -class PostureTaskConfig: - """Posture-regularization weights for the per-frame IK QP. - - Both costs default to ``0.0`` which means "no posture term at all" - -- the JSON block is optional, and configs that omit it land at - these defaults so existing behaviour is preserved bit-for-bit. - - The two costs are wired into Pink ``PostureTask`` instances that - only act on the actuated DoFs (Pink strips the floating-base - tangent prefix automatically). They are independent: ``q0_cost`` - pulls every frame's ``q`` toward ``robot.q0`` (a stationary - regularizer), while ``q_prev_cost`` pulls toward the previous - frame's IK solution (a temporal smoother). - - Attributes: - q0_cost: Cost weight (``[homogeneous_cost] / rad``) for the - "regularize toward ``robot.q0``" posture term. ``0.0`` - disables the term entirely. - q_prev_cost: Cost weight for the "track previous-frame ``q``" - posture term. On frame 0 this collapses to a pull toward - ``q0`` (since the warm-start ``qpos`` is ``q0``). - lm_damping: Levenberg-Marquardt damping used by both posture - tasks. Same units as Pink's ``Task.lm_damping``. - gain: Pink task gain in ``[0, 1]`` (low-pass-style filtering on - the posture error). ``1.0`` is dead-beat tracking. - """ - - q0_cost: float = 0.0 - q_prev_cost: float = 0.0 - lm_damping: float = 0.0 - gain: float = 1.0 - - -@dataclass(frozen=True) -class RobotRetargetConfig: - """Final, fully materialized robot retarget config. - - Attributes: - robot_name: Name of the robot (e.g. ``"g1"``). - source_model: Source skeleton this config is wired against - (e.g. ``"soma"``). The IK runtime uses this to dispatch to the - correct loader; we don't model two source skeletons in the - same config. - urdf_path: Resolved absolute path to the URDF file. - package_dirs: Resolved absolute paths used to resolve - ``package://`` mesh URLs. - ik_map: Mapping ``robot_frame -> IkMapEntry``. - foot_frames: Robot frame names used for ground anchoring. - ankle_roll_offset: Z distance from ``*_ankle_roll_link`` joint - origin down to the foot sole (meters). - base_source_joint: SOMA joint used as the scaling anchor; usually - ``"Hips"``. - r_world: ``(3, 3)`` rotation, source world -> robot world. - r_per_bone: ``soma_joint -> (3, 3)`` correction. Right-multiplied - onto the SOMA bone's global rotation; not currently consumed - directly by the runtime, but kept so probes / migration - scripts can inspect the per-bone offset before any per-link - tweak is applied. - r_per_link: ``robot_frame -> (3, 3)`` final composed correction - returned by ``get_frame_rotation_correction``. This is what - ``set_frame_tasks_target`` reads. - t_per_link: ``robot_frame -> (3,)`` translation offset, expressed - in the corrected robot-link local frame. Applied at runtime - as ``target_pos += target_rot @ t_offset`` in - ``set_frame_tasks_target`` (mirrors soma-retargeter's - ``wp.quat_rotate(q, offset_tx.p)`` term in - ``wp_compute_scaled_effectors``). Frames that have no - mapping return zeros. - joint_translation_offsets: ``soma_joint -> (3,)`` offset vector - from the frame_alignment config; the runtime consumes the - ``robot_frame``-keyed projection ``t_per_link`` instead, but - this dict is kept for tools that need to look up the offset - by SOMA joint. - auto_derived_joints: SOMA joint names whose ``q_offset`` was - computed by ``scripts/retarget/derive_soma_g1_corrections.py`` - (i.e. via the formula ``(R_world @ soma_world_rest).T @ - robot_world_q0``). The regression test uses this list to - decide which rows must satisfy the q0-residual invariant; - joints whose corrections were tuned empirically are - intentionally absent from this list. - posture_task: Optional posture-regularization weights. Defaults - to all-zero costs (no posture term), which is a strict - no-op compatible with the existing SOMA bit-equivalence - regression. Consumed by ``WholeBodyKinematics``. - """ - - robot_name: str - source_model: str - urdf_path: Path - package_dirs: list[Path] - ik_map: dict[str, IkMapEntry] - foot_frames: list[str] - ankle_roll_offset: float - base_source_joint: str - r_world: np.ndarray - r_per_bone: dict[str, np.ndarray] - r_per_link: dict[str, np.ndarray] - t_per_link: dict[str, np.ndarray] = field(default_factory=dict) - joint_translation_offsets: dict[str, np.ndarray] = field(default_factory=dict) - auto_derived_joints: list[str] = field(default_factory=list) - posture_task: PostureTaskConfig = field(default_factory=PostureTaskConfig) - - -def _decode_rotation( - payload: dict[str, Any], - *, - matrix_key: str, - quat_key: str, - context: str, -) -> np.ndarray: - """Decode either a 3x3 matrix field or an xyzw quaternion field. - - ``payload`` is the surrounding object, ``matrix_key`` and ``quat_key`` - name the two mutually-exclusive fields. Raises ``ValueError`` if both - or neither are present. - """ - has_matrix = matrix_key in payload - has_quat = quat_key in payload - if has_matrix == has_quat: - raise ValueError( - f"{context}: must specify exactly one of {matrix_key!r} or " - f"{quat_key!r}; got matrix={has_matrix}, quat={has_quat}." - ) - if has_matrix: - mat = np.asarray(payload[matrix_key], dtype=np.float64) - if mat.shape != (3, 3): - raise ValueError( - f"{context}: {matrix_key!r} must be a 3x3 list, got shape {mat.shape}." - ) - return mat - quat_xyzw = np.asarray(payload[quat_key], dtype=np.float64) - if quat_xyzw.shape != (4,): - raise ValueError( - f"{context}: {quat_key!r} must be a 4-element xyzw quaternion, " - f"got shape {quat_xyzw.shape}." - ) - return R.from_quat(quat_xyzw).as_matrix() - - -def _check_schema_version(payload: dict[str, Any], path: Path) -> None: - """Reject configs from a future or missing schema.""" - version = payload.get("schema_version") - if version is None: - raise ValueError( - f"{path}: missing required field 'schema_version'. " - f"Expected {SCHEMA_VERSION}." - ) - if version != SCHEMA_VERSION: - raise ValueError( - f"{path}: schema_version {version} is not supported by this " - f"loader (expected {SCHEMA_VERSION}). Update the loader or " - f"regenerate the config." - ) - - -def _check_required_fields( - payload: dict[str, Any], required: list[str], path: Path -) -> None: - """Raise ``ValueError`` if any of ``required`` is missing.""" - missing = [name for name in required if name not in payload] - if missing: - raise ValueError(f"{path}: missing required fields: {missing}.") - - -def _parse_posture_task(payload: dict[str, Any], path: Path) -> PostureTaskConfig: - """Parse the optional ``posture_task`` block from a retargeter payload. - - Missing block -> all-zero defaults, which the IK runtime treats as - "no posture term"; this is the strict no-op path for backward - compatibility with configs predating this field. - """ - raw = payload.get("posture_task") - if raw is None: - return PostureTaskConfig() - if not isinstance(raw, dict): - raise ValueError( - f"{path}: 'posture_task' must be an object, got " f"{type(raw).__name__}." - ) - allowed = {"q0_cost", "q_prev_cost", "lm_damping", "gain"} - unknown = set(raw) - allowed - if unknown: - raise ValueError( - f"{path}: 'posture_task' contains unknown fields {sorted(unknown)}; " - f"expected a subset of {sorted(allowed)}." - ) - q0_cost = float(raw.get("q0_cost", 0.0)) - q_prev_cost = float(raw.get("q_prev_cost", 0.0)) - lm_damping = float(raw.get("lm_damping", 0.0)) - gain = float(raw.get("gain", 1.0)) - if q0_cost < 0.0 or q_prev_cost < 0.0: - raise ValueError( - f"{path}: 'posture_task' costs must be non-negative; got " - f"q0_cost={q0_cost}, q_prev_cost={q_prev_cost}." - ) - if lm_damping < 0.0: - raise ValueError( - f"{path}: 'posture_task.lm_damping' must be non-negative; got " - f"{lm_damping}." - ) - if not 0.0 <= gain <= 1.0: - raise ValueError(f"{path}: 'posture_task.gain' must be in [0, 1]; got {gain}.") - return PostureTaskConfig( - q0_cost=q0_cost, - q_prev_cost=q_prev_cost, - lm_damping=lm_damping, - gain=gain, - ) - - -def _load_frame_alignment(alignment_path: Path) -> tuple[ - np.ndarray, - dict[str, np.ndarray], - dict[str, np.ndarray], - str, - str, - list[str], -]: - """Read ``frame_alignment.json``. - - Returns (R_world, r_per_bone, t_offsets, robot_name, source_model, - auto_derived_joints). - """ - with alignment_path.open() as f: - payload = json.load(f) - - _check_schema_version(payload, alignment_path) - _check_required_fields( - payload, - [ - "robot_name", - "source_model", - "joint_offsets", - ], - alignment_path, - ) - - robot_name = str(payload["robot_name"]) - source_model = str(payload["source_model"]) - - r_world = _decode_rotation( - payload, - matrix_key="world_axis_swap_matrix", - quat_key="world_axis_swap_xyzw", - context=f"{alignment_path}: world axis swap", - ) - - r_per_bone: dict[str, np.ndarray] = {} - t_offsets: dict[str, np.ndarray] = {} - for soma_joint, entry in payload["joint_offsets"].items(): - if not isinstance(entry, dict): - raise ValueError( - f"{alignment_path}: joint_offsets[{soma_joint!r}] must be an " - f"object, got {type(entry).__name__}." - ) - r_per_bone[soma_joint] = _decode_rotation( - entry, - matrix_key="q_offset_matrix", - quat_key="q_offset_xyzw", - context=f"{alignment_path}: joint_offsets[{soma_joint!r}]", - ) - t = entry.get("t_offset", [0.0, 0.0, 0.0]) - t_arr = np.asarray(t, dtype=np.float64) - if t_arr.shape != (3,): - raise ValueError( - f"{alignment_path}: joint_offsets[{soma_joint!r}].t_offset " - f"must be a 3-element vector, got shape {t_arr.shape}." - ) - t_offsets[soma_joint] = t_arr - - auto_derived_joints = [ - str(name) for name in payload.get("_auto_derived_joints", []) - ] - return ( - r_world, - r_per_bone, - t_offsets, - robot_name, - source_model, - auto_derived_joints, - ) - - -def _load_retargeter( - retargeter_path: Path, - *, - robot_name: str, - source_model: str, -) -> tuple[ - Path, - list[Path], - dict[str, IkMapEntry], - list[str], - float, - str, - dict[str, np.ndarray], - PostureTaskConfig, -]: - """Read ``retargeter.json``; cross-check robot_name/source_model with frame_alignment.""" - with retargeter_path.open() as f: - payload = json.load(f) - - _check_schema_version(payload, retargeter_path) - _check_required_fields( - payload, - [ - "robot_name", - "source_model", - "urdf", - "package_dirs", - "ik_map", - "foot_frames", - "ankle_roll_offset", - "base_source_joint", - ], - retargeter_path, - ) - - if str(payload["robot_name"]) != robot_name: - raise ValueError( - f"{retargeter_path}: robot_name mismatch; frame_alignment.json reports " - f"{robot_name!r}, retargeter.json reports {payload['robot_name']!r}." - ) - if str(payload["source_model"]) != source_model: - raise ValueError( - f"{retargeter_path}: source_model mismatch; frame_alignment.json reports " - f"{source_model!r}, retargeter.json reports " - f"{payload['source_model']!r}." - ) - - config_root = retargeter_path.parent - urdf_path = (config_root / payload["urdf"]).resolve() - package_dirs = [(config_root / p).resolve() for p in payload["package_dirs"]] - - ik_map_raw = payload["ik_map"] - ik_map: dict[str, IkMapEntry] = {} - for robot_frame, entry in ik_map_raw.items(): - if not isinstance(entry, dict): - raise ValueError( - f"{retargeter_path}: ik_map[{robot_frame!r}] must be an " - f"object, got {type(entry).__name__}." - ) - ik_map[robot_frame] = IkMapEntry( - soma_joint=str(entry["soma_joint"]), - position_cost=float(entry["position_cost"]), - orientation_cost=float(entry["orientation_cost"]), - ) - - foot_frames = [str(name) for name in payload["foot_frames"]] - ankle_roll_offset = float(payload["ankle_roll_offset"]) - base_source_joint = str(payload["base_source_joint"]) - - per_link_tweaks: dict[str, np.ndarray] = {} - raw_tweaks = payload.get("wrist_tweaks") or payload.get("per_link_tweaks") or {} - for robot_frame, entry in raw_tweaks.items(): - if not isinstance(entry, dict): - raise ValueError( - f"{retargeter_path}: per_link_tweaks[{robot_frame!r}] must " - f"be an object, got {type(entry).__name__}." - ) - per_link_tweaks[robot_frame] = _decode_rotation( - entry, - matrix_key="r_offset_matrix", - quat_key="r_offset_xyzw", - context=f"{retargeter_path}: per_link_tweaks[{robot_frame!r}]", - ) - - posture_task = _parse_posture_task(payload, retargeter_path) - - return ( - urdf_path, - package_dirs, - ik_map, - foot_frames, - ankle_roll_offset, - base_source_joint, - per_link_tweaks, - posture_task, - ) - - -def _compose_per_link( - *, - ik_map: dict[str, IkMapEntry], - r_per_bone: dict[str, np.ndarray], - per_link_tweaks: dict[str, np.ndarray], -) -> dict[str, np.ndarray]: - """Build the final ``robot_frame -> 3x3`` correction the IK consumes. - - Composition: ``r_per_bone[ik_map[frame].soma_joint] @ per_link_tweaks[frame]``. - Either piece may be missing; missing => identity. - """ - out: dict[str, np.ndarray] = {} - for robot_frame, entry in ik_map.items(): - bone_correction = r_per_bone.get(entry.soma_joint, np.eye(3)) - tweak = per_link_tweaks.get(robot_frame, np.eye(3)) - out[robot_frame] = bone_correction @ tweak - return out - - -def load_robot_config( - robot_name: str, - *, - configs_dir: Path | None = None, -) -> RobotRetargetConfig: - """Load and validate the per-robot retarget config bundle. - - Args: - robot_name: Folder name under ``configs_dir``. - configs_dir: Override path to the configs directory; defaults to - the in-repo ``configs/`` next to this module. - - Returns: - A fully materialized :class:`RobotRetargetConfig`. All matrices - are pre-composed numpy arrays; runtime code does not need to know - whether the JSON used quaternions or matrices. - """ - base = Path(configs_dir).resolve() if configs_dir is not None else CONFIGS_DIR - robot_root = base / robot_name - if not robot_root.is_dir(): - raise FileNotFoundError( - f"Robot config directory not found: {robot_root}. Available " - f"robots: {sorted(p.name for p in base.iterdir() if p.is_dir())}" - if base.is_dir() - else f"Robot config directory not found: {robot_root}." - ) - - alignment_path = robot_root / "frame_alignment.json" - retargeter_path = robot_root / "retargeter.json" - if not alignment_path.is_file(): - raise FileNotFoundError(f"Missing required config: {alignment_path}") - if not retargeter_path.is_file(): - raise FileNotFoundError(f"Missing required config: {retargeter_path}") - - ( - r_world, - r_per_bone, - t_offsets, - alignment_robot_name, - source_model, - auto_derived_joints, - ) = _load_frame_alignment(alignment_path) - - if alignment_robot_name != robot_name: - raise ValueError( - f"{alignment_path}: robot_name field {alignment_robot_name!r} does " - f"not match the requested {robot_name!r}." - ) - - ( - urdf_path, - package_dirs, - ik_map, - foot_frames, - ankle_roll_offset, - base_source_joint, - per_link_tweaks, - posture_task, - ) = _load_retargeter( - retargeter_path, - robot_name=robot_name, - source_model=source_model, - ) - - if not urdf_path.is_file(): - raise FileNotFoundError( - f"{retargeter_path}: urdf path resolves to {urdf_path}, which does not exist." - ) - - r_per_link = _compose_per_link( - ik_map=ik_map, - r_per_bone=r_per_bone, - per_link_tweaks=per_link_tweaks, - ) - - t_per_link: dict[str, np.ndarray] = {} - for robot_frame, entry in ik_map.items(): - t_per_link[robot_frame] = t_offsets.get( - entry.soma_joint, np.zeros(3, dtype=np.float64) - ) - - return RobotRetargetConfig( - robot_name=robot_name, - source_model=source_model, - urdf_path=urdf_path, - package_dirs=package_dirs, - ik_map=ik_map, - foot_frames=foot_frames, - ankle_roll_offset=ankle_roll_offset, - base_source_joint=base_source_joint, - r_world=r_world, - r_per_bone=r_per_bone, - r_per_link=r_per_link, - t_per_link=t_per_link, - joint_translation_offsets=t_offsets, - auto_derived_joints=auto_derived_joints, - posture_task=posture_task, - ) - - -__all__ = [ - "CONFIGS_DIR", - "SCHEMA_VERSION", - "IkMapEntry", - "PostureTaskConfig", - "RobotRetargetConfig", - "load_robot_config", -] - - -# Silence unused-import linters; ``ASSETS_DIR`` is intentionally re-exposed -# for tools that want to resolve mesh paths relative to the repo root. -_ = ASSETS_DIR diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/support_recon.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/support_recon.py deleted file mode 100644 index 4ee68e52..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/support_recon.py +++ /dev/null @@ -1,597 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Support-surface reconstruction from object still-poses. - -Auto-detects schema (motion_v1 / mano_sharpa) from parquet columns, finds -frames where each object body is at rest, and writes a USD file of disk -prims that the sim spawner can load. Used both by the standalone CLI -(``scripts/reconstruct_support_surfaces.py``) and by the planner -(``planner/g1_planner.py``) to regenerate surfaces for transformed object -trajectories. -""" - -from __future__ import annotations - -import colorsys -import random -from pathlib import Path -from typing import Any - -import numpy as np -import pyarrow.parquet as pq -import trimesh -from pxr import Gf, Usd, UsdGeom, UsdPhysics -from scipy.spatial.transform import Rotation - -from robotic_grounding.assets import ASSET_DIR -from robotic_grounding.motion_schema import load_motion_data_parquet -from robotic_grounding.retarget.data_logger import ManoSharpaData - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- - -DISK_HEIGHT = 0.01 # thin disk thickness in meters -DISK_RADIUS_SCALE = ( - 1.0 # multiplicative bloat on the computed radius; > 1.0 enlarges disks -) -GROUND_Z_THRESHOLD = 0.05 # disks below this Z are on the ground plane - - -# --------------------------------------------------------------------------- -# Data loading -# --------------------------------------------------------------------------- - - -def _resolve_local_mesh_path(path: str) -> str | None: - """Resolve a possibly-stale absolute path to a local file under ``ASSET_DIR``. - - Parquets emitted from container builds carry absolute Docker mesh paths - (``/workspace/.../assets/meshes/...``) that don't exist locally. We - take the suffix after ``assets/meshes/`` and re-root it under the - repo's local ``ASSET_DIR/meshes/``. - """ - if not path: - return None - if Path(path).exists(): - return path - if "assets/meshes/" not in path: - return None - suffix = path.rsplit("assets/meshes/", maxsplit=1)[-1] - local = Path(ASSET_DIR) / "meshes" / suffix - if local.exists(): - return str(local) - tex = local.parent / "mesh_tex.obj" - if tex.exists(): - return str(tex) - return None - - -def _load_object_meshes_from_paths( - object_mesh_paths: list[str], - object_body_names: list[str], -) -> dict[str, trimesh.Trimesh]: - """Load object meshes from schema paths (one per body). - - Paths ending with ``_cm.obj`` are scaled by 0.01 (cm -> m). Stale - container paths are remapped to the local ``ASSET_DIR``. - """ - meshes: dict[str, trimesh.Trimesh] = {} - for part, path in zip(object_body_names, object_mesh_paths, strict=True): - resolved = _resolve_local_mesh_path(path) - if resolved is None: - continue - mesh = trimesh.load(resolved) - if isinstance(mesh, trimesh.Scene): - mesh = mesh.dump(concatenate=True) - if resolved.endswith("_cm.obj"): - mesh.vertices *= 0.01 - meshes[part] = mesh - return meshes - - -def _detect_parquet_schema(input_dir: Path) -> str: - """Return ``"motion_v1"`` or ``"mano_sharpa"`` for a parquet dataset.""" - first = next(Path(input_dir).rglob("*.parquet"), None) - if first is None: - raise FileNotFoundError(f"No parquet files under {input_dir}") - columns = set(pq.ParquetFile(str(first)).schema.names) - if "schema_version" in columns: - return "motion_v1" - return "mano_sharpa" - - -def _load_parquet_data(input_dir: Path, sequence_id: str, schema: str) -> Any: - """Load one sequence from Parquet using the appropriate data logger class.""" - filters = [("sequence_id", "=", sequence_id)] - if schema == "motion_v1": - partition_dir = Path(input_dir) / f"sequence_id={sequence_id}" - inner = next(partition_dir.glob("robot_name=*"), None) - if inner is None: - raise FileNotFoundError(f"No robot_name=* partition under {partition_dir}") - return load_motion_data_parquet(str(inner)) - return ManoSharpaData.from_parquet(str(input_dir), filters=filters) - - -def load_object_mesh_and_poses( - input_dir: Path, - sequence_id: str, - schema: str | None = None, -) -> tuple[Any, dict[str, trimesh.Trimesh]]: - """Load one sequence and its object meshes. - - Returns: - ``(data, object_meshes)`` where ``data`` exposes ``object_body_position``, - ``object_body_wxyz``, etc., and ``object_meshes`` maps body name to - a ``trimesh.Trimesh``. - """ - if schema is None: - schema = _detect_parquet_schema(input_dir) - data = _load_parquet_data(input_dir, sequence_id, schema) - object_mesh_paths = getattr(data, "object_mesh_paths", None) or [] - object_body_names = getattr(data, "object_body_names", None) or [] - if object_mesh_paths and len(object_mesh_paths) == len(object_body_names): - object_meshes = _load_object_meshes_from_paths( - object_mesh_paths, - object_body_names, - ) - else: - object_meshes = {} - return data, object_meshes - - -# --------------------------------------------------------------------------- -# Still-frame detection -# --------------------------------------------------------------------------- - - -def _frames_where_object_still( - positions: np.ndarray, - quats_wxyz: np.ndarray | None = None, - pos_threshold_m: float = 0.001, - angle_threshold_rad: float = 0.01, - min_consecutive: int = 5, -) -> np.ndarray: - """Return frame indices where the object is still. - - A frame is locally still when its inter-frame displacement and orientation - change are both under the given thresholds. Only runs of at least - ``min_consecutive`` locally-still frames are returned. - """ - positions = np.asarray(positions, dtype=np.float64) - n_frames = positions.shape[0] - if n_frames == 0: - return np.array([], dtype=np.intp) - if n_frames == 1: - return np.array([0], dtype=np.intp) - - pos_delta = np.linalg.norm(np.diff(positions, axis=0), axis=1) - locally_still = np.zeros(n_frames, dtype=bool) - locally_still[0] = pos_delta[0] < pos_threshold_m - locally_still[1:] = pos_delta < pos_threshold_m - - if quats_wxyz is not None: - quats = np.asarray(quats_wxyz, dtype=np.float64) - if quats.shape[0] != n_frames or quats.shape[1] != 4: - raise ValueError("quats_wxyz must be (N, 4) with N = len(positions)") - quats = quats / np.linalg.norm(quats, axis=1, keepdims=True) - dot = np.abs(np.sum(quats[1:] * quats[:-1], axis=1)) - dot = np.clip(dot, 0.0, 1.0) - angle_delta = 2.0 * np.arccos(dot) - still_by_angle = np.zeros(n_frames, dtype=bool) - still_by_angle[0] = angle_delta[0] < angle_threshold_rad - still_by_angle[1:] = angle_delta < angle_threshold_rad - locally_still &= still_by_angle - - if min_consecutive <= 1: - return np.nonzero(locally_still)[0] - - result = np.zeros(n_frames, dtype=bool) - run_start = -1 - for i in range(n_frames): - if locally_still[i]: - if run_start < 0: - run_start = i - else: - if run_start >= 0 and (i - run_start) >= min_consecutive: - result[run_start:i] = True - run_start = -1 - if run_start >= 0 and (n_frames - run_start) >= min_consecutive: - result[run_start:n_frames] = True - - return np.nonzero(result)[0] - - -def still_frames_from_object_data( - data: Any, - body_index: int = 0, - pos_threshold_m: float = 0.001, - angle_threshold_rad: float = 0.01, -) -> np.ndarray: - """Return frame indices where the given object body is still.""" - obj_pos = data.object_body_position - obj_quat = data.object_body_wxyz - if hasattr(obj_pos, "cpu"): - obj_pos = obj_pos.cpu().numpy() - if hasattr(obj_quat, "cpu"): - obj_quat = obj_quat.cpu().numpy() - positions = np.array(obj_pos, dtype=np.float64) - quats = np.array(obj_quat, dtype=np.float64) - positions = positions[:, body_index, :] - quats = quats[:, body_index, :] - return _frames_where_object_still( - positions, - quats_wxyz=quats, - pos_threshold_m=pos_threshold_m, - angle_threshold_rad=angle_threshold_rad, - ) - - -# Alias kept for callers using the older name. -still_frames_from_mano_sharpa_data = still_frames_from_object_data - - -def extract_continuous_segments(still_frames: np.ndarray) -> list[np.ndarray]: - """Split sorted still-frame indices into contiguous runs.""" - if len(still_frames) == 0: - return [] - gaps = np.where(np.diff(still_frames) > 1)[0] + 1 - return np.split(still_frames, gaps) - - -# --------------------------------------------------------------------------- -# Support-disk computation -# --------------------------------------------------------------------------- - - -def _transform_vertices_to_world( - vertices: np.ndarray, - position: np.ndarray, - quat_wxyz: np.ndarray, -) -> np.ndarray: - """Rotate and translate local mesh vertices into world frame.""" - w, x, y, z = quat_wxyz - rot = Rotation.from_quat([x, y, z, w]) - return rot.apply(vertices) + np.asarray(position) - - -def compute_support_disk( - vertices_world: np.ndarray, -) -> tuple[float, float, float, float]: - """Compute a flat support disk enclosing the X-Y footprint of vertices. - - Returns ``(cx, cy, z, radius)`` — center of the X-Y AABB, minimum vertex - Z, and half the larger of the X/Y spans (scaled by ``DISK_RADIUS_SCALE``). - """ - xs = vertices_world[:, 0] - ys = vertices_world[:, 1] - zs = vertices_world[:, 2] - cx = (xs.min() + xs.max()) / 2.0 - cy = (ys.min() + ys.max()) / 2.0 - radius = max(xs.max() - xs.min(), ys.max() - ys.min()) / 2.0 * DISK_RADIUS_SCALE - z = float(zs.min()) - return cx, cy, z, radius - - -def _enclosing_circle_of_two( - cx1: float, - cy1: float, - r1: float, - cx2: float, - cy2: float, - r2: float, -) -> tuple[float, float, float]: - """Minimum enclosing circle of two circles.""" - dx = cx2 - cx1 - dy = cy2 - cy1 - d = np.hypot(dx, dy) - if d + r2 <= r1: - return cx1, cy1, r1 - if d + r1 <= r2: - return cx2, cy2, r2 - r_new = (d + r1 + r2) / 2.0 - ratio = (r_new - r1) / d if d > 0 else 0.5 - cx_new = cx1 + dx * ratio - cy_new = cy1 + dy * ratio - return cx_new, cy_new, r_new - - -def merge_overlapping_disks( - disks: list[tuple[float, float, float, float]], -) -> list[tuple[float, float, float, float]]: - """Iteratively merge disks that overlap in the X-Y plane. - - Each disk is ``(cx, cy, z, radius)``. The merged disk takes the minimum - Z of the pair. - """ - merged = list(disks) - changed = True - while changed: - changed = False - i = 0 - while i < len(merged): - j = i + 1 - while j < len(merged): - cx1, cy1, z1, r1 = merged[i] - cx2, cy2, z2, r2 = merged[j] - dist = np.hypot(cx2 - cx1, cy2 - cy1) - if dist < r1 + r2: - cx_n, cy_n, r_n = _enclosing_circle_of_two( - cx1, - cy1, - r1, - cx2, - cy2, - r2, - ) - merged[i] = (cx_n, cy_n, min(z1, z2), r_n) - merged.pop(j) - changed = True - else: - j += 1 - i += 1 - return merged - - -# --------------------------------------------------------------------------- -# USD output -# --------------------------------------------------------------------------- - - -def _random_pastel_color() -> Gf.Vec3f: - """Return a random pastel RGB color as a ``Gf.Vec3f``.""" - h = random.random() - s = random.uniform(0.25, 0.45) - v = random.uniform(0.85, 1.0) - r, g, b = colorsys.hsv_to_rgb(h, s, v) - return Gf.Vec3f(r, g, b) - - -def create_disk( - stage: Usd.Stage, - prim_path: str, - radius: float, - center: tuple[float, float, float], - height: float = DISK_HEIGHT, -) -> UsdGeom.Cylinder: - """Add a single Z-axis cylinder disk to a USD stage.""" - cyl = UsdGeom.Cylinder.Define(stage, prim_path) - cyl.CreateAxisAttr(UsdGeom.Tokens.z) - cyl.CreateHeightAttr(height) - cyl.CreateRadiusAttr(radius) - cyl.CreateDisplayColorAttr([_random_pastel_color()]) - UsdPhysics.CollisionAPI.Apply(cyl.GetPrim()) - cx, cy, cz = center - xf = UsdGeom.Xformable(cyl.GetPrim()) - xf.AddTranslateOp().Set((cx, cy, cz - height / 2.0)) - return cyl - - -def write_support_surfaces_usd( - all_disks: dict[str, list[tuple[float, float, float, float]]], - output_path: str, -) -> Usd.Stage: - """Write all support-surface disks into a single USD file.""" - output = Path(output_path) - if output.exists(): - output.unlink() - stage = Usd.Stage.CreateNew(str(output)) - stage.SetMetadata("metersPerUnit", 1.0) - UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) - - root = UsdGeom.Xform.Define(stage, "/support_surfaces") - stage.SetDefaultPrim(root.GetPrim()) - - disk_idx = 0 - for body_name, disks in all_disks.items(): - for cx, cy, z, radius in disks: - safe_name = "".join( - c if c.isalnum() or c == "_" else "_" for c in body_name - ) - if safe_name and safe_name[0].isdigit(): - safe_name = f"_{safe_name}" - prim_path = f"/support_surfaces/{safe_name}_{disk_idx}" - create_disk(stage, prim_path, radius=radius, center=(cx, cy, z)) - disk_idx += 1 - - stage.Save() - return stage - - -# --------------------------------------------------------------------------- -# Sequence-level driver -# --------------------------------------------------------------------------- - - -def _compute_height_offset(data: Any, schema: str) -> float: - """Z offset to align parquet data with the spawned scene. - - Object and support-surface positions from the retarget pipeline are - already ground-relative (Z=0 at foot level), so no offset is needed. - """ - return 0.0 - - -# A disk emitted for body N is treated as a phantom when ANOTHER body M sits -# inside the disk's xy circle for at least PHANTOM_TARGET_IN_DISK_FRAC of -# frames AND within PHANTOM_Z_OVERLAP_M of the disk z — body N is resting on -# body M rather than on a real support surface. -PHANTOM_TARGET_IN_DISK_FRAC = 0.90 -PHANTOM_Z_OVERLAP_M = 0.10 - - -def _filter_phantom_supports( - all_disks: dict[str, list[tuple[float, float, float, float]]], - object_body_names: list[str], - object_body_position: np.ndarray, -) -> dict[str, list[tuple[float, float, float, float]]]: - """Drop support disks that sit on another body's trajectory. - - A disk gets emitted wherever a body is still for long enough. For - multi-body scenes (a tool resting on a target — brush on helmet, cup - tipped into bowl) the tool body is also "still" for most of the motion, - producing a phantom support that physically intersects the target object - in sim. The cross-body trajectory check here catches that case without - needing a naming convention like a `tool_` prefix. - """ - if object_body_position.ndim != 3 or object_body_position.shape[1] < 2: - return all_disks # Single-body scene; nothing to cross-check. - - name_to_idx = {name: i for i, name in enumerate(object_body_names)} - filtered: dict[str, list[tuple[float, float, float, float]]] = {} - for body_name, disks in all_disks.items(): - my_idx = name_to_idx.get(body_name) - if my_idx is None: - filtered[body_name] = disks - continue - kept: list[tuple[float, float, float, float]] = [] - for disk in disks: - cx, cy, z, r = disk - phantom = False - for other_idx in range(object_body_position.shape[1]): - if other_idx == my_idx: - continue - other_xyz = object_body_position[:, other_idx, :] - d_xy = np.linalg.norm(other_xyz[:, :2] - np.array([cx, cy]), axis=1) - dz = np.abs(other_xyz[:, 2] - z) - frac = float(((d_xy < r) & (dz < PHANTOM_Z_OVERLAP_M)).mean()) - if frac >= PHANTOM_TARGET_IN_DISK_FRAC: - print( - f" filtered phantom support on '{body_name}' " - f"(center=({cx:.3f},{cy:.3f},{z:.3f}), r={r:.3f}) — " - f"body '{object_body_names[other_idx]}' sits in xy " - f"radius for {frac:.0%} of frames within " - f"{PHANTOM_Z_OVERLAP_M} m of disk z" - ) - phantom = True - break - if not phantom: - kept.append(disk) - if kept: - filtered[body_name] = kept - return filtered - - -def reconstruct_support_for_sequence( - input_dir: Path, - sequence_id: str, - output_override: str | None = None, - schema: str | None = None, - ground_z_threshold: float = GROUND_Z_THRESHOLD, -) -> None: - """Detect still frames, compute support disks, and write a USD file. - - Args: - input_dir: Parquet root containing ``sequence_id=<...>`` partitions. - sequence_id: Sequence identifier to process. - output_override: Optional explicit path for the .usda output; - otherwise written to - ``/reconstructed_stage/_support.usda``. - schema: Force a schema (``motion_v1`` or ``mano_sharpa``); auto-detected - from parquet columns when ``None``. - ground_z_threshold: Disks with z <= this are skipped (the sim ground - plane already covers them). - """ - if schema is None: - schema = _detect_parquet_schema(input_dir) - data, object_meshes = load_object_mesh_and_poses( - input_dir, sequence_id, schema=schema - ) - - height_offset = _compute_height_offset(data, schema) - if abs(height_offset) > 1e-6: - print(f" Applying height offset: {height_offset:.4f}m (schema={schema})") - - object_body_names = getattr(data, "object_body_names", None) or [] - num_frames = len(getattr(data, "object_body_position", [])) - print(f"\nSequence: {sequence_id}") - print(f"Frames: {num_frames}") - print(f"Object bodies: {object_body_names}") - for name in object_body_names: - mesh = object_meshes.get(name) - n_verts = mesh.vertices.shape[0] if mesh is not None else 0 - print(f" {name}: mesh vertices={n_verts}") - - if num_frames == 0 or not object_body_names: - print("No object trajectory data — skipping.") - return - - # For articulated objects, only reconstruct support for the root body - # (index 0). Child bodies are connected via joints and don't rest on - # surfaces. - articulation = getattr(data, "object_articulation", None) - is_articulated = articulation is not None and np.any(np.array(articulation) != 0.0) - - all_disks: dict[str, list[tuple[float, float, float, float]]] = {} - - for body_idx, body_name in enumerate(object_body_names): - if is_articulated and body_idx > 0: - print(f" Skipping {body_name}: child body of articulated object") - continue - mesh = object_meshes.get(body_name) - if mesh is None: - print(f" Skipping {body_name}: no mesh loaded") - continue - - still_frames = still_frames_from_object_data(data, body_index=body_idx) - segments = extract_continuous_segments(still_frames) - print( - f" {body_name}: {len(still_frames)} still frames, " - f"{len(segments)} segment(s)" - ) - - body_disks: list[tuple[float, float, float, float]] = [] - for seg in segments: - first_frame = int(seg[0]) - pos = np.asarray( - data.object_body_position[first_frame][body_idx], - dtype=np.float64, - ) - quat = np.asarray( - data.object_body_wxyz[first_frame][body_idx], - dtype=np.float64, - ) - verts_world = _transform_vertices_to_world(mesh.vertices, pos, quat) - disk = compute_support_disk(verts_world) - body_disks.append(disk) - - if body_disks: - merged = merge_overlapping_disks(body_disks) - if abs(height_offset) > 1e-6: - merged = [(cx, cy, z + height_offset, r) for cx, cy, z, r in merged] - above_ground = [d for d in merged if d[2] > ground_z_threshold] - on_ground = len(merged) - len(above_ground) - if on_ground: - print( - f" filtered {on_ground} disk(s) at ground level " - f"(z <= {ground_z_threshold:.3f}m)" - ) - print( - f" merged {len(body_disks)} disk(s) -> {len(merged)}, " - f"kept {len(above_ground)} above ground" - ) - if above_ground: - all_disks[body_name] = above_ground - - # Drop disks that sit on another body's trajectory (a tool resting on - # the target produces a phantom support inside the target body). - if all_disks: - body_pos = np.asarray( - getattr(data, "object_body_position", []), dtype=np.float64 - ) - all_disks = _filter_phantom_supports( - all_disks, list(object_body_names), body_pos - ) - - if not all_disks: - print("No support disks needed (object on ground or always held).") - return - - total = sum(len(v) for v in all_disks.values()) - default_output_dir = input_dir.parent / "reconstructed_stage" - default_output_dir.mkdir(parents=True, exist_ok=True) - output_path = output_override or str( - default_output_dir / f"{sequence_id}_support.usda" - ) - write_support_surfaces_usd(all_disks, output_path) - print(f"Wrote {total} support surface(s) to {output_path}") diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/utils.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/utils.py deleted file mode 100644 index f2d9a4ed..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/utils.py +++ /dev/null @@ -1,204 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch - - -@torch.jit.script -def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor: - """Returns torch.sqrt(torch.max(0, x)) but with a zero sub-gradient where x is 0. - - Reference: - https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L91-L99 - """ - ret = torch.zeros_like(x) - positive_mask = x > 0 - ret[positive_mask] = torch.sqrt(x[positive_mask]) - return ret - - -@torch.jit.script -def quat_conjugate(q: torch.Tensor) -> torch.Tensor: - """Computes the conjugate of a quaternion. - - Args: - q: The quaternion orientation in (w, x, y, z). Shape is (..., 4). - - Returns: - The conjugate quaternion in (w, x, y, z). Shape is (..., 4). - """ - shape = q.shape - q = q.reshape(-1, 4) - return torch.cat((q[..., 0:1], -q[..., 1:]), dim=-1).view(shape) - - -@torch.jit.script -def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor: - """Computes the inverse of a quaternion. - - Args: - q: The quaternion orientation in (w, x, y, z). Shape is (N, 4). - eps: A small value to avoid division by zero. Defaults to 1e-9. - - Returns: - The inverse quaternion in (w, x, y, z). Shape is (N, 4). - """ - return quat_conjugate(q) / q.pow(2).sum(dim=-1, keepdim=True).clamp(min=eps) - - -@torch.jit.script -def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor: - """Multiply two quaternions together. - - Args: - q1: The first quaternion in (w, x, y, z). Shape is (..., 4). - q2: The second quaternion in (w, x, y, z). Shape is (..., 4). - - Returns: - The product of the two quaternions in (w, x, y, z). Shape is (..., 4). - - Raises: - ValueError: Input shapes of ``q1`` and ``q2`` are not matching. - """ - # check input is correct - if q1.shape != q2.shape: - msg = f"Expected input quaternion shape mismatch: {q1.shape} != {q2.shape}." - raise ValueError(msg) - # reshape to (N, 4) for multiplication - shape = q1.shape - q1 = q1.reshape(-1, 4) - q2 = q2.reshape(-1, 4) - # extract components from quaternions - w1, x1, y1, z1 = q1[:, 0], q1[:, 1], q1[:, 2], q1[:, 3] - w2, x2, y2, z2 = q2[:, 0], q2[:, 1], q2[:, 2], q2[:, 3] - # perform multiplication - ww = (z1 + x1) * (x2 + y2) - yy = (w1 - y1) * (w2 + z2) - zz = (w1 + y1) * (w2 - z2) - xx = ww + yy + zz - qq = 0.5 * (xx + (z1 - x1) * (x2 - y2)) - w = qq - ww + (z1 - y1) * (y2 - z2) - x = qq - xx + (x1 + w1) * (x2 + w2) - y = qq - yy + (w1 - x1) * (y2 + z2) - z = qq - zz + (z1 + y1) * (w2 - x2) - - return torch.stack([w, x, y, z], dim=-1).view(shape) - - -@torch.jit.script -def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor: - """Apply a quaternion rotation to a vector. - - Args: - quat: The quaternion in (w, x, y, z). Shape is (..., 4). - vec: The vector in (x, y, z). Shape is (..., 3). - - Returns: - The rotated vector in (x, y, z). Shape is (..., 3). - """ - # store shape - shape = vec.shape - # reshape to (N, 3) for multiplication - quat = quat.reshape(-1, 4) - vec = vec.reshape(-1, 3) - # extract components from quaternions - xyz = quat[:, 1:] - t = xyz.cross(vec, dim=-1) * 2 - return (vec + quat[:, 0:1] * t + xyz.cross(t, dim=-1)).view(shape) - - -@torch.jit.script -def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: - """Convert rotations given as rotation matrices to quaternions. - - Args: - matrix: The rotation matrices. Shape is (..., 3, 3). - - Returns: - The quaternion in (w, x, y, z). Shape is (..., 4). - - Reference: - https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161 - """ - if matrix.size(-1) != 3 or matrix.size(-2) != 3: - raise ValueError(f"Invalid rotation matrix shape {matrix.shape}.") - - batch_dim = matrix.shape[:-2] - m00, m01, m02, m10, m11, m12, m20, m21, m22 = torch.unbind( - matrix.reshape(batch_dim + (9,)), dim=-1 - ) - - q_abs = _sqrt_positive_part( - torch.stack( - [ - 1.0 + m00 + m11 + m22, - 1.0 + m00 - m11 - m22, - 1.0 - m00 + m11 - m22, - 1.0 - m00 - m11 + m22, - ], - dim=-1, - ) - ) - - # we produce the desired quaternion multiplied by each of r, i, j, k - quat_by_rijk = torch.stack( - [ - # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. - torch.stack([q_abs[..., 0] ** 2, m21 - m12, m02 - m20, m10 - m01], dim=-1), - # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. - torch.stack([m21 - m12, q_abs[..., 1] ** 2, m10 + m01, m02 + m20], dim=-1), - # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. - torch.stack([m02 - m20, m10 + m01, q_abs[..., 2] ** 2, m12 + m21], dim=-1), - # pyre-fixme[58]: `**` is not supported for operand types `Tensor` and `int`. - torch.stack([m10 - m01, m20 + m02, m21 + m12, q_abs[..., 3] ** 2], dim=-1), - ], - dim=-2, - ) - - # We floor here at 0.1 but the exact level is not important; if q_abs is small, - # the candidate won't be picked. - flr = torch.tensor(0.1).to(dtype=q_abs.dtype, device=q_abs.device) - quat_candidates = quat_by_rijk / (2.0 * q_abs[..., None].max(flr)) - - # if not for numerical problems, quat_candidates[i] should be same (up to a sign), - # forall i; we pick the best-conditioned one (with the largest denominator) - return quat_candidates[ - torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, : - ].reshape(batch_dim + (4,)) - - -@torch.jit.script -def subtract_frame_transforms( - t01: torch.Tensor, - q01: torch.Tensor, - t02: torch.Tensor | None = None, - q02: torch.Tensor | None = None, -) -> tuple[torch.Tensor, torch.Tensor]: - r"""Subtract transformations between two reference frames into a stationary frame. - - It performs the following transformation operation: :math:`T_{12} = T_{01}^{-1} \times T_{02}`, - where :math:`T_{AB}` is the homogeneous transformation matrix from frame A to B. - - Args: - t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3). - q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). - t02: Position of frame 2 w.r.t. frame 0. Shape is (N, 3). - Defaults to None, in which case the position is assumed to be zero. - q02: Quaternion orientation of frame 2 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4). - Defaults to None, in which case the orientation is assumed to be identity. - - Returns: - A tuple containing the position and orientation of frame 2 w.r.t. frame 1. - Shape of the tensors are (N, 3) and (N, 4) respectively. - """ - # compute orientation - q10 = quat_inv(q01) - if q02 is not None: - q12 = quat_mul(q10, q02) - else: - q12 = q10 - # compute translation - if t02 is not None: - t12 = quat_apply(q10, t02 - t01) - else: - t12 = quat_apply(q10, -t01) - return t12, q12 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/viser_playback.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/viser_playback.py deleted file mode 100644 index 8f31aa36..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/viser_playback.py +++ /dev/null @@ -1,802 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Viser-based inspection tool for motion_v1 parquets and live retargeting. - -Two entry points share one scene graph: - -- ``ViserPlayback(motion_file=...)`` loads a parquet and exposes a Frame - slider + Play/Pause/FPS/Loop/Step controls. Used by - ``scripts/replay_viser.py``. -- ``ViserPlayback.for_live_retarget(server, pin_model, ...)`` attaches to an - already-created ``viser.ViserServer``, exposing only the per-frame draw - surface. Used by ``scripts/retarget/soma_to_g1.py`` to stream - in-progress frames. - -The shared scene primitives (object mesh + ``/object``/``/head``/``/root`` -frames + per-side contact spheres) live in private builders on the class so -both modes render the same handle paths. Parquet replay has no body mesh on -disk, so the body overlay is parquet-mode's one missing piece; live retarget -feeds it through ``LiveFrameState.body_vertices``. -""" - -from __future__ import annotations - -import logging -import time -from dataclasses import dataclass, field -from pathlib import Path -from typing import Any - -import numpy as np -import pinocchio as pin -import trimesh -import viser -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.motion_schema import MotionData, load_motion_data_parquet -from robotic_grounding.retarget.pinocchio_viser_visualizer import ViserVisualizer -from robotic_grounding.retarget.robot_config import load_robot_config -from robotic_grounding.retarget.whole_body_kinematics import ( - WholeBodyKinematics, -) - -# Silence the aborted-handshake tracebacks the `websockets` layer inside -# viser logs at INFO/WARNING when a browser tab reconnects mid-session or -# when a connection is dropped before the handshake finishes. They are -# cosmetic (viser reopens the connection and the scene re-syncs on the -# next heartbeat), but they look scary in the retarget/playback terminal. -# Bump each logger to ERROR so only genuine faults surface. -for _name in ("websockets", "websockets.server", "websockets.asyncio.server"): - logging.getLogger(_name).setLevel(logging.ERROR) - -# NOTE: We deliberately do NOT import from -# ``robotic_grounding.tasks.scene_utils.replay_data``: that package's -# ``__init__.py`` pulls in ``apply_scene_config`` which imports Isaac Lab, -# and Isaac Lab refuses to load without a running ``omni.physics``. Viser -# playback is a lightweight tool that must work in plain Python envs, so we -# normalize ``MotionData`` inline here. - -# Repo root used to resolve repo-relative mesh paths carried in the parquet. -# Mirrors soma_to_g1.py's REPO_ROOT so both agree on the anchor. -_REPO_ROOT = Path(__file__).resolve().parents[4] - - -# ============================================================================= -# Per-frame state for live (non-parquet) callers -# ============================================================================= - - -@dataclass -class LiveFrameState: - """Per-frame pose bundle for ``ViserPlayback.display(...)``. - - Every field is optional. Only the fields provided get written to the - scene; absent fields leave the corresponding handle untouched. This lets - retargeters grow their overlay incrementally without touching the - module. - """ - - # Pinocchio configuration vector. Must align with the model the playback - # was constructed against (free-flyer base + joints in model order). - q: np.ndarray | None = None - - # Object pose (world frame). - object_pos: np.ndarray | None = None - object_wxyz: np.ndarray | None = None - - # Anchor frames — useful during retargeting for head/root debug. - head_pos: np.ndarray | None = None - head_wxyz: np.ndarray | None = None - root_pos: np.ndarray | None = None - root_wxyz: np.ndarray | None = None - - # Per-side contact marker state. ``contact_wrists[i]`` is a length-3 - # xyz; ``contact_active[i]`` is a float in [0, 1] (threshold 0.5). The - # ordering follows ``hand_sides`` passed at construction time. - contact_wrists: list[np.ndarray] | None = None - contact_active: list[float] | None = None - - # Optional body mesh overlay. Vertices expected in world frame with the - # same ground alignment as the robot. Currently unused by the active - # SOMA retarget path; kept as a forward-compatible field. - body_vertices: np.ndarray | None = None - - # Optional IK-target frames for retargeter debug. Keys are frame names - # used to namespace the handle under ``/targets/``. - ik_target_poses: dict[str, tuple[np.ndarray, np.ndarray]] = field( - default_factory=dict - ) - - -# ============================================================================= -# Helpers -# ============================================================================= - - -def _to_np(value: Any) -> np.ndarray | None: - """Coerce torch tensors / sequences to ndarray; passthrough None.""" - if value is None: - return None - if isinstance(value, np.ndarray): - return value - if hasattr(value, "cpu"): - return value.cpu().numpy() - try: - return np.asarray(value) - except (TypeError, ValueError): - return None - - -def _axis_angle_to_wxyz(aa: np.ndarray) -> np.ndarray: - """Convert (T, 3) axis-angle rotations to (T, 4) wxyz quaternions.""" - return np.asarray( - [R.from_rotvec(v).as_quat(scalar_first=True) for v in aa], - dtype=np.float32, - ) - - -def _wxyz_to_xyzw(wxyz: np.ndarray) -> np.ndarray: - """Reorder a (4,) wxyz quaternion to (4,) xyzw for Pinocchio's q layout.""" - return np.asarray([wxyz[1], wxyz[2], wxyz[3], wxyz[0]], dtype=np.float64) - - -def _reorder_to_pinocchio( - parquet_joint_names: list[str], - pin_joint_names: list[str], -) -> np.ndarray | None: - """Index array mapping parquet joint order to Pinocchio's model order. - - Returns ``None`` when the orders already match, letting the caller skip - the permutation step. - """ - if list(parquet_joint_names) == list(pin_joint_names): - return None - name_to_idx = {n: i for i, n in enumerate(pin_joint_names)} - missing = [n for n in parquet_joint_names if n not in name_to_idx] - if missing: - raise ValueError( - f"Parquet joint(s) {missing!r} not present in Pinocchio model; " - f"model has {len(pin_joint_names)} joints." - ) - return np.array([name_to_idx[n] for n in parquet_joint_names], dtype=np.int64) - - -def _resolve_object_mesh_path(md: MotionData) -> str | None: - """Resolve the first object mesh path from ``MotionData`` to an absolute path. - - Parquets since the motion_v1 refactor store repo-relative paths; older - files may store absolute ones. We handle both. Returns ``None`` when no - mesh is listed. - """ - if not md.object_mesh_paths: - return None - raw = md.object_mesh_paths[0] - if not raw: - return None - p = Path(raw) - if not p.is_absolute(): - p = _REPO_ROOT / p - return str(p) - - -@dataclass -class _ParquetArrays: - """Normalized per-frame arrays plucked from a ``MotionData`` at load time. - - Replaces the ``replay_data._motion_v1_to_replay`` adapter so this module - stays free of the Isaac-Lab-dependent ``tasks.scene_utils`` package. - """ - - layout: str # "single_robot" | "dual_hand" - num_frames: int - fps: float - - # Single-robot fields (None when dual-hand). - robot_root_position: np.ndarray | None = None - robot_root_wxyz: np.ndarray | None = None - robot_joint_names: list[str] = field(default_factory=list) - robot_joint_positions: np.ndarray | None = None - - # Dual-hand fields (None when single-robot). - left_wrist_position: np.ndarray | None = None - left_wrist_wxyz: np.ndarray | None = None - right_wrist_position: np.ndarray | None = None - right_wrist_wxyz: np.ndarray | None = None - - # Object root trajectory (either layout). - object_root_position: np.ndarray | None = None - object_root_wxyz: np.ndarray | None = None - - -def _parse_motion_data(md: MotionData) -> _ParquetArrays: - """Normalize ``MotionData`` into ndarray slices used by parquet replay. - - Duplicates the minimal logic from - ``tasks.scene_utils.replay_data._motion_v1_to_replay`` but stops short - of importing that module (see file header for the rationale). - """ - root_pos = _to_np(md.robot_root_position) - root_wxyz = _to_np(md.robot_root_wxyz) - joint_pos = _to_np(md.robot_joint_positions) - - obj_root_pos = _to_np(md.object_root_position) - obj_root_aa = _to_np(md.object_root_axis_angle) - obj_root_wxyz: np.ndarray | None = None - if ( - obj_root_pos is not None - and obj_root_aa is not None - and obj_root_pos.ndim == 2 - and obj_root_pos.shape[1] == 3 - and obj_root_aa.ndim == 2 - and obj_root_aa.shape[1] == 3 - ): - obj_root_wxyz = _axis_angle_to_wxyz(obj_root_aa.astype(np.float64)) - obj_root_pos = obj_root_pos.astype(np.float32) - else: - obj_root_pos = None - - has_joints = ( - bool(md.robot_joint_names) - and joint_pos is not None - and joint_pos.size > 0 - and root_pos is not None - and root_wxyz is not None - ) - if has_joints: - assert root_pos is not None - assert root_wxyz is not None - assert joint_pos is not None - return _ParquetArrays( - layout="single_robot", - num_frames=int(root_pos.shape[0]), - fps=float(md.fps), - robot_root_position=root_pos.astype(np.float32), - robot_root_wxyz=root_wxyz.astype(np.float32), - robot_joint_names=list(md.robot_joint_names), - robot_joint_positions=joint_pos.astype(np.float32), - object_root_position=obj_root_pos, - object_root_wxyz=obj_root_wxyz, - ) - - # Dual-hand: derive wrist pose from ee_pose_w + ee_link_names. - ee_pose = _to_np(md.ee_pose_w) - if ee_pose is None or ee_pose.ndim != 3 or ee_pose.shape[1] < 2: - raise ValueError( - "motion_v1 file has no whole-body joint state and fewer than 2 EEs; " - "cannot build a viser playback trajectory." - ) - left_idx, right_idx = 0, 1 - for i, name in enumerate(md.ee_link_names or []): - lname = (name or "").lower() - if "left" in lname: - left_idx = i - elif "right" in lname: - right_idx = i - ee_pose_np = ee_pose.astype(np.float32) - return _ParquetArrays( - layout="dual_hand", - num_frames=int(ee_pose_np.shape[0]), - fps=float(md.fps), - left_wrist_position=ee_pose_np[:, left_idx, 0:3], - left_wrist_wxyz=ee_pose_np[:, left_idx, 3:7], - right_wrist_position=ee_pose_np[:, right_idx, 0:3], - right_wrist_wxyz=ee_pose_np[:, right_idx, 3:7], - object_root_position=obj_root_pos, - object_root_wxyz=obj_root_wxyz, - ) - - -# ============================================================================= -# Main class -# ============================================================================= - - -class ViserPlayback: - """Shared viser scene graph with two drivers: parquet trajectory + live stream. - - Use ``ViserPlayback(motion_file=...)`` for parquet replay; call ``run()`` - to enter the GUI-driven tick loop. Use - ``ViserPlayback.for_live_retarget(...)`` to reuse the scene primitives - inside a retargeter; drive with ``display(LiveFrameState(...))`` per - frame and skip ``run()``. - """ - - # ------------------------------------------------------------------ - # Construction - # ------------------------------------------------------------------ - - def __init__( - self, - motion_file: str, - port: int = 8080, - device: str = "cpu", - start_paused: bool = False, - start_frame: int = 0, - ) -> None: - """Open viser on ``port`` and pre-build the scene from the parquet. - - Args: - motion_file: Path to a motion_v1 parquet partition directory or file. - port: Viser HTTP port. Default 8080 matches the retargeter. - device: torch device passed to the motion reader. - start_paused: Open the viewer paused so the user can scrub. - start_frame: Initial frame shown after the scene is built. - """ - self._mode: str = "parquet" - self._server = viser.ViserServer(host="0.0.0.0", port=port) - - self._md: MotionData = load_motion_data_parquet(motion_file, device=device) - self._parquet: _ParquetArrays | None = _parse_motion_data(self._md) - self._num_frames: int = int(self._parquet.num_frames) - self._fps: float = float(self._parquet.fps) or 30.0 - - # Robot / wrist handles - self._pin_viz: ViserVisualizer | None = None - self._pin_model: pin.Model | None = None - self._pin_joint_names: list[str] = [] - self._reorder: np.ndarray | None = None - self._left_wrist_frame: Any = None - self._right_wrist_frame: Any = None - - # Static scene handles, built lazily below. - self._object_frame: Any = None - self._object_mesh: Any = None - self._head_frame: Any = None - self._root_frame: Any = None - self._contact_handles: dict[str, Any] = {} - self._ik_target_handles: dict[str, Any] = {} - self._body_mesh_handle: Any = None - - # Parquet-mode GUI handles, populated by ``_build_gui``. - self._gui_frame: Any = None - self._gui_play: Any = None - self._gui_fps: Any = None - self._gui_loop: Any = None - - # Parquet-mode: infer side list from hand_sides (fall back to both). - self._hand_sides: list[str] = list(self._md.hand_sides or ["left", "right"]) - - self._build_object_scene(_resolve_object_mesh_path(self._md)) - self._build_anchor_frames() - self._build_contact_markers(self._hand_sides) - - if self._parquet.layout == "single_robot": - self._build_pinocchio_visualizer_for_parquet() - else: - self._build_dual_hand_wrist_frames() - - self._build_gui( - start_paused=start_paused, - start_frame=max(0, min(int(start_frame), self._num_frames - 1)), - ) - - # Initial draw so the scene isn't blank until first play tick. - assert self._gui_frame is not None # just built by _build_gui - self.set_frame(int(self._gui_frame.value)) - - @classmethod - def for_live_retarget( - cls, - server: viser.ViserServer, - pin_model: pin.Model, - pin_visual_model: Any = None, - pin_collision_model: Any = None, - object_mesh_path: str | None = None, - hand_sides: tuple[str, ...] = ("left", "right"), - ) -> "ViserPlayback": - """Construct a no-parquet instance that shares the scene graph. - - The caller owns the ``viser.ViserServer`` and drives ``display(...)`` - each retargeted frame. No trajectory, no GUI slider, no tick loop. - - Args: - server: Externally-created viser server. - pin_model: Pinocchio kinodynamic model (free-flyer base expected). - pin_visual_model: Optional pinocchio visual geometry model. - pin_collision_model: Optional pinocchio collision geometry model. - object_mesh_path: Optional object mesh file; a frame is still - created even when the mesh is absent so ``display`` can - write the pose without a lazy-add. - hand_sides: Sides to pre-build contact marker handles for. - """ - self = cls.__new__(cls) - self._mode = "live" - self._server = server - - self._md = MotionData() # placeholder; live mode reads nothing from it - self._parquet = None - self._num_frames = 0 - self._fps = 0.0 - - self._pin_model = pin_model - self._pin_joint_names = [str(n) for n in pin_model.names] - self._pin_viz = ViserVisualizer( - viser_server=server, - model=pin_model, - visual_model=pin_visual_model, - collision_model=pin_collision_model, - ) - self._reorder = None - self._left_wrist_frame = None - self._right_wrist_frame = None - - self._object_frame = None - self._object_mesh = None - self._head_frame = None - self._root_frame = None - self._contact_handles = {} - self._ik_target_handles = {} - self._body_mesh_handle = None - self._hand_sides = list(hand_sides) - - self._gui_frame = None - self._gui_play = None - self._gui_fps = None - self._gui_loop = None - - self._build_object_scene(object_mesh_path) - self._build_anchor_frames() - self._build_contact_markers(self._hand_sides) - return self - - # ------------------------------------------------------------------ - # Private scene builders - # ------------------------------------------------------------------ - - def _build_object_scene(self, mesh_path: str | None) -> None: - """Create the ``/object`` frame and load the mesh when available.""" - self._object_frame = self._server.scene.add_frame( - "/object", - wxyz=(1, 0, 0, 0), - position=(0, 0, 0), - axes_length=0.018, - axes_radius=0.0008, - ) - if mesh_path is None: - return - p = Path(mesh_path) - if not p.is_file(): - print(f"[viser_playback] object mesh not found at {p}; skipping.") - return - mesh = trimesh.load(str(p)) - self._object_mesh = self._server.scene.add_mesh_trimesh( - "/object/mesh", mesh, position=(0, 0, 0), wxyz=(1, 0, 0, 0) - ) - - def _build_anchor_frames(self) -> None: - """Create ``/head`` and ``/root`` anchor frames (empty/origin-seeded).""" - self._head_frame = self._server.scene.add_frame( - "/head", - wxyz=(1, 0, 0, 0), - position=(0, 0, 0), - axes_length=0.1, - axes_radius=0.002, - ) - self._root_frame = self._server.scene.add_frame( - "/root", - wxyz=(1, 0, 0, 0), - position=(0, 0, 0), - axes_length=0.1, - axes_radius=0.002, - ) - - def _build_contact_markers(self, sides: list[str]) -> None: - """Add hidden per-side contact spheres under ``/contacts/{side}``.""" - colors = { - "left": np.array([230, 60, 60]), - "right": np.array([60, 200, 80]), - } - for side in sides: - handle = self._server.scene.add_icosphere( - name=f"/contacts/{side}", - position=np.array([0.0, 0.0, 0.0]), - radius=0.04, - color=colors.get(side, np.array([200, 200, 200])), - ) - handle.visible = False - self._contact_handles[side] = handle - - def _build_pinocchio_visualizer_for_parquet(self) -> None: - """Load the G1 whole-body Pinocchio model and wrap in ViserVisualizer. - - Only runs for single-robot parquets. Uses the canonical - ``WholeBodyKinematics`` (the same kinematics backend - ``soma_to_g1.py`` runs) loaded from the in-repo G1 config so - replay matches retarget. - """ - config = load_robot_config("g1") - kin = WholeBodyKinematics(config=config) - self._pin_model = kin.robot.model - self._pin_joint_names = [str(n) for n in self._pin_model.names] - self._pin_viz = ViserVisualizer( - viser_server=self._server, - model=kin.robot.model, - visual_model=kin.robot.visual_model, - collision_model=kin.robot.collision_model, - ) - assert self._parquet is not None and self._parquet.layout == "single_robot" - parquet_joint_names = list(self._parquet.robot_joint_names) - # Drop the first 2 entries in pin_joint_names (universe + free-flyer) - # so names align with the movable-joint portion of q. - movable_pin_joint_names = self._pin_joint_names[2:] - self._reorder = _reorder_to_pinocchio( - parquet_joint_names, movable_pin_joint_names - ) - - def _build_dual_hand_wrist_frames(self) -> None: - """Dual-hand replay: two wrist frames, no articulation in this pass.""" - self._left_wrist_frame = self._server.scene.add_frame( - "/left_wrist", - wxyz=(1, 0, 0, 0), - position=(0, 0, 0), - axes_length=0.08, - axes_radius=0.003, - ) - self._right_wrist_frame = self._server.scene.add_frame( - "/right_wrist", - wxyz=(1, 0, 0, 0), - position=(0, 0, 0), - axes_length=0.08, - axes_radius=0.003, - ) - - def _build_gui(self, start_paused: bool, start_frame: int) -> None: - """Playback folder: Frame slider + transport + FPS + Loop + Step + Reset.""" - with self._server.gui.add_folder("Playback"): - self._gui_frame = self._server.gui.add_slider( - "Frame", - min=0, - max=max(self._num_frames - 1, 0), - step=1, - initial_value=start_frame, - ) - self._gui_play = self._server.gui.add_checkbox( - "Play", initial_value=not start_paused - ) - fps_max = max(int(round(self._fps)), 120) - self._gui_fps = self._server.gui.add_slider( - "FPS", - min=1, - max=fps_max, - step=1, - initial_value=int(np.clip(round(self._fps), 1, fps_max)), - ) - self._gui_loop = self._server.gui.add_checkbox("Loop", initial_value=True) - step_back = self._server.gui.add_button("Step -1") - step_fwd = self._server.gui.add_button("Step +1") - reset = self._server.gui.add_button("Reset") - - @self._gui_frame.on_update - def _(_event: Any) -> None: - # Slider scrub: redraw immediately. Scrubbing while Play is on is - # allowed; the tick loop will advance from the new position on - # its next iteration. - self.set_frame(int(self._gui_frame.value)) - - @step_back.on_click - def _(_event: Any) -> None: - self._gui_frame.value = max(int(self._gui_frame.value) - 1, 0) - - @step_fwd.on_click - def _(_event: Any) -> None: - self._gui_frame.value = min( - int(self._gui_frame.value) + 1, self._num_frames - 1 - ) - - @reset.on_click - def _(_event: Any) -> None: - self._gui_frame.value = 0 - - # ------------------------------------------------------------------ - # Per-frame rendering - # ------------------------------------------------------------------ - - def set_frame(self, t: int) -> None: - """Parquet-mode draw: render frame ``t`` of the loaded trajectory.""" - if self._mode != "parquet": - raise RuntimeError( - "set_frame is parquet-mode only; use display(LiveFrameState) " - "with ViserPlayback.for_live_retarget." - ) - t = int(np.clip(t, 0, self._num_frames - 1)) - - assert self._parquet is not None - if self._parquet.layout == "single_robot": - self._draw_single_robot(t) - else: - self._draw_dual_hand(t) - - self._draw_parquet_object(t) - self._draw_parquet_contacts(t) - - def _draw_single_robot(self, t: int) -> None: - assert self._parquet is not None - assert self._pin_viz is not None - arr = self._parquet - assert arr.robot_root_position is not None - assert arr.robot_root_wxyz is not None - assert arr.robot_joint_positions is not None - root_pos = np.asarray(arr.robot_root_position[t], dtype=np.float64) - root_wxyz = np.asarray(arr.robot_root_wxyz[t], dtype=np.float64) - joints_parquet = np.asarray(arr.robot_joint_positions[t], dtype=np.float64) - if self._reorder is not None: - movable_count = len(self._pin_joint_names) - 2 - joints_pin = np.zeros(movable_count, dtype=np.float64) - joints_pin[self._reorder] = joints_parquet - else: - joints_pin = joints_parquet - q = np.concatenate([root_pos, _wxyz_to_xyzw(root_wxyz), joints_pin]) - self._pin_viz.display(q) - - def _draw_dual_hand(self, t: int) -> None: - assert self._parquet is not None - arr = self._parquet - if ( - self._left_wrist_frame is not None - and arr.left_wrist_position is not None - and arr.left_wrist_wxyz is not None - ): - self._left_wrist_frame.position = np.asarray( - arr.left_wrist_position[t], dtype=np.float64 - ) - self._left_wrist_frame.wxyz = np.asarray( - arr.left_wrist_wxyz[t], dtype=np.float64 - ) - if ( - self._right_wrist_frame is not None - and arr.right_wrist_position is not None - and arr.right_wrist_wxyz is not None - ): - self._right_wrist_frame.position = np.asarray( - arr.right_wrist_position[t], dtype=np.float64 - ) - self._right_wrist_frame.wxyz = np.asarray( - arr.right_wrist_wxyz[t], dtype=np.float64 - ) - - def _draw_parquet_object(self, t: int) -> None: - if self._parquet is None or self._object_frame is None: - return - pos = self._parquet.object_root_position - wxyz = self._parquet.object_root_wxyz - if pos is None or wxyz is None: - return - self._object_frame.position = np.asarray(pos[t], dtype=np.float64) - self._object_frame.wxyz = np.asarray(wxyz[t], dtype=np.float64) - - def _draw_parquet_contacts(self, t: int) -> None: - for side in self._hand_sides: - handle = self._contact_handles.get(side) - if handle is None: - continue - wrist = getattr(self._md, f"{side}_wrist_position", None) - active = getattr(self._md, f"{side}_hand_contact_active", None) - if wrist is None or active is None: - handle.visible = False - continue - tt = min(t, wrist.shape[0] - 1) - handle.position = np.asarray(wrist[tt].cpu().numpy(), dtype=np.float64) - handle.visible = float(active[tt]) > 0.5 - - def display( - self, - state: LiveFrameState, - body_model: Any | None = None, - ) -> None: - """Live-mode draw: apply whatever fields ``state`` provides. - - Args: - state: Per-frame pose bundle (all fields optional). - body_model: Optional source body model (e.g. a - ``retarget.read_soma.SOMA`` instance) used to draw the human - overlay from ``state.body_vertices``. It must expose - ``visualize(server, vertices=...)``. Passing it from the - caller avoids re-loading the body model here; the retargeter - already holds one. When omitted, the overlay is skipped. - """ - if state.q is not None and self._pin_viz is not None: - self._pin_viz.display(np.asarray(state.q, dtype=np.float64)) - - if state.object_pos is not None and self._object_frame is not None: - self._object_frame.position = np.asarray(state.object_pos, dtype=np.float64) - if state.object_wxyz is not None and self._object_frame is not None: - self._object_frame.wxyz = np.asarray(state.object_wxyz, dtype=np.float64) - - if state.head_pos is not None and self._head_frame is not None: - self._head_frame.position = np.asarray(state.head_pos, dtype=np.float64) - if state.head_wxyz is not None and self._head_frame is not None: - self._head_frame.wxyz = np.asarray(state.head_wxyz, dtype=np.float64) - - if state.root_pos is not None and self._root_frame is not None: - self._root_frame.position = np.asarray(state.root_pos, dtype=np.float64) - if state.root_wxyz is not None and self._root_frame is not None: - self._root_frame.wxyz = np.asarray(state.root_wxyz, dtype=np.float64) - - if state.contact_wrists is not None and state.contact_active is not None: - for side_idx, side in enumerate(self._hand_sides): - handle = self._contact_handles.get(side) - if handle is None or side_idx >= len(state.contact_wrists): - continue - wrist_xyz = state.contact_wrists[side_idx] - handle.position = np.asarray(wrist_xyz, dtype=np.float64) - active = ( - state.contact_active[side_idx] - if side_idx < len(state.contact_active) - else 0.0 - ) - handle.visible = float(active) > 0.5 - - if state.body_vertices is not None and body_model is not None: - body_model.visualize(self._server, vertices=state.body_vertices) - - if state.ik_target_poses: - self._update_ik_target_frames(state.ik_target_poses) - - def _update_ik_target_frames( - self, - poses: dict[str, tuple[np.ndarray, np.ndarray]], - ) -> None: - """Lazily create and update ``/targets/`` frames.""" - for name, (pos, wxyz) in poses.items(): - handle = self._ik_target_handles.get(name) - if handle is None: - handle = self._server.scene.add_frame( - f"/targets/{name}", - position=np.asarray(pos, dtype=np.float64), - wxyz=np.asarray(wxyz, dtype=np.float64), - axes_length=0.05, - axes_radius=0.003, - ) - self._ik_target_handles[name] = handle - else: - handle.position = np.asarray(pos, dtype=np.float64) - handle.wxyz = np.asarray(wxyz, dtype=np.float64) - - # ------------------------------------------------------------------ - # Parquet-mode tick loop - # ------------------------------------------------------------------ - - def run(self) -> None: - """Block on the playback tick loop until the process is killed. - - Advances the ``Frame`` slider in real time when ``Play`` is on. - Scrubbing the slider during playback seeks without pausing; hitting - the end stops (or wraps, with ``Loop`` checked). - """ - if self._mode != "parquet": - raise RuntimeError("run() is parquet-mode only.") - if self._num_frames <= 0: - return - assert self._gui_frame is not None - assert self._gui_play is not None - assert self._gui_fps is not None - assert self._gui_loop is not None - - last_wall = time.time() - try: - while True: - time.sleep(1.0 / 120.0) - if not self._gui_play.value: - last_wall = time.time() - continue - now = time.time() - dt = max(now - last_wall, 0.0) - last_wall = now - new_val = float(self._gui_frame.value) + dt * float(self._gui_fps.value) - if new_val >= self._num_frames: - if self._gui_loop.value: - new_val = new_val % self._num_frames - else: - new_val = float(self._num_frames - 1) - self._gui_play.value = False - # Assigning to the slider fires its on_update -> set_frame. - self._gui_frame.value = int(new_val) - except KeyboardInterrupt: - print("[viser_playback] exiting on Ctrl+C") - - # Helpful for callers that want to keep the server alive without using - # the tick loop (e.g. retargeters that close the process themselves). - @property - def server(self) -> viser.ViserServer: - """The underlying ``viser.ViserServer`` instance.""" - return self._server diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/whole_body_kinematics.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/whole_body_kinematics.py deleted file mode 100644 index be72a7dd..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/retarget/whole_body_kinematics.py +++ /dev/null @@ -1,551 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from typing import Any, Optional - -import numpy as np -import pink -import pinocchio as pin -import torch -import viser -from loop_rate_limiters import RateLimiter -from pink import solve_ik -from pink.limits import ConfigurationLimit, VelocityLimit -from pink.tasks import FrameTask, PostureTask, Task -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.retarget.params import ( - SOMA_JOINTS_ORDER, -) -from robotic_grounding.retarget.pinocchio_viser_visualizer import ViserVisualizer -from robotic_grounding.retarget.robot_config import RobotRetargetConfig - - -class WholeBodyKinematics: - """Whole-body IK driven by a :class:`RobotRetargetConfig`. - - SOMA -> robot core. All robot-specific data (URDF path, ik_map, - world axis swap, per-bone rotation corrections, per-bone translation - offsets, posture-regularization costs) comes from the config; the IK - runtime (:meth:`compute`, :meth:`set_frame_tasks_target`) consumes - that data without any further branching. - """ - - def __init__( - self, - config: RobotRetargetConfig, - *, - solver: str = "daqp", - max_iter: int = 200, - frequency: float = 200.0, - frame_tasks_converged_threshold: float = 1e-6, - ) -> None: - """Initialize from a fully materialized :class:`RobotRetargetConfig`. - - Args: - config: The robot retarget config bundle (URDF path, ik_map, - coordinate transforms, posture-task costs). - solver: IK solver to use. - max_iter: Maximum IK iterations per frame. - frequency: IK solve frequency. - frame_tasks_converged_threshold: Convergence threshold on - per-frame-task error delta (and on the QP velocity-norm - fallback used when posture tasks are active). - """ - # 1. Validate the source model exactly once. Every runtime - # method assumes SOMA from here on; there is no per-method - # branching on ``source_model``. - if config.source_model != "soma": - raise ValueError( - f"WholeBodyKinematics: source_model={config.source_model!r} " - f"is not supported (only 'soma' is implemented). Update the " - f"robot config before instantiating." - ) - - # 2. Config-derived transforms. Stored BEFORE - # ``setup_frame_tasks`` runs because - # ``get_target_to_source_mapping`` reads ``self._config.ik_map``. - self._config = config - self._R_source_to_robot = np.asarray(config.r_world, dtype=np.float64) - self._frame_corrections = { - frame_name: np.asarray(matrix, dtype=np.float64) - for frame_name, matrix in config.r_per_link.items() - } - self._frame_translations = { - frame_name: np.asarray(vec, dtype=np.float64) - for frame_name, vec in config.t_per_link.items() - } - - # 3. Base attributes from config + kwargs. - self.robot_asset_path = str(config.urdf_path) - self.package_dirs = [str(p) for p in config.package_dirs] - self.source_model = config.source_model - self.solver = solver - self.max_iter = max_iter - self.frequency = frequency - self.frame_tasks_converged_threshold = frame_tasks_converged_threshold - - # 4. Build robot and Pink configuration. - self.robot = self.load_robot_model() - - self.robot_joint_names = { - i: str(self.robot.model.names[i]) - for i in range(1, self.robot.model.njoints) - } - - self.robot_frame_names = { - i: str(self.robot.model.frames[i].name) - for i in range(len(self.robot.model.frames)) - } - - self.configuration_limits = [ - ConfigurationLimit(self.robot.model), - VelocityLimit(self.robot.model), - ] - - self.configuration = pink.Configuration( - self.robot.model, - self.robot.data, - self.robot.q0, - ) - - rate = RateLimiter(frequency=frequency, warn=False) - self.dt = rate.period - - # 5. IK mapping and frame tasks (read config-derived state). - self.source_joint_order = self.get_source_joint_order() - self.target_to_source = self.get_target_to_source_mapping() - self.frame_tasks = self.setup_frame_tasks() - - # 6. Posture-regularization tasks. Built only when their cost is - # strictly positive so the default-zero config path stays - # bit-equivalent to a frame-tasks-only IK (no extra task object, - # no extra QP block, ``_extra_tasks`` returns ``[]``). The - # ``q0`` task target never changes; the ``q_prev`` target is - # refreshed per frame from ``compute()``'s ``qpos`` argument. - pt = config.posture_task - self._posture_q0_task: Optional[PostureTask] = ( - self._build_posture_task(pt.q0_cost, pt.lm_damping, pt.gain) - if pt.q0_cost > 0.0 - else None - ) - self._posture_qprev_task: Optional[PostureTask] = ( - self._build_posture_task(pt.q_prev_cost, pt.lm_damping, pt.gain) - if pt.q_prev_cost > 0.0 - else None - ) - - @property - def config(self) -> RobotRetargetConfig: - """Return the underlying config bundle (read-only handle).""" - return self._config - - def load_robot_model(self) -> pin.RobotWrapper: - """Load the robot URDF with a free-flyer root joint.""" - return pin.RobotWrapper.BuildFromURDF( - filename=self.robot_asset_path, - package_dirs=self.package_dirs, - root_joint=pin.JointModelFreeFlyer(), - ) - - def get_source_joint_order(self) -> list[str]: - """Return the SOMA source joint order.""" - return SOMA_JOINTS_ORDER - - def get_target_to_source_mapping(self) -> dict[str, tuple[str, float, float]]: - """Build the IK frame->source mapping straight from the config. - - Returns: - Dictionary of ``{robot_frame: (source_joint, position_cost, - orientation_cost)}``. - """ - return { - frame_name: (entry.soma_joint, entry.position_cost, entry.orientation_cost) - for frame_name, entry in self._config.ik_map.items() - } - - def get_base_source_joint(self) -> str: - """Return the SOMA joint used as the scaling anchor.""" - return self._config.base_source_joint - - def transform_source_position(self, position: np.ndarray) -> np.ndarray: - """Transform a SOMA-world position into robot world (right-multiply by ``R.T``).""" - return position @ self._R_source_to_robot.T - - def transform_source_rotation(self, rotation: np.ndarray) -> np.ndarray: - """Similarity transform for body-local source rotations. - - Applies ``R_src_to_robot @ M @ R_src_to_robot.T`` so the - rotation remains expressed in the robot's basis while acting on - vectors from the body's local frame. Not used by the SOMA world - path (which goes through :meth:`transform_world_rotation`) but - retained because :meth:`set_frame_tasks_target` documents the - distinction. - """ - R_src = self._R_source_to_robot - return R_src @ rotation @ R_src.T - - def transform_world_rotation(self, rotation: np.ndarray) -> np.ndarray: - """Transform a SOMA-world rotation into robot world (left-multiply by ``R``).""" - return self._R_source_to_robot @ rotation - - def get_frame_rotation_correction(self, frame_name: str) -> np.ndarray: - """Return the per-link right-multiply correction for ``frame_name``. - - Falls back to identity for any frame the config does not - explicitly correct, so adding a position-only IK target is a - single JSON edit (no Python). - """ - correction = self._frame_corrections.get(frame_name) - if correction is None: - return np.eye(3) - return correction - - def get_frame_translation_offset(self, frame_name: str) -> np.ndarray: - """Return the per-link translation offset for ``frame_name``. - - Returned vector is expressed in the corrected robot-link local - frame and applied at runtime as ``target_pos += target_rot @ - t_offset`` (mirrors soma-retargeter's - ``wp.quat_rotate(q, offset_tx.p)`` term in - ``wp_compute_scaled_effectors``). Falls back to zeros for any - frame the config does not provide. - """ - offset = self._frame_translations.get(frame_name) - if offset is None: - return np.zeros(3, dtype=np.float64) - return offset - - def visualize( - self, - viser_server: viser.ViserServer, - qpos: np.ndarray, - ) -> None: - """Visualize the robot in viser.""" - if not hasattr(self, "robot_viser_model"): - self.robot_viser_model = ViserVisualizer( - viser_server=viser_server, - model=self.robot.model, - visual_model=self.robot.visual_model, - collision_model=self.robot.collision_model, - ) - self.robot_viser_model.display(qpos) - - def get_scale_anchor_position( - self, - source_joints: np.ndarray, - ) -> np.ndarray: - """Get the anchor position for scaling (lowest point in vertical axis). - - Scaling is anchored at the lowest joint (feet) so ground contact is - preserved. XY is centered on the base joint (hips). - - Args: - source_joints: Array of shape (num_joints, 3) with joint positions. - - Returns: - Anchor position of shape (3,) in robot coordinates. - """ - all_positions = np.array( - [ - self.transform_source_position(source_joints[i]) - for i in range(len(source_joints)) - ] - ) - lowest_idx = np.argmin(all_positions[:, 2]) - anchor = all_positions[lowest_idx].copy() - anchor[:2] = self.transform_source_position( - source_joints[self.source_joint_order.index(self.get_base_source_joint())] - )[:2] - return anchor - - def set_frame_tasks_target( - self, - source_joints: np.ndarray, - source_joints_wxyz: np.ndarray, - source_to_robot_scale: float = 1.0, - ) -> None: - """Set the target for the IK frame tasks. - - Scaling is applied relative to the lowest point (feet) so that - ground contact is preserved when scaling down. - - Args: - source_joints: Array of shape (num_joints, 3) with joint positions. - source_joints_wxyz: Array of shape (num_joints, 4) with joint orientations as wxyz quaternions. - source_to_robot_scale: Scale factor for positions relative to ground anchor. - """ - anchor_pos = self.get_scale_anchor_position(source_joints) - - for robot_frame_name, ( - source_joint_name, - _, - _, - ) in self.target_to_source.items(): - source_joint_idx = self.source_joint_order.index(source_joint_name) - target_pos = self.transform_source_position(source_joints[source_joint_idx]) - target_pos = anchor_pos + (target_pos - anchor_pos) * source_to_robot_scale - - target_wxyz = source_joints_wxyz[source_joint_idx] - target_rot = R.from_quat(target_wxyz, scalar_first=True).as_matrix() - # ``source_joints_wxyz`` carries world-frame joint orientations. - # World rotations come from SOMA FK and need a single - # left-multiply by ``R_src_to_robot`` to land in the robot - # world frame. The similarity-transform variant - # (``transform_source_rotation``) is intentionally NOT used - # here: when ``R_src_to_robot`` is not symmetric, the - # similarity form silently degrades alignment. - target_rot = self.transform_world_rotation(target_rot) - - frame_correction = self.get_frame_rotation_correction(robot_frame_name) - target_rot = target_rot @ frame_correction - - # Apply per-bone translation offset in the corrected effector - # frame, mirroring soma-retargeter's - # ``t = ... + wp.quat_rotate(q, offset_tx.p)``. ``target_rot`` - # at this point is already the final - # ``R_world @ source_rot @ correction`` that Pink consumes, - # so right-multiplying ``t_offset`` by it expresses the - # offset in robot world. Returns zeros for frames without a - # configured offset. - t_offset = self.get_frame_translation_offset(robot_frame_name) - if np.any(t_offset): - target_pos = target_pos + target_rot @ t_offset - - self.frame_tasks[robot_frame_name].transform_target_to_world.translation = ( - target_pos.copy() - ) - self.frame_tasks[robot_frame_name].transform_target_to_world.rotation = ( - target_rot.copy() - ) - - def _rebuild_configuration(self) -> None: - """Rebuild ``self.configuration`` and ``self.configuration_limits``. - - Works around an eigenpy attribute-cache flake on the IsaacSim-bundled - pinocchio build: the same ``pink.Configuration`` object, reused for - many ``solve_ik`` calls, sporadically mis-resolves ``self.model`` - mid-sequence. Building fresh objects (including the limits, which - hold the same ``Model`` reference) clears the per-instance cache. - - Preserves the current ``q`` so the IK warm-start is unaffected; the - frame-task targets are owned separately and also carry over. - """ - current_q = self.configuration.q.copy() - self.configuration = pink.Configuration( - self.robot.model, - self.robot.data, - current_q, - ) - self.configuration_limits = [ - ConfigurationLimit(self.robot.model), - VelocityLimit(self.robot.model), - ] - - def setup_frame_tasks(self) -> dict[str, FrameTask]: - """Setup the IK frame tasks.""" - frame_tasks = {} - for robot_frame_name, ( - _, - position_cost, - orientation_cost, - ) in self.target_to_source.items(): - frame_tasks[robot_frame_name] = FrameTask( - robot_frame_name, - position_cost=position_cost, - orientation_cost=orientation_cost, - lm_damping=1.0, - ) - frame_tasks[robot_frame_name].set_target_from_configuration( - self.configuration - ) - return frame_tasks - - def _build_posture_task( - self, cost: float, lm_damping: float, gain: float - ) -> PostureTask: - """Build a Pink ``PostureTask`` and seed its target at ``q0``. - - The "regularize toward q0" task keeps this initial target for - its lifetime; the "track previous q" task overwrites it on - every ``compute()`` call (see :meth:`_extra_tasks`). Seeding - both at q0 here avoids a separate "first call" branch in - :meth:`_extra_tasks`. - """ - task = PostureTask(cost=cost, lm_damping=lm_damping, gain=gain) - task.set_target(self.robot.q0.copy()) - return task - - def _extra_tasks(self, qpos: Optional[np.ndarray]) -> list[Task]: - """Return the configured posture tasks for this frame's solve. - - Refreshes the "track previous q" task's target from the - caller-provided warm-start ``qpos`` (the script passes the - previous frame's IK solution; for frame 0 ``qpos`` is ``q0`` so - the term collapses to a pull toward q0). Returns ``[]`` when - both posture costs are zero in the config, preserving the - legacy frame-tasks-only IK exactly. - """ - extras: list[Task] = [] - q0_task = self._posture_q0_task - if q0_task is not None: - extras.append(q0_task) - qprev_task = self._posture_qprev_task - if qprev_task is not None: - target = self.robot.q0.copy() if qpos is None else qpos.copy() - qprev_task.set_target(target) - extras.append(qprev_task) - return extras - - def compute( - self, - source_joints: torch.Tensor | np.ndarray, - source_joints_wxyz: torch.Tensor | np.ndarray, - source_to_robot_scale: float = 1.0, - qpos: Optional[np.ndarray] = None, - ) -> dict[str, Any]: - """Compute whole body IK. - - Args: - source_joints: Joint positions from source model, shape (num_joints, 3). - source_joints_wxyz: Joint orientations as wxyz quaternions, shape (num_joints, 4). - source_to_robot_scale: Scale factor for positions relative to ground anchor. - qpos: Initial joint configuration. Uses q0 if None. - - Returns: - Dictionary with keys: q, frame_pose, frame_task_errors, num_optimization_iterations. - """ - if isinstance(source_joints, torch.Tensor): - source_joints = source_joints.detach().cpu().numpy() - if isinstance(source_joints_wxyz, torch.Tensor): - source_joints_wxyz = source_joints_wxyz.detach().cpu().numpy() - - self.set_frame_tasks_target( - source_joints=source_joints, - source_joints_wxyz=source_joints_wxyz, - source_to_robot_scale=source_to_robot_scale, - ) - # Resolve config-driven extras (e.g. posture regularization) - # BEFORE seeding ``self.configuration.q`` from ``qpos`` so they - # can refresh per-frame targets using the caller's warm-start - # without depending on solver-side state. - extra_tasks = list(self._extra_tasks(qpos)) - tasks = list(self.frame_tasks.values()) + extra_tasks - has_extra_tasks = bool(extra_tasks) - - self.configuration.q = self.robot.q0.copy() if qpos is None else qpos.copy() - - frame_tasks_pos_error = { - task_name: float(np.inf) for task_name in self.frame_tasks - } - frame_tasks_converged = {task_name: False for task_name in self.frame_tasks} - - num_optimization_iterations = 0 - # Bound the number of times we'll rebuild `self.configuration` in - # response to the binding flake described below; 2 is enough in - # practice (seen once in a ~1200-frame sequence) and keeps us from - # masking an actual runaway bug. - max_rebuilds_this_frame = 2 - rebuilds_this_frame = 0 - for _ in range(self.max_iter): - # Defensive solve. Pink's ``Configuration.get_transform_frame_to_world`` - # resolves the frame id via ``self.model.getFrameId(frame)`` on a - # pinocchio ``Model``. On the IsaacSim-bundled pinocchio / eigenpy - # build, repeated use of the same ``Configuration`` object can - # cause eigenpy's attribute cache to mis-resolve ``self.model`` - # and surface as either ``TypeError: 'Model' object is not - # callable`` or ``AttributeError: 'Model' object has no attribute - # 'model'`` — random, mid-sequence, non-deterministic. See - # https://github.com/stack-of-tasks/eigenpy for similar reports. - # Rebuilding ``Configuration`` from scratch drops the offending - # per-instance attribute cache; we retry the current iteration - # once (per frame) before accepting the last good ``q``. - try: - vel = solve_ik( - configuration=self.configuration, - tasks=tasks, - dt=self.dt, - solver=self.solver, - safety_break=False, - limits=self.configuration_limits, - ) - except (AttributeError, TypeError) as exc: - if rebuilds_this_frame >= max_rebuilds_this_frame: - print( - f"[whole_body_kinematics] giving up on frame after " - f"{rebuilds_this_frame} Pink/eigenpy rebuilds: {exc}" - ) - break - rebuilds_this_frame += 1 - self._rebuild_configuration() - continue - self.configuration.integrate_inplace(vel, self.dt) - num_optimization_iterations += 1 - - # Convergence check. - # - # Even when ``solve_ik`` succeeded, the subsequent per-task - # ``compute_error`` path can still trip the same flake (it goes - # through the same ``get_transform_frame_to_world``). Use the - # solver's velocity norm as a fallback and as a secondary exit - # condition — equivalent equilibrium signal that avoids the - # flaky binding path entirely. - vel_norm = float(np.linalg.norm(vel)) - for task_name, task in self.frame_tasks.items(): - try: - err_vec = np.asarray(task.compute_error(self.configuration)) - pos_error = float(np.linalg.norm(err_vec[:3])) - except (AttributeError, TypeError): - pos_error = vel_norm - last_pos_error = frame_tasks_pos_error[task_name] - if ( - abs(pos_error - last_pos_error) - < self.frame_tasks_converged_threshold - ): - frame_tasks_converged[task_name] = True - frame_tasks_pos_error[task_name] = pos_error - - # When extra (e.g. posture) tasks are active, the - # frame-task plateau check is not safe as an exit - # condition: frame-task errors can be stationary while a - # posture task is still pulling the configuration. In that - # case fall back to the full-QP equilibrium signal - # (``vel_norm``), which sees every active task. With no - # extra tasks, behavior is bit-equivalent to before. - if not has_extra_tasks and all(frame_tasks_converged.values()): - break - if vel_norm < self.frame_tasks_converged_threshold: - break - - # Read each frame's world pose exactly ONCE. Access the pinocchio - # model through ``self.robot.model`` (the RobotWrapper's stable - # handle) rather than ``self.configuration.model`` to sidestep the - # eigenpy attribute-cache flake that can mis-resolve attributes on - # the ``Configuration`` object. Same data object throughout. - model = self.robot.model - data = self.configuration.data - try: - pin.forwardKinematics(model, data, self.configuration.q) - pin.updateFramePlacements(model, data) - except (AttributeError, TypeError) as exc: - print( - f"[whole_body_kinematics] forwardKinematics post-solve flake " - f"({exc}); rebuilding configuration and retrying once." - ) - self._rebuild_configuration() - data = self.configuration.data - pin.forwardKinematics(model, data, self.configuration.q) - pin.updateFramePlacements(model, data) - frame_pose = [] - for frame_id, _frame_name in self.robot_frame_names.items(): - oMf = data.oMf[frame_id] - pos = oMf.translation - wxyz = R.from_matrix(oMf.rotation).as_quat(scalar_first=True) - frame_pose.append(np.hstack([pos, wxyz]).tolist()) - frame_pose = np.asarray(frame_pose) - - return { - "q": self.configuration.q.copy(), - "frame_pose": frame_pose, - "frame_task_errors": [frame_tasks_pos_error[k] for k in self.frame_tasks], - "num_optimization_iterations": num_optimization_iterations, - } diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/__init__.py deleted file mode 100644 index 8f9a20b6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/__init__.py +++ /dev/null @@ -1,10 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Package containing task implementations for various robotic environments.""" - -from isaaclab_tasks.utils import import_packages - -# The blacklist is used to prevent importing configs from sub-packages -_BLACKLIST_PKGS = ["utils"] -# Import all configs in this package -import_packages(__name__, _BLACKLIST_PKGS) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/__init__.py deleted file mode 100644 index 8fb72703..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/__init__.py +++ /dev/null @@ -1,21 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Debug environments for interactive testing and visualization.""" - -import gymnasium as gym - -from . import agents, sharpa_debug_env_cfg - -################################################# -# Register Gym environments. -################################################# - -gym.register( - id="Sharpa-V2P-Debug-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_debug_env_cfg.SharpaDebugEnvCfg, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:DummyPpoRunnerCfg", - }, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/__init__.py deleted file mode 100644 index f5197d51..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Agent configurations for debug environments.""" - -from .rsl_rl_ppo_cfg import DummyPpoRunnerCfg - -__all__ = ["DummyPpoRunnerCfg"] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/rsl_rl_ppo_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/rsl_rl_ppo_cfg.py deleted file mode 100644 index 03640645..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/agents/rsl_rl_ppo_cfg.py +++ /dev/null @@ -1,49 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Dummy PPO runner configuration for debug environments. - -This configuration is not meant for actual training - it's just a placeholder -to satisfy the gym registry requirements for the debug environment. -""" - -from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import ( - RslRlOnPolicyRunnerCfg, - RslRlPpoActorCriticCfg, - RslRlPpoAlgorithmCfg, -) - - -@configclass -class DummyPpoRunnerCfg(RslRlOnPolicyRunnerCfg): - """Dummy PPO runner configuration for debug environments. - - This is a minimal configuration to satisfy the gym registry. - The debug environment uses GUI control, not RL training. - """ - - num_steps_per_env = 24 - max_iterations = 1 - save_interval = 1 - experiment_name = "debug" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[32, 32], - critic_hidden_dims=[32, 32], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.0, - num_learning_epochs=1, - num_mini_batches=1, - learning_rate=1.0e-3, - schedule="fixed", - gamma=0.99, - lam=0.95, - desired_kl=0.01, - max_grad_norm=1.0, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/__init__.py deleted file mode 100644 index 64609ede..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/__init__.py +++ /dev/null @@ -1,41 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MDP components for debug environments.""" - -from .joint_gui import JointGUIAction -from .joint_gui_cfg import JointGUIActionCfg -from .object_pose_gui_action import ( - ArticulatedObjectPoseGUIAction, - ObjectPoseGUIAction, - RigidObjectPoseGUIAction, -) -from .object_pose_gui_action_cfg import ( - ArticulatedObjectPoseGUIActionCfg, - ObjectPoseGUIActionCfg, - RigidObjectPoseGUIActionCfg, -) -from .reward_visualizer import RewardVisualizer -from .reward_visualizer_cfg import RewardVisualizerCfg -from .rewards import ( - contact_force, - contact_pos, -) - -__all__ = [ - "JointGUIAction", - "JointGUIActionCfg", - # Unified object pose GUI action - "ObjectPoseGUIAction", - "ObjectPoseGUIActionCfg", - # Backwards compatibility aliases - "ArticulatedObjectPoseGUIAction", - "ArticulatedObjectPoseGUIActionCfg", - "RigidObjectPoseGUIAction", - "RigidObjectPoseGUIActionCfg", - # Reward visualizer - "RewardVisualizer", - "RewardVisualizerCfg", - # Reward functions - "contact_force", - "contact_pos", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui.py deleted file mode 100644 index a0f7b136..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui.py +++ /dev/null @@ -1,597 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Joint GUI Action for interactive control of robot joints. - -Provides a DearPyGui window with separate collapsible sections for -position-controlled and velocity-controlled joints. The GUI runs in a -daemon thread so the physics simulation can continue on the main thread. -""" - -from __future__ import annotations - -import threading -from typing import TYPE_CHECKING, Any, Callable - -import torch -from isaaclab.envs.mdp.actions import JointPositionAction - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - - from .joint_gui_cfg import JointGUIActionCfg - - -class JointGUIAction(JointPositionAction): - """Interactive GUI action term for joint-level control. - - Control modes (position-controlled joints only): - - - ``"kinematic"``: Directly write joint state (bypasses PD controller, - immediate response). - - ``"pd_target"``: Set PD targets (requires actuator gains, physics-based - movement). - - Velocity-controlled joints are always driven via - ``set_joint_velocity_target`` regardless of the control mode. - - Usage: - Add this action term in the environment's action configuration instead - of a regular joint-position action. All environment instances receive - the same joint targets defined in the GUI. - """ - - cfg: JointGUIActionCfg - - # ------------------------------------------------------------------ - # Initialization - # ------------------------------------------------------------------ - - def __init__(self, cfg: JointGUIActionCfg, env: ManagerBasedEnv) -> None: - """Initialize the joint GUI action. - - Args: - cfg: Configuration for the action term. - env: The environment instance. - """ - super().__init__(cfg, env) - - # Use default_joint_pos (from config init_state) instead of current - # joint_pos — at __init__ time the sim may not have reset yet. - self._desired_pos = self._select_joints( - self._asset.data.default_joint_pos - ).clone() - - # -- PD gains (only relevant for pd_target mode) ------------------ - if cfg.control_mode == "pd_target" and cfg.show_gains_sliders: - num_all_joints = self._asset.num_joints - full_stiffness = torch.zeros( - self.num_envs, num_all_joints, device=self.device - ) - full_damping = torch.zeros( - self.num_envs, num_all_joints, device=self.device - ) - for actuator in self._asset.actuators.values(): - ids = self._asset.find_joints(actuator.joint_names)[0] - full_stiffness[:, ids] = actuator.stiffness - full_damping[:, ids] = actuator.damping - - self._desired_stiffness = self._select_joints(full_stiffness).clone() - self._desired_damping = self._select_joints(full_damping).clone() - self._default_stiffness = self._desired_stiffness.clone() - self._default_damping = self._desired_damping.clone() - else: - self._desired_stiffness = None - self._desired_damping = None - self._default_stiffness = None - self._default_damping = None - - # -- Velocity-controlled joints (optional) ------------------------ - vel_names = getattr(cfg, "velocity_joint_names", None) - if vel_names: - vel_ids, vel_names_resolved = self._asset.find_joints(vel_names) - self._velocity_joint_ids = torch.as_tensor( - vel_ids, device=self.device, dtype=torch.long - ) - self._velocity_names = list(vel_names_resolved) - num_vel = len(self._velocity_joint_ids) - self._desired_vel = torch.zeros(self.num_envs, num_vel, device=self.device) - self._vel_limits = self._asset.data.soft_joint_vel_limits[ - 0, self._velocity_joint_ids - ].cpu() - else: - self._velocity_joint_ids = None - self._velocity_names = [] - self._desired_vel = None - self._vel_limits = None - - # -- Position-only joint mask (exclude velocity joints) ----------- - self._pos_col_mask: list[int] | None = None - self._pos_joint_ids: list[int] | None = None - if self._velocity_joint_ids is not None: - vel_set = set(self._velocity_joint_ids.cpu().tolist()) - if isinstance(self._joint_ids, slice): - all_ids = list( - range( - self._joint_ids.start or 0, - ( - self._joint_ids.stop - if self._joint_ids.stop is not None - else self._asset.num_joints - ), - self._joint_ids.step or 1, - ) - ) - else: - all_ids = list(self._joint_ids) - self._pos_col_mask = [ - i for i, jid in enumerate(all_ids) if jid not in vel_set - ] - self._pos_joint_ids = [all_ids[i] for i in self._pos_col_mask] - - # -- Threading / GUI ---------------------------------------------- - self._lock = threading.Lock() - self._request_base_reset = False - self._gui_thread = threading.Thread( - target=self._launch_gui, name="JointGUI", daemon=True - ) - self._gui_thread.start() - - # ------------------------------------------------------------------ - # GUI - # ------------------------------------------------------------------ - - def _launch_gui(self) -> None: - """Create the DearPyGui window with position and velocity sections.""" - import dearpygui.dearpygui as dpg # noqa: PLC0415 - - dpg.create_context() - dpg.create_viewport(title="Debug Controller", width=600, height=1200) - - # Estimate window height - num_joints = len(self._joint_names) - row_height = 120 if self.cfg.show_gains_sliders else 50 - window_height = min(100 + num_joints * row_height, 1100) - - with dpg.window( - label="Joint Controller", - tag="joint_window", - width=580, - height=window_height, - pos=(10, 10), - ): - # -- Header --------------------------------------------------- - mode_text = { - "kinematic": "Mode: Kinematic (direct state write)", - "pd_target": "Mode: PD Target (actuator-based)", - } - dpg.add_text(mode_text.get(self.cfg.control_mode, "Unknown mode")) - dpg.add_text(f"Controlling {num_joints} joints") - dpg.add_separator() - - # Slider tag lists for programmatic updates - pos_slider_tags: list[int] = [] - stiffness_slider_tags: list[int] = [] - damping_slider_tags: list[int] = [] - effort_bar_tags: list[int] = [] - vel_slider_tags: list[int] = [] - - # Effort bar theme - with dpg.theme() as effort_bar_theme: - with dpg.theme_component(dpg.mvStemSeries): - dpg.add_theme_style( - dpg.mvPlotStyleVar_LineWeight, - 5, - category=dpg.mvThemeCat_Plots, - ) - - # -- Callbacks ------------------------------------------------- - - def _reset_joints_cb() -> None: - """Reset all joints to defaults and request base reset.""" - with self._lock: - default_pos = self._select_joints( - self._asset.data.default_joint_pos - ) - self._desired_pos[:] = default_pos.clone() - for i in range(len(self._joint_names)): - dpg.set_value( - pos_slider_tags[i], - float(self._desired_pos[0, i].cpu()), - ) - if ( - self._desired_stiffness is not None - and self._desired_damping is not None - and self._default_stiffness is not None - and self._default_damping is not None - ): - self._desired_stiffness[:] = self._default_stiffness.clone() - self._desired_damping[:] = self._default_damping.clone() - for i in range(len(self._joint_names)): - dpg.set_value( - stiffness_slider_tags[i], - float(self._desired_stiffness[0, i].cpu()), - ) - dpg.set_value( - damping_slider_tags[i], - float(self._desired_damping[0, i].cpu()), - ) - if self._desired_vel is not None: - self._desired_vel.zero_() - for tag in vel_slider_tags: - dpg.set_value(tag, 0.0) - self._request_base_reset = True - - def _randomize_joints_cb() -> None: - """Randomize position-controlled joints within limits.""" - with self._lock: - limits = self._select_joints( - self._asset.data.soft_joint_pos_limits[0].T - ).T.cpu() - low = limits[:, 0] - high = limits[:, 1] - random_pos = low + (high - low) * torch.rand_like(low) - self._desired_pos[:] = random_pos.unsqueeze(0) - for i in range(len(self._joint_names)): - dpg.set_value( - pos_slider_tags[i], - float(self._desired_pos[0, i].cpu()), - ) - - # -- Buttons --------------------------------------------------- - with dpg.group(horizontal=True): - dpg.add_button(label="Reset to Default", callback=_reset_joints_cb) - dpg.add_button(label="Randomize", callback=_randomize_joints_cb) - dpg.add_separator() - - # ============================================================= - # Velocity Control section - # ============================================================= - if ( - getattr(self.cfg, "velocity_joint_names", None) - and self._velocity_joint_ids is not None - ): - with dpg.collapsing_header(label="Velocity Control", default_open=True): - for local_id, joint_name in enumerate(self._velocity_names): - limit = float(self._vel_limits[local_id].item()) - if limit <= 0: - limit = 12.0 - - def _make_vel_cb( - idx: int, - ) -> Callable[[Any, float, Any], None]: - def _cb( - sender: Any, - app_data: float, - user_data: Any, # noqa: ARG001 - ) -> None: - with self._lock: - if self._desired_vel is not None: - self._desired_vel[:, idx] = float(app_data) - - return _cb - - tag = dpg.add_slider_float( - label=joint_name, - min_value=-limit, - max_value=limit, - default_value=0.0, - callback=_make_vel_cb(local_id), - format="%.2f", - width=400, - ) - vel_slider_tags.append(tag) - - # ============================================================= - # Position Control section - # ============================================================= - with dpg.collapsing_header(label="Position Control", default_open=True): - for local_id, joint_name in enumerate(self._joint_names): - joint_idx = ( - local_id - if isinstance(self._joint_ids, slice) - else self._joint_ids[local_id] - ) - - limits = self._asset.data.soft_joint_pos_limits[0, joint_idx].cpu() - low, high = float(limits[0]), float(limits[1]) - current_val = float(self._desired_pos[0, local_id].cpu()) - - def _make_pos_cb( - idx: int, - ) -> Callable[[Any, float, Any], None]: - def _cb( - sender: Any, - app_data: float, - user_data: Any, # noqa: ARG001 - ) -> None: - with self._lock: - self._desired_pos[:, idx] = float(app_data) - - return _cb - - pos_slider_tag = dpg.add_slider_float( - label=f"[{local_id}] {joint_name}", - min_value=low, - max_value=high, - default_value=current_val, - callback=_make_pos_cb(local_id), - format="%.3f", - width=400, - ) - pos_slider_tags.append(pos_slider_tag) - - # P/D gain sliders (pd_target mode only) - if ( - self.cfg.show_gains_sliders - and self._desired_stiffness is not None - and self._desired_damping is not None - ): - current_stiffness = float( - self._desired_stiffness[0, local_id].cpu() - ) - current_damping = float( - self._desired_damping[0, local_id].cpu() - ) - - def _make_stiffness_cb( - idx: int, - ) -> Callable[[Any, float, Any], None]: - def _cb( - sender: Any, - app_data: float, - user_data: Any, # noqa: ARG001 - ) -> None: - with self._lock: - if self._desired_stiffness is not None: - self._desired_stiffness[:, idx] = float( - app_data - ) - - return _cb - - def _make_damping_cb( - idx: int, - ) -> Callable[[Any, float, Any], None]: - def _cb( - sender: Any, - app_data: float, - user_data: Any, # noqa: ARG001 - ) -> None: - with self._lock: - if self._desired_damping is not None: - self._desired_damping[:, idx] = float(app_data) - - return _cb - - stiffness_tag = dpg.add_slider_float( - label="P-Gain", - min_value=0.0, - max_value=self.cfg.max_stiffness, - default_value=current_stiffness, - callback=_make_stiffness_cb(local_id), - format="%.1f", - indent=20, - width=380, - ) - stiffness_slider_tags.append(stiffness_tag) - - damping_tag = dpg.add_slider_float( - label="D-Gain", - min_value=0.0, - max_value=self.cfg.max_damping, - default_value=current_damping, - callback=_make_damping_cb(local_id), - format="%.1f", - indent=20, - width=380, - ) - damping_slider_tags.append(damping_tag) - - # Effort visualization - effort_limit = torch.cat( - [v.effort_limit for v in self._asset.actuators.values()], - dim=-1, - )[0, joint_idx].item() - with dpg.plot( - no_title=True, - no_menus=True, - no_box_select=True, - no_mouse_pos=True, - height=60, - width=100, - ): - dpg.add_plot_axis( - dpg.mvXAxis, - no_gridlines=True, - no_tick_marks=True, - no_tick_labels=True, - ) - dpg.set_axis_limits(dpg.last_item(), -1, 1) - with dpg.plot_axis( - dpg.mvYAxis, - no_gridlines=True, - no_tick_marks=True, - no_tick_labels=True, - ) as y_axis: - dpg.set_axis_limits(y_axis, -effort_limit, effort_limit) - bar_tag = dpg.add_stem_series([0.0], [0.0]) - dpg.bind_item_theme(bar_tag, effort_bar_theme) - effort_bar_tags.append(bar_tag) - - dpg.add_separator() - - # -- Event loop ---------------------------------------------------- - dpg.setup_dearpygui() - dpg.show_viewport() - - while dpg.is_dearpygui_running(): - if effort_bar_tags: - with self._lock: - applied_effort = self._select_joints( - self._asset.data.applied_torque - ).clone() - applied_effort_cpu = applied_effort.cpu() - for i in range(len(effort_bar_tags)): - effort_val = float(applied_effort_cpu[0, i]) - dpg.set_value(effort_bar_tags[i], [[0.0], [effort_val]]) - - dpg.render_dearpygui_frame() - - dpg.destroy_context() - - # ------------------------------------------------------------------ - # Overridden ActionTerm methods - # ------------------------------------------------------------------ - - def process_actions(self, actions: torch.Tensor) -> None: # noqa: ARG002 - """Ignore incoming policy actions; GUI values are authoritative.""" - return None - - def apply_actions(self) -> None: - """Send joint targets from GUI to the articulation.""" - # Handle pending base reset from the GUI thread - with self._lock: - do_base_reset = self._request_base_reset - if do_base_reset: - self._request_base_reset = False - if do_base_reset: - self._reset_robot_base_and_joints() - - with self._lock: - target_pos = self._desired_pos.clone() - if ( - self._desired_stiffness is not None - and self._desired_damping is not None - ): - target_stiffness = self._desired_stiffness.clone() - target_damping = self._desired_damping.clone() - else: - target_stiffness = None - target_damping = None - - target_pos = target_pos.to(device=self.device) - - # -- Velocity targets ---------------------------------------------- - if self._velocity_joint_ids is not None and self._desired_vel is not None: - with self._lock: - vel = self._desired_vel.clone().to(device=self.device) - self._asset.set_joint_velocity_target( - vel, joint_ids=self._velocity_joint_ids - ) - - # -- Position targets (exclude velocity-controlled joints) --------- - if self._pos_col_mask is not None: - pos_target = target_pos[:, self._pos_col_mask] - pos_ids = self._pos_joint_ids - else: - pos_target = target_pos - pos_ids = self._joint_ids - - if self.cfg.control_mode == "pd_target": - self._asset.set_joint_position_target(pos_target, joint_ids=pos_ids) - else: # kinematic - self._asset.write_joint_state_to_sim( - pos_target, - torch.zeros_like(pos_target), - joint_ids=pos_ids, - ) - - # -- PD gains (pd_target mode only) -------------------------------- - if ( - target_stiffness is not None - and target_damping is not None - and self.cfg.control_mode == "pd_target" - ): - target_stiffness = target_stiffness.to(device=self.device) - target_damping = target_damping.to(device=self.device) - - if isinstance(self._joint_ids, slice): - joint_ids_tensor = torch.arange( - self._joint_ids.start or 0, - self._joint_ids.stop, - self._joint_ids.step or 1, - device=self.device, - ) - else: - joint_ids_tensor = torch.tensor(self._joint_ids, device=self.device) - - full_stiffness = torch.cat( - [a.stiffness.clone() for a in self._asset.actuators.values()], - dim=1, - ) - full_damping = torch.cat( - [a.damping.clone() for a in self._asset.actuators.values()], - dim=1, - ) - - full_stiffness[:, joint_ids_tensor] = target_stiffness - full_damping[:, joint_ids_tensor] = target_damping - - offset = 0 - for actuator in self._asset.actuators.values(): - num_dof = actuator.stiffness.shape[1] - actuator.stiffness[:] = full_stiffness.narrow(1, offset, num_dof) - actuator.damping[:] = full_damping.narrow(1, offset, num_dof) - offset += num_dof - - # ------------------------------------------------------------------ - # Base + joint reset (for GUI "Reset to Default") - # ------------------------------------------------------------------ - - def _reset_robot_base_and_joints(self) -> None: - """Reset robot root pose/velocity and joints to config defaults.""" - env_ids = torch.arange(self.num_envs, device=self.device) - init_state = getattr(self._asset.cfg, "init_state", None) - if init_state is not None: - init_pos = getattr(init_state, "pos", (0.0, 0.0, 0.0)) - init_rot = getattr(init_state, "rot", (1.0, 0.0, 0.0, 0.0)) - root_pos = self._env.scene.env_origins.clone().to(device=self.device) - root_pos += torch.tensor(init_pos, dtype=root_pos.dtype, device=self.device) - root_quat = ( - torch.tensor(init_rot, dtype=root_pos.dtype, device=self.device) - .unsqueeze(0) - .expand(self.num_envs, 4) - ) - self._asset.write_root_pose_to_sim( - torch.cat([root_pos, root_quat], dim=-1), env_ids=env_ids - ) - self._asset.write_root_velocity_to_sim( - torch.zeros_like(self._asset.data.root_vel_w[env_ids]), - env_ids=env_ids, - ) - joint_pos = self._asset.data.default_joint_pos[env_ids].clone() - joint_vel = torch.zeros_like(self._asset.data.joint_vel[env_ids]) - self._asset.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) - - # ------------------------------------------------------------------ - # Misc helpers - # ------------------------------------------------------------------ - - def reset(self, env_ids: torch.Tensor | None = None) -> None: - """Reset the action term (called on environment reset).""" - super().reset(env_ids) - if env_ids is None or 0 in env_ids: - with self._lock: - self._desired_pos[...] = self._select_joints( - self._asset.data.default_joint_pos - ).clone() - if ( - self._desired_stiffness is not None - and self._desired_damping is not None - and self._default_stiffness is not None - and self._default_damping is not None - ): - self._desired_stiffness[...] = self._default_stiffness.clone() - self._desired_damping[...] = self._default_damping.clone() - - def _select_joints(self, tensor: torch.Tensor) -> torch.Tensor: - """Select columns for *self._joint_ids* from *tensor*.""" - if isinstance(self._joint_ids, slice): - slicer = [slice(None)] * tensor.ndim - slicer[1] = self._joint_ids - return tensor[tuple(slicer)] - - index_tensor = torch.as_tensor( - self._joint_ids, dtype=torch.long, device=tensor.device - ) - return tensor.index_select(1, index_tensor) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui_cfg.py deleted file mode 100644 index 09a80e74..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/joint_gui_cfg.py +++ /dev/null @@ -1,81 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Configuration for the Joint GUI Action term.""" - -from __future__ import annotations - -from dataclasses import field -from typing import Literal - -from isaaclab.envs import mdp -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils import configclass - - -@configclass -class JointGUIActionCfg(mdp.JointActionCfg): - """Configuration for the joint GUI action term. - - This action term provides interactive control of robot joints via a DearPyGui - window. Position-controlled joints are adjusted with position sliders, while - velocity-controlled joints (e.g. virtual base DOFs) get separate velocity - sliders. A separate GUI thread is spawned so that the physics simulation can - continue to run on the main thread. - - Control modes (for position-controlled joints): - - - ``"kinematic"``: Directly write joint state to simulation (bypasses the PD - controller). Joints move instantly—best for debugging and manual posing. - - ``"pd_target"``: Set joint position targets for the implicit PD controller. - Joints move according to actuator stiffness/damping. - """ - - class_type: type[ActionTerm] = field( - default_factory=lambda: _get_joint_gui_action_class() - ) - """The class type for this action term.""" - - use_default_offset: bool = True - """Whether to use default joint positions configured in the articulation asset - as offset. Defaults to ``True``. - - When enabled, the values of :attr:`offset` are overwritten with the default - joint positions from the articulation asset. - """ - - control_mode: Literal["kinematic", "pd_target"] = "kinematic" - """How to apply joint positions. Defaults to ``"kinematic"``. - - - ``"kinematic"``: Directly write joint state to simulation (bypasses - physics/PD controller). Best for debugging and manual positioning. - - ``"pd_target"``: Set joint position targets for the implicit PD controller. - Joint movement depends on actuator stiffness/damping. - """ - - show_gains_sliders: bool = False - """Whether to show P/D gain sliders in the GUI. Defaults to ``False``. - - Only relevant when ``control_mode`` is ``"pd_target"``. When enabled, users - can adjust stiffness (P-gain) and damping (D-gain) per joint. - """ - - max_stiffness: float = 200.0 - """Maximum stiffness for the P-gain slider. Defaults to ``200.0``.""" - - max_damping: float = 25.0 - """Maximum damping for the D-gain slider. Defaults to ``25.0``.""" - - velocity_joint_names: list[str] | None = None - """Optional list of joint names controlled via velocity sliders. - - When set, the listed joints are shown in a dedicated *Velocity Control* - section of the GUI. Velocity targets are applied via - ``set_joint_velocity_target``; all other joints still use position control. - """ - - -def _get_joint_gui_action_class() -> type[ActionTerm]: - """Lazy import to avoid circular dependency.""" - from .joint_gui import JointGUIAction # noqa: PLC0415 - - return JointGUIAction diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action.py deleted file mode 100644 index 1f2e9271..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action.py +++ /dev/null @@ -1,571 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unified Object Pose GUI Action for interactive control of object base pose.""" - -from __future__ import annotations - -import threading -import time -from typing import TYPE_CHECKING - -import torch -from isaaclab.assets import Articulation -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.math import euler_xyz_from_quat, quat_from_euler_xyz - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - - from .object_pose_gui_action_cfg import ObjectPoseGUIActionCfg - - -class ObjectPoseGUIAction(ActionTerm): - """Unified object pose GUI action. - - This action term allows controlling an object's 6DoF base pose interactively - via a DearPyGui window. It supports: - - 1. **RigidObject**: Direct pose via `write_root_link_pose_to_sim()` - 2. **Articulation with floating base joints**: Pose via floating base joints - 3. **Articulation without floating base joints**: Direct root pose write - - A separate thread is spawned for the GUI so that the physics simulation can continue - to run in the main thread. - - The pose is specified as: - - Position: x, y, z in meters (world frame) - - Orientation: roll, pitch, yaw in radians (Euler XYZ convention) - - Usage: - Add this action term in the environment's action configuration. The GUI - values are authoritative - any RL actions sent to this term are ignored. - """ - - cfg: ObjectPoseGUIActionCfg - - # --------------------------------------------------------------------- - # Initialization - # --------------------------------------------------------------------- - - def __init__(self, cfg: ObjectPoseGUIActionCfg, env: ManagerBasedEnv) -> None: - """Initialize the object pose GUI action. - - Args: - cfg: Configuration for the action term. - env: The environment instance. - """ - # Store config and environment - self.cfg = cfg - self._env = env - self._device = env.device - self._num_envs = env.num_envs - - # Get the object from the scene - self._object = env.scene[cfg.asset_name] - - # Determine object type and control mode - self._is_articulated = isinstance(self._object, Articulation) - self._use_floating_base_joints = ( - self._is_articulated and cfg.position_joint_names is not None - ) - - # Initialize joint ID mappings (None when not using floating base joints) - self._pos_joint_ids: dict[str, int] | None = None - self._rot_joint_ids: dict[str, int] | None = None - - # Validate configuration and setup floating base joints if configured - if self._use_floating_base_joints: - if cfg.rotation_joint_names is None: - raise ValueError( - "rotation_joint_names must be provided when position_joint_names is set" - ) - self._setup_floating_base_joints() - - # Initialize desired pose - self._init_pose_from_object() - - # Store initial/default pose for reset - self._default_pos = self._desired_pos.clone() - self._default_euler = self._desired_euler.clone() - - # Thread-safe lock for accessing pose from GUI - self._lock = threading.Lock() - - # Launch GUI in a daemon thread - self._gui_thread = threading.Thread( - target=self._launch_gui, - name=f"ObjectPoseGUI_{cfg.asset_name}", - daemon=True, - ) - self._gui_thread.start() - - def _setup_floating_base_joints(self) -> None: - """Resolve joint indices for floating base joints.""" - cfg = self.cfg - # Type narrowing - these are guaranteed non-None when this method is called - assert cfg.position_joint_names is not None - assert cfg.rotation_joint_names is not None - - pos_joint_ids: dict[str, int] = {} - rot_joint_ids: dict[str, int] = {} - - for axis, joint_name in cfg.position_joint_names.items(): - joint_ids, _ = self._object.find_joints(joint_name) - if len(joint_ids) > 0: - pos_joint_ids[axis] = joint_ids[0] - else: - raise ValueError( - f"Joint '{joint_name}' for position axis '{axis}' not found " - f"in articulation '{cfg.asset_name}'" - ) - - for axis, joint_name in cfg.rotation_joint_names.items(): - joint_ids, _ = self._object.find_joints(joint_name) - if len(joint_ids) > 0: - rot_joint_ids[axis] = joint_ids[0] - else: - raise ValueError( - f"Joint '{joint_name}' for rotation axis '{axis}' not found " - f"in articulation '{cfg.asset_name}'" - ) - - self._pos_joint_ids = pos_joint_ids - self._rot_joint_ids = rot_joint_ids - - def _init_pose_from_object(self) -> None: - """Initialize desired pose from object's current/default state.""" - if self._use_floating_base_joints: - # Type narrowing - these are guaranteed non-None when using floating base - assert self._pos_joint_ids is not None - assert self._rot_joint_ids is not None - # Initialize from default joint positions - joint_pos = self._object.data.default_joint_pos[0].cpu() - self._desired_pos = torch.tensor( - [ - joint_pos[self._pos_joint_ids["x"]].item(), - joint_pos[self._pos_joint_ids["y"]].item(), - joint_pos[self._pos_joint_ids["z"]].item(), - ] - ) - self._desired_euler = torch.tensor( - [ - joint_pos[self._rot_joint_ids["roll"]].item(), - joint_pos[self._rot_joint_ids["pitch"]].item(), - joint_pos[self._rot_joint_ids["yaw"]].item(), - ] - ) - else: - # Initialize from root link pose (works for both Rigid and Articulation) - current_pos = self._object.data.root_link_pos_w[0].clone() - current_quat = self._object.data.root_link_quat_w[0].clone() - - # Convert quaternion to Euler angles for GUI - roll, pitch, yaw = euler_xyz_from_quat(current_quat.unsqueeze(0)) - - self._desired_pos = current_pos.cpu() - self._desired_euler = torch.tensor([roll.item(), pitch.item(), yaw.item()]) - - # --------------------------------------------------------------------- - # Properties - # --------------------------------------------------------------------- - - @property - def action_dim(self) -> int: - """Dimension of the action term (not used for GUI action).""" - return 0 # No RL action dimension - GUI is authoritative - - @property - def device(self) -> str: - """Device for tensors.""" - return str(self._device) - - @property - def num_envs(self) -> int: - """Number of environments.""" - return int(self._num_envs) - - @property - def raw_actions(self) -> torch.Tensor: - """The input/raw actions sent to the term (unused for GUI).""" - return torch.empty(0, device=self._device) - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions (returns current desired pose).""" - with self._lock: - pos = self._desired_pos.clone() - euler = self._desired_euler.clone() - return torch.cat([pos, euler]) - - # --------------------------------------------------------------------- - # GUI Implementation - # --------------------------------------------------------------------- - - def _launch_gui(self) -> None: - """Create the DearPyGui window with pose control sliders.""" - import dearpygui.dearpygui as dpg # noqa: PLC0415 - - # Wait a bit to let joint GUI initialize first if it exists - time.sleep(0.5) - - # Check if DearPyGui context already exists (from joint GUI) - try: - context_exists = dpg.is_dearpygui_running() - except Exception: - context_exists = False - - # Only create context if it doesn't exist - owns_context = False - if not context_exists: - try: - dpg.create_context() - dpg.create_viewport( - title="Debug Controller", - width=600, - height=800, - ) - owns_context = True - except Exception: - # Context might have been created by another thread in the meantime - pass - - # Get limits from config - pos_limits = self.cfg.position_limits - rot_limits = self.cfg.rotation_limits - - # Store slider tags for programmatic updates - pos_slider_tags: dict[str, int] = {} - euler_slider_tags: dict[str, int] = {} - - # Use unique window tag to avoid conflicts - window_tag = f"object_pose_window_{self.cfg.asset_name}" - - # Determine control description for GUI - if self._use_floating_base_joints: - control_desc = "(floating base joints)" - elif self._is_articulated: - control_desc = "(direct root pose)" - else: - control_desc = "(rigid object)" - - with dpg.window( - label=self.cfg.gui_window_title, - tag=window_tag, - width=480, - height=350, - pos=(10, 450), - ): - dpg.add_text(f"Control pose of '{self.cfg.asset_name}' {control_desc}") - dpg.add_separator() - - # --- Buttons --- - def _reset_pose_cb() -> None: - """Reset object to default pose.""" - with self._lock: - self._desired_pos[:] = self._default_pos.clone() - self._desired_euler[:] = self._default_euler.clone() - # Update GUI sliders - for i, axis in enumerate(["x", "y", "z"]): - dpg.set_value( - pos_slider_tags[axis], float(self._desired_pos[i]) - ) - for i, axis in enumerate(["roll", "pitch", "yaw"]): - dpg.set_value( - euler_slider_tags[axis], float(self._desired_euler[i]) - ) - - def _randomize_pose_cb() -> None: - """Randomize object pose within limits.""" - with self._lock: - # Random position - for i, axis in enumerate(["x", "y", "z"]): - low, high = pos_limits[axis] - self._desired_pos[i] = low + (high - low) * torch.rand(1).item() - dpg.set_value( - pos_slider_tags[axis], float(self._desired_pos[i]) - ) - # Random orientation - for i, axis in enumerate(["roll", "pitch", "yaw"]): - low, high = rot_limits[axis] - self._desired_euler[i] = ( - low + (high - low) * torch.rand(1).item() - ) - dpg.set_value( - euler_slider_tags[axis], float(self._desired_euler[i]) - ) - - with dpg.group(horizontal=True): - dpg.add_button(label="Reset to Default", callback=_reset_pose_cb) - dpg.add_button(label="Randomize Pose", callback=_randomize_pose_cb) - - dpg.add_separator() - - # --- Position Sliders --- - dpg.add_text("POSITION (meters)") - - for i, axis in enumerate(["x", "y", "z"]): - low, high = pos_limits[axis] - current_val = float(self._desired_pos[i]) - - def _pos_slider_cb( - sender: int, app_data: float, user_data: int # noqa: ARG001 - ) -> None: - idx = user_data - with self._lock: - self._desired_pos[idx] = float(app_data) - - slider_tag = dpg.add_slider_float( - label=f"{axis.upper()}", - min_value=low, - max_value=high, - default_value=current_val, - callback=_pos_slider_cb, - user_data=i, - format="%.3f m", - width=350, - ) - pos_slider_tags[axis] = slider_tag - - dpg.add_separator() - - # --- Orientation Sliders --- - dpg.add_text("ORIENTATION (Euler XYZ, radians)") - - for i, axis in enumerate(["roll", "pitch", "yaw"]): - low, high = rot_limits[axis] - current_val = float(self._desired_euler[i]) - - def _euler_slider_cb( - sender: int, app_data: float, user_data: int # noqa: ARG001 - ) -> None: - idx = user_data - with self._lock: - self._desired_euler[idx] = float(app_data) - - slider_tag = dpg.add_slider_float( - label=f"{axis.capitalize()}", - min_value=low, - max_value=high, - default_value=current_val, - callback=_euler_slider_cb, - user_data=i, - format="%.3f rad", - width=350, - ) - euler_slider_tags[axis] = slider_tag - - dpg.add_separator() - - # --- Current Pose Display (read-only) --- - dpg.add_text("CURRENT POSE (read-only)") - pos_text_tag = dpg.add_text("Pos: [0.000, 0.000, 0.000]") - rot_text_tag = dpg.add_text("Rot: [0.000, 0.000, 0.000]") - - # Only run our own event loop if we own the context - if owns_context: - # Setup and show - dpg.setup_dearpygui() - dpg.show_viewport() - - # Main GUI loop - while dpg.is_dearpygui_running(): - self._update_pose_display(pos_text_tag, rot_text_tag) - dpg.render_dearpygui_frame() - - dpg.destroy_context() - else: - # Another GUI owns the context - just keep updating our display in a loop - while True: - try: - if not dpg.is_dearpygui_running(): - break - self._update_pose_display(pos_text_tag, rot_text_tag) - time.sleep(0.05) # Small delay to avoid busy loop - except Exception: - time.sleep(0.1) # Wait if not ready - - def _update_pose_display(self, pos_text_tag: int, rot_text_tag: int) -> None: - """Update the current pose display in the GUI.""" - import dearpygui.dearpygui as dpg # noqa: PLC0415 - - try: - if self._use_floating_base_joints: - # Type narrowing - these are guaranteed non-None when using floating base - assert self._pos_joint_ids is not None - assert self._rot_joint_ids is not None - # Read from joint positions - joint_pos = self._object.data.joint_pos[0].cpu() - current_pos = [ - joint_pos[self._pos_joint_ids["x"]].item(), - joint_pos[self._pos_joint_ids["y"]].item(), - joint_pos[self._pos_joint_ids["z"]].item(), - ] - current_euler = [ - joint_pos[self._rot_joint_ids["roll"]].item(), - joint_pos[self._rot_joint_ids["pitch"]].item(), - joint_pos[self._rot_joint_ids["yaw"]].item(), - ] - dpg.set_value( - pos_text_tag, - f"Pos: [{current_pos[0]:.3f}, {current_pos[1]:.3f}, {current_pos[2]:.3f}]", - ) - dpg.set_value( - rot_text_tag, - f"Euler: [{current_euler[0]:.3f}, {current_euler[1]:.3f}, " - f"{current_euler[2]:.3f}]", - ) - else: - # Read from root link pose - current_pos = self._object.data.root_link_pos_w[0].cpu() - current_quat = self._object.data.root_link_quat_w[0].cpu() - dpg.set_value( - pos_text_tag, - f"Pos: [{current_pos[0]:.3f}, {current_pos[1]:.3f}, {current_pos[2]:.3f}]", - ) - dpg.set_value( - rot_text_tag, - f"Quat: [{current_quat[0]:.3f}, {current_quat[1]:.3f}, " - f"{current_quat[2]:.3f}, {current_quat[3]:.3f}]", - ) - except Exception: - pass # Object may not be ready yet - - # --------------------------------------------------------------------- - # ActionTerm Interface - # --------------------------------------------------------------------- - - def process_actions(self, actions: torch.Tensor) -> None: # noqa: ARG002 - """Ignore incoming RL actions; GUI values are authoritative.""" - pass - - def apply_actions(self) -> None: - """Apply the GUI-specified pose to the object.""" - if self._use_floating_base_joints: - self._apply_via_floating_base_joints() - else: - self._apply_via_root_pose() - - def _apply_via_floating_base_joints(self) -> None: - """Apply pose via floating base joints (for Articulation with floating base).""" - # Type narrowing - these are guaranteed non-None when this method is called - assert self._pos_joint_ids is not None - assert self._rot_joint_ids is not None - - with self._lock: - pos = self._desired_pos.clone() - euler = self._desired_euler.clone() - - # Convert to device - pos = pos.to(self._device) - euler = euler.to(self._device) - - # Build joint position target tensor - joint_target = self._object.data.joint_pos.clone() - - # Set position joints for all environments - joint_target[:, self._pos_joint_ids["x"]] = pos[0] - joint_target[:, self._pos_joint_ids["y"]] = pos[1] - joint_target[:, self._pos_joint_ids["z"]] = pos[2] - - # Set rotation joints for all environments - joint_target[:, self._rot_joint_ids["roll"]] = euler[0] - joint_target[:, self._rot_joint_ids["pitch"]] = euler[1] - joint_target[:, self._rot_joint_ids["yaw"]] = euler[2] - - # Get the joint IDs for the floating base - floating_base_joint_ids = list(self._pos_joint_ids.values()) + list( - self._rot_joint_ids.values() - ) - - if self.cfg.control_mode == "kinematic": - # Directly write joint positions (immediate effect) - self._object.write_joint_state_to_sim( - joint_target[:, floating_base_joint_ids], - torch.zeros_like(joint_target[:, floating_base_joint_ids]), - joint_ids=floating_base_joint_ids, - ) - else: # pd_target - # Set joint position targets (follows PD controller) - self._object.set_joint_position_target( - joint_target[:, floating_base_joint_ids], - joint_ids=floating_base_joint_ids, - ) - - def _apply_via_root_pose(self) -> None: - """Apply pose directly via root link (for Rigid or Articulation without floating base).""" - with self._lock: - pos = self._desired_pos.clone() - euler = self._desired_euler.clone() - - # Convert to device - pos = pos.to(self._device) - euler = euler.to(self._device) - - # Convert Euler to quaternion (wxyz format) - quat = quat_from_euler_xyz( - euler[0:1], # roll - euler[1:2], # pitch - euler[2:3], # yaw - ).squeeze(0) - - # Build pose tensor [x, y, z, w, qx, qy, qz] - pose = torch.cat([pos, quat]) - - # Expand for all environments - pose_batch = pose.unsqueeze(0).expand(self._num_envs, -1) - - # Write pose to simulation - self._object.write_root_link_pose_to_sim(pose_batch) - - # Zero out velocities to prevent drift - zero_vel = torch.zeros(self._num_envs, 6, device=self._device) - self._object.write_root_com_velocity_to_sim(zero_vel) - - def reset(self, env_ids: torch.Tensor | None = None) -> None: - """Reset the action term (called on environment reset). - - Args: - env_ids: Environment indices to reset. If None, resets all. - """ - # Sync GUI with current object pose on reset - if env_ids is None or 0 in env_ids: - with self._lock: - if self._use_floating_base_joints: - # Type narrowing - these are guaranteed non-None when using floating base - assert self._pos_joint_ids is not None - assert self._rot_joint_ids is not None - # Reset from default joint positions - joint_pos = self._object.data.default_joint_pos[0].cpu() - - self._desired_pos[0] = joint_pos[self._pos_joint_ids["x"]].item() - self._desired_pos[1] = joint_pos[self._pos_joint_ids["y"]].item() - self._desired_pos[2] = joint_pos[self._pos_joint_ids["z"]].item() - - self._desired_euler[0] = joint_pos[ - self._rot_joint_ids["roll"] - ].item() - self._desired_euler[1] = joint_pos[ - self._rot_joint_ids["pitch"] - ].item() - self._desired_euler[2] = joint_pos[ - self._rot_joint_ids["yaw"] - ].item() - else: - # Reset from current root link pose - current_pos = self._object.data.root_link_pos_w[0].cpu() - current_quat = self._object.data.root_link_quat_w[0].cpu() - roll, pitch, yaw = euler_xyz_from_quat(current_quat.unsqueeze(0)) - - self._desired_pos[:] = current_pos - self._desired_euler[0] = roll.item() - self._desired_euler[1] = pitch.item() - self._desired_euler[2] = yaw.item() - - # Update defaults - self._default_pos[:] = self._desired_pos.clone() - self._default_euler[:] = self._desired_euler.clone() - - -# Backwards compatibility alias -ArticulatedObjectPoseGUIAction = ObjectPoseGUIAction -RigidObjectPoseGUIAction = ObjectPoseGUIAction diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action_cfg.py deleted file mode 100644 index ddce211d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/object_pose_gui_action_cfg.py +++ /dev/null @@ -1,126 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Configuration for the Unified Object Pose GUI Action.""" - -from __future__ import annotations - -import math -from dataclasses import field -from typing import TYPE_CHECKING, Literal - -from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg -from isaaclab.utils import configclass - -if TYPE_CHECKING: - pass - - -@configclass -class ObjectPoseGUIActionCfg(ActionTermCfg): - """Configuration for the unified object pose GUI action term. - - This action term allows interactive control of an object's 6DoF base pose - via a DearPyGui window. It supports: - - 1. **RigidObject**: Direct pose via `write_root_link_pose_to_sim()` - 2. **Articulation with floating base joints**: Pose via floating base joints - 3. **Articulation without floating base joints**: Direct root pose write - - The control mode is determined by the `position_joint_names` and `rotation_joint_names` - configuration: - - If joint names are provided, control is via floating base joints (for Articulation) - - If joint names are None, control is via direct root pose write (for both RigidObject - and Articulation without floating base) - - The user can adjust position (x, y, z) and orientation (roll, pitch, yaw) using sliders. - - Attributes: - asset_name: Name of the object in the scene to control. - position_limits: Dictionary mapping axis names to (min, max) tuples in meters. - rotation_limits: Dictionary mapping axis names to (min, max) tuples in radians. - position_joint_names: Optional mapping from axis names to joint names for position. - rotation_joint_names: Optional mapping from axis names to joint names for rotation. - control_mode: Control mode for floating base joints ("kinematic" or "pd_target"). - gui_window_title: Title of the DearPyGui window. - """ - - class_type: type[ActionTerm] = field( - default_factory=lambda: _get_object_pose_gui_action_class() - ) - """The class type for this action term.""" - - # Object to control (uses asset_name from base ActionTermCfg) - asset_name: str = "object" - """Name of the object entity in the scene. Defaults to 'object'.""" - - # Position limits (meters) - position_limits: dict[str, tuple[float, float]] = field( - default_factory=lambda: { - "x": (-2.0, 2.0), - "y": (-2.0, 2.0), - "z": (0.0, 3.0), - } - ) - """Position limits for each axis in meters. Defaults to +/-2m for x/y, 0-3m for z.""" - - # Rotation limits (radians) - rotation_limits: dict[str, tuple[float, float]] = field( - default_factory=lambda: { - "roll": (-math.pi, math.pi), - "pitch": (-math.pi, math.pi), - "yaw": (-math.pi, math.pi), - } - ) - """Rotation limits for each Euler angle in radians. Defaults to +/-pi for all axes.""" - - # Optional joint name mapping for floating base (Articulation only) - # If None, pose is applied directly to root (works for both Rigid and Articulation) - position_joint_names: dict[str, str] | None = None - """Optional mapping from axis names to joint names for position control. - - If provided, the object must be an Articulation with matching floating base joints. - Example: {"x": "base_x", "y": "base_y", "z": "base_z"} - - If None, pose is applied directly via write_root_link_pose_to_sim(). - """ - - rotation_joint_names: dict[str, str] | None = None - """Optional mapping from axis names to joint names for rotation control. - - If provided, the object must be an Articulation with matching floating base joints. - Example: {"roll": "base_roll", "pitch": "base_pitch", "yaw": "base_yaw"} - - If None, pose is applied directly via write_root_link_pose_to_sim(). - """ - - # Control mode for floating base joints - control_mode: Literal["kinematic", "pd_target"] = "kinematic" - """Control mode when using floating base joints. - - - "kinematic": Directly write joint positions (immediate, no dynamics) - - "pd_target": Set joint position targets (follows PD controller dynamics) - - Defaults to "kinematic" for direct pose control. - """ - - # GUI settings - gui_window_title: str = "Object Pose Controller" - """Title of the DearPyGui window. Defaults to 'Object Pose Controller'.""" - - gui_window_width: int = 500 - """Width of the GUI window in pixels. Defaults to 500.""" - - gui_window_height: int = 400 - """Height of the GUI window in pixels. Defaults to 400.""" - - -def _get_object_pose_gui_action_class() -> type[ActionTerm]: - """Lazy import to avoid circular dependency.""" - from .object_pose_gui_action import ObjectPoseGUIAction # noqa: PLC0415 - - return ObjectPoseGUIAction - - -# Backwards compatibility aliases -ArticulatedObjectPoseGUIActionCfg = ObjectPoseGUIActionCfg -RigidObjectPoseGUIActionCfg = ObjectPoseGUIActionCfg diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer.py deleted file mode 100644 index 497d69fb..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer.py +++ /dev/null @@ -1,413 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Reward Visualizer GUI for real-time reward monitoring.""" - -from __future__ import annotations - -import threading -from collections import deque -from typing import TYPE_CHECKING - -import torch -from isaaclab.managers.action_manager import ActionTerm - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - from .reward_visualizer_cfg import RewardVisualizerCfg - - -class RewardVisualizer(ActionTerm): - """Reward visualizer GUI action. - - This action term displays real-time reward values via a DearPyGui window. - It shows individual reward term values, weights, and cumulative episode sums. - Optionally displays a time series plot of total reward history. - - Features: - - Bar chart of individual reward term values with color coding - - Positive rewards shown in green, negative in red - - Time series plot of total reward over time - - Display of weights and episode cumulative sums - - Usage: - Add this action term to visualize rewards during interactive debugging. - The visualizer reads from the environment's reward manager. - """ - - cfg: RewardVisualizerCfg - - # --------------------------------------------------------------------- - # Initialization - # --------------------------------------------------------------------- - - def __init__(self, cfg: RewardVisualizerCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the reward visualizer. - - Args: - cfg: Configuration for the visualizer. - env: The environment instance. - """ - # Store config and environment (don't call super().__init__) - self.cfg = cfg - self._env = env - self._device = env.device - self._num_envs = env.num_envs - - # Reward manager reference (lazy init after load_managers) - self._reward_manager = None - self._initialized = False - - # Visible terms (resolved after reward manager is available) - self._visible_terms: list[str] = [] - self._term_indices: list[int] = [] - - # History buffer for time series plot - self._reward_history: deque = deque(maxlen=cfg.history_length) - self._step_count = 0 - - # Thread-safe lock - self._lock = threading.Lock() - - # GUI data buffers (updated from main thread, read by GUI thread) - self._current_rewards: dict[str, float] = {} - self._current_weights: dict[str, float] = {} - self._episode_sums: dict[str, float] = {} - self._total_reward: float = 0.0 - - # Launch GUI in a daemon thread - self._gui_thread = threading.Thread( - target=self._launch_gui, name="RewardVisualizerGUI", daemon=True - ) - self._gui_thread.start() - - # --------------------------------------------------------------------- - # Properties - # --------------------------------------------------------------------- - - @property - def action_dim(self) -> int: - """Dimension of the action term (not used for visualizer).""" - return 0 - - @property - def device(self) -> str: - """Device for tensors.""" - return str(self._device) - - @property - def num_envs(self) -> int: - """Number of environments.""" - return int(self._num_envs) - - @property - def raw_actions(self) -> torch.Tensor: - """The input/raw actions (unused for visualizer).""" - return torch.empty(0, device=self._device) - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions (unused for visualizer).""" - return torch.empty(0, device=self._device) - - # --------------------------------------------------------------------- - # Initialization Helpers - # --------------------------------------------------------------------- - - def _lazy_init(self) -> None: - """Initialize reward manager reference and resolve visible terms.""" - if self._initialized: - return - - # Get reward manager from environment - if not hasattr(self._env, "reward_manager") or self._env.reward_manager is None: - return - - self._reward_manager = self._env.reward_manager - self._resolve_visible_terms() - self._initialized = True - - def _resolve_visible_terms(self) -> None: - """Determine which reward terms to display based on config.""" - assert self._reward_manager is not None, "Reward manager not initialized" - - all_terms = self._reward_manager.active_terms - - if self.cfg.reward_terms: - # User specified specific terms - filter to valid ones - self._visible_terms = [t for t in self.cfg.reward_terms if t in all_terms] - else: - # Show all terms except excluded ones - self._visible_terms = [ - t for t in all_terms if t not in self.cfg.exclude_terms - ] - - # Get indices for efficient lookup - self._term_indices = [all_terms.index(t) for t in self._visible_terms] - - # Initialize weights from term configs - for term_name in self._visible_terms: - term_cfg = self._reward_manager.get_term_cfg(term_name) - self._current_weights[term_name] = term_cfg.weight - - # --------------------------------------------------------------------- - # GUI Implementation - # --------------------------------------------------------------------- - - def _launch_gui(self) -> None: - """Create the DearPyGui window with reward visualization.""" - import time # noqa: PLC0415 - - import dearpygui.dearpygui as dpg # noqa: PLC0415 - - # Wait for other GUIs to initialize - time.sleep(1.0) - - # Check if DearPyGui context already exists - try: - context_exists = dpg.is_dearpygui_running() - except Exception: - context_exists = False - - owns_context = False - if not context_exists: - try: - dpg.create_context() - dpg.create_viewport( - title="Debug Controller", - width=600, - height=1400, - ) - owns_context = True - except Exception: - pass - - # Create color themes for positive/negative rewards - with dpg.theme() as positive_theme: - with dpg.theme_component(dpg.mvProgressBar): - dpg.add_theme_color(dpg.mvThemeCol_PlotHistogram, (100, 200, 100, 255)) - - with dpg.theme() as negative_theme: - with dpg.theme_component(dpg.mvProgressBar): - dpg.add_theme_color(dpg.mvThemeCol_PlotHistogram, (200, 100, 100, 255)) - - # Use unique window tag - window_tag = "reward_visualizer_window" - - # Store UI element tags for updates - total_reward_tag = None - reward_bar_tags: dict[str, int] = {} - reward_value_tags: dict[str, int] = {} - episode_sum_tags: dict[str, int] = {} - history_plot_tag = None - history_x_data = [] - history_y_data = [] - - with dpg.window( - label=self.cfg.gui_window_title, - tag=window_tag, - width=self.cfg.gui_window_width, - height=self.cfg.gui_window_height, - pos=(10, 750), - ): - dpg.add_text("Real-time reward visualization") - dpg.add_text(f"Environment index: {self.cfg.env_index}") - dpg.add_separator() - - # Total reward display - if self.cfg.show_total_reward: - with dpg.group(horizontal=True): - dpg.add_text("TOTAL REWARD:") - total_reward_tag = dpg.add_text("0.0000", color=(255, 255, 100)) - - dpg.add_separator() - dpg.add_text("REWARD TERMS (per step)") - - # Create header row - if self.cfg.show_weights and self.cfg.show_episode_sum: - dpg.add_text( - "Name Weight Value Episode Sum" - ) - elif self.cfg.show_weights: - dpg.add_text("Name Weight Value") - elif self.cfg.show_episode_sum: - dpg.add_text( - "Name Value Episode Sum" - ) - else: - dpg.add_text("Name Value") - - # Placeholder for reward bars (will be populated after init) - reward_container_tag = dpg.add_group(tag="reward_bars_container") - - dpg.add_separator() - - # History plot - if self.cfg.enable_history_plot: - dpg.add_text("REWARD HISTORY") - with dpg.plot(label="Total Reward Over Time", height=150, width=-1): - dpg.add_plot_axis(dpg.mvXAxis, label="Step") - with dpg.plot_axis(dpg.mvYAxis, label="Reward"): - history_plot_tag = dpg.add_line_series([], [], label="Total") - - # Run GUI loop - if owns_context: - dpg.setup_dearpygui() - dpg.show_viewport() - - # Flag to track if reward bars have been created - bars_created = False - - while True: - try: - if owns_context and not dpg.is_dearpygui_running(): - break - if not owns_context: - try: - if not dpg.is_dearpygui_running(): - break - except Exception: - break - - # Update reward data from buffers - with self._lock: - current_rewards = self._current_rewards.copy() - current_weights = self._current_weights.copy() - episode_sums = self._episode_sums.copy() - total_reward = self._total_reward - history_list = list(self._reward_history) - - # Create reward bars if not yet created and we have terms - if not bars_created and self._visible_terms: - with dpg.group(parent=reward_container_tag): - for term_name in self._visible_terms: - with dpg.group(horizontal=True): - # Term name (truncated) - display_name = term_name[:20].ljust(20) - dpg.add_text(display_name) - - # Weight - if self.cfg.show_weights: - weight = current_weights.get(term_name, 0.0) - dpg.add_text(f"{weight:7.3f}") - - # Progress bar for value - bar_tag = dpg.add_progress_bar( - default_value=0.5, - width=100, - ) - reward_bar_tags[term_name] = bar_tag - - # Value text - value_tag = dpg.add_text(" 0.0000") - reward_value_tags[term_name] = value_tag - - # Episode sum - if self.cfg.show_episode_sum: - sum_tag = dpg.add_text(" 0.00") - episode_sum_tags[term_name] = sum_tag - - bars_created = True - - # Update total reward - if total_reward_tag is not None: - color = (100, 255, 100) if total_reward >= 0 else (255, 100, 100) - dpg.set_value(total_reward_tag, f"{total_reward:+.4f}") - dpg.configure_item(total_reward_tag, color=color) - - # Update reward bars and values - for term_name in self._visible_terms: - if term_name not in reward_bar_tags: - continue - - value = current_rewards.get(term_name, 0.0) - ep_sum = episode_sums.get(term_name, 0.0) - - # Normalize value for progress bar (0-1 range) - # Use sigmoid-like scaling for visualization - bar_value = ( - 0.5 + 0.5 * (value / (abs(value) + 0.1)) if value != 0 else 0.5 - ) - - dpg.set_value(reward_bar_tags[term_name], bar_value) - dpg.set_value(reward_value_tags[term_name], f"{value:+.4f}") - - # Apply color theme based on sign - if value >= 0: - dpg.bind_item_theme(reward_bar_tags[term_name], positive_theme) - else: - dpg.bind_item_theme(reward_bar_tags[term_name], negative_theme) - - # Update episode sum - if term_name in episode_sum_tags: - dpg.set_value(episode_sum_tags[term_name], f"{ep_sum:+8.2f}") - - # Update history plot - if history_plot_tag is not None and history_list: - history_x_data = list(range(len(history_list))) - history_y_data = history_list - dpg.set_value(history_plot_tag, [history_x_data, history_y_data]) - - if owns_context: - dpg.render_dearpygui_frame() - else: - time.sleep(0.05) - - except Exception: - time.sleep(0.1) - - if owns_context: - dpg.destroy_context() - - # --------------------------------------------------------------------- - # ActionTerm Interface - # --------------------------------------------------------------------- - - def process_actions(self, actions: torch.Tensor) -> None: # noqa: ARG002 - """Process actions (no-op for visualizer).""" - pass - - def apply_actions(self) -> None: - """Update reward visualization data.""" - # Lazy initialization - self._lazy_init() - - if not self._initialized or self._reward_manager is None: - return - - # Only update at specified interval - self._step_count += 1 - if self._step_count % self.cfg.update_interval != 0: - return - - env_idx = self.cfg.env_index - - # Get current step rewards - step_rewards = self._reward_manager._step_reward[env_idx].cpu() - total_reward = self._reward_manager._reward_buf[env_idx].item() - - # Update buffers (thread-safe) - with self._lock: - self._total_reward = total_reward - - for i, term_name in enumerate(self._visible_terms): - term_idx = self._term_indices[i] - self._current_rewards[term_name] = step_rewards[term_idx].item() - - # Get episode sum - if term_name in self._reward_manager._episode_sums: - self._episode_sums[term_name] = self._reward_manager._episode_sums[ - term_name - ][env_idx].item() - - # Update history - self._reward_history.append(total_reward) - - def reset(self, env_ids: torch.Tensor | None = None) -> None: - """Reset the visualizer (called on environment reset).""" - # Clear history on reset for the monitored env - if env_ids is None or self.cfg.env_index in env_ids: - with self._lock: - self._reward_history.clear() - for term_name in self._visible_terms: - self._episode_sums[term_name] = 0.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer_cfg.py deleted file mode 100644 index 4b09d8a6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/reward_visualizer_cfg.py +++ /dev/null @@ -1,88 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Configuration for the Reward Visualizer GUI.""" - -from __future__ import annotations - -from dataclasses import field - -from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg -from isaaclab.utils import configclass - - -@configclass -class RewardVisualizerCfg(ActionTermCfg): - """Configuration for the reward visualization GUI. - - This visualizer displays real-time reward values for each reward term, - allowing users to see how rewards change as the robot/object state changes. - - Users can register specific reward terms to visualize, or show all terms. - - Attributes: - reward_terms: List of reward term names to visualize. Empty list shows all. - exclude_terms: Terms to exclude from visualization when showing all. - show_total_reward: Whether to show the total summed reward. - show_weights: Whether to display the weight multiplier for each term. - show_episode_sum: Whether to show cumulative episode rewards. - env_index: Which environment index to visualize. - enable_history_plot: Whether to show time series plot of total reward. - history_length: Number of steps to keep in history buffer. - """ - - class_type: type[ActionTerm] = field( - default_factory=lambda: _get_reward_visualizer_class() - ) - """The class type for this action term.""" - - # Need a dummy asset_name to satisfy ActionTermCfg base class - asset_name: str = "robot" - """Dummy asset name (required by base class but not used).""" - - # --- Reward Term Selection --- - reward_terms: list[str] = field(default_factory=list) - """List of reward term names to visualize. Empty list shows all terms.""" - - exclude_terms: list[str] = field(default_factory=list) - """Terms to exclude from visualization (useful when showing all).""" - - # --- Display Options --- - show_total_reward: bool = True - """Whether to show the total (summed) reward at the top. Defaults to True.""" - - show_weights: bool = True - """Whether to show the weight multiplier for each term. Defaults to True.""" - - show_episode_sum: bool = True - """Whether to show cumulative episode reward for each term. Defaults to True.""" - - env_index: int = 0 - """Which environment index to visualize. Defaults to 0.""" - - # --- Time Series Plot --- - enable_history_plot: bool = True - """Whether to enable time series plot of total reward. Defaults to True.""" - - history_length: int = 200 - """Number of steps to keep in history buffer. Defaults to 200.""" - - # --- GUI Settings --- - gui_window_title: str = "Reward Monitor" - """Title of the DearPyGui window. Defaults to 'Reward Monitor'.""" - - gui_window_width: int = 500 - """Width of the GUI window in pixels. Defaults to 500.""" - - gui_window_height: int = 450 - """Height of the GUI window in pixels. Defaults to 450.""" - - # --- Update Settings --- - update_interval: int = 1 - """Update GUI every N simulation steps. Defaults to 1 (every step).""" - - -def _get_reward_visualizer_class() -> type[ActionTerm]: - """Lazy import to avoid circular dependency.""" - from .reward_visualizer import RewardVisualizer # noqa: PLC0415 - - return RewardVisualizer diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/rewards.py deleted file mode 100644 index a1965092..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/mdp/rewards.py +++ /dev/null @@ -1,80 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Debug reward functions for visualizing sensor data.""" - -from __future__ import annotations - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.sensors import ContactSensor - -# Axis name to index mapping -_AXIS_INDEX = {"x": 0, "y": 1, "z": 2} - - -# ============================================================================= -# Contact Force Functions -# ============================================================================= - - -def contact_force( - env: ManagerBasedRLEnv, - sensor_name: str = "right_pinky_contact_sensor", - axis: str = "x", -) -> torch.Tensor: - """Return the specified component of contact force in world frame. - - Args: - env: The RL environment. - sensor_name: Name of the contact sensor. - axis: Which axis to return ('x', 'y', or 'z'). - - Returns: - The force component along the specified axis. Shape: (num_envs,) - """ - if axis not in _AXIS_INDEX: - raise ValueError(f"Invalid axis '{axis}'. Must be one of: 'x', 'y', 'z'") - sensor: ContactSensor = env.scene[sensor_name] - net_forces_w = sensor.data.net_forces_w # (num_envs, num_bodies, 3) - total_force = net_forces_w.sum(dim=1) # (num_envs, 3) - return total_force[:, _AXIS_INDEX[axis]] # (num_envs,) - - -# ============================================================================= -# Contact Position Functions (actual contact point in world frame) -# ============================================================================= - - -def contact_pos( - env: ManagerBasedRLEnv, - sensor_name: str = "right_pinky_contact_sensor", - axis: str = "x", -) -> torch.Tensor: - """Return the specified position component of the actual contact point in world frame. - - Uses contact_pos_w which is the average position of contact points. - Returns 0 when there is no contact (NaN replaced with 0). - - Args: - env: The RL environment. - sensor_name: Name of the contact sensor. - axis: Which axis to return ('x', 'y', or 'z'). - - Returns: - The position component along the specified axis. Shape: (num_envs,) - - Note: - Requires ContactSensorCfg.track_contact_points=True and - ContactSensorCfg.max_contact_data_per_prim >= 1. - """ - if axis not in _AXIS_INDEX: - raise ValueError(f"Invalid axis '{axis}'. Must be one of: 'x', 'y', 'z'") - sensor: ContactSensor = env.scene[sensor_name] - contact_pos_w = sensor.data.contact_pos_w - # Return zeros if contact position tracking is not enabled - if contact_pos_w is None: - return torch.zeros(env.num_envs, device=env.device) - # contact_pos_w shape: (num_envs, num_bodies, num_filter_bodies, 3) - # Take first body and first filter body, specified axis component - pos = contact_pos_w[:, 0, 0, _AXIS_INDEX[axis]] # (num_envs,) - return torch.nan_to_num(pos, nan=0.0) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/sharpa_debug_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/sharpa_debug_env_cfg.py deleted file mode 100644 index f9d2388e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/debug/sharpa_debug_env_cfg.py +++ /dev/null @@ -1,234 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Debug environment configuration for Sharpa V2P with GUI control. - -This environment provides interactive GUI controls for both the dual Sharpa hands -and the Arctic object, useful for debugging contact sensors and MDP components. -""" - -import math - -import isaaclab.envs.mdp as isaac_mdp -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.utils import configclass - -from robotic_grounding.tasks.debug import mdp as debug_mdp -from robotic_grounding.tasks.debug.mdp import ( - JointGUIActionCfg, - ObjectPoseGUIActionCfg, - RewardVisualizerCfg, -) -from robotic_grounding.tasks.v2p.config.sharpa_wave.sharpa_v2p_env_cfg import ( - SharpaV2PEnvCfg, -) -from robotic_grounding.tasks.v2p.v2p_hand_env_cfg import ActionsCfg, RewardsCfg - - -@configclass -class DebugActionsCfg(ActionsCfg): - """Actions configuration with GUI control for all robot joints and object pose.""" - - # GUI control for ALL robot joints (wrist + finger) - # Wrist joints (x/y/z/roll/pitch/yaw) control hand root pose - # Finger joints control individual finger positions - joint_pos = JointGUIActionCfg( - asset_name="robot", - joint_names=[".*"], # All joints - scale=1.0, # Will be overridden in __post_init__ with proper scale - use_default_offset=True, - preserve_order=True, - max_stiffness=200.0, - max_damping=25.0, - ) - - # Object pose GUI control (unified for both articulated and rigid objects) - # - For articulated objects with floating base: provide position_joint_names and rotation_joint_names - # - For rigid objects or articulations without floating base: omit joint names (uses direct root pose) - object_pose = ObjectPoseGUIActionCfg( - asset_name="object", - position_limits={ - "x": (-1.0, 1.0), - "y": (-1.0, 1.0), - "z": (0.5, 3.0), - }, - rotation_limits={ - "roll": (-math.pi, math.pi), - "pitch": (-math.pi, math.pi), - "yaw": (-math.pi, math.pi), - }, - # For articulated object with floating base joints: - position_joint_names={ - "x": "base_x", - "y": "base_y", - "z": "base_z", - }, - rotation_joint_names={ - "roll": "base_roll", - "pitch": "base_pitch", - "yaw": "base_yaw", - }, - gui_window_title="Object Pose Controller", - ) - - # Reward visualizer GUI to monitor reward terms in real-time - reward_visualizer = RewardVisualizerCfg( - asset_name="robot", # Required by ActionTermCfg but not used - show_total_reward=True, - show_weights=True, - show_episode_sum=True, - env_index=0, - enable_history_plot=True, - history_length=200, - gui_window_title="Reward Monitor", - gui_window_width=500, - gui_window_height=450, - update_interval=1, - ) - - -@configclass -class DebugRewardsCfg(RewardsCfg): - """Minimal rewards for debugging contact sensors. - - Shows exact contact position (x, y, z) and contact force (x, y, z, magnitude) - for the right pinky contact sensor. - """ - - # Keep alive reward - is_alive = RewTerm(func=isaac_mdp.is_alive, weight=1.0) - - # --- Contact Position (world frame) --- - contact_pos_x = RewTerm( - func=debug_mdp.contact_pos, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "x"}, - ) - contact_pos_y = RewTerm( - func=debug_mdp.contact_pos, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "y"}, - ) - contact_pos_z = RewTerm( - func=debug_mdp.contact_pos, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "z"}, - ) - - # --- Contact Force (world frame, 0 when no contact) --- - contact_force_x = RewTerm( - func=debug_mdp.contact_force, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "x"}, - ) - contact_force_y = RewTerm( - func=debug_mdp.contact_force, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "y"}, - ) - contact_force_z = RewTerm( - func=debug_mdp.contact_force, - weight=1.0, - params={"sensor_name": "right_pinky_contact_sensor_bottom", "axis": "z"}, - ) - - # Disable heavy reward computations for debugging - action_rate_l2 = None - joint_limit = None - contact_force_penalty = None - - -@configclass -class DebugTerminationsCfg: - """Minimal terminations for debugging.""" - - # Only time out after very long episode for debugging - time_out = DoneTerm(func=isaac_mdp.time_out, time_out=True) - - -@configclass -class SharpaDebugEnvCfg(SharpaV2PEnvCfg): - """Debug environment for Sharpa V2P with GUI control. - - This environment provides: - - Joint GUI control for ALL robot joints with P/D gain adjustment: - - Wrist joints (x/y/z/roll/pitch/yaw) for 6DoF hand root pose control - - Finger joints for individual finger position control - - Object pose GUI control for the Arctic object (6DoF via floating base) - - Contact sensor visualization for debugging - - Useful for: - - Verifying contact sensor setup - - Debugging MDP observations and rewards - - Testing robot-object interaction manually - - Positioning hands and object to check contact forces - """ - - # Override actions with GUI-controlled versions - actions: DebugActionsCfg = DebugActionsCfg() - - # Use minimal rewards and terminations for debugging - rewards: DebugRewardsCfg = DebugRewardsCfg() - terminations: DebugTerminationsCfg = DebugTerminationsCfg() - - def __post_init__(self) -> None: - """Post initialization.""" - super().__post_init__() - - # Parent's __post_init__ sets DUAL_SHARPA_WAVE_ACTION_SCALE which is correct - # for all joints (wrist + finger), so we don't override it here - - # Override initial positions for better debugging: - # - Position hands to wrap around the object for contact testing - # - Hands face inward toward object (palms facing object) - self.scene.robot.init_state.joint_pos = { - # Right hand: positioned to the right of object, rotated to face left - "right_wrist_x": -0.15, # Close to object - "right_wrist_y": 0.0, - "right_wrist_z": 1.25, - "right_wrist_roll": -math.pi / 2, # Palm facing left (toward object) - "right_wrist_pitch": 0.0, - "right_wrist_yaw": 0.0, - # Left hand: positioned to the left of object, rotated to face right - "left_wrist_x": 0.15, # Close to object - "left_wrist_y": 0.0, - "left_wrist_z": 1.25, - "left_wrist_roll": math.pi / 2, # Palm facing right (toward object) - "left_wrist_pitch": 0.0, - "left_wrist_yaw": 0.0, - } - - # Position object between the hands - self.scene.object.init_state.joint_pos = { - "base_x": -0.030, - "base_y": 0.0, - "base_z": 1.2, - "base_roll": 0.0, - "base_pitch": 0.0, - "base_yaw": 0.7, - "rotation": 0.0, - } - - # Reduce number of environments for debugging (less GPU memory) - self.scene.num_envs = 1 - - # Enable contact sensor debug visualization - for sensor_name in self.finger_sensor_names: - sensor = getattr(self.scene, sensor_name, None) - if sensor is not None: - sensor.debug_vis = True - - # Extend episode length for extended debugging sessions - self.episode_length_s = 3600.0 # 1 hour - - # Disable events that interfere with manual control - if hasattr(self.events, "physics_material"): - self.events.physics_material = None - if hasattr(self.events, "reset_robot_and_object"): - self.events.reset_robot_and_object = None - - # Viewer settings for close-up view of hands and object - # Camera positioned to look at the manipulation area (z=1.25) - self.viewer.eye = (0.8, 0.8, 1.6) # Close diagonal view - self.viewer.lookat = (0.0, 0.0, 1.25) # Look at object/hands center - self.viewer.origin_type = "world" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/__init__.py deleted file mode 100644 index 543970e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/__init__.py +++ /dev/null @@ -1,32 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Scene loading utilities and config (motion_file → SceneConfig → apply to env_cfg).""" - -import os - -from .apply_scene_config import ( - apply_scene_commands, - apply_scene_config, - apply_scene_contact_sensors, - apply_scene_objects, - apply_scene_robot, -) -from .scene_config import ( - ArticulatedObjectConfig, - ObjectConfig, - SceneConfig, -) - -SCENE_CONFIG_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), "config")) - -__all__ = [ - "ArticulatedObjectConfig", - "ObjectConfig", - "SceneConfig", - "apply_scene_commands", - "apply_scene_config", - "apply_scene_contact_sensors", - "apply_scene_objects", - "apply_scene_robot", - "SCENE_CONFIG_DIR", -] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/apply_scene_config.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/apply_scene_config.py deleted file mode 100644 index 4be761f6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/apply_scene_config.py +++ /dev/null @@ -1,521 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Apply a SceneConfig to an IsaacLab environment configuration. - -Public API: - apply_scene_objects — spawn object + fixed objects (viewer and training) - apply_scene_robot — place robot hands from registry - apply_scene_commands — configure dual-hand tracking command - apply_scene_contact_sensors — set up per-side contact sensors - apply_scene_config — all of the above in one call (training entry point) -""" - -from __future__ import annotations - -import os -from pathlib import Path -from typing import Any - -import isaaclab.sim as sim_utils -from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg -from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg -from isaaclab.sim.schemas.schemas_cfg import RigidBodyPropertiesCfg -from pxr import Usd, UsdGeom - -from robotic_grounding.assets.articulated_object import ARTICULATED_OBJECT_CFG -from robotic_grounding.assets.rigid_object import RIGID_OBJECT_CFG -from robotic_grounding.assets.robot_registry import get_robot_spec -from robotic_grounding.tasks.scene_utils.scene_config import ( - ArticulatedObjectConfig, - ObjectConfig, - SceneConfig, -) -from robotic_grounding.tasks.v2p.mdp.actions import ( - VirtualArticulatedObjectControlCfg, - VirtualRigidObjectControlCfg, -) - -################################################### -# Parameters -################################################### - -virtual_object_control_linear_stiffness = 50.0 -virtual_object_control_linear_damping = 10.0 -virtual_object_control_angular_stiffness = 10.0 -virtual_object_control_angular_damping = 0.1 -virtual_object_control_max_force = 60.0 -virtual_object_control_max_torque = 60.0 - - -def _spawn_articulated( - obj: ArticulatedObjectConfig, - prim_path: str, -) -> ArticulationCfg: - """Build an ArticulationCfg for an articulated object.""" - obj_pos = tuple(float(p) for p in obj.init_pos) if obj.init_pos else (0.0, 0.0, 0.0) - obj_rot = ( - tuple(float(r) for r in obj.init_rot) if obj.init_rot else (1.0, 0.0, 0.0, 0.0) - ) - joint_pos = float(obj.init_joint_pos) if obj.init_joint_pos is not None else 0.0 - return ARTICULATED_OBJECT_CFG.replace( - prim_path=prim_path, - spawn=ARTICULATED_OBJECT_CFG.spawn.replace( - asset_path=obj.urdf_path, - fix_base=False, - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=obj_pos, - rot=obj_rot, - joint_pos={".*": joint_pos}, - joint_vel={".*": 0.0}, - ), - actuators={ - "joint": ImplicitActuatorCfg( - joint_names_expr=[".*"], - effort_limit_sim={".*": 75.0}, - velocity_limit_sim={".*": 15.0}, - stiffness={".*": 0.0}, - damping={".*": 0.0}, - armature={".*": 0.01}, - friction={".*": 0.1}, - ), - }, - ) - - -def _spawn_rigid( - obj: ObjectConfig, - prim_path: str, -) -> RigidObjectCfg: - """Build a RigidObjectCfg for a rigid object.""" - obj_pos = tuple(float(p) for p in obj.init_pos) if obj.init_pos else (0.0, 0.0, 0.0) - obj_rot = ( - tuple(float(r) for r in obj.init_rot) if obj.init_rot else (1.0, 0.0, 0.0, 0.0) - ) - - if obj.usd_path.endswith(".urdf"): - return RIGID_OBJECT_CFG.replace( - prim_path=prim_path, - spawn=RIGID_OBJECT_CFG.spawn.replace(asset_path=obj.usd_path), - init_state=RigidObjectCfg.InitialStateCfg(pos=obj_pos, rot=obj_rot), - ) - - obj_scale = tuple(float(s) for s in obj.scale) - return RigidObjectCfg( - prim_path=prim_path, - spawn=sim_utils.UsdFileCfg( - usd_path=obj.usd_path, - scale=obj_scale, - collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), - rigid_props=RigidBodyPropertiesCfg( - solver_position_iteration_count=16, - solver_velocity_iteration_count=1, - max_angular_velocity=1000.0, - max_linear_velocity=1000.0, - linear_damping=0.01, - angular_damping=0.01, - max_depenetration_velocity=1.0, - max_contact_impulse=1e3, - disable_gravity=False, - ), - activate_contact_sensors=True, - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=obj_pos, rot=obj_rot), - ) - - -def apply_scene_objects(env_cfg: Any, scene_config: SceneConfig) -> None: - """Spawn all scene objects and fixed objects into env_cfg.scene.""" - for obj in scene_config.scene_objects: - attr_name = obj.name - prim_path = f"{{ENV_REGEX_NS}}/{attr_name}" - - if isinstance(obj, ArticulatedObjectConfig): - cfg = _spawn_articulated(obj, prim_path) - else: - cfg = _spawn_rigid(obj, prim_path) - - setattr(env_cfg.scene, attr_name, cfg) - if hasattr(env_cfg, "events") and hasattr( - env_cfg.events, "setup_collision_groups" - ): - env_cfg.events.setup_collision_groups.params["object_names"].append( - attr_name - ) - - # Fixed objects (support surfaces, etc.) - for fixed_obj in scene_config.fixed_objects: - if fixed_obj.init_pos is None or fixed_obj.init_rot is None: - raise ValueError( - f"fixed_object {fixed_obj.name} must have init_pos and init_rot" - ) - stage = Usd.Stage.Open(fixed_obj.usd_path) - for idx, prim in enumerate(stage.Traverse()): - attr_name = f"{fixed_obj.name}_{idx}" - xf = UsdGeom.Xformable(prim) - ops = xf.GetOrderedXformOps() - translate = ops[0].Get() if ops else (0.0, 0.0, 0.0) - - if prim.IsA(UsdGeom.Cylinder): - cyl = UsdGeom.Cylinder(prim) - spawn_cfg = sim_utils.CylinderCfg( - radius=cyl.GetRadiusAttr().Get(), - height=cyl.GetHeightAttr().Get(), - ) - elif prim.IsA(UsdGeom.Cube): - cube = UsdGeom.Cube(prim) - size = cube.GetSizeAttr().Get() - # Cube scale encodes per-axis dimensions - scale_ops = [op for op in ops if "scale" in op.GetName().lower()] - if scale_ops: - sx, sy, sz = scale_ops[0].Get() - else: - sx = sy = sz = 1.0 - spawn_cfg = sim_utils.CuboidCfg( - size=(size * sx, size * sy, size * sz), - ) - else: - continue - - spawn_cfg.rigid_props = sim_utils.RigidBodyPropertiesCfg( - kinematic_enabled=True - ) - spawn_cfg.mass_props = sim_utils.MassPropertiesCfg(mass=100.0) - spawn_cfg.collision_props = sim_utils.CollisionPropertiesCfg( - collision_enabled=True - ) - spawn_cfg.physics_material = sim_utils.RigidBodyMaterialCfg( - static_friction=1.0 - ) - spawn_cfg.visual_material = sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.14, 0.14, 0.14), metallic=0.7 - ) - - fixed_cfg = AssetBaseCfg( - prim_path=f"{{ENV_REGEX_NS}}/{attr_name}", - spawn=spawn_cfg, - init_state=AssetBaseCfg.InitialStateCfg( - pos=translate, - rot=[1.0, 0.0, 0.0, 0.0], - ), - ) - setattr(env_cfg.scene, attr_name, fixed_cfg) - if hasattr(env_cfg, "events") and hasattr( - env_cfg.events, "setup_collision_groups" - ): - env_cfg.events.setup_collision_groups.params[ - "fixed_object_names" - ].append(attr_name) - - -def apply_scene_virtual_object_controls( - env_cfg: Any, scene_config: SceneConfig -) -> None: - """Spawn virtual object controls into env_cfg.scene.""" - # Determine command name based on env type - if hasattr(env_cfg, "commands") and hasattr(env_cfg.commands, "motion"): - command_name = "motion" - else: - command_name = "dual_hands_object_tracking_command" - - for obj in scene_config.scene_objects: - object_name = obj.name - if isinstance(obj, ArticulatedObjectConfig): - voc_cfg = VirtualArticulatedObjectControlCfg( - asset_name=object_name, - command_name=command_name, - root_body_name=obj.body_names[0], - tracking_controller_linear_stiffness=virtual_object_control_linear_stiffness, - tracking_controller_linear_damping=virtual_object_control_linear_damping, # critical damping: 2 * sqrt(kp * m) - tracking_controller_angular_stiffness=virtual_object_control_angular_stiffness, - tracking_controller_angular_damping=virtual_object_control_angular_damping, # critical damping: 2 * sqrt(kp * I) - max_force=virtual_object_control_max_force, - max_torque=virtual_object_control_max_torque, - ) - setattr( - env_cfg.actions, - f"virtual_articulated_object_control_{object_name}", - voc_cfg, - ) - else: - voc_cfg = VirtualRigidObjectControlCfg( - asset_name=object_name, - command_name=command_name, - tracking_controller_linear_stiffness=virtual_object_control_linear_stiffness, - tracking_controller_linear_damping=virtual_object_control_linear_damping, # critical damping: 2 * sqrt(kp * m) - tracking_controller_angular_stiffness=virtual_object_control_angular_stiffness, - tracking_controller_angular_damping=virtual_object_control_angular_damping, # critical damping: 2 * sqrt(kp * I) - max_force=virtual_object_control_max_force, - max_torque=virtual_object_control_max_torque, - ) - setattr( - env_cfg.actions, f"virtual_rigid_object_control_{object_name}", voc_cfg - ) - - -def apply_scene_robot( - env_cfg: Any, - scene_config: SceneConfig, - static: bool = False, - use_primitive_urdfs: bool = False, -) -> None: - """Place robot from the robot registry based on scene_config.robot_name. - - Args: - env_cfg: The environment configuration to modify. - scene_config: The scene configuration with robot_name. - static: If True, disable gravity so the robot holds its initial pose. - use_primitive_urdfs: If True, use primitive URDFs for the robot. - """ - if scene_config.robot_name is None: - raise ValueError("robot_name not set — cannot configure robot") - - robot_spec = get_robot_spec(scene_config.robot_name) - if robot_spec is None: - raise ValueError(f"Unknown robot: {scene_config.robot_name}") - - def _maybe_disable_gravity(cfg: ArticulationCfg) -> ArticulationCfg: - if not static: - return cfg - return cfg.replace( - spawn=cfg.spawn.replace( - rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=True), - ), - ) - - if robot_spec.is_dual_hand: - env_cfg.scene.right_robot = _maybe_disable_gravity( - robot_spec.right_cfg - if not use_primitive_urdfs - else robot_spec.right_primitive_cfg - ).replace(prim_path="{ENV_REGEX_NS}/RightRobot") - env_cfg.scene.left_robot = _maybe_disable_gravity( - robot_spec.left_cfg - if not use_primitive_urdfs - else robot_spec.left_primitive_cfg - ).replace(prim_path="{ENV_REGEX_NS}/LeftRobot") - if hasattr(env_cfg, "events") and hasattr( - env_cfg.events, "setup_collision_groups" - ): - env_cfg.events.setup_collision_groups.params["robot_names"].append( - "RightRobot" - ) - env_cfg.events.setup_collision_groups.params["robot_names"].append( - "LeftRobot" - ) - elif robot_spec.robot_cfg is not None: - env_cfg.scene.robot = _maybe_disable_gravity(robot_spec.robot_cfg).replace( - prim_path="{ENV_REGEX_NS}/Robot" - ) - if hasattr(env_cfg, "events") and hasattr( - env_cfg.events, "setup_collision_groups" - ): - env_cfg.events.setup_collision_groups.params["robot_names"].append("Robot") - - -def _is_sequence_robot_partition_path(path: str) -> bool: - parts = Path(path).parts - return any(p.startswith("sequence_id=") for p in parts) and any( - p.startswith("robot_name=") for p in parts - ) - - -def apply_scene_commands(env_cfg: Any, scene_config: SceneConfig) -> None: - """Configure the dual-hand tracking command from scene_config fields.""" - if scene_config.robot_name is None: - raise ValueError("robot_name not set — cannot configure commands") - - robot_spec = get_robot_spec(scene_config.robot_name) - if robot_spec is None: - raise ValueError(f"Unknown robot: {scene_config.robot_name}") - - motion_path = Path(scene_config.motion_file) - motion_filters: list[tuple[str, str, str]] - if _is_sequence_robot_partition_path(scene_config.motion_file): - motion_folder = str(motion_path if motion_path.is_dir() else motion_path.parent) - motion_filters = [] - else: - motion_folder = scene_config.motion_folder or os.path.dirname( - scene_config.motion_file - ) - motion_filters = scene_config.motion_filters or [] - - # Scene attribute names for the command term to look up objects - object_body_names = [obj.name for obj in scene_config.scene_objects] - - cmd = env_cfg.commands.dual_hands_object_tracking_command - cmd.wrist_joint_names = robot_spec.wrist_joint_names - cmd.finger_joint_names = robot_spec.finger_joint_names - cmd.wrist_body_name = robot_spec.wrist_body_name - cmd.fingertip_body_name = robot_spec.fingertip_body_name - cmd.object_body_names = object_body_names - cmd.motion_folder = motion_folder - cmd.motion_filters = motion_filters - - -def apply_scene_contact_sensors(env_cfg: Any, scene_config: SceneConfig) -> None: - """Set up per-side contact sensors between robot hands and the object.""" - if scene_config.robot_name is None: - raise ValueError("robot_name not set — cannot configure contacts") - - robot_spec = get_robot_spec(scene_config.robot_name) - if robot_spec is None: - raise ValueError(f"Unknown robot: {scene_config.robot_name}") - - right_robot_filter_prim_paths = [ - f"{{ENV_REGEX_NS}}/RightRobot/{b.replace('.*', 'right')}" - for b in robot_spec.hand_contact_bodies - ] - left_robot_filter_prim_paths = [ - f"{{ENV_REGEX_NS}}/LeftRobot/{b.replace('.*', 'left')}" - for b in robot_spec.hand_contact_bodies - ] - - # Contact sensor on the object body-hand pairs - env_cfg.object_to_hand_contact_sensor_names = [] - - for object in scene_config.scene_objects: - object_name = object.name - if isinstance(object, ArticulatedObjectConfig): - object_body_names = object.body_names - else: - object_body_names = ["object"] # URDF link name for rigid objects - - for body_name in object_body_names: - for side in ["right", "left"]: - sensor_name = f"{object_name}_{body_name}_to_{side}_hand_contact_sensor" - setattr( - env_cfg.scene, - sensor_name, - ContactSensorCfg( - prim_path=f"{{ENV_REGEX_NS}}/{object_name}/{body_name}", - track_pose=True, - debug_vis=False, - force_threshold=0.1, - history_length=3, - filter_prim_paths_expr=( - right_robot_filter_prim_paths - if side == "right" - else left_robot_filter_prim_paths - ), - track_contact_points=True, - track_air_time=True, - max_contact_data_count_per_prim=128, - ), - ) - env_cfg.object_to_hand_contact_sensor_names.append(sensor_name) - - -def apply_scene_config( - env_cfg: Any, scene_config: SceneConfig, use_primitive_urdfs: bool = False -) -> Any: - """Apply scene config: objects + robot + commands + contacts. - - Supports both dual-hands (V2P) and whole-body envs. Skips commands/contacts - if the env_cfg doesn't have the required fields (e.g. scene viewer). - """ - apply_scene_objects(env_cfg, scene_config) - apply_scene_virtual_object_controls(env_cfg, scene_config) - - is_dual_hands = hasattr(env_cfg, "commands") and hasattr( - env_cfg.commands, "dual_hands_object_tracking_command" - ) - is_whole_body = hasattr(env_cfg, "commands") and hasattr(env_cfg.commands, "motion") - - # V2P dual-hands: spawn robot + configure commands/contacts - if is_dual_hands: - if scene_config.robot_name: - apply_scene_robot( - env_cfg, scene_config, use_primitive_urdfs=use_primitive_urdfs - ) - apply_scene_commands(env_cfg, scene_config) - apply_scene_contact_sensors(env_cfg, scene_config) - env_cfg.episode_length_s = ( - scene_config.episode_length_s - / env_cfg.commands.dual_hands_object_tracking_command.motion_speed - ) - - # Whole-body: robot + actions/obs configured by env cfg, just set motion file - elif is_whole_body: - env_cfg.commands.motion.motion_file = scene_config.motion_file - object_attr_names = [obj.name for obj in scene_config.scene_objects] - if object_attr_names: - env_cfg.commands.motion.object_name = object_attr_names[0] - env_cfg.commands.motion.object_body_names = object_attr_names - whole_body_step_dt = float(env_cfg.sim.dt) * int(env_cfg.decimation) - reset_freeze_steps = int( - getattr(env_cfg.commands.motion, "reset_freeze_steps", 0) - ) - whole_body_episode_length_s = ( - scene_config.episode_length_s + reset_freeze_steps * whole_body_step_dt - ) - env_cfg.episode_length_s = min( - env_cfg.episode_length_s, whole_body_episode_length_s - ) - - # Contact sensors for whole-body - hand_contact_bodies = getattr( - env_cfg.commands.motion, "hand_contact_bodies", [] - ) - if hand_contact_bodies: - contact_sensor_names = [] - for obj in scene_config.scene_objects: - obj_name = obj.name - body_names = ( - obj.body_names - if isinstance(obj, ArticulatedObjectConfig) - else ["object"] - ) - for body_name in body_names: - for side in ["right", "left"]: - filter_prims = [ - f"{{ENV_REGEX_NS}}/Robot/{b.replace('.*', side)}" - for b in hand_contact_bodies - ] - sensor_name = f"{obj_name}_{body_name}_to_{side}_contact_sensor" - setattr( - env_cfg.scene, - sensor_name, - ContactSensorCfg( - prim_path=f"{{ENV_REGEX_NS}}/{obj_name}/{body_name}", - track_pose=True, - debug_vis=False, - force_threshold=0.1, - history_length=3, - filter_prim_paths_expr=filter_prims, - track_contact_points=True, - track_air_time=True, - max_contact_data_count_per_prim=128, - ), - ) - contact_sensor_names.append(sensor_name) - env_cfg.commands.motion.object_contact_sensor_names = contact_sensor_names - - # FrameTransformers for hand-object observations - hand_targets = getattr(env_cfg.commands.motion, "hand_frame_target_bodies", []) - if hand_targets: - obj = scene_config.scene_objects[0] - obj_name = obj.name - body_name = ( - obj.body_names[0] - if isinstance(obj, ArticulatedObjectConfig) - else "object" - ) - obj_prim = f"{{ENV_REGEX_NS}}/{obj_name}/{body_name}" - for target_body in hand_targets: - side = "left" if "left" in target_body else "right" - setattr( - env_cfg.scene, - f"{side}_hand_object_transform", - FrameTransformerCfg( - prim_path=obj_prim, - target_frames=[ - FrameTransformerCfg.FrameCfg( - prim_path=f"{{ENV_REGEX_NS}}/Robot/{target_body}", - ) - ], - ), - ) - - return env_cfg diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/config/arctic_scene.yaml b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/config/arctic_scene.yaml deleted file mode 100644 index 805fed6d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/config/arctic_scene.yaml +++ /dev/null @@ -1,4 +0,0 @@ -# Change motion_file to load a different sequence. -# Object name, type, URDF, poses, and support surface are all auto-discovered. - -motion_file: source/robotic_grounding/robotic_grounding/assets/human_motion_data/arctic_processed/sequence_id=arctic_s01_waffleiron_use_02/robot_name=sharpa_wave diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/replay_data.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/replay_data.py deleted file mode 100644 index 4f41b936..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/replay_data.py +++ /dev/null @@ -1,289 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Schema-aware replay trajectory loading for scene playback scripts.""" - -from __future__ import annotations - -from dataclasses import dataclass -from pathlib import Path -from typing import Any, Literal - -import numpy as np -import pyarrow.parquet as pq -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.motion_schema import ( - DUAL_HAND, - SCHEMA_VERSION, - SINGLE_ROBOT, - MotionData, - load_motion_data_parquet, -) -from robotic_grounding.retarget.data_logger import ManoSharpaData -from robotic_grounding.tasks.scene_utils.scene_config import SceneConfig - - -@dataclass -class ObjectTrajectory: - """Object root trajectory in world coordinates.""" - - root_position: np.ndarray # (T, 3) - root_wxyz: np.ndarray # (T, 4) - - -@dataclass -class SingleRobotTrajectory: - """Canonical replay data for a single whole-body robot.""" - - schema: Literal["motion_v1"] - robot_layout: Literal["single_robot"] - fps: float - num_frames: int - robot_joint_names: list[str] - robot_root_position: np.ndarray # (T, 3) - robot_root_wxyz: np.ndarray # (T, 4) - robot_joint_positions: np.ndarray # (T, N) - object_traj: ObjectTrajectory | None - - -@dataclass -class DualHandTrajectory: - """Canonical replay data for dual floating-hand robots.""" - - schema: Literal["mano_sharpa", "motion_v1"] - robot_layout: Literal["dual_hand"] - fps: float - num_frames: int - right_joint_names: list[str] - left_joint_names: list[str] - right_wrist_position: np.ndarray # (T, 3) - left_wrist_position: np.ndarray # (T, 3) - wrist_orientation_format: Literal["wxyz", "euler_xyz"] - right_wrist_orientation: np.ndarray # (T, 4) or (T, 3) - left_wrist_orientation: np.ndarray # (T, 4) or (T, 3) - right_finger_joints: np.ndarray - left_finger_joints: np.ndarray - object_traj: ObjectTrajectory | None - - -ReplayTrajectory = SingleRobotTrajectory | DualHandTrajectory - - -def _resolve_path_and_filters( - motion_file: str, -) -> tuple[str, list[tuple[str, str, str]] | None]: - """Resolve path and infer deterministic partition filters.""" - resolved = SceneConfig._resolve_motion_file(motion_file) - partition = SceneConfig._parse_partition_path(resolved) - filters = partition.get("motion_filters") - parts = Path(resolved).parts - has_seq_partition = any(p.startswith("sequence_id=") for p in parts) - has_robot_partition = any(p.startswith("robot_name=") for p in parts) - if has_seq_partition and has_robot_partition: - filters = None - return resolved, filters - - -def _build_object_traj_from_arrays( - object_root_position: Any, - object_root_axis_angle: Any, -) -> ObjectTrajectory | None: - """Convert object root position + axis-angle arrays to replay trajectory.""" - if object_root_position is None or object_root_axis_angle is None: - return None - pos = np.asarray(object_root_position, dtype=np.float32) - aa = np.asarray(object_root_axis_angle, dtype=np.float64) - if pos.ndim != 2 or pos.shape[1] != 3: - return None - if aa.ndim != 2 or aa.shape[1] != 3: - return None - root_wxyz = np.asarray( - [R.from_rotvec(v).as_quat(scalar_first=True) for v in aa], - dtype=np.float32, - ) - return ObjectTrajectory(root_position=pos, root_wxyz=root_wxyz) - - -def _to_np(value: Any) -> np.ndarray | None: - if value is None: - return None - if isinstance(value, np.ndarray): - return value - if hasattr(value, "cpu"): - return value.cpu().numpy() - return np.asarray(value) - - -def _motion_v1_to_replay(md: MotionData) -> ReplayTrajectory: - """Map a `motion_v1` MotionData into the appropriate replay trajectory shape. - - Branches on the file's explicit `motion_kind`: - - `single_robot` → `SingleRobotTrajectory` driven by whole-body joint state. - - `dual_hand` → `DualHandTrajectory` driven by `ee_pose_w`. - """ - object_traj = _build_object_traj_from_arrays( - object_root_position=_to_np(md.object_root_position), - object_root_axis_angle=_to_np(md.object_root_axis_angle), - ) - - if md.motion_kind == SINGLE_ROBOT: - root_pos = _to_np(md.robot_root_position) - root_wxyz = _to_np(md.robot_root_wxyz) - joint_pos = _to_np(md.robot_joint_positions) - if root_pos is None or root_wxyz is None or joint_pos is None: - raise ValueError( - "motion_v1 single_robot file is missing required whole-body " - "joint tensors. Re-run the producing retarget/planner script." - ) - return SingleRobotTrajectory( - schema="motion_v1", - robot_layout="single_robot", - fps=float(md.fps), - num_frames=int(root_pos.shape[0]), - robot_joint_names=list(md.robot_joint_names), - robot_root_position=root_pos.astype(np.float32), - robot_root_wxyz=root_wxyz.astype(np.float32), - robot_joint_positions=joint_pos.astype(np.float32), - object_traj=object_traj, - ) - - if md.motion_kind != DUAL_HAND: - raise ValueError( - f"Cannot build replay trajectory: unsupported motion_kind={md.motion_kind!r}." - ) - - ee_pose = _to_np(md.ee_pose_w) - if ee_pose is None or ee_pose.ndim != 3 or ee_pose.shape[1] < 2: - raise ValueError( - "motion_v1 dual_hand file has fewer than 2 end-effector poses; cannot build a replay trajectory." - ) - left_idx, right_idx = 0, 1 - names = md.ee_link_names or [] - for i, name in enumerate(names): - lname = (name or "").lower() - if "left" in lname: - left_idx = i - elif "right" in lname: - right_idx = i - left_pos = ee_pose[:, left_idx, 0:3] - left_quat = ee_pose[:, left_idx, 3:7] - right_pos = ee_pose[:, right_idx, 0:3] - right_quat = ee_pose[:, right_idx, 3:7] - - left_fj_arr = _to_np(md.left_finger_joints) - right_fj_arr = _to_np(md.right_finger_joints) - left_fj_names = md.left_finger_joint_names or [] - right_fj_names = md.right_finger_joint_names or [] - - return DualHandTrajectory( - schema="motion_v1", - robot_layout="dual_hand", - fps=float(md.fps), - num_frames=int(right_pos.shape[0]), - right_joint_names=list(right_fj_names), - left_joint_names=list(left_fj_names), - right_wrist_position=right_pos.astype(np.float32), - left_wrist_position=left_pos.astype(np.float32), - wrist_orientation_format="wxyz", - right_wrist_orientation=right_quat.astype(np.float32), - left_wrist_orientation=left_quat.astype(np.float32), - right_finger_joints=( - right_fj_arr.astype(np.float32) - if right_fj_arr is not None - else np.zeros((0,), dtype=np.float32) - ), - left_finger_joints=( - left_fj_arr.astype(np.float32) - if left_fj_arr is not None - else np.zeros((0,), dtype=np.float32) - ), - object_traj=object_traj, - ) - - -def _sharpa_to_dual_hand(data: ManoSharpaData) -> DualHandTrajectory: - object_traj = _build_object_traj_from_arrays( - object_root_position=getattr(data, "object_root_position", None), - object_root_axis_angle=getattr(data, "object_root_axis_angle", None), - ) - right_pos = np.asarray(data.robot_right_wrist_position, dtype=np.float32) - left_pos = np.asarray(data.robot_left_wrist_position, dtype=np.float32) - return DualHandTrajectory( - schema="mano_sharpa", - robot_layout="dual_hand", - fps=float(data.fps), - num_frames=int(right_pos.shape[0]), - right_joint_names=list(data.right_robot_finger_joint_names), - left_joint_names=list(data.left_robot_finger_joint_names), - right_wrist_position=right_pos, - left_wrist_position=left_pos, - wrist_orientation_format="wxyz", - right_wrist_orientation=np.asarray( - data.robot_right_wrist_wxyz, dtype=np.float32 - ), - left_wrist_orientation=np.asarray(data.robot_left_wrist_wxyz, dtype=np.float32), - right_finger_joints=np.asarray( - data.robot_right_finger_joints, dtype=np.float32 - ), - left_finger_joints=np.asarray(data.robot_left_finger_joints, dtype=np.float32), - object_traj=object_traj, - ) - - -def load_replay_trajectory( - motion_file: str, - trajectory_id: int = 0, - start_frame: int = 0, - end_frame: int | None = None, -) -> ReplayTrajectory: - """Load replay trajectory from a motion parquet path for known schemas. - - Args: - motion_file: Parquet file or partition directory. - trajectory_id: Trajectory index (legacy ManoSharpaData only). - start_frame: First frame to keep (motion_v1 only). Default 0. - end_frame: One past the last frame (motion_v1 only). None = full. - """ - resolved, filters = _resolve_path_and_filters(motion_file) - - resolved_path = Path(resolved) - if resolved_path.is_dir(): - matches = list(resolved_path.rglob("*.parquet")) - if not matches: - raise FileNotFoundError(f"No parquet files under {resolved_path}") - first_file = matches[0] - else: - first_file = resolved_path - columns = set(pq.ParquetFile(str(first_file)).schema_arrow.names) - - # motion_v1 is the unified format; if the file declares it, use the reader. - if "schema_version" in columns: - md = load_motion_data_parquet(resolved) - if md.schema_version != SCHEMA_VERSION: - raise ValueError( - f"Unsupported motion schema version: {md.schema_version!r}. " - f"Run scripts/motion_schema/migrate_to_v1.py to upgrade." - ) - md = md.trim(start_frame, end_frame) - return _motion_v1_to_replay(md) - - # Legacy: ManoSharpaData (dual-hand V2P pipeline, not yet on motion_v1). - if { - "robot_right_wrist_position", - "robot_right_wrist_wxyz", - "robot_right_finger_joints", - "robot_left_wrist_position", - "robot_left_wrist_wxyz", - "robot_left_finger_joints", - }.issubset(columns): - data = ManoSharpaData.from_parquet( - root_path=resolved, - filters=filters, - trajectory_id=trajectory_id, - ) - return _sharpa_to_dual_hand(data) - - raise ValueError( - "Unsupported replay schema. Expected motion_v1 (run migrate_to_v1.py on " - "legacy G1/Dex3 parquets) or ManoSharpaData." - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_config.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_config.py deleted file mode 100644 index 9e1fad31..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_config.py +++ /dev/null @@ -1,485 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import os -from dataclasses import dataclass, field -from pathlib import Path -from urllib.parse import unquote - -import numpy as np -import pyarrow.parquet as pq -from scipy.spatial.transform import Rotation as R - -from robotic_grounding.assets import ASSET_DIR -from robotic_grounding.assets.object_registry import get_object_spec - -HUMAN_MOTION_DATA_DIR = os.path.join(ASSET_DIR, "human_motion_data") -URDF_DIR = os.path.join(ASSET_DIR, "urdfs") - - -@dataclass -class ObjectConfig: - """Configuration for a rigid scene object (target or fixed).""" - - name: str - usd_path: str - position_key: str = "" - quaternion_key: str = "" - scale: tuple[float, float, float] = (1.0, 1.0, 1.0) - pos_offset: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) - - init_pos: list[float] | None = None - init_rot: list[float] | None = None - - -@dataclass -class ArticulatedObjectConfig: - """Configuration for an articulated (multi-body) scene object loaded from URDF.""" - - name: str - urdf_path: str - body_names: list[str] = field(default_factory=list) - scale: tuple[float, float, float] = (1.0, 1.0, 1.0) - pos_offset: list[float] = field(default_factory=lambda: [0.0, 0.0, 0.0]) - - init_pos: list[float] | None = None - init_rot: list[float] | None = None # wxyz quaternion - - body_init_positions: list[list[float]] | None = None - body_init_rotations: list[list[float]] | None = None # wxyz quaternions - - init_joint_pos: float | None = None - - -@dataclass -class SceneConfig: - """Scene configuration auto-discovered from parquet data. - - ``scene_objects[0]`` is the primary object used for command tracking and - contact sensors. All objects are spawned into the scene. - """ - - motion_file: str - episode_length_s: float - scene_objects: list[ObjectConfig | ArticulatedObjectConfig] - fixed_objects: list[ObjectConfig] - - # Auto-discovered from partition path - robot_name: str | None = None - sequence_id: str | None = None - motion_folder: str | None = None - motion_filters: list[tuple[str, str, str]] | None = None - object_body_names: list[str] | None = None - - @classmethod - def from_motion_file(cls, motion_file: str) -> SceneConfig: - """Build a SceneConfig from a parquet motion file path. Everything is auto-discovered.""" - motion_file = cls._resolve_motion_file(motion_file) - data = pq.read_table(motion_file).to_pydict() - partition = cls._parse_partition_path(motion_file) - - # Fail fast: check required assets exist before Isaac Sim loads objects - cls._validate_assets(data, motion_file) - - object_type = cls._detect_object_type(data) - scene_objects = cls._build_scene_objects(data, object_type, motion_file) - fixed_objects = cls._build_fixed_objects(motion_file) - object_body_names = ( - data.get("safe_object_body_names", [[]])[0] - or data.get("object_body_names", [[]])[0] - or None - ) - episode_length_s = cls._build_episode_length_s(data) - - return cls( - motion_file=motion_file, - episode_length_s=episode_length_s, - scene_objects=scene_objects, - fixed_objects=fixed_objects, - robot_name=partition.get("robot_name"), - sequence_id=partition.get("sequence_id"), - motion_folder=partition.get("motion_folder"), - motion_filters=partition.get("motion_filters"), - object_body_names=object_body_names, - ) - - @staticmethod - def _resolve_motion_file(raw_path: str) -> str: - """Resolve a motion file path. - - Accepts: - - Full path to a parquet file or partitioned dir - - dataset/dataset_retargeted/sequence_id/robot_name like "arctic/arctic_processed/arctic_s01_ketchup_use_01/sharpa_wave" - """ - motion_file = raw_path - if not Path(motion_file).is_absolute(): - motion_file = str(Path.cwd() / motion_file) - - if not Path(motion_file).exists(): - parts = raw_path.strip("/").split("/") - if len(parts) == 4: - dataset, dataset_retargeted, seq_id, robot = parts - motion_file = os.path.join( - HUMAN_MOTION_DATA_DIR, - dataset, - dataset_retargeted, - f"sequence_id={seq_id}", - f"robot_name={robot}", - ) - - if not Path(motion_file).exists(): - raise FileNotFoundError( - f"Motion file not found: {raw_path} (resolved: {motion_file})" - ) - - return motion_file - - @staticmethod - def _parse_partition_path(motion_file: str) -> dict: - """Extract robot_name, sequence_id, motion_folder, motion_filters from partition path.""" - result: dict = {} - path = Path(motion_file).resolve() - for parent in [path] + list(path.parents): - name = parent.name - if name.startswith("robot_name="): - result["robot_name"] = unquote(name.split("=", 1)[1]) - elif name.startswith("sequence_id="): - result["sequence_id"] = unquote(name.split("=", 1)[1]) - result["motion_folder"] = str(parent.parent) - - if "robot_name" in result and "sequence_id" in result: - result["motion_filters"] = [ - ("robot_name", "=", result["robot_name"]), - ("sequence_id", "=", result["sequence_id"]), - ] - return result - - @staticmethod - def _detect_object_type(data: dict) -> str: - """Detect whether the scene object is articulated or rigid. - - Checks the object registry first — if the object has a urdf_path it is - articulated, regardless of whether articulation values are zero - (e.g. grab sequences where the lid never moves). - Exception: if body_names == ["object"], this is the rigid URDF link - convention used by Arctic "rigid_*" sequences, so treat as rigid even - when the registry has an art URDF. - Multiple rigid bodies (TACO tool+target, OakInk2 multi-object) have no - urdf_path in the registry and fall through to "rigid". - """ - obj_name = ( - data.get("safe_object_name", [None])[0] - or data.get("object_name", [None])[0] - ) - if obj_name: - spec = get_object_spec(obj_name) - if spec and spec.urdf_path: - body_names = ( - data.get("safe_object_body_names", [[]])[0] - or data.get("object_body_names", [[]])[0] - or [] - ) - if body_names != ["object"]: - return "articulated" - return "rigid" - - @classmethod - def _build_scene_objects( - cls, data: dict, object_type: str, motion_file: str = "" - ) -> list[ObjectConfig | ArticulatedObjectConfig]: - """Build all scene objects from parquet data. - - For articulated objects (Arctic), builds a single ArticulatedObjectConfig. - For rigid objects (TACO/OakInk2), builds one ObjectConfig per body. - """ - if object_type == "articulated": - return [cls._build_articulated_object(data)] - - body_names = ( - data.get("safe_object_body_names", [[]])[0] - or data.get("object_body_names", [[]])[0] - or [] - ) - urdf_paths = data.get("object_urdf_paths", [[]])[0] or [] - mesh_paths = data.get("object_mesh_paths", [[]])[0] or [] - obj_name = ( - data.get("safe_object_name", [None])[0] - or data.get("object_name", [None])[0] - ) - dataset_root = ( - cls._dataset_root_from_motion_file(motion_file) if motion_file else None - ) - objects: list[ObjectConfig | ArticulatedObjectConfig] = [] - - for i, body_name in enumerate(body_names): - - # Try registry first (for first body with known object name) - if i == 0 and obj_name: - spec = get_object_spec(obj_name) - if spec and (spec.rigid_urdf_path or spec.usd_path): - asset_path = spec.rigid_urdf_path or spec.usd_path - obj = ObjectConfig( - name=body_name, - usd_path=asset_path, - scale=spec.scale, - ) - _load_body_pose(data, obj, i) - objects.append(obj) - continue - - # Resolve from parquet urdf_paths - urdf_path = urdf_paths[i] if i < len(urdf_paths) else None - - # Fallback: derive URDF path from mesh path by convention - # e.g. meshes/hot3d/12345.glb -> urdfs/hot3d/12345_rigid.urdf - if not urdf_path or not Path(urdf_path).exists(): - urdf_path = cls._urdf_from_mesh_path( - mesh_paths[i] if i < len(mesh_paths) else None - ) - - # Fallback: search for URDF by filename in the motion file's dataset - if ( - (not urdf_path or not Path(urdf_path).exists()) - and dataset_root - and urdf_path - ): - dataset_urdf = cls._find_asset_in_dataset( - Path(urdf_path).name, dataset_root - ) - if dataset_urdf: - urdf_path = dataset_urdf - - assert urdf_path and Path(urdf_path).exists(), ( - f"Could not resolve rigid object for object_name='{obj_name}', " - f"body='{body_name}'. Generate URDFs with scripts/generate_rigid_urdfs.py" - ) - - obj = ObjectConfig(name=body_name, usd_path=urdf_path) - _load_body_pose(data, obj, i) - objects.append(obj) - - if not objects: - raise ValueError("No scene objects could be built from parquet data") - - return objects - - @staticmethod - def _urdf_from_mesh_path(mesh_path: str | None) -> str | None: - """Derive a rigid URDF path from an object mesh path by convention. - - Example: .../meshes/hot3d/12345.glb -> .../urdfs/hot3d/12345_rigid.urdf - """ - if not mesh_path: - return None - mesh = Path(mesh_path) - # Convention: urdfs//_rigid.urdf - # Mesh is at meshes//, URDF is at urdfs//_rigid.urdf - dataset = mesh.parent.name - urdf_path = Path(URDF_DIR) / dataset / f"{mesh.stem}_rigid.urdf" - return str(urdf_path) if urdf_path.exists() else None - - @staticmethod - def _dataset_root_from_motion_file(motion_file: str) -> Path | None: - """Derive dataset root from a partitioned motion file path. - - Walks up the path past partition dirs (key=value format) and the - sequences subfolder to reach the dataset root. - - Example: - .../v2d_taco_retarget_exp_200/taco_processed/sequence_id=.../robot_name=... - → .../v2d_taco_retarget_exp_200/ - """ - path = Path(motion_file) - prev_no_eq = False - for _ in range(10): - if path == path.parent: - return None - has_eq = "=" in path.name - if not has_eq and prev_no_eq: - return path - prev_no_eq = not has_eq - path = path.parent - return None - - @staticmethod - def _find_asset_in_dataset(filename: str, dataset_root: Path) -> str | None: - """Search for an asset file in immediate subdirectories of the dataset root.""" - if not dataset_root or not dataset_root.is_dir(): - return None - direct = dataset_root / filename - if direct.exists(): - return str(direct) - for subdir in dataset_root.iterdir(): - if not subdir.is_dir(): - continue - candidate = subdir / filename - if candidate.exists(): - return str(candidate) - return None - - @staticmethod - def _validate_assets(data: dict, motion_file: str) -> None: - """Check that required asset files exist before building the scene. - - Raises FileNotFoundError with an actionable message if any URDF or - mesh file explicitly referenced by the parquet is missing. This - catches errors early — before Isaac Sim spends time loading — - rather than crashing mid-startup. - - Note: this only validates paths stored in the parquet. Objects - resolved via the object registry or the mesh-derived URDF fallback - are validated later in ``_build_scene_objects``. - - For URDFs, falls back to searching in the motion file's dataset root - (e.g. OSMO-mounted dataset taco_urdfs/ subfolder) when the workspace - path is absent. - """ - urdf_paths = data.get("object_urdf_paths", [[]])[0] or [] - mesh_paths = data.get("object_mesh_paths", [[]])[0] or [] - missing: list[str] = [] - - dataset_root = SceneConfig._dataset_root_from_motion_file(motion_file) - - for p in urdf_paths: - if p and not Path(p).exists(): - resolved = ( - SceneConfig._find_asset_in_dataset(Path(p).name, dataset_root) - if dataset_root - else None - ) - if not resolved: - missing.append(f"URDF: {p}") - for p in mesh_paths: - if p and not Path(p).exists(): - missing.append(f"Mesh: {p}") - - if missing: - raise FileNotFoundError( - f"Missing assets for motion file {motion_file}:\n" - + "\n".join(f" - {m}" for m in missing) - + "\n\nFix: python scripts/generate_rigid_urdfs.py --dataset " - ) - - @staticmethod - def _build_articulated_object(data: dict) -> ArticulatedObjectConfig: - """Build an articulated object from parquet data and the object registry.""" - obj_name = ( - data.get("safe_object_name", [None])[0] - or data.get("object_name", [None])[0] - ) - if not obj_name: - raise ValueError("Could not discover object_name from parquet") - - spec = get_object_spec(obj_name) - if not spec or not spec.urdf_path: - raise ValueError(f"No urdf_path for '{obj_name}' — add to object registry") - - obj = ArticulatedObjectConfig(name=obj_name, urdf_path=spec.urdf_path) - _load_articulated_poses(data, obj) - return obj - - @staticmethod - def _build_fixed_objects(motion_file: str) -> list[ObjectConfig]: - """Auto-discover fixed objects (support surfaces) from the motion file path.""" - fixed: list[ObjectConfig] = [] - support_path = _discover_support_surface(motion_file) - if support_path is not None: - fixed.append( - ObjectConfig( - name="support_surface", - usd_path=support_path, - init_pos=[0.0, 0.0, 0.0], - init_rot=[1.0, 0.0, 0.0, 0.0], - ) - ) - return fixed - - @staticmethod - def _build_episode_length_s(data: dict) -> float: - """Build the episode length from the parquet data.""" - try: - timesteps = len(data.get("object_body_position", [[]])[0]) - fps = data.get("fps", [30.0])[0] - episode_length_s = float(timesteps / fps) - except Exception: - episode_length_s = 20.0 - return episode_length_s - - -# Parquet pose loading - - -def _load_articulated_poses(data: dict, obj: ArticulatedObjectConfig) -> None: - """Populate an ArticulatedObjectConfig with frame-0 poses from parquet.""" - offset = obj.pos_offset - - if not obj.body_names: - names = ( - data.get("safe_object_body_names", [[]])[0] - or data.get("object_body_names", [[]])[0] - or [] - ) - if names: - obj.body_names = list(names) - - root_pos = data.get("object_root_position") - if root_pos and root_pos[0]: - pos = list(root_pos[0][0]) - obj.init_pos = [p + o for p, o in zip(pos, offset, strict=True)] - - root_aa = data.get("object_root_axis_angle") - if root_aa and root_aa[0]: - aa = np.array(root_aa[0][0]) - obj.init_rot = R.from_rotvec(aa).as_quat(scalar_first=True).tolist() - - body_pos = data.get("object_body_position") - if body_pos and body_pos[0]: - frame0 = body_pos[0][0] - obj.body_init_positions = [ - [p + o for p, o in zip(bp, offset, strict=True)] for bp in frame0 - ] - body0_pos = list(frame0[0]) - obj.init_pos = [p + o for p, o in zip(body0_pos, offset, strict=True)] - - body_rot = data.get("object_body_wxyz") - if body_rot and body_rot[0]: - frame0 = body_rot[0][0] - obj.body_init_rotations = [list(bw) for bw in frame0] - obj.init_rot = list(frame0[0]) - - art = data.get("object_articulation") - if art and art[0]: - obj.init_joint_pos = float(art[0][0]) - - -def _load_body_pose(data: dict, obj: ObjectConfig, body_index: int) -> None: - """Load frame-0 pose for a specific body index from parquet body arrays.""" - offset = obj.pos_offset - - body_pos = data.get("object_body_position") - if body_pos and body_pos[0]: - frame0 = body_pos[0][0] - if body_index < len(frame0): - pos = list(frame0[body_index]) - obj.init_pos = [p + o for p, o in zip(pos, offset, strict=True)] - - body_rot = data.get("object_body_wxyz") - if body_rot and body_rot[0]: - frame0 = body_rot[0][0] - if body_index < len(frame0): - obj.init_rot = list(frame0[body_index]) - - -def _discover_support_surface(motion_file: str) -> str | None: - """Find reconstructed support surface USDA from partitioned parquet path.""" - path = Path(motion_file).resolve() - for parent in [path] + list(path.parents): - if parent.name.startswith("sequence_id="): - seq_id = parent.name.split("=", 1)[1] - stage_dir = parent.parent.parent / "reconstructed_stage" - support_path = stage_dir / f"{seq_id}_support.usda" - if support_path.exists(): - return str(support_path) - return None - return None diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_viewer_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_viewer_env_cfg.py deleted file mode 100644 index 2f221096..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/scene_utils/scene_viewer_env_cfg.py +++ /dev/null @@ -1,192 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Minimal env config for verifying object spawning from a scene motion file. - -Uses ManagerBasedRLEnvCfg (with empty rewards/terminations) so the env can -be created via ``gym.make()`` and wrapped with ``RecordVideo`` for MP4 export. -""" - -from __future__ import annotations - -import isaaclab.sim as sim_utils -import isaaclab.terrains as terrain_gen -import torch -from isaaclab.assets import ArticulationCfg, AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.utils import configclass - -from robotic_grounding.tasks.scene_utils import ( - SceneConfig, - apply_scene_objects, - apply_scene_robot, -) -from robotic_grounding.tasks.scene_utils.replay_data import ( - SingleRobotTrajectory, - load_replay_trajectory, -) -from robotic_grounding.tasks.v2p import mdp - - -def _dummy_obs(env: object) -> torch.Tensor: - return torch.zeros(env.num_envs, 1, device=env.device) # type: ignore[attr-defined] - - -def _zero_reward(env: object) -> torch.Tensor: - return torch.zeros(env.num_envs, device=env.device) # type: ignore[attr-defined] - - -def _never_done(env: object) -> torch.Tensor: - return torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) # type: ignore[attr-defined] - - -def _seed_robot_init_state_from_motion(env_cfg: object, motion_file: str) -> None: - """Seed `env_cfg.scene.robot.init_state` from the saved frame-0 robot pose. - - Without this, the viewer spawns the URDF at the asset default pose - (e.g. world origin facing +X) and lets gravity drop it. The retargeted - object trajectory is stored in the same world frame as the retargeted - robot, so re-zeroing the robot rotates the relative robot/object - heading and the object can end up behind a forward-facing URDF when - the original body had a different yaw. - """ - try: - replay = load_replay_trajectory(motion_file) - except Exception: # noqa: BLE001 -- viewer-only path, missing fields are non-fatal - return - if not isinstance(replay, SingleRobotTrajectory): - return - if not hasattr(env_cfg.scene, "robot") or env_cfg.scene.robot is None: # type: ignore[attr-defined] - return - - pos = tuple(float(v) for v in replay.robot_root_position[0].tolist()) - rot = tuple(float(v) for v in replay.robot_root_wxyz[0].tolist()) - joint_names = list(replay.robot_joint_names) - joint_pos_arr = replay.robot_joint_positions[0].tolist() - joint_pos = { - name: float(value) - for name, value in zip(joint_names, joint_pos_arr, strict=True) - } - robot_cfg: ArticulationCfg = env_cfg.scene.robot # type: ignore[attr-defined] - env_cfg.scene.robot = robot_cfg.replace( # type: ignore[attr-defined] - init_state=ArticulationCfg.InitialStateCfg( - pos=pos, - rot=rot, - joint_pos=joint_pos, - joint_vel={".*": 0.0}, - ), - ) - - -@configclass -class SceneViewerSceneCfg(InteractiveSceneCfg): - """Scene with terrain and lights for the scene viewer.""" - - terrain = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", terrain_type="plane", debug_vis=False - ) - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - sky_light = AssetBaseCfg( - prim_path="/World/skyLight", - spawn=sim_utils.DomeLightCfg(color=(0.13, 0.13, 0.13), intensity=1000.0), - ) - - -@configclass -class SceneViewerObsCfg: - """Observation config with a single dummy term.""" - - @configclass - class PolicyCfg(ObsGroup): - """Single dummy observation group.""" - - dummy = ObsTerm(func=_dummy_obs) - - def __post_init__(self) -> None: - """Post-init: disable corruption and concatenate terms.""" - self.enable_corruption = False - self.concatenate_terms = True - - policy: PolicyCfg = PolicyCfg() - - -@configclass -class SceneViewerActionsCfg: - """Empty actions config for passive scene viewing.""" - - -@configclass -class SceneViewerEventCfg: - """Prestartup collision group setup; same contract as ``apply_scene_objects`` expects.""" - - setup_collision_groups = EventTerm( - func=mdp.configure_collision_groups, - mode="prestartup", - params={ - "robot_names": [], - "object_names": [], - "fixed_object_names": [], - "disable_robot_to_object_collisions": False, - "disable_robot_to_fixed_object_collisions": True, - }, - ) - - -@configclass -class SceneViewerRewardsCfg: - """No-op reward for passive scene viewing.""" - - dummy = RewTerm(func=_zero_reward, weight=0.0) - - -@configclass -class SceneViewerTerminationsCfg: - """Never terminate for passive scene viewing.""" - - dummy = DoneTerm(func=_never_done) - - -@configclass -class SceneViewerEnvCfg(ManagerBasedRLEnvCfg): - """Spawns object + support surface from a SceneConfig. No policy, no RL reward. - - Extends ManagerBasedRLEnvCfg so the env can be created with ``gym.make()`` - and wrapped with ``gym.wrappers.RecordVideo`` for MP4 export. Keeps an - event cfg so ``apply_scene_robot`` can register robot names for - collision-group configuration. - """ - - scene: SceneViewerSceneCfg = SceneViewerSceneCfg( - num_envs=1, - env_spacing=3.0, - replicate_physics=False, - ) - observations: SceneViewerObsCfg = SceneViewerObsCfg() - actions: SceneViewerActionsCfg = SceneViewerActionsCfg() - events: SceneViewerEventCfg = SceneViewerEventCfg() - rewards: SceneViewerRewardsCfg = SceneViewerRewardsCfg() - terminations: SceneViewerTerminationsCfg = SceneViewerTerminationsCfg() - - episode_length_s: float = 1000.0 # effectively infinite for passive viewing - motion_file: str | None = None - - def __post_init__(self) -> None: - """Post-init: configure simulation and load scene from motion file.""" - self.decimation = 1 - self.sim.dt = 0.01 - self.sim.render_interval = 1 - - if self.motion_file is not None: - scene_config = SceneConfig.from_motion_file(self.motion_file) - apply_scene_objects(self, scene_config) - if scene_config.robot_name is not None: - apply_scene_robot(self, scene_config, static=False) - _seed_robot_init_state_from_motion(self, self.motion_file) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/__init__.py deleted file mode 100644 index b7610a3f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/__init__.py +++ /dev/null @@ -1,85 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import gymnasium as gym - -from . import ( - agents, - sharpa_v2p_auto_curr_env_cfg, - sharpa_v2p_direct_env_cfg, - sharpa_v2p_env_cfg, - sharpa_v2p_tracking_env_cfg, -) - -################################################# -# Register Gym environments. -################################################# - -gym.register( - id="Sharpa-V2P-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_env_cfg.SharpaV2PEnvCfg, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-AutoCurr-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_auto_curr_env_cfg.SharpaV2PAutoCurrEnvCfg, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-v0-Play", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_env_cfg.SharpaV2PEnvCfgPlay, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-Direct-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_direct_env_cfg.SharpaV2PDirectEnvCfg, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PDirectPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-Direct-v0-Play", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_direct_env_cfg.SharpaV2PDirectEnvCfgPlay, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PDirectPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-Tracking-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_tracking_env_cfg.SharpaV2PTrackingEnvCfg, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PPPORunnerCfg", - }, -) - -gym.register( - id="Sharpa-V2P-Tracking-v0-Play", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": sharpa_v2p_tracking_env_cfg.SharpaV2PTrackingEnvCfgPlay, - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:SharpaV2PPPORunnerCfg", - }, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/rsl_rl_ppo_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/rsl_rl_ppo_cfg.py deleted file mode 100644 index 37573e2d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/agents/rsl_rl_ppo_cfg.py +++ /dev/null @@ -1,74 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import ( - RslRlOnPolicyRunnerCfg, - RslRlPpoActorCriticCfg, - RslRlPpoAlgorithmCfg, -) - - -@configclass -class SharpaV2PPPORunnerCfg(RslRlOnPolicyRunnerCfg): - """PPO runner configuration for the Sharpa V2P environment.""" - - num_steps_per_env = 24 - max_iterations = 20000 - save_interval = 200 - experiment_name = "sharpa_v2p" - empirical_normalization = True - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.1, - actor_hidden_dims=[1024, 512, 256, 128], - critic_hidden_dims=[1024, 512, 256, 128], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.1, - entropy_coef=0.001, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=1.0e-3, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.005, - max_grad_norm=1.0, - ) - logger = "wandb" - wandb_project = "v2p_hands" - - -@configclass -class SharpaV2PDirectPPORunnerCfg(RslRlOnPolicyRunnerCfg): - """PPO runner configuration for the Sharpa V2P direct environment.""" - - num_steps_per_env = 24 - max_iterations = 5000 - save_interval = 200 - experiment_name = "sharpa_v2p" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[1024, 512, 256, 128], - critic_hidden_dims=[1024, 512, 256, 128], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.005, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=1.0e-3, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.01, - max_grad_norm=1.0, - ) - logger = "wandb" - wandb_project = "v2p_hands_direct" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_auto_curr_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_auto_curr_env_cfg.py deleted file mode 100644 index 78ee7983..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_auto_curr_env_cfg.py +++ /dev/null @@ -1,18 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from isaaclab.utils import configclass - -from robotic_grounding.tasks.v2p.v2p_hand_env_cfg import CurriculumCfg, V2PHandEnvCfg - -_DEFAULT_MOTION_FILE = "arctic_processed/arctic_s01_box_grab_01/sharpa_wave" - - -@configclass -class SharpaV2PAutoCurrEnvCfg(V2PHandEnvCfg): - """Sharpa V2P environment using VirtualObjectControlCurriculum (adaptive gates).""" - - motion_file: str = _DEFAULT_MOTION_FILE - curriculum: CurriculumCfg = CurriculumCfg() - - def __post_init__(self) -> None: # noqa: D105 - super().__post_init__() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_direct_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_direct_env_cfg.py deleted file mode 100644 index fad534ac..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_direct_env_cfg.py +++ /dev/null @@ -1,64 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from isaaclab.utils import configclass - -from robotic_grounding.tasks.v2p import mdp -from robotic_grounding.tasks.v2p.v2p_hand_env_cfg import V2PHandEnvCfg - -_DEFAULT_MOTION_FILE = "arctic_processed/arctic_s01_box_grab_01/sharpa_wave" - - -@configclass -class SharpaV2PDirectEnvCfg(V2PHandEnvCfg): - """Sharpa V2P environment with direct position control (no tracking controller). - - The policy directly outputs PD targets instead of residuals on top - of a reference-tracking controller. Wrist targets accumulate deltas; - finger targets are set directly. - """ - - motion_file: str = _DEFAULT_MOTION_FILE - - def __post_init__(self) -> None: - """Post initialization.""" - super().__post_init__() - - # Replace residual action terms with direct position action terms. - # Attribute names are kept the same so observation/reward references still work. - self.actions.right_joint_residual_action = mdp.JointDirectPositionActionCfg( - asset_name="right_robot", - joint_names=[".*"], - tracking_controller_linear_stiffness=1000.0, - tracking_controller_linear_damping=100.0, - tracking_controller_angular_stiffness=40.0, - tracking_controller_angular_damping=0.01, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - finger_joint_clip=100.0, - ema_factor=0.0, - ) - - self.actions.left_joint_residual_action = mdp.JointDirectPositionActionCfg( - asset_name="left_robot", - joint_names=[".*"], - tracking_controller_linear_stiffness=1000.0, - tracking_controller_linear_damping=100.0, - tracking_controller_angular_stiffness=40.0, - tracking_controller_angular_damping=0.01, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - finger_joint_clip=100.0, - ema_factor=0.0, - ) - - -@configclass -class SharpaV2PDirectEnvCfgPlay(SharpaV2PDirectEnvCfg): - """Configuration for playing.""" - - def __post_init__(self) -> None: - """Post initialization.""" - super().__post_init__() - self.scene.num_envs = 16 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_env_cfg.py deleted file mode 100644 index 1154b811..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_env_cfg.py +++ /dev/null @@ -1,28 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from isaaclab.utils import configclass - -from robotic_grounding.tasks.v2p.v2p_hand_env_cfg import V2PHandEnvCfg - -_DEFAULT_MOTION_FILE = "arctic_processed/arctic_s01_box_grab_01/sharpa_wave" - - -@configclass -class SharpaV2PEnvCfg(V2PHandEnvCfg): - """Configuration for the Sharpa V2P environment.""" - - motion_file: str = _DEFAULT_MOTION_FILE - - def __post_init__(self) -> None: - """Post-init.""" - super().__post_init__() - - -@configclass -class SharpaV2PEnvCfgPlay(SharpaV2PEnvCfg): - """Configuration for the Sharpa V2P environment for playing.""" - - def __post_init__(self) -> None: - """Post-init: reduce num_envs for interactive play.""" - super().__post_init__() - self.scene.num_envs = 16 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_tracking_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_tracking_env_cfg.py deleted file mode 100644 index 1a5f0a2b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/config/sharpa_wave/sharpa_v2p_tracking_env_cfg.py +++ /dev/null @@ -1,61 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Sharpa Wave hand-only tracking environment configuration (no object).""" - -import os - -from isaaclab.utils import configclass - -from robotic_grounding.assets import ASSET_DIR -from robotic_grounding.assets.sharpa_wave import ( - FINGER_JOINTS, - FINGERTIP_BODY_NAME, - LEFT_SHARPA_WAVE_CFG, - RIGHT_SHARPA_WAVE_CFG, - WRIST_BODY_NAME, - WRIST_JOINTS, -) -from robotic_grounding.tasks.v2p.v2p_hand_tracking_env_cfg import V2PHandTrackingEnvCfg - - -@configclass -class SharpaV2PTrackingEnvCfg(V2PHandTrackingEnvCfg): - """Configuration for the Sharpa V2P tracking-only environment (no object).""" - - def __post_init__(self) -> None: - """Post initialization.""" - super().__post_init__() - - # Set robots - self.scene.right_robot = RIGHT_SHARPA_WAVE_CFG.replace( - prim_path="{ENV_REGEX_NS}/RightRobot" - ) - self.scene.left_robot = LEFT_SHARPA_WAVE_CFG.replace( - prim_path="{ENV_REGEX_NS}/LeftRobot" - ) - - # Set commands - self.commands.dual_hands_tracking_command.motion_folder = os.path.join( - ASSET_DIR, "human_motion_data", "arctic", "arctic_processed" - ) - self.commands.dual_hands_tracking_command.wrist_joint_names = WRIST_JOINTS - self.commands.dual_hands_tracking_command.finger_joint_names = FINGER_JOINTS - self.commands.dual_hands_tracking_command.wrist_body_name = WRIST_BODY_NAME - self.commands.dual_hands_tracking_command.fingertip_body_name = ( - FINGERTIP_BODY_NAME - ) - self.commands.dual_hands_tracking_command.motion_filters = [ - ("robot_name", "=", "sharpa_wave"), - ("sequence_id", "contains", "box_grab"), - ] - self.commands.dual_hands_tracking_command.motion_id = 0 - - -@configclass -class SharpaV2PTrackingEnvCfgPlay(SharpaV2PTrackingEnvCfg): - """Configuration for the Sharpa V2P tracking-only environment for playing.""" - - def __post_init__(self) -> None: - """Post initialization.""" - super().__post_init__() - self.scene.num_envs = 16 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/__init__.py deleted file mode 100644 index 32d949a2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/__init__.py +++ /dev/null @@ -1,14 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Contains the functions that are specific to the locomotion environments.""" - -from isaaclab.envs.mdp import * # noqa: F403 - -from .actions import * # noqa: F403 -from .commands.commands import * # noqa: F403 -from .commands.commands_cfg import * # noqa: F403 -from .curriculum import * # noqa: F403 -from .events import * # noqa: F403 -from .observations import * # noqa: F403 -from .rewards import * # noqa: F403 -from .terminations import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/__init__.py deleted file mode 100644 index 02f1389d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from .actions_cfg import * # noqa: F401, F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_chunks.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_chunks.py deleted file mode 100644 index cabfe6a4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_chunks.py +++ /dev/null @@ -1,216 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from __future__ import annotations - -import logging -from collections.abc import Sequence -from typing import TYPE_CHECKING - -import isaaclab.utils.string as string_utils -import torch -from isaaclab.assets.articulation import Articulation -from isaaclab.managers.action_manager import ActionTerm - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor - - from . import actions_cfg - -# import logger -logger = logging.getLogger(__name__) - - -class JointPositionActionChunk(ActionTerm): - """Joint action chunk term that applies the action chunk to the articulation's joints.""" - - cfg: actions_cfg.JointPositionActionChunkCfg - """The configuration of the action term.""" - _asset: Articulation - """The articulation asset on which the action term is applied.""" - _scale: torch.Tensor | float - """The scaling factor applied to the input action.""" - _offset: torch.Tensor | float - """The offset applied to the input action.""" - _clip: torch.Tensor - """The clip applied to the input action.""" - - def __init__( - self, cfg: actions_cfg.JointPositionActionChunkCfg, env: ManagerBasedEnv - ) -> None: - """Initialize the joint position action chunk term.""" - # initialize the action term - super().__init__(cfg, env) - - # resolve the joints over which the action term is applied - self._joint_ids, self._joint_names = self._asset.find_joints( - self.cfg.joint_names, preserve_order=self.cfg.preserve_order - ) - self._num_joints = len(self._joint_ids) - self._horizon = self.cfg.horizon - - # log the resolved joint names for debugging - logger.info( - f"Resolved joint names for the action term {self.__class__.__name__}:" - f" {self._joint_names} [{self._joint_ids}]" - ) - - # Avoid indexing across all joints for efficiency - if self._num_joints == self._asset.num_joints and not self.cfg.preserve_order: - self._joint_ids = slice(None) - - # create tensors for raw and processed actions - self._raw_actions = torch.zeros( - self.num_envs, self._horizon, self.action_dim, device=self.device - ) - self._processed_actions = torch.zeros_like(self.raw_actions) - self._prev_targets = self._asset.data.joint_pos[:, self._joint_ids] - self._action_chunk_execution_counter = torch.zeros( - self.num_envs, device=self.device - ) - - # parse scale - if isinstance(cfg.scale, (float, int)): - self._scale = float(cfg.scale) - elif isinstance(cfg.scale, dict): - self._scale = torch.ones(self.num_envs, self.action_dim, device=self.device) - # resolve the dictionary config - index_list, _, value_list = string_utils.resolve_matching_names_values( - self.cfg.scale, self._joint_names - ) - self._scale[:, index_list] = torch.tensor(value_list, device=self.device) - else: - raise ValueError( - f"Unsupported scale type: {type(cfg.scale)}. Supported types are float and dict." - ) - - # parse offset - if isinstance(cfg.offset, (float, int)): - self._offset = float(cfg.offset) - elif isinstance(cfg.offset, dict): - self._offset = torch.zeros_like(self._raw_actions) - # resolve the dictionary config - index_list, _, value_list = string_utils.resolve_matching_names_values( - self.cfg.offset, self._joint_names - ) - self._offset[:, index_list] = torch.tensor(value_list, device=self.device) - else: - raise ValueError( - f"Unsupported offset type: {type(cfg.offset)}. Supported types are float and dict." - ) - - # parse clip - if self.cfg.clip is not None: - if isinstance(cfg.clip, dict): - self._clip = torch.tensor( - [[-float("inf"), float("inf")]], device=self.device - ).repeat(self.num_envs, self.action_dim, 1) - index_list, _, value_list = string_utils.resolve_matching_names_values( - self.cfg.clip, self._joint_names - ) - self._clip[:, index_list] = torch.tensor(value_list, device=self.device) - else: - raise ValueError( - f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict." - ) - - """ - Properties. - """ - - @property - def action_dim(self) -> int: - """The dimension of the action.""" - return self._num_joints - - @property - def raw_actions(self) -> torch.Tensor: - """The raw actions from the policy.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions with the action chunk.""" - return self._processed_actions - - @property - def prev_targets(self) -> torch.Tensor: - """The previous targets of the joints.""" - return self._prev_targets - - @property - def horizon(self) -> int: - """The horizon of the action chunk.""" - return self._horizon - - @property - def IO_descriptor(self) -> GenericActionIODescriptor: # noqa: N802 - """The IO descriptor of the action term. - - This descriptor is used to describe the action term of the joint action. - It adds the following information to the base descriptor: - - joint_names: The names of the joints. - - scale: The scale of the action term. - - offset: The offset of the action term. - - clip: The clip of the action term. - - Returns: - The IO descriptor of the action term. - """ - super().IO_descriptor # noqa: B018 - self._IO_descriptor.shape = (self.action_dim,) - self._IO_descriptor.dtype = str(self.raw_actions.dtype) - self._IO_descriptor.action_type = "JointActionChunk" - self._IO_descriptor.joint_names = self._joint_names - self._IO_descriptor.scale = self._scale - # This seems to be always [4xNum_joints] IDK why. Need to check. - if isinstance(self._offset, torch.Tensor): - self._IO_descriptor.offset = self._offset[0].detach().cpu().numpy().tolist() - else: - self._IO_descriptor.offset = self._offset - # FIXME: This is not correct. Add list support. - if self.cfg.clip is not None: - if isinstance(self._clip, torch.Tensor): - self._IO_descriptor.clip = self._clip[0].detach().cpu().numpy().tolist() - else: - self._IO_descriptor.clip = self._clip - else: - self._IO_descriptor.clip = None - return self._IO_descriptor - - """ - Operations. - """ - - def process_actions(self, actions: torch.Tensor) -> None: - """Process the actions.""" - # store the raw actions - self._raw_actions[:] = actions - # apply the affine transformations - self._processed_actions = self.prev_targets + self._raw_actions * self._scale - # clip actions - if self.cfg.clip is not None: - self._processed_actions = torch.clamp( - self._processed_actions, - min=self._clip[:, :, 0], - max=self._clip[:, :, 1], - ) - # update the previous targets - self._prev_targets[:] = self._processed_actions - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Reset the joint position action chunk term.""" - self._prev_targets[env_ids] = self._asset.data.joint_pos[env_ids][ - ..., self._joint_ids - ] - self._raw_actions[env_ids] = 0.0 - - def apply_actions(self) -> None: - """Apply the actions.""" - # set position targets - self._asset.set_joint_position_target( - self.processed_actions, joint_ids=self._joint_ids - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_direct.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_direct.py deleted file mode 100644 index 4e59c97e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_direct.py +++ /dev/null @@ -1,338 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from collections.abc import Sequence -from typing import TYPE_CHECKING, Tuple - -import torch -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.math import ( - axis_angle_from_quat, - quat_apply, - quat_apply_inverse, - quat_from_euler_xyz, - quat_inv, - quat_mul, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor - - from . import actions_cfg - - -@torch.jit.script -def _process_direct_actions( - actions: torch.Tensor, - prev_actions: torch.Tensor, - raw_actions: torch.Tensor, - processed_actions: torch.Tensor, - finger_joint_limits: torch.Tensor, - scale: torch.Tensor, - clip: torch.Tensor, - ema_factor: float, -) -> None: - # Store raw policy outputs. - raw_actions[:] = actions - - # EMA-filtered, scaled, and delta-clipped. - actions = ema_factor * prev_actions + (1.0 - ema_factor) * actions * scale - actions = torch.clamp(actions, min=-clip, max=clip) - - # Clip finger targets to joint position limits. - actions[:, 6:] = torch.clamp( - actions[:, 6:], - min=finger_joint_limits[..., 0], - max=finger_joint_limits[..., 1], - ) - prev_actions[:] = actions - - # Wrist position: accumulate delta on previous target. - processed_actions[:, :3] = processed_actions[:, :3] + actions[:, :3] - - # Wrist orientation: accumulate delta rotation. - ori_delta = quat_from_euler_xyz(actions[:, 3], actions[:, 4], actions[:, 5]) - processed_actions[:, 3:7] = quat_mul(processed_actions[:, 3:7], ori_delta) - - # Finger joint targets: direct (already clipped above). - processed_actions[:, 7:] = actions[:, 6:] - - -@torch.jit.script -def _compute_wrist_wrench_direct( - wrist_position: torch.Tensor, - wrist_wxyz: torch.Tensor, - wrist_link_vel_w: torch.Tensor, - wrist_link_quat_w: torch.Tensor, - wrist_com_pos_b: torch.Tensor, - wrist_pose_target: torch.Tensor, - gravity_comp_e: torch.Tensor, - linear_stiffness: float, - linear_damping: float, - angular_stiffness: float, - angular_damping: float, - max_force: float, - max_torque: float, -) -> Tuple[torch.Tensor, torch.Tensor]: - # COM->link correction for linear velocity (mutates caller tensor, matching eager path). - wrist_link_vel_w[:, :3] += torch.linalg.cross( - wrist_link_vel_w[:, 3:], - quat_apply(wrist_link_quat_w, -wrist_com_pos_b), - dim=-1, - ) - wrist_linvel_b = quat_apply_inverse(wrist_link_quat_w, wrist_link_vel_w[:, :3]) - wrist_angvel_b = quat_apply_inverse(wrist_link_quat_w, wrist_link_vel_w[:, 3:]) - - # PD force (position in body frame). - position_error_e = wrist_pose_target[:, :3] - wrist_position - position_error_b = quat_apply_inverse(wrist_wxyz, position_error_e) - force = linear_stiffness * position_error_b - linear_damping * wrist_linvel_b - - # PD torque (orientation in body frame). - orientation_error_b = axis_angle_from_quat( - quat_mul(quat_inv(wrist_wxyz), wrist_pose_target[:, 3:7]) - ) - torque = angular_stiffness * orientation_error_b - angular_damping * wrist_angvel_b - - # Gravity compensation (world -> body). - gravity_force_b = quat_apply_inverse(wrist_wxyz, gravity_comp_e[..., :3]) - gravity_torque_b = quat_apply_inverse(wrist_wxyz, gravity_comp_e[..., 3:6]) - force = torch.clamp(force + gravity_force_b, min=-max_force, max=max_force) - torque = torch.clamp(torque + gravity_torque_b, min=-max_torque, max=max_torque) - - return force, torque - - -class JointDirectPositionAction(ActionTerm): - """Joint-space action term: policy directly outputs PD targets. - - Unlike JointResidualWithTrackingAction, there is no base tracking - controller following a reference trajectory. The policy output - (after scaling, EMA, clipping) IS the target position directly. - - Control is split by subsystem: - - - **Wrist**: Policy outputs target position and orientation. - External force and torque are applied via PD control with - gravity compensation. - - **Finger joints**: Policy outputs target joint positions. - The simulation PD controller drives the joints. - - Action dimension: 3 (wrist position) + 3 (wrist orientation, - Euler) + num_finger_joints. - """ - - cfg: actions_cfg.JointDirectPositionActionCfg - """The configuration of the action term.""" - _scale: torch.Tensor - """The scaling factor applied to the input action.""" - - def __init__( - self, cfg: actions_cfg.JointDirectPositionActionCfg, env: ManagerBasedEnv - ) -> None: - """Initialize the action term.""" - super().__init__(cfg, env) - - # Pointer to the command term and robot attributes - self.side = cfg.asset_name.split("_")[0] - self.command = env.command_manager.get_term(self.cfg.command_name) - if self.side == "right": - self.robot = self.command.right_robot - self.wrist_body_id = self.command.right_wrist_body_id - self.finger_joint_names = self.command.right_finger_joint_names - self.finger_joint_ids = self.command.right_finger_joint_ids - self._wrist_position_e = lambda: self.command.right_hand_wrist_position_e - self._wrist_wxyz_e = lambda: self.command.right_hand_wrist_wxyz_e - else: - self.robot = self.command.left_robot - self.wrist_body_id = self.command.left_wrist_body_id - self.finger_joint_names = self.command.left_finger_joint_names - self.finger_joint_ids = self.command.left_finger_joint_ids - self._wrist_position_e = lambda: self.command.left_hand_wrist_position_e - self._wrist_wxyz_e = lambda: self.command.left_hand_wrist_wxyz_e - - # Save the wrist com pose in the body frame - self.wrist_com_pose_b = self.robot.data._root_physx_view.get_coms()[:, 0].to( - self.device - ) # quat in xyzw format - - # Create tensors for policy raw and applied actions - self._raw_actions = torch.zeros( - self.num_envs, - self.action_dim, - device=self.device, - ) - self._processed_actions = torch.zeros( - self.num_envs, - self.action_dim + 1, - device=self.device, # Store wrist orientation as a quaternion (+1 dim) - ) - # Identity quaternion for wrist orientation. - self._processed_actions[:, 3] = 1.0 - # Track which envs need target re-init after reset - self._needs_target_init = torch.zeros( - self.num_envs, dtype=torch.bool, device=self.device - ) - self.prev_actions = torch.zeros( - self.num_envs, self.action_dim, device=self.device - ) - - self.wrist_forces = torch.zeros(self.num_envs, 3, device=self.device) - self.wrist_torques = torch.zeros(self.num_envs, 3, device=self.device) - self.finger_joint_pos = torch.zeros( - self.num_envs, len(self.finger_joint_ids), device=self.device - ) - self.zero_force_torque = torch.zeros(self.num_envs, 1, 3, device=self.device) - - # Parse scale - scale_tensor = torch.ones(self.num_envs, self.action_dim, device=self.device) - scale_tensor[:, :3] = cfg.wrist_position_scale - scale_tensor[:, 3:6] = cfg.wrist_orientation_scale - scale_tensor[:, 6:] = cfg.finger_joint_scale - self._scale: torch.Tensor = scale_tensor - - # Parse clip - clip_tensor = torch.ones(self.num_envs, self.action_dim, device=self.device) - clip_tensor[:, :3] = cfg.wrist_position_clip - clip_tensor[:, 3:6] = cfg.wrist_orientation_clip - clip_tensor[:, 6:] = cfg.finger_joint_clip - self._clip: torch.Tensor = clip_tensor - - # Parse EMA decay factor - self.ema_factor = float(cfg.ema_factor) - - # Set stiffness and damping for the PD controller - self._tracking_controller_linear_stiffness = float( - self.cfg.tracking_controller_linear_stiffness - ) - self._tracking_controller_linear_damping = float( - self.cfg.tracking_controller_linear_damping - ) - self._tracking_controller_angular_stiffness = float( - self.cfg.tracking_controller_angular_stiffness - ) - self._tracking_controller_angular_damping = float( - self.cfg.tracking_controller_angular_damping - ) - - """ - Properties. - """ - - @property - def action_dim(self) -> int: - """The dimension of the action.""" - return ( - len(self.finger_joint_ids) + 3 + 3 - ) # finger joints + wrist position + wrist Euler rotation - - @property - def raw_actions(self) -> torch.Tensor: - """The raw actions from the policy.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions: target positions for PD control.""" - return self._processed_actions - - @property - def IO_descriptor(self) -> GenericActionIODescriptor: # noqa: N802 - """The IO descriptor of the action term.""" - super().IO_descriptor # noqa: B018 - self._IO_descriptor.shape = (self.action_dim,) - self._IO_descriptor.dtype = str(self.raw_actions.dtype) - self._IO_descriptor.action_type = "JointDirectPositionAction" - self._IO_descriptor.joint_names = self.finger_joint_ids - self._IO_descriptor.scale = self._scale - return self._IO_descriptor - - """ - Operations. - """ - - def process_actions(self, actions: torch.Tensor) -> None: - """Process actions: wrist accumulates deltas, fingers are direct. - - Wrist: target_t = target_{t-1} + clipped(scaled_action). - The policy accumulates deltas to overcome persistent errors. - - Fingers: scaled action is the target directly. - """ - # Initialize target for freshly reset envs. Deferred from reset() - # because action reset runs BEFORE command reset — by the time - # process_actions runs, sim buffers are fresh. - if self._needs_target_init.any(): - init_ids = self._needs_target_init.nonzero(as_tuple=False).squeeze(-1) - self._processed_actions[init_ids, :3] = self._wrist_position_e()[init_ids] - self._processed_actions[init_ids, 3:7] = self._wrist_wxyz_e()[init_ids] - self._needs_target_init[init_ids] = False - - _process_direct_actions( - actions=actions, - prev_actions=self.prev_actions, - raw_actions=self._raw_actions, - processed_actions=self._processed_actions, - finger_joint_limits=self.robot.data.joint_pos_limits[ - :, self.finger_joint_ids - ], - scale=self._scale, - clip=self._clip, - ema_factor=self.ema_factor, - ) - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Reset the action term.""" - self._raw_actions[env_ids] = 0.0 - self.prev_actions[env_ids] = 0.0 - self.wrist_forces[env_ids] = 0.0 - self.wrist_torques[env_ids] = 0.0 - self.finger_joint_pos[env_ids] = 0.0 - - # Mark envs for target re-init in process_actions - # (command reset runs AFTER action reset, so command - # reference is stale here) - self._needs_target_init[env_ids] = True - - # Clear the external forces and torques - self.robot.set_external_force_and_torque( - forces=self.zero_force_torque[env_ids], - torques=self.zero_force_torque[env_ids], - body_ids=self.wrist_body_id, - env_ids=env_ids, - is_global=False, - ) - - def apply_actions(self) -> None: - """Apply the PD control to track the target positions.""" - force, torque = _compute_wrist_wrench_direct( - wrist_position=self._wrist_position_e(), - wrist_wxyz=self._wrist_wxyz_e(), - wrist_link_vel_w=self.robot.data.root_com_vel_w, - wrist_link_quat_w=self.robot.data.root_link_quat_w, - wrist_com_pos_b=self.wrist_com_pose_b[..., :3], - wrist_pose_target=self._processed_actions[:, :7], - gravity_comp_e=self.robot.root_physx_view.get_gravity_compensation_forces(), - linear_stiffness=self._tracking_controller_linear_stiffness, - linear_damping=self._tracking_controller_linear_damping, - angular_stiffness=self._tracking_controller_angular_stiffness, - angular_damping=self._tracking_controller_angular_damping, - max_force=float(self.cfg.max_force), - max_torque=float(self.cfg.max_torque), - ) - - self.wrist_forces[:] = force - self.wrist_torques[:] = torque - self.finger_joint_pos[:] = self._processed_actions[:, 7:] - - self.robot.set_external_force_and_torque( - forces=self.wrist_forces.reshape(self.num_envs, 1, 3), - torques=self.wrist_torques.reshape(self.num_envs, 1, 3), - body_ids=self.wrist_body_id, - is_global=False, - ) - self._asset.set_joint_position_target( - self.finger_joint_pos, joint_ids=self.finger_joint_ids - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_track_residual.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_track_residual.py deleted file mode 100644 index 9b51a08b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/action_track_residual.py +++ /dev/null @@ -1,384 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import logging -from collections.abc import Sequence -from typing import TYPE_CHECKING, Tuple - -import torch -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.math import ( - axis_angle_from_quat, - quat_apply, - quat_apply_inverse, - quat_from_euler_xyz, - quat_inv, - quat_mul, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor - - from . import actions_cfg - -logger = logging.getLogger(__name__) - - -@torch.jit.script -def _process_residual_actions( - actions: torch.Tensor, - prev_actions: torch.Tensor, - raw_actions: torch.Tensor, - processed_actions: torch.Tensor, - wrist_pose_command_e: torch.Tensor, - finger_joint_pos_command: torch.Tensor, - scale: torch.Tensor, - clip: torch.Tensor, - ema_factor: float, -) -> None: - # Store raw policy outputs. - raw_actions[:] = actions - - # EMA-filtered, scaled, clipped residual. - actions = ema_factor * prev_actions + (1.0 - ema_factor) * actions * scale - actions = torch.clamp(actions, min=-clip, max=clip) - prev_actions[:] = actions - - # Wrist position target = command + residual. - processed_actions[:, :3] = wrist_pose_command_e[:, :3] + actions[:, :3] - - # Wrist orientation target = command * quat(residual euler xyz). - wrist_orientation_residual = quat_from_euler_xyz( - actions[:, 3], actions[:, 4], actions[:, 5] - ) - processed_actions[:, 3:7] = quat_mul( - wrist_pose_command_e[:, 3:7], wrist_orientation_residual - ) - - # Finger joint targets = command + residual. - processed_actions[:, 7:] = finger_joint_pos_command + actions[:, 6:] - - -@torch.jit.script -def _compute_wrist_wrench_and_finger_target( - wrist_position: torch.Tensor, - wrist_wxyz: torch.Tensor, - wrist_link_vel_w: torch.Tensor, - wrist_link_quat_w: torch.Tensor, - wrist_com_pos_b: torch.Tensor, - wrist_pose_target: torch.Tensor, - finger_joint_target: torch.Tensor, - gravity_comp_e: torch.Tensor, - joint_damping: torch.Tensor, - joint_vel: torch.Tensor, - joint_effort_limits: torch.Tensor, - joint_stiffness: torch.Tensor, - joint_pos: torch.Tensor, - linear_stiffness: float, - linear_damping: float, - angular_stiffness: float, - angular_damping: float, - max_force: float, - max_torque: float, -) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - # COM->link correction for linear velocity (mutates caller tensor, matching eager path). - wrist_link_vel_w[:, :3] += torch.linalg.cross( - wrist_link_vel_w[:, 3:], - quat_apply(wrist_link_quat_w, -wrist_com_pos_b), - dim=-1, - ) - wrist_linvel_b = quat_apply_inverse(wrist_link_quat_w, wrist_link_vel_w[:, :3]) - wrist_angvel_b = quat_apply_inverse(wrist_link_quat_w, wrist_link_vel_w[:, 3:]) - - # PD force (position in body frame). - position_error_e = wrist_pose_target[:, :3] - wrist_position - position_error_b = quat_apply_inverse(wrist_wxyz, position_error_e) - force = linear_stiffness * position_error_b - linear_damping * wrist_linvel_b - - # PD torque (orientation in body frame). - orientation_error_b = axis_angle_from_quat( - quat_mul(quat_inv(wrist_wxyz), wrist_pose_target[:, 3:7]) - ) - torque = angular_stiffness * orientation_error_b - angular_damping * wrist_angvel_b - - # Gravity compensation (world -> body). - gravity_force_b = quat_apply_inverse(wrist_wxyz, gravity_comp_e[..., :3]) - gravity_torque_b = quat_apply_inverse(wrist_wxyz, gravity_comp_e[..., 3:6]) - force = torch.clamp(force + gravity_force_b, min=-max_force, max=max_force) - torque = torch.clamp(torque + gravity_torque_b, min=-max_torque, max=max_torque) - - # Finger joint position clamp from torque limits (inlined clip_actions_to_torque_limit). - kv_times_vel = joint_damping * joint_vel - max_joint_action = ( - joint_effort_limits + kv_times_vel - ) / joint_stiffness + joint_pos - min_joint_action = ( - -joint_effort_limits + kv_times_vel - ) / joint_stiffness + joint_pos - finger_target_clipped = torch.clamp( - finger_joint_target, min=min_joint_action, max=max_joint_action - ) - - return force, torque, finger_target_clipped - - -class JointResidualWithTrackingAction(ActionTerm): - """Joint-space action term: tracking controller plus policy residual. - - A low-level tracking controller drives the hand to follow the reference - trajectory (from the command term). The policy outputs a residual that is - added to that reference; the controller then tracks reference + residual. - - Control is split by subsystem: - - - **Wrist**: Position and orientation are tracked by applying external - force and torque to the wrist link (stiffness/damping from config) with gravity compensation. - - **Finger joints**: Tracked by setting joint position targets (the - simulation PD controller drives the joints to these targets). - - Action dimension: 3 (wrist position) + 3 (wrist orientation, e.g. Euler delta) - + num_finger_joints. Scales for each block are set in the action config. - """ - - cfg: actions_cfg.JointResidualWithTrackingActionCfg - """The configuration of the action term.""" - _scale: torch.Tensor - """The scaling factor applied to the input action.""" - - def __init__( - self, cfg: actions_cfg.JointResidualWithTrackingActionCfg, env: ManagerBasedEnv - ) -> None: - """Initialize the action term.""" - # initialize the action term - super().__init__(cfg, env) - - # Pointer to the command term and robot attributes - self.side = cfg.asset_name.split("_")[0] - self.command = env.command_manager.get_term(self.cfg.command_name) - if self.side == "right": - self.robot = self.command.right_robot - self.wrist_body_id = self.command.right_wrist_body_id - self.finger_joint_names = self.command.right_finger_joint_names - self.finger_joint_ids = self.command.right_finger_joint_ids - self._wrist_position_e = lambda: self.command.right_hand_wrist_position_e - self._wrist_wxyz_e = lambda: self.command.right_hand_wrist_wxyz_e - self._wrist_pose_command_e = ( - lambda: self.command.right_hand_wrist_pose_command_e - ) - self._finger_joint_pos_command = ( - lambda: self.command.right_hand_finger_joint_pos_command - ) - else: - self.robot = self.command.left_robot - self.wrist_body_id = self.command.left_wrist_body_id - self.finger_joint_names = self.command.left_finger_joint_names - self.finger_joint_ids = self.command.left_finger_joint_ids - self._wrist_position_e = lambda: self.command.left_hand_wrist_position_e - self._wrist_wxyz_e = lambda: self.command.left_hand_wrist_wxyz_e - self._wrist_pose_command_e = ( - lambda: self.command.left_hand_wrist_pose_command_e - ) - self._finger_joint_pos_command = ( - lambda: self.command.left_hand_finger_joint_pos_command - ) - - # Save the wrist com pose in the body frame - self.wrist_com_pose_b = self.robot.data._root_physx_view.get_coms()[:, 0].to( - self.device - ) # quat in xyzw format - - # Create tensors for tracking controller, policy raw and applied actions - self._raw_actions = torch.zeros( - self.num_envs, - self.action_dim, - device=self.device, # Store wrist orientation as an Euler angle - ) - self._processed_actions = torch.zeros( - self.num_envs, - self.action_dim + 1, - device=self.device, # Store wrist orientation as a quaternion - ) - self._processed_actions[..., 3] = 1.0 # Make quaternion correct - self.prev_actions = torch.zeros( - self.num_envs, self.action_dim, device=self.device - ) - - self.wrist_forces = torch.zeros(self.num_envs, 3, device=self.device) - self.wrist_torques = torch.zeros(self.num_envs, 3, device=self.device) - self.finger_joint_pos = torch.zeros( - self.num_envs, len(self.finger_joint_ids), device=self.device - ) - self.zero_force_torque = torch.zeros(self.num_envs, 1, 3, device=self.device) - - # Parse scale - scale_tensor = torch.ones(self.num_envs, self.action_dim, device=self.device) - scale_tensor[:, :3] = cfg.wrist_position_scale - scale_tensor[:, 3:6] = cfg.wrist_orientation_scale - scale_tensor[:, 6:] = cfg.finger_joint_scale - self._scale: torch.Tensor = scale_tensor - - # Parse clip - clip_tensor = torch.ones(self.num_envs, self.action_dim, device=self.device) - clip_tensor[:, :3] = cfg.wrist_position_clip - clip_tensor[:, 3:6] = cfg.wrist_orientation_clip - clip_tensor[:, 6:] = cfg.finger_joint_clip - self._clip: torch.Tensor = clip_tensor - - # Parse EMA decay factor - self.ema_factor = float(cfg.ema_factor) - - # Set stiffness and damping for the tracking controller - self._tracking_controller_linear_stiffness = float( - self.cfg.tracking_controller_linear_stiffness - ) - self._tracking_controller_linear_damping = float( - self.cfg.tracking_controller_linear_damping - ) - self._tracking_controller_angular_stiffness = float( - self.cfg.tracking_controller_angular_stiffness - ) - self._tracking_controller_angular_damping = float( - self.cfg.tracking_controller_angular_damping - ) - - """ - Properties. - """ - - @property - def action_dim(self) -> int: - """The dimension of the action.""" - return ( - len(self.finger_joint_ids) + 3 + 3 - ) # finger joints + wrist position + wrist Euler rotation - - @property - def raw_actions(self) -> torch.Tensor: - """The raw actions from the policy.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions that are applied to the robot joints. - - This is the sum of the actions from the tracking controller and the policy. - """ - return self._processed_actions - - @property - def IO_descriptor(self) -> GenericActionIODescriptor: # noqa: N802 - """The IO descriptor of the action term. - - This descriptor is used to describe the action term of the joint action. - It adds the following information to the base descriptor: - - joint_names: The names of the joints. - - scale: The scale of the action term. - - Returns: - The IO descriptor of the action term. - """ - super().IO_descriptor # noqa: B018 - self._IO_descriptor.shape = (self.action_dim,) - self._IO_descriptor.dtype = str(self.raw_actions.dtype) - self._IO_descriptor.action_type = "JointResidualWithTrackingAction" - self._IO_descriptor.joint_names = self.finger_joint_ids - self._IO_descriptor.scale = self._scale - return self._IO_descriptor - - """ - Operations. - """ - - def clip_actions_to_torque_limit(self, actions: torch.Tensor) -> torch.Tensor: - """Clip finger joint position targets so PD torque stays within effort limits. - - The simulation uses a PD controller: τ = Kp * (q_target - q) + Kd * (v_target - v), - with v_target = 0. To keep τ in [-L, L] (L = joint effort limit), we solve for - the allowed range of q_target and clamp the given position targets into that - range. Uses the robot's current joint stiffness, damping, velocity, and position. - """ - # Compute the projected limits of the position action based on the torque limit - kv_times_vel = self.robot.data.joint_damping * self.robot.data.joint_vel - max_actions = ( - self.robot.data.joint_effort_limits + kv_times_vel - ) / self.robot.data.joint_stiffness + self.robot.data.joint_pos - min_actions = ( - -self.robot.data.joint_effort_limits + kv_times_vel - ) / self.robot.data.joint_stiffness + self.robot.data.joint_pos - - return torch.clamp( - actions, - min=min_actions, - max=max_actions, - ) - - def process_actions(self, actions: torch.Tensor) -> None: - """Process the actions.""" - _process_residual_actions( - actions=actions, - prev_actions=self.prev_actions, - raw_actions=self._raw_actions, - processed_actions=self._processed_actions, - wrist_pose_command_e=self._wrist_pose_command_e(), - finger_joint_pos_command=self._finger_joint_pos_command(), - scale=self._scale, - clip=self._clip, - ema_factor=self.ema_factor, - ) - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Reset the action term.""" - self._raw_actions[env_ids] = 0.0 - self.prev_actions[env_ids] = 0.0 - self.wrist_forces[env_ids] = 0.0 - self.wrist_torques[env_ids] = 0.0 - self.finger_joint_pos[env_ids] = 0.0 - - # Clear the external forces and torques - self.robot.set_external_force_and_torque( - forces=self.zero_force_torque[env_ids], - torques=self.zero_force_torque[env_ids], - body_ids=self.wrist_body_id, - env_ids=env_ids, - is_global=False, - ) - - def apply_actions(self) -> None: - """Apply the actions, recomputing the tracking actions to allow better performance.""" - force, torque, finger_target = _compute_wrist_wrench_and_finger_target( - wrist_position=self._wrist_position_e(), - wrist_wxyz=self._wrist_wxyz_e(), - wrist_link_vel_w=self.robot.data.root_com_vel_w, - wrist_link_quat_w=self.robot.data.root_link_quat_w, - wrist_com_pos_b=self.wrist_com_pose_b[..., :3], - wrist_pose_target=self._processed_actions[:, :7], - finger_joint_target=self._processed_actions[:, 7:], - gravity_comp_e=self.robot.root_physx_view.get_gravity_compensation_forces(), - joint_damping=self.robot.data.joint_damping, - joint_vel=self.robot.data.joint_vel, - joint_effort_limits=self.robot.data.joint_effort_limits, - joint_stiffness=self.robot.data.joint_stiffness, - joint_pos=self.robot.data.joint_pos, - linear_stiffness=self._tracking_controller_linear_stiffness, - linear_damping=self._tracking_controller_linear_damping, - angular_stiffness=self._tracking_controller_angular_stiffness, - angular_damping=self._tracking_controller_angular_damping, - max_force=float(self.cfg.max_force), - max_torque=float(self.cfg.max_torque), - ) - - self.wrist_forces[:] = force - self.wrist_torques[:] = torque - self.finger_joint_pos[:] = finger_target - - self.robot.set_external_force_and_torque( - forces=self.wrist_forces.reshape(self.num_envs, 1, 3), - torques=self.wrist_torques.reshape(self.num_envs, 1, 3), - body_ids=self.wrist_body_id, - is_global=False, - ) - self._asset.set_joint_position_target( - self.finger_joint_pos, joint_ids=self.finger_joint_ids - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/actions_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/actions_cfg.py deleted file mode 100644 index 08232a6c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/actions_cfg.py +++ /dev/null @@ -1,242 +0,0 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - - -from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg -from isaaclab.utils import configclass - -from robotic_grounding.tasks.v2p.mdp.actions.action_chunks import ( - JointPositionActionChunk, -) -from robotic_grounding.tasks.v2p.mdp.actions.action_direct import ( - JointDirectPositionAction, -) -from robotic_grounding.tasks.v2p.mdp.actions.action_track_residual import ( - JointResidualWithTrackingAction, -) -from robotic_grounding.tasks.v2p.mdp.actions.virtual_articulated_object_control import ( - VirtualArticulatedObjectControl, -) -from robotic_grounding.tasks.v2p.mdp.actions.virtual_rigid_object_control import ( - VirtualRigidObjectControl, -) - - -@configclass -class JointPositionActionChunkCfg(ActionTermCfg): - """Configuration for the joint position action term. - - See :class:`JointPositionActionChunk` for more details. - """ - - class_type: type[ActionTerm] = JointPositionActionChunk - - joint_names: list[str] = [""] - """List of joint names or regex expressions that the action will be mapped to.""" - - scale: float | dict[str, float] = 0.1 - """Scale factor for the action (float or dict of regex expressions). Defaults to 1.0.""" - - offset: float | dict[str, float] = 0.0 - """Offset factor for the action (float or dict of regex expressions). Defaults to 0.0.""" - - num_prediction_nodes: int = 4 - """The number of prediction nodes in the action chunk. Defaults to 4.""" - - num_execution_steps: int = 100 - """The number of execution steps, expand prediction nodes to execution steps for execution. Defaults to 100.""" - - preserve_order: bool = False - """Whether to preserve the order of the joint names in the action output. Defaults to False.""" - - use_relative_actions: bool = True - """Whether to use relative actions. Defaults to True.""" - - -@configclass -class JointResidualWithTrackingActionCfg(ActionTermCfg): - """Configuration for the residual joint position with tracking controller action term. - - See :class:`JointResidualWithTrackingAction` for more details. - """ - - class_type: type[ActionTerm] = JointResidualWithTrackingAction - - joint_names: list[str] = [""] - """List of joint names or regex expressions that the action will be mapped to.""" - - command_name: str = "dual_hands_object_tracking_command" - """Name of the command to use for the action.""" - - wrist_position_scale: float = 0.05 - """Scale factor for policy's residual action for wrist position.""" - - wrist_orientation_scale: float = 0.1 - """Scale factor for policy's residual action for wrist orientation.""" - - finger_joint_scale: float = 0.1 - """Scale factor for policy's residual action for finger joints.""" - - wrist_position_clip: float = 0.2 - """Clip factor for policy's residual action for wrist position.""" - - wrist_orientation_clip: float = 1.0 - """Clip factor for policy's residual action for wrist orientation.""" - - finger_joint_clip: float = 1.0 - """Clip factor for policy's residual action for finger joints.""" - - ema_factor: float = 0.1 - """The EMA decay factor for the actions. The higher the factor, the more weight is given to the previous actions. Defaults to 0.1.""" - - preserve_order: bool = False - """Whether to preserve the order of the joint names in the action output. Defaults to False.""" - - clip_to_torque_limit: bool = True - """Whether to clip the actions to the torque limit. Defaults to True.""" - - tracking_controller_linear_stiffness: float = 100.0 - """Stiffness gain for the tracking controller in linear direction.""" - - tracking_controller_linear_damping: float = 10.0 - """Damping gain for the tracking controller in linear direction.""" - - tracking_controller_angular_stiffness: float = 50.0 - """Stiffness gain for the tracking controller in angular direction.""" - - tracking_controller_angular_damping: float = 0.0 - """Damping gain for the tracking controller in angular direction.""" - - max_force: float = 60.0 - """Maximum force for the tracking controller.""" - - max_torque: float = 60.0 - """Maximum torque for the tracking controller.""" - - -@configclass -class JointDirectPositionActionCfg(ActionTermCfg): - """Configuration for the direct joint position action term. - - See :class:`JointDirectPositionAction` for more details. - """ - - class_type: type[ActionTerm] = JointDirectPositionAction - - joint_names: list[str] = [""] - """List of joint names or regex expressions that the action will be mapped to.""" - - command_name: str = "dual_hands_object_tracking_command" - """Name of the command to use for the action.""" - - wrist_position_scale: float = 0.05 - """Scale factor for policy's action for wrist position delta.""" - - wrist_orientation_scale: float = 0.15 - """Scale factor for policy's action for wrist orientation delta.""" - - finger_joint_scale: float = 0.15 - """Scale factor for policy's action for finger joint delta.""" - - wrist_position_clip: float = 1.0 - """Clip factor for policy's action for wrist position.""" - - wrist_orientation_clip: float = 1.0 - """Clip factor for policy's action for wrist orientation.""" - - finger_joint_clip: float = 100.0 - """Clip factor for policy's action for finger joints.""" - - ema_factor: float = 0.0 - """The EMA decay factor for the actions.""" - - preserve_order: bool = False - """Whether to preserve the order of the joint names in the action output.""" - - clip_to_torque_limit: bool = True - """Whether to clip the actions to the torque limit.""" - - tracking_controller_linear_stiffness: float = 100.0 - """Stiffness gain for the PD controller in linear direction.""" - - tracking_controller_linear_damping: float = 10.0 - """Damping gain for the PD controller in linear direction.""" - - tracking_controller_angular_stiffness: float = 50.0 - """Stiffness gain for the PD controller in angular direction.""" - - tracking_controller_angular_damping: float = 0.0 - """Damping gain for the PD controller in angular direction.""" - - max_force: float = 60.0 - """Maximum force for the PD controller.""" - - max_torque: float = 60.0 - """Maximum torque for the PD controller.""" - - -@configclass -class VirtualRigidObjectControlCfg(ActionTermCfg): - """Configuration for the virtual rigid object control action term. - - See :class:`VirtualRigidObjectControl` for more details. - """ - - class_type: type[ActionTerm] = VirtualRigidObjectControl - - command_name: str = "dual_hands_object_tracking_command" - """Name of the command to use for the action.""" - - tracking_controller_linear_stiffness: float = 50.0 - """Stiffness gain for the tracking controller in linear direction.""" - - tracking_controller_linear_damping: float = 10.0 - """Damping gain for the tracking controller in linear direction.""" - - tracking_controller_angular_stiffness: float = 10.0 - """Stiffness gain for the tracking controller in angular direction.""" - - tracking_controller_angular_damping: float = 0.1 - """Damping gain for the tracking controller in angular direction.""" - - max_force: float = 60.0 - """Maximum force for the tracking controller.""" - - max_torque: float = 60.0 - """Maximum torque for the tracking controller.""" - - -@configclass -class VirtualArticulatedObjectControlCfg(ActionTermCfg): - """Configuration for the virtual articulated object control action term. - - See :class:`VirtualArticulatedObjectControl` for more details. - """ - - class_type: type[ActionTerm] = VirtualArticulatedObjectControl - - command_name: str = "dual_hands_object_tracking_command" - """Name of the command to use for the action.""" - - root_body_name: str = "bottom" - """Name of the base body of the articulated object.""" - - tracking_controller_linear_stiffness: float = 50.0 - """Stiffness gain for the tracking controller in linear direction.""" - - tracking_controller_linear_damping: float = 10.0 - """Damping gain for the tracking controller in linear direction.""" - - tracking_controller_angular_stiffness: float = 10.0 - """Stiffness gain for the tracking controller in angular direction.""" - - tracking_controller_angular_damping: float = 0.1 - """Damping gain for the tracking controller in angular direction.""" - - max_force: float = 60.0 - """Maximum force for the tracking controller.""" - - max_torque: float = 60.0 - """Maximum torque for the tracking controller.""" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_articulated_object_control.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_articulated_object_control.py deleted file mode 100644 index 90f4c9a9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_articulated_object_control.py +++ /dev/null @@ -1,285 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import logging -from collections.abc import Sequence -from typing import TYPE_CHECKING, Tuple - -import torch -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.math import ( - axis_angle_from_quat, - quat_apply, - quat_apply_inverse, - quat_inv, - quat_mul, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor - - from . import actions_cfg - -logger = logging.getLogger(__name__) - - -@torch.jit.script -def _compute_wrench_and_effort( - root_body_position_e: torch.Tensor, - root_body_wxyz: torch.Tensor, - root_link_vel_w: torch.Tensor, - root_link_quat_w: torch.Tensor, - root_com_pos_b: torch.Tensor, - command_position_e: torch.Tensor, - command_wxyz_e: torch.Tensor, - object_mass: torch.Tensor, - projected_gravity_b: torch.Tensor, - scale_factor: torch.Tensor, - command_joint_pos: torch.Tensor, - joint_pos: torch.Tensor, - joint_vel: torch.Tensor, - linear_stiffness: float, - linear_damping: float, - angular_stiffness: float, - angular_damping: float, - max_force: float, - max_torque: float, -) -> Tuple[torch.Tensor, torch.Tensor, torch.Tensor]: - # Correct COM->link offset for linear velocity (mutates caller tensor, matching eager path). - root_link_vel_w[:, :3] += torch.linalg.cross( - root_link_vel_w[:, 3:], - quat_apply(root_link_quat_w, -root_com_pos_b), - dim=-1, - ) - root_body_linvel_b = quat_apply_inverse(root_link_quat_w, root_link_vel_w[:, :3]) - root_body_angvel_b = quat_apply_inverse(root_link_quat_w, root_link_vel_w[:, 3:]) - - # PD force (position in body frame). - position_error_e = command_position_e - root_body_position_e - position_error_b = quat_apply_inverse(root_body_wxyz, position_error_e) - force = linear_stiffness * position_error_b - linear_damping * root_body_linvel_b - - # PD torque (orientation in body frame). - orientation_error_b = axis_angle_from_quat( - quat_mul(quat_inv(root_body_wxyz), command_wxyz_e) - ) - torque = ( - angular_stiffness * orientation_error_b - angular_damping * root_body_angvel_b - ) - - # Gravity compensation on the root body. - force = force + (-9.81) * object_mass * projected_gravity_b - - # Curriculum scale + clamp. - force = torch.clamp(force * scale_factor, min=-max_force, max=max_force) - torque = torch.clamp(torque * scale_factor, min=-max_torque, max=max_torque) - - # Joint PD effort (reusing linear stiffness / angular damping to match eager path). - effort = ( - linear_stiffness * (command_joint_pos - joint_pos) - angular_damping * joint_vel - ) - effort = torch.clamp(effort * scale_factor, min=-max_force, max=max_force) - - return force, torque, effort - - -class VirtualArticulatedObjectControl(ActionTerm): - """Virtual articulated object control: PD wrench on root body + joint torques. - - Applies a PD-based wrench to the root body (identical to VirtualRigidObjectControl) - to track the reference base position/orientation, then apply target joint torques to the joints - at each step. - """ - - cfg: actions_cfg.VirtualArticulatedObjectControlCfg - """The configuration of the action term.""" - - def __init__( - self, cfg: actions_cfg.VirtualArticulatedObjectControlCfg, env: ManagerBasedEnv - ) -> None: - """Initialize the action term.""" - super().__init__(cfg, env) - - # Pointer to the command term and object attribute - self.command = env.command_manager.get_term(self.cfg.command_name) - self.object_idx = self.command.cfg.object_body_names.index(cfg.asset_name) - self.object = self.command.objects[self.object_idx] - self.num_bodies = len(self.object.data.body_names) - - assert self.num_bodies == self.command.object_position_e.shape[1], ( - f"Currently only support single articulated object. " - f"Find {self.command.object_position_e.shape[1]} body in motion file, " - f"but {self.num_bodies} body in the object." - ) - assert ( - self.command.retargeted_object_body_names == self.object.data.body_names - ), ( - f"The body names in the motion file and the object do not match. " - f"Find {self.command.retargeted_object_body_names} in motion file, " - f"but {self.object.data.body_names} in the object." - ) - - # Object physical properties - self._root_body_idx = self.object.find_bodies([self.cfg.root_body_name])[0][0] - body_masses = self.object.root_physx_view.get_masses().to(self.device) - self.object_mass = body_masses.sum(dim=-1).unsqueeze(-1) # (num_envs, 1) - body_inertias = self.object.root_physx_view.get_inertias().to(self.device) - self.object_inertia = body_inertias[:, self._root_body_idx, :] # (num_envs, 9) - body_coms = self.object.root_physx_view.get_coms().to(self.device) - self.object_com = body_coms[:, self._root_body_idx, :] # (num_envs, 7) - - self.root_com_pose_b = self.object.data._root_physx_view.get_coms()[:, 0].to( - self.device - ) # quat in xyzw format - - # Joint IDs - joint_ids, _ = self.object.find_joints(".*") - self._joint_ids = joint_ids - self._num_joints = len(joint_ids) - assert ( - self._num_joints == 1 - ), "Currently only support single joint articulated object." - - # Raw/processed actions store root-body 6D wrench and joint effort - self._raw_actions = torch.zeros( - self.num_envs, 6 + self._num_joints, device=self.device - ) - self._processed_actions = torch.zeros_like(self._raw_actions) - - # Forces, torques, and efforts tensors - self._forces = torch.zeros( - self.num_envs, self.num_bodies, 3, device=self.device - ) - self._torques = torch.zeros( - self.num_envs, self.num_bodies, 3, device=self.device - ) - self._efforts = torch.zeros(self.num_envs, self._num_joints, device=self.device) - - # PD gains - self._tracking_controller_linear_stiffness = float( - self.cfg.tracking_controller_linear_stiffness - ) - self._tracking_controller_linear_damping = float( - self.cfg.tracking_controller_linear_damping - ) - self._tracking_controller_angular_stiffness = float( - self.cfg.tracking_controller_angular_stiffness - ) - self._tracking_controller_angular_damping = float( - self.cfg.tracking_controller_angular_damping - ) - - """ - Properties. - """ - - @property - def action_dim(self) -> int: - """Policy should not control the object joints.""" - return 0 - - @property - def raw_actions(self) -> torch.Tensor: - """The raw actions from the policy.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions that are applied to the object joints.""" - return self._processed_actions - - @property - def IO_descriptor(self) -> GenericActionIODescriptor: # noqa: N802 - """The IO descriptor of the action term. - - This descriptor is used to describe the action term of the joint action. - - Returns: - The IO descriptor of the action term. - """ - super().IO_descriptor # noqa: B018 - self._IO_descriptor.shape = (self.action_dim,) - self._IO_descriptor.dtype = str(self.raw_actions.dtype) - self._IO_descriptor.action_type = "VirtualArticulatedObjectControl" - self._IO_descriptor.num_bodies = self.num_bodies - return self._IO_descriptor - - """ - Operations. - """ - - def process_actions(self, actions: torch.Tensor) -> None: - """Process the actions.""" - del actions # unused, should be empty - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Reset the action term.""" - self._raw_actions[env_ids] = 0.0 - self._processed_actions[env_ids] = 0.0 - self._forces[env_ids] = 0.0 - self._torques[env_ids] = 0.0 - self._efforts[env_ids] = 0.0 - - # clear external forces, torques, and joint efforts - self.object.set_external_force_and_torque( - forces=self._forces[env_ids], - torques=self._torques[env_ids], - env_ids=env_ids, - is_global=False, - ) - self.object.set_joint_effort_target(self._efforts[env_ids], env_ids=env_ids) - self.object.write_data_to_sim() - - def apply_actions(self) -> None: - """Apply PD wrench to root body and effort to joints based on reference trajectory.""" - command_joint_pos = self.command.retargeted_object_articulation[ - self.command.timestep_counter - ].view(-1, self._num_joints) - - force, torque, effort = _compute_wrench_and_effort( - root_body_position_e=self.command.object_position_e[ - :, self._root_body_idx, : - ], - root_body_wxyz=self.command.object_orientation_e[:, self._root_body_idx, :], - root_link_vel_w=self.object.data.root_com_vel_w, - root_link_quat_w=self.object.data.root_link_quat_w, - root_com_pos_b=self.root_com_pose_b[..., :3], - command_position_e=self.command.object_body_position_command_e[ - :, self._root_body_idx, : - ], - command_wxyz_e=self.command.object_body_wxyz_command_e[ - :, self._root_body_idx, : - ], - object_mass=self.object_mass, - projected_gravity_b=self.object.data.projected_gravity_b, - scale_factor=self.command.virtual_object_controller_scale_factor_per_env, - command_joint_pos=command_joint_pos, - joint_pos=self.object.data.joint_pos, - joint_vel=self.object.data.joint_vel, - linear_stiffness=self._tracking_controller_linear_stiffness, - linear_damping=self._tracking_controller_linear_damping, - angular_stiffness=self._tracking_controller_angular_stiffness, - angular_damping=self._tracking_controller_angular_damping, - max_force=float(self.cfg.max_force), - max_torque=float(self.cfg.max_torque), - ) - - # Apply wrench to root body. - self._forces[:, self._root_body_idx] = force - self._torques[:, self._root_body_idx] = torque - self.object.set_external_force_and_torque( - forces=self._forces, - torques=self._torques, - is_global=False, - ) - - # Apply joint effort. - self.object.set_joint_effort_target(effort) - self.object.write_data_to_sim() - - self._raw_actions[..., :3] = force - self._raw_actions[..., 3:6] = torque - self._raw_actions[..., 6:] = effort - self._processed_actions = self._raw_actions diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_rigid_object_control.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_rigid_object_control.py deleted file mode 100644 index 6d2bd8fc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/actions/virtual_rigid_object_control.py +++ /dev/null @@ -1,219 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import logging -from collections.abc import Sequence -from typing import TYPE_CHECKING, Tuple - -import torch -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.math import ( - axis_angle_from_quat, - quat_apply, - quat_apply_inverse, - quat_inv, - quat_mul, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedEnv - from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor - - from . import actions_cfg - -logger = logging.getLogger(__name__) - - -@torch.jit.script -def _compute_wrench( - object_position_e: torch.Tensor, - object_wxyz: torch.Tensor, - root_link_vel_w: torch.Tensor, - root_link_quat_w: torch.Tensor, - root_com_pos_b: torch.Tensor, - command_position_e: torch.Tensor, - command_wxyz_e: torch.Tensor, - object_mass: torch.Tensor, - projected_gravity_b: torch.Tensor, - scale_factor: torch.Tensor, - linear_stiffness: float, - linear_damping: float, - angular_stiffness: float, - angular_damping: float, - max_force: float, - max_torque: float, -) -> Tuple[torch.Tensor, torch.Tensor]: - # Correct COM->link offset for linear velocity (mutates caller tensor, matching eager path). - root_link_vel_w[:, :3] += torch.linalg.cross( - root_link_vel_w[:, 3:], - quat_apply(root_link_quat_w, -root_com_pos_b), - dim=-1, - ) - object_linvel_b = quat_apply_inverse(root_link_quat_w, root_link_vel_w[:, :3]) - object_angvel_b = quat_apply_inverse(root_link_quat_w, root_link_vel_w[:, 3:]) - - # PD force (position in body frame). - position_error_e = command_position_e - object_position_e - position_error_b = quat_apply_inverse(object_wxyz, position_error_e) - force = linear_stiffness * position_error_b - linear_damping * object_linvel_b - - # PD torque (orientation in body frame). - orientation_error_b = axis_angle_from_quat( - quat_mul(quat_inv(object_wxyz), command_wxyz_e) - ) - torque = angular_stiffness * orientation_error_b - angular_damping * object_angvel_b - - # Gravity compensation. - force = force + (-9.81) * object_mass * projected_gravity_b - - # Curriculum scale + clamp. - force = torch.clamp(force * scale_factor, min=-max_force, max=max_force) - torque = torch.clamp(torque * scale_factor, min=-max_torque, max=max_torque) - - return force, torque - - -class VirtualRigidObjectControl(ActionTerm): - """Virtual rigid object control action that applies to the object's joints.""" - - cfg: actions_cfg.VirtualRigidObjectControlCfg - """The configuration of the action term.""" - - def __init__( - self, cfg: actions_cfg.VirtualRigidObjectControlCfg, env: ManagerBasedEnv - ) -> None: - """Initialize the action term.""" - # initialize the action term - super().__init__(cfg, env) - - # Pointer to the command term and object attribute - self.command = env.command_manager.get_term(self.cfg.command_name) - self.object_idx = self.command.cfg.object_body_names.index(cfg.asset_name) - self.object = self.command.objects[self.object_idx] - - self.object_mass = self.object.root_physx_view.get_masses().to( - self.device - ) # (num_envs, 1) - self.object_inertia = self.object.root_physx_view.get_inertias().to( - self.device - ) # (num_envs, 9) - self.object_com = self.object.root_physx_view.get_coms().to( - self.device - ) # (num_envs, 7) - - self.root_com_pose_b = self.object.data._root_physx_view.get_coms().to( - self.device - ) # quat in xyzw format - - # Create tensors for raw and processed actions with force and torque - self._raw_actions = torch.zeros(self.num_envs, 6, device=self.device) - self._processed_actions = torch.zeros_like(self._raw_actions) - - # Set stiffness and damping for the tracking controller - self._tracking_controller_linear_stiffness = float( - self.cfg.tracking_controller_linear_stiffness - ) - self._tracking_controller_linear_damping = float( - self.cfg.tracking_controller_linear_damping - ) - self._tracking_controller_angular_stiffness = float( - self.cfg.tracking_controller_angular_stiffness - ) - self._tracking_controller_angular_damping = float( - self.cfg.tracking_controller_angular_damping - ) - - """ - Properties. - """ - - @property - def action_dim(self) -> int: - """Policy should not control the object joints.""" - return 0 - - @property - def raw_actions(self) -> torch.Tensor: - """The raw actions from the policy.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """The processed actions that are applied to the object joints.""" - return self._processed_actions - - @property - def IO_descriptor(self) -> GenericActionIODescriptor: # noqa: N802 - """The IO descriptor of the action term. - - This descriptor is used to describe the action term of the joint action. - It adds the following information to the base descriptor: - - joint_names: The names of the joints. - - scale: The scale of the action term. - - offset: The offset of the action term. - - clip: The clip of the action term. - - Returns: - The IO descriptor of the action term. - """ - super().IO_descriptor # noqa: B018 - self._IO_descriptor.shape = (self.action_dim,) - self._IO_descriptor.dtype = str(self.raw_actions.dtype) - self._IO_descriptor.action_type = "VirtualRigidObjectControl" - self._IO_descriptor.num_bodies = self.num_bodies - return self._IO_descriptor - - """ - Operations. - """ - - def process_actions(self, actions: torch.Tensor) -> None: - """Process the actions.""" - del actions # unused, should be empty - - def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Reset the action term.""" - self._raw_actions[env_ids] = 0.0 - self._processed_actions[env_ids] = 0.0 - - # clear external forces and torques - self.object.set_external_force_and_torque( - forces=self._raw_actions[env_ids, :3].view(-1, 1, 3), - torques=self._raw_actions[env_ids, 3:].view(-1, 1, 3), - env_ids=env_ids, - is_global=False, - ) - - def apply_actions(self) -> None: - """Apply virtual force torque to the rigid object using a Position PD Controller.""" - force, torque = _compute_wrench( - object_position_e=self.command.object_position_e[:, self.object_idx], - object_wxyz=self.command.object_orientation_e[:, self.object_idx], - root_link_vel_w=self.object.data.root_com_vel_w, - root_link_quat_w=self.object.data.root_link_quat_w, - root_com_pos_b=self.root_com_pose_b[..., :3], - command_position_e=self.command.object_body_position_command_e[ - :, self.object_idx - ], - command_wxyz_e=self.command.object_body_wxyz_command_e[:, self.object_idx], - object_mass=self.object_mass, - projected_gravity_b=self.object.data.projected_gravity_b, - scale_factor=self.command.virtual_object_controller_scale_factor_per_env, - linear_stiffness=self._tracking_controller_linear_stiffness, - linear_damping=self._tracking_controller_linear_damping, - angular_stiffness=self._tracking_controller_angular_stiffness, - angular_damping=self._tracking_controller_angular_damping, - max_force=float(self.cfg.max_force), - max_torque=float(self.cfg.max_torque), - ) - - self._raw_actions[..., :3] = force - self._raw_actions[..., 3:] = torque - self._processed_actions = self._raw_actions - - self.object.set_external_force_and_torque( - forces=force.reshape(self.num_envs, 1, 3), - torques=torque.reshape(self.num_envs, 1, 3), - is_global=False, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands.py deleted file mode 100644 index 56d60d5f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands.py +++ /dev/null @@ -1,1002 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Command definitions for the V2P environment.""" - -from __future__ import annotations - -import os -from collections.abc import Sequence -from pathlib import Path -from typing import TYPE_CHECKING, Any - -import isaaclab.utils.math as math_utils -import numpy as np -import torch -from isaaclab.assets import Articulation -from isaaclab.managers import CommandTerm, CommandTermCfg -from isaaclab.markers.visualization_markers import VisualizationMarkers - -from robotic_grounding.retarget.data_logger import ManoSharpaData -from robotic_grounding.tasks.v2p.mdp.utils import ( - interpolate_robot_motion_data, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -class TrackingCommand(CommandTerm): - """Command term that generates pose commands for tracking task.""" - - cfg: CommandTermCfg - """Configuration for the command term.""" - - def __init__(self, cfg: CommandTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the command term class. - - Args: - cfg: The configuration parameters for the command term. - env: The environment object. - """ - # initialize the base class - super().__init__(cfg, env) - - # robot - self.robot: Articulation = env.scene[cfg.asset_name] - - # Load motion data - assert os.path.exists(cfg.file_path), f"Motion file not found: {cfg.file_path}" - qpos_data = torch.from_numpy(np.load(cfg.file_path)).to(self.device) - self.pos_trajectory_w = qpos_data[:, :3] + torch.tensor( - cfg.pos_offset, dtype=torch.float, device=self.device - ) - self.quat_trajectory_w = qpos_data[:, 3:7] - self.num_timesteps = len(qpos_data) - - self.joint_ids, self.joint_names = self.robot.find_joints(self.cfg.joint_names) - MUJOCO_TO_ISAAC_UPPER_BODY = [ - cfg.file_joint_order.index(joint_name) for joint_name in self.joint_names - ] - self.target_full_body_joint_pos = qpos_data[:, 7:] - self.target_upper_body_joint_pos = self.target_full_body_joint_pos[ - :, MUJOCO_TO_ISAAC_UPPER_BODY - ] - - # Load tips_distance if available (ManipTrans approach) - self.tips_distance: torch.Tensor | None = None - if ( - hasattr(cfg, "tips_distance_file_path") - and cfg.tips_distance_file_path is not None - ): - tips_dist_path = Path(cfg.tips_distance_file_path) - if tips_dist_path.exists(): - tips_data = ( - torch.from_numpy(np.load(tips_dist_path)).to(self.device).float() - ) - # Verify shape: (T, 10) - 5 per hand - if ( - tips_data.shape[0] == self.num_timesteps - and tips_data.shape[1] == 10 - ): - self.tips_distance = tips_data - else: - print( - f"Warning: tips_distance shape mismatch. " - f"Expected ({self.num_timesteps}, 10), got {tips_data.shape}" - ) - else: - print(f"Warning: tips_distance file not found: {tips_dist_path}") - - # -- buffer - self.timestep_counter = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.pos_command_e = torch.zeros(self.num_envs, 3, device=self.device) - self.quat_command_e = torch.zeros(self.num_envs, 4, device=self.device) - self.upper_body_joint_pos_command = torch.zeros( - self.num_envs, len(self.joint_names), device=self.device - ) - - # -- unit vectors - self._X_UNIT_VEC = torch.tensor([1.0, 0, 0], device=self.device).repeat( - (self.num_envs, 1) - ) - self._Y_UNIT_VEC = torch.tensor([0, 1.0, 0], device=self.device).repeat( - (self.num_envs, 1) - ) - self._Z_UNIT_VEC = torch.tensor([0, 0, 1.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.QUAT_UNIT_VEC = torch.tensor( - [1.0, 0.0, 0.0, 0.0], device=self.device - ).repeat((self.num_envs, 1)) - - # -- metrics - self.metrics["orientation_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["position_error"] = torch.zeros(self.num_envs, device=self.device) - self.metrics["joint_pos_error"] = torch.zeros(self.num_envs, device=self.device) - - def __str__(self) -> str: - """String representation of the command term.""" - msg = "TrackingCommandGenerator:\n" - msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" - return msg - - """ - Properties - """ - - @property - def command(self) -> torch.Tensor: - """The desired goal pose in the environment frame. Shape is (num_envs, 38).""" - return torch.cat( - ( - self.pos_command_e, - self.quat_command_e, - self.upper_body_joint_pos_command, - ), - dim=-1, - ) - - """ - Implementation specific functions. - """ - - def _update_metrics(self) -> None: - """Update the metrics.""" - pos_command_w = self.pos_command_e + self._env.scene.env_origins - self.metrics["position_error"] = torch.norm( - self.robot.data.root_pos_w - pos_command_w, dim=1 - ) - - self.metrics["orientation_error"] = math_utils.quat_error_magnitude( - self.robot.data.root_quat_w, self.quat_command_e - ) - - self.metrics["joint_pos_error"] = torch.norm( - self.robot.data.joint_pos[..., self.joint_ids] - - self.upper_body_joint_pos_command, - dim=1, - ) - - def _resample_command(self, env_ids: Sequence[int]) -> None: - """Resample the command.""" - # Reset the timestep counter to 0 - # FIXME: this can also be reset to a random value between 0 and num_timesteps - 1 - self.timestep_counter[env_ids] *= 0 - # Reset the command - self.pos_command_e[env_ids] = self.pos_trajectory_w[ - self.timestep_counter[env_ids] - ].float() - self.quat_command_e[env_ids] = ( - math_utils.quat_unique( - self.quat_trajectory_w[self.timestep_counter[env_ids]].float() - ) - if self.cfg.make_quat_unique - else self.quat_trajectory_w[self.timestep_counter[env_ids]].float() - ) - self.upper_body_joint_pos_command[env_ids] = self.target_upper_body_joint_pos[ - self.timestep_counter[env_ids] - ].float() - - self._reset_robot(env_ids) - - def _reset_robot(self, env_ids: Sequence[int]) -> None: - """Reset the robot.""" - # Reset the robot base - positions = self.pos_command_e[env_ids] + self._env.scene.env_origins[env_ids] - orientations = self.quat_command_e[env_ids] - self.robot.write_root_pose_to_sim( - torch.cat([positions, orientations], dim=-1), env_ids=env_ids - ) - self.robot.write_root_velocity_to_sim( - torch.zeros_like(self.robot.data.root_vel_w[env_ids]), env_ids=env_ids - ) - - # Reset the robot joints - self.robot.write_joint_state_to_sim( - self.robot.data.joint_pos[env_ids], - torch.zeros_like(self.robot.data.joint_vel[env_ids]), - env_ids=env_ids, - ) - self.robot.write_joint_state_to_sim( - self.upper_body_joint_pos_command[env_ids], - torch.zeros_like(self.robot.data.joint_vel[..., self.joint_ids][env_ids]), - joint_ids=self.joint_ids, - env_ids=env_ids, - ) - - def _update_command(self) -> None: - """Update the command.""" - if self.cfg.update_goal_on_reach: - pos_command_w = self.pos_command_e + self._env.scene.env_origins - pos_error = torch.norm(self.robot.data.root_pos_w - pos_command_w, dim=1) - if pos_error < self.cfg.goal_reach_threshold: - self.timestep_counter += 1 - else: - self.timestep_counter += 1 - - self.pos_command_e = self.pos_trajectory_w[self.timestep_counter].float() - self.quat_command_e = ( - math_utils.quat_unique( - self.quat_trajectory_w[self.timestep_counter].float() - ) - if self.cfg.make_quat_unique - else self.quat_trajectory_w[self.timestep_counter].float() - ) - self.upper_body_joint_pos_command = self.target_upper_body_joint_pos[ - self.timestep_counter - ].float() - - def get_tips_distance(self) -> torch.Tensor | None: - """Get pre-computed tips_distance for current timestep per environment. - - This follows the ManipTrans approach of using pre-computed reference distances - from MANO fingertips to object surface for contact rewards. - - Returns: - Tips distance tensor of shape (num_envs, 10) with distances for each fingertip, - or None if tips_distance data is not available. - Order: left_thumb, left_index, left_middle, left_ring, left_pinky, - right_thumb, right_index, right_middle, right_ring, right_pinky. - """ - if self.tips_distance is None: - return None - # Index by per-env timestep counter (clamped to valid range) - indices = self.timestep_counter.clamp(0, self.num_timesteps - 1) - return self.tips_distance[indices] # (num_envs, 10) - - def _set_debug_vis_impl(self, debug_vis: bool) -> None: - """Set the debug visibility.""" - # set visibility of markers - # note: parent only deals with callbacks. not their visibility - if debug_vis: - # create markers if necessary for the first time - if not hasattr(self, "goal_pose_visualizer"): - self.goal_pose_visualizer = VisualizationMarkers( - self.cfg.goal_pose_visualizer_cfg - ) - # set visibility - self.goal_pose_visualizer.set_visibility(True) - elif hasattr(self, "goal_pose_visualizer"): - self.goal_pose_visualizer.set_visibility(False) - - def _debug_vis_callback(self, event: Any) -> None: - """Visualize the goal marker.""" - del event # unused - # visualize the goal marker - self.goal_pose_visualizer.visualize( - translations=self.pos_command_e, - orientations=self.quat_command_e, - ) - - -class DualHandsTrackingCommand(CommandTerm): - """Command term that generates pose commands for dual-hand tracking WITHOUT an object. - - This is a tracking-only variant of DualHandsObjectTrackingCommand. It removes all - object-related functionality (object asset, object reset, object commands/observations, - contact sensors, contact data, virtual object controller curriculum, object-frame - transforms). Wrist and fingertip commands are expressed directly in the environment - frame using the retargeted motion data, rather than relative to an object. - """ - - cfg: CommandTermCfg - """Configuration for the command term.""" - - def __init__(self, cfg: CommandTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the dual-hand tracking command term. - - Args: - cfg: Command term configuration (motion path, asset names, FPS, etc.). - env: The RL environment instance; used to resolve scene assets. - """ - super().__init__(cfg, env) - self.step_dt = env.step_dt - - # Retrieve both side robots - for side in ["right", "left"]: - side_robot_name = getattr(cfg, f"{side}_robot_name") - side_robot = env.scene[side_robot_name] - side_finger_joint_names = side_robot.data.joint_names - finger_joint_ids, _ = side_robot.find_joints(side_finger_joint_names) - finger_joint_ids = torch.tensor(finger_joint_ids, device=self.device) - setattr(self, f"{side}_robot", side_robot) - setattr(self, f"{side}_finger_joint_names", side_finger_joint_names) - setattr(self, f"{side}_finger_joint_ids", finger_joint_ids) - - wrist_body_ids, wrist_body_name = side_robot.find_bodies( - cfg.wrist_body_name - ) - setattr( - self, - f"{side}_wrist_body_id", - torch.tensor(wrist_body_ids, device=self.device), - ) - setattr(self, f"{side}_wrist_body_name", wrist_body_name) - - fingertip_body_ids, fingertip_body_names = side_robot.find_bodies( - cfg.fingertip_body_name - ) - setattr( - self, - f"{side}_fingertip_body_ids", - torch.tensor(fingertip_body_ids, device=self.device), - ) - setattr(self, f"{side}_fingertip_body_names", fingertip_body_names) - - # Load motion data - try: - self._retargeted_motion_data = ManoSharpaData.from_parquet( - root_path=str(cfg.motion_folder), - filters=cfg.motion_filters, - trajectory_id=cfg.motion_id, - ) - target_num_frames = int(1 / (self.step_dt * cfg.motion_speed)) - self._retargeted_motion_data = interpolate_robot_motion_data( - motion_data=self._retargeted_motion_data, - target_num_frames=target_num_frames, - ) - except Exception as e: - raise ValueError( - "Failed to load retargeted motion data from " - f"{cfg.motion_folder} with filters {cfg.motion_filters} and " - f"trajectory_id {cfg.motion_id}. Please check if the data exists " - f"and is valid. Error: {e}" - ) from e - - # Buffers - self.retargeted_horizon = len( - self._retargeted_motion_data.robot_right_wrist_position - ) - self.timestep_counter = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.tracking_lengths = self.retargeted_horizon * torch.ones( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.steps_since_last_reset = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - - # Reset wrist pose buffers (written by _resample_command, - # read by action terms for PD target initialization) - self.reset_right_wrist_position_e = torch.zeros( - self.num_envs, 3, device=self.device - ) - self.reset_right_wrist_wxyz = torch.zeros(self.num_envs, 4, device=self.device) - self.reset_left_wrist_position_e = torch.zeros( - self.num_envs, 3, device=self.device - ) - self.reset_left_wrist_wxyz = torch.zeros(self.num_envs, 4, device=self.device) - - # Hand data - for side in ["right", "left"]: - # Store wrist position and orientation - retargeted_wrist_position = getattr( - self._retargeted_motion_data, f"robot_{side}_wrist_position" - ) - retargeted_wrist_wxyz = getattr( - self._retargeted_motion_data, f"robot_{side}_wrist_wxyz" - ) - setattr( - self, - f"retargeted_{side}_wrist_position", - torch.tensor(retargeted_wrist_position, device=self.device), - ) - setattr( - self, - f"retargeted_{side}_wrist_wxyz", - torch.tensor(retargeted_wrist_wxyz, device=self.device), - ) - - # Store finger joints in ISAAC joint order - retargeted_finger_joint_names = getattr( - self._retargeted_motion_data, f"{side}_robot_finger_joint_names" - ) - isaac_finger_joint_names = getattr(self, f"{side}_finger_joint_names") - retargeted_to_isaac_joint_order = [ - retargeted_finger_joint_names.index(joint_name) - for joint_name in isaac_finger_joint_names - ] - retargeted_finger_joints = getattr( - self._retargeted_motion_data, f"robot_{side}_finger_joints" - ) - retargeted_finger_joints = torch.tensor( - retargeted_finger_joints, device=self.device - )[:, retargeted_to_isaac_joint_order] - setattr(self, f"retargeted_{side}_finger_joints", retargeted_finger_joints) - - # Store hand frame data - retargeted_hand_frames = getattr( - self._retargeted_motion_data, f"robot_{side}_frames" - ) - retargeted_hand_frame_names = getattr( - self._retargeted_motion_data, f"{side}_robot_frame_names" - ) - setattr( - self, - f"retargeted_{side}_hand_frames", - torch.tensor(retargeted_hand_frames, device=self.device), - ) - setattr( - self, f"retargeted_{side}_hand_frame_names", retargeted_hand_frame_names - ) - - # Command fingertip index - fingertip_body_names = getattr(self, f"{side}_fingertip_body_names") - retargeted_fingertip_indices = [] - for fingertip_body_name in fingertip_body_names: - fingertip_index = retargeted_hand_frame_names.index(fingertip_body_name) - retargeted_fingertip_indices.append(fingertip_index) - setattr( - self, - f"retargeted_{side}_fingertip_indices", - torch.tensor(retargeted_fingertip_indices, device=self.device), - ) - - # Unit vectors - self.X_UNIT_VEC = torch.tensor([1.0, 0.0, 0.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.Y_UNIT_VEC = torch.tensor([0.0, 1.0, 0.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.Z_UNIT_VEC = torch.tensor([0.0, 0.0, 1.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.QUAT_UNIT_VEC = torch.tensor( - [1.0, 0.0, 0.0, 0.0], device=self.device - ).repeat((self.num_envs, 1)) - - # Metrics - self.metrics["right_hand_wrist_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["right_hand_wrist_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["right_hand_finger_joints_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["left_hand_wrist_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["left_hand_wrist_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["left_hand_finger_joints_error"] = torch.zeros( - self.num_envs, device=self.device - ) - - def __str__(self) -> str: - """String representation of the command term.""" - msg = f"{self.__class__.__name__}:\n" - msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" - return msg - - """ - Properties - """ - - ########################################################## - # Commands - ########################################################## - - @property - def command(self) -> torch.Tensor: - """The desired goal pose in the environment frame. Shape is (num_envs, -1).""" - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - right_wrist_current_p_command, right_wrist_current_q_command = ( - math_utils.subtract_frame_transforms( - self.right_hand_wrist_position_e, - self.right_hand_wrist_wxyz_e, - right_hand_wrist_pose_command_e[:, :3], - right_hand_wrist_pose_command_e[:, 3:], - ) - ) - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - left_wrist_current_p_command, left_wrist_current_q_command = ( - math_utils.subtract_frame_transforms( - self.left_hand_wrist_position_e, - self.left_hand_wrist_wxyz_e, - left_hand_wrist_pose_command_e[:, :3], - left_hand_wrist_pose_command_e[:, 3:], - ) - ) - - right_joint_pos_delta = ( - self.right_hand_finger_joint_pos_command - self.right_robot.data.joint_pos - ) - left_joint_pos_delta = ( - self.left_hand_finger_joint_pos_command - self.left_robot.data.joint_pos - ) - - return torch.cat( - ( - right_wrist_current_p_command, - right_wrist_current_q_command, - left_wrist_current_p_command, - left_wrist_current_q_command, - right_joint_pos_delta, - left_joint_pos_delta, - ), - dim=-1, - ) - - @property - def right_hand_wrist_pose_command_e(self) -> torch.Tensor: - """The desired goal position and wxyz in the environment frame for the right hand wrist. Shape is (num_envs, 7).""" - position = self.retargeted_right_wrist_position[self.timestep_counter].float() - wxyz = self.retargeted_right_wrist_wxyz[self.timestep_counter].float() - wxyz = math_utils.quat_unique(wxyz) if self.cfg.make_quat_unique else wxyz - return torch.cat((position, wxyz), dim=-1) - - @property - def left_hand_wrist_pose_command_e(self) -> torch.Tensor: - """The desired goal position and wxyz in the environment frame for the left hand wrist. Shape is (num_envs, 7).""" - position = self.retargeted_left_wrist_position[self.timestep_counter].float() - wxyz = self.retargeted_left_wrist_wxyz[self.timestep_counter].float() - wxyz = math_utils.quat_unique(wxyz) if self.cfg.make_quat_unique else wxyz - return torch.cat((position, wxyz), dim=-1) - - @property - def right_hand_finger_joint_pos_command(self) -> torch.Tensor: - """The desired goal finger joint position for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.retargeted_right_finger_joints[self.timestep_counter].float() - - @property - def left_hand_finger_joint_pos_command(self) -> torch.Tensor: - """The desired goal finger joint position for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.retargeted_left_finger_joints[self.timestep_counter].float() - - @property - def right_hand_fingertip_position_command_e(self) -> torch.Tensor: - """The desired goal fingertip position in the environment frame for the right hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.retargeted_right_hand_frames[self.timestep_counter][ - :, self.retargeted_right_fingertip_indices, :3 - ].float() - - @property - def left_hand_fingertip_position_command_e(self) -> torch.Tensor: - """The desired goal fingertip position in the environment frame for the left hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.retargeted_left_hand_frames[self.timestep_counter][ - :, self.retargeted_left_fingertip_indices, :3 - ].float() - - ########################################################## - # Observations - ########################################################## - - @property - def right_hand_wrist_position_w(self) -> torch.Tensor: - """The current position in the world frame for the right hand wrist. Shape is (num_envs, 3).""" - return self.right_robot.data.root_link_pos_w - - @property - def left_hand_wrist_position_w(self) -> torch.Tensor: - """The current position in the world frame for the left hand wrist. Shape is (num_envs, 3).""" - return self.left_robot.data.root_link_pos_w - - @property - def right_hand_wrist_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the right hand wrist. Shape is (num_envs, 3).""" - return (self.right_hand_wrist_position_w - self._env.scene.env_origins).float() - - @property - def left_hand_wrist_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the left hand wrist. Shape is (num_envs, 3).""" - return (self.left_hand_wrist_position_w - self._env.scene.env_origins).float() - - @property - def right_hand_wrist_wxyz_e(self) -> torch.Tensor: - """The current wxyz in the environment frame for the right hand wrist. Shape is (num_envs, 4).""" - right_wrist_wxyz = self.right_robot.data.root_link_quat_w.float() - return ( - math_utils.quat_unique(right_wrist_wxyz) - if self.cfg.make_quat_unique - else right_wrist_wxyz - ) - - @property - def left_hand_wrist_wxyz_e(self) -> torch.Tensor: - """The current wxyz in the environment frame for the left hand wrist. Shape is (num_envs, 4).""" - left_wrist_wxyz = self.left_robot.data.root_link_quat_w.float() - return ( - math_utils.quat_unique(left_wrist_wxyz) - if self.cfg.make_quat_unique - else left_wrist_wxyz - ) - - @property - def right_hand_wrist_velocity_b(self) -> torch.Tensor: - """The current velocity in the body frame for the right hand wrist. Shape is (num_envs, 6).""" - return torch.cat( - [ - self.right_robot.data.root_lin_vel_b, - self.right_robot.data.root_ang_vel_b, - ], - dim=-1, - ).float() - - @property - def left_hand_wrist_velocity_b(self) -> torch.Tensor: - """The current velocity in the body frame for the left hand wrist. Shape is (num_envs, 6).""" - return torch.cat( - [ - self.left_robot.data.root_lin_vel_b, - self.left_robot.data.root_ang_vel_b, - ], - dim=-1, - ).float() - - @property - def right_hand_finger_joint_pos(self) -> torch.Tensor: - """The current joint position for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.right_robot.data.joint_pos.float() - - @property - def left_hand_finger_joint_pos(self) -> torch.Tensor: - """The current joint position for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.left_robot.data.joint_pos.float() - - @property - def right_hand_finger_joint_vel(self) -> torch.Tensor: - """The current joint velocity for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.right_robot.data.joint_vel.float() - - @property - def left_hand_finger_joint_vel(self) -> torch.Tensor: - """The current joint velocity for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.left_robot.data.joint_vel.float() - - @property - def right_hand_fingertip_position_w(self) -> torch.Tensor: - """The current position in the world frame for the right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.right_robot.data.body_link_pos_w[:, self.right_fingertip_body_ids] - - @property - def left_hand_fingertip_position_w(self) -> torch.Tensor: - """The current position in the world frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.left_robot.data.body_link_pos_w[:, self.left_fingertip_body_ids] - - @property - def right_hand_fingertip_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return ( - self.right_hand_fingertip_position_w - - self._env.scene.env_origins.unsqueeze(1) - ).float() - - @property - def left_hand_fingertip_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return ( - self.left_hand_fingertip_position_w - - self._env.scene.env_origins.unsqueeze(1) - ).float() - - @property - def right_hand_fingertip_orientation_e(self) -> torch.Tensor: - """The current orientation in the environment frame for the right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 4).""" - return self.right_robot.data.body_link_quat_w[ - :, self.right_fingertip_body_ids - ].float() - - @property - def left_hand_fingertip_orientation_e(self) -> torch.Tensor: - """The current orientation in the environment frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 4).""" - return self.left_robot.data.body_link_quat_w[ - :, self.left_fingertip_body_ids - ].float() - - """ - Implementation specific functions. - """ - - def _update_metrics(self) -> None: - """Update the metrics.""" - # Right hand - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - self.metrics["right_hand_wrist_position_error"] = torch.norm( - self.right_hand_wrist_position_e - right_hand_wrist_pose_command_e[:, :3], - dim=-1, - ) - self.metrics["right_hand_wrist_wxyz_error"] = math_utils.quat_error_magnitude( - self.right_hand_wrist_wxyz_e, right_hand_wrist_pose_command_e[:, 3:] - ) - self.metrics["right_hand_finger_joints_error"] = torch.norm( - self.right_hand_finger_joint_pos - self.right_hand_finger_joint_pos_command, - dim=-1, - ) - # Left hand - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - self.metrics["left_hand_wrist_position_error"] = torch.norm( - self.left_hand_wrist_position_e - left_hand_wrist_pose_command_e[:, :3], - dim=-1, - ) - self.metrics["left_hand_wrist_wxyz_error"] = math_utils.quat_error_magnitude( - self.left_hand_wrist_wxyz_e, left_hand_wrist_pose_command_e[:, 3:] - ) - self.metrics["left_hand_finger_joints_error"] = torch.norm( - self.left_hand_finger_joint_pos - self.left_hand_finger_joint_pos_command, - dim=-1, - ) - - def _resample_command(self, env_ids: Sequence[int]) -> None: - """Resample the command.""" - # Reset to a random frame from the original retargeted motion data - self.timestep_counter[env_ids] = torch.randint( - low=0, - high=self.retargeted_horizon - 1, - size=(len(env_ids),), - device=self.device, - dtype=self.timestep_counter.dtype, - ) - - # Get robot wrist position and orientation from the retargeted motion data - right_hand_wrist_position = self.retargeted_right_wrist_position[ - self.timestep_counter[env_ids] - ] - right_hand_wrist_wxyz = self.retargeted_right_wrist_wxyz[ - self.timestep_counter[env_ids] - ] - left_hand_wrist_position = self.retargeted_left_wrist_position[ - self.timestep_counter[env_ids] - ] - left_hand_wrist_wxyz = self.retargeted_left_wrist_wxyz[ - self.timestep_counter[env_ids] - ] - - # Store reset wrist poses (env frame, no env_origins) for action terms - self.reset_right_wrist_position_e[env_ids] = right_hand_wrist_position - self.reset_right_wrist_wxyz[env_ids] = right_hand_wrist_wxyz - self.reset_left_wrist_position_e[env_ids] = left_hand_wrist_position - self.reset_left_wrist_wxyz[env_ids] = left_hand_wrist_wxyz - - # Finger joints: interpolate between open (0) and reference - finger_factor = ( - torch.rand(len(env_ids), 1, device=self.device) - * self.cfg.reset_finger_openness - ) - right_hand_finger_joint_pos = ( - finger_factor - * self.retargeted_right_finger_joints[self.timestep_counter[env_ids]] - ) - left_hand_finger_joint_pos = ( - finger_factor - * self.retargeted_left_finger_joints[self.timestep_counter[env_ids]] - ) - - # Update the tracking length - self.tracking_lengths[env_ids] = ( - self.retargeted_horizon - self.timestep_counter[env_ids] - ).clamp(min=1) - - self.steps_since_last_reset[env_ids] = 0 - - ########################################################## - # Reset the robot - ########################################################## - - # Robot wrist position and orientation - right_hand_wrist_position = ( - right_hand_wrist_position + self._env.scene.env_origins[env_ids] - ) - right_hand_wrist_pose = torch.cat( - [right_hand_wrist_position, right_hand_wrist_wxyz], dim=-1 - ).float() - right_hand_wrist_velocity = torch.zeros_like( - right_hand_wrist_pose[..., :6] - ).float() - - left_hand_wrist_position = ( - left_hand_wrist_position + self._env.scene.env_origins[env_ids] - ) - left_hand_wrist_pose = torch.cat( - [left_hand_wrist_position, left_hand_wrist_wxyz], dim=-1 - ).float() - left_hand_wrist_velocity = torch.zeros_like( - left_hand_wrist_pose[..., :6] - ).float() - - self.right_robot.write_root_pose_to_sim(right_hand_wrist_pose, env_ids=env_ids) - self.right_robot.write_root_velocity_to_sim( - right_hand_wrist_velocity, env_ids=env_ids - ) - self.left_robot.write_root_pose_to_sim(left_hand_wrist_pose, env_ids=env_ids) - self.left_robot.write_root_velocity_to_sim( - left_hand_wrist_velocity, env_ids=env_ids - ) - - # Clear residual external forces from previous episode - zero_forces = torch.zeros(len(env_ids), 1, 3, device=self.device) - self.right_robot.set_external_force_and_torque( - forces=zero_forces, - torques=zero_forces, - body_ids=self.right_wrist_body_id, - env_ids=env_ids, - is_global=False, - ) - self.left_robot.set_external_force_and_torque( - forces=zero_forces, - torques=zero_forces, - body_ids=self.left_wrist_body_id, - env_ids=env_ids, - is_global=False, - ) - - # Right robot finger joint positions - right_hand_joint_velocity = torch.zeros_like( - right_hand_finger_joint_pos - ).float() - right_hand_joint_pos_limits = self.right_robot.data.soft_joint_pos_limits[ - env_ids, : - ] - right_hand_finger_joint_pos = right_hand_finger_joint_pos.clamp_( - right_hand_joint_pos_limits[..., 0], right_hand_joint_pos_limits[..., 1] - ) - self.right_robot.write_joint_state_to_sim( - right_hand_finger_joint_pos, right_hand_joint_velocity, env_ids=env_ids - ) - - # Left robot finger joint positions - left_hand_joint_velocity = torch.zeros_like(left_hand_finger_joint_pos).float() - left_hand_joint_pos_limits = self.left_robot.data.soft_joint_pos_limits[ - env_ids, : - ] - left_hand_finger_joint_pos = left_hand_finger_joint_pos.clamp_( - left_hand_joint_pos_limits[..., 0], left_hand_joint_pos_limits[..., 1] - ) - self.left_robot.write_joint_state_to_sim( - left_hand_finger_joint_pos, left_hand_joint_velocity, env_ids=env_ids - ) - - # Force a kinematic/data refresh after reset writes so the first - # post-reset control step reads synchronized wrist states. - self._env.sim.forward() - self._env.scene.update(dt=self._env.physics_dt) - - def _update_command(self) -> None: - """Update the command.""" - self.steps_since_last_reset += 1 - self.timestep_counter += 1 - - def _set_debug_vis_impl(self, debug_vis: bool) -> None: - """Set the debug visibility.""" - if debug_vis: - if not hasattr(self, "right_hand_pose_visualizer"): - self.right_hand_pose_visualizer = VisualizationMarkers( - self.cfg.right_hand_pose_visualizer_cfg - ) - self.right_hand_pose_visualizer.set_visibility(True) - if not hasattr(self, "left_hand_pose_visualizer"): - self.left_hand_pose_visualizer = VisualizationMarkers( - self.cfg.left_hand_pose_visualizer_cfg - ) - self.left_hand_pose_visualizer.set_visibility(True) - if not hasattr(self, "right_hand_goal_pose_visualizer"): - self.right_hand_goal_pose_visualizer = VisualizationMarkers( - self.cfg.right_hand_goal_pose_visualizer_cfg - ) - self.right_hand_goal_pose_visualizer.set_visibility(True) - if not hasattr(self, "left_hand_goal_pose_visualizer"): - self.left_hand_goal_pose_visualizer = VisualizationMarkers( - self.cfg.left_hand_goal_pose_visualizer_cfg - ) - self.left_hand_goal_pose_visualizer.set_visibility(True) - - # Per-body-frame markers (deferred: hand frame data may not - # exist yet when super().__init__ calls set_debug_vis) - if hasattr(self, "retargeted_right_hand_frames"): - num_target = ( - self.retargeted_right_hand_frames.shape[1] - + self.retargeted_left_hand_frames.shape[1] - ) - num_current = len(self.right_robot.data.body_names) + len( - self.left_robot.data.body_names - ) - num_markers = max(num_target, num_current) - if not hasattr(self, "target_frame_visualizers"): - self.target_frame_visualizers = [] - self.current_frame_visualizers = [] - for i in range(num_markers): - target_cfg = self.cfg.target_fingertip_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/TargetFrame_{i}" - ) - self.target_frame_visualizers.append( - VisualizationMarkers(target_cfg) - ) - current_cfg = self.cfg.current_fingertip_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/CurrentFrame_{i}" - ) - self.current_frame_visualizers.append( - VisualizationMarkers(current_cfg) - ) - self._num_target_frames = num_target - self._num_current_frames = num_current - for vis in self.target_frame_visualizers: - vis.set_visibility(True) - for vis in self.current_frame_visualizers: - vis.set_visibility(True) - else: - for attr in [ - "right_hand_pose_visualizer", - "left_hand_pose_visualizer", - "right_hand_goal_pose_visualizer", - "left_hand_goal_pose_visualizer", - ]: - if hasattr(self, attr): - getattr(self, attr).set_visibility(False) - for vis in getattr(self, "target_frame_visualizers", []): - vis.set_visibility(False) - for vis in getattr(self, "current_frame_visualizers", []): - vis.set_visibility(False) - - def _debug_vis_callback(self, event: Any) -> None: - """Visualize the goal marker.""" - del event # unused - - # Current state visualizers - self.right_hand_pose_visualizer.visualize( - translations=self.right_robot.data.root_link_pos_w, - orientations=self.right_robot.data.root_link_quat_w, - ) - self.left_hand_pose_visualizer.visualize( - translations=self.left_robot.data.root_link_pos_w, - orientations=self.left_robot.data.root_link_quat_w, - ) - # Command visualizers - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - self.right_hand_goal_pose_visualizer.visualize( - translations=right_hand_wrist_pose_command_e[:, :3] - + self._env.scene.env_origins, - orientations=right_hand_wrist_pose_command_e[:, 3:], - ) - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - self.left_hand_goal_pose_visualizer.visualize( - translations=left_hand_wrist_pose_command_e[:, :3] - + self._env.scene.env_origins, - orientations=left_hand_wrist_pose_command_e[:, 3:], - ) - - # Hand body frame visualizers - if not hasattr(self, "target_frame_visualizers"): - # Deferred creation: first callback after __init__ completes - self._set_debug_vis_impl(True) - if hasattr(self, "target_frame_visualizers"): - env_origins = self._env.scene.env_origins - - # Target: retargeted hand frame positions (env frame → world) - num_right_target = self.retargeted_right_hand_frames.shape[1] - right_target = self.retargeted_right_hand_frames[self.timestep_counter][ - :, :, :3 - ].float() - left_target = self.retargeted_left_hand_frames[self.timestep_counter][ - :, :, :3 - ].float() - for i in range(self._num_target_frames): - if i < num_right_target: - pos = right_target[:, i] + env_origins - else: - pos = left_target[:, i - num_right_target] + env_origins - self.target_frame_visualizers[i].visualize( - translations=pos, - orientations=self.QUAT_UNIT_VEC, - ) - - # Current: all sim body link positions (already world frame) - num_right_bodies = len(self.right_robot.data.body_names) - right_body_pos_w = self.right_robot.data.body_link_pos_w - left_body_pos_w = self.left_robot.data.body_link_pos_w - for i in range(self._num_current_frames): - if i < num_right_bodies: - pos = right_body_pos_w[:, i] - else: - pos = left_body_pos_w[:, i - num_right_bodies] - self.current_frame_visualizers[i].visualize( - translations=pos, - orientations=self.QUAT_UNIT_VEC, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands_cfg.py deleted file mode 100644 index e0eabbd2..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/commands_cfg.py +++ /dev/null @@ -1,401 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import isaaclab.sim as sim_utils -from isaaclab.managers import CommandTermCfg -from isaaclab.markers import VisualizationMarkersCfg -from isaaclab.markers.config import FRAME_MARKER_CFG -from isaaclab.utils import configclass - -from robotic_grounding.tasks.v2p.mdp.commands.commands import ( - DualHandsTrackingCommand, - TrackingCommand, -) -from robotic_grounding.tasks.v2p.mdp.commands.hand_object_commands import ( - DualHandsObjectTrackingCommand, -) - - -@configclass -class TrackingCommandCfg(CommandTermCfg): - """Configuration for the tracking command term.""" - - class_type: type = TrackingCommand - resampling_time_range: tuple[float, float] = ( - 1e6, - 1e6, - ) # no resampling based on time - - asset_name: str = "robot" - """Name of the asset in the environment for which the commands are generated.""" - - joint_names: list[str] = [""] - """The names of the joints to track.""" - - make_quat_unique: bool = True - """Whether to make the quaternion unique or not. - - If True, the quaternion is made unique by ensuring the real part is positive. - """ - - file_path: str = "" - """Path to the file containing the motion data.""" - - file_joint_order: list[str] = [""] - """The order of the joints to track in the motion data file.""" - - pos_offset: tuple[float, float, float] = (0.0, 0.0, 0.0) - """Offset of the root pose in the environment frame.""" - - update_goal_on_reach: bool = False - """Whether to update the goal when the goal is reached.""" - - goal_reach_threshold: float = 0.1 - """Threshold for considering the goal as reached.""" - - goal_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/goal_marker" - ) - goal_pose_visualizer_cfg.markers["frame"].scale = (0.15, 0.15, 0.15) - - tips_distance_file_path: str | None = None - """Path to the tips_distance file (optional). - - If provided, the file should contain a numpy array of shape (T, F) where T is the - number of timesteps and F is the number of fingertips or finger sensors, as defined - by the environment configuration. The order of fingers should match that used by the - environment for contact sensing and observation. - - This follows the ManipTrans approach of pre-computing reference fingertip-to-object - surface distances during data processing for use in contact rewards. - """ - - -@configclass -class DualHandsObjectTrackingCommandCfg(CommandTermCfg): - """Configuration for the tracking command term.""" - - class_type: type = DualHandsObjectTrackingCommand - resampling_time_range: tuple[float, float] = (1e6, 1e6) - """No resampling based on time.""" - - debug_vis: bool = False - """Whether to visualize the debug markers.""" - - right_robot_name: str = "right_robot" - """Name of the robot in the environment for which the commands are generated.""" - - left_robot_name: str = "left_robot" - """Name of the left robot in the environment for which the commands are generated.""" - - wrist_joint_names: list[str] = [""] - """Names of the wrist joints in the robot.""" - - finger_joint_names: list[str] = [""] - """Names of the finger joints in the robot.""" - - wrist_body_name: str = "" - """Name of the hand body in the robot.""" - - fingertip_body_name: str = "" - """Name of the fingertip body in the robot.""" - - object_body_names: list[str] = ["object"] - """Names of the object body in the environment for which the commands are generated.""" - - make_quat_unique: bool = True - """Whether to make the quaternion unique or not. - - If True, the quaternion is made unique by ensuring the real part is positive. - """ - - motion_folder: str = "" - """Path to the folder containing the motion data.""" - - motion_filters: list[tuple[str, str, str]] = [ - ("robot_name", "=", "sharpa_wave"), - ("sequence_id", "contains", "box_grab"), - ] - """Filters to apply to the motion data.""" - - motion_id: int = 0 - """Index of the motion to use after applying the filters.""" - - motion_speed: float = 0.5 - """Speed factor to interpolate the motion data to.""" - - reset_finger_openness: float = 0.7 - """Max interpolation factor for finger joints at reset. - - At reset, each env samples a uniform factor in [0, reset_finger_openness]. - The finger joint positions are then: factor * reference_finger_joints. - 0.0 = fully open, 1.0 = fully matching reference. - """ - - always_reset_to_first_frame: bool = False - """Whether to always reset to the first frame of the motion data.""" - - reset_to_first_frame_prob: float = 0.0 - """Probability of resetting to the first trajectory frame (tc=0) instead of a random frame. - - Only applied when ``always_reset_to_first_frame`` is False AND the global VOC scale is - below 0.1 (i.e. the curriculum has nearly or fully decayed). This closes the train/eval - distribution gap: eval always starts from tc=0, but without this flag training never - sees tc=0 starts once the curriculum is active, allowing the policy to reward-hack by - learning to hold-in-place from mid-trajectory positions while failing from the start. - Recommended value: 0.1–0.2. Has no effect during the VOC-assisted curriculum phase. - """ - - initial_virtual_object_control_curriculum_scale: float = 1.0 - """Initial virtual object control curriculum scale.""" - - virtual_object_control_decay_steps: int = 20 - """Number of steps over which the virtual object control factor decays after reset.""" - - virtual_object_control_decay_mode: str = "step" - """Decay mode for the virtual object control factor after reset. - - - ``"linear"``: linearly decay from 1 to ``virtual_object_control_curriculum_scale`` - over ``virtual_object_control_decay_steps``. - - ``"step"``: hold at 1 for ``virtual_object_control_decay_steps``, then drop to - ``virtual_object_control_decay_steps`` instantly. - """ - - recompute_hand_keypoints_from_object: bool = True - """Whether to recompute the hand keypoints based on the object frame.""" - - num_friction_cone_edges: int = 8 - """Number of friction cone edges to sample.""" - - num_wrench_space_basis_samples: int = 512 - """Number of basis samples to draw for the wrench space support function.""" - - friction_coefficients: float = 0.1 - """Friction coefficient for the wrench space support function.""" - - enable_additional_metrics: bool = False - """Whether to log additional diagnostic metrics (contact wrench CV, coverage, etc.). - - Enables contact_wrench_support_reward_cv and related W&B metrics. - Off by default to avoid compute overhead in production runs. - """ - - relative_object_proximity_threshold: float = 0.3 - """Demo inter-object root distance (m) below which the relative-pose reward/metric is active.""" - - relative_object_reward_pos_sigma: float = 0.05 - """Position sigma (m) for the relative-pose reward: exp(-pos_err / sigma).""" - - relative_object_reward_rot_sigma: float = 0.5 - """Rotation sigma (rad) for the relative-pose reward: exp(-rot_err / sigma).""" - - ################################################### - # Visualizer markers - ################################################### - - object_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/object_marker" - ) - object_pose_visualizer_cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - """Visualizer for the object pose.""" - - object_com_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/object_com_marker" - ) - object_com_pose_visualizer_cfg.markers["frame"].scale = (0.05, 0.05, 0.05) - """Visualizer for the object COM frame.""" - - right_hand_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/right_hand_marker" - ) - right_hand_pose_visualizer_cfg.markers["frame"].scale = (0.05, 0.05, 0.05) - """Visualizer for the right hand pose.""" - - left_hand_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/left_hand_marker" - ) - left_hand_pose_visualizer_cfg.markers["frame"].scale = (0.05, 0.05, 0.05) - """Visualizer for the left hand pose.""" - - object_goal_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/object_goal_marker" - ) - object_goal_pose_visualizer_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) - """Visualizer for the object goal pose.""" - - right_hand_goal_pose_visualizer_cfg: VisualizationMarkersCfg = ( - FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/right_hand_goal_marker") - ) - right_hand_goal_pose_visualizer_cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - """Visualizer for the right hand goal pose.""" - - left_hand_goal_pose_visualizer_cfg: VisualizationMarkersCfg = ( - FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/left_hand_goal_marker") - ) - left_hand_goal_pose_visualizer_cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - """Visualizer for the left hand goal pose.""" - - target_contact_visualizer_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/Command/TargetContact", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.0, 0.4, 1.0) - ), - ), - }, - ) - """Visualizer for demo target contact link positions (from parquet). Same idea as arctic_to_sharpa add_icosphere(radius=0.005).""" - - current_contact_visualizer_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/Command/CurrentContact", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.4, 0.0, 1.0) - ), - ), - }, - ) - """Visualizer for current contact link positions. """ - - target_fingertip_position_visualizer_cfg: VisualizationMarkersCfg = ( - VisualizationMarkersCfg( - prim_path="/Visuals/Command/TargetFingertip", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(1.0, 0.0, 0.0) - ), - ), - }, - ) - ) - - current_fingertip_position_visualizer_cfg: VisualizationMarkersCfg = ( - VisualizationMarkersCfg( - prim_path="/Visuals/Command/CurrentFingertip", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.0, 1.0, 0.0) - ), - ), - }, - ) - ) - - -@configclass -class DualHandsTrackingCommandCfg(CommandTermCfg): - """Configuration for the dual-hand tracking command term (no object).""" - - class_type: type = DualHandsTrackingCommand - resampling_time_range: tuple[float, float] = ( - 1e6, - 1e6, - ) # no resampling based on time - - right_robot_name: str = "right_robot" - """Name of the robot in the environment for which the commands are generated.""" - - left_robot_name: str = "left_robot" - """Name of the left robot in the environment for which the commands are generated.""" - - wrist_joint_names: list[str] = [""] - """Names of the wrist joints in the robot.""" - - finger_joint_names: list[str] = [""] - """Names of the finger joints in the robot.""" - - wrist_body_name: str = "" - """Name of the hand body in the robot.""" - - fingertip_body_name: str = "" - """Name of the fingertip body in the robot.""" - - make_quat_unique: bool = True - """Whether to make the quaternion unique or not. - - If True, the quaternion is made unique by ensuring the real part is positive. - """ - - motion_folder: str = "" - """Path to the folder containing the motion data.""" - - motion_filters: list[tuple[str, str, str]] = [ - ("robot_name", "=", "sharpa_wave"), - ("sequence_id", "contains", "box_grab"), - ] - """Filters to apply to the motion data.""" - - motion_id: int = 0 - """Index of the motion to use after applying the filters.""" - - motion_speed: float = 0.2 - """Speed factor to interpolate the motion data to.""" - - reset_finger_openness: float = 0.7 - """Max interpolation factor for finger joints at reset. - - At reset, each env samples a uniform factor in [0, reset_finger_openness]. - The finger joint positions are then: factor * reference_finger_joints. - 0.0 = fully open, 1.0 = fully matching reference. - """ - - ################################################### - # Visualizer markers - ################################################### - - right_hand_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/right_hand_marker" - ) - right_hand_pose_visualizer_cfg.markers["frame"].scale = (0.05, 0.05, 0.05) - """Visualizer for the right hand pose.""" - - left_hand_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/left_hand_marker" - ) - left_hand_pose_visualizer_cfg.markers["frame"].scale = (0.05, 0.05, 0.05) - """Visualizer for the left hand pose.""" - - right_hand_goal_pose_visualizer_cfg: VisualizationMarkersCfg = ( - FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/right_hand_goal_marker") - ) - right_hand_goal_pose_visualizer_cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - """Visualizer for the right hand goal pose.""" - - left_hand_goal_pose_visualizer_cfg: VisualizationMarkersCfg = ( - FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/left_hand_goal_marker") - ) - left_hand_goal_pose_visualizer_cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - """Visualizer for the left hand goal pose.""" - - target_fingertip_visualizer_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/Command/TargetFingertip", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(1.0, 0.0, 0.0) - ), - ), - }, - ) - """Visualizer for target fingertip positions (red).""" - - current_fingertip_visualizer_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/Command/CurrentFingertip", - markers={ - "sphere": sim_utils.SphereCfg( - radius=0.005, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.0, 1.0, 0.0) - ), - ), - }, - ) - """Visualizer for current fingertip positions (green).""" diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/hand_object_commands.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/hand_object_commands.py deleted file mode 100644 index e78be32e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/commands/hand_object_commands.py +++ /dev/null @@ -1,2637 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Command definitions for the V2P environment.""" - -from __future__ import annotations - -import collections -from collections.abc import Sequence -from typing import TYPE_CHECKING, Any, Tuple - -import isaaclab.utils.math as math_utils -import torch -from isaaclab.assets import Articulation, RigidObject -from isaaclab.managers import CommandTerm, CommandTermCfg -from isaaclab.markers.visualization_markers import VisualizationMarkers - -try: - import isaacsim.util.debug_draw._debug_draw as omni_debug_draw -except ImportError: - print("[WARNING]: Debug draw is not available. No lines will be drawn.") - omni_debug_draw = None - -from robotic_grounding.retarget.data_logger import ManoSharpaData -from robotic_grounding.tasks.v2p.mdp.utils import ( - compute_wrench_space, - compute_wrench_space_support_function, - interpolate_robot_motion_data, - sample_wrench_space_basis_scaled, -) -from robotic_grounding.tasks.v2p.mdp.utils_jit import ( - contact_wrench_support_reward_jit, - refresh_jit, - resample_compute_tensors_jit, - wrench_preprocess_jit, - wrench_support_one_body_jit, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - -ENABLE_ADDITIONAL_METRICS = False - - -class DualHandsObjectTrackingCommand(CommandTerm): - """Command term that generates pose commands for dual-hand object tracking. - - This term is the central data hub for the tracking task. It bridges - retargeted human hand motion data with the Isaac Lab simulation, - providing both **command targets** (what the policy should achieve) and - **current state observations** (where things are now). - - Initialization is split into private helpers — see each for details: - - 1. :meth:`_init_scene_references` — resolve robot / object assets and - cache body/joint indices and contact sensors. - 2. :meth:`_load_motion_data` — load retargeted motion from parquet and - interpolate to the simulation FPS. - 3. :meth:`_init_buffers` — allocate per-env counters, reset-pose buffers, - curriculum scale factors, and constant unit vectors. - 4. :meth:`_init_hand_data` — convert per-side hand motion arrays (wrist, - fingers, frames, fingertip indices) to GPU tensors. - 5. :meth:`_init_object_data` — convert object body motion arrays to GPU - tensors. - 6. :meth:`_init_contact_data` — load per-link contact positions, normals, - and part IDs for both hands. - 7. :meth:`_precompute_contact_positions_in_object_frame` — transform - contact positions into each contacted object body's local frame. - 8. :meth:`_precompute_hand_keypoints_in_object_frame` — express hand - frame keypoints and wrist poses in the object body's local frame. - 9. :meth:`_init_metrics` — zero-initialize tracking-error metric buffers. - - At runtime the class exposes: - - - **Command properties** (``*_command_e``, ``*_command_o``) — goal poses - for wrists, fingers, fingertips, object bodies, and contact points. - Wrist commands are stored in the object's local frame and recomputed - each step from the current object pose, so hand targets automatically - follow object drift. - - **State properties** — current wrist / finger / fingertip / object - poses, velocities, and contact forces read from the simulation. - - **Reset / stepping** — :meth:`_resample_command` teleports hands and - object to a random trajectory frame; :meth:`_update_command` advances - the timestep counter and decays the virtual-object-control curriculum. - - **Visualization** — optional debug markers for goal vs. current poses, - contact points, and fingertip positions. - """ - - cfg: CommandTermCfg - """Configuration for the command term.""" - - def __init__(self, cfg: CommandTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the dual-hand object tracking command term. - - Loads motion data from the configured folder, resolves scene assets - (object and left/right robots), and sets up command buffers and reset - state tensors. - - Args: - cfg: Command term configuration (motion path, asset names, FPS, etc.). - env: The RL environment instance; used to resolve scene assets. - """ - super().__init__(cfg, env) - self.step_dt = env.step_dt - - self._init_scene_references(cfg, env) - self._load_motion_data(cfg) - self._init_buffers(cfg) - self._init_hand_data() - self._init_object_data() - self._init_relative_object_data() - self._init_contact_data() - self._precompute_contact_positions_normals_in_object_frame() - self._precompute_hand_keypoints_in_object_frame() - self._precompute_contact_wrench_support_values() - self._meshvert_reward_enabled = False - if self.cfg.enable_additional_metrics: - self._precompute_bbox_corner_vecs() - self._precompute_object_mesh_vertices() - self._set_contact_vis_impl(getattr(self.cfg, "debug_vis", False)) - self._init_metrics(cfg) - - # Lazy per-step refresh cache. refresh_tensors() recomputes all cached - # tensors on first access when _tensors_dirty is True (set here, at end - # of _update_command, and at end of _resample_command). - self._tensors_dirty = True - - def __str__(self) -> str: - """String representation of the command term.""" - msg = f"{self.__class__.__name__}:\n" - msg += f"\tCommand dimension: {tuple(self.command.shape[1:])}\n" - return msg - - ###################################################################### - # Initialization Helpers - ###################################################################### - - def _init_scene_references( - self, cfg: CommandTermCfg, env: ManagerBasedRLEnv - ) -> None: - """Resolve simulation scene assets and cache body/joint indices. - - Retrieves object assets, and for each hand side (left/right) resolves - the robot articulation, finger joint IDs, wrist body IDs, fingertip - body IDs, and object-to-hand contact sensors from the scene. - - Sets per-side attributes via ``setattr``: - - ``{side}_robot``, ``{side}_finger_joint_names/ids`` - - ``{side}_wrist_body_id/name``, ``{side}_fingertip_body_ids/names`` - - ``object_to_{side}_hand_contact_sensors`` - """ - # Retrieve objects - self.objects: list[Articulation | RigidObject] = [ - env.scene[body_name] for body_name in cfg.object_body_names - ] - self.num_bodies = self.object_position_e.shape[1] - - # Retrieve both side robots and cache useful indices - for side in ["right", "left"]: - # Robot asset handle - side_robot_name = getattr(cfg, f"{side}_robot_name") - side_robot = env.scene[side_robot_name] - setattr(self, f"{side}_robot", side_robot) - - # Finger joint names and ids - side_finger_joint_names = side_robot.data.joint_names - finger_joint_ids, _ = side_robot.find_joints(side_finger_joint_names) - finger_joint_ids = torch.tensor(finger_joint_ids, device=self.device) - setattr(self, f"{side}_finger_joint_names", side_finger_joint_names) - setattr(self, f"{side}_finger_joint_ids", finger_joint_ids) - - # Wrist body ids and names - wrist_body_ids, wrist_body_name = side_robot.find_bodies( - cfg.wrist_body_name - ) - setattr( - self, - f"{side}_wrist_body_id", - torch.tensor(wrist_body_ids, device=self.device), - ) - setattr(self, f"{side}_wrist_body_name", wrist_body_name) - - # Fingertip body ids and names - fingertip_body_ids, fingertip_body_names = side_robot.find_bodies( - cfg.fingertip_body_name - ) - setattr( - self, - f"{side}_fingertip_body_ids", - torch.tensor(fingertip_body_ids, device=self.device), - ) - setattr(self, f"{side}_fingertip_body_names", fingertip_body_names) - - # Object-hand contact sensors - contact_sensors = [ - env.scene[sensor_name] - for sensor_name in env.cfg.object_to_hand_contact_sensor_names - if side in sensor_name - ] - setattr(self, f"object_to_{side}_hand_contact_sensors", contact_sensors) - - def _load_motion_data(self, cfg: CommandTermCfg) -> None: - """Load retargeted motion data from parquet and interpolate to target FPS. - - Reads the ``ManoSharpaData`` parquet dataset specified by - ``cfg.motion_folder`` / ``cfg.motion_filters`` / ``cfg.motion_id``, - then resamples it to match the simulation timestep (or an explicit - ``cfg.motion_speed``). - - Sets: - - ``self._retargeted_motion_data`` — the loaded + interpolated data. - """ - try: - self._retargeted_motion_data = ManoSharpaData.from_parquet( - root_path=str(cfg.motion_folder), - filters=cfg.motion_filters, - trajectory_id=cfg.motion_id, - ) - - # Interpolate the motion data to the target FPS - target_num_frames = int(1 / (self.step_dt * cfg.motion_speed)) - self._retargeted_motion_data = interpolate_robot_motion_data( - motion_data=self._retargeted_motion_data, - target_num_frames=target_num_frames, - ) - except Exception as e: - raise ValueError( - "Failed to load retargeted motion data from " - f"{cfg.motion_folder} with filters {cfg.motion_filters} and " - f"trajectory_id {cfg.motion_id} or interpolate the motion data. " - f"Please check if the data exists and is valid. Error: {e}" - ) from e - - def _init_buffers(self, cfg: CommandTermCfg) -> None: - """Allocate per-env bookkeeping tensors and precomputed constant vectors. - - Creates: - - Timestep / tracking counters: ``timestep_counter``, - ``tracking_lengths``, ``steps_since_last_reset``, ``all_env_ids``. - - Reset wrist pose buffers (written by ``_resample_command``, read by - action terms for PD target initialization). - - Virtual object controller curriculum scale factors. - - Constant unit vectors used by reward/visualization code: - ``X/Y/Z_UNIT_VEC``, ``QUAT_UNIT_VEC``, ``KEYPOINT_VECS``. - - ``contact_sensor_history_length`` from the first right-hand sensor. - """ - # Horizon and per-env counters - self.retargeted_horizon = len( - self._retargeted_motion_data.robot_right_wrist_position - ) - self.timestep_counter = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.tracking_lengths = self.retargeted_horizon * torch.ones( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.steps_since_last_reset = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.all_env_ids = torch.arange(self.num_envs, device=self.device) - - # Reset wrist pose buffers (written by _resample_command, - # read by action terms for PD target initialization) - self.reset_right_wrist_position_e = torch.zeros( - self.num_envs, 3, device=self.device - ) - self.reset_right_wrist_wxyz = torch.zeros(self.num_envs, 4, device=self.device) - self.reset_left_wrist_position_e = torch.zeros( - self.num_envs, 3, device=self.device - ) - self.reset_left_wrist_wxyz = torch.zeros(self.num_envs, 4, device=self.device) - - # Virtual object controller scale factor for curriculum - self.virtual_object_controller_scale_factor = torch.tensor( - [cfg.initial_virtual_object_control_curriculum_scale], device=self.device - ) # (1,) - self.virtual_object_controller_scale_factor_per_env = ( - cfg.initial_virtual_object_control_curriculum_scale - * torch.ones(self.num_envs, 1, device=self.device) # (num_envs, 1) - ) - - # Unit vectors reused by reward computation and visualization - self.X_UNIT_VEC = torch.tensor([1.0, 0.0, 0.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.Y_UNIT_VEC = torch.tensor([0.0, 1.0, 0.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.Z_UNIT_VEC = torch.tensor([0.0, 0.0, 1.0], device=self.device).repeat( - (self.num_envs, 1) - ) - self.QUAT_UNIT_VEC = torch.tensor( - [1.0, 0.0, 0.0, 0.0], device=self.device - ).repeat((self.num_envs, 1)) - - # 6 axis-aligned directions for object keypoint computation - self.KEYPOINT_VECS = ( - torch.tensor( - [ - [1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, 0.0, 1.0], - [-1.0, 0.0, 0.0], - [0.0, -1.0, 0.0], - [0.0, 0.0, -1.0], - ], - device=self.device, - dtype=torch.float32, - ) - .unsqueeze(0) - .unsqueeze(1) - .expand(self.num_envs, self.num_bodies, -1, -1) - ) # (num_envs, num_bodies, 6, 3) - - # Contact sensor history length - self.contact_sensor_history_length = self.object_to_right_hand_contact_sensors[ - 0 - ].cfg.history_length - - # Number of robot contacts - self.num_robot_contacts_left = len( - self.object_to_left_hand_contact_sensors[0].cfg.filter_prim_paths_expr - ) - self.num_robot_contacts_right = len( - self.object_to_right_hand_contact_sensors[0].cfg.filter_prim_paths_expr - ) - assert ( - self.num_robot_contacts_left == self.num_robot_contacts_right - ), "The number of robot contacts for the left and right hand must be the same." - self.total_num_robot_contacts = ( - self.num_robot_contacts_left + self.num_robot_contacts_right - ) - - # Env origin - self.env_origins_expanded = self._env.scene.env_origins.reshape( - self.num_envs, 1, 1, 3 - ).expand(-1, self.num_bodies, self.num_robot_contacts_right, -1) - - def _init_hand_data(self) -> None: - """Convert retargeted hand motion arrays to GPU tensors for both sides. - - For each hand side (left/right), loads from ``_retargeted_motion_data`` - and stores as tensors: - - ``retargeted_{side}_wrist_position`` — (horizon, 3) - - ``retargeted_{side}_wrist_wxyz`` — (horizon, 4) - - ``retargeted_{side}_finger_joints`` — (horizon, N_joints), reordered - from retarget joint order to Isaac joint order. - - ``retargeted_{side}_hand_frames`` — (horizon, N_frames, 7) - - ``retargeted_{side}_hand_frame_names`` — list of frame names - - ``retargeted_{side}_fingertip_indices`` — indices into hand_frames - for fingertip bodies. - """ - for side in ["right", "left"]: - # Wrist position and orientation - retargeted_wrist_position = getattr( - self._retargeted_motion_data, f"robot_{side}_wrist_position" - ) - retargeted_wrist_wxyz = getattr( - self._retargeted_motion_data, f"robot_{side}_wrist_wxyz" - ) - setattr( - self, - f"retargeted_{side}_wrist_position", - torch.tensor(retargeted_wrist_position, device=self.device), - ) - setattr( - self, - f"retargeted_{side}_wrist_wxyz", - torch.tensor(retargeted_wrist_wxyz, device=self.device), - ) - - # Finger joints reordered to Isaac joint order - retargeted_finger_joint_names = getattr( - self._retargeted_motion_data, f"{side}_robot_finger_joint_names" - ) - isaac_finger_joint_names = getattr(self, f"{side}_finger_joint_names") - retargeted_to_isaac_joint_order = [ - retargeted_finger_joint_names.index(joint_name) - for joint_name in isaac_finger_joint_names - ] - retargeted_finger_joints = getattr( - self._retargeted_motion_data, f"robot_{side}_finger_joints" - ) - retargeted_finger_joints = torch.tensor( - retargeted_finger_joints, device=self.device - )[:, retargeted_to_isaac_joint_order] - setattr(self, f"retargeted_{side}_finger_joints", retargeted_finger_joints) - - # Hand frames (position + quaternion per body) - retargeted_hand_frames = getattr( - self._retargeted_motion_data, f"robot_{side}_frames" - ) - retargeted_hand_frame_names = getattr( - self._retargeted_motion_data, f"{side}_robot_frame_names" - ) - setattr( - self, - f"retargeted_{side}_hand_frames", - torch.tensor(retargeted_hand_frames, device=self.device), - ) - setattr( - self, f"retargeted_{side}_hand_frame_names", retargeted_hand_frame_names - ) - - # Fingertip indices into the hand frame array - fingertip_body_names = getattr(self, f"{side}_fingertip_body_names") - retargeted_fingertip_indices = [ - retargeted_hand_frame_names.index(name) for name in fingertip_body_names - ] - setattr( - self, - f"retargeted_{side}_fingertip_indices", - torch.tensor(retargeted_fingertip_indices, device=self.device), - ) - - def _init_object_data(self) -> None: - """Convert retargeted object motion arrays to GPU tensors. - - Stores: - - ``retargeted_object_body_position`` — (horizon, N_body, 3) - - ``retargeted_object_body_wxyz`` — (horizon, N_body, 4) - - ``retargeted_object_articulation`` — (horizon, N_joints) - - ``retargeted_object_body_names`` — list of body names - - Asserts that the number of body names in the motion file matches the - number of bodies in the simulation object. - """ - self.retargeted_object_body_position = torch.tensor( - self._retargeted_motion_data.object_body_position, device=self.device - ).float() - self.retargeted_object_body_wxyz = torch.tensor( - self._retargeted_motion_data.object_body_wxyz, device=self.device - ).float() - self.retargeted_object_articulation = torch.tensor( - self._retargeted_motion_data.object_articulation, device=self.device - ).float() - self.retargeted_object_body_names = ( - self._retargeted_motion_data.object_body_names - ) - assert len(self.retargeted_object_body_names) == self.num_bodies, ( - f"The number of body names in the motion file and the object do not match. " - f"Find {len(self.retargeted_object_body_names)} in motion file, " - f"but {self.num_bodies} in the object." - ) - - def _init_relative_object_data(self) -> None: - """Precompute demo relative pose (obj1 in obj0's frame) for multi-object tasks. - - Sets: - - ``self._has_multi_object`` — False for single-object sequences, skips all relative-pose logic. - - ``self._obj1_root_body_idx`` — index of obj1's root body in the concatenated body array. - - ``self._demo_rel_pos`` — (T, 3) demo relative position of obj1 root in obj0 root frame. - - ``self._demo_rel_quat`` — (T, 4) demo relative quaternion of obj1 root in obj0 root frame. - - ``self._demo_inter_object_dist`` — (T,) L2 distance between obj0 and obj1 roots in demo. - """ - if len(self.objects) < 2: - self._has_multi_object = False - return - self._has_multi_object = True - self._obj1_root_body_idx = self.objects[0].data.body_link_pos_w.shape[1] - - # env-frame positions and world-frame quats (env origin cancels in relative transform) - demo_obj0_pos = self.retargeted_object_body_position[:, 0, :] - demo_obj0_quat = self.retargeted_object_body_wxyz[:, 0, :] - demo_obj1_pos = self.retargeted_object_body_position[ - :, self._obj1_root_body_idx, : - ] - demo_obj1_quat = self.retargeted_object_body_wxyz[ - :, self._obj1_root_body_idx, : - ] - - self._demo_rel_pos, self._demo_rel_quat = math_utils.subtract_frame_transforms( - demo_obj0_pos, demo_obj0_quat, demo_obj1_pos, demo_obj1_quat - ) # (T, 3) and (T, 4) - self._demo_inter_object_dist = torch.norm( - demo_obj1_pos - demo_obj0_pos, dim=-1 - ) # (T,) - - def _init_contact_data(self) -> None: - """Load per-link contact data from retargeted motion for both hand sides. - - For each side, stores tensors with shape (horizon, num_contact_links, 3): - - ``retargeted_{side}_link_contact_positions_e`` - - ``retargeted_{side}_link_contact_normals_e`` pointing outward from the hand (inward to object). - - ``retargeted_{side}_object_contact_positions_e`` - - ``retargeted_{side}_object_contact_normals_e`` - - ``retargeted_{side}_object_contact_part_ids`` — (horizon, num_links), - read from ``mano_{side}_object_contact_part_ids`` (0 = no contact, - 1-indexed body IDs). - """ - data = self._retargeted_motion_data - - for side in ["left", "right"]: - link_contact_positions = torch.tensor( - getattr(data, f"mano_{side}_link_contact_positions"), - dtype=torch.float32, - device=self.device, - ) # (horizon, num_contact_links, 3) - link_contact_normals = torch.tensor( - getattr(data, f"mano_{side}_link_contact_normals"), - dtype=torch.float32, - device=self.device, - ) # (horizon, num_contact_links, 3) - link_contact_normals = link_contact_normals / link_contact_normals.norm( - dim=-1, keepdim=True - ).clamp(min=1e-6) - object_contact_positions = torch.tensor( - getattr(data, f"mano_{side}_object_contact_positions"), - dtype=torch.float32, - device=self.device, - ) # (horizon, num_contact_links, 3) - object_contact_normals = torch.tensor( - getattr(data, f"mano_{side}_object_contact_normals"), - dtype=torch.float32, - device=self.device, - ) # (horizon, num_contact_links, 3) - contact_part_ids = torch.tensor( - getattr(data, f"mano_{side}_object_contact_part_ids"), - dtype=torch.long, - device=self.device, - ) # (horizon, num_contact_links) - - setattr( - self, - f"retargeted_{side}_link_contact_positions_e", - link_contact_positions, - ) - setattr( - self, - f"retargeted_{side}_link_contact_normals_e", - link_contact_normals, - ) - setattr( - self, - f"retargeted_{side}_object_contact_positions_e", - object_contact_positions, - ) - setattr( - self, - f"retargeted_{side}_object_contact_normals_e", - object_contact_normals, - ) - setattr( - self, f"retargeted_{side}_object_contact_part_ids", contact_part_ids - ) - - def _compute_contact_positions_normals_in_object_frame(self, side: str) -> None: - """Transform env-frame contact positions and normals into the contacted object body's local frame. - - For the given hand side, reads ``retargeted_{side}_object_contact_positions_e`` - and ``retargeted_{side}_link_contact_normals_e`` - and ``retargeted_{side}_object_contact_part_ids``, then uses - ``subtract_frame_transforms`` to express each contact point in the local - frame of the object body it belongs to. - retargeted_{side}_link_contact_normals_e is the normal of the human hand, - pointing outward from the hand. - - Sets: - - ``retargeted_{side}_object_contact_positions_o`` — (horizon, N_links, 3) - - ``retargeted_{side}_link_contact_normals_o`` — (horizon, N_links, 3) - - ``retargeted_{side}_object_contact_positions_com`` — (horizon, N_links, 3) - - ``retargeted_{side}_link_contact_normals_com`` — (horizon, N_links, 3) - - ``retargeted_{side}_object_contact_is_valid`` — (horizon, N_links) bool - - ``retargeted_{side}_object_has_contact`` — (horizon,) bool - - Args: - side: ``"left"`` or ``"right"``. - """ - contact_positions_e = getattr( - self, f"retargeted_{side}_object_contact_positions_e" - ) - contact_normals_e = getattr(self, f"retargeted_{side}_link_contact_normals_e") - contact_part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids") - horizon_ids = torch.arange( - self.retargeted_object_body_position.shape[0], device=self.device - ) - - object_o_t_com = torch.cat( - [object.data.body_com_pose_b for object in self.objects], dim=1 - ).float() - object_o_p_com = object_o_t_com[..., :3].mean(dim=0) # (num_bodies, 3) - object_o_q_com = object_o_t_com[0, :, 3:7] # (num_bodies, 4) - - contact_positions_o = torch.zeros_like(contact_positions_e[..., :3]) - contact_normals_o = torch.zeros_like(contact_normals_e[..., :3]) - contact_positions_com = torch.zeros_like(contact_positions_e[..., :3]) - contact_normals_com = torch.zeros_like(contact_normals_e[..., :3]) - is_valid = contact_part_ids > 0 - has_contact = is_valid.sum(dim=-1) > 1e-5 - - for link_idx in range(contact_positions_e.shape[1]): - # Part IDs are 1-indexed; 0 means no contact - part_id = (contact_part_ids[:, link_idx] - 1).clamp( - min=0, max=self.num_bodies - 1 - ) - # convert environment frame contact to object frame - object_e_p_o = self.retargeted_object_body_position[horizon_ids, part_id] - object_e_q_o = self.retargeted_object_body_wxyz[horizon_ids, part_id] - contact_positions_o[:, link_idx], _ = math_utils.subtract_frame_transforms( - object_e_p_o, - object_e_q_o, - contact_positions_e[:, link_idx, :3], - q02=None, - ) - contact_normals_o[:, link_idx], _ = math_utils.subtract_frame_transforms( - torch.zeros_like(object_e_p_o), - object_e_q_o, - contact_normals_e[:, link_idx, :3], - q02=None, - ) - # convert object frame contact to object com frame - _object_o_p_com = object_o_p_com[part_id] - _object_o_q_com = object_o_q_com[part_id] - contact_positions_com[:, link_idx], _ = ( - math_utils.subtract_frame_transforms( - _object_o_p_com, - _object_o_q_com, - contact_positions_o[:, link_idx, :3], - q02=None, - ) - ) - contact_normals_com[:, link_idx], _ = math_utils.subtract_frame_transforms( - torch.zeros_like(_object_o_p_com), - _object_o_q_com, - contact_normals_o[:, link_idx, :3], - q02=None, - ) - - contact_positions_o.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - contact_normals_o.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - contact_positions_com.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - contact_normals_com.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - - setattr( - self, f"retargeted_{side}_object_contact_positions_o", contact_positions_o - ) - setattr(self, f"retargeted_{side}_link_contact_normals_o", contact_normals_o) - setattr( - self, - f"retargeted_{side}_object_contact_positions_com", - contact_positions_com, - ) - setattr( - self, f"retargeted_{side}_link_contact_normals_com", contact_normals_com - ) - setattr(self, f"retargeted_{side}_object_contact_is_valid", is_valid) - setattr(self, f"retargeted_{side}_object_has_contact", has_contact) - - def _precompute_contact_positions_normals_in_object_frame(self) -> None: - """Transform contact positions into object frame for both hands and compute contact counts. - - Calls ``_compute_contact_positions_normals_in_object_frame`` for each hand side, - then ``_get_contact_counts`` to cache the total number of contact points. - """ - for side in ["left", "right"]: - self._compute_contact_positions_normals_in_object_frame(side) - self._get_retargeted_contact_counts() - - def _compute_hand_keypoints_in_object_frame(self, side: str) -> None: - """Express hand frame keypoints and wrist pose in the contacted object body's local frame. - - For the given hand side: - - 1. Determines the dominant contact body per timestep by taking the mode - of per-link contact part IDs, with backward-fill for frames without - contact. - 2. Transforms all hand frame positions and orientations into that - object body's local frame via ``subtract_frame_transforms``. - 3. Extracts the wrist-specific position and orientation as a convenience - slice. - - Sets: - - ``retargeted_{side}_object_contact_part_ids_per_hand`` — (horizon,) - - ``retargeted_{side}_hand_frame_position_o`` — (horizon, N_frames, 3) - - ``retargeted_{side}_hand_frame_wxyz_o`` — (horizon, N_frames, 4) - - ``retargeted_{side}_wrist_position_o`` — (horizon, 3) - - ``retargeted_{side}_wrist_wxyz_o`` — (horizon, 4) - - Args: - side: ``"left"`` or ``"right"``. - """ - contact_part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids") - hand_frames = getattr(self, f"retargeted_{side}_hand_frames") - hand_frame_names = getattr(self, f"retargeted_{side}_hand_frame_names") - wrist_body_name = getattr(self, f"{side}_wrist_body_name") - - horizon_ids = torch.arange( - self.retargeted_object_body_position.shape[0], device=self.device - ) - - # Determine the dominant contact object body per timestep (backward fill) - part_ids_per_hand = torch.zeros( - self.retargeted_horizon, dtype=torch.int64, device=self.device - ) - last_contact_part_ids = torch.ones(1, dtype=torch.int64, device=self.device) - for horizon_idx in range(self.retargeted_horizon - 1, -1, -1): - hand_contact_part_ids = contact_part_ids[horizon_idx].mode().values - part_ids_per_hand[horizon_idx] = ( - hand_contact_part_ids - if hand_contact_part_ids > 0 - else last_contact_part_ids - ) - last_contact_part_ids = part_ids_per_hand[horizon_idx] - - setattr( - self, - f"retargeted_{side}_object_contact_part_ids_per_hand", - part_ids_per_hand, - ) - - # Transform hand frames into the contact object body's local frame - frame_position_o = torch.zeros_like(hand_frames[..., :3]) - frame_wxyz_o = torch.zeros_like(hand_frames[..., 3:7]) - - contact_body_position = self.retargeted_object_body_position[ - horizon_ids, part_ids_per_hand - 1 - ] - contact_body_wxyz = self.retargeted_object_body_wxyz[ - horizon_ids, part_ids_per_hand - 1 - ] - - for frame_idx in range(hand_frames.shape[1]): - frame_position_o[:, frame_idx], frame_wxyz_o[:, frame_idx] = ( - math_utils.subtract_frame_transforms( - contact_body_position, - contact_body_wxyz, - hand_frames[:, frame_idx, :3], - hand_frames[:, frame_idx, 3:7], - ) - ) - - setattr(self, f"retargeted_{side}_hand_frame_position_o", frame_position_o) - setattr(self, f"retargeted_{side}_hand_frame_wxyz_o", frame_wxyz_o) - - # Extract wrist-specific slice for convenience - wrist_index = hand_frame_names.index(wrist_body_name[0]) - setattr( - self, - f"retargeted_{side}_wrist_position_o", - frame_position_o[:, wrist_index], - ) - setattr(self, f"retargeted_{side}_wrist_wxyz_o", frame_wxyz_o[:, wrist_index]) - - def _precompute_hand_keypoints_in_object_frame(self) -> None: - """Express hand frame keypoints in object frame for both hand sides. - - Calls ``_compute_hand_keypoints_in_object_frame`` for left and right. - """ - for side in ["left", "right"]: - self._compute_hand_keypoints_in_object_frame(side) - - def _precompute_contact_wrench_support_values(self) -> None: - """Precompute the contact wrench space support function over sampled basis directions for each hand-body pairs.""" - # 1. Sample basis directions for each object body - self.object_mesh_radius = self._retargeted_motion_data.object_mesh_radius - self.wrench_space_bases = torch.cat( - [ - sample_wrench_space_basis_scaled( - self.cfg.num_wrench_space_basis_samples, rc=1.0, device=self.device - ).unsqueeze(0) - for _ in self.object_mesh_radius - ], - dim=0, - ) - - # 2. Expand contact positions and normals to (horizon, num_bodies, num_hand_contact_links, 3) - t_idx = torch.arange(self.retargeted_horizon, device=self.device)[ - :, None - ] # (horizon, 1) - c_idx = torch.arange(self.num_retargeted_contacts_left, device=self.device)[ - None, : - ] # (1, num_hand_contact_links) - - retargeted_left_contact_positions_com = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.num_retargeted_contacts_left, - 3, - device=self.device, - ) - retargeted_left_contact_normals_com = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.num_retargeted_contacts_left, - 3, - device=self.device, - ) - left_command_contact_part_ids = ( - self.retargeted_left_object_contact_part_ids - 1 - ).clamp( - min=0, max=self.num_bodies - 1 - ) # (horizon, num_hand_contact_links), 0-indexed - left_command_valid = self.retargeted_left_object_contact_is_valid.unsqueeze( - -1 - ) # (horizon, num_hand_contact_links, 1) - retargeted_left_contact_positions_com[ - t_idx, left_command_contact_part_ids, c_idx - ] = (self.retargeted_left_object_contact_positions_com * left_command_valid) - retargeted_left_contact_normals_com[ - t_idx, left_command_contact_part_ids, c_idx - ] = (self.retargeted_left_link_contact_normals_com * left_command_valid) - - retargeted_right_contact_positions_com = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.num_retargeted_contacts_right, - 3, - device=self.device, - ) - retargeted_right_contact_normals_com = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.num_retargeted_contacts_right, - 3, - device=self.device, - ) - right_command_contact_part_ids = ( - self.retargeted_right_object_contact_part_ids - 1 - ).clamp( - min=0, max=self.num_bodies - 1 - ) # (horizon, num_hand_contact_links), 0-indexed - right_command_valid = self.retargeted_right_object_contact_is_valid.unsqueeze( - -1 - ) # (horizon, num_hand_contact_links, 1) - retargeted_right_contact_positions_com[ - t_idx, right_command_contact_part_ids, c_idx - ] = (self.retargeted_right_object_contact_positions_com * right_command_valid) - retargeted_right_contact_normals_com[ - t_idx, right_command_contact_part_ids, c_idx - ] = (self.retargeted_right_link_contact_normals_com * right_command_valid) - - # 3. Compute support function over sampled basis directions for each hand-body pairs - self.retargeted_left_contact_wrench_supports = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - self.retargeted_right_contact_wrench_supports = torch.zeros( - self.retargeted_horizon, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - - theta = torch.linspace( - 0.0, - 2.0 * torch.pi, - steps=self.cfg.num_friction_cone_edges + 1, - device=retargeted_left_contact_positions_com.device, - dtype=retargeted_left_contact_positions_com.dtype, - )[:-1] - self.friction_cone_edge_cosines = torch.cos(theta).view(1, -1, 1) # (1, K, 1) - self.friction_cone_edge_sines = torch.sin(theta).view(1, -1, 1) # (1, K, 1) - - for body_idx, body_radius in enumerate(self.object_mesh_radius): - left_wrench_space = compute_wrench_space( - contact_points=retargeted_left_contact_positions_com[:, body_idx], - contact_normals=retargeted_left_contact_normals_com[:, body_idx], - cos_t=self.friction_cone_edge_cosines, - sin_t=self.friction_cone_edge_sines, - rc=body_radius, - friction_coefficients=self.cfg.friction_coefficients, - ) # (horizon, 6, _) - self.retargeted_left_contact_wrench_supports[:, body_idx] = ( - compute_wrench_space_support_function( - wrench_space=left_wrench_space, - basis=self.wrench_space_bases[body_idx], - ) - ) # (horizon, num_wrench_space_basis_samples) - - right_wrench_space = compute_wrench_space( - contact_points=retargeted_right_contact_positions_com[:, body_idx], - contact_normals=retargeted_right_contact_normals_com[:, body_idx], - cos_t=self.friction_cone_edge_cosines, - sin_t=self.friction_cone_edge_sines, - rc=body_radius, - friction_coefficients=self.cfg.friction_coefficients, - ) # (horizon, _, 6) - self.retargeted_right_contact_wrench_supports[:, body_idx] = ( - compute_wrench_space_support_function( - wrench_space=right_wrench_space, - basis=self.wrench_space_bases[body_idx], - ) - ) # (horizon, num_wrench_space_basis_samples) - - # Buffer for current contact wrench supports - self.left_contact_wrench_supports = torch.zeros( - self.num_envs, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - self.right_contact_wrench_supports = torch.zeros( - self.num_envs, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - - def _compute_object_body_half_extents(self) -> torch.Tensor: - """Compute per-body AABB half-extents in local frame from USD geometry. - - Called once at init. Uses UsdGeom.BBoxCache on the live USD stage so no - precomputed tables or parquet changes are needed — generalizes to any object. - Falls back to isotropic sphere radius per body if the prim is not found or - has empty bounds. - - Returns: - Tensor of shape (num_bodies, 3) with per-body half-extents [hx, hy, hz]. - """ - import isaaclab.sim.utils as sim_utils # noqa: PLC0415 - from pxr import Usd, UsdGeom # noqa: PLC0415 - - half_extents: list[list[float]] = [] - body_idx = 0 - - for obj in self.objects: - first_prim = sim_utils.find_first_matching_prim(obj.cfg.prim_path) - root_path = first_prim.GetPath().pathString - stage = first_prim.GetStage() - bbox_cache = UsdGeom.BBoxCache( - Usd.TimeCode.Default(), ["default", "proxy", "render"] - ) - - for link_name in obj.data.body_names: - body_prim = stage.GetPrimAtPath(f"{root_path}/{link_name}") - if body_prim.IsValid(): - bound = bbox_cache.ComputeUntransformedBound(body_prim) - r3d = bound.GetRange() - if not r3d.IsEmpty(): - mn, mx = r3d.GetMin(), r3d.GetMax() - half_extents.append([(mx[i] - mn[i]) / 2.0 for i in range(3)]) - body_idx += 1 - continue - # Fallback: isotropic sphere radius - r = ( - self.object_mesh_radius[body_idx] - if body_idx < len(self.object_mesh_radius) - else 0.1 - ) - half_extents.append([r, r, r]) - body_idx += 1 - - return torch.tensor(half_extents, device=self.device, dtype=torch.float32) - - def _precompute_bbox_corner_vecs(self) -> None: - """Pre-compute the 8 AABB corner vectors scaled by per-body half-extents. - - Stores ``self.BBOX_CORNER_VECS`` of shape (num_envs, num_bodies, 8, 3). - Each of the 8 corners is (±hx, ±hy, ±hz) in the body's local frame. - Used in ``_update_metrics`` to compute a size-normalized pose-tracking error. - """ - self.object_body_half_extents = self._compute_object_body_half_extents() - # (num_bodies, 3) - - signs = torch.tensor( - [ - [s0, s1, s2] - for s0 in [1.0, -1.0] - for s1 in [1.0, -1.0] - for s2 in [1.0, -1.0] - ], - device=self.device, - dtype=torch.float32, - ) # (8, 3) - - # Scale unit corners by per-body half-extents - self.BBOX_CORNER_VECS = ( - (signs.unsqueeze(0) * self.object_body_half_extents.unsqueeze(1)) - .unsqueeze(0) - .expand(self.num_envs, -1, -1, -1) - ) - # (num_envs, num_bodies, 8, 3) - - def _precompute_object_mesh_vertices(self, num_samples: int = 500) -> None: - """Sample deterministic object mesh vertices for mesh-vertex tracking.""" - import numpy as np # noqa: PLC0415 - import trimesh # noqa: PLC0415 - - self._meshvert_reward_enabled = False - self._meshvert_num_verts = int(num_samples) - - mesh_paths = list( - getattr(self._retargeted_motion_data, "object_mesh_paths", []) or [] - ) - num_bodies = int(self.object_body_half_extents.shape[0]) - if len(mesh_paths) != num_bodies: - print( - "[object_meshvert_tracking_fine] WARNING: " - f"expected {num_bodies} object mesh paths, got {len(mesh_paths)}" - ) - return - - sampled_verts = [] - for mesh_path in mesh_paths: - try: - mesh = trimesh.load(mesh_path, force="mesh", process=False) - if hasattr(mesh, "geometry"): - mesh = trimesh.util.concatenate(tuple(mesh.geometry.values())) - vertices = np.asarray(mesh.vertices, dtype=np.float32) - if vertices.shape[0] == 0: - raise ValueError("mesh has no vertices") - - rng = np.random.default_rng(42) - indices = rng.choice( - vertices.shape[0], - size=self._meshvert_num_verts, - replace=vertices.shape[0] < self._meshvert_num_verts, - ) - except Exception as exc: # noqa: BLE001 - print( - "[object_meshvert_tracking_fine] WARNING: " - f"failed to sample vertices from {mesh_path}: {exc}" - ) - return - - sampled_verts.append( - torch.tensor( - vertices[indices], - device=self.device, - dtype=torch.float32, - ) - ) - - self.OBJECT_MESHVERT_SAMPLED_VERTS = ( - torch.stack(sampled_verts, dim=0) - .unsqueeze(0) - .expand(self.num_envs, -1, -1, -1) - ) - self._meshvert_reward_enabled = True - - def _init_metrics(self, cfg: CommandTermCfg) -> None: - """Initialize all tracking-error metric buffers to zero. - - Allocates per-env zero tensors for wrist position/orientation errors, - finger joint errors, object body pose errors, object articulation - errors, and the virtual object controller scale factor. - """ - for side in ["right", "left"]: - self.metrics[f"{side}_hand_wrist_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics[f"{side}_hand_wrist_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics[f"{side}_hand_finger_joints_error"] = torch.zeros( - self.num_envs, device=self.device - ) - - self.metrics["object_body_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["object_body_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["object_articulation_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["virtual_object_controller_scale_factor"] = ( - cfg.initial_virtual_object_control_curriculum_scale - * torch.ones(self.num_envs, device=self.device) - ) - - if self.cfg.enable_additional_metrics: - # Contact wrench level metrics (left/right separate) - for side in ["right", "left"]: - self.metrics[f"contact_wrench_command_support_mean_{side}"] = ( - torch.zeros(self.num_envs, device=self.device) - ) - self.metrics[f"contact_wrench_current_support_mean_{side}"] = ( - torch.zeros(self.num_envs, device=self.device) - ) - self.metrics[f"contact_wrench_support_ratio_{side}"] = torch.zeros( - self.num_envs, device=self.device - ) - - # Object bounding-box tracking and lift metrics - self.metrics["object_bbox_corner_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["object_body_z_error"] = torch.zeros( - self.num_envs, device=self.device - ) - # Rolling step-level buffer for contact_wrench_support_reward CV (W=200). - # Initialized to 1.0 (high = not plateaued) until the buffer fills. - self._cws_reward_step_buf: collections.deque = collections.deque(maxlen=200) - self.metrics["contact_wrench_support_reward_cv"] = torch.ones( - self.num_envs, device=self.device - ) - - if self._has_multi_object: - self.metrics["relative_object_pos_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["relative_object_rot_error"] = torch.zeros( - self.num_envs, device=self.device - ) - - ###################################################################### - # Commands - ###################################################################### - - @property - def command(self) -> torch.Tensor: - """The desired goal pose in the environment frame. Shape is (num_envs, -1).""" - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - right_wrist_current_p_command, right_wrist_current_q_command = ( - math_utils.subtract_frame_transforms( - self.right_hand_wrist_position_e, - self.right_hand_wrist_wxyz_e, - right_hand_wrist_pose_command_e[:, :3], - right_hand_wrist_pose_command_e[:, 3:], - ) - ) - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - left_wrist_current_p_command, left_wrist_current_q_command = ( - math_utils.subtract_frame_transforms( - self.left_hand_wrist_position_e, - self.left_hand_wrist_wxyz_e, - left_hand_wrist_pose_command_e[:, :3], - left_hand_wrist_pose_command_e[:, 3:], - ) - ) - - right_joint_pos_delta = ( - self.right_hand_finger_joint_pos_command - self.right_robot.data.joint_pos - ) - left_joint_pos_delta = ( - self.left_hand_finger_joint_pos_command - self.left_robot.data.joint_pos - ) - - object_current_p_command, object_current_q_command = ( - math_utils.subtract_frame_transforms( - self.object_position_e, - self.object_orientation_e, - self.object_body_position_command_e, - self.object_body_wxyz_command_e, - ) - ) - - return torch.cat( - ( - right_wrist_current_p_command, - right_wrist_current_q_command, - left_wrist_current_p_command, - left_wrist_current_q_command, - right_joint_pos_delta, - left_joint_pos_delta, - object_current_p_command.reshape(self.num_envs, -1), - object_current_q_command.reshape(self.num_envs, -1), - ), - dim=-1, - ) - - def get_command_contact_part_id(self, side: str) -> torch.Tensor: - """Get the contact part id for the entire hand. Shape is (num_envs,).""" - contact_part_id = ( - # object contact part id start from 1 - self.retargeted_left_object_contact_part_ids_per_hand[self.timestep_counter] - - 1 - if side == "left" - else self.retargeted_right_object_contact_part_ids_per_hand[ - self.timestep_counter - ] - - 1 - ) - contact_part_id = contact_part_id.clamp(min=0, max=self.num_bodies - 1) - return contact_part_id - - def get_command_contact_object_position_orientation( - self, side: str - ) -> Tuple[torch.Tensor, torch.Tensor]: - """Get the contact object position and orientation in the environment frame for the given side. Shape is (num_envs, 3).""" - contact_part_id = self.get_command_contact_part_id(side) - object_position = self.object_position_e[self.all_env_ids, contact_part_id] - object_orientation = self.object_orientation_e[ - self.all_env_ids, contact_part_id - ] - - return object_position, object_orientation - - @property - def right_hand_wrist_pose_command_e(self) -> torch.Tensor: - """The desired goal position and wxyz in the environment frame for the right hand wrist. Shape is (num_envs, 7).""" - object_position, object_orientation = ( - self.get_command_contact_object_position_orientation("right") - ) - position, wxyz = math_utils.combine_frame_transforms( - object_position, - object_orientation, - self.retargeted_right_wrist_position_o[self.timestep_counter], - self.retargeted_right_wrist_wxyz_o[self.timestep_counter], - ) - wxyz = math_utils.quat_unique(wxyz) if self.cfg.make_quat_unique else wxyz - return torch.cat((position, wxyz), dim=-1) - - @property - def left_hand_wrist_pose_command_e(self) -> torch.Tensor: - """The desired goal position and wxyz in the environment frame for the left hand wrist. Shape is (num_envs, 7).""" - object_position, object_orientation = ( - self.get_command_contact_object_position_orientation("left") - ) - position, wxyz = math_utils.combine_frame_transforms( - object_position, - object_orientation, - self.retargeted_left_wrist_position_o[self.timestep_counter], - self.retargeted_left_wrist_wxyz_o[self.timestep_counter], - ) - wxyz = math_utils.quat_unique(wxyz) if self.cfg.make_quat_unique else wxyz - return torch.cat((position, wxyz), dim=-1) - - @property - def right_hand_finger_joint_pos_command(self) -> torch.Tensor: - """The desired goal finger joint position for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.retargeted_right_finger_joints[self.timestep_counter].float() - - @property - def left_hand_finger_joint_pos_command(self) -> torch.Tensor: - """The desired goal finger joint position for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.retargeted_left_finger_joints[self.timestep_counter].float() - - @property - def right_hand_fingertip_position_command_o(self) -> torch.Tensor: - """The desired goal fingertip position in the object frame for the right hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.retargeted_right_hand_frame_position_o[self.timestep_counter][ - :, self.retargeted_right_fingertip_indices - ].float() - - @property - def right_hand_fingertip_position_command_e(self) -> torch.Tensor: - """The desired goal fingertip position in the environment frame for the right hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - if self.cfg.recompute_hand_keypoints_from_object: - object_position, object_orientation = ( - self.get_command_contact_object_position_orientation("right") - ) - return math_utils.combine_frame_transforms( - object_position.unsqueeze(1).expand( - -1, len(self.retargeted_right_fingertip_indices), -1 - ), - object_orientation.unsqueeze(1).expand( - -1, len(self.retargeted_right_fingertip_indices), -1 - ), - self.right_hand_fingertip_position_command_o, - q12=None, - )[0] - else: - return self.retargeted_right_hand_frames[self.timestep_counter][ - :, self.retargeted_right_fingertip_indices, :3 - ].float() - - @property - def left_hand_fingertip_position_command_o(self) -> torch.Tensor: - """The desired goal fingertip position in the object frame for the left hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.retargeted_left_hand_frame_position_o[self.timestep_counter][ - :, self.retargeted_left_fingertip_indices - ].float() - - @property - def left_hand_fingertip_position_command_e(self) -> torch.Tensor: - """The desired goal fingertip position in the environment frame for the left hand. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - if self.cfg.recompute_hand_keypoints_from_object: - object_position, object_orientation = ( - self.get_command_contact_object_position_orientation("left") - ) - return math_utils.combine_frame_transforms( - object_position.unsqueeze(1).expand( - -1, len(self.retargeted_left_fingertip_indices), -1 - ), - object_orientation.unsqueeze(1).expand( - -1, len(self.retargeted_left_fingertip_indices), -1 - ), - self.left_hand_fingertip_position_command_o, - q12=None, - )[0] - else: - return self.retargeted_left_hand_frames[self.timestep_counter][ - :, self.retargeted_left_fingertip_indices, :3 - ].float() - - @property - def object_body_position_command_e(self) -> torch.Tensor: - """The desired goal position in the environment frame for the object. Shape is (num_envs, NUM_BODY, 3).""" - return self.retargeted_object_body_position[self.timestep_counter].float() - - @property - def object_body_wxyz_command_e(self) -> torch.Tensor: - """The desired goal orientation in the environment frame for the object. Shape is (num_envs, NUM_BODY, 4).""" - retargeted_object_wxyz = self.retargeted_object_body_wxyz[ - self.timestep_counter - ].float() - retargeted_object_wxyz = ( - math_utils.quat_unique(retargeted_object_wxyz) - if self.cfg.make_quat_unique - else retargeted_object_wxyz - ) - return retargeted_object_wxyz.float() - - @property - def right_hand_object_contact_command_positions_o(self) -> torch.Tensor: - """The target contact positions in the object frame for the right hand. Shape is (num_envs, num_retargeted_contacts_right, 3).""" - return self.retargeted_right_object_contact_positions_o[self.timestep_counter] - - @property - def right_hand_link_contact_command_normals_o(self) -> torch.Tensor: - """The target contact normals in the object frame for the right hand. Shape is (num_envs, num_retargeted_contacts_right, 3).""" - return self.retargeted_right_link_contact_normals_o[self.timestep_counter] - - @property - def right_hand_object_contact_command_positions_and_normals_e(self) -> torch.Tensor: - """The target contact positions and normals in the environment frame for the right hand. - - retargeted_{side}_link_contact_normals_e is the normal of the human hand, pointing outward from the hand (inward to object). - Shape is (num_envs, num_retargeted_contacts_right, 6). - """ - valid_contact_mask = self.retargeted_right_object_contact_is_valid[ - self.timestep_counter - ] # (num_envs, num_retargeted_contacts_right) - - contact_part_id = ( - # 0 for no contact, object contact part id start from 1 - self.retargeted_right_object_contact_part_ids[self.timestep_counter] - - 1 - ) - contact_part_id = contact_part_id.clamp(min=0, max=self.num_bodies - 1) - object_position = torch.gather( - self.object_position_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 3), - ) - object_orientation = torch.gather( - self.object_orientation_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 4), - ) - - right_hand_object_contact_command_positions_e, _ = ( - math_utils.combine_frame_transforms( - object_position, - object_orientation, - self.right_hand_object_contact_command_positions_o, - q12=None, - ) - ) - right_hand_object_contact_command_normals_e, _ = ( - math_utils.combine_frame_transforms( - torch.zeros_like(object_position), - object_orientation, - self.right_hand_link_contact_command_normals_o, - q12=None, - ) - ) - - right_hand_object_contact_command_positions_e.masked_fill_( - ~valid_contact_mask.unsqueeze(-1), 0.0 - ) - right_hand_object_contact_command_normals_e.masked_fill_( - ~valid_contact_mask.unsqueeze(-1), 0.0 - ) - return torch.cat( - [ - right_hand_object_contact_command_positions_e, - right_hand_object_contact_command_normals_e, - ], - dim=-1, - ) - - @property - def left_hand_object_contact_command_positions_o(self) -> torch.Tensor: - """The target contact positions in the object frame for the left hand. Shape is (num_envs, num_retargeted_contacts_left, 3).""" - return self.retargeted_left_object_contact_positions_o[self.timestep_counter] - - @property - def left_hand_link_contact_command_normals_o(self) -> torch.Tensor: - """The target contact normals in the object frame for the left hand. Shape is (num_envs, num_retargeted_contacts_left, 3).""" - return self.retargeted_left_link_contact_normals_o[self.timestep_counter] - - @property - def left_hand_object_contact_command_positions_and_normals_e(self) -> torch.Tensor: - """The target contact positions and normals in the environment frame for the left hand. - - retargeted_{side}_link_contact_normals_e is the normal of the human hand, pointing outward from the hand (inward to object). - Shape is (num_envs, num_retargeted_contacts_left, 6). - """ - valid_contact_mask = self.retargeted_left_object_contact_is_valid[ - self.timestep_counter - ] # (num_envs, num_retargeted_contacts_left) - contact_part_id = ( - # 0 for no contact, object contact part id start from 1 - self.retargeted_left_object_contact_part_ids[self.timestep_counter] - - 1 - ) - contact_part_id = contact_part_id.clamp(min=0, max=self.num_bodies - 1) - object_position = torch.gather( - self.object_position_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 3), - ) - object_orientation = torch.gather( - self.object_orientation_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 4), - ) - - left_hand_object_contact_command_positions_e, _ = ( - math_utils.combine_frame_transforms( - object_position, - object_orientation, - self.left_hand_object_contact_command_positions_o, - q12=None, - ) - ) - left_hand_object_contact_command_normals_e, _ = ( - math_utils.combine_frame_transforms( - torch.zeros_like(object_position), - object_orientation, - self.left_hand_link_contact_command_normals_o, - q12=None, - ) - ) - - left_hand_object_contact_command_positions_e.masked_fill_( - ~valid_contact_mask.unsqueeze(-1), 0.0 - ) - left_hand_object_contact_command_normals_e.masked_fill_( - ~valid_contact_mask.unsqueeze(-1), 0.0 - ) - return torch.cat( - [ - left_hand_object_contact_command_positions_e, - left_hand_object_contact_command_normals_e, - ], - dim=-1, - ) - - @property - def right_hand_contact_wrench_supports_command(self) -> torch.Tensor: - """The contact wrench supports for the right hand contact. Shape is (num_envs, num_bodies, num_wrench_space_basis_samples).""" - # Alias of the cached ref_right (same retargeted[timestep_counter] indexing). - return self.ref_right - - @property - def left_hand_contact_wrench_supports_command(self) -> torch.Tensor: - """The contact wrench supports for the left hand contact. Shape is (num_envs, num_bodies, num_wrench_space_basis_samples).""" - return self.ref_left - - ###################################################################### - # Observations - ###################################################################### - - @property - def right_hand_wrist_position_w(self) -> torch.Tensor: - """The current position in the environment frame for the right hand wrist. Shape is (num_envs, 3).""" - return self.right_robot.data.root_link_pos_w - - @property - def left_hand_wrist_position_w(self) -> torch.Tensor: - """The current position in the environment frame for the left hand wrist. Shape is (num_envs, 3).""" - return self.left_robot.data.root_link_pos_w - - @property - def right_hand_wrist_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the right hand wrist. Shape is (num_envs, 3).""" - return (self.right_hand_wrist_position_w - self._env.scene.env_origins).float() - - @property - def left_hand_wrist_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the left hand wrist. Shape is (num_envs, 3).""" - return (self.left_hand_wrist_position_w - self._env.scene.env_origins).float() - - @property - def right_hand_wrist_wxyz_e(self) -> torch.Tensor: - """The current wxyz in the environment frame for the right hand wrist. Shape is (num_envs, 4).""" - right_wrist_wxyz = self.right_robot.data.root_link_quat_w.float() - return ( - math_utils.quat_unique(right_wrist_wxyz) - if self.cfg.make_quat_unique - else right_wrist_wxyz - ) - - @property - def left_hand_wrist_wxyz_e(self) -> torch.Tensor: - """The current wxyz in the environment frame for the left hand wrist. Shape is (num_envs, 4).""" - left_wrist_wxyz = self.left_robot.data.root_link_quat_w.float() - return ( - math_utils.quat_unique(left_wrist_wxyz) - if self.cfg.make_quat_unique - else left_wrist_wxyz - ) - - @property - def right_hand_wrist_velocity_b(self) -> torch.Tensor: - """The current velocity in the body frame for the right hand wrist. Shape is (num_envs, 6).""" - return torch.cat( - [ - self.right_robot.data.root_lin_vel_b, - self.right_robot.data.root_ang_vel_b, - ], - dim=-1, - ).float() - - @property - def left_hand_wrist_velocity_b(self) -> torch.Tensor: - """The current velocity in the body frame for the left hand wrist. Shape is (num_envs, 6).""" - return torch.cat( - [ - self.left_robot.data.root_lin_vel_b, - self.left_robot.data.root_ang_vel_b, - ], - dim=-1, - ).float() - - @property - def right_hand_finger_joint_pos(self) -> torch.Tensor: - """The current joint position for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.right_robot.data.joint_pos.float() - - @property - def left_hand_finger_joint_pos(self) -> torch.Tensor: - """The current joint position for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.left_robot.data.joint_pos.float() - - @property - def right_hand_finger_joint_vel(self) -> torch.Tensor: - """The current joint velocity for the right hand. Shape is (num_envs, NUM_RIGHT_HAND_FINGER_JOINTS).""" - return self.right_robot.data.joint_vel.float() - - @property - def left_hand_finger_joint_vel(self) -> torch.Tensor: - """The current joint velocity for the left hand. Shape is (num_envs, NUM_LEFT_HAND_FINGER_JOINTS).""" - return self.left_robot.data.joint_vel.float() - - @property - def right_hand_fingertip_position_w(self) -> torch.Tensor: - """The current position in the environment frame for the right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.right_robot.data.body_link_pos_w[:, self.right_fingertip_body_ids] - - @property - def left_hand_fingertip_position_w(self) -> torch.Tensor: - """The current position in the environment frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - return self.left_robot.data.body_link_pos_w[:, self.left_fingertip_body_ids] - - @property - def right_hand_fingertip_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - fingertip_position_e = ( - self.right_hand_fingertip_position_w - - self._env.scene.env_origins.unsqueeze(1) - ) - return fingertip_position_e.float() - - @property - def left_hand_fingertip_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 3).""" - fingertip_position_e = ( - self.left_hand_fingertip_position_w - - self._env.scene.env_origins.unsqueeze(1) - ) - return fingertip_position_e.float() - - @property - def right_hand_fingertip_orientation_e(self) -> torch.Tensor: - """The current orientation in the environment frame for the left and right fingertip. Shape is (num_envs, NUM_FINGERTIPS, 4).""" - return self.right_robot.data.body_link_quat_w[ - :, self.right_fingertip_body_ids - ].float() - - @property - def left_hand_fingertip_orientation_e(self) -> torch.Tensor: - """The current orientation in the environment frame for the left fingertip. Shape is (num_envs, NUM_FINGERTIPS, 4).""" - return self.left_robot.data.body_link_quat_w[ - :, self.left_fingertip_body_ids - ].float() - - @property - def object_position_w(self) -> torch.Tensor: - """The current position in the environment frame for the object. Shape is (num_envs, NUM_BODY, 3).""" - return torch.cat( - [object.data.body_link_pos_w for object in self.objects], dim=1 - ).float() - - @property - def object_position_e(self) -> torch.Tensor: - """The current position in the environment frame for the object. Shape is (num_envs, NUM_BODY, 3).""" - object_position_e = ( - self.object_position_w - self._env.scene.env_origins.unsqueeze(1) - ) - return object_position_e.float() - - @property - def object_orientation_e(self) -> torch.Tensor: - """The current orientation in the environment frame for the object. Shape is (num_envs, NUM_BODY, 4).""" - return torch.cat( - [object.data.body_link_quat_w for object in self.objects], dim=1 - ).float() - - @property - def relative_object_pose_error(self) -> tuple[torch.Tensor, torch.Tensor]: - """Pose error between current and demo relative pose of obj1 in obj0's frame. - - Returns: - pos_err: (num_envs,) L2 position error in metres. - rot_err: (num_envs,) geodesic rotation error in radians. - - Only call when self._has_multi_object is True. - """ - obj0_pos = self.object_position_e[:, 0, :] - obj0_quat = self.object_orientation_e[:, 0, :] - obj1_pos = self.object_position_e[:, self._obj1_root_body_idx, :] - obj1_quat = self.object_orientation_e[:, self._obj1_root_body_idx, :] - cur_rel_pos, cur_rel_quat = math_utils.subtract_frame_transforms( - obj0_pos, obj0_quat, obj1_pos, obj1_quat - ) - demo_rel_pos = self._demo_rel_pos[self.timestep_counter] - demo_rel_quat = self._demo_rel_quat[self.timestep_counter] - pos_err = torch.norm(cur_rel_pos - demo_rel_pos, dim=-1) - rot_err = math_utils.quat_error_magnitude(cur_rel_quat, demo_rel_quat) - return pos_err, rot_err - - @property - def relative_object_proximity_mask(self) -> torch.Tensor: - """(num_envs,) bool — True when demo inter-object distance < proximity_threshold.""" - demo_dist = self._demo_inter_object_dist[self.timestep_counter] - return demo_dist < self.cfg.relative_object_proximity_threshold - - @property - def object_com_position_and_wxyz_w(self) -> torch.Tensor: - """The current position and orientation in the environment frame for the object com. Shape is (num_envs, num_bodies, 7).""" - return torch.cat( - [object.data.body_com_state_w[..., :7] for object in self.objects], dim=1 - ).float() - - @property - def right_hand_object_contact_positions_w(self) -> torch.Tensor: - """The current contact positions in the world frame for the right hand. Shape is (num_envs, num_bodies, num_hand_link_w_sensor, 3).""" - contact_positions = torch.cat( - [ - contact_sensor.data.contact_pos_w - for contact_sensor in self.object_to_right_hand_contact_sensors - ], - dim=1, - ) # (num_envs, num_bodies, num_robot_links, 3) - - return torch.nan_to_num(contact_positions, nan=0.0) - - @property - def right_hand_object_contact_positions_e(self) -> torch.Tensor: - """The current contact positions in the environment frame for the right hand. Shape is (num_envs, num_bodies, num_hand_link_w_sensor, 3).""" - return self.right_hand_object_contact_positions_w - self.env_origins_expanded - - @property - def right_hand_object_contact_forces_w(self) -> torch.Tensor: - """The current contact forces in the world frame for the right hand. - - Force is hand to object, the direction is inward to object. - Shape is (num_envs, contact_sensor_history_length, num_bodies, num_hand_link_w_sensor, 3). - """ - return torch.cat( - [ - contact_sensor.data.force_matrix_w_history - for contact_sensor in self.object_to_right_hand_contact_sensors - ], - dim=2, - ).view( - self.num_envs, - self.contact_sensor_history_length, - self.num_bodies, - self.num_robot_contacts_right, - 3, - ) - - @property - def left_hand_object_contact_positions_w(self) -> torch.Tensor: - """The current contact positions in the environment frame for the left hand. Shape is (num_envs, num_bodies, num_hand_link_w_sensor, 3).""" - contact_positions = torch.cat( - [ - contact_sensor.data.contact_pos_w - for contact_sensor in self.object_to_left_hand_contact_sensors - ], - dim=1, - ) # (num_envs, num_bodies, num_robot_links, 3) - - return torch.nan_to_num(contact_positions, nan=0.0) - - @property - def left_hand_object_contact_positions_e(self) -> torch.Tensor: - """The current contact positions in the environment frame for the left hand. Shape is (num_envs, num_bodies, num_hand_link_w_sensor, 3).""" - return self.left_hand_object_contact_positions_w - self.env_origins_expanded - - @property - def left_hand_object_contact_forces_w(self) -> torch.Tensor: - """The current contact forces in the world frame for the left hand. - - Force is hand to object, the direction is inward to object. - Shape is (num_envs, contact_sensor_history_length, num_bodies, num_hand_link_w_sensor, 3). - """ - return torch.cat( - [ - contact_sensor.data.force_matrix_w_history - for contact_sensor in self.object_to_left_hand_contact_sensors - ], - dim=2, - ).view( - self.num_envs, - self.contact_sensor_history_length, - self.num_bodies, - self.num_robot_contacts_left, - 3, - ) - - @property - def right_hand_contact_wrench_supports(self) -> torch.Tensor: - """The wrench representation for the right hand contact. Shape is (num_envs, num_bodies, num_wrench_space_basis_samples).""" - self.refresh_tensors() - return self.right_contact_wrench_supports - - @property - def left_hand_contact_wrench_supports(self) -> torch.Tensor: - """The wrench representation for the left hand contact. Shape is (num_envs, num_bodies, num_wrench_space_basis_samples).""" - self.refresh_tensors() - return self.left_contact_wrench_supports - - ###################################################################### - # Cached refresh accessors (populated by refresh_tensors). - ###################################################################### - - @property - def right_force_sq_per_link(self) -> torch.Tensor: - """Per-link squared force magnitude for the right hand. - - Shape ``(num_envs, num_right_hand_links_w_sensor)``. Computed as - ``forces_w.square().sum(-1).mean(dim=history).sum(dim=bodies)``. - """ - self.refresh_tensors() - return self._cached_right_force_sq_per_link - - @property - def left_force_sq_per_link(self) -> torch.Tensor: - """Per-link squared force magnitude for the left hand. See :meth:`right_force_sq_per_link`.""" - self.refresh_tensors() - return self._cached_left_force_sq_per_link - - @property - def right_link_in_contact(self) -> torch.Tensor: - """Per-link in-contact boolean for the right hand (threshold 1e-3). Shape ``(num_envs, num_links)``.""" - self.refresh_tensors() - return self._cached_right_link_in_contact - - @property - def left_link_in_contact(self) -> torch.Tensor: - """Per-link in-contact boolean for the left hand (threshold 1e-3). Shape ``(num_envs, num_links)``.""" - self.refresh_tensors() - return self._cached_left_link_in_contact - - @property - def right_in_contact(self) -> torch.Tensor: - """Per-env right-hand in-contact boolean (threshold 1e-3). Shape ``(num_envs,)``.""" - self.refresh_tensors() - return self._cached_right_in_contact - - @property - def left_in_contact(self) -> torch.Tensor: - """Per-env left-hand in-contact boolean (threshold 1e-3). Shape ``(num_envs,)``.""" - self.refresh_tensors() - return self._cached_left_in_contact - - @property - def in_contact(self) -> torch.Tensor: - """Per-env either-hand in-contact boolean. Shape ``(num_envs,)``.""" - self.refresh_tensors() - return self._cached_in_contact - - @property - def ref_left(self) -> torch.Tensor: - """Retargeted left wrench supports at the current timestep. Shape ``(num_envs, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_ref_L - - @property - def ref_right(self) -> torch.Tensor: - """Retargeted right wrench supports at the current timestep. Shape ``(num_envs, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_ref_R - - @property - def mask_left(self) -> torch.Tensor: - """``ref_left > 1e-6``. Same shape as ``ref_left``.""" - self.refresh_tensors() - return self._cached_mask_L - - @property - def mask_right(self) -> torch.Tensor: - """``ref_right > 1e-6``. Same shape as ``ref_right``.""" - self.refresh_tensors() - return self._cached_mask_R - - @property - def ref_active_per_cell(self) -> torch.Tensor: - """Per-body per-basis reference-active mask: ``mask_left | mask_right``. Shape ``(num_envs, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_ref_active_per_cell - - @property - def ref_active_per_body(self) -> torch.Tensor: - """Per-body reference-active mask (any basis). Shape ``(num_envs, num_bodies)``.""" - self.refresh_tensors() - return self._cached_ref_active_per_body - - @property - def ref_active_global(self) -> torch.Tensor: - """Global per-env reference-active mask. Shape ``(num_envs,)``.""" - self.refresh_tensors() - return self._cached_ref_active_global - - @property - def right_wrench_cmd_active(self) -> torch.Tensor: - """``ref_right > 1e-3`` ("meaningful" support). Shape ``(N, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_right_wrench_cmd_active - - @property - def left_wrench_cmd_active(self) -> torch.Tensor: - """``ref_left > 1e-3`` ("meaningful" support). Shape ``(N, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_left_wrench_cmd_active - - @property - def right_wrench_cur_active(self) -> torch.Tensor: - """``right_contact_wrench_supports > 1e-3``. Shape ``(N, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_right_wrench_cur_active - - @property - def left_wrench_cur_active(self) -> torch.Tensor: - """``left_contact_wrench_supports > 1e-3``. Shape ``(N, num_bodies, num_wrench_basis)``.""" - self.refresh_tensors() - return self._cached_left_wrench_cur_active - - @property - def right_wrench_cmd_active_per_body(self) -> torch.Tensor: - """``right_wrench_cmd_active.any(-1)``. Shape ``(N, num_bodies)``.""" - self.refresh_tensors() - return self._cached_right_wrench_cmd_active_per_body - - @property - def left_wrench_cmd_active_per_body(self) -> torch.Tensor: - """``left_wrench_cmd_active.any(-1)``. Shape ``(N, num_bodies)``.""" - self.refresh_tensors() - return self._cached_left_wrench_cmd_active_per_body - - @property - def right_wrench_cur_active_per_body(self) -> torch.Tensor: - """``right_wrench_cur_active.any(-1)``. Shape ``(N, num_bodies)``.""" - self.refresh_tensors() - return self._cached_right_wrench_cur_active_per_body - - @property - def left_wrench_cur_active_per_body(self) -> torch.Tensor: - """``left_wrench_cur_active.any(-1)``. Shape ``(N, num_bodies)``.""" - self.refresh_tensors() - return self._cached_left_wrench_cur_active_per_body - - @property - def object_position_e_sq(self) -> torch.Tensor: - """Object position in env frame with body dim squeezed. Shape ``(num_envs, 3)``.""" - self.refresh_tensors() - return self._cached_object_position_e_sq - - @property - def object_wxyz_e_sq(self) -> torch.Tensor: - """Object orientation in env frame with body dim squeezed. Shape ``(num_envs, 4)``.""" - self.refresh_tensors() - return self._cached_object_wxyz_e_sq - - ###################################################################### - # Refresh machinery. - ###################################################################### - - def refresh_tensors(self) -> None: - """Recompute the shared tensors consumed by rewards / observations. - - Lazy: no-op unless ``self._tensors_dirty`` is True. Dirty is raised in - ``__init__``, at the end of ``_update_command`` (after ``timestep_counter`` - increments), and at the end of ``_resample_command``. Reward and observation - phases each trigger one refresh; within a phase repeated reads are free. - """ - if not self._tensors_dirty: - return - self._tensors_dirty = False - - # Wrench supports: fill existing self.{right,left}_contact_wrench_supports - # in place (Python-side, contains a per-body loop so it is not JIT-compiled). - self._compute_contact_wrench_supports("right") - self._compute_contact_wrench_supports("left") - - # Pure-tensor derivations fused by refresh_jit. - ( - self._cached_right_force_sq_per_link, - self._cached_left_force_sq_per_link, - self._cached_right_link_in_contact, - self._cached_left_link_in_contact, - self._cached_right_in_contact, - self._cached_left_in_contact, - self._cached_in_contact, - self._cached_ref_L, - self._cached_ref_R, - self._cached_mask_L, - self._cached_mask_R, - self._cached_ref_active_per_cell, - self._cached_ref_active_per_body, - self._cached_ref_active_global, - self._cached_right_wrench_cmd_active, - self._cached_left_wrench_cmd_active, - self._cached_right_wrench_cur_active, - self._cached_left_wrench_cur_active, - self._cached_right_wrench_cmd_active_per_body, - self._cached_left_wrench_cmd_active_per_body, - self._cached_right_wrench_cur_active_per_body, - self._cached_left_wrench_cur_active_per_body, - self._cached_object_position_e_sq, - self._cached_object_wxyz_e_sq, - ) = refresh_jit( - right_forces_w=self.right_hand_object_contact_forces_w, - left_forces_w=self.left_hand_object_contact_forces_w, - retargeted_left_contact_wrench_supports=self.retargeted_left_contact_wrench_supports, - retargeted_right_contact_wrench_supports=self.retargeted_right_contact_wrench_supports, - timestep_counter=self.timestep_counter, - right_contact_wrench_supports=self.right_contact_wrench_supports, - left_contact_wrench_supports=self.left_contact_wrench_supports, - object_position_e=self.object_position_e, - object_orientation_e=self.object_orientation_e, - ) - - def _compute_contact_wrench_supports(self, side: str) -> None: - """Fill ``self.{side}_contact_wrench_supports`` in place for one hand. - - Thin Python wrapper over two JIT helpers: ``wrench_preprocess_jit`` fuses - the contact-position/normal transform into the object COM frame for all - bodies at once, then ``wrench_support_one_body_jit`` fuses the - friction-cone edges + wrench-space assembly + support-function reduction - per body. - """ - if side == "right": - contact_positions_w = self.right_hand_object_contact_positions_w - contact_forces_w = self.right_hand_object_contact_forces_w - buffer = self.right_contact_wrench_supports - else: - contact_positions_w = self.left_hand_object_contact_positions_w - contact_forces_w = self.left_hand_object_contact_forces_w - buffer = self.left_contact_wrench_supports - - # Note: Use num_robot_contacts_right for both hands when expanding the - # object COM state (see legacy left property). Preserving that behavior. - num_robot_contacts = self.num_robot_contacts_right - - com_state = self.object_com_position_and_wxyz_w # (N, bodies, 7) - object_com_position_w = com_state[..., :3].unsqueeze(2) # (N, bodies, 1, 3) - object_com_orientation_w = com_state[..., 3:7].unsqueeze(2) # (N, bodies, 1, 4) - - contact_positions_com, contact_normals_com = wrench_preprocess_jit( - contact_positions_w=contact_positions_w, - contact_forces_first_hist_w=contact_forces_w[:, 0], - object_com_position_w=object_com_position_w, - object_com_orientation_w=object_com_orientation_w, - num_envs=self.num_envs, - num_bodies=self.num_bodies, - num_robot_contacts=num_robot_contacts, - ) - - friction_coefficients = float(self.cfg.friction_coefficients) - for body_idx, body_radius in enumerate(self.object_mesh_radius): - buffer[:, body_idx] = wrench_support_one_body_jit( - contact_points=contact_positions_com[:, body_idx], - contact_normals=contact_normals_com[:, body_idx], - cos_t=self.friction_cone_edge_cosines, - sin_t=self.friction_cone_edge_sines, - basis=self.wrench_space_bases[body_idx], - rc=float(body_radius), - friction_coefficients=friction_coefficients, - ) - - ###################################################################### - # Specific functions. - ###################################################################### - - def _update_metrics(self) -> None: - """Update the metrics.""" - # Right hand - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - self.metrics["right_hand_wrist_position_error"] = torch.norm( - self.right_hand_wrist_position_e - right_hand_wrist_pose_command_e[:, :3], - dim=-1, - ) - self.metrics["right_hand_wrist_wxyz_error"] = math_utils.quat_error_magnitude( - self.right_hand_wrist_wxyz_e, right_hand_wrist_pose_command_e[:, 3:] - ) - self.metrics["right_hand_finger_joints_error"] = torch.norm( - self.right_hand_finger_joint_pos - self.right_hand_finger_joint_pos_command, - dim=-1, - ) - # Left hand - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - self.metrics["left_hand_wrist_position_error"] = torch.norm( - self.left_hand_wrist_position_e - left_hand_wrist_pose_command_e[:, :3], - dim=-1, - ) - self.metrics["left_hand_wrist_wxyz_error"] = math_utils.quat_error_magnitude( - self.left_hand_wrist_wxyz_e, left_hand_wrist_pose_command_e[:, 3:] - ) - self.metrics["left_hand_finger_joints_error"] = torch.norm( - self.left_hand_finger_joint_pos - self.left_hand_finger_joint_pos_command, - dim=-1, - ) - # Object - self.metrics["object_body_position_error"] = torch.norm( - self.object_position_e - self.object_body_position_command_e, - dim=-1, - ).mean(dim=-1) - self.metrics["object_body_wxyz_error"] = math_utils.quat_error_magnitude( - self.object_orientation_e, - self.object_body_wxyz_command_e, - ).mean(dim=-1) - - # Log the per-env VOC scale actually applied to the object controller. - self.metrics["virtual_object_controller_scale_factor"] = ( - self.virtual_object_controller_scale_factor_per_env.squeeze(-1) - ) - - if self._has_multi_object: - pos_err, rot_err = self.relative_object_pose_error - self.metrics["relative_object_pos_error"] = pos_err - self.metrics["relative_object_rot_error"] = rot_err - - if not self.cfg.enable_additional_metrics: - return - - # ------------------------------------------------------------------ # - # Bounding-box corner tracking error - # BBOX_CORNER_VECS: (num_envs, num_bodies, 8, 3) — corners (±hx, ±hy, ±hz) - # scaled by per-body AABB half-extents computed at init from USD geometry. - # - # Conditioned on envs that are past the per-episode VOC warmup phase. - # During the warmup (steps_since_last_reset < virtual_object_control_decay_steps), - # the timestep_counter is frozen at the reset frame and VOC ≈ 1.0 — so - # error is trivially near 0 regardless of policy quality and would dilute - # the mean downward. - # ------------------------------------------------------------------ # - past_reset_phase = ( - self.steps_since_last_reset >= self.cfg.virtual_object_control_decay_steps - ) # (num_envs,) - n_past_reset = past_reset_phase.float().sum().clamp(min=1.0) - - object_position_exp = self.object_position_e.unsqueeze(2).expand(-1, -1, 8, -1) - object_wxyz_exp = self.object_orientation_e.unsqueeze(2).expand(-1, -1, 8, -1) - cmd_position_exp = self.object_body_position_command_e.unsqueeze(2).expand( - -1, -1, 8, -1 - ) - cmd_wxyz_exp = self.object_body_wxyz_command_e.unsqueeze(2).expand( - -1, -1, 8, -1 - ) - - current_corners, _ = math_utils.combine_frame_transforms( - object_position_exp, object_wxyz_exp, self.BBOX_CORNER_VECS - ) # (num_envs, num_bodies, 8, 3) - command_corners, _ = math_utils.combine_frame_transforms( - cmd_position_exp, cmd_wxyz_exp, self.BBOX_CORNER_VECS - ) - corner_error_per_env = torch.norm( - current_corners - command_corners, dim=-1 - ).mean( - dim=(-2, -1) - ) # (num_envs,), meters - self.metrics["object_bbox_corner_error"] = ( - (corner_error_per_env * past_reset_phase.float()).sum() / n_past_reset - ).expand(self.num_envs) - - # Z-only tracking error — detects "object stays on table" failure mode - z_error_per_env = torch.abs( - self.object_position_e[..., 2] - self.object_body_position_command_e[..., 2] - ).mean( - dim=-1 - ) # (num_envs,), meters - self.metrics["object_body_z_error"] = ( - (z_error_per_env * past_reset_phase.float()).sum() / n_past_reset - ).expand(self.num_envs) - - # ------------------------------------------------------------------ # - # Contact wrench support reward CV (W=200 steps) - # Scale-invariant plateau indicator: CV = std/mean over the rolling - # buffer. Low CV (≲0.07) means the reward has plateaued; high CV means - # it is still actively changing. Used as a curriculum decay gate. - # ------------------------------------------------------------------ # - cws_step_val = ( - contact_wrench_support_reward_jit( - right_cmd_active=self.right_wrench_cmd_active, - right_cur_active=self.right_wrench_cur_active, - left_cmd_active=self.left_wrench_cmd_active, - left_cur_active=self.left_wrench_cur_active, - right_cmd_active_per_body=self.right_wrench_cmd_active_per_body, - left_cmd_active_per_body=self.left_wrench_cmd_active_per_body, - right_cmd_supports=self.right_hand_contact_wrench_supports_command, - right_cur_supports=self.right_hand_contact_wrench_supports, - left_cmd_supports=self.left_hand_contact_wrench_supports_command, - left_cur_supports=self.left_hand_contact_wrench_supports, - tolerance=0.1, - var=0.1, - ) - .mean() - .item() - ) - self._cws_reward_step_buf.append(cws_step_val) - if len(self._cws_reward_step_buf) >= 10: - buf = torch.tensor( - list(self._cws_reward_step_buf), - dtype=torch.float32, - device=self.device, - ) - mean_abs = buf.mean().abs().clamp(min=1e-6) - cv = buf.std() / mean_abs - self.metrics["contact_wrench_support_reward_cv"] = cv.expand(self.num_envs) - - def _resample_command(self, env_ids: Sequence[int]) -> None: - """Resample the command.""" - n = len(env_ids) - - # Reset to a random frame from the original retargeted motion data - self.timestep_counter[env_ids] = torch.randint( - low=0, - high=self.retargeted_horizon - 1, - size=(n,), - device=self.device, - dtype=self.timestep_counter.dtype, - ) - - if self.cfg.always_reset_to_first_frame: - self.timestep_counter[env_ids] = 0 - elif ( - self.cfg.reset_to_first_frame_prob > 0.0 - and float(self.virtual_object_controller_scale_factor) < 0.1 - ): - # Close the train/eval gap: once the VOC curriculum has nearly decayed - # (scale < 0.1), randomly reset some envs to tc=0 with the configured - # probability. Eval always starts from tc=0; without this the policy - # never sees trajectory-start episodes during training and can reward-hack - # by learning to hold-in-place from mid-trajectory positions. - first_frame_mask = ( - torch.rand(n, device=self.device) < self.cfg.reset_to_first_frame_prob - ) - first_frame_local_ids = first_frame_mask.nonzero(as_tuple=False).squeeze(-1) - if first_frame_local_ids.numel() > 0: - env_ids_t = ( - env_ids - if isinstance(env_ids, torch.Tensor) - else torch.tensor(env_ids, device=self.device) - ) - self.timestep_counter[env_ids_t[first_frame_local_ids]] = 0 - - # Cache the per-env timestep indices and env-origin offsets once. - tc = self.timestep_counter[env_ids] - env_origins_sel = self._env.scene.env_origins[env_ids] - - # Update the tracking length, reset curriculum + reset-step counters. - self.tracking_lengths[env_ids] = (self.retargeted_horizon - tc).clamp(min=1) - self.virtual_object_controller_scale_factor_per_env[env_ids] = 1.0 - self.steps_since_last_reset[env_ids] = 0 - - # ── JIT-compiled pure-tensor derivations (indexing, cat, rand, clamp) ── - ( - object_pose, - object_velocity, - right_hand_wrist_position_e, - right_hand_wrist_wxyz, - left_hand_wrist_position_e, - left_hand_wrist_wxyz, - right_hand_wrist_pose, - left_hand_wrist_pose, - wrist_zero_velocity, - right_hand_finger_joint_pos, - left_hand_finger_joint_pos, - finger_zero_velocity, - ) = resample_compute_tensors_jit( - tc=tc, - env_origins_sel=env_origins_sel, - retargeted_object_body_position=self.retargeted_object_body_position, - retargeted_object_body_wxyz=self.retargeted_object_body_wxyz, - retargeted_right_wrist_position=self.retargeted_right_wrist_position, - retargeted_right_wrist_wxyz=self.retargeted_right_wrist_wxyz, - retargeted_left_wrist_position=self.retargeted_left_wrist_position, - retargeted_left_wrist_wxyz=self.retargeted_left_wrist_wxyz, - retargeted_right_finger_joints=self.retargeted_right_finger_joints, - retargeted_left_finger_joints=self.retargeted_left_finger_joints, - right_soft_joint_pos_limits_sel=self.right_robot.data.soft_joint_pos_limits[ - env_ids, : - ], - left_soft_joint_pos_limits_sel=self.left_robot.data.soft_joint_pos_limits[ - env_ids, : - ], - reset_finger_openness=float(self.cfg.reset_finger_openness), - n=n, - ) - - # Store reset wrist poses (env frame, no env_origins) for action terms. - self.reset_right_wrist_position_e[env_ids] = right_hand_wrist_position_e - self.reset_right_wrist_wxyz[env_ids] = right_hand_wrist_wxyz - self.reset_left_wrist_position_e[env_ids] = left_hand_wrist_position_e - self.reset_left_wrist_wxyz[env_ids] = left_hand_wrist_wxyz - - ########################################################## - # Reset the object - ########################################################## - - # Articulation joint state is kept in Python because it's conditional on - # the object type and handles both (horizon,) and (horizon, num_joints) - # source shapes with a dim-check the scripter can't cleanly express. - has_object_articulation = self.retargeted_object_articulation.numel() > 0 - object_joint_pos: torch.Tensor | None = None - if has_object_articulation: - object_joint_pos = self.retargeted_object_articulation[tc] - if object_joint_pos.dim() == 1: - object_joint_pos = object_joint_pos.unsqueeze(-1) - - for object_idx, object in enumerate(self.objects): - object.write_root_pose_to_sim(object_pose[:, object_idx], env_ids=env_ids) - object.write_root_velocity_to_sim( - object_velocity[:, object_idx], env_ids=env_ids - ) - if isinstance(object, Articulation) and object_joint_pos is not None: - object.write_joint_state_to_sim( - object_joint_pos, - torch.zeros_like(object_joint_pos), - env_ids=env_ids, - ) - - ########################################################## - # Reset the robots (wrists + finger joints) - ########################################################## - - self.right_robot.write_root_pose_to_sim(right_hand_wrist_pose, env_ids=env_ids) - self.right_robot.write_root_velocity_to_sim( - wrist_zero_velocity, env_ids=env_ids - ) - self.left_robot.write_root_pose_to_sim(left_hand_wrist_pose, env_ids=env_ids) - self.left_robot.write_root_velocity_to_sim(wrist_zero_velocity, env_ids=env_ids) - - self.right_robot.write_joint_state_to_sim( - right_hand_finger_joint_pos, finger_zero_velocity, env_ids=env_ids - ) - self.left_robot.write_joint_state_to_sim( - left_hand_finger_joint_pos, finger_zero_velocity, env_ids=env_ids - ) - - ########################################################## - # Refresh the simulation - ########################################################## - - # Force a kinematic/data refresh after reset to synchronize states. - self._env.sim.forward() - self._env.scene.update(dt=self._env.physics_dt) - - # Reset invalidates all cached tensors. - self._tensors_dirty = True - - def _update_command(self) -> None: - """Update the command.""" - self.steps_since_last_reset += 1 - - # Decay virtual object control scale factor toward curriculum scale - if self.cfg.virtual_object_control_decay_mode == "linear": - progress = self.steps_since_last_reset.float() / max( - self.cfg.virtual_object_control_decay_steps, 1 - ) - self.virtual_object_controller_scale_factor_per_env[:] = ( - ( - 1.0 - + (self.virtual_object_controller_scale_factor - 1.0) - * progress.clamp(max=1.0) - ) - .clamp(min=0.0) - .view(self.num_envs, 1) - ) - elif self.cfg.virtual_object_control_decay_mode in ( - "step", - "fixed_schedule", - "custom_schedule", - ): - self.virtual_object_controller_scale_factor_per_env[ - self.steps_since_last_reset - >= self.cfg.virtual_object_control_decay_steps - ] = self.virtual_object_controller_scale_factor - else: - raise ValueError( - f"Unknown virtual_object_control_decay_mode: {self.cfg.virtual_object_control_decay_mode!r}. " - "Expected one of: 'linear', 'step', 'fixed_schedule', 'custom_schedule'." - ) - - # If still in the reset phase, don't step the timestep counter - # Note: This will make each episode length vary, which might cause the episode rewards to be variant. - not_in_reset_phase_env_ids = ( - self.steps_since_last_reset >= self.cfg.virtual_object_control_decay_steps - ) - self.timestep_counter[not_in_reset_phase_env_ids] += 1 - - # Mark cached tensors stale; next call to refresh_tensors() (first obs of - # this step, or first reward/obs of step N+1) will repopulate them. - self._tensors_dirty = True - - def _get_retargeted_contact_counts(self) -> None: - """Get the number of retargeted contacts for each hand from retargeted contact data.""" - self.num_retargeted_contacts_left = ( - self.retargeted_left_object_contact_positions_o.shape[1] - ) - self.num_retargeted_contacts_right = ( - self.retargeted_right_object_contact_positions_o.shape[1] - ) - self.total_num_retargeted_contacts = ( - self.num_retargeted_contacts_left + self.num_retargeted_contacts_right - ) - - def _set_debug_vis_impl(self, debug_vis: bool) -> None: - """Set the debug visibility.""" - if debug_vis: - if not hasattr(self, "right_hand_pose_visualizer"): - self.right_hand_pose_visualizer = VisualizationMarkers( - self.cfg.right_hand_pose_visualizer_cfg - ) - self.right_hand_pose_visualizer.set_visibility(True) - if not hasattr(self, "left_hand_pose_visualizer"): - self.left_hand_pose_visualizer = VisualizationMarkers( - self.cfg.left_hand_pose_visualizer_cfg - ) - self.left_hand_pose_visualizer.set_visibility(True) - - # Command visualizers - if not hasattr(self, "right_hand_goal_pose_visualizer"): - self.right_hand_goal_pose_visualizer = VisualizationMarkers( - self.cfg.right_hand_goal_pose_visualizer_cfg - ) - self.right_hand_goal_pose_visualizer.set_visibility(True) - if not hasattr(self, "left_hand_goal_pose_visualizer"): - self.left_hand_goal_pose_visualizer = VisualizationMarkers( - self.cfg.left_hand_goal_pose_visualizer_cfg - ) - self.left_hand_goal_pose_visualizer.set_visibility(True) - - elif hasattr(self, "goal_pose_visualizer"): - self.right_hand_pose_visualizer.set_visibility(False) - self.left_hand_pose_visualizer.set_visibility(False) - self.object_goal_pose_visualizer.set_visibility(False) - self.right_hand_goal_pose_visualizer.set_visibility(False) - self.left_hand_goal_pose_visualizer.set_visibility(False) - for visualizer in getattr(self, "object_pose_visualizers", []): - visualizer.set_visibility(False) - for visualizer in getattr(self, "object_com_visualizers", []): - visualizer.set_visibility(False) - for visualizer in getattr(self, "contact_marker_visualizers", []): - visualizer.set_visibility(False) - for visualizer in getattr(self, "robot_contact_marker_visualizers", []): - visualizer.set_visibility(False) - - def _set_contact_vis_impl(self, debug_vis: bool) -> None: - """Set the contact visibility.""" - if debug_vis: - # Current object pose visualizers - self.object_pose_visualizers = [] - self.object_com_visualizers = [] - self.object_goal_pose_visualizers = [] - for body_idx in range(self.num_bodies): - object_pose_visualizer_cfg = ( - self.cfg.object_pose_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/object_marker_{body_idx}" - ) - ) - object_pose_visualizer = VisualizationMarkers( - object_pose_visualizer_cfg - ) - object_pose_visualizer.set_visibility(True) - self.object_pose_visualizers.append(object_pose_visualizer) - - object_com_visualizer_cfg = ( - self.cfg.object_com_pose_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/object_com_marker_{body_idx}" - ) - ) - object_com_visualizer = VisualizationMarkers(object_com_visualizer_cfg) - object_com_visualizer.set_visibility(True) - self.object_com_visualizers.append(object_com_visualizer) - - object_goal_pose_visualizer_cfg = ( - self.cfg.object_goal_pose_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/object_goal_marker_{body_idx}" - ) - ) - object_goal_pose_visualizer = VisualizationMarkers( - object_goal_pose_visualizer_cfg - ) - object_goal_pose_visualizer.set_visibility(True) - self.object_goal_pose_visualizers.append(object_goal_pose_visualizer) - - # Contact marker visualizers - self.command_contact_marker_visualizers = [] - for i in range(self.total_num_retargeted_contacts): - command_contact_vis_cfg = ( - self.cfg.target_contact_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/TargetContact_{i}" - ) - ) - command_contact_marker_visualizer = VisualizationMarkers( - command_contact_vis_cfg - ) - command_contact_marker_visualizer.set_visibility(True) - self.command_contact_marker_visualizers.append( - command_contact_marker_visualizer - ) - - # Robot current contact marker visualizers - self.robot_contact_marker_visualizers = [] - for i in range(self.total_num_robot_contacts * self.num_bodies): - robot_contact_vis_cfg = self.cfg.current_contact_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/CurrentContact_{i}" - ) - robot_contact_marker_visualizer = VisualizationMarkers( - robot_contact_vis_cfg - ) - robot_contact_marker_visualizer.set_visibility(True) - self.robot_contact_marker_visualizers.append( - robot_contact_marker_visualizer - ) - - self.command_fingertip_marker_visualizers = [] - self.robot_fingertip_marker_visualizers = [] - - for i in range( - len(self.right_fingertip_body_ids) + len(self.left_fingertip_body_ids) - ): - command_fingertip_vis_cfg = ( - self.cfg.target_fingertip_position_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/TargetFingertip_{i}" - ) - ) - command_fingertip_marker_visualizer = VisualizationMarkers( - command_fingertip_vis_cfg - ) - command_fingertip_marker_visualizer.set_visibility(True) - self.command_fingertip_marker_visualizers.append( - command_fingertip_marker_visualizer - ) - - robot_fingertip_vis_cfg = ( - self.cfg.current_fingertip_position_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/CurrentFingertip_{i}" - ) - ) - robot_fingertip_marker_visualizer = VisualizationMarkers( - robot_fingertip_vis_cfg - ) - robot_fingertip_marker_visualizer.set_visibility(True) - self.robot_fingertip_marker_visualizers.append( - robot_fingertip_marker_visualizer - ) - - if omni_debug_draw is not None: - self.draw_interface = omni_debug_draw.acquire_debug_draw_interface() - - def _debug_vis_callback(self, event: Any) -> None: - """Visualize the goal marker.""" - del event # unused - if not hasattr(self._env, "scene"): - return - if hasattr(self, "draw_interface"): - self.draw_interface.clear_lines() - - # Current state visualizers - for body_idx, object_pose_visualizer in enumerate(self.object_pose_visualizers): - object_pose_visualizer.visualize( - translations=self.object_position_w[:, body_idx], - orientations=self.object_orientation_e[:, body_idx], - ) - object_com_state_w = self.object_com_position_and_wxyz_w - for body_idx, object_com_visualizer in enumerate(self.object_com_visualizers): - object_com_visualizer.visualize( - translations=object_com_state_w[:, body_idx, :3], - orientations=object_com_state_w[:, body_idx, 3:7], - ) - self.right_hand_pose_visualizer.visualize( - translations=self.right_robot.data.root_link_pos_w, - orientations=self.right_robot.data.root_link_quat_w, - ) - self.left_hand_pose_visualizer.visualize( - translations=self.left_robot.data.root_link_pos_w, - orientations=self.left_robot.data.root_link_quat_w, - ) - # Command visualizers - for body_idx, object_goal_pose_visualizer in enumerate( - self.object_goal_pose_visualizers - ): - object_goal_pose_visualizer.visualize( - translations=self.object_body_position_command_e[:, body_idx] - + self._env.scene.env_origins, - orientations=self.object_body_wxyz_command_e[:, body_idx], - ) - right_hand_wrist_pose_command_e = self.right_hand_wrist_pose_command_e - self.right_hand_goal_pose_visualizer.visualize( - translations=right_hand_wrist_pose_command_e[:, :3] - + self._env.scene.env_origins, - orientations=right_hand_wrist_pose_command_e[:, 3:], - ) - left_hand_wrist_pose_command_e = self.left_hand_wrist_pose_command_e - self.left_hand_goal_pose_visualizer.visualize( - translations=left_hand_wrist_pose_command_e[:, :3] - + self._env.scene.env_origins, - orientations=left_hand_wrist_pose_command_e[:, 3:], - ) - - # Visualize target contact location on object - left_hand_contact_command_positions_and_normals_e = ( - self.left_hand_object_contact_command_positions_and_normals_e - ) - right_hand_contact_command_positions_and_normals_e = ( - self.right_hand_object_contact_command_positions_and_normals_e - ) - for i in range(self.total_num_retargeted_contacts): - # Select left or right hand contacts - if i < self.num_retargeted_contacts_left: - # Left contacts - contact_command_positions_e = ( - left_hand_contact_command_positions_and_normals_e[:, i, :3] - ) - contact_command_normals_e = ( - left_hand_contact_command_positions_and_normals_e[:, i, 3:] - ) - else: - # Right contacts - contact_command_positions_e = ( - right_hand_contact_command_positions_and_normals_e[ - :, i - self.num_retargeted_contacts_left, :3 - ] - ) - contact_command_normals_e = ( - right_hand_contact_command_positions_and_normals_e[ - :, i - self.num_retargeted_contacts_left, 3: - ] - ) - - self.command_contact_marker_visualizers[i].visualize( - translations=contact_command_positions_e + self._env.scene.env_origins, - orientations=self.QUAT_UNIT_VEC, - ) - if hasattr(self, "draw_interface"): - self.draw_interface.draw_lines( - ( - contact_command_positions_e + self._env.scene.env_origins - ).tolist(), - ( - contact_command_positions_e - + self._env.scene.env_origins - + contact_command_normals_e * 0.05 - ).tolist(), - [[0.0, 0.4, 1.0, 1.0]] * len(contact_command_positions_e), - [3.0] * len(contact_command_positions_e), - ) - - # Visualize current contacts - right_hand_contact_positions_w = ( - self.right_hand_object_contact_positions_w.view(self.num_envs, -1, 3) - ) - right_hand_contact_forces_w = self.right_hand_object_contact_forces_w[ - :, 0 - ].view(self.num_envs, -1, 3) - for i in range(self.num_bodies * self.num_robot_contacts_right): - self.robot_contact_marker_visualizers[i].visualize( - translations=right_hand_contact_positions_w[:, i], - orientations=self.QUAT_UNIT_VEC, - ) - if hasattr(self, "draw_interface"): - self.draw_interface.draw_lines( - (right_hand_contact_positions_w[:, i]).tolist(), - ( - right_hand_contact_positions_w[:, i] - + right_hand_contact_forces_w[:, i] * 0.1 - ).tolist(), - [[0.4, 0.0, 1.0, 1.0]] * len(right_hand_contact_positions_w), - [3.0] * len(right_hand_contact_positions_w), - ) - - left_hand_contact_positions_w = self.left_hand_object_contact_positions_w.view( - self.num_envs, -1, 3 - ) - left_hand_contact_forces_w = self.left_hand_object_contact_forces_w[:, 0].view( - self.num_envs, -1, 3 - ) - for i in range(self.num_bodies * self.num_robot_contacts_left): - self.robot_contact_marker_visualizers[ - i + self.num_robot_contacts_right - ].visualize( - translations=left_hand_contact_positions_w[:, i], - orientations=self.QUAT_UNIT_VEC, - ) - if hasattr(self, "draw_interface"): - self.draw_interface.draw_lines( - (left_hand_contact_positions_w[:, i]).tolist(), - ( - left_hand_contact_positions_w[:, i] - + left_hand_contact_forces_w[:, i] * 0.1 - ).tolist(), - [[0.4, 0.0, 1.0, 1.0]] * len(left_hand_contact_positions_w), - [3.0] * len(left_hand_contact_positions_w), - ) - - # Visualize fingertip positions - for i in range( - len(self.right_fingertip_body_ids) + len(self.left_fingertip_body_ids) - ): - if i < len(self.right_fingertip_body_ids): - self.command_fingertip_marker_visualizers[i].visualize( - translations=self.right_hand_fingertip_position_command_e[:, i] - + self._env.scene.env_origins, - orientations=self.QUAT_UNIT_VEC, - ) - self.robot_fingertip_marker_visualizers[i].visualize( - translations=self.right_hand_fingertip_position_e[:, i] - + self._env.scene.env_origins, - orientations=self.QUAT_UNIT_VEC, - ) - else: - self.command_fingertip_marker_visualizers[i].visualize( - translations=self.left_hand_fingertip_position_command_e[ - :, i - len(self.right_fingertip_body_ids) - ] - + self._env.scene.env_origins, - orientations=self.QUAT_UNIT_VEC, - ) - self.robot_fingertip_marker_visualizers[i].visualize( - translations=self.left_hand_fingertip_position_e[ - :, i - len(self.right_fingertip_body_ids) - ] - + self._env.scene.env_origins, - orientations=self.QUAT_UNIT_VEC, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/curriculum.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/curriculum.py deleted file mode 100644 index 2a7c6a99..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/curriculum.py +++ /dev/null @@ -1,858 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import ast as _ast -import bisect -import logging -from collections.abc import Callable, Sequence - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import CurriculumTermCfg, ManagerTermBase - -from robotic_grounding.tasks.v2p.mdp.utils import TensorDeque - -logger = logging.getLogger(__name__) - - -class VirtualObjectControlCurriculum(ManagerTermBase): - """Curriculum for virtual object control. - - Decay the virtual object control scale factor when: - 1. Passed the initial wait period. - 2. Passed the cooldown period. - 3. The episode reward deque is full. - 4. The mean episode length ratio exceeds the threshold. - 5. The mean episode rewards exceed the thresholds. - 6. (Optional) The mean command metric values exceed the metric_thresholds. - """ - - def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the term. - - Args: - cfg: The configuration of the curriculum. - env: The RL environment instance. - """ - super().__init__(cfg, env) - - self._step_dt = self._env.step_dt - - # Command term - self._command = env.command_manager.get_term(cfg.params["command_name"]) - - # Reward manager, reward names, and weights - self._reward_manager = env.reward_manager - self._reward_names = list(cfg.params["reward_thresholds"].keys()) - _available_reward_names = self._env.reward_manager._term_names - for reward_name in self._reward_names: - assert ( - reward_name in _available_reward_names - ), f"Reward name {reward_name} not found in available reward names {_available_reward_names}" - self._reward_weights = torch.tensor( - [ - self._reward_manager.get_term_cfg(reward_name).weight - for reward_name in self._reward_names - ], - device=self._env.device, - ) - - # Reward thresholds - self._reward_thresholds = torch.tensor( - list(cfg.params["reward_thresholds"].values()), device=self._env.device - ) # (num_reward,) - - # Reward episode previous means - self._episode_reward_prev_means: torch.Tensor = torch.zeros( - len(self._reward_names), device=self._env.device - ) # (num_reward,) - - # Episode reward deque - self._episode_reward_deque = TensorDeque( - capacity=cfg.params["deque_maxlen"], - feature_shape=len(self._reward_names), - device=self._env.device, - ) - - # Episode length ratio deque - self._episode_length_ratio_deque = TensorDeque( - capacity=cfg.params["deque_maxlen"], - feature_shape=1, - device=self._env.device, - ) - - # The common step counter when the last decay was applied - self._last_decay_common_step_counter: int = 0 - - # Cached deque statistics — updated each curriculum call and written to - # the command term's metrics so convergence is visible in W&B even when - # the deque is cleared after a decay. - self._deque_reward_means = torch.zeros( - len(self._reward_names), device=self._env.device - ) - self._deque_reward_stds = torch.zeros( - len(self._reward_names), device=self._env.device - ) - self._deque_ep_len_ratio_mean = torch.zeros(1, device=self._env.device) - self._deque_ep_len_ratio_std = torch.zeros(1, device=self._env.device) - - # Optional metric thresholds (e.g. contact_wrench_support_ratio). - # Sampled at episode end from command metrics; averaged over the deque window. - # Entries with threshold == 0.0 are treated as disabled and excluded. - _raw = cfg.params.get("metric_thresholds", {}) - self._metric_names: list[str] = [k for k, v in _raw.items() if float(v) > 0.0] - self._metric_thresholds = torch.tensor( - [float(v) for v in _raw.values() if float(v) > 0.0], device=self._env.device - ) - if self._metric_names: - self._metric_deque: TensorDeque | None = TensorDeque( - capacity=cfg.params["deque_maxlen"], - feature_shape=len(self._metric_names), - device=self._env.device, - ) - else: - self._metric_deque = None - self._deque_metric_means = torch.zeros( - len(self._metric_names), device=self._env.device - ) - self._deque_metric_stds = torch.zeros( - len(self._metric_names), device=self._env.device - ) - - # Optional upper-bound metric thresholds: metric must be BELOW the threshold. - # Read directly from command metrics each step (not averaged via deque). - # Use for scale-invariant stability signals like contact_wrench_support_reward_cv. - # Entries with threshold == 0.0 are treated as disabled and excluded. - # If metric_upper_thresholds_initial_only=True, gate only applies before the first decay. - _raw_upper = cfg.params.get("metric_upper_thresholds", {}) - self._metric_upper_names: list[str] = [ - k for k, v in _raw_upper.items() if float(v) > 0.0 - ] - self._metric_upper_thresholds_vals: list[float] = [ - float(v) for v in _raw_upper.values() if float(v) > 0.0 - ] - self._metric_upper_initial_only: bool = bool( - cfg.params.get("metric_upper_thresholds_initial_only", False) - ) - - # Custom VOC schedule (decay_mode == "custom_schedule"): explicit list of - # VOC values to step through one-at-a-time when gates pass, with optional - # paired reward-weight updates. Empty lists → custom_schedule disabled. - _raw_voc = cfg.params.get("custom_voc_schedule", []) - if isinstance(_raw_voc, str): - _raw_voc = _ast.literal_eval(_raw_voc) - self._custom_voc_schedule: list[float] = [float(v) for v in (_raw_voc or [])] - - _raw_reward_sched = cfg.params.get("custom_reward_schedules", {}) - self._custom_reward_schedules: dict[str, list[float]] = {} - for _rname, _weights in (_raw_reward_sched or {}).items(): - if isinstance(_weights, str): - _weights = _ast.literal_eval(_weights) - _wlist = [float(w) for w in (_weights or [])] - if _wlist: - self._custom_reward_schedules[_rname] = _wlist - - if self._custom_voc_schedule: - _avail = self._env.reward_manager._term_names - for _rname, _wlist in self._custom_reward_schedules.items(): - assert ( - _rname in _avail - ), f"custom_reward_schedules key '{_rname}' not in rewards: {_avail}" - assert len(_wlist) == len(self._custom_voc_schedule), ( - f"custom_reward_schedules['{_rname}'] length {len(_wlist)} != " - f"custom_voc_schedule length {len(self._custom_voc_schedule)}" - ) - - # Index into custom_voc_schedule; -1 means no decay has fired yet. - self._schedule_index: int = -1 - - # Force-decay timeout: common_step when this decay opportunity first became - # eligible (past both initial_wait and cooldown). Reset to None after each decay. - # Used by max_eligible_wait_env_steps to force a decay if gates never fire. - self._eligible_since_common_step: int | None = None - - # Reward baseline retention: gates the NEXT decay on current deque mean - # being >= (deque mean at last decay) * retention_ratio. - # Trajectory-relative — handles varying plateau values across sequences. - # Skipped before the first decay (no baseline yet). - _raw_retention = cfg.params.get("reward_baseline_retention", {}) - # 0.0 = disabled (same convention as metric_thresholds / reward_thresholds) - self._baseline_reward_names: list[str] = [ - k for k, v in _raw_retention.items() if float(v) > 0.0 - ] - for rname in self._baseline_reward_names: - assert rname in self._reward_names, ( - f"reward_baseline_retention key '{rname}' not in reward_thresholds " - f"(available: {self._reward_names})" - ) - self._baseline_reward_indices: list[int] = [ - self._reward_names.index(n) for n in self._baseline_reward_names - ] - self._baseline_retention_ratios = torch.tensor( - [float(v) for v in _raw_retention.values() if float(v) > 0.0], - device=self._env.device, - ) # (num_baseline_rewards,) - # None until the first decay fires; no retention check before first decay. - self._reward_baselines: torch.Tensor | None = None - # Cached for W&B logging (0 until first decay). - self._deque_reward_baselines = torch.zeros( - len(self._baseline_reward_names), device=self._env.device - ) - - # Deferred eval-before-decay: when env.pre_decay_eval_enabled is True, - # decay application is deferred until the eval callback clears - # env.pre_decay_eval_pending. _deferred_decay_fn is a zero-arg callable - # that applies the pending decay once the eval pass finishes. - self._decay_deferred: bool = False - self._deferred_decay_fn: Callable[[], None] | None = None - - def __call__( - self, - env: ManagerBasedRLEnv, - env_ids: Sequence[int], - reward_thresholds: dict[str, float], - episode_length_ratio_threshold: float, - decay_mode: str, - deque_maxlen: int, - command_name: str, - zero_scale_factor_threshold: float, - initial_wait_env_steps: int, - wait_env_steps_since_last_decay: int, - exponential_decay_factor: float, - linear_decay_step: float, - fixed_schedule_steps: list | None = None, - fixed_schedule_values: list | None = None, - metric_thresholds: dict[str, float] | None = None, - reward_baseline_retention: dict[str, float] | None = None, - custom_voc_schedule: list | None = None, - custom_reward_schedules: dict | None = None, - max_eligible_wait_env_steps: int = 0, - metric_upper_thresholds: dict[str, float] | None = None, - metric_upper_thresholds_initial_only: bool = False, - ) -> torch.Tensor: - """Apply the curriculum.""" - # 1 Add normalized episode reward to the deque - num_reset_envs = len(env_ids) - - # 1.1 Get episode length - L = self._env.episode_length_buf[env_ids].unsqueeze(1) # (num_reset_envs, 1) - L_max = self._command.tracking_lengths[env_ids].unsqueeze( - 1 - ) # (num_reset_envs, 1) - self._episode_length_ratio_deque.append_batch(L / L_max) - - # 1.2 Get episode reward - episode_rewards: dict[str, torch.Tensor] = getattr( - self._reward_manager, "_episode_sums", {} - ) - episode_rewards_batch: torch.Tensor = torch.zeros( - num_reset_envs, len(self._reward_names), device=self._env.device - ) # (num_reset_envs, num_reward) - for reward_idx, reward_name in enumerate(self._reward_names): - episode_rewards_batch[:, reward_idx] = episode_rewards[reward_name][ - env_ids - ] # (num_reset_envs,) - episode_rewards_batch = episode_rewards_batch / ( - self._step_dt * L.clamp(min=1) - ) # (num_reset_envs, num_reward) — normalize by actual episode length, not fixed horizon - self._episode_reward_deque.append_batch(episode_rewards_batch) - - # 1.3 Update and log deque convergence statistics. - # Written to the command term's metrics so they appear in W&B. - # Cached so values persist across the deque clear that follows a decay. - n = env.num_envs - if len(self._episode_reward_deque) > 0: - all_rewards = self._episode_reward_deque.get_all() # (size, num_rewards) - self._deque_reward_means = all_rewards.mean(dim=0) - self._deque_reward_stds = ( - all_rewards.std(dim=0) - if all_rewards.shape[0] > 1 - else torch.zeros_like(self._deque_reward_means) - ) - if len(self._episode_length_ratio_deque) > 0: - all_ratios = self._episode_length_ratio_deque.get_all() # (size, 1) - self._deque_ep_len_ratio_mean = all_ratios.mean(dim=0) - self._deque_ep_len_ratio_std = ( - all_ratios.std(dim=0) - if all_ratios.shape[0] > 1 - else torch.zeros_like(self._deque_ep_len_ratio_mean) - ) - for i, name in enumerate(self._reward_names): - self._command.metrics[f"curriculum_reward_mean_{name}"] = ( - self._deque_reward_means[i].expand(n) - ) - self._command.metrics[f"curriculum_reward_std_{name}"] = ( - self._deque_reward_stds[i].expand(n) - ) - self._command.metrics["curriculum_ep_len_ratio_mean"] = ( - self._deque_ep_len_ratio_mean.expand(n) - ) - self._command.metrics["curriculum_ep_len_ratio_std"] = ( - self._deque_ep_len_ratio_std.expand(n) - ) - for i, name in enumerate(self._baseline_reward_names): - self._command.metrics[f"curriculum_reward_baseline_{name}"] = ( - self._deque_reward_baselines[i].expand(n) - ) - - # 1.4 Sample command metrics at episode end and update metric deque. - # Metrics are global averages (same across all envs); we replicate num_reset_envs - # times so the metric deque fills at the same rate as the reward deque. - if self._metric_deque is not None: - metric_vals = torch.stack( - [self._command.metrics[name][0:1] for name in self._metric_names], - dim=-1, - ) # (1, num_metrics) - self._metric_deque.append_batch(metric_vals.expand(num_reset_envs, -1)) - if len(self._metric_deque) > 0: - all_metrics = self._metric_deque.get_all() # (size, num_metrics) - self._deque_metric_means = all_metrics.mean(dim=0) - self._deque_metric_stds = ( - all_metrics.std(dim=0) - if all_metrics.shape[0] > 1 - else torch.zeros_like(self._deque_metric_means) - ) - for i, name in enumerate(self._metric_names): - self._command.metrics[f"curriculum_metric_mean_{name}"] = ( - self._deque_metric_means[i].expand(n) - ) - self._command.metrics[f"curriculum_metric_std_{name}"] = ( - self._deque_metric_stds[i].expand(n) - ) - - # 1.5 Deferred eval-before-decay guard. - # While a decay is pending an eval pass, collect episode data (section 1) - # normally but skip all gate evaluation. Once the eval callback clears - # env.pre_decay_eval_pending, apply the stored decay and return. - if self._decay_deferred: - if not getattr(self._env, "pre_decay_eval_pending", False): - if self._deferred_decay_fn is not None: - self._deferred_decay_fn() - self._deferred_decay_fn = None - self._decay_deferred = False - return self._command.virtual_object_controller_scale_factor - - # 2 Fixed schedule shortcut: set VOC based on common_step_counter thresholds, - # bypassing all adaptive conditions (episode length, reward thresholds, etc.) - if decay_mode == "fixed_schedule": - steps = [int(s) for s in (fixed_schedule_steps or [])] - values = [float(v) for v in (fixed_schedule_values or [])] - current_step: int = int(self._env.common_step_counter) - current_scale = float(self._command.virtual_object_controller_scale_factor) - # Walk through schedule: last threshold that has been passed wins - target_scale = current_scale - for step_threshold, value in zip(steps, values, strict=False): - if current_step >= step_threshold: - target_scale = value - if target_scale != current_scale: - if getattr(self._env, "pre_decay_eval_enabled", False): - _cs, _ts, _cur_s = current_step, target_scale, current_scale - - def _apply_fixed( - *, _cs: int = _cs, _ts: float = _ts, _cur_s: float = _cur_s - ) -> None: - self._command.virtual_object_controller_scale_factor = _ts - self._last_decay_common_step_counter = _cs - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - logger.info( - "[FixedSchedule] VOC scale %.3f → %.3f at common_step=%d", - _cur_s, - _ts, - _cs, - ) - - self._env.pre_decay_eval_pending = True - self._deferred_decay_fn = _apply_fixed - self._decay_deferred = True - else: - env.video_trigger_pending = True - self._command.virtual_object_controller_scale_factor = target_scale - self._last_decay_common_step_counter = current_step - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - logger.info( - "[FixedSchedule] VOC scale %.3f → %.3f at common_step=%d", - current_scale, - target_scale, - current_step, - ) - return self._command.virtual_object_controller_scale_factor - - # 2.5 Custom schedule: exit early if all VOC levels already applied - if decay_mode == "custom_schedule": - if self._schedule_index >= len(self._custom_voc_schedule) - 1: - return self._command.virtual_object_controller_scale_factor - - # 3 Whether the control scale is already zero - control_scale_is_zero = ( - self._command.virtual_object_controller_scale_factor == 0.0 - ) - if control_scale_is_zero: - return self._command.virtual_object_controller_scale_factor - - # 4 Wait initial_wait_env_steps steps until the first decay - pass_wait_iterations = self._env.common_step_counter > initial_wait_env_steps - if not pass_wait_iterations: - return self._command.virtual_object_controller_scale_factor - - # 5 Wait for a cooldown period - pass_cooldown_period = ( - self._env.common_step_counter - > self._last_decay_common_step_counter + wait_env_steps_since_last_decay - ) - if not pass_cooldown_period: - self._eligible_since_common_step = None - return self._command.virtual_object_controller_scale_factor - - # 5.5 Track when this decay opportunity first became eligible (past both - # initial_wait and cooldown). If max_eligible_wait_env_steps is set and we - # have been eligible for longer than that without the gates firing, force decay. - current_common_step: int = int(self._env.common_step_counter) - if self._eligible_since_common_step is None: - self._eligible_since_common_step = current_common_step - - force_decay = ( - decay_mode == "custom_schedule" - and max_eligible_wait_env_steps > 0 - and current_common_step - self._eligible_since_common_step - >= max_eligible_wait_env_steps - ) - if force_decay: - if getattr(self._env, "pre_decay_eval_enabled", False): - _idx = self._schedule_index + 1 - _old = float(self._command.virtual_object_controller_scale_factor) - _new = self._custom_voc_schedule[_idx] - _ef = current_common_step - self._eligible_since_common_step - _ccs = current_common_step - _slen = len(self._custom_voc_schedule) - - def _apply_force( - *, - _idx: int = _idx, - _old: float = _old, - _new: float = _new, - _ef: int = _ef, - _ccs: int = _ccs, - _slen: int = _slen, - ) -> None: - self._schedule_index = _idx - self._command.virtual_object_controller_scale_factor = _new - for rname, weights in self._custom_reward_schedules.items(): - self._reward_manager.get_term_cfg(rname).weight = weights[_idx] - self._last_decay_common_step_counter = _ccs - self._eligible_since_common_step = None - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - if self._metric_deque is not None: - self._metric_deque.clear() - logger.info( - "[CustomSchedule] FORCED VOC %.3f → %.3f at common_step=%d (eligible for %d steps, stage %d/%d)", - _old, - _new, - _ccs, - _ef, - _idx + 1, - _slen, - ) - - self._env.pre_decay_eval_pending = True - self._deferred_decay_fn = _apply_force - self._decay_deferred = True - return self._command.virtual_object_controller_scale_factor - - env.video_trigger_pending = True - self._schedule_index += 1 - old_voc = float(self._command.virtual_object_controller_scale_factor) - new_voc = self._custom_voc_schedule[self._schedule_index] - self._command.virtual_object_controller_scale_factor = new_voc - for rname, weights in self._custom_reward_schedules.items(): - self._reward_manager.get_term_cfg(rname).weight = weights[ - self._schedule_index - ] - eligible_for = current_common_step - self._eligible_since_common_step - self._last_decay_common_step_counter = current_common_step - self._eligible_since_common_step = None - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - if self._metric_deque is not None: - self._metric_deque.clear() - logger.info( - "[CustomSchedule] FORCED VOC %.3f → %.3f at common_step=%d (eligible for %d steps, stage %d/%d)", - old_voc, - new_voc, - current_common_step, - eligible_for, - self._schedule_index + 1, - len(self._custom_voc_schedule), - ) - return self._command.virtual_object_controller_scale_factor - - # 6 Wait until the deque is full - queue_is_full = self._episode_reward_deque.is_full() - if not queue_is_full: - return self._command.virtual_object_controller_scale_factor - - # 7 Mean episode lengths exceed the thresholds - episode_length_ratio_means = torch.mean( - self._episode_length_ratio_deque.get_all() - ).item() - pass_episode_length_ratio_threshold = ( - episode_length_ratio_means > episode_length_ratio_threshold - ) - - if not pass_episode_length_ratio_threshold: - return self._command.virtual_object_controller_scale_factor - - # 8 Mean episode rewards exceed thresholds - episode_reward_means = torch.mean( - self._episode_reward_deque.get_all(), dim=0 - ) # (num_reward,) - pass_episode_reward_threshold = torch.all( - episode_reward_means >= self._reward_thresholds - ).item() - self._episode_reward_prev_means[:] = episode_reward_means - - if not pass_episode_reward_threshold: - return self._command.virtual_object_controller_scale_factor - - # 8.3 Reward baseline retention — require current deque mean >= (baseline at last - # decay) * retention_ratio. Trajectory-relative: prevents decay when contact quality - # has regressed since the previous VOC level. Skipped before the first decay. - if ( - self._reward_baselines is not None - and len(self._baseline_reward_indices) > 0 - ): - current_vals = episode_reward_means[self._baseline_reward_indices] - required = self._reward_baselines * self._baseline_retention_ratios - if not torch.all(current_vals >= required).item(): - return self._command.virtual_object_controller_scale_factor - - # 8.5 Mean command metric values exceed thresholds (if metric_thresholds configured) - if self._metric_deque is not None and len(self._metric_deque) > 0: - metric_means = self._metric_deque.get_all().mean(dim=0) # (num_metrics,) - if not torch.all(metric_means >= self._metric_thresholds).item(): - return self._command.virtual_object_controller_scale_factor - - # 8.6 Upper-bound metric thresholds — current metric must be BELOW threshold. - # Designed for scale-invariant plateau signals like contact_wrench_support_reward_cv - # where a LOW value indicates the reward has stabilized. - # When metric_upper_thresholds_initial_only=True, skip this gate after the first decay. - _upper_gate_active = self._metric_upper_names and ( - not self._metric_upper_initial_only or self._schedule_index == -1 - ) - if _upper_gate_active: - for name, threshold in zip( # noqa: B905 - self._metric_upper_names, - self._metric_upper_thresholds_vals, - ): - if name not in self._command.metrics: - logger.warning( - "[Curriculum] metric_upper_thresholds: '%s' not found in command metrics, skipping.", - name, - ) - continue - current_val = self._command.metrics[name][0].item() - if current_val > threshold: - return self._command.virtual_object_controller_scale_factor - - # 9 Signal that a decay is about to happen. With pre_decay_eval_enabled, - # defer the actual decay until the eval callback has run at the pre-decay - # VOC level and cleared env.pre_decay_eval_pending. - if getattr(self._env, "pre_decay_eval_enabled", False): - _cmode = decay_mode - _exp_f = exponential_decay_factor - _lin_s = linear_decay_step - _zero_t = zero_scale_factor_threshold - _erm = episode_reward_means.clone() - _ccs = int(self._env.common_step_counter) - _slen = len(self._custom_voc_schedule) - - def _apply_adaptive( - *, - _cmode: str = _cmode, - _exp_f: float = _exp_f, - _lin_s: float = _lin_s, - _zero_t: float = _zero_t, - _erm: torch.Tensor = _erm, - _ccs: int = _ccs, - _slen: int = _slen, - ) -> None: - old_voc = float(self._command.virtual_object_controller_scale_factor) - if _cmode == "exponential": - self._command.virtual_object_controller_scale_factor *= _exp_f - elif _cmode == "linear": - self._command.virtual_object_controller_scale_factor -= _lin_s - elif _cmode == "custom_schedule": - self._schedule_index += 1 - new_voc = self._custom_voc_schedule[self._schedule_index] - self._command.virtual_object_controller_scale_factor = new_voc - for rname, weights in self._custom_reward_schedules.items(): - self._reward_manager.get_term_cfg(rname).weight = weights[ - self._schedule_index - ] - logger.info( - "[CustomSchedule] VOC %.3f → %.3f at common_step=%d (stage %d/%d)", - old_voc, - new_voc, - _ccs, - self._schedule_index + 1, - _slen, - ) - else: - raise ValueError(f"Invalid decay mode: {_cmode}") - if _cmode != "custom_schedule" and ( - self._command.virtual_object_controller_scale_factor <= _zero_t - ): - self._command.virtual_object_controller_scale_factor *= 0.0 - self._last_decay_common_step_counter = _ccs - self._eligible_since_common_step = None - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - if self._metric_deque is not None: - self._metric_deque.clear() - if len(self._baseline_reward_indices) > 0: - self._reward_baselines = _erm[self._baseline_reward_indices].clone() - self._deque_reward_baselines[:] = self._reward_baselines - logger.info( - "[AdaptiveCurriculum] VOC scale decayed to %.3f at common_step=%d", - float(self._command.virtual_object_controller_scale_factor), - _ccs, - ) - - self._env.pre_decay_eval_pending = True - self._deferred_decay_fn = _apply_adaptive - self._decay_deferred = True - return self._command.virtual_object_controller_scale_factor - - env.video_trigger_pending = True - - # 10 Apply decay, set buffer, and clear the deque - if decay_mode == "exponential": - self._command.virtual_object_controller_scale_factor *= ( - exponential_decay_factor - ) - elif decay_mode == "linear": - self._command.virtual_object_controller_scale_factor -= linear_decay_step - elif decay_mode == "custom_schedule": - self._schedule_index += 1 - old_voc = float(self._command.virtual_object_controller_scale_factor) - new_voc = self._custom_voc_schedule[self._schedule_index] - self._command.virtual_object_controller_scale_factor = new_voc - for rname, weights in self._custom_reward_schedules.items(): - self._reward_manager.get_term_cfg(rname).weight = weights[ - self._schedule_index - ] - logger.info( - "[CustomSchedule] VOC %.3f → %.3f at common_step=%d (stage %d/%d)", - old_voc, - new_voc, - self._env.common_step_counter, - self._schedule_index + 1, - len(self._custom_voc_schedule), - ) - else: - raise ValueError(f"Invalid decay mode: {decay_mode}") - - if decay_mode != "custom_schedule" and ( - self._command.virtual_object_controller_scale_factor - <= zero_scale_factor_threshold - ): - self._command.virtual_object_controller_scale_factor *= 0.0 - - self._last_decay_common_step_counter = int(self._env.common_step_counter) - self._eligible_since_common_step = None - self._episode_reward_deque.clear() - self._episode_length_ratio_deque.clear() - if self._metric_deque is not None: - self._metric_deque.clear() - - # Store current deque means as the baseline for the next decay's retention check. - if len(self._baseline_reward_indices) > 0: - self._reward_baselines = self._episode_reward_prev_means[ - self._baseline_reward_indices - ].clone() - self._deque_reward_baselines[:] = self._reward_baselines - - logger.info( - "[AdaptiveCurriculum] VOC scale decayed to %.3f at common_step=%d", - float(self._command.virtual_object_controller_scale_factor), - self._env.common_step_counter, - ) - return self._command.virtual_object_controller_scale_factor - - -class FixedTimestepCurriculum(ManagerTermBase): - """Curriculum for virtual object control with a fixed timestep decay. - - Decay the virtual object control scale factor at a pre-defined timestep schedule - """ - - def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the term. - - Args: - cfg: The configuration of the curriculum. - env: The RL environment instance. - """ - super().__init__(cfg, env) - - self._step_dt = self._env.step_dt - - # Number of simulation steps per PPO update - self._num_steps_per_env = cfg.params["num_steps_per_env"] - self._last_schedule_index: int = -1 - - # Command term - self._command = env.command_manager.get_term(cfg.params["command_name"]) - - # Timestep schedule - self._timestep_schedule = [ - sim_step * self._num_steps_per_env - for sim_step in cfg.params["timestep_schedule"] - ] - len_decay_schedule = len(self._timestep_schedule) - - # VOC scale factor schedule - self._voc_scale_factor_schedule = cfg.params[ - "virtual_object_control_scale_factor" - ] - assert ( - len(self._voc_scale_factor_schedule) == len_decay_schedule - ), f"Length of VOC scale factor schedule must be equal to the length of timestep schedule, got {len(self._voc_scale_factor_schedule)} and {len_decay_schedule}" - - # Reward manager, reward names - self._reward_manager = env.reward_manager - self._schedule_reward_names = [ - key.replace("rewards_", "") - for key in cfg.params.keys() - if key.startswith("rewards_") - ] - _available_reward_names = self._env.reward_manager._term_names - self._schedule_reward_weights = {} - for reward_name in self._schedule_reward_names: - assert ( - reward_name in _available_reward_names - ), f"Reward name {reward_name} not found in available reward names {_available_reward_names}" - - if isinstance(cfg.params[f"rewards_{reward_name}"], float): - self._schedule_reward_weights[reward_name] = [ - cfg.params[f"rewards_{reward_name}"] - ] * len_decay_schedule - else: - assert ( - len(cfg.params[f"rewards_{reward_name}"]) == len_decay_schedule - ), f"Length of reward {reward_name} schedule must be equal to the length of timestep schedule" - self._schedule_reward_weights[reward_name] = cfg.params[ - f"rewards_{reward_name}" - ] - - # Termination manager — per-step threshold schedules. - # Keys prefixed with "termination__" define a list of - # values (one per schedule step) that override the named termination - # term's params at each curriculum transition. - # E.g. "termination_object_away_from_trajectory_position_threshold" - # maps to env.termination_manager.get_term_cfg( - # "object_away_from_trajectory").params["position_threshold"]. - self._termination_manager = env.termination_manager - self._schedule_termination_params: dict[tuple[str, str], list] = {} - for key in cfg.params.keys(): - if not key.startswith("termination_"): - continue - # Convention: termination__ - # Try to match known termination terms greedily. - suffix = key[len("termination_") :] - term_name = param_name = None - for tname in self._termination_manager._term_names: - if suffix.startswith(tname + "_"): - term_name = tname - param_name = suffix[len(tname) + 1 :] - break - if term_name is None or param_name is None: - continue - vals = cfg.params[key] - if vals is None: - continue # None = no override for this termination param - if isinstance(vals, (int, float)): - vals = [float(vals)] * len_decay_schedule - else: - assert len(vals) == len_decay_schedule, ( - f"Termination schedule '{key}' must have {len_decay_schedule} " - f"entries (one per timestep_schedule entry), got {len(vals)}" - ) - self._schedule_termination_params[(term_name, param_name)] = list(vals) - - def __call__( - self, - env: ManagerBasedRLEnv, - env_ids: Sequence[int], - command_name: str, - num_steps_per_env: int, - timestep_schedule: list[int], - virtual_object_control_scale_factor: list[float], - rewards_object_keypoints_tracking_exp: list[float], - rewards_hand_keypoints_tracking_exp: list[float], - rewards_hand_joint_pos_tracking_exp: list[float], - rewards_contact_wrench_support_reward: float | list[float], - rewards_unintended_contact_penalty: float | list[float], - rewards_missed_contact_penalty: float | list[float], - rewards_object_meshvert_tracking_fine: float | list[float] = 0.0, - rewards_dexmachina_contact_tracking_reward: float | list[float] = 0.0, - rewards_relative_object_pos_reward: float | list[float] = 0.0, - rewards_relative_object_rot_reward: float | list[float] = 0.0, - rewards_inter_object_proximity_reward: float | list[float] = 0.0, - termination_object_away_from_trajectory_position_threshold: ( - list[float] | None - ) = None, - termination_object_away_from_trajectory_orientation_threshold: ( - list[float] | None - ) = None, - termination_hand_wrist_away_from_trajectory_threshold: ( - list[float] | None - ) = None, - ) -> torch.Tensor: - """Apply the curriculum.""" - # 1 Check if the current timestep triggers the decay, if not, return the current VOC scale factor - sim_step_counter = self._env.common_step_counter - current_schedule_index = min( - bisect.bisect_right(self._timestep_schedule, sim_step_counter), - len(self._voc_scale_factor_schedule) - 1, # clamp to the last index - ) - # DIAG: print common_step + idx every 200 sim-steps so we can confirm - # whether common_step is growing across resume + curriculum stuck. - if ( - sim_step_counter % 200 - ) == 0 or current_schedule_index != self._last_schedule_index: - print( - f"[curr/voc] common_step={sim_step_counter} idx={current_schedule_index} " - f"last_idx={self._last_schedule_index} sched_len={len(self._timestep_schedule)} " - f"voc_target={self._voc_scale_factor_schedule[current_schedule_index]}", - flush=True, - ) - if current_schedule_index == self._last_schedule_index: - return self._command.virtual_object_controller_scale_factor - self._last_schedule_index = current_schedule_index - - # 2 Set the VOC scale factor for the current timestep - self._command.virtual_object_controller_scale_factor = ( - 0.0 * self._command.virtual_object_controller_scale_factor - + self._voc_scale_factor_schedule[current_schedule_index] - ) - - # 3 Set the reward weights for the current timestep - for reward_name in self._schedule_reward_names: - self._reward_manager.get_term_cfg(reward_name).weight = ( - self._schedule_reward_weights[reward_name][current_schedule_index] - ) - - # 4 Set per-step termination thresholds (if any scheduled) - for ( - term_name, - param_name, - ), schedule in self._schedule_termination_params.items(): - self._termination_manager.get_term_cfg(term_name).params[param_name] = ( - schedule[current_schedule_index] - ) - - return self._command.virtual_object_controller_scale_factor diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/events.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/events.py deleted file mode 100644 index 4ea35df5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/events.py +++ /dev/null @@ -1,104 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from isaaclab.envs import ManagerBasedRLEnv -from pxr import UsdGeom, UsdPhysics - - -def configure_collision_groups( - env: ManagerBasedRLEnv, - env_ids: list[int] | None, - robot_names: list[str], - object_names: list[str], - fixed_object_names: list[str], - disable_robot_to_object_collisions: bool = False, - disable_robot_to_fixed_object_collisions: bool = True, - disable_inter_object_collisions: bool = False, - disable_object_to_fixed_object_collisions: bool = False, -) -> None: - """Prestartup event to configure collision groups. - - Robot attributes are added to the RobotGroup. - Object attributes are added to the ObjectGroup. - Fixed object attributes are added to the FixedObjectGroup. - ObjectGroup and FixedObjectGroup are always collide to each other. - If disable_robot_to_object_collisions is True, the RobotGroup is filtered to not collide with the ObjectGroup. - If disable_robot_to_fixed_object_collisions is True, the RobotGroup is filtered to not collide with the FixedObjectGroup. - - Args: - env: The environment instance. - env_ids: Environment IDs. - robot_names: List of robot names to add to the RobotGroup. - object_names: List of object names to add to the ObjectGroup. - fixed_object_names: List of fixed object names to add to the FixedObjectGroup. - disable_robot_to_object_collisions: Whether to disable collisions between robots and objects. - disable_robot_to_fixed_object_collisions: Whether to disable collisions between robots and fixed objects. - disable_inter_object_collisions: Whether to disable collisions between objects. - disable_object_to_fixed_object_collisions: Whether to disable collisions between objects and fixed objects. - """ - del env_ids - - stage = env.sim.stage - num_envs = env.scene.cfg.num_envs - - # Create collision groups root - collision_groups_root = "/World/collisionGroups" - if not stage.GetPrimAtPath(collision_groups_root): - UsdGeom.Xform.Define(stage, collision_groups_root) - - # Create collision group path and group entities - robot_group_path = f"{collision_groups_root}/RobotGroup" - robot_group = UsdPhysics.CollisionGroup.Define(stage, robot_group_path) - - object_group_path = f"{collision_groups_root}/ObjectGroup" - object_group = UsdPhysics.CollisionGroup.Define(stage, object_group_path) - - fixed_object_group_path = f"{collision_groups_root}/FixedObjectGroup" - fixed_object_group = UsdPhysics.CollisionGroup.Define( - stage, fixed_object_group_path - ) - - # Populate RobotGroup — expandPrims so all descendant collision meshes are included - robot_api = robot_group.GetCollidersCollectionAPI() - robot_api.CreateExpansionRuleAttr().Set("expandPrims") - for idx in range(num_envs): - for robot_name in robot_names: - robot_api.GetIncludesRel().AddTarget(f"/World/envs/env_{idx}/{robot_name}") - - # Populate ObjectGroup — expandPrims for descendant colliders - object_api = object_group.GetCollidersCollectionAPI() - object_api.CreateExpansionRuleAttr().Set("expandPrims") - for idx in range(num_envs): - for object_name in object_names: - object_api.GetIncludesRel().AddTarget( - f"/World/envs/env_{idx}/{object_name}" - ) - - # Populate FixedObjectGroup — expandPrims for descendant colliders - fixed_object_api = fixed_object_group.GetCollidersCollectionAPI() - fixed_object_api.CreateExpansionRuleAttr().Set("expandPrims") - for idx in range(num_envs): - for fixed_object_name in fixed_object_names: - fixed_object_api.GetIncludesRel().AddTarget( - f"/World/envs/env_{idx}/{fixed_object_name}" - ) - - # Disable collisions between robot and object groups - if disable_robot_to_object_collisions: - robot_group.GetFilteredGroupsRel().AddTarget(object_group_path) - object_group.GetFilteredGroupsRel().AddTarget(robot_group_path) - - # Disable collisions between robot and fixed object groups - if disable_robot_to_fixed_object_collisions: - robot_group.GetFilteredGroupsRel().AddTarget(fixed_object_group_path) - fixed_object_group.GetFilteredGroupsRel().AddTarget(robot_group_path) - - # Disable collisions between objects (intra-ObjectGroup self-filter) - if disable_inter_object_collisions: - object_group.GetFilteredGroupsRel().AddTarget(object_group_path) - - # Disable collisions between objects and fixed objects (support surfaces, etc.) - if disable_object_to_fixed_object_collisions: - object_group.GetFilteredGroupsRel().AddTarget(fixed_object_group_path) - fixed_object_group.GetFilteredGroupsRel().AddTarget(object_group_path) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/observations.py deleted file mode 100644 index 8b7bc002..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/observations.py +++ /dev/null @@ -1,503 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import isaaclab.utils.math as math_utils -import torch -from isaaclab.envs import ManagerBasedEnv -from isaaclab.managers import SceneEntityCfg -from isaaclab.sensors import ContactSensor - - -def finger_contact_forces( - env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg | None = None -) -> torch.Tensor: - """Contact force magnitudes for finger sensors. - - Args: - env: The environment instance. - sensor_cfg: If provided, return force for this specific sensor only. - If None, return forces for all sensors in env.cfg.finger_sensor_names. - - Returns: - If sensor_cfg is None: Tensor of shape (num_envs, num_fingers) with force magnitudes. - If sensor_cfg is provided: Tensor of shape (num_envs, 1) with force magnitude. - """ - if sensor_cfg is not None: - sensor: ContactSensor = env.scene[sensor_cfg.name] - net_forces = sensor.data.net_forces_w # (num_envs, num_bodies, 3) - return torch.norm(net_forces, dim=-1) # (num_envs, num_bodies) - - # Return all finger sensors - sensor_names = env.cfg.finger_sensor_names - force_list: list[torch.Tensor] = [] - for sensor_name in sensor_names: - s: ContactSensor = env.scene[sensor_name] - net_forces = s.data.net_forces_w # (num_envs, 1, 3) - force_magnitude = torch.norm(net_forces, dim=-1) # (num_envs, 1) - force_list.append(force_magnitude) - - return torch.cat(force_list, dim=-1) # (num_envs, num_fingers) - - -def _get_contact_pos(env: ManagerBasedEnv, sensor: ContactSensor) -> torch.Tensor: - """Helper to extract contact position from a sensor. - - Returns: - Tensor of shape (num_envs, 1, 3) with contact position. - """ - contact_pos_w = sensor.data.contact_pos_w - # Return zeros if contact position tracking is not enabled - if contact_pos_w is None: - return torch.zeros(env.num_envs, 1, 3, device=env.device) - # contact_pos_w shape: (num_envs, num_bodies, num_filter_bodies, 3) - # Take first body and first filter body - pos = contact_pos_w[:, 0, 0, :].unsqueeze(1) # (num_envs, 1, 3) - return torch.nan_to_num(pos, nan=0.0) - - -def finger_contact_positions( - env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg | None = None -) -> torch.Tensor: - """Contact positions in world frame. - - Returns 0 for positions when there is no contact (NaN replaced with 0). - - Args: - env: The environment instance. - sensor_cfg: If provided, return position for this specific sensor only. - If None, return positions for all sensors in env.cfg.finger_sensor_names. - - Note: - Requires ContactSensorCfg.track_contact_points=True and - ContactSensorCfg.max_contact_data_per_prim >= 1. - - Returns: - If sensor_cfg is None: Tensor of shape (num_envs, num_fingers, 3) with contact positions. - If sensor_cfg is provided: Tensor of shape (num_envs, 1, 3) with contact position. - """ - if sensor_cfg is not None: - sensor: ContactSensor = env.scene[sensor_cfg.name] - return _get_contact_pos(env, sensor) - - # Return all finger sensors - sensor_names = env.cfg.finger_sensor_names - pos_list: list[torch.Tensor] = [] - for sensor_name in sensor_names: - s: ContactSensor = env.scene[sensor_name] - pos_list.append(_get_contact_pos(env, s)) - - return torch.cat(pos_list, dim=1) # (num_envs, num_fingers, 3) - - -def total_contact_force( - env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg | None = None -) -> torch.Tensor: - """Total contact force across all fingertips (num_envs, 1).""" - force_magnitudes = finger_contact_forces(env, sensor_cfg) - return force_magnitudes.sum(dim=-1, keepdim=True) - - -def finger_contact_force_vectors( - env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg | None = None -) -> torch.Tensor: - """Contact force vectors for all finger sensors (num_envs, num_fingers, 3). - - Collects 3D force vectors from contact sensors (one per finger). - Each sensor is filtered to report only contacts with the object. - """ - sensor_names = env.cfg.finger_sensor_names - force_list = [] - for sensor_name in sensor_names: - sensor: ContactSensor = env.scene[sensor_name] - force_list.append(sensor.data.net_forces_w) # (num_envs, 1, 3) - - # Concatenate to (num_envs, num_fingers, 3) - return torch.cat(force_list, dim=1) - - -def contact_link_pos_and_valid( - env: ManagerBasedEnv, - side: str, -) -> tuple[torch.Tensor, torch.Tensor]: - """Per-link contact position and validity grouped by object part for one hand. - - Args: - env: The environment instance. - side: 'left' or 'right'. - - Returns: - contact_pos: Tensor shaped ``(num_envs, 2, num_links, 3)`` with contact - points in world frame for each part/link slot. - contact_valid: Tensor shaped ``(num_envs, 2, num_links)`` where entries - are ``True`` when the sensor force norm exceeds its threshold. - """ - num_parts = 2 - attr = f"contact_link_sensor_names_{side}" - sensor_names = list(getattr(env.cfg, attr, [])) - - if len(sensor_names) == 0: - num_envs = env.num_envs - return ( - torch.zeros(num_envs, num_parts, 0, 3, device=env.device), - torch.zeros(num_envs, num_parts, 0, dtype=torch.bool, device=env.device), - ) - - if len(sensor_names) % num_parts != 0: - raise ValueError( - f"Expected an even number of {side} contact-link sensors, got {len(sensor_names)}." - ) - - pos_list: list[torch.Tensor] = [] - valid_list: list[torch.Tensor] = [] - for sensor_name in sensor_names: - sensor: ContactSensor = env.scene[sensor_name] - pos_list.append(_get_contact_pos(env, sensor)) # (num_envs, 1, 3) - force = torch.norm(sensor.data.net_forces_w, dim=-1) # (num_envs, 1) - threshold = getattr(getattr(sensor, "cfg", None), "force_threshold", 0.1) - valid_list.append(force > threshold) - pos_flat = torch.cat(pos_list, dim=1) # (num_envs, 2 * num_links, 3) - valid_flat = torch.cat(valid_list, dim=1) # (num_envs, 2 * num_links) - # Reorder to (part, link): part 0 uses indices [0, 2, ...], part 1 uses [1, 3, ...]. - part0_idx = torch.arange(0, len(sensor_names), num_parts, device=env.device) - part1_idx = torch.arange(1, len(sensor_names), num_parts, device=env.device) - contact_pos = torch.stack( - [ - pos_flat[:, part0_idx, :], - pos_flat[:, part1_idx, :], - ], - dim=1, - ) # (num_envs, 2, num_links, 3) - contact_valid = torch.stack( - [ - valid_flat[:, part0_idx], - valid_flat[:, part1_idx], - ], - dim=1, - ) # (num_envs, 2, num_links) - return contact_pos, contact_valid - - -def finger_joint_pos(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Finger joint positions. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, num_fingers) with finger joint positions. - """ - command = env.command_manager.get_term(command_name) - return torch.cat( - [ - math_utils.scale_transform( - command.right_hand_finger_joint_pos, - command.right_robot.data.joint_pos_limits[..., 0], - command.right_robot.data.joint_pos_limits[..., 1], - ), - math_utils.scale_transform( - command.left_hand_finger_joint_pos, - command.left_robot.data.joint_pos_limits[..., 0], - command.left_robot.data.joint_pos_limits[..., 1], - ), - ], - dim=-1, - ).float() - - -def finger_joint_vel(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Finger joint velocity. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, num_fingers) with finger joint velocities. - """ - command = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_finger_joint_vel, - command.left_hand_finger_joint_vel, - ], - dim=-1, - ).float() - - -def wrist_position_e(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Wrist position in the environment frame. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, 3 x NUM_HANDS) with wrist position in the environment frame. - """ - command = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_wrist_position_e, - command.left_hand_wrist_position_e, - ], - dim=-1, - ) - - -def wrist_orientation_e(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Wrist orientation in the environment frame. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, 4 x NUM_HANDS) with wrist orientation in the environment frame. - """ - command = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_wrist_wxyz_e, - command.left_hand_wrist_wxyz_e, - ], - dim=-1, - ) - - -def wrist_velocity_b(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Wrist velocity in the body frame. - - Args: - env: The environment instance. - command_name: The name of the command. - """ - command = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_wrist_velocity_b, - command.left_hand_wrist_velocity_b, - ], - dim=-1, - ).float() - - -def object_position_e(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Object position in the environment frame. - - Args: - env: The environment instance. - command_name: The name of the command. - """ - command = env.command_manager.get_term(command_name) - return command.object_position_e.reshape(command.num_envs, -1) - - -def object_orientation_e(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Object orientation in the environment frame. - - Args: - env: The environment instance. - command_name: The name of the command. - """ - command = env.command_manager.get_term(command_name) - return command.object_orientation_e.reshape(command.num_envs, -1) - - -def object_t_wrist(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Wrist in object transformation. FIXME: Adapt to multiple objects. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, 7 x NUM_HANDS) transformation of wrist in object frame. - """ - command = env.command_manager.get_term(command_name) - - object_pos_e = command.object_position_e.squeeze(1) - object_ori_e = command.object_orientation_e.squeeze(1) - - object_p_right_wrist, object_q_right_wrist = math_utils.subtract_frame_transforms( - object_pos_e, - object_ori_e, # world_t_object - command.right_hand_wrist_position_e, - command.right_hand_wrist_wxyz_e, # world_t_wrist - ) - object_p_left_wrist, object_q_left_wrist = math_utils.subtract_frame_transforms( - object_pos_e, - object_ori_e, # world_t_object - command.left_hand_wrist_position_e, - command.left_hand_wrist_wxyz_e, # world_t_wrist - ) - - return torch.cat( - [ - object_p_right_wrist, - object_q_right_wrist, - object_p_left_wrist, - object_q_left_wrist, - ], - dim=-1, - ).float() - - -def object_p_fingertip(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Fingertips to object transformation fingertip_t_object. - - Args: - env: The environment instance. - command_name: The name of the command. - - Returns: - Tensor of shape (num_envs, 7 x NUM_FINGERTIPS) with fingertips to object transformation. - """ - command = env.command_manager.get_term(command_name) - - num_fingertips = len(command.right_fingertip_body_ids) - - object_position_e = command.object_position_e.expand(-1, num_fingertips, -1) - object_orientation_e = command.object_orientation_e.expand(-1, num_fingertips, -1) - - object_p_right_fingertip, _ = math_utils.subtract_frame_transforms( - object_position_e.reshape(-1, 3), - object_orientation_e.reshape(-1, 4), # world_t_object - command.right_hand_fingertip_position_e.reshape(-1, 3), - command.right_hand_fingertip_orientation_e.reshape(-1, 4), # world_t_fingertip - ) - object_p_right_fingertip = object_p_right_fingertip.reshape(env.num_envs, -1, 3) - - object_p_left_fingertip, _ = math_utils.subtract_frame_transforms( - object_position_e.reshape(-1, 3), - object_orientation_e.reshape(-1, 4), # world_t_object - command.left_hand_fingertip_position_e.reshape(-1, 3), - command.left_hand_fingertip_orientation_e.reshape(-1, 4), # world_t_fingertip - ) - object_p_left_fingertip = object_p_left_fingertip.reshape(env.num_envs, -1, 3) - - return torch.cat( - [ - object_p_right_fingertip.reshape(env.num_envs, -1), - object_p_left_fingertip.reshape(env.num_envs, -1), - ], - dim=-1, - ).float() - - -def prev_action(env: ManagerBasedEnv, action_name: str) -> torch.Tensor: - """The previous actions to the environment. - - The name of the action term for which the action is required. - """ - return env.action_manager.get_term(action_name).prev_actions - - -def processed_action(env: ManagerBasedEnv, action_name: str) -> torch.Tensor: - """The processed actions to the environment. - - The name of the action term for which the action is required. - """ - return env.action_manager.get_term(action_name).processed_actions - - -def contact_position_direction_in_wrist( - env: ManagerBasedEnv, command_name: str -) -> torch.Tensor: - """Wrist position in contact position. - - Args: - env: The environment instance. - command_name: The name of the command. - """ - command = env.command_manager.get_term(command_name) - - # Extract and expand the wrist position and orientation - left_hand_wrist_position_w = command.left_hand_wrist_position_w.reshape( - env.num_envs, 1, 1, 3 - ).expand( - -1, command.num_bodies, command.num_robot_contacts_left, 3 - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - left_hand_wrist_wxyz_w = command.left_hand_wrist_wxyz_e.reshape( - env.num_envs, 1, 1, 4 - ).expand( - -1, command.num_bodies, command.num_robot_contacts_left, 4 - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 4) - - right_hand_wrist_position_w = command.right_hand_wrist_position_w.reshape( - env.num_envs, 1, 1, 3 - ).expand( - -1, command.num_bodies, command.num_robot_contacts_right, 3 - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - right_hand_wrist_wxyz_w = command.right_hand_wrist_wxyz_e.reshape( - env.num_envs, 1, 1, 4 - ).expand( - -1, command.num_bodies, command.num_robot_contacts_right, 4 - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 4) - - # Get valid contact mask - left_active_contact = ( - command.left_hand_object_contact_positions_w.amax(dim=-1) > 1e-3 - ) # (num_envs, num_bodies, num_hand_link_w_sensor) - right_active_contact = ( - command.right_hand_object_contact_positions_w.amax(dim=-1) > 1e-3 - ) # (num_envs, num_bodies, num_hand_link_w_sensor) - - # Compute contact position in wrist frame - wrist_p_left_contact_positions, _ = math_utils.subtract_frame_transforms( - left_hand_wrist_position_w, # world_t_wrist - left_hand_wrist_wxyz_w, - command.left_hand_object_contact_positions_w, # world_t_contact - q02=None, - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - wrist_p_left_contact_positions *= left_active_contact.unsqueeze(-1) - - wrist_p_right_contact_positions, _ = math_utils.subtract_frame_transforms( - right_hand_wrist_position_w, # world_t_wrist - right_hand_wrist_wxyz_w, - command.right_hand_object_contact_positions_w, # world_t_contact - q02=None, - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - wrist_p_right_contact_positions *= right_active_contact.unsqueeze(-1) - - # Compute contact force direction in wrist frame - left_contact_direction_w = command.left_hand_object_contact_forces_w[ - :, 0 - ] # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - left_contact_direction_w = left_contact_direction_w / left_contact_direction_w.norm( - dim=-1 - ).unsqueeze(-1).clamp(min=1e-5) - wrist_q_left_contact_direction, _ = math_utils.subtract_frame_transforms( - torch.zeros_like(left_hand_wrist_position_w), - left_hand_wrist_wxyz_w, - left_contact_direction_w, - q02=None, - ) - wrist_q_left_contact_direction *= left_active_contact.unsqueeze(-1) - - right_contact_direction_w = command.right_hand_object_contact_forces_w[ - :, 0 - ] # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - right_contact_direction_w = ( - right_contact_direction_w - / right_contact_direction_w.norm(dim=-1).unsqueeze(-1).clamp(min=1e-5) - ) - wrist_q_right_contact_direction, _ = math_utils.subtract_frame_transforms( - torch.zeros_like(right_hand_wrist_position_w), - right_hand_wrist_wxyz_w, - right_contact_direction_w, - q02=None, - ) - wrist_q_right_contact_direction *= right_active_contact.unsqueeze(-1) - - # Combine contact position and direction - return torch.cat( - [ - wrist_p_left_contact_positions.reshape(env.num_envs, -1), - wrist_q_left_contact_direction.reshape(env.num_envs, -1), - wrist_p_right_contact_positions.reshape(env.num_envs, -1), - wrist_q_right_contact_direction.reshape(env.num_envs, -1), - ], - dim=-1, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/rewards.py deleted file mode 100644 index 485104f6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/rewards.py +++ /dev/null @@ -1,1122 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import isaaclab.utils.math as math_utils -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import SceneEntityCfg - -from robotic_grounding.tasks.v2p.mdp.observations import ( - finger_contact_force_vectors, - finger_contact_forces, -) -from robotic_grounding.tasks.v2p.mdp.utils import chamfer_distance -from robotic_grounding.tasks.v2p.mdp.utils_jit import ( - contact_wrench_support_reward_jit, - hand_keypoints_tracking_jit, - missed_contact_penalty_jit, - unintended_contact_penalty_jit, -) - - -def contact_force_penalty( - env: ManagerBasedRLEnv, - sensor_cfg: SceneEntityCfg | None = None, - max_force: float = 50.0, -) -> torch.Tensor: - """Penalty for excessive contact forces (num_envs,).""" - force_magnitudes = finger_contact_forces(env, sensor_cfg) - excess_forces = torch.clamp(force_magnitudes - max_force, min=0.0) - return -excess_forces.sum(dim=-1) - - -def grasp_force_reward( - env: ManagerBasedRLEnv, - sensor_cfg: SceneEntityCfg | None = None, - target_force: float = 5.0, -) -> torch.Tensor: - """Reward for maintaining target grasp force (num_envs,).""" - force_magnitudes = finger_contact_forces(env, sensor_cfg) - total_force = force_magnitudes.sum(dim=-1) - return -torch.abs(total_force - target_force) - - -def maniptrans_contact_reward( - env: ManagerBasedRLEnv, - contact_range_min: float = 0.02, - contact_range_max: float = 0.03, - decay_constant: float = 1.0, - epsilon: float = 1e-5, -) -> torch.Tensor: - """ManipTrans-style contact reward with distance-weighted force masking (num_envs,). - - This reward encourages meaningful contact by weighting fingertip forces based on - distance to the object. Forces from fingertips close to the object contribute more - to the reward than forces from fingertips far away. - - Pre-computed reference distances are loaded from ``env.cfg.tips_distance_data`` - (set by the task-specific env config from parquet data). Episode step is used to - index into the reference trajectory with FPS ratio scaling. - - The soft distance weight is computed as: - weight = clamp((contact_range_max - distance) / (contact_range_max - contact_range_min), 0, 1) - - The reward is computed as: - reward = exp(-decay_constant / (total_masked_force + epsilon)) - - Args: - env: The environment instance. - contact_range_min: Distance below which weight is 1.0 (in contact). Default: 0.02m. - contact_range_max: Distance above which weight is 0.0 (too far). Default: 0.03m. - decay_constant: Controls reward sensitivity to force magnitude. Default: 1.0. - epsilon: Small constant to prevent division by zero. Default: 1e-5. - - Returns: - Reward tensor of shape (num_envs,) with values in [0, 1). - Higher total masked force leads to higher reward (approaches 1.0). - Returns zeros if tips_distance data is not available. - """ - # Lazy-cache the tips_distance tensor on GPU - if not hasattr(env, "_tips_distance_tensor"): - if ( - hasattr(env.cfg, "tips_distance_data") - and env.cfg.tips_distance_data is not None - ): - env._tips_distance_tensor = ( - torch.from_numpy(env.cfg.tips_distance_data).float().to(env.device) - ) - else: - env._tips_distance_tensor = None - - if env._tips_distance_tensor is None: - return torch.zeros(env.num_envs, device=env.device) - - # Index by episode step, accounting for FPS difference between source data and env - source_fps = getattr(env.cfg, "tips_distance_fps", 30.0) - env_fps = 1.0 / env.step_dt - fps_ratio = source_fps / env_fps - source_indices = (env.episode_length_buf.float() * fps_ratio).long() - source_indices = source_indices.clamp(0, env._tips_distance_tensor.shape[0] - 1) - distances = env._tips_distance_tensor[source_indices] # (num_envs, 10) - - # Soft distance weights: (num_envs, 10) - weights = torch.clamp( - (contact_range_max - distances) / (contact_range_max - contact_range_min), - min=0.0, - max=1.0, - ) - - # Get 3D force vectors from contact sensors and apply distance mask: (num_envs, num_fingers, 3) - force_vectors = finger_contact_force_vectors(env) - masked_forces = force_vectors * weights.unsqueeze(-1) - - # Sum of masked force magnitudes: (num_envs,) - total_force = torch.norm(masked_forces, dim=-1).sum(dim=-1) - - # Reward: exp(-decay_constant / (total_force + epsilon)) - # Higher force -> higher reward (approaches 1.0) - return torch.exp(-decay_constant / (total_force + epsilon)) - - -def object_position_tracking_exp( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.3, -) -> torch.Tensor: - """Compute task reward based on object tracking (num_envs,). - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - var: Variance for the exponential reward. - - Returns: - Task reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - # Get current object state and position error - object_position_e = command.object_position_e_sq - object_position_error_e = torch.sum( - torch.square(command.object_body_position_command_e - object_position_e), - dim=-1, - ) - - # Compute exponential rewards - object_position_tracking_rew = torch.exp(-object_position_error_e / var) - - return object_position_tracking_rew - - -def object_wxyz_tracking_exp( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.3, -) -> torch.Tensor: - """Compute task reward based on object tracking (num_envs,). - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - var: Variance for the exponential reward. - - Returns: - Task reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - # Get current object state and orientation error - object_wxyz = command.object_wxyz_e_sq - object_orientation_error_e = math_utils.quat_error_magnitude( - command.object_body_wxyz_command_e, object_wxyz - ) - - # Compute exponential rewards - object_orientation_tracking_rew = torch.exp(-object_orientation_error_e / var) - - return object_orientation_tracking_rew - - -def object_keypoints_tracking_exp( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.1, -) -> torch.Tensor: - """ - Compute the exponential reward for object keypoints tracking. - - This reward encourages the agent to align the object's pose with the demonstration target pose by matching key positions ("keypoints") in the object frame. - Keypoints are defined as the 6 principal unit vectors (+X, +Y, +Z, -X, -Y, -Z) transformed to world space using the current and command object poses. - The reward decays exponentially with the aggregate error between the tracked keypoints and their respective targets in the demonstration trajectory. - - Args: - env (ManagerBasedRLEnv): The RL environment instance. - command_name (str, optional): The name of the command term providing trajectory data. Defaults to "dual_hands_object_tracking_command". - var (float, optional): Variance (decay scale) for the exponential reward. Smaller values penalize deviations more sharply. - - Returns: - torch.Tensor: A tensor of shape (num_envs,) containing the keypoints tracking reward for each environment. - """ - command = env.command_manager.get_term(command_name) - - # Get current object state - object_position = command.object_position_e.unsqueeze(2).expand( - -1, -1, 6, -1 - ) # (num_envs, k, 6, 3) - object_wxyz = command.object_orientation_e.unsqueeze(2).expand( - -1, -1, 6, -1 - ) # (num_envs, k, 6, 4) - - # Compute keypoints - object_keypoints, _ = math_utils.combine_frame_transforms( - object_position, - object_wxyz, - command.KEYPOINT_VECS, - q12=None, - ) # (num_envs, k, 6, 3) - object_command_keypoints, _ = math_utils.combine_frame_transforms( - command.object_body_position_command_e.unsqueeze(2).expand(-1, -1, 6, -1), - command.object_body_wxyz_command_e.unsqueeze(2).expand(-1, -1, 6, -1), - command.KEYPOINT_VECS, - q12=None, - ) # (num_envs, k, 6, 3) - - # Compute keypoints error - object_keypoints_error = torch.sum( - torch.square(object_keypoints - object_command_keypoints), dim=-1 - ) # (num_envs, k, 6) - object_keypoints_tracking_rew = torch.exp(-object_keypoints_error / var).mean( - dim=(-2, -1) - ) - - return object_keypoints_tracking_rew - - -def hand_keypoints_tracking_exp( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.1, - threshold: float = 0.0, -) -> torch.Tensor: - """ - Compute the exponential imitation reward for hand keypoints tracking, including both wrist and fingertip positions. - - This reward encourages the agent's hand (both wrists and fingertips) to closely follow the demonstration trajectory. - Specifically, the wrist position and all fingertips are treated as "keypoints," and their current positions are compared - against the corresponding target positions from the demonstration. For each keypoint, the Euclidean distance to the - reference is computed, and an exponential penalty is applied based on the provided variance. - - The final reward is the sum of the exponentiated negative errors for: - - Right wrist position - - Left wrist position - - Right-hand fingertip positions - - Left-hand fingertip positions - - Args: - env (ManagerBasedRLEnv): The RL environment instance. - command_name (str, optional): Name of the command term providing demonstration data. Defaults to "dual_hands_object_tracking_command". - var (float, optional): Variance (decay scale) for the exponential reward. Smaller values penalize deviations more sharply. - threshold (float, optional): Threshold to saturate the reward. Errors below this threshold are set to 1.0. - - Returns: - torch.Tensor: A tensor of shape (num_envs,) containing the imitation reward for each environment. - """ - command = env.command_manager.get_term(command_name) - return hand_keypoints_tracking_jit( - left_wrist_cmd=command.left_hand_wrist_pose_command_e[:, :3], - right_wrist_cmd=command.right_hand_wrist_pose_command_e[:, :3], - left_fingertip_cmd=command.left_hand_fingertip_position_command_e[..., :3], - right_fingertip_cmd=command.right_hand_fingertip_position_command_e[..., :3], - left_wrist_cur=command.left_hand_wrist_position_e, - right_wrist_cur=command.right_hand_wrist_position_e, - left_fingertip_cur=command.left_hand_fingertip_position_e[..., :3], - right_fingertip_cur=command.right_hand_fingertip_position_e[..., :3], - var=float(var), - threshold=float(threshold), - ) - - -def hand_joint_pos_tracking_exp( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.1, - threshold: float = 0.0, -) -> torch.Tensor: - """Compute imitation reward based on hand joint positions (num_envs,).""" - command = env.command_manager.get_term(command_name) - - # Compute keypoints error for hand joint positions - left_hand_joint_pos_error = torch.sum( - torch.square( - command.left_hand_finger_joint_pos_command - - command.left_hand_finger_joint_pos - ), - dim=-1, - ) - right_hand_joint_pos_error = torch.sum( - torch.square( - command.right_hand_finger_joint_pos_command - - command.right_hand_finger_joint_pos - ), - dim=-1, - ) - - return ( - torch.exp(-(left_hand_joint_pos_error - threshold).clamp(min=0.0) / var) - + torch.exp(-(right_hand_joint_pos_error - threshold).clamp(min=0.0) / var) - ) / 2.0 - - -def dexmachina_contact_tracking_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.03, - mask_zero_contact: bool = True, -) -> torch.Tensor: - """Chamfer-style contact reward: match policy contact points to demo contact. - - Demo contact comes from command (contact_links_left/right_command_e); policy contact - from env contact sensors (per-link, per-object-part). - - Args: - env: The RL environment. - command_name: Command term that provides demo contact and object pose. - var: Scale for exp(-chamfer_dist / var). - mask_zero_contact: If True, reward is 0 when both demo and policy have no contact. - - Returns: - Reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - # Extract contact positions and validity - right_hand_object_contact_positions_e = ( - command.right_hand_object_contact_positions_e - ) # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - right_hand_object_contact_positions_is_valid = ( - command.right_hand_object_contact_positions_w.sum(dim=-1) > 1e-5 - ) # (num_envs, num_bodies, num_hand_link_w_sensor) - left_hand_object_contact_positions_e = command.left_hand_object_contact_positions_e - # (num_envs, num_bodies, num_hand_link_w_sensor, 3) - left_hand_object_contact_positions_is_valid = ( - command.left_hand_object_contact_positions_w.sum(dim=-1) > 1e-5 - ) # (num_envs, num_bodies, num_hand_link_w_sensor) - - # Extract desired contact part id for each hand and contact positions for each hand and desired part - right_hand_object_contact_command_part_ids_per_hand = ( - command.get_command_contact_part_id("right") - ) - right_hand_object_contact_positions_e = right_hand_object_contact_positions_e[ - command.all_env_ids, right_hand_object_contact_command_part_ids_per_hand - ] # (num_envs, num_hand_link_w_sensor, 3) - right_hand_object_contact_positions_is_valid = ( - right_hand_object_contact_positions_is_valid[ - command.all_env_ids, right_hand_object_contact_command_part_ids_per_hand - ] - ) # (num_envs, num_hand_link_w_sensor) - - left_hand_object_contact_command_part_ids_per_hand = ( - command.get_command_contact_part_id("left") - ) - left_hand_object_contact_positions_e = left_hand_object_contact_positions_e[ - command.all_env_ids, left_hand_object_contact_command_part_ids_per_hand - ] # (num_envs, num_hand_link_w_sensor, 3) - left_hand_object_contact_positions_is_valid = ( - left_hand_object_contact_positions_is_valid[ - command.all_env_ids, left_hand_object_contact_command_part_ids_per_hand - ] - ) # (num_envs, num_hand_link_w_sensor) - - # Extract desired contact positions and validity for each hand - right_hand_object_contact_command_positions_e = ( - command.right_hand_object_contact_command_positions_and_normals_e[..., :3] - ) - right_hand_object_contact_command_positions_is_valid = ( - command.retargeted_right_object_contact_is_valid[command.timestep_counter] - ) - left_hand_object_contact_command_positions_e = ( - command.left_hand_object_contact_command_positions_and_normals_e[..., :3] - ) - left_hand_object_contact_command_positions_is_valid = ( - command.retargeted_left_object_contact_is_valid[command.timestep_counter] - ) - - right_hand_object_contact_dist = chamfer_distance( - right_hand_object_contact_positions_e, - right_hand_object_contact_command_positions_e, - right_hand_object_contact_positions_is_valid, - right_hand_object_contact_command_positions_is_valid, - ) - left_hand_object_contact_dist = chamfer_distance( - left_hand_object_contact_positions_e, - left_hand_object_contact_command_positions_e, - left_hand_object_contact_positions_is_valid, - left_hand_object_contact_command_positions_is_valid, - ) - - right_hand_object_contact_rew = torch.exp(-right_hand_object_contact_dist / var) - left_hand_object_contact_rew = torch.exp(-left_hand_object_contact_dist / var) - - if mask_zero_contact: - right_hand_object_contact_rew[right_hand_object_contact_dist < 1e-5] = 0.0 - left_hand_object_contact_rew[left_hand_object_contact_dist < 1e-5] = 0.0 - - return right_hand_object_contact_rew + left_hand_object_contact_rew - - -def contact_force_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 1.0, - threshold: float = 0.0, -) -> torch.Tensor: - """Contact force reward. - - If no contact is present, reward is 0. - If contact is present, reward forces within the range of lower_force_squared and upper_force_squared. - - Args: - env: The RL environment. - command_name: Command term that provides contact forces. - var: Scale for exp(-contact_force / var). - threshold: Threshold for contact allowrance. - - Returns: - Reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - right_hand_object_contact_forces_norm = command.right_force_sq_per_link - right_hand_link_in_contact = command.right_link_in_contact - num_right_hand_links_in_contact = right_hand_link_in_contact.sum(dim=-1) - - left_hand_object_contact_forces_norm = command.left_force_sq_per_link - left_hand_link_in_contact = command.left_link_in_contact - num_left_hand_links_in_contact = left_hand_link_in_contact.sum(dim=-1) - - contact_force_reward = ( - right_hand_link_in_contact - * torch.exp( - -(right_hand_object_contact_forces_norm - threshold).clamp(min=0.0) / var - ) - + left_hand_link_in_contact - * torch.exp( - -(left_hand_object_contact_forces_norm - threshold).clamp(min=0.0) / var - ) - ).sum(dim=-1) / ( - num_right_hand_links_in_contact + num_left_hand_links_in_contact - ).clamp( - min=1e-5 - ) - - return contact_force_reward - - -def contact_force_range_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 1.0, - lower_force_squared: float = 4.0, - upper_force_squared: float = 16.0, -) -> torch.Tensor: - """Contact force reward. - - If no contact is present, reward is 0. - If contact is present, reward forces within the range of lower_force_squared and upper_force_squared. - - Args: - env: The RL environment. - command_name: Command term that provides contact forces. - var: Scale for exp(-contact_force / var). - lower_force_squared: Lower force squared to be rewarded. - upper_force_squared: Upper force squared to be rewarded. - - Returns: - Reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - right_hand_object_contact_forces_norm = command.right_force_sq_per_link - right_hand_link_in_contact = command.right_link_in_contact - num_right_hand_links_in_contact = right_hand_link_in_contact.sum(dim=-1) - - left_hand_object_contact_forces_norm = command.left_force_sq_per_link - left_hand_link_in_contact = command.left_link_in_contact - num_left_hand_links_in_contact = left_hand_link_in_contact.sum(dim=-1) - - contact_force_reward = ( - right_hand_link_in_contact - * torch.exp( - -(lower_force_squared - right_hand_object_contact_forces_norm).clamp( - min=0.0 - ) - / var - ) - * torch.exp( - -(right_hand_object_contact_forces_norm - upper_force_squared).clamp( - min=0.0 - ) - / var - ) - + left_hand_link_in_contact - * torch.exp( - -(lower_force_squared - left_hand_object_contact_forces_norm).clamp(min=0.0) - / var - ) - * torch.exp( - -(left_hand_object_contact_forces_norm - upper_force_squared).clamp(min=0.0) - / var - ) - ).sum(dim=-1) / ( - num_right_hand_links_in_contact + num_left_hand_links_in_contact - ).clamp( - min=1e-5 - ) - - return contact_force_reward - - -def contact_force_rate_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 1.0, -) -> torch.Tensor: - """Contact force rate reward. - - Args: - env: The RL environment. - command_name: Command term that provides contact forces. - var: Scale for exp(-contact_force_rate / var). - threshold: Threshold for contact force rate. - - Returns: - Reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - - right_hand_object_contact_forces_norm = ( - command.right_hand_object_contact_forces_w.norm(dim=-1) - ).sum( - dim=2 - ) # (num_envs, timesteps, num_hand_link_w_sensor) - left_hand_object_contact_forces_norm = ( - command.left_hand_object_contact_forces_w.norm(dim=-1) - ).sum( - dim=2 - ) # (num_envs, timesteps, num_hand_link_w_sensor) - - right_hand_link_in_contact = ( - right_hand_object_contact_forces_norm.sum(dim=1) > 1e-3 - ) # (num_envs, num_hand_link_w_sensor) - left_hand_link_in_contact = ( - left_hand_object_contact_forces_norm.sum(dim=1) > 1e-3 - ) # (num_envs, num_hand_link_w_sensor) - - num_right_hand_links_in_contact = right_hand_link_in_contact.sum(dim=-1) - num_left_hand_links_in_contact = left_hand_link_in_contact.sum(dim=-1) - - right_hand_object_contact_forces_norm_diff = torch.abs( - torch.diff(right_hand_object_contact_forces_norm, dim=1) - ).mean( - dim=1 - ) # (num_envs, num_hand_link_w_sensor) - left_hand_object_contact_forces_norm_diff = torch.abs( - torch.diff(left_hand_object_contact_forces_norm, dim=1) - ).mean( - dim=1 - ) # (num_envs, num_hand_link_w_sensor) - - contact_force_rate_reward = ( - right_hand_link_in_contact - * torch.exp(-right_hand_object_contact_forces_norm_diff / var) - + left_hand_link_in_contact - * torch.exp(-left_hand_object_contact_forces_norm_diff / var) - ).sum(dim=1) / ( - num_right_hand_links_in_contact + num_left_hand_links_in_contact - ).clamp( - min=1e-5 - ) - - return contact_force_rate_reward - - -def termination_penalty( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - """Penalty for termination (num_envs,).""" - return env.termination_manager.terminated - - -def action_norm( - env: ManagerBasedRLEnv, - action_names: list[str], -) -> torch.Tensor: - """Lp norm of the actions (num_envs,).""" - for i, action_name in enumerate(action_names): - if i == 0: - action_norm = torch.sum( - torch.square(env.action_manager.get_term(action_name).raw_actions), - dim=-1, - ) - else: - action_norm += torch.sum( - torch.square(env.action_manager.get_term(action_name).raw_actions), - dim=-1, - ) - return action_norm - - -def contact_wrench_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - in_contact_force_threshold: float = 1e-3, -) -> torch.Tensor: - r"""Contact wrench reward based on per-direction alignment with the reference. - - Reference and agent wrench supports are evaluated **per hand** (left / right) - against ``retargeted_{left,right}_contact_wrench_supports`` and - ``{left,right}_hand_contact_wrench_supports``; combined activity is derived - as the union of left and right reference supports. - - For each hand and sampled wrench-space basis direction ``b_i``: - - * If that hand's reference is zero (``ref_{h,i} \approx 0``) and its agent - support is zero: **no contribution** from that hand for this direction. - * If ``ref_{h,i} \approx 0`` but the agent support is non-zero: a per-cell - penalty of **-1** (spurious wrench support where the demo has none). - * If ``ref_{h,i} > 0`` and the agent support is non-zero: alignment - ``clamp(agent_{h,i} / ref_{h,i}, 0, 1)`` (partial credit, capped at 1 when - the agent meets or exceeds the reference). - * If ``ref_{h,i} > 0`` but the agent support is zero: per-cell penalty **-1** - (missing support where the demo requires it). - - Per basis direction, scores from hands with ``ref_{h,i} > 0`` are averaged. - If **both** hands have zero reference on that direction but either hand shows - non-zero agent support, the direction scores **-1**; if both agent supports - are zero, the score is **0**. - - Alignment contributions are averaged over directions where the **combined** - demo reference is non-zero; spurious-support penalties on directions where - the combined reference is zero are averaged separately over all basis - directions (so each such fault contributes ``-1 / B`` on average). - - Reward is only positive when the agent has at least one hand in contact with - the object (contact force exceeds ``in_contact_force_threshold``). When the - reference requires contact but the agent has none at all, a **negative** - penalty of ``-1`` is added, so the overall signal can span ``[-1, +1]``. - - Args: - env: The RL environment. - command_name: Command term that provides reference contact wrench repr. - in_contact_force_threshold: Minimum contact force magnitude (N) to count - a link as in contact. - - Returns: - Reward tensor of shape (num_envs,) in [-1, 1]. - """ - cmd = env.command_manager.get_term(command_name) - - # ── reference and current wrench supports ───────────────────────────────── - # Shapes: (N, num_bodies, B) — one support scalar per body per basis direction. - # All cached by refresh_tensors(); cmd.ref_left/right are the timestep-indexed - # references, cmd.mask_left/right are `ref > eps`, and cmd.ref_active_per_cell - # / per_body are their unions. cmd.in_contact / right_in_contact / - # left_in_contact use the 1e-3 threshold baked into refresh_tensors(). - ref_l = cmd.ref_left - ref_r = cmd.ref_right - curr_l = cmd.left_hand_contact_wrench_supports - curr_r = cmd.right_hand_contact_wrench_supports - - eps = 1e-6 - - def _hand_cell_score(ref_h: torch.Tensor, curr_h: torch.Tensor) -> torch.Tensor: - """Per-hand per-body per-direction score in {-1, 0} ∪ (0, 1].""" - pos_ref = ref_h > eps - pos_curr = curr_h > eps - align = (curr_h / ref_h.clamp(min=eps)).clamp(0.0, 1.0) - return torch.where( - pos_ref & pos_curr, - align, - torch.where( - pos_ref & ~pos_curr, - torch.full_like(ref_h, -1.0), - torch.where( - ~pos_ref & pos_curr, - torch.full_like(ref_h, -1.0), - torch.zeros_like(ref_h), - ), - ), - ) - - mask_l = cmd.mask_left # (N, num_bodies, B) - mask_r = cmd.mask_right # (N, num_bodies, B) - w = mask_l.float() + mask_r.float() # (N, num_bodies, B) - hs_l = _hand_cell_score(ref_l, curr_l) - hs_r = _hand_cell_score(ref_r, curr_r) - # Combined activity: either hand has non-zero reference for this body/direction - active_c = cmd.ref_active_per_cell # (N, num_bodies, B) - s_dir = torch.where( - w > 0, - (hs_l * mask_l.float() + hs_r * mask_r.float()) / w.clamp(min=1.0), - torch.zeros_like(ref_l), - ) # (N, num_bodies, B) - num_active = active_c.float().sum(dim=-1) # (N, num_bodies) - has_ref_dir = num_active > 0 # (N, num_bodies) - mean_from_ref = torch.where( - has_ref_dir, - (s_dir * active_c.float()).sum(dim=-1) / num_active.clamp(min=1.0), - torch.zeros_like(num_active), - ) # (N, num_bodies) - spurious_inactive = (~active_c) & ((curr_l > eps) | (curr_r > eps)) - mean_spurious = -spurious_inactive.float().mean(dim=-1) # (N, num_bodies) - mean_alignment = mean_from_ref + mean_spurious # (N, num_bodies) - - # ── contact gating ──────────────────────────────────────────────────────── - in_contact = cmd.in_contact # (N,) - - # Per-body: is the demo requesting contact on this body? - ref_active_per_body = cmd.ref_active_per_body # (N, num_bodies) - - # Average quality and penalty over bodies then return (N,) - quality = ( - mean_alignment * in_contact.unsqueeze(-1).float() * ref_active_per_body.float() - ).mean(dim=-1) - penalty = -(ref_active_per_body.float() * (~in_contact).unsqueeze(-1).float()).mean( - dim=-1 - ) - - return quality + penalty - - -def contact_wrench_cumulative_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - eps: float = 1e-6, - streak_scale: float = 20.0, -) -> torch.Tensor: - """Per-body streak reward encouraging sustained contact on the correct object bodies. - - For each object body, tracks how many consecutive steps the policy has maintained - wrench support where the reference requires it. Streaks are per-body, so touching - the wrong body does not inflate the reward for a body the demo actually needs. - - * **Reference active & policy active (per body):** positive streak increments; - reward is ``+tanh(streak / streak_scale)`` in ``(0, 1)``. - * **Reference inactive & policy active (per body):** spurious contact — negative - streak increments; reward is ``-tanh(streak / streak_scale)`` in ``(-1, 0)``. - * **Reference active & policy inactive:** streak resets; reward is **0**. - * **Both inactive:** streak resets; reward is **0**. - - Per-body rewards are averaged over bodies where the reference wants contact. - Streaks reset at episode boundaries. - - Args: - env: The RL environment. - command_name: Command term providing retargeted wrench repr and current - hand wrench repr. - eps: Threshold for treating a support scalar as non-zero. - streak_scale: Step count at which tanh reaches ~0.76. Larger values - make the reward grow more slowly with streak length. - - Returns: - Reward tensor of shape ``(num_envs,)`` in ``(-1, 1)``. - """ - cmd = env.command_manager.get_term(command_name) - - # Shapes: (N, num_bodies, B). ref_left/right and ref_active_per_body are - # cached by refresh_tensors(); curr_l/curr_r still come from the per-property - # path. - curr_l = cmd.left_hand_contact_wrench_supports - curr_r = cmd.right_hand_contact_wrench_supports - - # Per-body active masks (cached for reference; computed fresh for policy). - ref_active = cmd.ref_active_per_body # (N, num_bodies) - policy_active = (curr_l > eps).any(dim=-1) | (curr_r > eps).any( - dim=-1 - ) # (N, num_bodies) - - N, num_bodies = ref_active.shape - - # Lazy init: streaks are (N, num_bodies) so we can track persistence per body - if not hasattr(env, "_cwc_good_steps") or env._cwc_good_steps.shape != ( - N, - num_bodies, - ): - env._cwc_good_steps = torch.zeros( - N, num_bodies, dtype=torch.long, device=env.device - ) - env._cwc_bad_steps = torch.zeros( - N, num_bodies, dtype=torch.long, device=env.device - ) - env._cwc_prev_ep_len = torch.zeros(N, dtype=torch.long, device=env.device) - - g = env._cwc_good_steps - b = env._cwc_bad_steps - - # Reset streaks at episode boundaries; unsqueeze to broadcast over num_bodies - el = env.episode_length_buf - reset_episode = ((el == 0) | (el < env._cwc_prev_ep_len)).unsqueeze(-1) # (N, 1) - env._cwc_prev_ep_len = el.clone() - g = torch.where(reset_episode, torch.zeros_like(g), g) - b = torch.where(reset_episode, torch.zeros_like(b), b) - - good = ref_active & policy_active # (N, num_bodies) - bad_spurious = (~ref_active) & policy_active # (N, num_bodies) - - g_new = torch.where(good, g + 1, torch.zeros_like(g)) - b_new = torch.where(bad_spurious, b + 1, torch.zeros_like(b)) - - env._cwc_good_steps = g_new - env._cwc_bad_steps = b_new - - scale = max(float(streak_scale), 1e-6) - good_mag = torch.tanh(g_new.float() / scale) # (N, num_bodies) - bad_mag = torch.tanh(b_new.float() / scale) # (N, num_bodies) - per_body_reward = torch.where( - good, - good_mag, - torch.where(bad_spurious, -bad_mag, torch.zeros_like(good_mag)), - ) # (N, num_bodies) - - # Average over bodies where reference wants contact - num_ref_bodies = ref_active.float().sum(dim=-1).clamp(min=1.0) # (N,) - return (per_body_reward * ref_active.float()).sum(dim=-1) / num_ref_bodies - - -def contact_wrench_continuous_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - approach_var: float = 0.05, - in_contact_force_threshold: float = 1e-3, -) -> torch.Tensor: - """Continuous contact wrench reward combining wrench alignment (A) and approach distance (B). - - ``total = A + B`` where: - - * **A** = ``contact_wrench_reward``: quality in (0, 1] when in contact with the - object as the demo requires; ``-1`` flat penalty when the reference is active but - the agent has no contact at all. - * **B** = ``exp(-avg_min_dist / approach_var)``, gated to **zero** whenever the - agent is already in contact. ``avg_min_dist`` is the average over all valid - reference contact points of the distance to the nearest fingertip. - - Args: - env: The RL environment. - command_name: Command term that provides reference contact wrench repr and - contact positions. - approach_var: Distance scale for the exponential approach reward (metres). - in_contact_force_threshold: Minimum contact force (N) to count a link as - in contact. - - Returns: - Reward tensor of shape (num_envs,) in [-1, 1]. - """ - A = contact_wrench_reward(env, command_name, in_contact_force_threshold) - - cmd = env.command_manager.get_term(command_name) - - in_contact = cmd.in_contact # (N,) — threshold 1e-3, cached - ref_active = cmd.ref_active_global # (N,) — cached - - right_ref_pts = cmd.right_hand_object_contact_command_positions_e # (N, P_r, 3) - right_ref_valid = cmd.retargeted_right_object_contact_is_valid[ - cmd.timestep_counter - ] # (N, P_r) - left_ref_pts = cmd.left_hand_object_contact_command_positions_e # (N, P_l, 3) - left_ref_valid = cmd.retargeted_left_object_contact_is_valid[ - cmd.timestep_counter - ] # (N, P_l) - - right_tips = cmd.right_hand_fingertip_position_e[..., :3] # (N, F_r, 3) - left_tips = cmd.left_hand_fingertip_position_e[..., :3] # (N, F_l, 3) - - right_pair_dist = torch.norm( - right_ref_pts.unsqueeze(2) - right_tips.unsqueeze(1), dim=-1 - ) # (N, P_r, F_r) - right_min_dist = right_pair_dist.min(dim=-1).values # (N, P_r) - right_num_valid = right_ref_valid.float().sum(dim=-1).clamp(min=1.0) # (N,) - right_avg_dist = (right_min_dist * right_ref_valid.float()).sum( - dim=-1 - ) / right_num_valid # (N,) - - left_pair_dist = torch.norm( - left_ref_pts.unsqueeze(2) - left_tips.unsqueeze(1), dim=-1 - ) # (N, P_l, F_l) - left_min_dist = left_pair_dist.min(dim=-1).values # (N, P_l) - left_num_valid = left_ref_valid.float().sum(dim=-1).clamp(min=1.0) # (N,) - left_avg_dist = (left_min_dist * left_ref_valid.float()).sum( - dim=-1 - ) / left_num_valid # (N,) - - has_right_valid = right_ref_valid.any(dim=-1) # (N,) - has_left_valid = left_ref_valid.any(dim=-1) # (N,) - num_valid_hands = (has_right_valid.float() + has_left_valid.float()).clamp(min=1.0) - avg_dist = ( - right_avg_dist * has_right_valid.float() - + left_avg_dist * has_left_valid.float() - ) / num_valid_hands # (N,) - - needs_approach = ref_active & ~in_contact & (has_right_valid | has_left_valid) - B = torch.exp(-avg_dist / approach_var) * needs_approach.float() # (N,) in (0, 1] - - return A + B - - -def contact_wrench_support_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - tolerance: float = 0.1, - var: float = 0.1, -) -> torch.Tensor: - """Contact wrench support reward. - - Args: - env: The RL environment. - command_name: Command term that provides contact wrench supports. - tolerance: Tolerance for current wrench support compared to command wrench support. - var: Scale for exp(-contact_loss / var). - - Returns: - Reward tensor (num_envs,). - """ - command = env.command_manager.get_term(command_name) - return contact_wrench_support_reward_jit( - right_cmd_active=command.right_wrench_cmd_active, - right_cur_active=command.right_wrench_cur_active, - left_cmd_active=command.left_wrench_cmd_active, - left_cur_active=command.left_wrench_cur_active, - right_cmd_active_per_body=command.right_wrench_cmd_active_per_body, - left_cmd_active_per_body=command.left_wrench_cmd_active_per_body, - right_cmd_supports=command.right_hand_contact_wrench_supports_command, - right_cur_supports=command.right_hand_contact_wrench_supports, - left_cmd_supports=command.left_hand_contact_wrench_supports_command, - left_cur_supports=command.left_hand_contact_wrench_supports, - tolerance=float(tolerance), - var=float(var), - ) - - -def unintended_contact_penalty( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", -) -> torch.Tensor: - """Unintended contact penalty where the command has no contact but current has contact. - - Args: - env: The RL environment. - command_name: Command term that provides contact wrench supports. - penalty: Penalty for unintended contact. - """ - command = env.command_manager.get_term(command_name) - return unintended_contact_penalty_jit( - right_cmd_active_per_body=command.right_wrench_cmd_active_per_body, - right_cur_active_per_body=command.right_wrench_cur_active_per_body, - left_cmd_active_per_body=command.left_wrench_cmd_active_per_body, - left_cur_active_per_body=command.left_wrench_cur_active_per_body, - right_cur_supports=command.right_hand_contact_wrench_supports, - left_cur_supports=command.left_hand_contact_wrench_supports, - num_bodies=int(command.num_bodies), - ) - - -def missed_contact_penalty( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", -) -> torch.Tensor: - """Missed contact penalty. - - Args: - env: The RL environment. - command_name: Command term that provides contact wrench supports. - penalty: Penalty for missed contact. - """ - command = env.command_manager.get_term(command_name) - return missed_contact_penalty_jit( - right_cmd_active=command.right_wrench_cmd_active, - right_cur_active=command.right_wrench_cur_active, - left_cmd_active=command.left_wrench_cmd_active, - left_cur_active=command.left_wrench_cur_active, - right_cmd_active_per_body=command.right_wrench_cmd_active_per_body, - left_cmd_active_per_body=command.left_wrench_cmd_active_per_body, - ) - - -def relative_object_pose_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - pos_sigma: float = 0.05, - rot_sigma: float = 0.5, -) -> torch.Tensor: - """Proximity-gated reward for inter-object relative pose tracking (num_envs,). - - Measures how well the policy maintains the correct spatial relationship between - object0 and object1 as demonstrated in the reference motion. Only active when - the demo shows the objects within ``relative_object_proximity_threshold`` of each - other. Returns zeros for single-object tasks. - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - pos_sigma: Position tolerance in metres; reward = 1/e at this error. Default 5 cm. - rot_sigma: Rotation tolerance in radians; reward = 1/e at this error. Default ~29 deg. - """ - command = env.command_manager.get_term(command_name) - if not getattr(command, "_has_multi_object", False): - return torch.zeros(env.num_envs, device=env.device) - pos_err, rot_err = command.relative_object_pose_error - proximity_mask = command.relative_object_proximity_mask - reward = torch.exp(-pos_err / pos_sigma) * torch.exp(-rot_err / rot_sigma) - return reward * proximity_mask.float() - - -def relative_object_pos_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - pos_sigma: float = 0.02, -) -> torch.Tensor: - """Proximity-gated position component of inter-object relative pose reward (num_envs,). - - Decoupled from rotation so pos and rot weights can be tuned independently. - Returns zeros for single-object tasks. - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - pos_sigma: Position tolerance in metres; reward = 1/e at this error. Default 2 cm. - """ - command = env.command_manager.get_term(command_name) - if not getattr(command, "_has_multi_object", False): - return torch.zeros(env.num_envs, device=env.device) - pos_err, _ = command.relative_object_pose_error - proximity_mask = command.relative_object_proximity_mask - return torch.exp(-pos_err / pos_sigma) * proximity_mask.float() - - -def relative_object_rot_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - rot_sigma: float = 0.3, -) -> torch.Tensor: - """Proximity-gated rotation component of inter-object relative pose reward (num_envs,). - - Decoupled from position so rot weight can be tuned independently. Default rot_sigma - is tighter (0.3 rad ≈ 17°) than the combined form to give stronger gradient signal. - Returns zeros for single-object tasks. - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - rot_sigma: Rotation tolerance in radians; reward = 1/e at this error. Default ~17 deg. - """ - command = env.command_manager.get_term(command_name) - if not getattr(command, "_has_multi_object", False): - return torch.zeros(env.num_envs, device=env.device) - _, rot_err = command.relative_object_pose_error - proximity_mask = command.relative_object_proximity_mask - return torch.exp(-rot_err / rot_sigma) * proximity_mask.float() - - -def inter_object_proximity_reward( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - dist_sigma: float = 0.05, -) -> torch.Tensor: - """Scalar inter-object distance tracking reward (num_envs,). - - Always-on companion to the proximity-gated relative pose rewards: encourages - |‖obj1 − obj0‖ − demo_distance| → 0 even when the two objects are far apart - in the demo. Provides a long-range signal so the policy is incentivised to - close the gap (e.g. spoon moving toward pan) before the proximity-gated - pose rewards activate. Returns zeros for single-object tasks. - - Args: - env: The RL environment. - command_name: Name of the command term to get demo data from. - dist_sigma: Distance tolerance in metres; reward = 1/e at this error. Default 5 cm. - """ - command = env.command_manager.get_term(command_name) - if not getattr(command, "_has_multi_object", False): - return torch.zeros(env.num_envs, device=env.device) - obj0_pos = command.object_position_e[:, 0, :] - obj1_pos = command.object_position_e[:, command._obj1_root_body_idx, :] - cur_dist = torch.norm(obj1_pos - obj0_pos, dim=-1) - demo_dist = command._demo_inter_object_dist[command.timestep_counter] - return torch.exp(-(cur_dist - demo_dist).abs() / dist_sigma) - - -def object_meshvert_tracking_fine( - env: ManagerBasedRLEnv, - command_name: str = "dual_hands_object_tracking_command", - var: float = 0.001, -) -> torch.Tensor: - """Fine object-pose reward from sampled mesh vertices (num_envs,).""" - command = env.command_manager.get_term(command_name) - if not getattr(command, "_meshvert_reward_enabled", False): - return torch.zeros(env.num_envs, device=env.device) - - num_verts = command._meshvert_num_verts - verts = command.OBJECT_MESHVERT_SAMPLED_VERTS - pos_current = command.object_position_e.unsqueeze(2).expand(-1, -1, num_verts, -1) - quat_current = command.object_orientation_e.unsqueeze(2).expand( - -1, -1, num_verts, -1 - ) - pos_target = command.object_body_position_command_e.unsqueeze(2).expand( - -1, -1, num_verts, -1 - ) - quat_target = command.object_body_wxyz_command_e.unsqueeze(2).expand( - -1, -1, num_verts, -1 - ) - - verts_current, _ = math_utils.combine_frame_transforms( - pos_current, - quat_current, - verts, - ) - verts_target, _ = math_utils.combine_frame_transforms( - pos_target, - quat_target, - verts, - ) - add_per_env = torch.norm(verts_current - verts_target, dim=-1).mean(dim=(-2, -1)) - return torch.exp(-(add_per_env**2) / var) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/terminations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/terminations.py deleted file mode 100644 index 6f8ff92d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/terminations.py +++ /dev/null @@ -1,146 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from typing import TYPE_CHECKING - -import isaaclab.utils.math as math_utils -import torch - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - -def hand_to_object_away_from_trajectory( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float, -) -> torch.Tensor: - """Terminate when hands deviate too far from the commanded trajectory. - - Compares per-hand wrist-to-object distances against the commanded - wrist-to-object distances and terminates if any hand exceeds - `threshold` times its commanded distance. - - Args: - env: The environment instance. - command_name: The name of the command term. - threshold: Ratio threshold for termination. - - Returns: - Tensor of shape (num_envs,) indicating whether to terminate. - """ - command = env.command_manager.get_term(command_name) - - right_hand_wrist_object_position_difference_command = torch.norm( - command.right_hand_wrist_pose_command_e[:, :3] - - command.object_body_position_command_e, - dim=-1, - ) - left_hand_wrist_object_position_difference_command = torch.norm( - command.left_hand_wrist_pose_command_e[:, :3] - - command.object_body_position_command_e, - dim=-1, - ) - - right_hand_wrist_object_position_difference = torch.norm( - command.right_robot.data.body_link_pos_w[:, command.right_wrist_body_id] - - command.object_position_w, - dim=-1, - ).squeeze() - left_hand_wrist_object_position_difference = torch.norm( - command.left_robot.data.body_link_pos_w[:, command.left_wrist_body_id] - - command.object_position_w, - dim=-1, - ).squeeze() - - right_hand_difference_ratio = ( - right_hand_wrist_object_position_difference - / right_hand_wrist_object_position_difference_command - ) - left_hand_difference_ratio = ( - left_hand_wrist_object_position_difference - / left_hand_wrist_object_position_difference_command - ) - - return torch.logical_and( - right_hand_difference_ratio > threshold, - left_hand_difference_ratio > threshold, - ) - - -def hand_wrist_away_from_trajectory( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float, -) -> torch.Tensor: - """Terminate when the hands are away from the trajectory.""" - command = env.command_manager.get_term(command_name) - right_hand_position_difference = torch.norm( - command.right_hand_wrist_pose_command_e[:, :3] - - command.right_hand_wrist_position_e, - dim=-1, - ) - left_hand_position_difference = torch.norm( - command.left_hand_wrist_pose_command_e[:, :3] - - command.left_hand_wrist_position_e, - dim=-1, - ) - return torch.logical_or( - right_hand_position_difference > threshold, - left_hand_position_difference > threshold, - ) - - -def object_away_from_trajectory_z( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float, -) -> torch.Tensor: - """Terminate when the object is away from the trajectory. - - Args: - env: The environment instance. - command_name: The name of the command. - threshold: The threshold for the termination. - - Returns: - Tensor of shape (num_envs,) indicating whether to terminate. - """ - command = env.command_manager.get_term(command_name) - object_position_z_difference = torch.abs( - command.object_body_position_command_e[..., 2] - - command.object_position_e[..., 2].squeeze() - ) - return object_position_z_difference > threshold - - -def object_away_from_trajectory( - env: ManagerBasedRLEnv, - command_name: str, - position_threshold: float, - orientation_threshold: float, -) -> torch.Tensor: - """Terminate when the object is away from the trajectory.""" - command = env.command_manager.get_term(command_name) - object_position_difference = torch.norm( - command.object_body_position_command_e - command.object_position_e, - dim=-1, - ) - object_orientation_difference = math_utils.quat_error_magnitude( - command.object_orientation_e, - command.object_body_wxyz_command_e, - ) - return torch.logical_or( - (object_position_difference > position_threshold).any(dim=-1), - (object_orientation_difference > orientation_threshold).any(dim=-1), - ) - - -def timestep_timeout( - env: ManagerBasedRLEnv, - command_name: str, -) -> torch.Tensor: - """Terminate when the command is completed.""" - command = env.command_manager.get_term(command_name) - return command.timestep_counter >= command.retargeted_horizon - 1 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils.py deleted file mode 100644 index d06407bc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils.py +++ /dev/null @@ -1,537 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Utility functions for the V2P environment.""" - -import numpy as np -import torch -from scipy.interpolate import interp1d -from scipy.spatial.transform import Rotation, Slerp - -from robotic_grounding.retarget.data_logger import ManoSharpaData - -########################################################## -# Interpolation -########################################################## - - -def interpolate_robot_motion_data( - motion_data: ManoSharpaData, - target_num_frames: float, -) -> ManoSharpaData: - """Interpolate the robot motion data to the target FPS. - - Now only supports interpolation of the following fields: - - object_body_position -> linear interpolation - - object_body_wxyz -> Slerp - - object_articulation -> linear interpolation - - robot_right_wrist_position -> linear interpolation - - robot_right_wrist_wxyz -> Slerp interpolation - - robot_right_finger_joints -> linear interpolation - - robot_left_wrist_position -> linear interpolation - - robot_left_wrist_wxyz -> Slerp interpolation - - robot_left_finger_joints -> linear interpolation - - robot_right_frames -> linear interpolation for position, Slerp for orientation - - robot_left_frames -> linear interpolation for position, Slerp for orientation - - mano_*_link_contact_positions / mano_*_object_contact_positions -> linear - between non-zero values only; frames between 0 and non-zero are set to 0 - TODO: we may need to improve this function for more general cases. - - Args: - motion_data: The motion data to interpolate. - target_num_frames: The target number of frames to interpolate to. - - Returns: - The interpolated motion data. - """ - - # --- Helper Functions --- - def get_timestamps() -> tuple[np.ndarray, np.ndarray]: - """Get the timestamps for the motion data.""" - n_frames = len(motion_data.robot_right_wrist_position) - src = np.arange(n_frames) / motion_data.fps - tgt = np.linspace(0, src[-1], int(src[-1] * target_num_frames)) - return src, tgt - - def interp_linear(data: list[float]) -> list[float]: - """Vectorized linear interpolation for any shape (T, ...).""" - data_arr = np.asarray(data) - return interp1d(src_times, data_arr, kind="linear", axis=0)(tgt_times).tolist() - - def interp_contact_linear( - data: list[list[list[float]]] | None, - ) -> list[list[list[float]]] | None: - """Linear interpolation for contact xyz tensors (T, n_links, 3). - - For each link, each of x/y/z is only interpolated when both bracketing - source values are non-zero; if either is zero, the result is zero - (contacts come and go discontinuously, so interpolating through a - zero sentinel would produce spurious in-between positions). - - Returns None if data is None or empty. - """ - if not data: - return None - data_arr = np.asarray(data) # (T, n_links, 3) - interp_result = interp1d(src_times, data_arr, kind="linear", axis=0)(tgt_times) - tol = 1e-8 - nonzero_src = np.abs(data_arr) > tol - idx_lo = np.searchsorted(src_times, tgt_times, side="right") - 1 - idx_lo = np.clip(idx_lo, 0, len(src_times) - 1) - idx_hi = np.minimum(idx_lo + 1, len(src_times) - 1) - mask_both = nonzero_src[idx_lo] & nonzero_src[idx_hi] - interp_result = np.where(mask_both, interp_result, 0.0) - return interp_result.tolist() - - def interp_part_ids( - part_ids: list[list[int]] | None, - ) -> list[list[int]] | None: - """Nearest-neighbor interpolation for integer contact part IDs (T, n_links). - - Integer semantics — must not blend 0/1 into 0.5. - """ - if not part_ids: - return None - arr = np.asarray(part_ids) # (T, n_links) - interp_result = interp1d(src_times, arr, kind="nearest", axis=0)(tgt_times) - return interp_result.astype(np.int64).tolist() - - def interp_slerp(quat_data: list[list[float]]) -> list[list[float]]: - """Slerp for a single time sequence of quaternions (T, 4).""" - quats = np.asarray(quat_data) - rotations = Rotation.from_quat(quats, scalar_first=True) - slerp = Slerp(src_times, rotations) - return slerp(tgt_times).as_quat(scalar_first=True).tolist() - - def interp_slerp_batch( - quat_data: list[list[list[float]]], - ) -> list[list[list[float]]]: - """Slerp for batched quaternions (T, N, 4). Loops over N.""" - data_arr = np.asarray(quat_data) - # Shape is (Time, N, 4) -> Iterate over N - n_items = data_arr.shape[1] - results = [] - for i in range(n_items): - # Extract (Time, 4) for the i-th item - results.append(interp_slerp(data_arr[:, i, :])) - # Stack back to (Time, N, 4) and convert to list - return np.array(results).transpose(1, 0, 2).tolist() - - def interp_frames(frame_data: list[list[list[float]]]) -> list[list[list[float]]]: - """Handles (T, N, 7) arrays: Split Pos (linear) and Rot (Slerp).""" - arr = np.asarray(frame_data) - # 1. Linear interp on position (first 3 cols) - Vectorized - pos_interp = np.array(interp_linear(arr[:, :, :3])) - # 2. Slerp on rotation (last 4 cols) - Batched - rot_interp = np.array(interp_slerp_batch(arr[:, :, 3:])) - # 3. Combine - return np.concatenate([pos_interp, rot_interp], axis=2).tolist() - - # --- Execution --- - - # 0. Setup Times - src_times, tgt_times = get_timestamps() - - # 1. Simple Linear Fields - motion_data.object_articulation = interp_linear(motion_data.object_articulation) - motion_data.robot_right_finger_joints = interp_linear( - motion_data.robot_right_finger_joints - ) - motion_data.robot_left_finger_joints = interp_linear( - motion_data.robot_left_finger_joints - ) - motion_data.robot_right_wrist_position = interp_linear( - motion_data.robot_right_wrist_position - ) - motion_data.robot_left_wrist_position = interp_linear( - motion_data.robot_left_wrist_position - ) - - # 2. Simple Rotation Fields (Slerp) - motion_data.robot_right_wrist_wxyz = interp_slerp( - motion_data.robot_right_wrist_wxyz - ) - motion_data.robot_left_wrist_wxyz = interp_slerp(motion_data.robot_left_wrist_wxyz) - - # 3. Object Bodies (Batched Position & Rotation) - # Note: interp_linear handles (Time, N, 3) automatically without loops - motion_data.object_body_position = interp_linear(motion_data.object_body_position) - motion_data.object_body_wxyz = interp_slerp_batch(motion_data.object_body_wxyz) - - # 4. Frames (Batched Mixed Data) - motion_data.robot_right_frames = interp_frames(motion_data.robot_right_frames) - motion_data.robot_left_frames = interp_frames(motion_data.robot_left_frames) - - # 5. Contact xyz tensors (T, num_links, 3) — interpolate only between non-zero - # values; samples bracketed by a zero are set to zero - motion_data.mano_right_link_contact_positions = interp_contact_linear( - getattr(motion_data, "mano_right_link_contact_positions", None), - ) - motion_data.mano_right_link_contact_normals = interp_contact_linear( - getattr(motion_data, "mano_right_link_contact_normals", None), - ) - motion_data.mano_right_object_contact_positions = interp_contact_linear( - getattr(motion_data, "mano_right_object_contact_positions", None), - ) - motion_data.mano_right_object_contact_normals = interp_contact_linear( - getattr(motion_data, "mano_right_object_contact_normals", None), - ) - motion_data.mano_left_link_contact_positions = interp_contact_linear( - getattr(motion_data, "mano_left_link_contact_positions", None), - ) - motion_data.mano_left_link_contact_normals = interp_contact_linear( - getattr(motion_data, "mano_left_link_contact_normals", None), - ) - motion_data.mano_left_object_contact_positions = interp_contact_linear( - getattr(motion_data, "mano_left_object_contact_positions", None), - ) - motion_data.mano_left_object_contact_normals = interp_contact_linear( - getattr(motion_data, "mano_left_object_contact_normals", None), - ) - - # 6. Contact part IDs (T, num_links) — nearest-neighbor so integers stay sane - motion_data.mano_right_object_contact_part_ids = interp_part_ids( - getattr(motion_data, "mano_right_object_contact_part_ids", None), - ) - motion_data.mano_left_object_contact_part_ids = interp_part_ids( - getattr(motion_data, "mano_left_object_contact_part_ids", None), - ) - - return motion_data - - -########################################################## -# Tensor Deque -########################################################## - - -class TensorDeque: - """A deque for storing tensors.""" - - def __init__(self, capacity: int, feature_shape: int, device: str) -> None: - """Initialize the deque.""" - self.capacity = capacity - self.buffer: torch.Tensor = torch.zeros( - (capacity, feature_shape), device=device - ) # (capacity, feature_shape) - self.head: int = 0 # index of the first element - self.tail: int = 0 # index of the last element - self.size: int = 0 # number of elements in the deque - - def append(self, x: torch.Tensor) -> None: - """Append a tensor to the deque.""" - self.buffer[self.tail] = x # (feature_shape,) - self.tail = (self.tail + 1) % self.capacity # (1,) - if self.size < self.capacity: - self.size += 1 # (1,) - else: - # overwrite oldest - self.head = (self.head + 1) % self.capacity # (1,) - - def append_batch(self, x: torch.Tensor) -> None: - """Append batch with shape = (K, feature_shape).""" - K = len(x) - - # If batch larger than capacity → keep only last capacity elements - if K >= self.capacity: - x = x[-self.capacity :] - K = self.capacity - self.buffer[:] = x - self.head = 0 - self.tail = 0 - self.size = self.capacity - return - - space_left = self.capacity - self.tail - - if K <= space_left: - # No wrap - self.buffer[self.tail : self.tail + K] = x - else: - # Wrap around - self.buffer[self.tail :] = x[:space_left] - self.buffer[: K - space_left] = x[space_left:] - - # Update tail - self.tail = (self.tail + K) % self.capacity - - # Update size/head - if self.size + K <= self.capacity: - self.size += K - else: - overflow = self.size + K - self.capacity - self.size = self.capacity - self.head = (self.head + overflow) % self.capacity - - def popleft(self) -> torch.Tensor: - """Pop the oldest tensor from the deque.""" - if self.size == 0: - raise IndexError("pop from empty deque") - x = self.buffer[self.head] - self.head = (self.head + 1) % self.capacity - self.size -= 1 - return x - - def is_full(self) -> bool: - """Check if the deque is full.""" - return self.size == self.capacity - - def __len__(self) -> int: - """Get the number of elements in the deque.""" - return self.size # (1,) - - def get_all(self) -> torch.Tensor: - """Get all the tensors in the deque.""" - if self.size == 0: - return torch.empty(0, *self.buffer.shape[1:], device=self.buffer.device) - if self.head < self.tail: - return self.buffer[self.head : self.tail] - else: - return torch.cat( - [self.buffer[self.head :], self.buffer[: self.tail]], dim=0 - ) - - def clear(self) -> None: - """Reset deque without reallocating memory. Does NOT zero memory for performance reasons.""" - self.head = 0 - self.tail = 0 - self.size = 0 - - -########################################################## -# Contact Reward -########################################################## - - -def chamfer_distance( - pts1: torch.Tensor, - pts2: torch.Tensor, - pts1_valid: torch.Tensor, - pts2_valid: torch.Tensor, - max_dist: float = 100.0, -) -> torch.Tensor: - """Symmetric chamfer distance ignoring invalid points. - - Args: - pts1: (B, N1, 3). - pts2: (B, N2, 3). - pts1_valid: (B, N1). - pts2_valid: (B, N2). - max_dist: Value used for invalid min-distances. - - Returns: - (B,) symmetric chamfer distance per batch. - """ - # Compute the distance between all points in pts1 and pts2 - dist1 = torch.cdist(pts1, pts2, p=2.0) # (B, N1, N2) - # Fill invalid points in pts2 with max distance - dist1 = dist1.masked_fill((~pts2_valid).unsqueeze(1), max_dist) - min_dists1 = torch.min(dist1, dim=-1).values # (B, N1) - num_valids1 = pts1_valid.sum(dim=-1) # (B,) - - dist2 = torch.cdist(pts2, pts1, p=2.0) # (B, N2, N1) - # Fill invalid points in pts1 with max distance - dist2 = dist2.masked_fill((~pts1_valid).unsqueeze(1), max_dist) - min_dists2 = torch.min(dist2, dim=-1).values # (B, N2) - num_valids2 = pts2_valid.sum(dim=-1) - - both_zero = (num_valids1 == 0) & (num_valids2 == 0) - one_zero = (num_valids1 == 0) ^ (num_valids2 == 0) - both_has_valid = (num_valids1 > 0) & (num_valids2 > 0) - - chamfer_forward = torch.zeros(pts1.shape[0], device=pts1.device) - chamfer_backward = torch.zeros(pts2.shape[0], device=pts2.device) - chamfer_forward[both_zero] = 0.0 - chamfer_backward[both_zero] = 0.0 - chamfer_forward[one_zero] = max_dist - chamfer_backward[one_zero] = max_dist - if both_has_valid.sum() > 0: - valid_sums1 = torch.sum(min_dists1 * pts1_valid, dim=-1) - valid_sums2 = torch.sum(min_dists2 * pts2_valid, dim=-1) - chamfer_forward[both_has_valid] = (valid_sums1 / num_valids1.clamp(min=1e-5))[ - both_has_valid - ] - chamfer_backward[both_has_valid] = (valid_sums2 / num_valids2.clamp(min=1e-5))[ - both_has_valid - ] - return (chamfer_forward + chamfer_backward) / 2.0 - - -def sample_wrench_space_basis_scaled( - num_samples: int, - rc: float, - device: torch.device, - dtype: torch.dtype = torch.float32, - eps: float = 1e-8, -) -> torch.Tensor: - """Sample and scale wrench space basis directions uniformly on the surface of a 6D unit sphere. - - Args: - num_samples: number of samples to draw - rc: scale of the torque, typically the radius of the object's bounding ball - device: device for the output tensor - dtype: dtype for the output tensor - eps: numerical stability - - Returns: - (num_samples, dim) - """ - basis = torch.randn(num_samples, 6, device=device, dtype=dtype) - basis[:, 3:] = basis[:, 3:] / rc - return basis / basis.norm(dim=-1, keepdim=True).clamp_min(eps) - - -def compute_tangent_basis( - n: torch.Tensor, - eps: float = 1e-6, -) -> tuple[torch.Tensor, torch.Tensor]: - """Compute orthonormal tangent basis from normal vectors (Frisvad 2012). - - Args: - n: (..., 3) normal vectors (need not be unit length) - eps: numerical stability - - Returns: - t1: (..., 3) - t2: (..., 3) - """ - nx, ny, nz = n.unbind(dim=-1) - - sign = torch.where(nz >= 0, 1.0, -1.0) - den = sign + nz - den = torch.where(den.abs() < eps, sign * eps, den) - - a = -1.0 / den - b = nx * ny * a - - t1 = torch.stack( - (1.0 + sign * nx * nx * a, sign * b, -sign * nx), - dim=-1, - ) - t2 = torch.stack( - (b, sign + ny * ny * a, -ny), - dim=-1, - ) - - t1 = t1 / t1.norm(dim=-1, keepdim=True).clamp_min(eps) - t2 = t2 / t2.norm(dim=-1, keepdim=True).clamp_min(eps) - - return t1, t2 - - -def compute_friction_cone_edges( - normals: torch.Tensor, - cos_t: torch.Tensor, - sin_t: torch.Tensor, - friction_coefficients: float = 0.5, - eps: float = 1e-6, - append_normal: bool = True, -) -> torch.Tensor: - """Build the edges of the friction cone based on contact normals and friction coefficients. - - Args: - normals: (batch_size, num_contacts, 3) - cos_t: the cosine of the friction cone edges phase angles (1, num_friction_cone_edges, 1) - sin_t: the sine of the friction cone edges phase angles (1, num_friction_cone_edges, 1) - friction_coefficients: float - eps: float - append_normal: bool whether to append the contact normal to the friction cone edges - - Returns: - (batch_size, num_contacts, num_friction_cone_edges, 3) - """ - batch_size, num_contacts, _ = normals.shape - - # Ensure unit normals before tangent construction. - normals_flat = normals.reshape(-1, 3) - t1, t2 = compute_tangent_basis(normals_flat, eps=eps) # (B*C, 2, 3) - - n_exp = normals_flat.unsqueeze(1) # (B*C, 1, 3) - t1_exp = t1.unsqueeze(1) # (B*C, 1, 3) - t2_exp = t2.unsqueeze(1) # (B*C, 1, 3) - - # Polyhedral approximation rays: n + mu * (cos(theta) * t1 + sin(theta) * t2) - edges = n_exp + friction_coefficients * ( - cos_t * t1_exp + sin_t * t2_exp - ) # (B*C, K, 3) - edges = edges / edges.norm(dim=-1, keepdim=True).clamp_min(eps) - - if append_normal: - edges = torch.cat([edges, normals_flat.unsqueeze(1)], dim=1) # (B*C, K+1, 3) - - num_edges = cos_t.shape[1] + int(append_normal) - return edges.view(batch_size, num_contacts, num_edges, 3) - - -def compute_wrench_space( - contact_points: torch.Tensor, - contact_normals: torch.Tensor, - cos_t: torch.Tensor, - sin_t: torch.Tensor, - rc: float, - friction_coefficients: float = 0.5, - eps: float = 1e-6, -) -> torch.Tensor: - """Compute the wrench space from contact points and contact normals. - - Args: - contact_points: in the object frame (batch_size, num_contacts, 3) - contact_normals: pointing into the object (batch_size, num_contacts, 3) - cos_t: the cosine of the friction cone edges phase angles (1, num_friction_cone_edges, 1) - sin_t: the sine of the friction cone edges phase angles (1, num_friction_cone_edges, 1) - rc: scale of the torque, typically the radius of the object's bounding ball - friction_coefficients: float - eps: float - - Returns: - (batch_size, 6, num_contacts * num_friction_cone_edges,) - """ - batch_size = len(contact_points) - - contact_normals = contact_normals / contact_normals.norm( - dim=-1, keepdim=True - ).clamp_min(eps) - - contact_is_active = contact_normals.norm(dim=-1) > 1e-3 - - forces = compute_friction_cone_edges( - normals=contact_normals, - cos_t=cos_t, - sin_t=sin_t, - friction_coefficients=friction_coefficients, - eps=eps, - ) # (batch_size, num_contacts, num_friction_cone_edges, 3) - - torques = torch.cross( - contact_points.unsqueeze(2).expand_as(forces), forces, dim=-1 - ) # (batch_size, num_contacts, num_friction_cone_edges, 3) - - wrench_space = torch.cat( - (forces, torques / rc), dim=-1 - ) # (batch_size, num_contacts, num_friction_cone_edges, 6) - wrench_space *= contact_is_active.view(batch_size, -1, 1, 1) - wrench_space = wrench_space.view(batch_size, -1, 6) - - return wrench_space.transpose(1, 2).contiguous() - - -def compute_wrench_space_support_function( - wrench_space: torch.Tensor, - basis: torch.Tensor, -) -> torch.Tensor: - """ - Compute the wrench space support function at basis directions. - - The support function of wrench space W at direction d is $max_{w in W} d^T w$. - The support function is clamped to be non-negative in wrench space. - - Args: - wrench_space: (batch_size, 6, num_contacts) - basis: (num_basis, 6) - - Returns: - (batch_size, num_basis) - """ - return torch.clamp( - torch.matmul(basis.unsqueeze(0), wrench_space).amax(dim=-1), - min=0.0, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils_jit.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils_jit.py deleted file mode 100644 index f092a949..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/mdp/utils_jit.py +++ /dev/null @@ -1,670 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""JIT utility functions for the V2P environment.""" - -from typing import Tuple - -import torch -from isaaclab.utils.math import quat_inv - - -@torch.jit.script -def refresh_jit( - right_forces_w: torch.Tensor, - left_forces_w: torch.Tensor, - retargeted_left_contact_wrench_supports: torch.Tensor, - retargeted_right_contact_wrench_supports: torch.Tensor, - timestep_counter: torch.Tensor, - right_contact_wrench_supports: torch.Tensor, - left_contact_wrench_supports: torch.Tensor, - object_position_e: torch.Tensor, - object_orientation_e: torch.Tensor, -) -> Tuple[ - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, -]: - """Pure-tensor derivations for DualHandsObjectTrackingCommand.refresh_tensors. - - Fuses the force-reduction, in-contact, reference-indexing, 1e-3 wrench-mask - and pose-squeeze computations into a single scripted graph. The wrench support - body loop stays in Python (``_compute_contact_wrench_supports``) because it - calls external utility functions and iterates over object bodies. - - Returns (in order): - 0 right_force_sq_per_link (N, num_right_links) - 1 left_force_sq_per_link (N, num_left_links) - 2 right_link_in_contact (N, num_right_links) bool - 3 left_link_in_contact (N, num_left_links) bool - 4 right_in_contact (N,) bool - 5 left_in_contact (N,) bool - 6 in_contact (N,) bool - 7 ref_L (N, num_bodies, B) - 8 ref_R (N, num_bodies, B) - 9 mask_L (N, num_bodies, B) bool - 10 mask_R (N, num_bodies, B) bool - 11 ref_active_per_cell (N, num_bodies, B) bool - 12 ref_active_per_body (N, num_bodies) bool - 13 ref_active_global (N,) bool - 14 right_wrench_cmd_active (N, num_bodies, B) bool - 15 left_wrench_cmd_active (N, num_bodies, B) bool - 16 right_wrench_cur_active (N, num_bodies, B) bool - 17 left_wrench_cur_active (N, num_bodies, B) bool - 18 right_wrench_cmd_active_per_body (N, num_bodies) bool - 19 left_wrench_cmd_active_per_body (N, num_bodies) bool - 20 right_wrench_cur_active_per_body (N, num_bodies) bool - 21 left_wrench_cur_active_per_body (N, num_bodies) bool - 22 object_position_e_sq (N, 3) - 23 object_wxyz_e_sq (N, 4) - """ - eps = 1e-6 - in_contact_force_threshold = 1e-3 - support_thr = 1e-3 - - # Per-link squared-force reduction (contact_force_reward / contact_force_range_reward). - right_force_sq_per_link = right_forces_w.square().sum(dim=-1).mean(dim=1).sum(dim=1) - left_force_sq_per_link = left_forces_w.square().sum(dim=-1).mean(dim=1).sum(dim=1) - right_link_in_contact = right_force_sq_per_link > in_contact_force_threshold - left_link_in_contact = left_force_sq_per_link > in_contact_force_threshold - - # Per-env in-contact (contact_wrench_reward / contact_wrench_continuous_reward). - # Use torch.linalg.vector_norm explicitly — Tensor.norm(dim=...) trips the - # TorchScript overload resolver on the default `p` argument. - right_force_mag = torch.linalg.vector_norm( - torch.linalg.vector_norm(right_forces_w, dim=1), dim=-1 - ) - left_force_mag = torch.linalg.vector_norm( - torch.linalg.vector_norm(left_forces_w, dim=1), dim=-1 - ) - right_in_contact = ( - (right_force_mag > in_contact_force_threshold).any(dim=-1).any(dim=-1) - ) - left_in_contact = ( - (left_force_mag > in_contact_force_threshold).any(dim=-1).any(dim=-1) - ) - in_contact = right_in_contact | left_in_contact - - # Reference wrench indexing and active masks at eps=1e-6 ("any non-zero"). - ref_L = retargeted_left_contact_wrench_supports[timestep_counter] - ref_R = retargeted_right_contact_wrench_supports[timestep_counter] - mask_L = ref_L > eps - mask_R = ref_R > eps - ref_active_per_cell = mask_L | mask_R - ref_active_per_body = ref_active_per_cell.any(dim=-1) - ref_active_global = ref_active_per_body.any(dim=-1) - - # 1e-3 "meaningful support" masks (contact_wrench_support_reward, - # unintended_contact_penalty, missed_contact_penalty). - right_wrench_cmd_active = ref_R > support_thr - left_wrench_cmd_active = ref_L > support_thr - right_wrench_cur_active = right_contact_wrench_supports > support_thr - left_wrench_cur_active = left_contact_wrench_supports > support_thr - right_wrench_cmd_active_per_body = right_wrench_cmd_active.any(dim=-1) - left_wrench_cmd_active_per_body = left_wrench_cmd_active.any(dim=-1) - right_wrench_cur_active_per_body = right_wrench_cur_active.any(dim=-1) - left_wrench_cur_active_per_body = left_wrench_cur_active.any(dim=-1) - - # Object pose body-dim squeeze. - object_position_e_sq = object_position_e.squeeze(1) - object_wxyz_e_sq = object_orientation_e.squeeze(1) - - return ( - right_force_sq_per_link, - left_force_sq_per_link, - right_link_in_contact, - left_link_in_contact, - right_in_contact, - left_in_contact, - in_contact, - ref_L, - ref_R, - mask_L, - mask_R, - ref_active_per_cell, - ref_active_per_body, - ref_active_global, - right_wrench_cmd_active, - left_wrench_cmd_active, - right_wrench_cur_active, - left_wrench_cur_active, - right_wrench_cmd_active_per_body, - left_wrench_cmd_active_per_body, - right_wrench_cur_active_per_body, - left_wrench_cur_active_per_body, - object_position_e_sq, - object_wxyz_e_sq, - ) - - -@torch.jit.script -def _quat_rotate_broadcast(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: - """Apply quaternion rotation to vectors, supporting broadcasting. - - Unlike quat_apply which reshapes to (-1, N) and requires matching shapes, - this uses element-wise ops that broadcast naturally. - - Args: - q: Quaternion (wxyz). Shape is (..., 4), broadcastable against *v*. - v: Vectors. Shape is (..., 3). - - Returns: - Rotated vectors. Shape is broadcast(q[..., :3], v). - """ - xyz = q[..., 1:] - t = torch.linalg.cross(xyz, v) * 2 - return v + q[..., 0:1] * t + torch.linalg.cross(xyz, t) - - -@torch.jit.script -def wrench_preprocess_jit( - contact_positions_w: torch.Tensor, - contact_forces_first_hist_w: torch.Tensor, - object_com_position_w: torch.Tensor, - object_com_orientation_w: torch.Tensor, - num_envs: int, - num_bodies: int, - num_robot_contacts: int, -) -> Tuple[torch.Tensor, torch.Tensor]: - """Fuse the per-refresh wrench-support preprocessing into one scripted graph. - - Inlines the `subtract_frame_transforms` logic so we don't pay the non-scripted - Python wrapper. Returns contact positions and contact normals, both expressed in - the object COM frame and masked by the active-contact flag. - - Args: - contact_positions_w: (N, bodies, num_robot_contacts, 3) world-frame positions. - contact_forces_first_hist_w: (N, bodies, num_robot_contacts, 3) world-frame forces, - already sliced to the latest history entry. - object_com_position_w: (N, bodies, 1, 3) unexpanded COM position. - object_com_orientation_w: (N, bodies, 1, 4) unexpanded COM quat (wxyz). - num_envs: Number of environments; used to reshape the per-cell active mask. - num_bodies: Number of object bodies; used to reshape the active mask. - num_robot_contacts: Number of per-body robot contact points; used to - reshape the active mask. - - Returns: - contact_positions_com: (N, bodies, num_robot_contacts, 3) - contact_normals_com: (N, bodies, num_robot_contacts, 3) - """ - active_contact = ( - torch.linalg.vector_norm(contact_positions_w, dim=-1) > 1e-3 - ) # (N, bodies, num_robot_contacts) - active_mask = active_contact.view(num_envs, num_bodies, num_robot_contacts, 1).to( - contact_positions_w.dtype - ) - - # quat_inv on (N, bodies, 1, 4) — num_robot_contacts times cheaper - # than the old (N, bodies, num_robot_contacts, 4) path. - q10 = quat_inv(object_com_orientation_w) - - # Subtraction broadcasts (N,B,C,3) - (N,B,1,3). The rotation helper - # broadcasts q10 (N,B,1,4) against the (N,B,C,3) result via cross/mul. - contact_positions_com = _quat_rotate_broadcast( - q10, contact_positions_w - object_com_position_w - ) - contact_positions_com = contact_positions_com * active_mask - - # Normals = unit-normalized force direction (contact sensors report normal forces only). - normals_w = contact_forces_first_hist_w / torch.linalg.vector_norm( - contact_forces_first_hist_w, dim=-1, keepdim=True - ).clamp(min=1e-5) - contact_normals_com = _quat_rotate_broadcast(q10, normals_w) - contact_normals_com = contact_normals_com * active_mask - - return contact_positions_com, contact_normals_com - - -@torch.jit.script -def friction_cone_edges_jit( - normals: torch.Tensor, - cos_t: torch.Tensor, - sin_t: torch.Tensor, - friction_coefficients: float, - eps: float, -) -> torch.Tensor: - """Polyhedral friction-cone rays with the contact normal appended. - - Mirrors `v2p.mdp.utils.compute_friction_cone_edges` but is JIT-compiled and - uses `torch.linalg.vector_norm` (Tensor.norm's default-p overload trips JIT). - - Args: - normals: (batch_size, num_contacts, 3) contact normals. - cos_t: (1, K, 1) cosines of the friction-cone edge phase angles. - sin_t: (1, K, 1) sines of the friction-cone edge phase angles. - friction_coefficients: Scalar friction coefficient. - eps: Scalar epsilon for safe divisions / sign handling. - - Returns: - (batch_size, num_contacts, K+1, 3) - """ - batch_size = normals.shape[0] - num_contacts = normals.shape[1] - - # Frisvad 2012 tangent basis, inlined (compute_tangent_basis). - normals_flat = normals.reshape(-1, 3) - nx = normals_flat[:, 0] - ny = normals_flat[:, 1] - nz = normals_flat[:, 2] - sign = torch.where( - nz >= 0, - torch.ones_like(nz), - -torch.ones_like(nz), - ) - den = sign + nz - den = torch.where(den.abs() < eps, sign * eps, den) - a = -1.0 / den - b = nx * ny * a - t1 = torch.stack( - (1.0 + sign * nx * nx * a, sign * b, -sign * nx), - dim=-1, - ) - t2 = torch.stack( - (b, sign + ny * ny * a, -ny), - dim=-1, - ) - t1 = t1 / torch.linalg.vector_norm(t1, dim=-1, keepdim=True).clamp(min=eps) - t2 = t2 / torch.linalg.vector_norm(t2, dim=-1, keepdim=True).clamp(min=eps) - - n_exp = normals_flat.unsqueeze(1) # (B*C, 1, 3) - t1_exp = t1.unsqueeze(1) - t2_exp = t2.unsqueeze(1) - - edges = n_exp + friction_coefficients * (cos_t * t1_exp + sin_t * t2_exp) - edges = edges / torch.linalg.vector_norm(edges, dim=-1, keepdim=True).clamp(min=eps) - edges = torch.cat([edges, n_exp], dim=1) # append_normal=True - - num_edges = cos_t.shape[1] + 1 - return edges.view(batch_size, num_contacts, num_edges, 3) - - -@torch.jit.script -def wrench_support_one_body_jit( - contact_points: torch.Tensor, - contact_normals: torch.Tensor, - cos_t: torch.Tensor, - sin_t: torch.Tensor, - basis: torch.Tensor, - rc: float, - friction_coefficients: float, -) -> torch.Tensor: - """Compute the wrench-space support for one object body. - - Fuses `compute_wrench_space` + `compute_wrench_space_support_function` for a - single body. - - Args: - contact_points: (N, num_contacts, 3) in object COM frame. - contact_normals: (N, num_contacts, 3) in object COM frame. - cos_t: (1, K, 1) cosines of the friction-cone edge phase angles. - sin_t: (1, K, 1) sines of the friction-cone edge phase angles. - basis: (num_basis, 6) sampled wrench-space basis directions. - rc: body radius scale for torque. - friction_coefficients: friction coefficient scalar. - - Returns: - support: (N, num_basis) non-negative support function. - """ - eps = 1e-6 - batch_size = contact_points.shape[0] - - # Re-normalize and active mask for the per-body subset. - normals_norm = torch.linalg.vector_norm(contact_normals, dim=-1, keepdim=True) - normals = contact_normals / normals_norm.clamp(min=eps) - contact_is_active = torch.linalg.vector_norm(normals, dim=-1) > 1e-3 - - forces = friction_cone_edges_jit( - normals, cos_t, sin_t, friction_coefficients, eps - ) # (N, num_contacts, K+1, 3) - - torques = torch.cross(contact_points.unsqueeze(2).expand_as(forces), forces, dim=-1) - - wrench_space = torch.cat( - (forces, torques / rc), dim=-1 - ) # (N, num_contacts, K+1, 6) - wrench_space = wrench_space * contact_is_active.view(batch_size, -1, 1, 1).to( - wrench_space.dtype - ) - wrench_space = wrench_space.view(batch_size, -1, 6).transpose(1, 2).contiguous() - - # Support function: max over wrench-space contributions along each basis direction. - support = torch.matmul(basis.unsqueeze(0), wrench_space).amax(dim=-1) - return torch.clamp(support, min=0.0) - - -@torch.jit.script -def resample_compute_tensors_jit( - tc: torch.Tensor, - env_origins_sel: torch.Tensor, - retargeted_object_body_position: torch.Tensor, - retargeted_object_body_wxyz: torch.Tensor, - retargeted_right_wrist_position: torch.Tensor, - retargeted_right_wrist_wxyz: torch.Tensor, - retargeted_left_wrist_position: torch.Tensor, - retargeted_left_wrist_wxyz: torch.Tensor, - retargeted_right_finger_joints: torch.Tensor, - retargeted_left_finger_joints: torch.Tensor, - right_soft_joint_pos_limits_sel: torch.Tensor, - left_soft_joint_pos_limits_sel: torch.Tensor, - reset_finger_openness: float, - n: int, -) -> Tuple[ - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, - torch.Tensor, -]: - """Fused pure-tensor derivations for DualHandsObjectTrackingCommand._resample_command. - - Takes the timestep indices ``tc`` (already gathered from ``timestep_counter[env_ids]``), - the selected ``env_origins`` offsets, the retargeted motion buffers, and the per-env - soft joint limit slices. Returns every tensor the Python wrapper needs to scatter into - ``self.*`` buffers or pass to ``write_*_to_sim``. - - Returns (in order): - 0 object_pose (n, num_bodies, 7) world-frame - 1 object_velocity (n, num_bodies, 6) zeros - 2 right_hand_wrist_position_e (n, 3) env-frame (for reset_*_wrist scatter) - 3 right_hand_wrist_wxyz (n, 4) - 4 left_hand_wrist_position_e (n, 3) - 5 left_hand_wrist_wxyz (n, 4) - 6 right_hand_wrist_pose (n, 7) world-frame - 7 left_hand_wrist_pose (n, 7) - 8 wrist_zero_velocity (n, 6) shared by both wrists - 9 right_hand_finger_joint_pos (n, num_right_finger_joints) - 10 left_hand_finger_joint_pos (n, num_left_finger_joints) - 11 finger_zero_velocity (n, num_right_finger_joints) shared across hands - """ - # ── Object pose ────────────────────────────────────────────────────────── - object_position_e = retargeted_object_body_position[tc] - object_wxyz = retargeted_object_body_wxyz[tc] - object_pose = torch.cat( - [object_position_e + env_origins_sel.unsqueeze(1), object_wxyz], dim=-1 - ) - object_velocity = torch.zeros( - object_pose.size(0), - object_pose.size(1), - 6, - device=object_pose.device, - dtype=object_pose.dtype, - ) - - # ── Wrist poses (both env and world frames) ────────────────────────────── - right_hand_wrist_position_e = retargeted_right_wrist_position[tc] - right_hand_wrist_wxyz = retargeted_right_wrist_wxyz[tc] - left_hand_wrist_position_e = retargeted_left_wrist_position[tc] - left_hand_wrist_wxyz = retargeted_left_wrist_wxyz[tc] - - right_hand_wrist_pose = torch.cat( - [right_hand_wrist_position_e + env_origins_sel, right_hand_wrist_wxyz], - dim=-1, - ) - left_hand_wrist_pose = torch.cat( - [left_hand_wrist_position_e + env_origins_sel, left_hand_wrist_wxyz], - dim=-1, - ) - wrist_zero_velocity = torch.zeros( - n, - 6, - device=right_hand_wrist_pose.device, - dtype=right_hand_wrist_pose.dtype, - ) - - # ── Finger joint targets (scaled then clamped to soft limits) ──────────── - finger_factor = torch.rand(n, 1, device=tc.device) * reset_finger_openness - right_hand_finger_joint_pos = finger_factor * retargeted_right_finger_joints[tc] - left_hand_finger_joint_pos = finger_factor * retargeted_left_finger_joints[tc] - right_hand_finger_joint_pos.clamp_( - right_soft_joint_pos_limits_sel[..., 0], - right_soft_joint_pos_limits_sel[..., 1], - ) - left_hand_finger_joint_pos.clamp_( - left_soft_joint_pos_limits_sel[..., 0], - left_soft_joint_pos_limits_sel[..., 1], - ) - finger_zero_velocity = torch.zeros_like(right_hand_finger_joint_pos) - - return ( - object_pose, - object_velocity, - right_hand_wrist_position_e, - right_hand_wrist_wxyz, - left_hand_wrist_position_e, - left_hand_wrist_wxyz, - right_hand_wrist_pose, - left_hand_wrist_pose, - wrist_zero_velocity, - right_hand_finger_joint_pos, - left_hand_finger_joint_pos, - finger_zero_velocity, - ) - - -@torch.jit.script -def contact_wrench_support_reward_jit( - right_cmd_active: torch.Tensor, - right_cur_active: torch.Tensor, - left_cmd_active: torch.Tensor, - left_cur_active: torch.Tensor, - right_cmd_active_per_body: torch.Tensor, - left_cmd_active_per_body: torch.Tensor, - right_cmd_supports: torch.Tensor, - right_cur_supports: torch.Tensor, - left_cmd_supports: torch.Tensor, - left_cur_supports: torch.Tensor, - tolerance: float, - var: float, -) -> torch.Tensor: - """Pure-tensor body of contact_wrench_support_reward. - - Stacks right+left hands along a new leading axis so each downstream op - (clamp / square / exp / sum / division) runs once on a doubled tensor - instead of twice on separate tensors. Cuts kernel-launch overhead roughly - in half for the hot reward path. - - Inputs are all already cached by ``DualHandsObjectTrackingCommand.refresh_tensors``; - the wrapper just hands them to this scripted graph. - """ - # Fuse right+left into one leading-axis batch. - # per-cell bool masks: (N, B, K) → (2, N, B, K) - # per-body bool masks: (N, B) → (2, N, B) - # wrench-support scalars: (N, B, K) → (2, N, B, K) - cmd_active = torch.stack((right_cmd_active, left_cmd_active), dim=0) - cur_active = torch.stack((right_cur_active, left_cur_active), dim=0) - cmd_active_per_body = torch.stack( - (right_cmd_active_per_body, left_cmd_active_per_body), dim=0 - ) - cmd_supports = torch.stack((right_cmd_supports, left_cmd_supports), dim=0) - cur_supports = torch.stack((right_cur_supports, left_cur_supports), dim=0) - - # Counts (float-cast so downstream divisions stay in float). - cmd_num = cmd_active.sum(dim=-1).float().clamp(min=1e-6) # (2, N, B) - cmd_num_body = cmd_active_per_body.sum(dim=-1).float().clamp(min=1e-6) # (2, N) - num_active_hands = (cmd_num_body > 1e-3).float().sum(dim=0).clamp(min=1e-6) # (N,) - - # Current supports must be within (1 ± tolerance) of the commanded supports. - # Both directional excess terms get squared and summed into a per-cell loss. - better = ((1.0 - tolerance) * cmd_supports - cur_supports).clamp(min=0.0) - too_large = (cur_supports - (1.0 + tolerance) * cmd_supports).clamp(min=0.0) - loss = better.square() + too_large.square() - - # Per-body inclusion reward: only count cells where the command and the agent - # both have meaningful support, then average over the commanded basis count. - reward = ((cmd_active & cur_active).float() * torch.exp(-loss / var)).sum( - dim=-1 - ) / cmd_num # (2, N, B) - per_hand_reward = reward.sum(dim=-1) / cmd_num_body # (2, N) - return per_hand_reward.sum(dim=0) / num_active_hands # (N,) - - -@torch.jit.script -def unintended_contact_penalty_jit( - right_cmd_active_per_body: torch.Tensor, - right_cur_active_per_body: torch.Tensor, - left_cmd_active_per_body: torch.Tensor, - left_cur_active_per_body: torch.Tensor, - right_cur_supports: torch.Tensor, - left_cur_supports: torch.Tensor, - num_bodies: int, -) -> torch.Tensor: - """Pure-tensor body of unintended_contact_penalty.""" - right_cmd_num = right_cmd_active_per_body.sum(dim=-1).float() - left_cmd_num = left_cmd_active_per_body.sum(dim=-1).float() - - right_unintended = torch.logical_and( - ~right_cmd_active_per_body, right_cur_active_per_body - ) - left_unintended = torch.logical_and( - ~left_cmd_active_per_body, left_cur_active_per_body - ) - - # Continuous penalty: when the command says "no contact" on a body, score - # the squared-then-mean-over-basis support magnitude there. - right_unintended_support = (~right_cmd_active_per_body).float() * ( - right_cur_supports.clamp(min=0.0).square().mean(dim=-1) - ) - right_unintended_support = right_unintended_support.sum(dim=-1) / ( - float(num_bodies) - right_cmd_num - ).clamp(min=1e-3) - - left_unintended_support = (~left_cmd_active_per_body).float() * ( - left_cur_supports.clamp(min=0.0).square().mean(dim=-1) - ) - left_unintended_support = left_unintended_support.sum(dim=-1) / ( - float(num_bodies) - left_cmd_num - ).clamp(min=1e-3) - - return ( - right_unintended.float().mean(dim=-1) - + right_unintended_support - + left_unintended.float().mean(dim=-1) - + left_unintended_support - ) - - -@torch.jit.script -def hand_keypoints_tracking_jit( - left_wrist_cmd: torch.Tensor, - right_wrist_cmd: torch.Tensor, - left_fingertip_cmd: torch.Tensor, - right_fingertip_cmd: torch.Tensor, - left_wrist_cur: torch.Tensor, - right_wrist_cur: torch.Tensor, - left_fingertip_cur: torch.Tensor, - right_fingertip_cur: torch.Tensor, - var: float, - threshold: float, -) -> torch.Tensor: - """Pure-tensor body of hand_keypoints_tracking_exp. - - Fuses per-hand keypoint assembly (wrist + fingertips) + L2 error + the - ``exp(-(err - threshold).clamp(min=0)/var)`` decay + the per-keypoint mean - into a single scripted graph. The two hands return the symmetric mean. - - Args: - left_wrist_cmd: (N, 3) commanded left wrist position in env frame. - right_wrist_cmd: (N, 3) commanded right wrist position in env frame. - left_fingertip_cmd: (N, F_left, 3) commanded left fingertip positions. - right_fingertip_cmd: (N, F_right, 3) commanded right fingertip positions. - left_wrist_cur: (N, 3) current left wrist position. - right_wrist_cur: (N, 3) current right wrist position. - left_fingertip_cur: (N, F_left, 3) current left fingertip positions. - right_fingertip_cur: (N, F_right, 3) current right fingertip positions. - var: Exp decay scale. - threshold: Saturation threshold; errors below it are clamped to zero. - - Returns: - Reward tensor of shape ``(N,)``. - """ - # Build the per-hand keypoint stacks inline (wrist as the first keypoint). - left_cmd = torch.cat([left_wrist_cmd.unsqueeze(1), left_fingertip_cmd], dim=1) - right_cmd = torch.cat([right_wrist_cmd.unsqueeze(1), right_fingertip_cmd], dim=1) - left_cur = torch.cat([left_wrist_cur.unsqueeze(1), left_fingertip_cur], dim=1) - right_cur = torch.cat([right_wrist_cur.unsqueeze(1), right_fingertip_cur], dim=1) - - # Per-keypoint squared L2 error. - left_error = (left_cmd - left_cur).square().sum(dim=-1) - right_error = (right_cmd - right_cur).square().sum(dim=-1) - - # Saturated exponential decay, averaged across keypoints per hand. - left_reward = torch.exp(-(left_error - threshold).clamp(min=0.0) / var).mean(dim=-1) - right_reward = torch.exp(-(right_error - threshold).clamp(min=0.0) / var).mean( - dim=-1 - ) - - return (left_reward + right_reward) / 2.0 - - -@torch.jit.script -def missed_contact_penalty_jit( - right_cmd_active: torch.Tensor, - right_cur_active: torch.Tensor, - left_cmd_active: torch.Tensor, - left_cur_active: torch.Tensor, - right_cmd_active_per_body: torch.Tensor, - left_cmd_active_per_body: torch.Tensor, -) -> torch.Tensor: - """Pure-tensor body of missed_contact_penalty. - - For each hand, counts the fraction of commanded basis directions per body - that the agent failed to cover, averages over bodies the command actually - requests, then averages over hands that have any commanded contact at all. - """ - # Right hand. - right_missed = right_cmd_active & ~right_cur_active - right_n_expected = right_cmd_active.sum(dim=-1).float() - right_n_missed = right_missed.sum(dim=-1).float() - right_missing_frac = right_n_missed / right_n_expected.clamp(min=1e-6) - right_body_has_contact = right_n_expected > 0 - right_n_active_bodies = right_body_has_contact.sum(dim=-1).float().clamp(min=1e-6) - right_penalty = (right_missing_frac * right_body_has_contact.float()).sum( - dim=-1 - ) / right_n_active_bodies - - # Left hand. - left_missed = left_cmd_active & ~left_cur_active - left_n_expected = left_cmd_active.sum(dim=-1).float() - left_n_missed = left_missed.sum(dim=-1).float() - left_missing_frac = left_n_missed / left_n_expected.clamp(min=1e-6) - left_body_has_contact = left_n_expected > 0 - left_n_active_bodies = left_body_has_contact.sum(dim=-1).float().clamp(min=1e-6) - left_penalty = (left_missing_frac * left_body_has_contact.float()).sum( - dim=-1 - ) / left_n_active_bodies - - # Hand-active normalization (count hands that have any commanded contact). - right_hand_active = right_cmd_active_per_body.any(dim=-1) - left_hand_active = left_cmd_active_per_body.any(dim=-1) - num_active_hands = (right_hand_active.float() + left_hand_active.float()).clamp( - min=1e-6 - ) - - return (right_penalty + left_penalty) / num_active_hands diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_env_cfg.py deleted file mode 100644 index 0ca3bfcb..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_env_cfg.py +++ /dev/null @@ -1,718 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -import isaaclab.envs.mdp as isaac_mdp -import isaaclab.sim as sim_utils -import isaaclab.terrains as terrain_gen -from isaaclab.assets import ArticulationCfg, AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import CurriculumTermCfg as CurrTerm -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import SceneEntityCfg -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.utils import configclass -from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise -from omegaconf import MISSING - -from robotic_grounding.tasks.v2p import mdp - -################################################# -# Scene definition -################################################# - - -@configclass -class V2PSceneCfg(InteractiveSceneCfg): - """Configuration for the terrain scene with two robots.""" - - terrain = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", terrain_type="plane", debug_vis=False - ) - - # robots - right_robot: ArticulationCfg = MISSING - left_robot: ArticulationCfg = MISSING - - # lights - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - sky_light = AssetBaseCfg( - prim_path="/World/skyLight", - spawn=sim_utils.DomeLightCfg(color=(0.13, 0.13, 0.13), intensity=1000.0), - ) - - -################################################# -# MDP settings -################################################# - - -@configclass -class CommandsCfg: - """Command specifications for the MDP.""" - - # Patched by apply_scene_commands with scene-specific fields. - dual_hands_object_tracking_command = mdp.DualHandsObjectTrackingCommandCfg( - motion_speed=0.5, - reset_finger_openness=0.7, - initial_virtual_object_control_curriculum_scale=1.0, - virtual_object_control_decay_steps=20, - virtual_object_control_decay_mode="step", - recompute_hand_keypoints_from_object=True, - ) - - -@configclass -class ActionsCfg: - """Action specifications for the MDP.""" - - # Virtual object control is added in apply_scene_commands for each object - - right_joint_residual_action = mdp.JointResidualWithTrackingActionCfg( - asset_name="right_robot", - joint_names=[".*"], - tracking_controller_linear_stiffness=50.0, - tracking_controller_linear_damping=10.0, - tracking_controller_angular_stiffness=12.0, - tracking_controller_angular_damping=0.1, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - ema_factor=0.3, - ) - - left_joint_residual_action = mdp.JointResidualWithTrackingActionCfg( - asset_name="left_robot", - joint_names=[".*"], - tracking_controller_linear_stiffness=50.0, - tracking_controller_linear_damping=10.0, - tracking_controller_angular_stiffness=12.0, - tracking_controller_angular_damping=0.1, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - ema_factor=0.3, - ) - - -@configclass -class ObservationsCfg: - """Observation specifications for the MDP.""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for policy group. Order preserved.""" - - wrist_position_e = ObsTerm( - func=mdp.wrist_position_e, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_orientation_e = ObsTerm( - func=mdp.wrist_orientation_e, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_velocity_b = ObsTerm( - func=mdp.wrist_velocity_b, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - finger_joint_pos = ObsTerm( - func=mdp.finger_joint_pos, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - finger_joint_vel = ObsTerm( - func=mdp.finger_joint_vel, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_position_e = ObsTerm( - func=mdp.object_position_e, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_orientation_e = ObsTerm( - func=mdp.object_orientation_e, - params={"command_name": "dual_hands_object_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - - # object_t_wrist = ObsTerm( - # func=mdp.object_t_wrist, - # params={"command_name": "dual_hands_object_tracking_command"}, - # ) - - # object_p_fingertip = ObsTerm( - # func=mdp.object_p_fingertip, - # params={"command_name": "dual_hands_object_tracking_command"}, - # ) - - command = ObsTerm( - func=isaac_mdp.generated_commands, - params={"command_name": "dual_hands_object_tracking_command"}, - ) - - actions = ObsTerm(func=isaac_mdp.last_action) - processed_right_actions = ObsTerm( - func=mdp.processed_action, - params={"action_name": "right_joint_residual_action"}, - ) - processed_left_actions = ObsTerm( - func=mdp.processed_action, - params={"action_name": "left_joint_residual_action"}, - ) - - contact_position_direction_in_wrist = ObsTerm( - func=mdp.contact_position_direction_in_wrist, - params={"command_name": "dual_hands_object_tracking_command"}, - ) - - def __post_init__(self) -> None: - """Post initialization.""" - self.enable_corruption = False - self.concatenate_terms = True - - # observation groups - policy: PolicyCfg = PolicyCfg() - - -@configclass -class EventCfg: - """Configuration for events.""" - - # prestartup - setup_collision_groups = EventTerm( - func=mdp.configure_collision_groups, - mode="prestartup", - params={ - "robot_names": [], - "object_names": [], - "fixed_object_names": [], - "disable_robot_to_object_collisions": False, - "disable_robot_to_fixed_object_collisions": True, - }, - ) - - # startup - right_physics_material = EventTerm( - func=isaac_mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("right_robot", body_names=".*"), - "static_friction_range": (2.0, 2.01), - "dynamic_friction_range": (2.0, 2.01), - "restitution_range": (0.0, 0.0), - "num_buckets": 64, - }, - ) - - left_physics_material = EventTerm( - func=isaac_mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("left_robot", body_names=".*"), - "static_friction_range": (2.0, 2.01), - "dynamic_friction_range": (2.0, 2.01), - "restitution_range": (0.0, 0.0), - "num_buckets": 64, - }, - ) - - # Object physics material event term disabled — SceneEntityCfg("object") does - # not match any real scene entity (objects spawn per-sequence with dynamic - # names). Objects use IsaacLab's default RigidBodyMaterialCfg (static=0.5, - # dynamic=0.5, restitution=0.0). - # object_physics_material = EventTerm( - # func=isaac_mdp.randomize_rigid_body_material, - # mode="startup", - # params={ - # "asset_cfg": SceneEntityCfg("object", body_names=".*"), - # "static_friction_range": (0.99, 1.01), - # "dynamic_friction_range": (0.99, 1.01), - # "restitution_range": (0.0, 0.0), - # "num_buckets": 64, - # }, - # ) - - -@configclass -class RewardsCfg: - """Reward terms for the MDP.""" - - action_rate_l2 = RewTerm(func=isaac_mdp.action_rate_l2, weight=-5e-3) - action_l1 = RewTerm( - func=mdp.action_norm, - weight=-2e-3, - params={ - "action_names": [ - "right_joint_residual_action", - "left_joint_residual_action", - ] - }, - ) - - # right_joint_limit = RewTerm( - # func=isaac_mdp.joint_pos_limits, - # weight=-10.0, - # params={"asset_cfg": SceneEntityCfg("right_robot", joint_names=[".*"])}, - # ) - # left_joint_limit = RewTerm( - # func=isaac_mdp.joint_pos_limits, - # weight=-10.0, - # params={"asset_cfg": SceneEntityCfg("left_robot", joint_names=[".*"])}, - # ) - - object_keypoints_tracking_exp = RewTerm( - func=mdp.object_keypoints_tracking_exp, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 0.1, - }, - ) - - object_meshvert_tracking_fine = RewTerm( - func=mdp.object_meshvert_tracking_fine, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 0.001, - }, - ) - - hand_keypoints_tracking_exp = RewTerm( - func=mdp.hand_keypoints_tracking_exp, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 0.1, - }, - ) - - hand_joint_pos_tracking_exp = RewTerm( - func=mdp.hand_joint_pos_tracking_exp, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 1.0, - }, - ) - - termination_penalty = RewTerm( - func=mdp.termination_penalty, - weight=-100.0, - ) - - contact_wrench_support_reward = RewTerm( - func=mdp.contact_wrench_support_reward, - weight=10.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "tolerance": 0.1, - "var": 0.1, - }, - ) - - contact_wrench_continuous_reward = RewTerm( - func=mdp.contact_wrench_continuous_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "approach_var": 0.05, - "in_contact_force_threshold": 1e-3, - }, - ) - - contact_wrench_cumulative_reward = RewTerm( - func=mdp.contact_wrench_cumulative_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "eps": 1e-6, - "streak_scale": 20.0, - }, - ) - - unintended_contact_penalty = RewTerm( - func=mdp.unintended_contact_penalty, - weight=-2.5, - params={ - "command_name": "dual_hands_object_tracking_command", - }, - ) - missed_contact_penalty = RewTerm( - func=mdp.missed_contact_penalty, - weight=-0.25, - params={ - "command_name": "dual_hands_object_tracking_command", - }, - ) - - dexmachina_contact_tracking_reward = RewTerm( - func=mdp.dexmachina_contact_tracking_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 0.03, - "mask_zero_contact": True, - }, - ) - - relative_object_pose_reward = RewTerm( - func=mdp.relative_object_pose_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "pos_sigma": 0.05, - "rot_sigma": 0.5, - }, - ) - - relative_object_pos_reward = RewTerm( - func=mdp.relative_object_pos_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "pos_sigma": 0.02, - }, - ) - - relative_object_rot_reward = RewTerm( - func=mdp.relative_object_rot_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "rot_sigma": 0.3, - }, - ) - - inter_object_proximity_reward = RewTerm( - func=mdp.inter_object_proximity_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "dist_sigma": 0.05, - }, - ) - - contact_force_reward = RewTerm( - func=mdp.contact_force_reward, - weight=0.0, - params={ - "command_name": "dual_hands_object_tracking_command", - "var": 2.0, - "threshold": 0.0, - }, - ) - - # contact_force_range_reward = RewTerm( - # func=mdp.contact_force_range_reward, - # weight=0.0, - # params={ - # "command_name": "dual_hands_object_tracking_command", - # "var": 1.0, - # "lower_force_squared": 4.0, - # "upper_force_squared": 16.0, - # }, - # ) - - # contact_force_rate_reward = RewTerm( - # func=mdp.contact_force_rate_reward, - # weight=0.0, - # params={ - # "command_name": "dual_hands_object_tracking_command", - # "var": 1.0, - # }, - # ) - - # contact_slippage_reward = RewTerm( - # func=mdp.contact_slippage_reward, - # weight=1.0, - # params={ - # "command_name": "dual_hands_object_tracking_command", - # "var": 1.0, - # }, - # ) - - -@configclass -class TerminationsCfg: - """Termination terms for the MDP.""" - - time_out = DoneTerm( - func=mdp.timestep_timeout, - time_out=True, - params={ - "command_name": "dual_hands_object_tracking_command", - }, - ) - - hand_wrist_away_from_trajectory = DoneTerm( - func=mdp.hand_wrist_away_from_trajectory, - params={ - "command_name": "dual_hands_object_tracking_command", - "threshold": 0.25, - }, - ) - - object_away_from_trajectory = DoneTerm( - func=mdp.object_away_from_trajectory, - params={ - "command_name": "dual_hands_object_tracking_command", - "position_threshold": 0.15, - "orientation_threshold": 0.7, - }, - ) - - -@configclass -class CurriculumCfg: - """Curriculum terms for the MDP.""" - - virtual_object_control_curriculum = CurrTerm( - func=mdp.VirtualObjectControlCurriculum, - params={ - "reward_thresholds": { - "object_keypoints_tracking_exp": 0.1, - "hand_keypoints_tracking_exp": 0.0, - "contact_wrench_support_reward": 1.6, - "contact_force_reward": 0.0, - }, - "episode_length_ratio_threshold": 0.95, - "decay_mode": "exponential", - "deque_maxlen": 500, - "command_name": "dual_hands_object_tracking_command", - "zero_scale_factor_threshold": 0.05, - "initial_wait_env_steps": 2000, - "wait_env_steps_since_last_decay": 1000, - "exponential_decay_factor": 0.9, - "linear_decay_step": 10.0, - "fixed_schedule_steps": None, - "fixed_schedule_values": None, - # 0.0 = disabled (filtered out at runtime); set > 0 to activate gate - "metric_thresholds": { - "contact_wrench_support_ratio_right": 0.0, - "contact_wrench_support_ratio_left": 0.0, - }, - # 0.0 = disabled; set > 0 to require current deque mean >= baseline * ratio - "reward_baseline_retention": { - "contact_wrench_support_reward": 0.0, - }, - # custom_schedule mode: explicit VOC levels + paired reward weight changes. - # Each list has one entry per decay event. Empty = custom_schedule disabled. - "custom_voc_schedule": [], - "custom_reward_schedules": { - "object_keypoints_tracking_exp": [], - "hand_keypoints_tracking_exp": [], - "hand_joint_pos_tracking_exp": [], - "object_meshvert_tracking_fine": [], - }, - # Force decay after this many env steps of being gate-eligible without firing. - # 0 = disabled. Only active in custom_schedule mode. - "max_eligible_wait_env_steps": 0, - # 0.0 = disabled; set > 0 to require metric <= threshold before decay. - "metric_upper_thresholds": { - "contact_wrench_support_reward_cv": 0.0, - # For multi-object tasks: gate VOC decay on relative orientation quality. - # Set to e.g. 0.15 (rad) to prevent decay while rot_err is too large. - # Ignored (with a warning suppressed by 0.0) on single-object tasks. - "relative_object_rot_error": 0.0, - }, - # If True, metric_upper_thresholds gate only applies before the first decay. - "metric_upper_thresholds_initial_only": False, - }, - ) - - -@configclass -class FixedTimestepCurriculumCfg: - """Curriculum for virtual object control with a fixed timestep decay.""" - - fixed_timestep_curriculum = CurrTerm( - func=mdp.FixedTimestepCurriculum, - params={ - "command_name": "dual_hands_object_tracking_command", - "num_steps_per_env": 24, - # Last entry (16500) is a post-training reward boost: VOC has been 0 - # since iter 14500, so the policy is already self-driving; the jump - # from 0.5 -> 20.0 on object_keypoints_tracking_exp amplifies the - # tracking signal for fine-grained object pose convergence. - "timestep_schedule": [ - 2000, - 4000, - 5500, - 7000, - 8500, - 10000, - 11500, - 13000, - 14500, - 16000, - 16500, - ], - "virtual_object_control_scale_factor": [ - 1.0, - 1.0, - 0.75, - 0.5, - 0.25, - 0.1, - 0.05, - 0.025, - 0.01, - 0.0, - 0.0, - ], - "rewards_object_keypoints_tracking_exp": [ - 0.0, - 0.0, - 0.1, - 0.1, - 0.2, - 0.25, - 0.25, - 0.5, - 0.5, - 0.5, - 20.0, - ], - "rewards_hand_keypoints_tracking_exp": [ - 0.25, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.25, - ], - "rewards_hand_joint_pos_tracking_exp": [ - 0.25, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.25, - ], - "rewards_contact_wrench_support_reward": 10.0, - "rewards_unintended_contact_penalty": -20.0, - "rewards_missed_contact_penalty": -5.0, - # Optional per-step scheduled rewards — 0.0 scalar expands to match - # the timestep_schedule length at runtime. - "rewards_object_meshvert_tracking_fine": 0.0, - "rewards_dexmachina_contact_tracking_reward": 0.0, - "rewards_relative_object_pos_reward": 0.0, - "rewards_relative_object_rot_reward": 0.0, - "rewards_inter_object_proximity_reward": 0.0, - # Optional per-step termination thresholds — None means no override. - "termination_object_away_from_trajectory_position_threshold": None, - "termination_object_away_from_trajectory_orientation_threshold": None, - "termination_hand_wrist_away_from_trajectory_threshold": None, - }, - ) - - -################################################# -# Environment configuration -################################################# - - -@configclass -class V2PHandEnvCfg(ManagerBasedRLEnvCfg): - """Configuration for the locomotion velocity-tracking environment.""" - - # Scene settings - scene: V2PSceneCfg = V2PSceneCfg( - num_envs=4096, - env_spacing=1.5, - replicate_physics=False, - filter_collisions=False, - ) - # Basic settings - observations: ObservationsCfg = ObservationsCfg() - actions: ActionsCfg = ActionsCfg() - commands: CommandsCfg = CommandsCfg() - # MDP settings - rewards: RewardsCfg = RewardsCfg() - terminations: TerminationsCfg = TerminationsCfg() - events: EventCfg = EventCfg() - curriculum: FixedTimestepCurriculumCfg = FixedTimestepCurriculumCfg() - - max_contact_data_count_per_prim: int = 1024 - - def __post_init__(self) -> None: - """Post initialization.""" - # general settings - self.decimation = 5 # 20 Hz control - self.episode_length_s = 20.0 # Overridden by the scene config - # simulation settings - self.sim.dt = 0.01 # 100 Hz simulation - self.sim.render_interval = self.decimation # 20 Hz rendering - self.sim.physics_material = self.scene.terrain.physics_material - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.gpu_max_rigid_contact_count = 2**23 - self.sim.physx.gpu_max_rigid_patch_count = 2**23 - - # Make the environment more compliant - self.sim.physics_material.compliant_contact_stiffness = 10.0 - self.sim.physics_material.compliant_contact_damping = 1.0 - - # viewer settings - self.viewer.eye = (-0.5, 0.5, 1.5) - self.viewer.lookat = (0.0, 0.0, 1.2) - # self.viewer.resolution = (3840, 2160) - self.viewer.origin_type = "env" - self.viewer.env_index = 6 - - -@configclass -class V2PHandEnvCfgEnvOnly(ManagerBasedRLEnvCfg): - """Configuration for the locomotion velocity-tracking environment.""" - - # Scene settings - scene: V2PSceneCfg = V2PSceneCfg(num_envs=4096, env_spacing=1.5) - # Basic settings - observations: ObservationsCfg = ObservationsCfg() - actions: ActionsCfg = ActionsCfg() - commands: CommandsCfg = CommandsCfg() - # MDP settings - rewards: RewardsCfg = RewardsCfg() - terminations: TerminationsCfg = TerminationsCfg() - events: EventCfg = EventCfg() - curriculum: FixedTimestepCurriculumCfg = FixedTimestepCurriculumCfg() - - max_contact_data_count_per_prim: int = 1024 - - def __post_init__(self) -> None: - """Post initialization.""" - # general settings - self.decimation = 5 # 20 Hz control - self.episode_length_s = 20.0 # Overridden by the scene config - # simulation settings - self.sim.dt = 0.01 # 100 Hz simulation - self.sim.render_interval = self.decimation # 20 Hz rendering - self.sim.physics_material = self.scene.terrain.physics_material - self.sim.physx.bounce_threshold_velocity = 0.2 - self.sim.physx.gpu_max_rigid_contact_count = 2**23 - self.sim.physx.gpu_max_rigid_patch_count = 2**23 - - # Make the environment more compliant - self.sim.physics_material.compliant_contact_stiffness = 10.0 - self.sim.physics_material.compliant_contact_damping = 1.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_tracking_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_tracking_env_cfg.py deleted file mode 100644 index a1c4b73e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p/v2p_hand_tracking_env_cfg.py +++ /dev/null @@ -1,298 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Hand-only tracking environment configuration (no object).""" - -from __future__ import annotations - -from dataclasses import MISSING - -import isaaclab.envs.mdp as isaac_mdp -import isaaclab.sim as sim_utils -import isaaclab.terrains as terrain_gen -from isaaclab.assets import ArticulationCfg, AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import SceneEntityCfg -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.utils import configclass -from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise - -from robotic_grounding.tasks.v2p import mdp - -################################################# -# Scene definition -################################################# - - -@configclass -class V2PTrackingSceneCfg(InteractiveSceneCfg): - """Configuration for the tracking-only scene with two robots (no object).""" - - terrain = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", terrain_type="plane", debug_vis=False - ) - - # robots - right_robot: ArticulationCfg = MISSING - left_robot: ArticulationCfg = MISSING - - # lights - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - sky_light = AssetBaseCfg( - prim_path="/World/skyLight", - spawn=sim_utils.DomeLightCfg(color=(0.13, 0.13, 0.13), intensity=1000.0), - ) - - -################################################# -# MDP settings -################################################# - - -@configclass -class CommandsCfg: - """Command specifications for the MDP.""" - - dual_hands_tracking_command = mdp.DualHandsTrackingCommandCfg( - debug_vis=False, - motion_speed=0.2, - reset_finger_openness=0.7, - ) - - -@configclass -class ActionsCfg: - """Action specifications for the MDP.""" - - right_joint_direct_action = mdp.JointDirectPositionActionCfg( - asset_name="right_robot", - joint_names=[".*"], - command_name="dual_hands_tracking_command", - tracking_controller_linear_stiffness=50.0, - tracking_controller_linear_damping=10.0, - tracking_controller_angular_stiffness=12.0, - tracking_controller_angular_damping=0.1, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - finger_joint_clip=100.0, - ema_factor=0.9, - ) - - left_joint_direct_action = mdp.JointDirectPositionActionCfg( - asset_name="left_robot", - joint_names=[".*"], - command_name="dual_hands_tracking_command", - tracking_controller_linear_stiffness=50.0, - tracking_controller_linear_damping=10.0, - tracking_controller_angular_stiffness=12.0, - tracking_controller_angular_damping=0.1, - wrist_position_scale=0.05, - wrist_orientation_scale=0.15, - finger_joint_scale=0.15, - finger_joint_clip=100.0, - ema_factor=0.9, - ) - - -@configclass -class ObservationsCfg: - """Observation specifications for the MDP.""" - - @configclass - class PolicyCfg(ObsGroup): - """Observations for policy group. Order preserved.""" - - wrist_position_e = ObsTerm( - func=mdp.wrist_position_e, - params={"command_name": "dual_hands_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_orientation_e = ObsTerm( - func=mdp.wrist_orientation_e, - params={"command_name": "dual_hands_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - finger_joint_pos = ObsTerm( - func=mdp.finger_joint_pos, - params={"command_name": "dual_hands_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - finger_joint_vel = ObsTerm( - func=mdp.finger_joint_vel, - params={"command_name": "dual_hands_tracking_command"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - - command = ObsTerm( - func=isaac_mdp.generated_commands, - params={"command_name": "dual_hands_tracking_command"}, - ) - - actions = ObsTerm(func=isaac_mdp.last_action) - prev_right_actions = ObsTerm( - func=mdp.prev_action, params={"action_name": "right_joint_direct_action"} - ) - prev_left_actions = ObsTerm( - func=mdp.prev_action, params={"action_name": "left_joint_direct_action"} - ) - - def __post_init__(self) -> None: - """Post initialization.""" - self.enable_corruption = False - self.concatenate_terms = True - - # observation groups - policy: PolicyCfg = PolicyCfg() - - -@configclass -class EventCfg: - """Configuration for events.""" - - # startup - right_physics_material = EventTerm( - func=isaac_mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("right_robot", body_names=".*"), - "static_friction_range": (0.99, 1.01), - "dynamic_friction_range": (0.99, 1.01), - "restitution_range": (0.0, 0.0), - "num_buckets": 64, - }, - ) - - left_physics_material = EventTerm( - func=isaac_mdp.randomize_rigid_body_material, - mode="startup", - params={ - "asset_cfg": SceneEntityCfg("left_robot", body_names=".*"), - "static_friction_range": (0.99, 1.01), - "dynamic_friction_range": (0.99, 1.01), - "restitution_range": (0.0, 0.0), - "num_buckets": 64, - }, - ) - - -@configclass -class RewardsCfg: - """Reward terms for the MDP.""" - - action_rate_l2 = RewTerm(func=isaac_mdp.action_rate_l2, weight=-5e-4) - action_l1 = RewTerm( - func=mdp.action_norm, - weight=-5e-3, - params={ - "action_names": [ - "right_joint_direct_action", - "left_joint_direct_action", - ] - }, - ) - - right_joint_limit = RewTerm( - func=isaac_mdp.joint_pos_limits, - weight=-10.0, - params={"asset_cfg": SceneEntityCfg("right_robot", joint_names=[".*"])}, - ) - left_joint_limit = RewTerm( - func=isaac_mdp.joint_pos_limits, - weight=-10.0, - params={"asset_cfg": SceneEntityCfg("left_robot", joint_names=[".*"])}, - ) - - hand_keypoints_tracking_exp = RewTerm( - func=mdp.hand_keypoints_tracking_exp, - weight=1.0, - params={ - "command_name": "dual_hands_tracking_command", - "var": 0.05, - "threshold": 0.02, - }, - ) - - hand_joint_pos_tracking_exp = RewTerm( - func=mdp.hand_joint_pos_tracking_exp, - weight=1.0, - params={ - "command_name": "dual_hands_tracking_command", - "var": 0.05, - }, - ) - - termination_penalty = RewTerm( - func=mdp.termination_penalty, - weight=-300.0, - ) - - -@configclass -class TerminationsCfg: - """Termination terms for the MDP.""" - - time_out = DoneTerm( - func=mdp.timestep_timeout, - time_out=True, - params={ - "command_name": "dual_hands_tracking_command", - }, - ) - - hand_wrist_away_from_trajectory = DoneTerm( - func=mdp.hand_wrist_away_from_trajectory, - params={ - "command_name": "dual_hands_tracking_command", - "threshold": 0.15, - }, - ) - - -################################################# -# Environment configuration -################################################# - - -@configclass -class V2PHandTrackingEnvCfg(ManagerBasedRLEnvCfg): - """Configuration for the hand-only tracking environment (no object).""" - - # Scene settings - scene: V2PTrackingSceneCfg = V2PTrackingSceneCfg(num_envs=4096, env_spacing=1.5) - # Basic settings - observations: ObservationsCfg = ObservationsCfg() - actions: ActionsCfg = ActionsCfg() - commands: CommandsCfg = CommandsCfg() - # MDP settings - rewards: RewardsCfg = RewardsCfg() - terminations: TerminationsCfg = TerminationsCfg() - events: EventCfg = EventCfg() - - def __post_init__(self) -> None: - """Post initialization.""" - # general settings - self.decimation = 4 - self.episode_length_s = 41.4 - # simulation settings - self.sim.dt = 0.005 - self.sim.render_interval = self.decimation - self.sim.physics_material = self.scene.terrain.physics_material - self.sim.physx.gpu_max_rigid_patch_count = 17 * 2**15 - - # Make the environment more compliant - self.sim.physics_material.compliant_contact_stiffness = 10.0 - self.sim.physics_material.compliant_contact_damping = 1.0 - - # viewer settings - self.viewer.eye = (1.5, 1.5, 2.5) - self.viewer.lookat = (0.0, 0.0, 1.5) - self.viewer.origin_type = "env" - self.viewer.env_index = 0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/README.md deleted file mode 100644 index c2d06cf4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/README.md +++ /dev/null @@ -1,65 +0,0 @@ -# V2P Whole Body - -Whole-body humanoid manipulation environments using SONIC controller with RL residuals. The robot tracks reference motions from either third-person video reconstruction (ReconBody) or EE-based motion planning (ReconHand). - -## Environments - -| Gym ID | Config | Description | -|--------|--------|-------------| -| `SonicG1-v0` | `G1SonicEnvCfg` | Base G1 env. JOINT_RESIDUAL action, unified observation space, all reward weights at zero. For custom reward configs via Hydra. | -| `SonicG1-ReconBody-v0` | `G1SonicReconBodyEnvCfg` | Body-accurate reference (MHR/third-person video). Rewards: anchor, joint, object, EE tracking + force closure. Residual scale 0.15. | -| `SonicG1-ReconHand-v0` | `G1SonicReconHandEnvCfg` | Hand-accurate reference (planner pipeline). Rewards: hand keypoints, finger joints, contact tracking. Residual scale 0.5 (from exp201). | - -## Architecture - -``` -V2PEnvCfg (base_env_cfg.py) - scene, commands, events, sim settings - -G1SonicEnvCfg (g1_sonic_env_cfg.py) - robot (G1 + dex hands), SONIC action, unified obs, terminations, - contact sensors, FrameTransformers, action scale - -G1SonicReconBodyEnvCfg G1SonicReconHandEnvCfg - body tracking rewards hand/contact rewards - residual_scale=0.15 residual_scale=0.5 -``` - -## Data Flow - -All reference data loads from a single Hive-partitioned parquet: -``` -whole_body/{dataset}/sequence_id={seq}/robot_name={robot}/data.parquet -``` - -The parquet uses the unified `motion_v1` schema (see `robotic_grounding.motion_schema`): robot root pose, joint positions, EE targets, hand keypoints, finger joints, contacts, object trajectory, and binary contact labels. `SceneConfig.from_motion_file()` auto-discovers objects, support surfaces, and episode length. - -## Key Components - -- **TrackingCommand** (`mdp/commands/`): Central data hub. Loads parquet, provides command targets and sim state. Handles VOC decay, freeze periods, shoulder spread, action history. -- **SONIC Action** (`mdp/actions/`): JOINT_RESIDUAL — SONIC encodes reference trajectory, RL adds residuals after decode. Supports finger residuals. -- **Observations** (`mdp/observations/`): Egocentric body-frame state, 6D rotations throughout, future frame deltas, hand-object transforms, action history. -- **Rewards** (`mdp/rewards/`): Tracking rewards (Gaussian kernel) + contact/wrench rewards. ReconBody uses force closure; ReconHand uses wrench support matching. -- **Events** (`mdp/events/`): Reset to trajectory frame with configurable freeze, shoulder spread, root Z clamp, yaw-only quaternion. -- **Terminations** (`mdp/terminations/`): Freeze-aware for EE/object terms. Anchor terminations always active. - -## PPO Configs - -| Config | init_std | entropy | lr | network | kl | -|--------|----------|---------|-------|---------|-----| -| `G1SonicRslRlPpoCfg` (base) | 0.5 | 5e-4 | 5e-4 | [1024,512,256,128] | 0.02 | -| `G1SonicReconHandRslRlPpoCfg` | 0.5 | 1e-4 | 5e-4 | [1024,512,256,128] | 0.02 | - -## Running - -```bash -# Training -python experiments/run_experiment.py recon_body_apple --local - -# Eval with checkpoint -isaaclab.sh -p scripts/rsl_rl/eval.py \ - --task SonicG1-ReconBody-v0 \ - --motion_file whole_body/soma/sequence_id=apple_pick_optimized/robot_name=g1 \ - --checkpoint /model_N.pt \ - --enable_cameras -``` diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/__init__.py deleted file mode 100644 index 6abe4686..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/__init__.py +++ /dev/null @@ -1,16 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""V2P Whole Body base task configurations.""" - -from .base_env_cfg import ( # noqa: F401 - BaseCommandsCfg, - BaseEventsCfg, - V2PEnvCfg, - V2PSceneCfg, -) -from .config.sonic import ( # noqa: F401 - G1_SONIC_JOINT_NAMES, - G1SonicEnvCfg, - G1SonicReconBodyEnvCfg, - G1SonicReconHandEnvCfg, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/base_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/base_env_cfg.py deleted file mode 100644 index ecc602f9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/base_env_cfg.py +++ /dev/null @@ -1,97 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Base V2P whole-body environment configuration. - -Provides scene, commands, and events. Child configs (G1SonicEnvCfg etc.) -add robot, actions, observations, rewards, and terminations. -""" - -from dataclasses import MISSING - -import isaaclab.sim as sim_utils -import isaaclab.terrains as terrain_gen -from isaaclab.assets import ArticulationCfg, AssetBaseCfg -from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.utils import configclass - -from robotic_grounding.tasks.scene_utils import SceneConfig, apply_scene_config -from robotic_grounding.tasks.v2p_whole_body.mdp.commands import TrackingCommandCfg -from robotic_grounding.tasks.v2p_whole_body.mdp.events import ( - reset_robot_to_trajectory_start, -) - - -@configclass -class V2PSceneCfg(InteractiveSceneCfg): - """Scene configuration for V2P whole-body tasks. Objects are added dynamically via scene config.""" - - terrain = terrain_gen.TerrainImporterCfg( - prim_path="/World/ground", terrain_type="plane", debug_vis=False - ) - - robot: ArticulationCfg = MISSING - - light = AssetBaseCfg( - prim_path="/World/light", - spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), - ) - - sky_light = AssetBaseCfg( - prim_path="/World/skyLight", - spawn=sim_utils.DomeLightCfg(color=(0.13, 0.13, 0.13), intensity=1000.0), - ) - - -@configclass -class BaseEventsCfg: - """Base event configuration.""" - - reset_to_trajectory_frame = EventTerm( - func=reset_robot_to_trajectory_start, - params={"command_name": "motion", "trajectory_time_index": (0, 999999)}, - mode="reset", - ) - - -@configclass -class BaseCommandsCfg: - """Base command configuration.""" - - motion: TrackingCommandCfg = TrackingCommandCfg( - asset_name="robot", - motion_file=MISSING, - anchor_body_name="pelvis", - dt=0.02, - num_future_frames=10, - dt_future_frames=0.1, - debug_vis=True, - ) - - -@configclass -class V2PEnvCfg(ManagerBasedRLEnvCfg): - """Base V2P whole-body environment configuration.""" - - scene: V2PSceneCfg = V2PSceneCfg(num_envs=1, env_spacing=3.0) - events: BaseEventsCfg = BaseEventsCfg() - commands: BaseCommandsCfg = BaseCommandsCfg() - scene_config_path: str | None = None - - def __post_init__(self) -> None: - """Configure simulation defaults and apply scene config.""" - self.decimation = 4 - self.episode_length_s = 5.0 - - self.sim.dt = 0.005 - self.sim.render_interval = self.decimation - self.sim.physics_material = self.scene.terrain.physics_material - - self.viewer.eye = (-2.5, -5.0, 2.0) - self.viewer.lookat = (0.0, 0.0, 0.75) - self.viewer.origin_type = "world" - - if isinstance(self.scene_config_path, str): - scene_config = SceneConfig.from_motion_file(self.scene_config_path) - apply_scene_config(self, scene_config) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/__init__.py deleted file mode 100644 index 8598130d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Controller and robot-specific configurations for v2p_whole_body tasks.""" - -from . import sonic # noqa: F401 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/__init__.py deleted file mode 100644 index 35d5fc4f..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/__init__.py +++ /dev/null @@ -1,11 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Sonic controller configurations.""" - -from . import g1 # noqa: F401 -from .g1 import ( # noqa: F401 - G1_SONIC_JOINT_NAMES, - G1SonicEnvCfg, - G1SonicReconBodyEnvCfg, - G1SonicReconHandEnvCfg, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/__init__.py deleted file mode 100644 index 485d4eda..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/__init__.py +++ /dev/null @@ -1,41 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import gymnasium as gym - -from . import agents # noqa: F401 -from .g1_sonic_env_cfg import ( # noqa: F401 - G1_SONIC_JOINT_NAMES, - G1SonicEnvCfg, - G1SonicReconBodyEnvCfg, - G1SonicReconHandEnvCfg, -) - -gym.register( - id="SonicG1-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.g1_sonic_env_cfg:G1SonicEnvCfg", - "rsl_rl_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.agents.rsl_rl_ppo_cfg:G1SonicRslRlPpoCfg", - }, -) - -gym.register( - id="SonicG1-ReconBody-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.g1_sonic_env_cfg:G1SonicReconBodyEnvCfg", - "rsl_rl_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.agents.rsl_rl_ppo_cfg:G1SonicRslRlPpoCfg", - }, -) - -gym.register( - id="SonicG1-ReconHand-v0", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.g1_sonic_env_cfg:G1SonicReconHandEnvCfg", - "rsl_rl_cfg_entry_point": "robotic_grounding.tasks.v2p_whole_body.config.sonic.g1.agents.rsl_rl_ppo_cfg:G1SonicReconHandRslRlPpoCfg", - }, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/__init__.py deleted file mode 100644 index a9503eb0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from .rsl_rl_ppo_cfg import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/rsl_rl_ppo_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/rsl_rl_ppo_cfg.py deleted file mode 100644 index 47ee418b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/agents/rsl_rl_ppo_cfg.py +++ /dev/null @@ -1,69 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import ( - RslRlOnPolicyRunnerCfg, - RslRlPpoActorCriticCfg, - RslRlPpoAlgorithmCfg, -) - - -@configclass -class G1SonicRslRlPpoCfg(RslRlOnPolicyRunnerCfg): - """Base PPO config for G1 SONIC environments.""" - - num_steps_per_env = 24 - max_iterations = 100_000 - save_interval = 500 - experiment_name = "g1_sonic" - empirical_normalization = True - - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.5, - actor_hidden_dims=[1024, 512, 256, 128], - critic_hidden_dims=[1024, 512, 256, 128], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.0005, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=5.0e-4, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.02, - max_grad_norm=0.1, - ) - - -@configclass -class G1SonicReconHandRslRlPpoCfg(G1SonicRslRlPpoCfg): - """PPO config for ReconHand (planner reference). From exp201.""" - - experiment_name = "g1_sonic_recon_hand" - max_iterations = 5_000 - - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.5, - actor_hidden_dims=[1024, 512, 256, 128], - critic_hidden_dims=[1024, 512, 256, 128], - activation="elu", - ) - algorithm = RslRlPpoAlgorithmCfg( - value_loss_coef=1.0, - use_clipped_value_loss=True, - clip_param=0.2, - entropy_coef=0.0001, - num_learning_epochs=5, - num_mini_batches=4, - learning_rate=5.0e-4, - schedule="adaptive", - gamma=0.99, - lam=0.95, - desired_kl=0.02, - max_grad_norm=0.1, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/g1_sonic_env_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/g1_sonic_env_cfg.py deleted file mode 100644 index 27ac6551..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/config/sonic/g1/g1_sonic_env_cfg.py +++ /dev/null @@ -1,765 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import isaaclab.envs.mdp as il_mdp -from isaaclab.envs.mdp import observations -from isaaclab.managers import CurriculumTermCfg as CurrTerm -from isaaclab.managers import EventTermCfg as EventTerm -from isaaclab.managers import ObservationGroupCfg as ObsGroup -from isaaclab.managers import ObservationTermCfg as ObsTerm -from isaaclab.managers import RewardTermCfg as RewTerm -from isaaclab.managers import SceneEntityCfg -from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.utils import configclass -from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise - -from robotic_grounding.assets import POLICY_ASSET_DIR -from robotic_grounding.assets.g1 import ( - G1_CYLINDER_MODEL_12_HANDS_DEX_DELAYED_CFG, - G1_DEX_CONTACT_BODIES, - G1_HAND_JOINT_NAMES, - G1_MODEL_12_ACTION_SCALE, -) -from robotic_grounding.tasks.v2p.mdp.events import configure_collision_groups -from robotic_grounding.tasks.v2p_whole_body.base_env_cfg import BaseEventsCfg, V2PEnvCfg -from robotic_grounding.tasks.v2p_whole_body.mdp import observations as obs -from robotic_grounding.tasks.v2p_whole_body.mdp.actions import ( - SONICActionCfg, - SONICActionType, -) -from robotic_grounding.tasks.v2p_whole_body.mdp.curriculum import ( - WholeBodyFixedTimestepVOCCurriculum, -) -from robotic_grounding.tasks.v2p_whole_body.mdp.rewards import ( - contact_rewards, - tracking_rewards, -) -from robotic_grounding.tasks.v2p_whole_body.mdp.terminations import ( - anchor_pos_error, - anchor_quat_error, - ee_position_error, - ee_quat_error, - hand_wrist_away_from_trajectory, - object_pos_error, - object_quat_error, - timestep_termination, -) - -POLICY_DIR = f"{POLICY_ASSET_DIR}/sonic" - -# G1 joints that SONIC controls -G1_SONIC_JOINT_NAMES = [ - "left_hip_pitch_joint", - "left_hip_roll_joint", - "left_hip_yaw_joint", - "left_knee_joint", - "left_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_hip_pitch_joint", - "right_hip_roll_joint", - "right_hip_yaw_joint", - "right_knee_joint", - "right_ankle_pitch_joint", - "right_ankle_roll_joint", - "waist_yaw_joint", - "waist_roll_joint", - "waist_pitch_joint", - "left_shoulder_pitch_joint", - "left_shoulder_roll_joint", - "left_shoulder_yaw_joint", - "left_elbow_joint", - "left_wrist_roll_joint", - "left_wrist_pitch_joint", - "left_wrist_yaw_joint", - "right_shoulder_pitch_joint", - "right_shoulder_roll_joint", - "right_shoulder_yaw_joint", - "right_elbow_joint", - "right_wrist_roll_joint", - "right_wrist_pitch_joint", - "right_wrist_yaw_joint", -] - - -# --------------------------------------------------------------------------- -# Observation groups -# --------------------------------------------------------------------------- - - -@configclass -class G1SONICEncoderCfg(ObsGroup): - """SONIC tokenizer observations (29 body joints only).""" - - encoder_index = ObsTerm(func=obs.encoder_mode, params={"command_name": "motion"}) - command_joint_pos_multi_future = ObsTerm( - func=obs.command_joint_pos, - params={ - "command_name": "motion", - "sonic_joints_only": True, - "action_name": "joint_pos", - }, - ) - command_joint_vel_multi_future = ObsTerm( - func=obs.command_joint_vel, - params={ - "command_name": "motion", - "sonic_joints_only": True, - "action_name": "joint_pos", - }, - ) - padding_1 = ObsTerm(func=obs.encoder_padding, params={"dim": 17}) - motion_anchor_ori_b = ObsTerm( - func=obs.motion_anchor_ori_b, params={"command_name": "motion"} - ) - padding_2 = ObsTerm(func=obs.encoder_padding, params={"dim": 1762 - 17 - 644}) - concatenate_terms = True - - -@configclass -class G1SONICDecoderCfg(ObsGroup): - """SONIC decoder observations (29 body joints only, history_length=10).""" - - base_ang_vel = ObsTerm( - func=observations.base_ang_vel, params={"asset_cfg": SceneEntityCfg("robot")} - ) - joint_pos = ObsTerm( - func=obs.joint_pos_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": True, - "action_name": "joint_pos", - }, - ) - joint_vel = ObsTerm( - func=obs.joint_vel_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": True, - "action_name": "joint_pos", - }, - ) - actions = ObsTerm( - func=obs.last_action, - params={"action_name": "joint_pos", "sonic_joints_only": True}, - ) - gravity_dir = ObsTerm( - func=observations.projected_gravity, - params={"asset_cfg": SceneEntityCfg("robot")}, - ) - concatenate_terms = True - history_length = 10 - - -@configclass -class G1HandPolicyCfg(ObsGroup): - """Hand-object transform observations.""" - - left_hand_object_transform = ObsTerm( - func=obs.hand_object_transform, - params={ - "frame_transform_cfg": SceneEntityCfg("left_hand_object_transform"), - "threshold": 10.0, - }, - ) - right_hand_object_transform = ObsTerm( - func=obs.hand_object_transform, - params={ - "frame_transform_cfg": SceneEntityCfg("right_hand_object_transform"), - "threshold": 10.0, - }, - ) - - -@configclass -class G1PolicyCfg(ObsGroup): - """Unified policy observations for whole-body tracking. - - Egocentric (body frame) for hand/object state, 6D rotation throughout, - and legacy absolute anchor positions for checkpoint compatibility. - """ - - wrist_position_b = ObsTerm( - func=obs.wrist_position_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_orientation_b = ObsTerm( - func=obs.wrist_orientation_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_velocity_b = ObsTerm( - func=obs.wrist_velocity_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_position_b = ObsTerm( - func=obs.object_position_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_orientation_b = ObsTerm( - func=obs.object_orientation_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - joint_pos_rel = ObsTerm( - func=obs.joint_pos_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": False, - "action_name": "joint_pos", - }, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - joint_vel_rel = ObsTerm( - func=obs.joint_vel_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": False, - "action_name": "joint_pos", - }, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - motion_anchor_pos_b = ObsTerm( - func=obs.motion_anchor_pos_b, - params={ - "command_name": "motion", - "num_future_frames": 3, - "frame": "absolute", - }, - ) - motion_anchor_ori_b = ObsTerm( - func=obs.motion_anchor_ori_b, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_joint_pos_delta = ObsTerm( - func=obs.motion_joint_pos_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_ee_pos_delta = ObsTerm( - func=obs.motion_ee_pos_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_ee_quat_delta = ObsTerm( - func=obs.motion_ee_quat_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - left_hand_object_transform = ObsTerm( - func=obs.hand_object_transform_6d, - params={ - "frame_transform_cfg": SceneEntityCfg("left_hand_object_transform"), - "threshold": 10.0, - }, - ) - right_hand_object_transform = ObsTerm( - func=obs.hand_object_transform_6d, - params={ - "frame_transform_cfg": SceneEntityCfg("right_hand_object_transform"), - "threshold": 10.0, - }, - ) - object_pose_delta = ObsTerm(func=obs.object_pose_delta_6d) - trajectory_progress = ObsTerm(func=obs.command_trajectory_progress) - action_history = ObsTerm(func=obs.action_history, params={"command_name": "motion"}) - concatenate_terms = True - - -@configclass -class G1SonicObservationsCfg: - """Complete observation config with all groups.""" - - policy: G1PolicyCfg = G1PolicyCfg() - sonic_tokenizer: G1SONICEncoderCfg = G1SONICEncoderCfg() - sonic_policy: G1SONICDecoderCfg = G1SONICDecoderCfg() - hand_policy: G1HandPolicyCfg = G1HandPolicyCfg() - - -# --------------------------------------------------------------------------- -# ReconHand observations -# --------------------------------------------------------------------------- - - -@configclass -class G1ReconHandPolicyCfg(ObsGroup): - """Policy (actor) observations for the hand-recon whole-body task. - - Shape: 385 for single-body objects, plus 14 dims per additional object body. - """ - - wrist_velocity_b = ObsTerm( - func=obs.wrist_velocity_full_b, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - joint_pos_rel = ObsTerm( - func=obs.joint_pos_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": False, - "action_name": "joint_pos", - }, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - joint_vel_rel = ObsTerm( - func=obs.joint_vel_rel, - params={ - "asset_cfg": SceneEntityCfg("robot"), - "sonic_joints_only": False, - "action_name": "joint_pos", - }, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - motion_anchor_pos_b = ObsTerm( - func=obs.motion_anchor_pos_b, - params={ - "command_name": "motion", - "num_future_frames": 3, - "frame": "relative", - }, - ) - motion_anchor_ori_b = ObsTerm( - func=obs.motion_anchor_ori_b, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_joint_pos_delta = ObsTerm( - func=obs.motion_joint_pos_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_ee_pos_delta = ObsTerm( - func=obs.motion_ee_pos_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - motion_ee_quat_delta = ObsTerm( - func=obs.motion_ee_quat_delta, - params={"command_name": "motion", "num_future_frames": 3}, - ) - left_hand_object_transform = ObsTerm( - func=obs.hand_object_reference_transform, - params={ - "side": "left", - "command_name": "motion", - "threshold": 10.0, - }, - ) - right_hand_object_transform = ObsTerm( - func=obs.hand_object_reference_transform, - params={ - "side": "right", - "command_name": "motion", - "threshold": 10.0, - }, - ) - object_pose_delta = ObsTerm( - func=obs.object_pose_delta, - params={"command_name": "motion"}, - ) - trajectory_progress = ObsTerm(func=obs.command_trajectory_progress) - base_ang_vel = ObsTerm( - func=observations.base_ang_vel, - params={"asset_cfg": SceneEntityCfg("robot")}, - ) - actions = ObsTerm( - func=obs.last_action, - params={"action_name": "joint_pos", "sonic_joints_only": False}, - ) - wrist_position_e = ObsTerm( - func=obs.wrist_position_e, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - wrist_wxyz_e = ObsTerm( - func=obs.wrist_wxyz_e, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_position_e = ObsTerm( - func=obs.object_position_e, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - object_wxyz_e = ObsTerm( - func=obs.object_wxyz_e, - params={"command_name": "motion"}, - noise=Unoise(n_min=-0.01, n_max=0.01), - ) - concatenate_terms = True - - -@configclass -class G1SonicReconHandObservationsCfg: - """Observation config for the hand-recon whole-body task.""" - - policy: G1ReconHandPolicyCfg = G1ReconHandPolicyCfg() - sonic_tokenizer: G1SONICEncoderCfg = G1SONICEncoderCfg() - sonic_policy: G1SONICDecoderCfg = G1SONICDecoderCfg() - - -# --------------------------------------------------------------------------- -# Actions -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicActionsCfg: - """JOINT_RESIDUAL: SONIC encodes reference, RL adds residuals after decode.""" - - joint_pos = SONICActionCfg( - action_type=SONICActionType.JOINT_RESIDUAL, - policy_dir=POLICY_DIR, - asset_name="robot", - joint_names=[".*"], - sonic_joint_names=G1_SONIC_JOINT_NAMES, - command_name="motion", - use_default_offset=True, - residual_scale=0.5, - use_tanh=False, - finger_residual=True, - finger_residual_scale=0.15, - ) - - -# --------------------------------------------------------------------------- -# Rewards -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicRewardsCfg: - """Base reward config — termination penalty and regularization only.""" - - termination_penalty = RewTerm(func=il_mdp.is_terminated, weight=-300.0) - action_rate = RewTerm(func=il_mdp.action_rate_l2, weight=-1e-6) - action_l2 = RewTerm(func=il_mdp.action_l2, weight=-1e-6) - joint_pos_limit = RewTerm( - func=il_mdp.joint_pos_limits, - weight=-0.001, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, - ) - - -# --------------------------------------------------------------------------- -# Terminations -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicTerminationsCfg: - """Shared termination config.""" - - timeout = DoneTerm( - func=timestep_termination, - params={"command_name": "motion"}, - time_out=True, - ) - anchor_pos_error = DoneTerm( - func=anchor_pos_error, - params={"command_name": "motion", "threshold": 0.70}, - ) - anchor_quat_error = DoneTerm( - func=anchor_quat_error, - params={"command_name": "motion", "threshold": 1.50}, - ) - ee_pos_error = DoneTerm( - func=ee_position_error, - params={"command_name": "motion", "threshold": 0.15}, - ) - ee_quat_error = DoneTerm( - func=ee_quat_error, - params={"command_name": "motion", "threshold": 1.50}, - ) - object_pos_error = DoneTerm( - func=object_pos_error, - params={"command_name": "motion", "threshold": 0.10}, - ) - object_quat_error = DoneTerm( - func=object_quat_error, - params={"command_name": "motion", "threshold": 1.50}, - ) - - -# --------------------------------------------------------------------------- -# Base G1 SONIC env -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicEnvCfg(V2PEnvCfg): - """Base G1 whole-body env with SONIC JOINT_RESIDUAL action and unified observations.""" - - actions: G1SonicActionsCfg = G1SonicActionsCfg() - observations: G1SonicObservationsCfg = G1SonicObservationsCfg() - rewards: G1SonicRewardsCfg = G1SonicRewardsCfg() - terminations: G1SonicTerminationsCfg = G1SonicTerminationsCfg() - - def __post_init__(self) -> None: - """Configure G1 robot, action scale, and hand sensor bodies.""" - self.scene.robot = G1_CYLINDER_MODEL_12_HANDS_DEX_DELAYED_CFG.replace( - prim_path="{ENV_REGEX_NS}/Robot", - ) - self.actions.joint_pos.scale = G1_MODEL_12_ACTION_SCALE - self.commands.motion.hand_contact_bodies = list(G1_DEX_CONTACT_BODIES) - self.commands.motion.hand_frame_target_bodies = [ - "left_hand_palm_link", - "right_hand_palm_link", - ] - super().__post_init__() - - -# --------------------------------------------------------------------------- -# ReconBody: body-accurate reference (MHR) -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicReconBodyRewardsCfg(G1SonicRewardsCfg): - """Rewards for body-accurate references. Emphasizes body/joint/object tracking.""" - - motion_anchor_position_error_exp = RewTerm( - func=tracking_rewards.motion_global_anchor_position_error_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.3}, - ) - motion_anchor_orientation_error_exp = RewTerm( - func=tracking_rewards.motion_global_anchor_orientation_error_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.4}, - ) - motion_joint_pos_error_exp = RewTerm( - func=tracking_rewards.motion_joint_pos_error_exp, - weight=5.0, - params={ - "command_name": "motion", - "std": 1.0, - "joint_names": G1_SONIC_JOINT_NAMES, - }, - ) - motion_object_position_error_exp = RewTerm( - func=tracking_rewards.motion_object_position_error_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.2}, - ) - motion_progress = RewTerm( - func=tracking_rewards.motion_progress, - weight=1.0, - params={"command_name": "motion"}, - ) - motion_ee_position_error_exp = RewTerm( - func=tracking_rewards.motion_ee_position_error_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.2}, - ) - motion_ee_orientation_error_exp = RewTerm( - func=tracking_rewards.motion_ee_orientation_error_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.4}, - ) - force_closure = RewTerm( - func=contact_rewards.force_closure_reward, - weight=5.0, - params={"command_name": "motion", "min_support": 0.01}, - ) - action_rate = RewTerm(func=il_mdp.action_rate_l2, weight=-0.0001) - - -@configclass -class G1SonicReconBodyVOCCurriculumCfg: - """Fixed-timestep curriculum that decays the VOC target scale over PPO updates. - - The schedule is expressed in PPO update indices and converted to env steps - via ``num_steps_per_env``. Reward weights are intentionally not scheduled - here; see ``WholeBodyFixedTimestepVOCCurriculum`` for the rationale. - """ - - voc_curriculum = CurrTerm( - func=WholeBodyFixedTimestepVOCCurriculum, - params={ - "command_name": "motion", - # Match num_steps_per_env in agents/rsl_rl_ppo_cfg.py::G1SonicRslRlPpoCfg. - "num_steps_per_env": 24, - # PPO update indices (not seconds, not raw env steps). - "timestep_schedule": [0, 2000, 4000, 6000, 8000, 10000, 12000], - # VOC target scale per stage; final stage drives VOC fully off. - "virtual_object_control_scale_factor": [ - 1.0, - 0.75, - 0.5, - 0.25, - 0.1, - 0.05, - 0.0, - ], - }, - ) - - -@configclass -class G1SonicReconBodyEnvCfg(G1SonicEnvCfg): - """Body-accurate reference env (MHR pipeline).""" - - rewards: G1SonicReconBodyRewardsCfg = G1SonicReconBodyRewardsCfg() - curriculum: G1SonicReconBodyVOCCurriculumCfg = G1SonicReconBodyVOCCurriculumCfg() - - def __post_init__(self) -> None: - """Set residual scales for body-accurate tracking.""" - super().__post_init__() - self.actions.joint_pos.residual_scale = 0.15 - self.actions.joint_pos.finger_residual_scale = 0.15 - - -# --------------------------------------------------------------------------- -# ReconHand: hand-accurate reference (planner) -# --------------------------------------------------------------------------- - - -@configclass -class G1SonicReconHandRewardsCfg(G1SonicRewardsCfg): - """Rewards for the hand-recon whole-body task.""" - - termination_penalty = RewTerm(func=il_mdp.is_terminated, weight=-100.0) - motion_hand_keypoints_gaussian_exp = RewTerm( - func=tracking_rewards.motion_hand_keypoints_gaussian_exp, - weight=1.0, - params={"command_name": "motion", "std": 0.1}, - ) - motion_finger_joint_pos_gaussian_exp = RewTerm( - func=tracking_rewards.motion_finger_joint_pos_gaussian_exp, - weight=1.0, - params={"command_name": "motion", "std": 1.0}, - ) - motion_object_keypoints_tracking_exp = RewTerm( - func=tracking_rewards.motion_object_keypoints_tracking_exp, - weight=0.0, - params={"command_name": "motion", "var": 0.1}, - ) - motion_contact_tracking_gaussian_exp = RewTerm( - func=tracking_rewards.motion_contact_tracking_gaussian_exp, - weight=0.0, - params={"command_name": "motion", "std": 0.05}, - ) - contact_wrench_support_reward = RewTerm( - func=contact_rewards.contact_wrench_support_reward, - weight=0.0, - params={"command_name": "motion", "tolerance": 0.1, "var": 0.1}, - ) - unintended_contact_penalty = RewTerm( - func=contact_rewards.unintended_contact_penalty, - weight=0.0, - params={"command_name": "motion"}, - ) - missed_contact_penalty = RewTerm( - func=contact_rewards.missed_contact_penalty, - weight=0.0, - params={"command_name": "motion"}, - ) - action_rate = RewTerm(func=il_mdp.action_rate_l2, weight=-0.001) - action_l2 = RewTerm(func=il_mdp.action_l2, weight=-0.0001) - joint_pos_limit = RewTerm( - func=il_mdp.joint_pos_limits, - weight=0.0, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*"])}, - ) - - -@configclass -class G1SonicReconHandCurriculumCfg: - """ReconHand curriculum hooks. - - Defaults to no-op. Experiment configs can enable fixed VOC schedules by - overriding `timestep_schedule` and `virtual_object_control_scale_factor`. - """ - - voc_curriculum = CurrTerm( - func=WholeBodyFixedTimestepVOCCurriculum, - params={ - "command_name": "motion", - "num_steps_per_env": 24, - # Single disabled stage (VOC target 0.0). Experiments override these two - # equal-length lists to enable a PPO-update VOC decay schedule. - "timestep_schedule": [0], - "virtual_object_control_scale_factor": [0.0], - }, - ) - - -@configclass -class G1SonicReconHandTerminationsCfg: - """Termination config for the hand-recon task. - - Drops EE pos/quat terminations (redundant: the reference EE depends on the - current object pose, covered by hand_wrist_away_from_trajectory) and adds the - hand-away termination. Object terminations are effectively disabled. - """ - - timeout = DoneTerm( - func=timestep_termination, - time_out=True, - params={"command_name": "motion"}, - ) - anchor_pos_error = DoneTerm( - func=anchor_pos_error, - params={"command_name": "motion", "threshold": 0.70}, - ) - anchor_quat_error = DoneTerm( - func=anchor_quat_error, - params={"command_name": "motion", "threshold": 1.50}, - ) - hand_wrist_away = DoneTerm( - func=hand_wrist_away_from_trajectory, - params={"command_name": "motion", "threshold": 0.15}, - ) - object_pos_error = DoneTerm( - func=object_pos_error, - params={"command_name": "motion", "threshold": 100.0}, - ) - object_quat_error = DoneTerm( - func=object_quat_error, - params={"command_name": "motion", "threshold": 100.0}, - ) - - -@configclass -class G1SonicReconHandEventsCfg(BaseEventsCfg): - """Events for hand-recon scene collision grouping.""" - - setup_collision_groups = EventTerm( - func=configure_collision_groups, - mode="prestartup", - params={ - "robot_names": ["Robot"], - "object_names": [], - "fixed_object_names": [], - "disable_robot_to_object_collisions": False, - "disable_robot_to_fixed_object_collisions": False, - }, - ) - - -@configclass -class G1SonicReconHandEnvCfg(G1SonicEnvCfg): - """Hand-accurate reference env (planner pipeline).""" - - # Hand-recon actor observations; overrides the inherited generic obs config. - events: G1SonicReconHandEventsCfg = G1SonicReconHandEventsCfg() # type: ignore[assignment] - observations: G1SonicReconHandObservationsCfg = G1SonicReconHandObservationsCfg() # type: ignore[assignment] - rewards: G1SonicReconHandRewardsCfg = G1SonicReconHandRewardsCfg() - terminations: G1SonicReconHandTerminationsCfg = G1SonicReconHandTerminationsCfg() # type: ignore[assignment] - curriculum: G1SonicReconHandCurriculumCfg = G1SonicReconHandCurriculumCfg() # type: ignore[assignment] - - def __post_init__(self) -> None: - """Configure Dex3 hand tracking: EE links, fingertips, VOC, freeze.""" - super().__post_init__() - self.scene.replicate_physics = False - self.scene.filter_collisions = False - self.commands.motion.ee_link_names = [ - "left_hand_palm_link", - "right_hand_palm_link", - ] - self.commands.motion.fingertip_body_name = ".*_(thumb_2|index_1|middle_1)_link" - self.commands.motion.finger_joint_names = G1_HAND_JOINT_NAMES - self.commands.motion.reset_freeze_steps = 50 - self.commands.motion.initial_virtual_object_control_curriculum_scale = 1.0 - self.commands.motion.reset_shoulder_spread = 0.5 - self.commands.motion.voc_decay_steps = 10 - self.commands.motion.voc_reset_scale = 1.0 - # Upper bound; apply_scene_config() clips this down to the trajectory length. - self.episode_length_s = 70.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/__init__.py deleted file mode 100644 index 0c6c4ed6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""MDP components for v2p_whole_body tasks.""" - -from . import ( # noqa: F401 - actions, - commands, - curriculum, - events, - observations, - rewards, - terminations, -) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/README.md deleted file mode 100644 index ba30f409..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/README.md +++ /dev/null @@ -1,45 +0,0 @@ -# Actions - -## Action Types - -| Type | Class | File | Description | -|------|-------|------|-------------| -| `HIERARCHICAL` | `SONICHierarchicalAction` | `sonic_hierarchical_action.py` | RL outputs joint commands + base orientation fed INTO SONIC encoder. | -| `HIERARCHICAL_RESIDUAL` | `SONICHierarchicalResidualAction` | `sonic_hierarchical_residual_action.py` | RL residuals added to commanded joints BEFORE SONIC. | -| `JOINT_RESIDUAL` | `SONICJointResidualAction` | `sonic_joint_residual_action.py` | RL residuals added AFTER SONIC output. Default for whole-body envs. | -| `LATENT_RESIDUAL` | `SONICLatentResidualAction` | `sonic_latent_residual_action.py` | RL residuals in SONIC latent (token) space. | -| `LATENT` | `SONICLatentAction` | `sonic_latent_action.py` | RL directly outputs full latent state for decoder. No encoder pass. | -| `LATENT_HAND_POLICY` | `SONICLatentHandPolicyAction` | `sonic_latent_hand_policy_action.py` | Latent body + pretrained hand policy for fingers. | - -## Configuration - -`SONICActionCfg` (`sonic_action_cfg.py`): - -| Field | Type | Default | Description | -|-------|------|---------|-------------| -| `action_type` | `SONICActionType` | `HIERARCHICAL` | Selects action class | -| `policy_dir` | str | required | Path to SONIC ONNX models (`encoder_batched.onnx`, `decoder_batched.onnx`) | -| `sonic_joint_names` | list[str] | required | Joints controlled by SONIC (29 for G1) | -| `command_name` | str | `"motion"` | Tracking command term name | -| `use_default_offset` | bool | True | Add default joint positions to output | -| `scale` | dict | — | Per-joint action scale (PD gains) | -| `residual_scale` | float | 0.1 | Scale on RL residuals (`JOINT_RESIDUAL`) | -| `finger_residual` | bool | False | RL also outputs finger residuals (`JOINT_RESIDUAL`) | -| `finger_residual_scale` | float | -1.0 | Separate finger scale (-1 = use `residual_scale`) | -| `use_tanh` | bool | True | Tanh squashing on residuals | -| `residual_joint_names` | list[str] \| None | None | Restrict residuals to subset of SONIC joints | -| `hand_policy_class` | type \| None | None | Pretrained hand policy (`LATENT_HAND_POLICY`) | -| `hand_policy_cfg` | object | None | Hand policy config (`LATENT_HAND_POLICY`) | - -## Base Class - -`SONICActionBase` (`sonic_actions.py`): -- Loads `encoder_batched.onnx` + `decoder_batched.onnx` via `SonicPolicy` -- Splits joints into SONIC-controlled vs direct (fingers) -- Builds observation dicts for tokenizer and decoder from command term -- Handles default joint position offsets -- Calls `command.update_action_history(processed_actions)` each step - -Key methods: -- `get_sonic_joint_indices()` / `get_sonic_joint_ids()` — joint index lookups -- `get_last_sonic_actions()` — for decoder observation (last SONIC output) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/__init__.py deleted file mode 100644 index 1e9fc8fc..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Actions for robotic grounding environment.""" - -from .sonic_action_cfg import * # noqa: F403 -from .sonic_actions import * # noqa: F403 -from .sonic_hierarchical_action import * # noqa: F403 -from .sonic_hierarchical_residual_action import * # noqa: F403 -from .sonic_joint_residual_action import * # noqa: F403 -from .sonic_latent_action import * # noqa: F403 -from .sonic_latent_hand_policy_action import * # noqa: F403 -from .sonic_latent_residual_action import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_action_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_action_cfg.py deleted file mode 100644 index 34a7ad8b..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_action_cfg.py +++ /dev/null @@ -1,84 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# ruff: noqa: PLC0415 — lazy imports in __post_init__ for circular dep avoidance -from dataclasses import MISSING -from enum import Enum - -from isaaclab.managers.action_manager import ActionTermCfg -from isaaclab.utils import configclass - - -class SONICActionType(Enum): - """Types of SONIC action terms.""" - - HIERARCHICAL = "hierarchical" - HIERARCHICAL_RESIDUAL = "hierarchical_residual" # residuals BEFORE SONIC - JOINT_RESIDUAL = "joint_residual" # residuals AFTER SONIC - LATENT_RESIDUAL = "latent_residual" - LATENT = "latent" - LATENT_HAND_POLICY = "latent_hand_policy" - - -@configclass -class SONICActionCfg(ActionTermCfg): - """Common configuration for all SONIC action terms.""" - - action_type: SONICActionType = SONICActionType.HIERARCHICAL - - policy_dir: str = MISSING # type: ignore[assignment] - """Path to directory containing SONIC ONNX models.""" - - asset_name: str = "robot" - joint_names: list[str] = [".*"] - sonic_joint_names: list[str] = MISSING # type: ignore[assignment] - command_name: str = "motion" - use_default_offset: bool = True - scale: float | dict[str, float] = 1.0 - - # Hand policy (for LATENT_HAND_POLICY) - hand_policy_class: type | None = None - hand_policy_cfg: object = None - - # Joint residual params (for JOINT_RESIDUAL) - residual_scale: float = 0.1 - """Scale applied to RL residuals before adding to SONIC output.""" - - residual_joint_names: list[str] | None = None - """If set, RL only outputs residuals for these joints (subset of sonic joints).""" - - finger_residual: bool = False - """If True, RL also outputs residuals for non-SONIC (finger) joints.""" - - finger_residual_scale: float = -1.0 - """Scale for finger residuals. -1.0 means use residual_scale.""" - - use_tanh: bool = True - """Whether to apply tanh squashing to residuals.""" - - debug: bool = False - - def __post_init__(self) -> None: - """Dispatch class_type from action_type enum.""" - from .sonic_hierarchical_action import SONICHierachicalAction - from .sonic_hierarchical_residual_action import ( - SONICHierarchicalResidualAction, - ) - from .sonic_joint_residual_action import SONICJointResidualAction - from .sonic_latent_action import SONICLatentAction - from .sonic_latent_hand_policy_action import SONICLatentHandPolicyAction - from .sonic_latent_residual_action import SONICLatentResidualAction - - _dispatch = { - SONICActionType.HIERARCHICAL: SONICHierachicalAction, - SONICActionType.HIERARCHICAL_RESIDUAL: SONICHierarchicalResidualAction, - SONICActionType.JOINT_RESIDUAL: SONICJointResidualAction, - SONICActionType.LATENT_RESIDUAL: SONICLatentResidualAction, - SONICActionType.LATENT: SONICLatentAction, - SONICActionType.LATENT_HAND_POLICY: SONICLatentHandPolicyAction, - } - - if self.action_type not in _dispatch: - raise ValueError(f"Unknown SONIC action type: {self.action_type}") - self.class_type = _dispatch[self.action_type] - - super().__post_init__() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_actions.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_actions.py deleted file mode 100644 index 287145e0..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_actions.py +++ /dev/null @@ -1,306 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from pathlib import Path -from typing import Any - -import isaaclab.utils.string as string_utils -import numpy as np -import onnxruntime as ort -import torch -from isaaclab.assets import Articulation -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg - - -class SonicPolicy: - """Class to load and inference SONIC ONNX policy checkpoints.""" - - def __init__( - self, - policy_dir: str, - num_fsq_levels: int = 64, - fsq_level_list: int | list[int] = 32, - ) -> None: - """Load encoder and decoder ONNX sessions from policy_dir.""" - self.policy_dir = policy_dir - encoder_path = Path(policy_dir) / "encoder_batched.onnx" - decoder_path = Path(policy_dir) / "decoder_batched.onnx" - - if torch.cuda.is_available(): - providers = ["CUDAExecutionProvider", "CPUExecutionProvider"] - provider_options = [{"device_id": 0}, {}] - else: - providers = ["CPUExecutionProvider"] - provider_options = [{}] - - self.encoder = ort.InferenceSession( - str(encoder_path), providers=providers, provider_options=provider_options - ) - self.decoder = ort.InferenceSession( - str(decoder_path), providers=providers, provider_options=provider_options - ) - - self.encoder_output_dim = self.encoder.get_outputs()[0].shape[1] - self.decoder_output_dim = self.decoder.get_outputs()[0].shape[1] - - # fsq - levels: list[int] = ( - [fsq_level_list] * num_fsq_levels - if isinstance(fsq_level_list, int) - else fsq_level_list - ) - self.fsq_level_list = torch.tensor( - levels, dtype=torch.int32, device=torch.device("cuda") - ) - - def _onnx_inference_gpu( - self, - model: ort.InferenceSession, - input_tensor: torch.Tensor, - output_shape: tuple, - input_name: str = "obs_dict", - ) -> torch.Tensor: - """Run ONNX inference on GPU using zero-copy io_binding.""" - input_tensor = input_tensor.contiguous().to(dtype=torch.float32) - output_tensor = torch.empty( - output_shape, dtype=torch.float32, device=input_tensor.device - ) - - io_binding = model.io_binding() - io_binding.bind_input( - name=input_name, - device_type="cuda", - device_id=0, - element_type=np.float32, - shape=tuple(input_tensor.shape), - buffer_ptr=input_tensor.data_ptr(), - ) - - output_name = model.get_outputs()[0].name - io_binding.bind_output( - name=output_name, - device_type="cuda", - device_id=0, - element_type=np.float32, - shape=tuple(output_tensor.shape), - buffer_ptr=output_tensor.data_ptr(), - ) - - model.run_with_iobinding(io_binding) - return output_tensor - - def __call__(self, obs: dict) -> torch.Tensor: - """Run encoder + decoder inference.""" - encoder_obs = obs["sonic_tokenizer"] - decoder_obs = obs["sonic_policy"] - - batch_size = encoder_obs.shape[0] - token_state = self._onnx_inference_gpu( - self.encoder, - encoder_obs, - output_shape=(batch_size, self.encoder_output_dim), - ) - - decoder_input = torch.cat([token_state, decoder_obs], dim=1) - actions = self._onnx_inference_gpu( - self.decoder, - decoder_input, - output_shape=(batch_size, self.decoder_output_dim), - ) - - return actions - - def _round_ste(self, x: torch.Tensor) -> torch.Tensor: - """Round such that gradient can be backpropagated through the quantization.""" - return x + (torch.round(x) - x).detach() - - def quantize(self, token_state: torch.Tensor, eps: float = 1e-3) -> torch.Tensor: - """Quantize token state to finite scalar quantizer range. - - See https://arxiv.org/pdf/2309.15505. - """ - half_l = (self.fsq_level_list - 1) * (1 + eps) / 2 - offset = torch.where(self.fsq_level_list % 2 == 0, 0.5, 0.0) - shift = torch.atanh(offset / half_l) - bounded_z = torch.tanh(token_state + shift) * half_l - offset - half_width = self.fsq_level_list // 2 - - return self._round_ste(bounded_z) / half_width - - def encode(self, obs: dict) -> torch.Tensor: - """Run encoder inference only to get token state.""" - encoder_obs = obs["sonic_tokenizer"] - batch_size = encoder_obs.shape[0] - token_state = self._onnx_inference_gpu( - self.encoder, - encoder_obs, - output_shape=(batch_size, self.encoder_output_dim), - ) - return token_state - - def decode( - self, token_state: torch.Tensor, decoder_obs: torch.Tensor - ) -> torch.Tensor: - """Run decoder inference with given token state and decoder observations.""" - batch_size = token_state.shape[0] - decoder_input = torch.cat([token_state, decoder_obs], dim=1) - actions = self._onnx_inference_gpu( - self.decoder, - decoder_input, - output_shape=(batch_size, self.decoder_output_dim), - ) - return actions - - -class SONICActionBase(ActionTerm): - """ - Base class for all SONIC action terms. - - Provides common initialization and properties for joint mapping, scaling, - and filtering used by all SONIC action terms. - """ - - cfg: ActionTermCfg - _asset: Articulation - _env: ManagerBasedRLEnv - _policy: SonicPolicy - _num_envs: int - _device: torch.device - - # Joint mapping attributes - _joint_ids: torch.Tensor - _joint_names: list[str] - _num_joints: int - _sonic_joint_ids: torch.Tensor - _sonic_joint_names: list[str] - _num_sonic_joints: int - _sonic_joint_indices: torch.Tensor - _direct_joint_mask: torch.Tensor - _direct_joint_indices: torch.Tensor - - # Command term access - _command: Any # TrackingCommand instance - _num_future_frames: int - - # Action buffers - _processed_actions: torch.Tensor - _raw_actions: torch.Tensor - _last_sonic_actions: torch.Tensor - - # Scaling and offset - _use_default_offset: bool - _joint_pos_default: torch.Tensor - _scale: torch.Tensor - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize common SONIC action term components. - - Sets up joint mappings, scaling, default offsets, and action buffers. - Derived classes should call this first, then add their specific initialization. - """ - super().__init__(cfg, env) - - self._env = env - self._policy = SonicPolicy(cfg.policy_dir) - self._num_envs = env.num_envs - self._device = env.device - - # Find all controllable joints and SONIC-controlled joints - self._joint_ids, self._joint_names = self._asset.find_joints(cfg.joint_names) - self._num_joints = len(self._joint_names) - - self._sonic_joint_ids, self._sonic_joint_names = self._asset.find_joints( - cfg.sonic_joint_names - ) - self._num_sonic_joints = len(self._sonic_joint_names) - - # Map SONIC joints to their indices in the full joint list - self._sonic_joint_indices = torch.tensor( - [self._joint_names.index(name) for name in self._sonic_joint_names], - dtype=torch.long, - device=self._device, - ) - - # Identify non-SONIC joints for direct control - self._direct_joint_mask = torch.ones( - self._num_joints, dtype=torch.bool, device=self._device - ) - self._direct_joint_mask[self._sonic_joint_indices] = False - self._direct_joint_indices = torch.where(self._direct_joint_mask)[0] - self._num_direct_joints = len(self._direct_joint_indices) - - # Get command term for accessing base actions and future frames - self._command = env.command_manager.get_term(cfg.command_name) - self._num_future_frames = self._command.num_future_frames - - # Set up scaling for SONIC outputs - self._use_default_offset = ( - cfg.use_default_offset if hasattr(cfg, "use_default_offset") else True - ) - if self._use_default_offset: - self._joint_pos_default = self._asset.data.default_joint_pos[ - :, self._sonic_joint_ids - ].clone() - - if isinstance(cfg.scale, dict): - index_list, _, value_list = string_utils.resolve_matching_names_values( - cfg.scale, self._sonic_joint_names - ) - self._scale = torch.ones( - self._num_envs, self._num_sonic_joints, device=self._device - ) - self._scale[:, index_list] = torch.tensor(value_list, device=self._device) - else: - self._scale = ( - torch.ones(self._num_envs, self._num_sonic_joints, device=self._device) - * cfg.scale - ) - - # Initialize last actions buffer - if self._use_default_offset: - self._last_sonic_actions = self._joint_pos_default.clone() - else: - self._last_sonic_actions = torch.zeros( - self._num_envs, self._num_sonic_joints, device=self._device - ) - - @property - def policy(self) -> SonicPolicy: - """Get the SONIC policy instance.""" - return self._policy - - @property - def raw_actions(self) -> torch.Tensor: - """Get the raw actions received from the agent.""" - return self._raw_actions - - @property - def processed_actions(self) -> torch.Tensor: - """Get the processed actions.""" - return self._processed_actions - - def get_sonic_joint_ids(self) -> torch.Tensor: - """Get joint IDs for SONIC-controlled joints in the asset.""" - return self._sonic_joint_ids - - def get_sonic_joint_indices(self) -> torch.Tensor: - """Get indices of SONIC-controlled joints in the full joint list.""" - return self._sonic_joint_indices - - def get_last_sonic_actions(self) -> torch.Tensor: - """Get last SONIC output actions (before scaling/offset).""" - return self._last_sonic_actions - - def _build_sonic_observations(self, *args: Any, **kwargs: Any) -> dict[str, Any]: - """Build observation dictionary for SONIC from environment observations.""" - tokenizer_obs = self._env.obs_buf["sonic_tokenizer"] - policy_obs = self._env.obs_buf["sonic_policy"] - - return {"sonic_tokenizer": tokenizer_obs, "sonic_policy": policy_obs} - - def apply_actions(self) -> None: - """Apply processed actions to robot joints.""" - self._asset.set_joint_position_target( - self._processed_actions, joint_ids=self._joint_ids - ) - self._command.update_action_history(self._processed_actions) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_action.py deleted file mode 100644 index ab36a650..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_action.py +++ /dev/null @@ -1,115 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from typing import Any - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICHierachicalAction(SONICActionBase): - """ - Hierarchical action for SONIC. - - Action structure: [joint_commands, base_ori_6d] - SONIC-controlled joints use SONIC output, other joints use commands directly. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize hierarchical SONIC action term.""" - super().__init__(cfg, env) - - # Initialize action buffers with correct dimensions - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Return total action dimension (joints + 6D base orientation).""" - return self._num_joints + 6 - - def process_actions(self, actions: torch.Tensor) -> None: - """Process hierarchical actions through SONIC.""" - self._raw_actions[:] = actions - - joint_commands = actions[:, : self._num_joints] - base_ori_6d = actions[:, self._num_joints :] - - sonic_obs = self._build_sonic_observations(joint_commands, base_ori_6d) - sonic_actions = self._policy(sonic_obs) - - sonic_actions_scaled = sonic_actions * self._scale - if self._use_default_offset: - sonic_actions_absolute = sonic_actions_scaled + self._joint_pos_default - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_actions_absolute - ) - else: - sonic_actions_absolute = sonic_actions_scaled - self._processed_actions[:, self._sonic_joint_indices] = sonic_actions_scaled - - self._last_sonic_actions[:] = sonic_actions - - if len(self._direct_joint_indices) > 0: - self._processed_actions[:, self._direct_joint_indices] = joint_commands[ - :, self._direct_joint_indices - ] - - def _build_sonic_observations( - self, joint_commands: torch.Tensor, base_ori_6d: torch.Tensor - ) -> dict[str, Any]: - """Build observation dictionary for SONIC by modifying first future frame.""" - obs_manager = self._env.observation_manager - tokenizer_term_names = obs_manager._group_obs_term_names["sonic_tokenizer"] - tokenizer_term_cfgs = obs_manager._group_obs_term_cfgs["sonic_tokenizer"] - tokenizer_terms = [] - modified_joint_pos = None - - # Extract SONIC joint commands (filter from all joints to SONIC joints only) - sonic_joint_commands = joint_commands[:, self._sonic_joint_indices] - - for term_name, term_cfg in zip( - tokenizer_term_names, tokenizer_term_cfgs, strict=True - ): - if term_name == "command_joint_pos_multi_future": - original = term_cfg.func(self._env, **term_cfg.params) - original_reshaped = original.reshape( - self._num_envs, self._num_future_frames, -1 - ) - original_reshaped[:, 0, :] = sonic_joint_commands - modified_joint_pos = original_reshaped.clone() - tokenizer_terms.append(original_reshaped.reshape(self._num_envs, -1)) - - elif term_name == "command_joint_vel_multi_future": - original = term_cfg.func(self._env, **term_cfg.params) - original_reshaped = original.reshape( - self._num_envs, self._num_future_frames, -1 - ) - if modified_joint_pos is not None and self._num_future_frames > 1: - dt = self._command.cfg.dt_future_frames / self._command.frame_step - original_reshaped[:, 0, :] = ( - modified_joint_pos[:, 1, :] - modified_joint_pos[:, 0, :] - ) / dt - tokenizer_terms.append(original_reshaped.reshape(self._num_envs, -1)) - - elif term_name == "motion_anchor_ori_b": - original = term_cfg.func(self._env, **term_cfg.params) - original_reshaped = original.reshape( - self._num_envs, self._num_future_frames, 6 - ) - original_reshaped[:, 0, :] = base_ori_6d - tokenizer_terms.append(original_reshaped.reshape(self._num_envs, -1)) - - else: - tokenizer_terms.append(term_cfg.func(self._env, **term_cfg.params)) - - tokenizer_obs = torch.cat(tokenizer_terms, dim=-1) - policy_obs = self._env.obs_buf["sonic_policy"] - - return {"sonic_tokenizer": tokenizer_obs, "sonic_policy": policy_obs} diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_residual_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_residual_action.py deleted file mode 100644 index 55062d3a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_hierarchical_residual_action.py +++ /dev/null @@ -1,70 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICHierarchicalResidualAction(SONICActionBase): - """Hierarchical residual action: residuals added BEFORE SONIC. - - Action structure: [joint_residuals] - Residuals are added to commanded joint positions for all joints. - SONIC-controlled joints: (base_pos + residual) processed through SONIC. - Non-SONIC joints: (base_pos + residual) applied directly. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize residual SONIC action term.""" - super().__init__(cfg, env) - - # Initialize action buffers with correct dimensions - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Action dimension is all joints (residuals for all joints).""" - return self._num_joints - - def process_actions(self, actions: torch.Tensor) -> None: - """Process residual actions by adding to commanded positions.""" - self._raw_actions[:] = actions - - # Get base joint positions from command (first future frame) - joint_pos_multi_future = self._command.command_joint_pos_multi_future - base_joint_pos = joint_pos_multi_future[:, 0, :] # (num_envs, num_joints) - - # Add residuals to base positions for all joints - modified_joint_pos = base_joint_pos + actions # (num_envs, num_joints) - - # Build SONIC observations using environment's standard observations - sonic_obs = self._build_sonic_observations() - - # Run SONIC policy - sonic_actions = self._policy(sonic_obs) - - # Scale and offset SONIC outputs - sonic_actions_scaled = sonic_actions * self._scale - if self._use_default_offset: - sonic_actions_absolute = sonic_actions_scaled + self._joint_pos_default - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_actions_absolute - ) - else: - self._processed_actions[:, self._sonic_joint_indices] = sonic_actions_scaled - - # Store raw SONIC output for observation functions - self._last_sonic_actions[:] = sonic_actions - - # For non-SONIC joints, use (base_pos + residual) directly - if len(self._direct_joint_indices) > 0: - self._processed_actions[:, self._direct_joint_indices] = modified_joint_pos[ - :, self._direct_joint_indices - ] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_joint_residual_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_joint_residual_action.py deleted file mode 100644 index 0b8436e4..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_joint_residual_action.py +++ /dev/null @@ -1,131 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""SONIC action with joint-level residuals added AFTER the SONIC encoder/decoder. - -RL outputs scaled per-joint residuals that are added to the SONIC decoder output. -Optionally controls finger joints via residuals on top of tracking command reference. -""" - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICJointResidualAction(SONICActionBase): - """Joint residual action: RL residuals added AFTER SONIC output. - - SONIC encoder+decoder produces base joint targets from the tracking trajectory. - RL adds scaled per-joint residuals on top. Optionally, non-SONIC joints - (fingers) can also receive RL residuals on top of the tracking command reference. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize joint residual action with optional finger control.""" - super().__init__(cfg, env) - - if cfg.hand_policy_class is not None: - self._hand_policy = cfg.hand_policy_class(cfg.hand_policy_cfg, env) - else: - self._hand_policy = None - - self._robot = env.scene[cfg.asset_name] - self._residual_scale = getattr(cfg, "residual_scale", 0.1) - _frs = getattr(cfg, "finger_residual_scale", -1.0) - self._finger_residual_scale = _frs if _frs >= 0 else None - self._finger_residual = getattr(cfg, "finger_residual", False) - self._use_tanh = getattr(cfg, "use_tanh", True) - - # Optional subset of SONIC joints for residuals - self._residual_joint_names = getattr(cfg, "residual_joint_names", None) - if self._residual_joint_names is not None: - sonic_names = [ - self._robot.joint_names[i] for i in self._sonic_joint_indices - ] - self._residual_sonic_indices = [ - sonic_names.index(name) for name in self._residual_joint_names - ] - self._num_residual_joints = len(self._residual_sonic_indices) - else: - self._residual_sonic_indices = None # type: ignore[assignment] - self._num_residual_joints = self._num_sonic_joints - - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Total action dimensions: body residuals + optional finger residuals.""" - dim = self._num_residual_joints - if self._finger_residual: - dim += self._num_direct_joints - return dim - - def process_actions(self, actions: torch.Tensor) -> None: - """Apply RL residuals to SONIC output and finger commands.""" - self._raw_actions[:] = actions - - body_residuals = actions[:, : self._num_residual_joints] - finger_residuals = ( - actions[:, self._num_residual_joints :] if self._finger_residual else None - ) - - # Run SONIC encoder+decoder on tracking data - sonic_obs = self._build_sonic_observations() - sonic_actions = self._policy(sonic_obs) - - # Add scaled RL residuals to SONIC output - if self._residual_sonic_indices is not None: - full_residuals = torch.zeros_like(sonic_actions) - full_residuals[:, self._residual_sonic_indices] = body_residuals - else: - full_residuals = body_residuals - - squashed = torch.tanh(full_residuals) if self._use_tanh else full_residuals - sonic_with_residual = sonic_actions + squashed * self._residual_scale - - # Scale and offset - sonic_scaled = sonic_with_residual * self._scale - if self._use_default_offset: - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_scaled + self._joint_pos_default - ) - else: - self._processed_actions[:, self._sonic_joint_indices] = sonic_scaled - - self._last_sonic_actions[:] = sonic_actions - - # Finger joints - if self._hand_policy is not None and len(self._direct_joint_indices) > 0: - hand_obs = {"hand_policy": self._env.obs_buf["hand_policy"]} - hand_actions = self._hand_policy(hand_obs, self._env, self.cfg.command_name) - self._processed_actions[:, self._hand_policy.left_hand_joints_ids] = ( - hand_actions["left_hand_actions"] - ) - self._processed_actions[:, self._hand_policy.right_hand_joints_ids] = ( - hand_actions["right_hand_actions"] - ) - elif len(self._direct_joint_indices) > 0: - base_joint_pos = self._command.command_joint_pos_multi_future[:, 0, :] - if finger_residuals is not None: - finger_scale = ( - self._finger_residual_scale - if self._finger_residual_scale is not None - else self._residual_scale - ) - squashed_fingers = ( - torch.tanh(finger_residuals) if self._use_tanh else finger_residuals - ) - self._processed_actions[:, self._direct_joint_indices] = ( - base_joint_pos[:, self._direct_joint_indices] - + squashed_fingers * finger_scale - ) - else: - self._processed_actions[:, self._direct_joint_indices] = base_joint_pos[ - :, self._direct_joint_indices - ] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_action.py deleted file mode 100644 index 25855c48..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_action.py +++ /dev/null @@ -1,69 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICLatentAction(SONICActionBase): - """ - Latent action for SONIC. - - Action structure: [latent_state, direct_joint_pos] - Latent state is directly used as input to decoder. - Direct joint positions are used for non-SONIC joints. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize latent SONIC action term.""" - super().__init__(cfg, env) - - # Initialize action buffers with correct dimensions - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Action dimension is encoder output dimension (latent space + direct joint positions).""" - return self._policy.encoder_output_dim + self._num_direct_joints - - def process_actions(self, actions: torch.Tensor) -> None: - """Process latent residual actions by adding to token state before decoder.""" - self._raw_actions[:] = actions - - # Interpret actions as token state - token_state = actions[:, : self._policy.encoder_output_dim] - direct_joint_pos = actions[:, self._policy.encoder_output_dim :] - - # Ensure token state is a code in finite scalar quantizer - token_state = self._policy.quantize(token_state) - - # Build SONIC observations using environment's standard observations - sonic_obs = self._build_sonic_observations() - - # Run decoder with modified token state - decoder_obs = sonic_obs["sonic_policy"] - sonic_actions = self._policy.decode(token_state, decoder_obs) - - # Scale and offset SONIC outputs - sonic_actions_scaled = sonic_actions * self._scale - if self._use_default_offset: - sonic_actions_absolute = sonic_actions_scaled + self._joint_pos_default - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_actions_absolute - ) - else: - self._processed_actions[:, self._sonic_joint_indices] = sonic_actions_scaled - - # Store raw SONIC output for observation functions - self._last_sonic_actions[:] = sonic_actions - - # For non-SONIC joints, use direct joint positions from action - if len(self._direct_joint_indices) > 0: - self._processed_actions[:, self._direct_joint_indices] = direct_joint_pos diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_hand_policy_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_hand_policy_action.py deleted file mode 100644 index bd3e1e7a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_hand_policy_action.py +++ /dev/null @@ -1,93 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICLatentHandPolicyAction(SONICActionBase): - """ - Latent action for SONIC with a pretrained hand policy for direct joint control. - - Action structure: [latent_state] - Latent state is directly used as input to decoder for SONIC joints (arm). - Direct joints (fingers) are controlled by a separate pretrained hand policy - rather than policy outputs. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize latent hand policy SONIC action term.""" - super().__init__(cfg, env) - - # Initialize hand policy for direct joint (finger) control - self._hand_policy = cfg.hand_policy_class(cfg.hand_policy_cfg, env) - - # Initialize action buffers with correct dimensions - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Action dimension is just the encoder output dimension (latent space only).""" - return self._policy.encoder_output_dim - - def process_actions(self, actions: torch.Tensor) -> None: - """Process latent actions for SONIC arm control and use hand policy for fingers.""" - self._raw_actions[:] = actions - - # Interpret actions as token state (full action is latent space) - token_state = actions - - # Ensure token state is a code in finite scalar quantizer - token_state = self._policy.quantize(token_state) - - # Build SONIC observations using environment's standard observations - sonic_obs = self._build_sonic_observations() - - # Run decoder with token state for arm control - decoder_obs = sonic_obs["sonic_policy"] - sonic_actions = self._policy.decode(token_state, decoder_obs) - - if self.cfg.debug: - # just directly use reference motion - sonic_obs = self._build_sonic_observations() - sonic_actions = self._policy(sonic_obs) - - # Scale and offset SONIC outputs for arm joints - sonic_actions_scaled = sonic_actions * self._scale - if self._use_default_offset: - sonic_actions_absolute = sonic_actions_scaled + self._joint_pos_default - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_actions_absolute - ) - else: - self._processed_actions[:, self._sonic_joint_indices] = sonic_actions_scaled - - # Store raw SONIC output for observation functions - self._last_sonic_actions[:] = sonic_actions - - # Use hand policy for direct joints (fingers) - if len(self._direct_joint_indices) > 0: - hand_policy_obs = self._build_hand_policy_observations() - hand_policy_actions = self._hand_policy( - hand_policy_obs, self._env, self.cfg.command_name - ) - left_hand_actions = hand_policy_actions["left_hand_actions"] - right_hand_actions = hand_policy_actions["right_hand_actions"] - self._processed_actions[:, self._hand_policy.left_hand_joints_ids] = ( - left_hand_actions - ) - self._processed_actions[:, self._hand_policy.right_hand_joints_ids] = ( - right_hand_actions - ) - - def _build_hand_policy_observations(self) -> dict: - """Build observation dictionary for hand policy.""" - hand_policy_obs = self._env.obs_buf["hand_policy"] - return {"hand_policy": hand_policy_obs} diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_residual_action.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_residual_action.py deleted file mode 100644 index c915e620..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/actions/sonic_latent_residual_action.py +++ /dev/null @@ -1,71 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers.action_manager import ActionTermCfg - -from .sonic_actions import SONICActionBase - - -class SONICLatentResidualAction(SONICActionBase): - """ - Latent residual action for SONIC. - - Action structure: [latent_residuals] - Latent residuals are added to encoder token state before decoder inference. - """ - - def __init__(self, cfg: ActionTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize latent residual SONIC action term.""" - super().__init__(cfg, env) - - # Initialize action buffers with correct dimensions - self._processed_actions = torch.zeros( - self._num_envs, self._num_joints, device=self._device - ) - self._raw_actions = torch.zeros( - self._num_envs, self.action_dim, device=self._device - ) - - @property - def action_dim(self) -> int: - """Action dimension is encoder output dimension (latent space).""" - return self._policy.encoder_output_dim - - def process_actions(self, actions: torch.Tensor) -> None: - """Process latent residual actions by adding to token state before decoder.""" - self._raw_actions[:] = actions - - # Build SONIC observations using environment's standard observations - sonic_obs = self._build_sonic_observations() - - # Run encoder to get base token state - token_state = self._policy.encode(sonic_obs) - - # Add latent residuals to token state - modified_token_state = token_state + actions - - # Run decoder with modified token state - decoder_obs = sonic_obs["sonic_policy"] - sonic_actions = self._policy.decode(modified_token_state, decoder_obs) - - # Scale and offset SONIC outputs - sonic_actions_scaled = sonic_actions * self._scale - if self._use_default_offset: - sonic_actions_absolute = sonic_actions_scaled + self._joint_pos_default - self._processed_actions[:, self._sonic_joint_indices] = ( - sonic_actions_absolute - ) - else: - self._processed_actions[:, self._sonic_joint_indices] = sonic_actions_scaled - - # Store raw SONIC output for observation functions - self._last_sonic_actions[:] = sonic_actions - - # For non-SONIC joints, use base positions from command directly - if len(self._direct_joint_indices) > 0: - joint_pos_multi_future = self._command.command_joint_pos_multi_future - base_joint_pos = joint_pos_multi_future[:, 0, :] # (num_envs, num_joints) - self._processed_actions[:, self._direct_joint_indices] = base_joint_pos[ - :, self._direct_joint_indices - ] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/README.md deleted file mode 100644 index 718a8373..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/README.md +++ /dev/null @@ -1,127 +0,0 @@ -# Whole-Body Tracking Command - -The `TrackingCommand` loads all reference data from a single `motion_v1` parquet (via `robotic_grounding.motion_schema`) and provides command targets and current state observations consumed by rewards, observations, terminations, and actions. - -## Data Flow - -``` -motion_v1 Parquet (Hive-partitioned: sequence_id=.../robot_name=.../*.parquet) -| -+-- robot_root_position (T, 3) --> root_pos_w -+-- robot_root_wxyz (T, 4) --> root_quat_w -+-- robot_joint_positions (T, J) --> joint_pos, joint_vel (via FD) -+-- robot_joint_names (J,) --> joint reordering against live robot -+-- ee_link_names, ee_pose_w (T, E, 7) --> EE tracking + wrist body ID resolution -+-- object_body_position/wxyz (T, B, ...) --> object tracking targets -| -+-- hand_frames_w[side] (T, K, 7) --> _precompute_hand_keypoints_in_object_frame() -+-- hand_finger_joints[side] (T, Jf) | wrist + fingertip positions in object frame -| | -+-- hand_link_contact_positions[side] --> _precompute_contact_positions_in_object_frame() -+-- hand_object_contact_normals[side] | contacts + normals in object COM frame -+-- hand_object_contact_part_ids[side] | -| | -+-- hand_contact_active[side] --> binary contact labels (for force closure reward) -+-- object_mesh_radius --> _precompute_contact_wrench_support_values() -``` - -At runtime each step: -1. `_update_command()` advances timestep and decays VOC (right-justified within freeze) -2. Properties index precomputed data by `timestep` and transform from object frame to env/world frame using the live object pose -3. `_spread_offset_blended` anneals shoulder spread during freeze period - -## Initialization - -| Method | Purpose | -|--------|---------| -| `_init_scene_references` | Resolve robot, object, wrist/fingertip/finger IDs, contact sensors | -| `_load_and_process_motion` | Load motion_v1 parquet, populate root/joints/EE tensors, resolve wrist body IDs | -| `_init_buffers` | Per-env counters, VOC scale, action history, spread offset | -| `_init_hand_data` | Fingertip/finger joint IDs, retargeted hand data, contact labels | -| `_init_contact_data` | Contact positions, normals, part IDs, validity masks | -| `_precompute_hand_keypoints_in_object_frame` | Hand frames + wrist poses -> object frame | -| `_precompute_contact_positions_in_object_frame` | Contact positions + normals -> object COM frame | -| `_init_wrench_data` | Wrench basis (512 samples), friction cone, runtime buffers | -| `_precompute_contact_wrench_support_values` | Per-timestep wrench supports from retargeted contacts | - -## Properties - -### Command Targets (single frame) - -| Property | Shape | Frame | -|----------|-------|-------| -| `command_anchor_pos_w` | (E, 3) | world | -| `command_anchor_quat_w` | (E, 4) | world | -| `command_joint_pos` | (E, J) | joint (includes spread blend) | -| `command_object_pos_w` | (E, 3) | world | -| `command_object_quat_w` | (E, 4) | world | -| `command_ee_pos_w` | (E, L, 3) | world | -| `command_ee_quat_w` | (E, L, 4) | world | - -### Command Targets (multi-future) - -| Property | Shape | -|----------|-------| -| `command_joint_pos_multi_future` | (E, F, J) — includes spread blend | -| `command_joint_vel_multi_future` | (E, F, J) | -| `command_anchor_pos_w_multi_future` | (E, F, 3) | -| `command_anchor_rot_diff_l_multi_future` | (E, F, 6) — 6D rotation deltas | -| `command_anchor_z_multi_future` | (E, F) — root Z positions | -| `command_ee_pos_w_multi_future` | (E, F, L, 3) | -| `command_ee_quat_w_multi_future` | (E, F, L, 4) | -| `command_object_pos_w_multi_future` | (E, F, 3) | -| `command_multi_future` | (E, F, 2J) — joint pos + vel concatenated | - -### Simulation State - -| Property | Shape | Frame | -|----------|-------|-------| -| `robot_anchor_pos_w` | (E, 3) | world | -| `robot_anchor_quat_w` | (E, 4) | world | -| `robot_joint_pos` | (E, J) | joint | -| `robot_joint_vel` | (E, J) | joint | -| `robot_ee_pos_w` | (E, L, 3) | world | -| `robot_ee_quat_w` | (E, L, 4) | world | -| `object_pos_w` | (E, 3) | world | -| `object_quat_w` | (E, 4) | world | - -### Hand Keypoints and Fingers - -| Property | Shape | Frame | -|----------|-------|-------| -| `{side}_hand_wrist_pose_command_e` | (E, 7) | env | -| `{side}_hand_wrist_position_e` | (E, 3) | env | -| `{side}_hand_fingertip_position_command_e` | (E, K, 3) | env | -| `{side}_hand_fingertip_position_e` | (E, K, 3) | env | -| `{side}_hand_finger_joint_pos` | (E, Jf) | joint | -| `{side}_hand_finger_joint_pos_command` | (E, Jf) | joint | - -### Contact and Wrench - -| Property | Shape | Description | -|----------|-------|-------------| -| `{side}_hand_object_contact_command_positions_e` | (E, N, 3) | Target contacts (env frame) | -| `{side}_hand_object_contact_positions_e` | (E, B, N, 3) | Live contacts (env frame) | -| `{side}_hand_object_contact_forces_w` | (E, H, B, N, 3) | Live force history | -| `{side}_hand_contact_wrench_supports_command` | (E, B, S) | Precomputed wrench supports | -| `{side}_hand_contact_wrench_supports` | (E, B, S) | Live wrench supports | -| `{side}_hand_contact_active_command` | (E,) | Binary contact label at timestep | - -### VOC and Action History - -| Property | Shape | Description | -|----------|-------|-------------| -| `virtual_object_controller_scale_factor` | (1,) | Global VOC curriculum target | -| `virtual_object_controller_scale_factor_per_env` | (E, 1) | Per-env VOC (decays during freeze) | -| `object_position_e` | (E, 1, 3) | Current object position (env frame) | -| `object_orientation_e` | (E, 1, 4) | Current object orientation | -| `object_body_position_command_e` | (E, B, 3) | Target object body positions | -| `object_body_wxyz_command_e` | (E, B, 4) | Target object body orientations | -| `action_history` | (E, H*A) | Flattened past processed actions | - -## Shape Legend - -- **E** = num_envs, **F** = num_future_frames, **J** = num_tracked_joints -- **Jf** = num_finger_joints (per hand), **L** = num_ee_links, **K** = num_fingertips -- **N** = num_contact_filter_prims (per hand), **H** = history length, **B** = num_object_bodies -- **S** = num_wrench_space_basis_samples (default 512), **A** = action_dim diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/__init__.py deleted file mode 100644 index 85fcc0ee..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Commands for robotic grounding tasks.""" - -from .tracking_command import * # noqa: F403 -from .tracking_command_cfg import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command.py deleted file mode 100644 index 6ccfe44e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command.py +++ /dev/null @@ -1,1764 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Whole-body tracking command term. - -Loads all reference data from a single planner parquet and provides command -targets + current state observations for body tracking, hand keypoint tracking, -contact tracking, and virtual object control. - -Follows the same initialization pattern as DualHandsObjectTrackingCommand -but operates on a single-articulation whole-body robot (not dual floating-base hands). -""" - -from __future__ import annotations - -import warnings -from collections.abc import Sequence -from typing import TYPE_CHECKING - -import isaaclab.utils.math as math_utils -import torch -from isaaclab.assets import Articulation, RigidObject -from isaaclab.managers import CommandTerm -from isaaclab.markers import VisualizationMarkers - -from robotic_grounding.tasks.v2p.mdp.utils import ( - compute_wrench_space, - compute_wrench_space_support_function, - sample_wrench_space_basis_scaled, -) -from robotic_grounding.tasks.v2p.mdp.utils_jit import ( - wrench_preprocess_jit, - wrench_support_one_body_jit, -) - -from .tracking_utils import load_motion_data - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - - from .tracking_command_cfg import TrackingCommandCfg - - -class TrackingCommand(CommandTerm): - """Whole-body tracking command term. - - Loads reference motion from a planner parquet and exposes: - - Body tracking: root pose, joint positions/velocities, multi-future frames - - EE tracking: wrist positions/orientations - - Hand keypoints: fingertip positions (in object frame, transformed per step) - - Contact targets: desired contact positions on object surface - - Object tracking: object body pose trajectory - - VOC: virtual object control curriculum scale - """ - - cfg: TrackingCommandCfg - - def __init__(self, cfg: TrackingCommandCfg, env: ManagerBasedRLEnv) -> None: - """Initialize tracking command from planner parquet.""" - super().__init__(cfg, env) - - self._init_scene_references(cfg, env) - self._load_and_process_motion(cfg) - self._init_buffers(cfg) - self._init_metrics() - self._init_hand_data(cfg) - self._init_contact_data() - self._precompute_hand_keypoints_in_object_frame() - self._precompute_contact_positions_in_object_frame() - self._init_wrench_data(cfg) - self._precompute_contact_wrench_support_values() - - # Re-trigger debug vis now that ee_link_names is known - if cfg.debug_vis: - self.set_debug_vis(True) - - # ------------------------------------------------------------------ - # Initialization helpers - # ------------------------------------------------------------------ - - def _init_scene_references( - self, cfg: TrackingCommandCfg, env: ManagerBasedRLEnv - ) -> None: - """Resolve robot, object, and contact sensor assets from the scene.""" - self.robot: Articulation = env.scene[cfg.asset_name] - self.object: RigidObject = env.scene[cfg.object_name] - self._env_origins = env.scene.env_origins - - # Object list (for VOC compatibility) - if cfg.object_body_names: - self.objects = [env.scene[name] for name in cfg.object_body_names] - else: - self.objects = [self.object] - cfg.object_body_names = [cfg.object_name] - - # Anchor body (pelvis) - self._anchor_body_ids, _ = self.robot.find_bodies([cfg.anchor_body_name]) - self._anchor_body_id = self._anchor_body_ids[0] - - # Defaults — resolved after motion data is loaded - self._left_wrist_body_id = None - self._right_wrist_body_id = None - self._left_fingertip_body_ids: list[int] = [] - self._right_fingertip_body_ids: list[int] = [] - self._left_finger_joint_ids: list[int] = [] - self._right_finger_joint_ids: list[int] = [] - - # Contact sensors — grouped by side, list per body (matching floating hand env) - self.object_to_right_hand_contact_sensors = [] - self.object_to_left_hand_contact_sensors = [] - for sensor_name in cfg.object_contact_sensor_names: - if sensor_name in env.scene.sensors: - sensor = env.scene.sensors[sensor_name] - if "_to_right_" in sensor_name: - self.object_to_right_hand_contact_sensors.append(sensor) - elif "_to_left_" in sensor_name: - self.object_to_left_hand_contact_sensors.append(sensor) - - if self.object_to_right_hand_contact_sensors: - self.num_robot_contacts_right = len( - self.object_to_right_hand_contact_sensors[0].cfg.filter_prim_paths_expr - ) - else: - self.num_robot_contacts_right = 0 - if self.object_to_left_hand_contact_sensors: - self.num_robot_contacts_left = len( - self.object_to_left_hand_contact_sensors[0].cfg.filter_prim_paths_expr - ) - else: - self.num_robot_contacts_left = 0 - - def _load_and_process_motion(self, cfg: TrackingCommandCfg) -> None: - """Load motion data from parquet and set up body tracking tensors.""" - motion_data = load_motion_data(cfg, self.robot, self.device) - - # Body motion (already split on-disk and in-memory). - self.root_pos_w = motion_data.robot_root_position.float() + torch.tensor( - cfg.robot_anchor_pos_offset, device=self.device - ) - self.root_quat_w = motion_data.robot_root_wxyz.float() - self._joint_pos_file = motion_data.robot_joint_positions.float() - self._object_pos_w = motion_data.object_pos_w.float() + torch.tensor( - cfg.object_pos_offset, device=self.device - ) - self._object_quat_w = motion_data.object_quat_w.float() - # Multi-body reference object trajectories (E, T, B, *) for the command - # target. Used by the multi-object command-frame observations/metrics. - self._object_body_pos_w = ( - motion_data.object_body_position.float() - + torch.tensor(cfg.object_pos_offset, device=self.device) - ) - self._object_body_quat_w = motion_data.object_body_wxyz.float() - if motion_data.object_articulation is not None: - self.retargeted_object_articulation = ( - motion_data.object_articulation.float() - ) - else: - self.retargeted_object_articulation = torch.zeros( - self._object_body_pos_w.shape[0], 0, device=self.device - ) - self.retargeted_object_body_names = motion_data.object_body_names - self.num_timesteps = self.root_pos_w.shape[0] - - # EE data - if motion_data.ee_pos_w is not None: - self.ee_link_names = motion_data.ee_link_names - self.ee_link_ids = motion_data.ee_link_ids - self.ee_pos_w = motion_data.ee_pos_w - self.ee_quat_w = motion_data.ee_quat_w - - # Joint velocity via finite differences - self._joint_vel_file = torch.zeros_like(self._joint_pos_file) - self._joint_vel_file[:-1] = ( - self._joint_pos_file[1:] - self._joint_pos_file[:-1] - ) / cfg.dt - - # Future frame config - self.num_future_frames = cfg.num_future_frames - self.frame_step = int(cfg.dt_future_frames / cfg.dt) - self._future_frame_offsets = torch.arange( - 0, - self.num_future_frames * self.frame_step, - self.frame_step, - dtype=torch.int32, - device=self.device, - ) - - # Joint reordering - self._tracked_joint_ids, self._tracked_joint_names = self.robot.find_joints( - cfg.joint_names - ) - file_joint_names = motion_data.file_joint_names or cfg.file_joint_names - if file_joint_names is not None: - file_to_isaac = [ - file_joint_names.index(n) - for n in self._tracked_joint_names - if n in file_joint_names - ] - if len(file_to_isaac) == len(self._tracked_joint_names): - self.joint_pos = self._joint_pos_file[:, file_to_isaac] - self.joint_vel = self._joint_vel_file[:, file_to_isaac] - else: - self.joint_pos = self._joint_pos_file - self.joint_vel = self._joint_vel_file - else: - self.joint_pos = self._joint_pos_file - self.joint_vel = self._joint_vel_file - - # Object height peak (for reward phase detection) - self.object_height_peak_timestep = torch.argmax(self._object_pos_w[:, 2]).item() - - # Hand data flag - # Resolve wrist body IDs now that ee_link_names is known from parquet - ee_names = getattr(self, "ee_link_names", None) or [] - if ee_names: - all_body_ids, all_body_names = self.robot.find_bodies(ee_names) - for bid, bname in zip(all_body_ids, all_body_names, strict=False): - if "left" in bname.lower(): - self._left_wrist_body_id = bid - elif "right" in bname.lower(): - self._right_wrist_body_id = bid - - self._motion_data = motion_data - - def _init_buffers(self, cfg: TrackingCommandCfg) -> None: - """Allocate per-env counters, VOC scale, and encoder mode.""" - self.timestep = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.reset_timestep = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self.trajectory_end_timestep = torch.full( - (self.num_envs,), - self.num_timesteps - 1, - dtype=torch.int32, - device=self.device, - ) - self.steps_since_last_reset = torch.zeros( - self.num_envs, dtype=torch.int32, device=self.device - ) - self._encoder_mode = torch.zeros(self.num_envs, 4, device=self.device) - - # VOC - self.virtual_object_controller_scale_factor = torch.tensor( - [cfg.initial_virtual_object_control_curriculum_scale], device=self.device - ) - self.virtual_object_controller_scale_factor_per_env = ( - cfg.initial_virtual_object_control_curriculum_scale - * torch.ones(self.num_envs, 1, device=self.device) - ) - - # Tracking lengths (for metrics normalization) - max_ep_steps = int(self._env.max_episode_length) - self.tracking_lengths = torch.full( - (self.num_envs,), - min(self.num_timesteps, max_ep_steps), - dtype=torch.int32, - device=self.device, - ) - - # Action history buffer (lazy init on first update_action_history call) - self._action_history: torch.Tensor | None = None - self._action_history_len = cfg.action_history_length - - # Shoulder spread offset (for annealing during freeze period) - num_joints = self.joint_pos.shape[1] - self._spread_joint_offset = torch.zeros( - self.num_envs, num_joints, device=self.device - ) - - self.all_env_ids = torch.arange(self.num_envs, device=self.device) - self.num_bodies = self.object_position_e.shape[1] - if ( - self.retargeted_object_body_names is None - or len(self.retargeted_object_body_names) != self.num_bodies - ): - self.retargeted_object_body_names = list(self.object.data.body_names) - assert len(self.retargeted_object_body_names) == self.num_bodies, ( - "The number of body names in the motion file and the object do not match. " - f"Find {len(self.retargeted_object_body_names)} in motion file, " - f"but {self.num_bodies} in the object." - ) - self.KEYPOINT_VECS = ( - torch.tensor( - [ - [1.0, 0.0, 0.0], - [-1.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [0.0, -1.0, 0.0], - [0.0, 0.0, 1.0], - [0.0, 0.0, -1.0], - ], - dtype=torch.float32, - device=self.device, - ) - .unsqueeze(0) - .unsqueeze(1) - .expand(self.num_envs, self.num_bodies, -1, -1) - ) - - def _init_metrics(self) -> None: - """Allocate per-env tracking metric buffers.""" - self.metrics["anchor_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["anchor_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["joint_pos_error"] = torch.zeros(self.num_envs, device=self.device) - for side in ("left", "right"): - self.metrics[f"{side}_hand_wrist_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics[f"{side}_hand_wrist_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics[f"{side}_hand_finger_joints_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["object_body_position_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["object_body_wxyz_error"] = torch.zeros( - self.num_envs, device=self.device - ) - self.metrics["virtual_object_controller_scale_factor"] = torch.zeros( - self.num_envs, device=self.device - ) - - def _init_hand_data(self, cfg: TrackingCommandCfg) -> None: - """Assign retargeted hand data from motion data. Fields may be None.""" - md = self._motion_data - - # Fingertip body IDs (from cfg, robot-level) - if cfg.fingertip_body_name: - tip_ids, tip_names = self.robot.find_bodies(cfg.fingertip_body_name) - self._left_fingertip_body_ids = [ - i - for i, n in zip(tip_ids, tip_names, strict=False) - if "left" in n.lower() - ] - self._right_fingertip_body_ids = [ - i - for i, n in zip(tip_ids, tip_names, strict=False) - if "right" in n.lower() - ] - - # Finger joint IDs (from cfg, robot-level). Track names alongside IDs so - # parquet finger joint values can be reordered to IsaacLab's joint order. - self._left_finger_joint_names: list[str] = [] - self._right_finger_joint_names: list[str] = [] - if cfg.finger_joint_names: - all_fj_ids, all_fj_names = self.robot.find_joints(cfg.finger_joint_names) - for i, n in zip(all_fj_ids, all_fj_names, strict=False): - if "left" in n.lower(): - self._left_finger_joint_ids.append(i) - self._left_finger_joint_names.append(n) - elif "right" in n.lower(): - self._right_finger_joint_ids.append(i) - self._right_finger_joint_names.append(n) - - # Retargeted hand data (from parquet, may be None) - self.retargeted_left_wrist_position = md.left_wrist_position # (T, 3) - self.retargeted_left_wrist_wxyz = md.left_wrist_wxyz # (T, 4) - self.retargeted_right_wrist_position = md.right_wrist_position - self.retargeted_right_wrist_wxyz = md.right_wrist_wxyz - self.retargeted_left_hand_frames = md.left_hand_frames # (T, K, 7) - self.retargeted_right_hand_frames = md.right_hand_frames - self.retargeted_left_hand_frame_names = md.left_hand_frame_names - self.retargeted_right_hand_frame_names = md.right_hand_frame_names - - # Fingertip indices within hand frames - for side in ("left", "right"): - frame_names = getattr(self, f"retargeted_{side}_hand_frame_names") or [] - tip_body_ids = getattr(self, f"_{side}_fingertip_body_ids") - tip_body_names = ( - [self.robot.body_names[i] for i in tip_body_ids] if tip_body_ids else [] - ) - indices = [] - for tname in tip_body_names: - if tname in frame_names: - indices.append(frame_names.index(tname)) - setattr( - self, - f"_retargeted_{side}_fingertip_indices", - torch.tensor(indices, dtype=torch.long, device=self.device), - ) - - for side in ("left", "right"): - parquet_vals = getattr(md, f"{side}_finger_joints") - parquet_names = getattr(md, f"{side}_finger_joint_names") - sim_names = getattr(self, f"_{side}_finger_joint_names") - if parquet_vals is not None and parquet_names and sim_names: - reorder = [parquet_names.index(n) for n in sim_names] - setattr( - self, f"retargeted_{side}_finger_joints", parquet_vals[:, reorder] - ) - else: - setattr(self, f"retargeted_{side}_finger_joints", parquet_vals) - - # Binary per-frame contact labels. - # If a side is absent on disk, we substitute an all-zero mask of length - # `num_timesteps` so the getters stay branch-free and tensor-typed. The - # downstream `force_closure_reward` multiplies by these, so a missing - # mask silently zeros the reward term; warn loudly once per init. - left_active = md.left_hand_contact_active - right_active = md.right_hand_contact_active - missing_sides = [ - side - for side, tensor in (("left", left_active), ("right", right_active)) - if tensor is None - ] - if len(missing_sides) == 2: - raise ValueError( - "TrackingCommand: motion data is missing per-frame contact-active " - "labels for both hands. `force_closure_reward` and any other " - "consumer of `{left,right}_hand_contact_active_command` would " - "contribute 0 for the entire motion, which silently disables " - "contact-driven learning. Re-run the retargeter so " - "`left_hand_contact_active` / `right_hand_contact_active` are " - f"populated (motion_file={self.cfg.motion_file})." - ) - if missing_sides: - warnings.warn( - "TrackingCommand: motion data is missing per-frame contact-active " - f"labels for side(s) {missing_sides}. Falling back to all-zero masks; " - "`force_closure_reward` and any other consumer of " - "`{left,right}_hand_contact_active_command` will contribute 0 from " - f"this motion file ({self.cfg.motion_file}).", - stacklevel=2, - ) - zero_mask = torch.zeros(self.num_timesteps, device=self.device) - self.retargeted_left_contact_active = ( - left_active.to(self.device) if left_active is not None else zero_mask - ) - self.retargeted_right_contact_active = ( - right_active.to(self.device) if right_active is not None else zero_mask - ) - - def _init_contact_data(self) -> None: - """Load contact positions, normals, and part IDs from motion data.""" - md = self._motion_data - for side in ("left", "right"): - link_contacts = getattr(md, f"{side}_link_contact_positions") # (T, N, 4) - obj_contacts = getattr(md, f"{side}_object_contact_positions") # (T, N, 4) - link_normals = getattr( - md, f"{side}_link_contact_normals" - ) # (T, N, 4) or None - obj_normals = getattr( - md, f"{side}_object_contact_normals" - ) # (T, N, 4) or None - part_ids = getattr(md, f"{side}_object_contact_part_ids") # (T, N) or None - - if link_contacts is not None: - # Normalize link contact normals - if link_normals is not None: - norms = ( - link_normals[..., :3].norm(dim=-1, keepdim=True).clamp(min=1e-6) - ) - link_normals = link_normals.clone() - link_normals[..., :3] = link_normals[..., :3] / norms - - # Extract part IDs from 4th column if not separately provided - if part_ids is None and link_contacts.shape[-1] > 3: - part_ids = link_contacts[..., 3].long() - is_valid = ( - part_ids > 0 - if part_ids is not None - else link_contacts[..., :3].abs().sum(dim=-1) > 1e-5 - ) - has_contact = is_valid.any(dim=-1) - - setattr( - self, f"retargeted_{side}_link_contact_positions_e", link_contacts - ) - setattr( - self, f"retargeted_{side}_object_contact_positions_e", obj_contacts - ) - setattr(self, f"retargeted_{side}_link_contact_normals_e", link_normals) - setattr( - self, f"retargeted_{side}_object_contact_normals_e", obj_normals - ) - setattr(self, f"retargeted_{side}_object_contact_part_ids", part_ids) - setattr(self, f"retargeted_{side}_object_contact_is_valid", is_valid) - setattr(self, f"retargeted_{side}_object_has_contact", has_contact) - setattr(self, f"num_contacts_{side}", link_contacts.shape[1]) - setattr(self, f"num_retargeted_contacts_{side}", link_contacts.shape[1]) - else: - for attr in ( - "link_contact_positions_e", - "object_contact_positions_e", - "link_contact_normals_e", - "object_contact_normals_e", - "object_contact_part_ids", - "object_contact_is_valid", - "object_has_contact", - ): - setattr(self, f"retargeted_{side}_{attr}", None) - setattr(self, f"num_contacts_{side}", 0) - setattr(self, f"num_retargeted_contacts_{side}", 0) - - def _precompute_hand_keypoints_in_object_frame(self) -> None: - """Express hand frames and wrist poses in the contacted object's local frame. - - Stored as object-frame tensors so they can be quickly transformed - to env frame each step using the current object pose. - """ - horizon = self._object_body_pos_w.shape[0] - horizon_ids = torch.arange(horizon, device=self.device) - - for side in ("left", "right"): - frames = getattr(self, f"retargeted_{side}_hand_frames") - wrist_pos = getattr(self, f"retargeted_{side}_wrist_position") - wrist_wxyz = getattr(self, f"retargeted_{side}_wrist_wxyz") - part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids", None) - - part_ids_per_hand = torch.ones( - horizon, dtype=torch.int64, device=self.device - ) - if part_ids is not None: - T_ids = min(horizon, part_ids.shape[0]) - last_contact_part_id = torch.ones( - (), dtype=torch.int64, device=self.device - ) - for horizon_idx in range(T_ids - 1, -1, -1): - hand_contact_part_id = part_ids[horizon_idx].mode().values - part_ids_per_hand[horizon_idx] = ( - hand_contact_part_id - if hand_contact_part_id > 0 - else last_contact_part_id - ) - last_contact_part_id = part_ids_per_hand[horizon_idx] - part_ids_per_hand = part_ids_per_hand.clamp(min=1, max=self.num_bodies) - setattr( - self, - f"retargeted_{side}_object_contact_part_ids_per_hand", - part_ids_per_hand, - ) - - contact_body_position = self._object_body_pos_w[ - horizon_ids, part_ids_per_hand - 1 - ] - contact_body_wxyz = self._object_body_quat_w[ - horizon_ids, part_ids_per_hand - 1 - ] - - if frames is not None and wrist_pos is not None: - T_frames = min(frames.shape[0], horizon) - - # Hand frame positions → object frame - frame_pos = frames[:T_frames, :, :3] # (T, K, 3) - obj_pos_exp = contact_body_position[:T_frames].unsqueeze(1) - obj_quat_exp = contact_body_wxyz[:T_frames].unsqueeze(1) - frame_pos_o = math_utils.quat_apply_inverse( - obj_quat_exp.expand_as(frame_pos[..., :1].expand(-1, -1, 4)), - frame_pos - obj_pos_exp, - ) - setattr(self, f"retargeted_{side}_hand_frame_positions_o", frame_pos_o) - - if frames.shape[-1] >= 7: - frame_wxyz_o = math_utils.quat_mul( - math_utils.quat_conjugate( - obj_quat_exp.expand(-1, frame_pos.shape[1], -1) - ), - frames[:T_frames, :, 3:7], - ) - setattr( - self, - f"retargeted_{side}_hand_frame_wxyz_o", - frame_wxyz_o, - ) - - # Wrist pose → object frame - T_wrist = min(wrist_pos.shape[0], horizon) - wrist_pos_o = math_utils.quat_apply_inverse( - contact_body_wxyz[:T_wrist], - wrist_pos[:T_wrist] - contact_body_position[:T_wrist], - ) - wrist_wxyz_o = math_utils.quat_mul( - math_utils.quat_conjugate(contact_body_wxyz[:T_wrist]), - wrist_wxyz[:T_wrist], - ) - setattr(self, f"retargeted_{side}_wrist_position_o", wrist_pos_o) - setattr(self, f"retargeted_{side}_wrist_wxyz_o", wrist_wxyz_o) - - def _precompute_contact_positions_in_object_frame(self) -> None: - """Transform contact positions and normals into per-body object/COM frames.""" - object_o_t_com = torch.cat( - [obj.data.body_com_pose_b for obj in self.objects], dim=1 - ).float() - object_o_p_com = object_o_t_com[..., :3].mean(dim=0) - object_o_q_com = object_o_t_com[0, :, 3:7] - - for side in ("left", "right"): - obj_contacts = getattr( - self, f"retargeted_{side}_object_contact_positions_e" - ) - link_normals = getattr(self, f"retargeted_{side}_link_contact_normals_e") - contact_part_ids = getattr( - self, f"retargeted_{side}_object_contact_part_ids", None - ) - - if obj_contacts is not None: - T_c = min(obj_contacts.shape[0], self._object_body_pos_w.shape[0]) - contact_pos = obj_contacts[:T_c, :, :3] - horizon_ids = torch.arange(T_c, device=self.device) - if contact_part_ids is None: - if obj_contacts.shape[-1] > 3: - contact_part_ids = obj_contacts[..., 3].long() - else: - contact_part_ids = torch.ones( - obj_contacts.shape[:2], - dtype=torch.long, - device=self.device, - ) - is_valid = getattr(self, f"retargeted_{side}_object_contact_is_valid")[ - :T_c - ] - - contact_pos_o = torch.zeros_like(contact_pos) - contact_pos_com = torch.zeros_like(contact_pos) - normals_o = None - normals_com = None - if link_normals is not None: - normals = link_normals[:T_c, :, :3] - normals_o = torch.zeros_like(normals) - normals_com = torch.zeros_like(normals) - - for link_idx in range(contact_pos.shape[1]): - part_id = (contact_part_ids[:T_c, link_idx] - 1).clamp( - min=0, max=self.num_bodies - 1 - ) - object_e_p_o = self._object_body_pos_w[horizon_ids, part_id] - object_e_q_o = self._object_body_quat_w[horizon_ids, part_id] - - contact_pos_o[:, link_idx] = math_utils.quat_apply_inverse( - object_e_q_o, - contact_pos[:, link_idx] - object_e_p_o, - ) - - _object_o_p_com = object_o_p_com[part_id] - _object_o_q_com = object_o_q_com[part_id] - contact_pos_com[:, link_idx], _ = ( - math_utils.subtract_frame_transforms( - _object_o_p_com, - _object_o_q_com, - contact_pos_o[:, link_idx], - q02=None, - ) - ) - - if normals_o is not None and normals_com is not None: - normals_o[:, link_idx] = math_utils.quat_apply_inverse( - object_e_q_o, - normals[:, link_idx], - ) - normals_com[:, link_idx], _ = ( - math_utils.subtract_frame_transforms( - torch.zeros_like(_object_o_p_com), - _object_o_q_com, - normals_o[:, link_idx], - q02=None, - ) - ) - - contact_pos_o.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - contact_pos_com.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - setattr( - self, - f"retargeted_{side}_object_contact_positions_com", - contact_pos_com, - ) - setattr( - self, - f"retargeted_{side}_object_contact_positions_o", - contact_pos_o, - ) - - if normals_o is not None and normals_com is not None: - normals_o.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - normals_com.masked_fill_(~is_valid.unsqueeze(-1), 0.0) - setattr( - self, f"retargeted_{side}_link_contact_normals_o", normals_o - ) - setattr( - self, f"retargeted_{side}_link_contact_normals_com", normals_com - ) - - def _init_wrench_data(self, cfg: TrackingCommandCfg) -> None: - """Set up wrench computation buffers: basis, friction cone, mesh radius.""" - md = self._motion_data - self.object_mesh_radius = list(md.object_mesh_radius or [0.05]) - self.object_mesh_radius = self.object_mesh_radius[: self.num_bodies] - if len(self.object_mesh_radius) < self.num_bodies: - self.object_mesh_radius = self.object_mesh_radius + [ - self.object_mesh_radius[-1] - ] * (self.num_bodies - len(self.object_mesh_radius)) - self.retargeted_horizon = min( - self._object_body_pos_w.shape[0], - ( - getattr( - self, "retargeted_left_link_contact_positions_e", torch.empty(0) - ).shape[0] - if getattr(self, "retargeted_left_link_contact_positions_e", None) - is not None - else 0 - ), - ) - - self.wrench_space_bases = torch.cat( - [ - sample_wrench_space_basis_scaled( - cfg.num_wrench_space_basis_samples, rc=1.0, device=self.device - ).unsqueeze(0) - for _ in self.object_mesh_radius - ], - dim=0, - ) - - theta = torch.linspace( - 0, 2 * torch.pi, steps=cfg.num_friction_cone_edges + 1, device=self.device - )[:-1] - self.friction_cone_edge_cosines = torch.cos(theta).view(1, -1, 1) - self.friction_cone_edge_sines = torch.sin(theta).view(1, -1, 1) - - # Buffers for runtime wrench supports - self.left_contact_wrench_supports = torch.zeros( - self.num_envs, - self.num_bodies, - cfg.num_wrench_space_basis_samples, - device=self.device, - ) - self.right_contact_wrench_supports = torch.zeros( - self.num_envs, - self.num_bodies, - cfg.num_wrench_space_basis_samples, - device=self.device, - ) - self._tensors_dirty = True - - def _precompute_contact_wrench_support_values(self) -> None: - """Precompute wrench supports for all timesteps from retargeted contacts.""" - cfg = self.cfg - T = self.retargeted_horizon - if T == 0: - return - - t_idx = torch.arange(T, device=self.device)[:, None] - - for side in ("left", "right"): - num_contacts = getattr(self, f"num_retargeted_contacts_{side}", 0) - if num_contacts == 0: - continue - - c_idx = torch.arange(num_contacts, device=self.device)[None, :] - contact_pos_com = getattr( - self, f"retargeted_{side}_object_contact_positions_com" - ) - contact_normals_com = getattr( - self, f"retargeted_{side}_link_contact_normals_com", None - ) - - if contact_pos_com is None or contact_normals_com is None: - continue - - # Expand to (T, num_bodies, num_contacts, 3) - pos_expanded = torch.zeros( - T, self.num_bodies, num_contacts, 3, device=self.device - ) - normals_expanded = torch.zeros( - T, self.num_bodies, num_contacts, 3, device=self.device - ) - - part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids") - if part_ids is not None: - part_ids_clamped = (part_ids[:T] - 1).clamp( - min=0, max=self.num_bodies - 1 - ) - else: - part_ids_clamped = torch.zeros( - T, num_contacts, dtype=torch.long, device=self.device - ) - - is_valid = getattr(self, f"retargeted_{side}_object_contact_is_valid")[ - :T - ].unsqueeze(-1) - pos_expanded[t_idx, part_ids_clamped, c_idx] = ( - contact_pos_com[:T] * is_valid - ) - normals_expanded[t_idx, part_ids_clamped, c_idx] = ( - contact_normals_com[:T] * is_valid - ) - - # Compute wrench supports per body - supports = torch.zeros( - T, - self.num_bodies, - cfg.num_wrench_space_basis_samples, - device=self.device, - ) - for body_idx, body_radius in enumerate(self.object_mesh_radius): - wrench_space = compute_wrench_space( - contact_points=pos_expanded[:, body_idx], - contact_normals=normals_expanded[:, body_idx], - cos_t=self.friction_cone_edge_cosines, - sin_t=self.friction_cone_edge_sines, - rc=body_radius, - friction_coefficients=cfg.friction_coefficients, - ) - supports[:, body_idx] = compute_wrench_space_support_function( - wrench_space=wrench_space, - basis=self.wrench_space_bases[body_idx], - ) - - setattr(self, f"retargeted_{side}_contact_wrench_supports", supports) - - # ------------------------------------------------------------------ - # Properties: current simulation state - # ------------------------------------------------------------------ - - @property - def robot_anchor_pos_w(self) -> torch.Tensor: - """Current anchor (pelvis) position in world frame. (E, 3).""" - return self.robot.data.body_pos_w[:, self._anchor_body_id] - - @property - def robot_anchor_quat_w(self) -> torch.Tensor: - """Current anchor (pelvis) quaternion in world frame. (E, 4).""" - return self.robot.data.body_quat_w[:, self._anchor_body_id] - - @property - def robot_joint_pos(self) -> torch.Tensor: - """Current tracked joint positions. (E, J).""" - return self.robot.data.joint_pos[:, self._tracked_joint_ids] - - @property - def robot_joint_vel(self) -> torch.Tensor: - """Current tracked joint velocities. (E, J).""" - return self.robot.data.joint_vel[:, self._tracked_joint_ids] - - @property - def robot_ee_pos_w(self) -> torch.Tensor: - """Current EE positions in world frame. (E, num_ee, 3).""" - if hasattr(self, "ee_link_ids") and self.ee_link_ids: - return self.robot.data.body_pos_w[:, self.ee_link_ids] - return torch.zeros(self.num_envs, 0, 3, device=self.device) - - @property - def robot_ee_quat_w(self) -> torch.Tensor: - """Current EE quaternions in world frame. (E, num_ee, 4).""" - if hasattr(self, "ee_link_ids") and self.ee_link_ids: - return self.robot.data.body_quat_w[:, self.ee_link_ids] - return torch.zeros(self.num_envs, 0, 4, device=self.device) - - @property - def object_pos_w(self) -> torch.Tensor: - """Current object position in world frame. (E, 3).""" - return self.object.data.root_pos_w - - @property - def object_quat_w(self) -> torch.Tensor: - """Current object quaternion in world frame. (E, 4).""" - return self.object.data.root_quat_w - - @property - def encoder_mode(self) -> torch.Tensor: - """SONIC encoder mode flags. (E, 4).""" - return self._encoder_mode - - @property - def timestep_counter(self) -> torch.Tensor: - """Alias for timestep (backward compat). (E,).""" - return self.timestep - - # ------------------------------------------------------------------ - # Properties: command targets (indexed by timestep) - # ------------------------------------------------------------------ - - @property - def future_timesteps(self) -> torch.Tensor: - """Clamped future frame indices per env. (E, num_future_frames).""" - future_timesteps = torch.clamp( - self.timestep[:, None] + self._future_frame_offsets[None, :], - 0, - self.num_timesteps - 1, - ) - return torch.minimum(future_timesteps, self.trajectory_end_timestep[:, None]) - - def update_action_history(self, actions: torch.Tensor) -> None: - """Push current processed actions into the history buffer.""" - if self._action_history is None: - self._action_history = torch.zeros( - self.num_envs, - self._action_history_len, - actions.shape[-1], - device=self.device, - ) - self._action_history = torch.roll(self._action_history, -1, dims=1) - self._action_history[:, -1] = actions - - @property - def action_history(self) -> torch.Tensor: - """(num_envs, history_len * action_dim) flattened past actions.""" - if self._action_history is None: - # First call — allocate from action term's processed action dim - action_dim = self._env.action_manager.get_term( - "joint_pos" - ).processed_actions.shape[-1] - self._action_history = torch.zeros( - self.num_envs, self._action_history_len, action_dim, device=self.device - ) - return self._action_history.reshape(self.num_envs, -1) - - @property - def _spread_blend_factor(self) -> torch.Tensor: - """Per-env blend: 1.0 at reset, anneals to 0.0 before VOC decay starts.""" - if self.cfg.reset_shoulder_spread == 0.0 or self.cfg.reset_freeze_steps == 0: - return torch.zeros(self.num_envs, device=self.device) - anneal_steps = max(self.cfg.reset_freeze_steps - self.cfg.voc_decay_steps, 1) - return (1.0 - self.steps_since_last_reset.float() / anneal_steps).clamp(min=0.0) - - @property - def _spread_offset_blended(self) -> torch.Tensor: - """Per-env blended spread offset. (num_envs, num_joints).""" - return self._spread_joint_offset * self._spread_blend_factor.unsqueeze(-1) - - @property - def command(self) -> torch.Tensor: - """Current command tensor (joint position targets).""" - return self.command_joint_pos - - @property - def command_anchor_pos_w(self) -> torch.Tensor: - """Target anchor position in world frame. (E, 3).""" - return self.root_pos_w[self.timestep] + self._env_origins - - @property - def command_anchor_quat_w(self) -> torch.Tensor: - """Target anchor quaternion in world frame. (E, 4).""" - return math_utils.quat_unique(self.root_quat_w[self.timestep]) - - @property - def command_joint_pos(self) -> torch.Tensor: - """Target joint positions with spread offset. (E, J).""" - return self.joint_pos[self.timestep] + self._spread_offset_blended - - @property - def command_object_pos_w(self) -> torch.Tensor: - """Target object position in world frame. (E, 3).""" - return self._object_pos_w[self.timestep] + self._env_origins - - @property - def command_object_quat_w(self) -> torch.Tensor: - """Target object quaternion in world frame. (E, 4).""" - return math_utils.quat_unique(self._object_quat_w[self.timestep]) - - @property - def command_ee_pos_w(self) -> torch.Tensor: - """Target EE positions in world frame. (E, num_ee, 3).""" - if hasattr(self, "ee_pos_w"): - return self.ee_pos_w[self.timestep] + self._env_origins.unsqueeze(1) - return torch.zeros(self.num_envs, 0, 3, device=self.device) - - @property - def command_ee_quat_w(self) -> torch.Tensor: - """Target EE quaternions in world frame. (E, num_ee, 4).""" - ee_quat = getattr(self, "ee_quat_w", None) - if ee_quat is not None: - return math_utils.quat_unique(ee_quat[self.timestep]) - return torch.zeros(self.num_envs, 0, 4, device=self.device) - - # Multi-future variants - @property - def command_joint_pos_multi_future(self) -> torch.Tensor: - """Future joint position targets with spread. (E, F, J).""" - return self.joint_pos[ - self.future_timesteps - ] + self._spread_offset_blended.unsqueeze(1) - - @property - def command_joint_vel_multi_future(self) -> torch.Tensor: - """Future joint velocity targets. (E, F, J).""" - return self.joint_vel[self.future_timesteps] - - @property - def command_anchor_pos_w_multi_future(self) -> torch.Tensor: - """Future anchor positions in world frame. (E, F, 3).""" - return self.root_pos_w[self.future_timesteps] + self._env_origins.unsqueeze(1) - - @property - def command_ee_pos_w_multi_future(self) -> torch.Tensor: - """Future EE positions in world frame. (E, F, num_ee, 3).""" - if hasattr(self, "ee_pos_w"): - return self.ee_pos_w[self.future_timesteps] + self._env_origins.unsqueeze( - 1 - ).unsqueeze(1) - return torch.zeros( - self.num_envs, self.num_future_frames, 0, 3, device=self.device - ) - - @property - def command_ee_quat_w_multi_future(self) -> torch.Tensor: - """Future EE quaternions in world frame. (E, F, num_ee, 4).""" - ee_quat = getattr(self, "ee_quat_w", None) - if ee_quat is not None: - return math_utils.quat_unique(ee_quat[self.future_timesteps]) - return torch.zeros( - self.num_envs, self.num_future_frames, 0, 4, device=self.device - ) - - @property - def command_object_pos_w_multi_future(self) -> torch.Tensor: - """Future object positions in world frame. (E, F, 3).""" - return self._object_pos_w[self.future_timesteps] + self._env_origins.unsqueeze( - 1 - ) - - @property - def command_anchor_rot_diff_l_multi_future(self) -> torch.Tensor: - """(E, F, 6) future root rotation differences in 6D rotation format.""" - future_quat = self.root_quat_w[self.future_timesteps] # (E, F, 4) - current_quat = self.root_quat_w[self.timestep] # (E, 4) - # Compute relative rotation: q_diff = q_future * q_current^-1 - q_inv = math_utils.quat_conjugate(current_quat).unsqueeze(1) # (E, 1, 4) - q_diff = math_utils.quat_mul(future_quat, q_inv.expand_as(future_quat)) - # Convert to 6D rotation (first two columns of rotation matrix) - rot_mat = math_utils.matrix_from_quat(q_diff) # (E, F, 3, 3) - return rot_mat[..., :2].reshape(self.num_envs, self.num_future_frames, 6) - - @property - def command_anchor_z_multi_future(self) -> torch.Tensor: - """(E, F) future root Z positions.""" - return self.root_pos_w[self.future_timesteps][..., 2] # (E, F) - - @property - def command_multi_future(self) -> torch.Tensor: - """Future joint pos + vel concatenated. (E, F, 2*J).""" - return torch.cat( - [self.command_joint_pos_multi_future, self.command_joint_vel_multi_future], - dim=-1, - ) - - # ------------------------------------------------------------------ - # Properties: hand keypoints (object-frame → env-frame per step) - # ------------------------------------------------------------------ - - @property - def left_hand_wrist_pose_command_e(self) -> torch.Tensor: - """(E, 7) left wrist [pos(3), wxyz(4)] in env frame.""" - return self._wrist_command_e("left") - - @property - def right_hand_wrist_pose_command_e(self) -> torch.Tensor: - """(E, 7) right wrist [pos(3), wxyz(4)] in env frame.""" - return self._wrist_command_e("right") - - @property - def left_hand_wrist_position_e(self) -> torch.Tensor: - """(E, 3) current left wrist position in env frame.""" - if self._left_wrist_body_id is not None: - return ( - self.robot.data.body_pos_w[:, self._left_wrist_body_id] - - self._env_origins - ) - return torch.zeros(self.num_envs, 3, device=self.device) - - @property - def right_hand_wrist_position_e(self) -> torch.Tensor: - """(E, 3) current right wrist position in env frame.""" - if self._right_wrist_body_id is not None: - return ( - self.robot.data.body_pos_w[:, self._right_wrist_body_id] - - self._env_origins - ) - return torch.zeros(self.num_envs, 3, device=self.device) - - @property - def left_hand_wrist_wxyz_e(self) -> torch.Tensor: - """(E, 4) current left wrist quaternion.""" - if self._left_wrist_body_id is not None: - return math_utils.quat_unique( - self.robot.data.body_quat_w[:, self._left_wrist_body_id] - ) - quat = torch.zeros(self.num_envs, 4, device=self.device) - quat[:, 0] = 1.0 - return quat - - @property - def right_hand_wrist_wxyz_e(self) -> torch.Tensor: - """(E, 4) current right wrist quaternion.""" - if self._right_wrist_body_id is not None: - return math_utils.quat_unique( - self.robot.data.body_quat_w[:, self._right_wrist_body_id] - ) - quat = torch.zeros(self.num_envs, 4, device=self.device) - quat[:, 0] = 1.0 - return quat - - def get_command_contact_object_position_orientation( - self, side: str - ) -> tuple[torch.Tensor, torch.Tensor]: - """Get the current contacted object body pose for a hand side.""" - contact_part_id = self.get_command_contact_part_id(side) - object_position = self.object_position_e[self.all_env_ids, contact_part_id] - object_orientation = self.object_orientation_e[ - self.all_env_ids, contact_part_id - ] - return object_position, object_orientation - - @property - def left_hand_fingertip_position_command_e(self) -> torch.Tensor: - """(E, K, 3) left fingertip command positions in env frame.""" - return self._fingertip_command_e("left") - - @property - def right_hand_fingertip_position_command_e(self) -> torch.Tensor: - """(E, K, 3) right fingertip command positions in env frame.""" - return self._fingertip_command_e("right") - - @property - def left_hand_fingertip_position_e(self) -> torch.Tensor: - """(E, K, 3) current left fingertip positions in env frame.""" - if self._left_fingertip_body_ids: - return self.robot.data.body_pos_w[ - :, self._left_fingertip_body_ids - ] - self._env_origins.unsqueeze(1) - return torch.zeros(self.num_envs, 0, 3, device=self.device) - - @property - def right_hand_fingertip_position_e(self) -> torch.Tensor: - """(E, K, 3) current right fingertip positions in env frame.""" - if self._right_fingertip_body_ids: - return self.robot.data.body_pos_w[ - :, self._right_fingertip_body_ids - ] - self._env_origins.unsqueeze(1) - return torch.zeros(self.num_envs, 0, 3, device=self.device) - - # ------------------------------------------------------------------ - # Properties: contact tracking - # ------------------------------------------------------------------ - - @property - def left_hand_object_contact_command_positions_e(self) -> torch.Tensor: - """(E, N, 3) left hand target contact positions in env frame.""" - return self._contact_command_e("left") - - @property - def right_hand_object_contact_command_positions_e(self) -> torch.Tensor: - """(E, N, 3) right hand target contact positions in env frame.""" - return self._contact_command_e("right") - - @property - def left_hand_object_contact_positions_e(self) -> torch.Tensor: - """(E, B, N, 3) live left hand contact positions in env frame.""" - return self._live_contact_positions_e("left") - - @property - def right_hand_object_contact_positions_e(self) -> torch.Tensor: - """(E, B, N, 3) live right hand contact positions in env frame.""" - return self._live_contact_positions_e("right") - - @property - def left_hand_object_contact_positions_w(self) -> torch.Tensor: - """(E, B, N, 3) live left hand contact positions in world frame.""" - return self._live_contact_positions_w("left") - - @property - def right_hand_object_contact_positions_w(self) -> torch.Tensor: - """(E, B, N, 3) live right hand contact positions in world frame.""" - return self._live_contact_positions_w("right") - - @property - def left_hand_object_contact_forces_w(self) -> torch.Tensor: - """(E, H, B, N, 3) left hand contact force history in world frame.""" - return self._live_contact_forces_w("left") - - @property - def right_hand_object_contact_forces_w(self) -> torch.Tensor: - """(E, H, B, N, 3) right hand contact force history in world frame.""" - return self._live_contact_forces_w("right") - - # ------------------------------------------------------------------ - # Properties: VOC - # ------------------------------------------------------------------ - - @property - def object_position_e(self) -> torch.Tensor: - """Object body positions in env frame. (E, B, 3) over all object bodies.""" - object_position_w = torch.cat( - [obj.data.body_link_pos_w for obj in self.objects], dim=1 - ) - return (object_position_w - self._env_origins.unsqueeze(1)).float() - - @property - def object_orientation_e(self) -> torch.Tensor: - """Object body quaternions in env frame. (E, B, 4) over all object bodies.""" - return torch.cat( - [obj.data.body_link_quat_w for obj in self.objects], dim=1 - ).float() - - @property - def object_body_position_command_e(self) -> torch.Tensor: - """(E, B, 3) command object body positions in env frame.""" - return self._object_body_pos_w[self.timestep] - - @property - def object_body_wxyz_command_e(self) -> torch.Tensor: - """(E, B, 4) command object body quaternions in env frame.""" - return math_utils.quat_unique(self._object_body_quat_w[self.timestep]) - - @property - def object_body_ids(self) -> torch.Tensor: - """Object body indices. (B,).""" - return torch.arange(self.num_bodies, device=self.device) - - @property - def object_com_position_and_wxyz_w(self) -> torch.Tensor: - """(E, num_bodies, 7) object COM state.""" - return torch.cat( - [obj.data.body_com_state_w[..., :7] for obj in self.objects], dim=1 - ).float() - - # ------------------------------------------------------------------ - # Properties: finger joints - # ------------------------------------------------------------------ - - @property - def left_hand_finger_joint_pos(self) -> torch.Tensor: - """Current left finger joint positions. (E, J_left).""" - if self._left_finger_joint_ids: - return self.robot.data.joint_pos[:, self._left_finger_joint_ids] - return torch.zeros(self.num_envs, 0, device=self.device) - - @property - def right_hand_finger_joint_pos(self) -> torch.Tensor: - """Current right finger joint positions. (E, J_right).""" - if self._right_finger_joint_ids: - return self.robot.data.joint_pos[:, self._right_finger_joint_ids] - return torch.zeros(self.num_envs, 0, device=self.device) - - @property - def left_hand_finger_joint_pos_command(self) -> torch.Tensor: - """Target left finger joint positions from retargeting. (E, J_left).""" - if self.retargeted_left_finger_joints is not None: - t = self.timestep.clamp(max=self.retargeted_left_finger_joints.shape[0] - 1) - return self.retargeted_left_finger_joints[t] - return torch.zeros(self.num_envs, 0, device=self.device) - - @property - def right_hand_finger_joint_pos_command(self) -> torch.Tensor: - """Target right finger joint positions from retargeting. (E, J_right).""" - if self.retargeted_right_finger_joints is not None: - t = self.timestep.clamp( - max=self.retargeted_right_finger_joints.shape[0] - 1 - ) - return self.retargeted_right_finger_joints[t] - return torch.zeros(self.num_envs, 0, device=self.device) - - # ------------------------------------------------------------------ - # Properties: contact positions + normals - # ------------------------------------------------------------------ - - @property - def left_hand_object_contact_command_positions_and_normals_e(self) -> torch.Tensor: - """(E, N, 6) contact command positions + normals in env frame.""" - return self._contact_command_pos_normals_e("left") - - @property - def right_hand_object_contact_command_positions_and_normals_e(self) -> torch.Tensor: - """(E, N, 6) right hand contact command positions + normals in env frame.""" - return self._contact_command_pos_normals_e("right") - - def get_command_contact_part_id(self, side: str) -> torch.Tensor: - """Get dominant contact body index per env. (E,) — 0-indexed.""" - per_hand = getattr( - self, f"retargeted_{side}_object_contact_part_ids_per_hand", None - ) - if per_hand is not None: - t = self.timestep.clamp(max=per_hand.shape[0] - 1) - return (per_hand[t] - 1).clamp(min=0, max=self.num_bodies - 1) - - part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids") - if part_ids is not None: - t = self.timestep.clamp(max=part_ids.shape[0] - 1) - # Mode across contact links, then clamp to valid body range - per_frame = part_ids[t] # (E, N) - dominant = per_frame.mode(dim=-1).values - 1 # 1-indexed → 0-indexed - return dominant.clamp(min=0, max=self.num_bodies - 1) - return torch.zeros(self.num_envs, dtype=torch.long, device=self.device) - - # ------------------------------------------------------------------ - # Properties: wrench - # ------------------------------------------------------------------ - - @property - def left_hand_contact_wrench_supports_command(self) -> torch.Tensor: - """(E, num_bodies, num_basis) precomputed wrench supports from retargeted data.""" - supports = getattr(self, "retargeted_left_contact_wrench_supports", None) - if supports is not None: - t = self.timestep.clamp(max=supports.shape[0] - 1) - return supports[t] - return torch.zeros( - self.num_envs, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - - @property - def right_hand_contact_wrench_supports_command(self) -> torch.Tensor: - """(E, num_bodies, num_basis) precomputed right hand wrench supports.""" - supports = getattr(self, "retargeted_right_contact_wrench_supports", None) - if supports is not None: - t = self.timestep.clamp(max=supports.shape[0] - 1) - return supports[t] - return torch.zeros( - self.num_envs, - self.num_bodies, - self.cfg.num_wrench_space_basis_samples, - device=self.device, - ) - - @property - def left_hand_contact_wrench_supports(self) -> torch.Tensor: - """(E, num_bodies, num_basis) live wrench supports from sim contacts.""" - self.refresh_tensors() - return self.left_contact_wrench_supports - - @property - def right_hand_contact_wrench_supports(self) -> torch.Tensor: - """(E, num_bodies, num_basis) live right hand wrench supports from sim.""" - self.refresh_tensors() - return self.right_contact_wrench_supports - - @property - def left_hand_contact_active_command(self) -> torch.Tensor: - """(E,) binary contact label for left hand at current timestep.""" - t = self.timestep.clamp(max=self.retargeted_left_contact_active.shape[0] - 1) - return self.retargeted_left_contact_active[t] - - @property - def right_hand_contact_active_command(self) -> torch.Tensor: - """(E,) binary contact label for right hand at current timestep.""" - t = self.timestep.clamp(max=self.retargeted_right_contact_active.shape[0] - 1) - return self.retargeted_right_contact_active[t] - - # ------------------------------------------------------------------ - # Command lifecycle - # ------------------------------------------------------------------ - - def _resample_command(self, env_ids: Sequence[int]) -> None: - """Reset state for specified environments on episode reset. - - Note: timestep and reset_timestep are set by the reset event (events.py), - which runs BEFORE _resample_command. Do not overwrite them here. - Similarly, _spread_joint_offset is set by the reset event — not cleared here. - """ - self.steps_since_last_reset[env_ids] = 0 - if self.cfg.voc_decay_steps > 0: - self.virtual_object_controller_scale_factor_per_env[env_ids] = ( - self.cfg.voc_reset_scale - ) - else: - self.virtual_object_controller_scale_factor_per_env[env_ids] = ( - self.virtual_object_controller_scale_factor.item() - ) - if self._action_history is not None: - self._action_history[env_ids] = 0.0 - if hasattr(self, "_tensors_dirty"): - self._tensors_dirty = True - - def _update_command(self) -> None: - """Advance timestep and decay VOC.""" - self.steps_since_last_reset += 1 - - # VOC decay - voc_decay_steps = self.cfg.voc_decay_steps - if voc_decay_steps > 0: - decay_start = max(self.cfg.reset_freeze_steps - voc_decay_steps, 0) - steps_into_decay = ( - self.steps_since_last_reset.float() - decay_start - ).clamp(min=0.0) - progress = (steps_into_decay / voc_decay_steps).clamp(max=1.0) - target = self.virtual_object_controller_scale_factor.item() - start = self.cfg.voc_reset_scale - decayed = (start + (target - start) * progress).clamp(min=0.0) - self.virtual_object_controller_scale_factor_per_env[:] = decayed.view( - self.num_envs, 1 - ) - - # Advance timestep — frozen until max(freeze_steps, voc_decay_steps) - effective_freeze = max(self.cfg.reset_freeze_steps, voc_decay_steps) - if effective_freeze > 0: - past_freeze = self.steps_since_last_reset > effective_freeze - self.timestep[past_freeze] += 1 - else: - self.timestep += 1 - self.timestep.clamp_(0, self.num_timesteps - 1) - self.timestep.copy_(torch.minimum(self.timestep, self.trajectory_end_timestep)) - if hasattr(self, "_tensors_dirty"): - self._tensors_dirty = True - - def _update_metrics(self) -> None: - """Track per-step errors for logging.""" - self.metrics["anchor_position_error"] = torch.norm( - self.robot_anchor_pos_w - self.command_anchor_pos_w, dim=-1 - ) - self.metrics["anchor_wxyz_error"] = math_utils.quat_error_magnitude( - self.robot_anchor_quat_w, self.command_anchor_quat_w - ) - self.metrics["joint_pos_error"] = torch.norm( - self.robot_joint_pos - self.command_joint_pos, dim=-1 - ) - - self.metrics["object_body_position_error"] = torch.norm( - self.object_position_e - self.object_body_position_command_e, - dim=-1, - ).mean(dim=-1) - self.metrics["object_body_wxyz_error"] = math_utils.quat_error_magnitude( - self.object_orientation_e, - self.object_body_wxyz_command_e, - ).mean(dim=-1) - - wrist_metrics = ( - ( - "left", - self._left_wrist_body_id, - self.left_hand_wrist_position_e, - self.left_hand_wrist_pose_command_e, - ), - ( - "right", - self._right_wrist_body_id, - self.right_hand_wrist_position_e, - self.right_hand_wrist_pose_command_e, - ), - ) - for ( - side, - wrist_body_id, - wrist_position_e, - wrist_pose_command_e, - ) in wrist_metrics: - self.metrics[f"{side}_hand_wrist_position_error"] = torch.norm( - wrist_position_e - wrist_pose_command_e[:, :3], - dim=-1, - ) - if wrist_body_id is not None: - self.metrics[f"{side}_hand_wrist_wxyz_error"] = ( - math_utils.quat_error_magnitude( - self.robot.data.body_quat_w[:, wrist_body_id], - wrist_pose_command_e[:, 3:], - ) - ) - else: - self.metrics[f"{side}_hand_wrist_wxyz_error"].zero_() - - self.metrics["left_hand_finger_joints_error"] = torch.norm( - self.left_hand_finger_joint_pos - self.left_hand_finger_joint_pos_command, - dim=-1, - ) - self.metrics["right_hand_finger_joints_error"] = torch.norm( - self.right_hand_finger_joint_pos - self.right_hand_finger_joint_pos_command, - dim=-1, - ) - self.metrics["virtual_object_controller_scale_factor"] = ( - self.virtual_object_controller_scale_factor_per_env.squeeze(-1) - ) - - def _set_debug_vis_impl(self, debug_vis: bool = True) -> None: - """Enable/disable debug visualization of tracking targets.""" - if debug_vis: - if not hasattr(self, "goal_pose_visualizer"): - cfg = self.cfg.pose_visualizer_cfg.replace( - prim_path="/Visuals/Command/goal_marker" - ) - self.goal_pose_visualizer = VisualizationMarkers(cfg) - if not hasattr(self, "object_pose_visualizer"): - cfg = self.cfg.pose_visualizer_cfg.replace( - prim_path="/Visuals/Command/object_marker" - ) - self.object_pose_visualizer = VisualizationMarkers(cfg) - if not hasattr(self, "actual_object_pose_visualizer"): - cfg = self.cfg.pose_visualizer_cfg.replace( - prim_path="/Visuals/Command/actual_object_marker" - ) - cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - self.actual_object_pose_visualizer = VisualizationMarkers(cfg) - ee_names = getattr(self, "ee_link_names", []) or [] - if not getattr(self, "ee_pose_visualizer", None): - self.ee_pose_visualizer = {} - for ee_name in ee_names: - cfg = self.cfg.pose_visualizer_cfg.replace( - prim_path=f"/Visuals/Command/ee_marker_{ee_name}" - ) - cfg.markers["frame"].scale = (0.10, 0.10, 0.10) - self.ee_pose_visualizer[ee_name] = VisualizationMarkers(cfg) - if not getattr(self, "wrist_pose_visualizer", None): - self.wrist_pose_visualizer = {} - for ee_name in ee_names: - cfg = self.cfg.pose_visualizer_cfg.replace( - prim_path=f"/Visuals/Current/wrist_marker_{ee_name}" - ) - cfg.markers["frame"].scale = (0.07, 0.07, 0.07) - self.wrist_pose_visualizer[ee_name] = VisualizationMarkers(cfg) - self.goal_pose_visualizer.set_visibility(True) - self.object_pose_visualizer.set_visibility(True) - self.actual_object_pose_visualizer.set_visibility(True) - for v in self.ee_pose_visualizer.values(): - v.set_visibility(True) - for v in self.wrist_pose_visualizer.values(): - v.set_visibility(True) - elif hasattr(self, "goal_pose_visualizer"): - self.goal_pose_visualizer.set_visibility(False) - self.object_pose_visualizer.set_visibility(False) - self.actual_object_pose_visualizer.set_visibility(False) - for v in self.ee_pose_visualizer.values(): - v.set_visibility(False) - for v in self.wrist_pose_visualizer.values(): - v.set_visibility(False) - - def _debug_vis_callback(self, event: object) -> None: - """Visualize tracking targets: anchor, object, EE.""" - if not hasattr(self, "goal_pose_visualizer"): - return - self.goal_pose_visualizer.visualize( - translations=self.command_anchor_pos_w, - orientations=self.command_anchor_quat_w, - ) - self.object_pose_visualizer.visualize( - translations=self.command_object_pos_w, - orientations=self.command_object_quat_w, - ) - self.actual_object_pose_visualizer.visualize( - translations=self.object_pos_w, - orientations=self.object_quat_w, - ) - ee_names = getattr(self, "ee_link_names", []) or [] - for i, ee_name in enumerate(ee_names): - if ee_name in self.ee_pose_visualizer: - self.ee_pose_visualizer[ee_name].visualize( - translations=self.command_ee_pos_w[:, i], - orientations=self.command_ee_quat_w[:, i], - ) - if ee_name in self.wrist_pose_visualizer and self.ee_link_ids: - wrist_pos = self.robot.data.body_pos_w[:, self.ee_link_ids[i]] - wrist_quat = self.robot.data.body_quat_w[:, self.ee_link_ids[i]] - self.wrist_pose_visualizer[ee_name].visualize( - translations=wrist_pos, - orientations=wrist_quat, - ) - - # ------------------------------------------------------------------ - # Internal helpers for object-frame → env-frame transforms - # ------------------------------------------------------------------ - - def _wrist_command_e(self, side: str) -> torch.Tensor: - """Wrist command in env frame from object-frame precomputation.""" - wrist_pos_o = getattr(self, f"retargeted_{side}_wrist_position_o", None) - wrist_wxyz_o = getattr(self, f"retargeted_{side}_wrist_wxyz_o", None) - if wrist_pos_o is None or wrist_wxyz_o is None: - return torch.zeros(self.num_envs, 7, device=self.device) - - t = self.timestep.clamp(max=wrist_pos_o.shape[0] - 1) - obj_pos, obj_quat = self.get_command_contact_object_position_orientation(side) - - pos_o = wrist_pos_o[t] # (E, 3) - wxyz_o = wrist_wxyz_o[t] # (E, 4) - - pos_e = math_utils.quat_apply(obj_quat, pos_o) + obj_pos - wxyz_e = math_utils.quat_mul(obj_quat, wxyz_o) - return torch.cat([pos_e, wxyz_e], dim=-1) - - def _fingertip_command_e(self, side: str) -> torch.Tensor: - """Fingertip command positions in env frame.""" - frame_pos_o = getattr(self, f"retargeted_{side}_hand_frame_positions_o", None) - tip_indices = getattr(self, f"_retargeted_{side}_fingertip_indices", None) - if frame_pos_o is None or tip_indices is None or len(tip_indices) == 0: - return torch.zeros(self.num_envs, 0, 3, device=self.device) - - t = self.timestep.clamp(max=frame_pos_o.shape[0] - 1) - tips_o = frame_pos_o[t][:, tip_indices] # (E, K, 3) - - obj_pos, obj_quat = self.get_command_contact_object_position_orientation(side) - obj_pos = obj_pos.unsqueeze(1) - obj_quat = obj_quat.unsqueeze(1).expand_as(tips_o[..., :1].expand(-1, -1, 4)) - return math_utils.quat_apply(obj_quat, tips_o) + obj_pos - - def _contact_command_e(self, side: str) -> torch.Tensor: - """Contact command positions in env frame.""" - contact_o = getattr(self, f"retargeted_{side}_object_contact_positions_o", None) - if contact_o is None: - n = getattr(self, f"num_contacts_{side}", 0) - return torch.zeros(self.num_envs, n, 3, device=self.device) - - t = self.timestep.clamp(max=contact_o.shape[0] - 1) - contacts = contact_o[t] # (E, N, 3) - - object_position, object_orientation = self._contact_object_pose_e( - side, contacts.shape[1] - ) - pos_e = math_utils.quat_apply(object_orientation, contacts) + object_position - valid = getattr(self, f"retargeted_{side}_object_contact_is_valid", None) - if valid is not None: - valid_t = valid[t] - pos_e.masked_fill_(~valid_t.unsqueeze(-1), 0.0) - return pos_e - - def _contact_object_pose_e( - self, side: str, num_contacts: int - ) -> tuple[torch.Tensor, torch.Tensor]: - """Current object body poses for each contact link. Shapes: (E, N, 3/4).""" - part_ids = getattr(self, f"retargeted_{side}_object_contact_part_ids", None) - if part_ids is not None: - t = self.timestep.clamp(max=part_ids.shape[0] - 1) - contact_part_id = (part_ids[t] - 1).clamp(min=0, max=self.num_bodies - 1) - else: - contact_part_id = torch.zeros( - self.num_envs, num_contacts, dtype=torch.long, device=self.device - ) - object_position = torch.gather( - self.object_position_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 3), - ) - object_orientation = torch.gather( - self.object_orientation_e, - dim=1, - index=contact_part_id.unsqueeze(-1).expand(-1, -1, 4), - ) - return object_position, object_orientation - - def _live_contact_positions_w(self, side: str) -> torch.Tensor: - """(E, B, N, 3) contact positions from per-body sensors.""" - sensors = getattr(self, f"object_to_{side}_hand_contact_sensors", []) - num_contacts = getattr(self, f"num_robot_contacts_{side}", 0) - if sensors: - raw = torch.nan_to_num( - torch.cat([s.data.contact_pos_w for s in sensors], dim=1), nan=0.0 - ) - return raw.view(self.num_envs, self.num_bodies, num_contacts, 3) - return torch.zeros(self.num_envs, 0, 0, 3, device=self.device) - - def _live_contact_positions_e(self, side: str) -> torch.Tensor: - pos_w = self._live_contact_positions_w(side) - if pos_w.numel() > 0: - return pos_w - self._env_origins.view(self.num_envs, 1, 1, 3) - return pos_w - - def _live_contact_forces_w(self, side: str) -> torch.Tensor: - """(E, H, B, N, 3) contact force history from per-body sensors.""" - sensors = getattr(self, f"object_to_{side}_hand_contact_sensors", []) - num_contacts = getattr(self, f"num_robot_contacts_{side}", 0) - if sensors: - H = sensors[0].data.force_matrix_w_history.shape[1] - raw = torch.nan_to_num( - torch.cat([s.data.force_matrix_w_history for s in sensors], dim=2), - nan=0.0, - ) - return raw.view(self.num_envs, H, self.num_bodies, num_contacts, 3) - return torch.zeros(self.num_envs, 0, 0, 0, 3, device=self.device) - - def _contact_command_pos_normals_e(self, side: str) -> torch.Tensor: - """Combined contact positions + normals in env frame. (E, N, 6).""" - contact_o = getattr(self, f"retargeted_{side}_object_contact_positions_o", None) - normals_com = getattr(self, f"retargeted_{side}_link_contact_normals_com", None) - if contact_o is None: - n = getattr(self, f"num_contacts_{side}", 0) - return torch.zeros(self.num_envs, n, 6, device=self.device) - - t = self.timestep.clamp(max=contact_o.shape[0] - 1) - object_position, object_orientation = self._contact_object_pose_e( - side, contact_o.shape[1] - ) - - pos_e = ( - math_utils.quat_apply(object_orientation, contact_o[t]) + object_position - ) - if normals_com is not None: - normals_e = math_utils.quat_apply(object_orientation, normals_com[t]) - else: - normals_e = torch.zeros_like(pos_e) - valid = getattr(self, f"retargeted_{side}_object_contact_is_valid", None) - if valid is not None: - valid_t = valid[t] - pos_e.masked_fill_(~valid_t.unsqueeze(-1), 0.0) - normals_e.masked_fill_(~valid_t.unsqueeze(-1), 0.0) - return torch.cat([pos_e, normals_e], dim=-1) - - def refresh_tensors(self) -> None: - """Refresh shared contact/wrench tensors once per sim step.""" - if not self._tensors_dirty: - return - self._tensors_dirty = False - - self._compute_contact_wrench_supports("right") - self._compute_contact_wrench_supports("left") - - self._cached_right_wrench_cmd_supports = ( - self.right_hand_contact_wrench_supports_command - ) - self._cached_left_wrench_cmd_supports = ( - self.left_hand_contact_wrench_supports_command - ) - self._cached_right_wrench_cmd_active = ( - self._cached_right_wrench_cmd_supports > 1e-3 - ) - self._cached_left_wrench_cmd_active = ( - self._cached_left_wrench_cmd_supports > 1e-3 - ) - self._cached_right_wrench_cur_active = self.right_contact_wrench_supports > 1e-3 - self._cached_left_wrench_cur_active = self.left_contact_wrench_supports > 1e-3 - self._cached_right_wrench_cmd_active_per_body = ( - self._cached_right_wrench_cmd_active.any(dim=-1) - ) - self._cached_left_wrench_cmd_active_per_body = ( - self._cached_left_wrench_cmd_active.any(dim=-1) - ) - self._cached_right_wrench_cur_active_per_body = ( - self._cached_right_wrench_cur_active.any(dim=-1) - ) - self._cached_left_wrench_cur_active_per_body = ( - self._cached_left_wrench_cur_active.any(dim=-1) - ) - - def _compute_contact_wrench_supports(self, side: str) -> None: - """Fill live wrench supports in place for one hand.""" - sensors = getattr(self, f"object_to_{side}_hand_contact_sensors", []) - supports = getattr(self, f"{side}_contact_wrench_supports") - supports.zero_() - if not sensors or not hasattr(self, "wrench_space_bases"): - return - - num_contacts = getattr(self, f"num_robot_contacts_{side}", 0) - if num_contacts == 0: - return - - contact_pos_w = self._live_contact_positions_w(side) # (E, B, N, 3) - contact_forces_w = self._live_contact_forces_w(side) # (E, H, B, N, 3) - - obj_com = self.object_com_position_and_wxyz_w # (E, B, 7) - object_com_position_w = obj_com[..., :3].unsqueeze(2) - object_com_orientation_w = obj_com[..., 3:7].unsqueeze(2) - - contact_positions_com, contact_normals_com = wrench_preprocess_jit( - contact_positions_w=contact_pos_w, - contact_forces_first_hist_w=contact_forces_w[:, 0], - object_com_position_w=object_com_position_w, - object_com_orientation_w=object_com_orientation_w, - num_envs=self.num_envs, - num_bodies=self.num_bodies, - num_robot_contacts=num_contacts, - ) - friction_coefficients = float(self.cfg.friction_coefficients) - for body_idx, body_radius in enumerate(self.object_mesh_radius): - supports[:, body_idx] = wrench_support_one_body_jit( - contact_points=contact_positions_com[:, body_idx], - contact_normals=contact_normals_com[:, body_idx], - cos_t=self.friction_cone_edge_cosines, - sin_t=self.friction_cone_edge_sines, - basis=self.wrench_space_bases[body_idx], - rc=float(body_radius), - friction_coefficients=friction_coefficients, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command_cfg.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command_cfg.py deleted file mode 100644 index 8905db74..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_command_cfg.py +++ /dev/null @@ -1,167 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from isaaclab.managers import CommandTermCfg -from isaaclab.markers import VisualizationMarkersCfg -from isaaclab.markers.config import FRAME_MARKER_CFG -from isaaclab.utils import configclass - -from .tracking_command import TrackingCommand - - -@configclass -class TrackingCommandCfg(CommandTermCfg): - """Configuration for the whole-body tracking command term. - - Loads motion data from a `motion_v1` parquet containing robot root pose, - joint positions, EE targets, hand keypoints, contacts, and object - trajectories. See `robotic_grounding.motion_schema` for the schema. - """ - - class_type: type = TrackingCommand - - # Asset names in the scene - asset_name: str = "robot" - """Name of the robot articulation asset in the scene.""" - - object_name: str = "object" - """Name of the primary object asset in the scene.""" - - object_body_names: list[str] = [] - """Scene attribute names for object assets. Set by apply_scene_config().""" - - # Motion data source - motion_file: str = "" - """Path to the `motion_v1` parquet file or Hive-partitioned directory.""" - - motion_start_frame: int = 0 - """First frame of the source motion to use (inclusive). 0 = beginning.""" - - motion_end_frame: int = -1 - """One past the last frame to use. ``-1`` means end of sequence. - - Stored as int (not ``int | None``) because Isaac Lab's - ``update_class_from_dict`` infers the expected override type from the - current value, so a ``None`` default would reject int Hydra overrides. - """ - - # Offsets applied to loaded trajectories - object_pos_offset: list[float] = [0.0, 0.0, 0.0] - """Offset applied to object position trajectory.""" - - robot_anchor_pos_offset: list[float] = [0.0, 0.0, 0.0] - """Offset applied to robot root position trajectory.""" - - # Timing - dt: float = 0.02 - """Time step of the motion data (50 Hz = 0.02s).""" - - # Body tracking - anchor_body_name: str = "pelvis" - """Name of the root/anchor body on the robot.""" - - joint_names: list[str] = [".*"] - """Joint name patterns to track (IsaacLab ordering).""" - - file_joint_names: list[str] | None = None - """Joint names in the motion file (for reordering). Auto-detected from parquet.""" - - # EE tracking - ee_link_names: list[str] = [] - """EE link names on the robot for tracking. Auto-detected from parquet.""" - - # Future frame observations (for SONIC encoder) - num_future_frames: int = 10 - """Number of future frames in encoder observations.""" - - dt_future_frames: float = 0.1 - """Time step between future frames.""" - - # Hand skeleton configuration - fingertip_body_name: str = "" - """Regex for fingertip body names on the robot (e.g. '.*_DP' for Sharpa).""" - - finger_joint_names: list[str] = [] - """Names of finger joints on the robot.""" - - # Contact and hand-object configuration - hand_contact_bodies: list[str] = [] - """Hand body patterns for contact sensors (e.g. ['.*_hand_palm_link', ...]). - Set by env cfg. Used by apply_scene_config to create contact sensors.""" - - hand_frame_target_bodies: list[str] = [] - """Left and right hand body names for FrameTransformer targets. - E.g. ['left_hand_palm_link', 'right_hand_palm_link']. Set by env cfg.""" - - object_contact_sensor_names: list[str] = [] - """Scene sensor names. Populated by apply_scene_config, not set manually.""" - - # Virtual Object Control (VOC) curriculum - initial_virtual_object_control_curriculum_scale: float = 0.0 - """VOC scale at episode start. 0.0 = disabled.""" - - voc_decay_steps: int = 0 - """Steps to decay VOC from voc_reset_scale toward curriculum scale.""" - - voc_reset_scale: float = 1.0 - """VOC scale to set on reset (before decay).""" - - # Action history - action_history_length: int = 3 - """Number of past processed actions to store for policy observation.""" - - # Reset behavior - always_reset_to_first_frame: bool = False - """Force reset to frame 0 (for eval). Overrides trajectory_time_index.""" - - reset_freeze_steps: int = 0 - """Steps after reset to freeze the timestep counter (robot settling time).""" - - reset_shoulder_spread: float = 0.0 - """Shoulder yaw spread (radians) applied on reset when freeze_steps > 0. - Widens arms + zeros fingers to prevent hand-object collision during settling.""" - - reset_root_height_min: float | None = None - """Clamp root Z to this minimum on reset. None = no clamp. Use 0.795 for G1.""" - - reset_yaw_only: bool = False - """Zero roll/pitch from root quaternion on reset, keeping only yaw.""" - - target_fps: float | None = None - """Resample V2P data to this FPS. If None, uses data as-is.""" - - # Wrench computation - num_wrench_space_basis_samples: int = 512 - """Number of basis samples for wrench space support function.""" - - num_friction_cone_edges: int = 8 - """Number of edges in the friction cone approximation.""" - - friction_coefficients: float = 0.1 - """Friction coefficient for contact wrench computation.""" - - # Visualization - debug_vis: bool = True - """Whether to enable debug visualization of tracking targets.""" - - pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace( - prim_path="/Visuals/Command/pose_marker" - ) - pose_visualizer_cfg.markers["frame"].scale = (0.15, 0.15, 0.15) - - resampling_time_range: tuple[float, float] = (1e6, 1e6) - """Disable Isaac Lab's mid-episode command resampling. - - The base ``CommandManager`` decrements a per-env ``time_left`` by ``dt`` - each step and calls ``_resample`` whenever ``time_left <= 0``. For - trajectory tracking we never want a mid-episode resample: episode - termination (timeout or tracking-error) already drives a reset through - ``command_manager.reset(env_ids)``, and the reset event picks a fresh - frame in ``[0, num_timesteps)`` of the (trimmed) motion. - - Setting both bounds to ``1e6`` means each call to ``_resample`` writes - ``time_left = 1e6`` (≈ 11 days of sim time), so the time-based path - cannot re-fire during any realistic ``episode_length_s``. Do not lower - this without also overriding ``compute()``. - """ diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_utils.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_utils.py deleted file mode 100644 index ef84a488..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/commands/tracking_utils.py +++ /dev/null @@ -1,71 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Motion data loading for tracking commands. - -Thin shim over `robotic_grounding.motion_schema`: resolves a `cfg.motion_file` -(file or Hive partition directory) into a populated `MotionData`, then -resolves the EE body IDs on the live robot. - -The `MotionData` dataclass is re-exported for backward compatibility with -callers that used to import it from this module. -""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from robotic_grounding.motion_schema import ( - SINGLE_ROBOT, - MotionData, - load_motion_data_parquet, -) - -if TYPE_CHECKING: - import torch - from isaaclab.assets import Articulation - - from .tracking_command_cfg import TrackingCommandCfg - - -__all__ = ["MotionData", "load_motion_data"] - - -def load_motion_data( - cfg: TrackingCommandCfg, - robot: Articulation, - device: torch.device, -) -> MotionData: - """Load motion data from a `motion_v1` parquet and resolve robot body IDs. - - Args: - cfg: The tracking command configuration (uses `cfg.motion_file`). - robot: The live robot articulation, used to resolve EE body IDs. - device: Target torch device for tensors. - - Returns: - A populated `MotionData` with `ee_link_ids` resolved against the robot. - - Raises: - ValueError: If the loaded file is not a `single_robot` motion. The - whole-body `TrackingCommand` indexes whole-body joint state and - cannot consume `dual_hand` files; those should be loaded through - the dual-hand command term or `replay_data.load_replay_trajectory`. - """ - md = load_motion_data_parquet(cfg.motion_file, device=str(device)) - - if md.motion_kind != SINGLE_ROBOT: - raise ValueError( - f"TrackingCommand requires motion_kind={SINGLE_ROBOT!r} but the " - f"file at {cfg.motion_file!r} has motion_kind={md.motion_kind!r}. " - f"Dual-hand motions belong to `dual_hands_object_tracking_command` " - f"or `replay_data.load_replay_trajectory`." - ) - - end_frame = None if cfg.motion_end_frame < 0 else cfg.motion_end_frame - md = md.trim(cfg.motion_start_frame, end_frame) - - if md.ee_link_names: - ee_link_ids, _ = robot.find_bodies(list(md.ee_link_names)) - md.ee_link_ids = ee_link_ids - - return md diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/__init__.py deleted file mode 100644 index 421d3f1d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/__init__.py +++ /dev/null @@ -1,7 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Curriculum terms for whole-body tracking tasks.""" - -from .curriculum import WholeBodyFixedTimestepVOCCurriculum - -__all__ = ["WholeBodyFixedTimestepVOCCurriculum"] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/curriculum.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/curriculum.py deleted file mode 100644 index 65f7c93d..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/curriculum/curriculum.py +++ /dev/null @@ -1,100 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Curriculum terms for whole-body tracking tasks. - -Currently exposes a fixed-timestep curriculum that mutates the global VOC -target on the whole-body :class:`TrackingCommand` (see -``tracking_command.py``). The per-env applied scale follows the global target -through the existing per-step decay in ``TrackingCommand._update_command``. - -Reward-weight scheduling is intentionally out of scope here to avoid -hard-coding reward parameter names like the v2p hand variant does. -""" - -from __future__ import annotations - -import bisect -from collections.abc import Sequence - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import CurriculumTermCfg, ManagerTermBase - - -class WholeBodyFixedTimestepVOCCurriculum(ManagerTermBase): - """Fixed-timestep curriculum that schedules the VOC target scale. - - The schedule is expressed in PPO update indices and converted to env - steps via ``num_steps_per_env``. The active stage is selected with - ``bisect_right`` against ``env.common_step_counter``; stage transitions - overwrite the global VOC scale on the configured command term. - """ - - def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv) -> None: - """Resolve the command term and pre-compute env-step thresholds.""" - super().__init__(cfg, env) - - self._command = env.command_manager.get_term(cfg.params["command_name"]) - - num_steps_per_env = int(cfg.params["num_steps_per_env"]) - timestep_schedule: list[int] = list(cfg.params["timestep_schedule"]) - scale_schedule: list[float] = list( - cfg.params["virtual_object_control_scale_factor"] - ) - if len(timestep_schedule) != len(scale_schedule): - raise ValueError( - "timestep_schedule and virtual_object_control_scale_factor must have the " - f"same length, got {len(timestep_schedule)} and {len(scale_schedule)}." - ) - - self._env_step_schedule = [ - ppo_step * num_steps_per_env for ppo_step in timestep_schedule - ] - self._scale_schedule = scale_schedule - self._last_schedule_index: int = -1 - - def __call__( - self, - env: ManagerBasedRLEnv, - env_ids: Sequence[int], - command_name: str, - num_steps_per_env: int, - timestep_schedule: list[int], - virtual_object_control_scale_factor: list[float], - ) -> torch.Tensor: - """Set the global VOC target if the active stage changed. - - Args: - env: The RL environment instance. - env_ids: Environment ids being reset (unused; the schedule is global). - command_name: Name of the command term to mutate. - num_steps_per_env: PPO ``num_steps_per_env`` from the agent config. - timestep_schedule: PPO update indices at which the VOC scale changes. - virtual_object_control_scale_factor: VOC target scale per stage. - - Returns: - The current global VOC scale tensor on the command term. - """ - del env_ids, command_name # Bound at __init__; ignored here. - del num_steps_per_env, timestep_schedule # Pre-computed in __init__. - del virtual_object_control_scale_factor # Pre-computed in __init__. - - sim_step_counter = self._env.common_step_counter - current_schedule_index = min( - bisect.bisect_right(self._env_step_schedule, sim_step_counter) - 1, - len(self._scale_schedule) - 1, - ) - current_schedule_index = max(current_schedule_index, 0) - - if current_schedule_index == self._last_schedule_index: - return self._command.virtual_object_controller_scale_factor - - self._last_schedule_index = current_schedule_index - new_scale = float(self._scale_schedule[current_schedule_index]) - - # Preserve dtype/device of the existing (1,) tensor. - self._command.virtual_object_controller_scale_factor = ( - 0.0 * self._command.virtual_object_controller_scale_factor + new_scale - ) - - return self._command.virtual_object_controller_scale_factor diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/README.md deleted file mode 100644 index 14809ad7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Events - -## `events.py` - -| Function | Mode | Description | -|----------|------|-------------| -| `reset_robot_to_trajectory_start` | reset | Reset robot + object to a trajectory frame. Configurable via `TrackingCommandCfg`. | - -### Reset behavior (all configurable): - -- **Frame selection**: random within `trajectory_time_index`, or frame 0 if `always_reset_to_first_frame=True` -- **Root Z clamp**: `reset_root_height_min` prevents ground penetration at random start frames -- **Yaw-only**: `reset_yaw_only` zeros roll/pitch from root quaternion -- **Shoulder spread**: `reset_shoulder_spread > 0` + `freeze_steps > 0` widens arms and zeros fingers during freeze. Offset stored on command for smooth annealing via `_spread_blend_factor` -- **Object reset**: teleports object to trajectory frame. `RigidObject` guard (TODO: ArticulatedObject support) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/__init__.py deleted file mode 100644 index 56a3bd32..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Events for SONIC environment.""" - -from .events import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/events.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/events.py deleted file mode 100644 index f09ad68c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/events/events.py +++ /dev/null @@ -1,161 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.assets import Articulation -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import SceneEntityCfg - -from robotic_grounding.tasks.v2p_whole_body.mdp.commands import TrackingCommand - - -def reset_robot_to_trajectory_start( - env: ManagerBasedRLEnv, - env_ids: torch.Tensor, - command_name: str = "motion", - asset_cfg: SceneEntityCfg | None = None, - trajectory_time_index: tuple[int, int] = (0, 0), -) -> None: - """Reset robot and object to a frame in the motion trajectory. - - Frame-window selection: - - trajectory_time_index is the inclusive command window [start, end]. - - always_reset_to_first_frame: reset to the window start. - - Otherwise: random reset inside the window. - - The timestep termination ends the episode at the window end. - - Post-processing (all configurable on TrackingCommandCfg): - - Optional root Z clamp (reset_root_height_min) - - Optional yaw-only root quaternion (reset_yaw_only) - - Optional shoulder spread + finger zeroing during freeze (reset_shoulder_spread) - """ - if asset_cfg is None: - asset_cfg = SceneEntityCfg("robot") - command: TrackingCommand = env.command_manager.get_term(command_name) - robot: Articulation = env.scene[asset_cfg.name] - - # --- Frame selection --- - low = max(0, int(trajectory_time_index[0])) - high = min(command.num_timesteps - 1, int(trajectory_time_index[1])) - high = max(low, high) - if command.cfg.always_reset_to_first_frame: - reset_ts = torch.full( - (len(env_ids),), low, dtype=torch.int32, device=env.device - ) - else: - reset_ts = torch.randint( - low, - high + 1, - (len(env_ids),), - dtype=torch.int32, - device=env.device, - ) - - command.timestep[env_ids] = reset_ts - command.reset_timestep[env_ids] = reset_ts - command.trajectory_end_timestep[env_ids] = high - command.tracking_lengths[env_ids] = (high - reset_ts + 1).clamp(min=1) - - # --- Read trajectory frame --- - initial_root_pos = command.root_pos_w[reset_ts].clone() - initial_root_quat = command.root_quat_w[reset_ts].clone() - initial_joint_pos = command.joint_pos[reset_ts].clone() - - # --- Root Z clamp --- - if command.cfg.reset_root_height_min is not None: - initial_root_pos[:, 2] = initial_root_pos[:, 2].clamp( - min=command.cfg.reset_root_height_min - ) - - # --- Yaw-only root quaternion --- - if command.cfg.reset_yaw_only: - w = initial_root_quat[:, 0] - x = initial_root_quat[:, 1] - y = initial_root_quat[:, 2] - z = initial_root_quat[:, 3] - yaw = torch.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) - half_yaw = yaw * 0.5 - initial_root_quat = torch.stack( - [ - torch.cos(half_yaw), - torch.zeros_like(half_yaw), - torch.zeros_like(half_yaw), - torch.sin(half_yaw), - ], - dim=-1, - ) - - # --- Shoulder spread + finger zeroing --- - freeze_steps = command.cfg.reset_freeze_steps - shoulder_spread = command.cfg.reset_shoulder_spread - if shoulder_spread > 0.0 and freeze_steps > 0: - spread_offset = torch.zeros_like(initial_joint_pos) - joint_names = robot.joint_names - for i, name in enumerate(joint_names): - if name == "left_shoulder_yaw_joint": - spread_offset[:, i] = shoulder_spread - elif name == "right_shoulder_yaw_joint": - spread_offset[:, i] = -shoulder_spread - elif any( - finger in name - for finger in ("thumb", "index", "middle", "ring", "pinky") - ): - spread_offset[:, i] = -initial_joint_pos[:, i] - initial_joint_pos = initial_joint_pos + spread_offset - command._spread_joint_offset[env_ids] = spread_offset - else: - command._spread_joint_offset[env_ids] = 0.0 - - # --- Write to sim --- - root_pos_w = initial_root_pos + env.scene.env_origins[env_ids] - robot.write_root_pose_to_sim( - torch.cat([root_pos_w, initial_root_quat], dim=-1), env_ids=env_ids - ) - robot.write_root_velocity_to_sim( - torch.zeros_like(robot.data.root_vel_w[env_ids]), env_ids=env_ids - ) - robot.write_joint_state_to_sim( - initial_joint_pos, - torch.zeros_like(robot.data.joint_vel[env_ids]), - env_ids=env_ids, - ) - - # --- Reset objects --- - scene_objects = getattr(command, "objects", None) or [ - env.scene[command.cfg.object_name] - ] - object_pose = torch.cat( - [ - command._object_body_pos_w[reset_ts] + env.scene.env_origins[env_ids, None], - command._object_body_quat_w[reset_ts], - ], - dim=-1, - ) - object_velocity = torch.zeros( - object_pose.shape[0], - object_pose.shape[1], - 6, - device=object_pose.device, - dtype=object_pose.dtype, - ) - object_joint_pos = None - if command.retargeted_object_articulation.numel() > 0: - object_joint_pos = command.retargeted_object_articulation[reset_ts] - if object_joint_pos.dim() == 1: - object_joint_pos = object_joint_pos.unsqueeze(-1) - - for object_idx, scene_object in enumerate(scene_objects): - scene_object.write_root_pose_to_sim(object_pose[:, object_idx], env_ids=env_ids) - scene_object.write_root_velocity_to_sim( - object_velocity[:, object_idx], env_ids=env_ids - ) - if isinstance(scene_object, Articulation) and object_joint_pos is not None: - if len(scene_objects) > 1: - raise NotImplementedError( - "Multi-object trajectory reset currently supports separate " - "RigidObject assets or a single Articulation asset." - ) - scene_object.write_joint_state_to_sim( - object_joint_pos, - torch.zeros_like(scene_object.data.joint_vel[env_ids]), - env_ids=env_ids, - ) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/README.md deleted file mode 100644 index 82dc498e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/README.md +++ /dev/null @@ -1,66 +0,0 @@ -# Observations - -## `policy_observations.py` — RL policy inputs - -### Body-frame state (egocentric) - -| Function | Shape | Frame | Description | -|----------|-------|-------|-------------| -| `wrist_position_b` | (E, 6) | body | Both wrists relative to pelvis [R3+L3] | -| `wrist_orientation_b` | (E, 12) | body | Both wrist orientations as 6D rotation [R6+L6] | -| `wrist_velocity_b` | (E, 6) | body | Both wrist linear velocities in pelvis frame [R3+L3] | -| `object_position_b` | (E, 3) | body | Object position relative to pelvis | -| `object_orientation_b` | (E, 6) | body | Object orientation as 6D rotation relative to pelvis | - -### Future frame deltas - -| Function | Shape | Description | -|----------|-------|-------------| -| `motion_anchor_pos_b` | (E, F*3) | Future root position deltas, flattened | -| `motion_joint_pos_delta` | (E, F*J) | Future joint position deltas from current | -| `motion_ee_pos_delta` | (E, F*L*3) | Future EE position deltas | -| `motion_ee_quat_delta` | (E, L*F*6) | Future EE rotation deltas as 6D | -| `object_pos_delta` | (E, F*3) | Future object position deltas | - -### Scene and tracking - -| Function | Shape | Description | -|----------|-------|-------------| -| `object_pose_delta_6d` | (E, 9) | Object error vs command [pos(3) + 6D rot(6)] | -| `hand_object_transform_6d` | (E, 9) | Hand-object transform [pos(3) + 6D rot(6)], distance-gated | -| `command_trajectory_progress` | (E, 1) | Normalized trajectory progress [0, 1] | -| `action_history` | (E, H*A) | Past H processed actions, flattened. Zeroed on reset | -| `contact_desired_positions_e` | (E, 2*N*3) | Target contact positions (left+right, env frame) | - -## `sonic_tokenizer_observations.py` — SONIC encoder inputs - -| Function | Shape | Description | -|----------|-------|-------------| -| `encoder_mode` | (E, 4) | One-hot encoder mode selection | -| `command_joint_pos` | (E, F*J) | Future joint positions, optionally SONIC-joints-only | -| `command_joint_vel` | (E, F*J) | Future joint velocities, optionally SONIC-joints-only | -| `motion_anchor_ori_b` | (E, F*6) | Future root orientation diffs as 6D | -| `command_z` | (E, F) | Future root Z (height) positions | -| `encoder_padding` | (E, dim) | Zero padding to match encoder input dimension | - -## `sonic_policy_observations.py` — SONIC decoder inputs - -| Function | Shape | Description | -|----------|-------|-------------| -| `joint_pos_rel` | (E, J) | Current joint positions relative to default pose | -| `joint_vel_rel` | (E, J) | Current joint velocities, optionally SONIC-only | -| `last_action` | (E, J) | Last SONIC output actions or raw RL actions | - -## `rnd_observations.py` — Hand-object and contact - -| Function | Shape | Description | -|----------|-------|-------------| -| `hand_object_transform` | (E, B*7) | Transform from hand to object (quat), distance-gated | -| `contact_force` | (E, N*3) | Mean contact forces from sensor history | - -## Shape Legend - -- **E** = num_envs, **F** = num_future_frames, **J** = num_tracked_joints -- **L** = num_ee_links, **N** = num_contact_links, **B** = num_object_bodies -- **H** = action_history_length, **A** = action_dim -- All orientations use 6D rotation (first two columns of rotation matrix) unless noted diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/__init__.py deleted file mode 100644 index 6a58d6e8..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/__init__.py +++ /dev/null @@ -1,8 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Observation functions for robotic grounding environment.""" - -from .policy_observations import * # noqa: F403 -from .rnd_observations import * # noqa: F403 -from .sonic_policy_observations import * # noqa: F403 -from .sonic_tokenizer_observations import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/policy_observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/policy_observations.py deleted file mode 100644 index a3997522..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/policy_observations.py +++ /dev/null @@ -1,440 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Observation functions for SONIC encoder/tokenizer.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING, Any - -import torch -from isaaclab.utils.math import ( - matrix_from_quat, - quat_apply_inverse, - quat_inv, - quat_mul, - subtract_frame_transforms, -) - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - -from robotic_grounding.tasks.v2p_whole_body.mdp.commands import TrackingCommand - - -def motion_anchor_pos_b( - env: ManagerBasedRLEnv, - command_name: str = "motion", - num_future_frames: int = 10, - frame: str = "absolute", -) -> torch.Tensor: - """Get future anchor positions. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - frame: ``"absolute"`` returns legacy world-frame positions. ``"relative"`` - returns deltas in the current anchor frame. - - Returns: - Future anchor positions or deltas (num_envs, num_future_frames * 3). - """ - if frame not in ("absolute", "relative"): - raise ValueError( - "motion_anchor_pos_b frame must be 'absolute' or 'relative', " - f"got {frame!r}." - ) - - command: TrackingCommand = env.command_manager.get_term(command_name) - future_pos_w = command.command_anchor_pos_w_multi_future[:, :num_future_frames, :] - if frame == "absolute": - return future_pos_w.reshape(env.num_envs, -1) - - delta_w = future_pos_w - command.robot_anchor_pos_w.unsqueeze(1) - num_frames = future_pos_w.shape[1] - anchor_quat_w = command.robot_anchor_quat_w.unsqueeze(1).expand(-1, num_frames, -1) - delta_b = quat_apply_inverse(anchor_quat_w, delta_w) - return delta_b.reshape(env.num_envs, -1) - - -def motion_joint_pos_delta( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """ - Get future joint position deltas from the robot's current joint position. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future joint position deltas (num_envs, num_future_frames * num_joints) - flattened joint position deltas - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - joint_pos_delta = command.command_joint_pos_multi_future[ - :, :num_future_frames, : - ] - command.robot_joint_pos.unsqueeze(1) - return joint_pos_delta.reshape(env.num_envs, -1) - - -def motion_ee_pos_delta( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future EE position deltas.""" - command: TrackingCommand = env.command_manager.get_term(command_name) - ee_pos_delta = command.command_ee_pos_w_multi_future[ - :, :num_future_frames, : - ] - command.robot_ee_pos_w.unsqueeze(1) - return ee_pos_delta.reshape(env.num_envs, -1) - - -def motion_ee_quat_delta( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future EE quaternion deltas as 6D rotation representation. - - Computes the rotation difference between desired EE quaternion trajectory and current EE quaternion. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future EE rotation deltas in 6D representation (num_envs, num_ee_links * num_future_frames * 6) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - - # Get reference EE quaternions: (num_envs, num_future_frames, num_ee_links, 4) - ref_ee_quat = command.command_ee_quat_w_multi_future[:, :num_future_frames, :, :] - - # Get current robot EE quaternions: (num_envs, num_ee_links, 4) - robot_ee_quat = command.robot_ee_quat_w - - num_envs = env.num_envs - num_ee_links = robot_ee_quat.shape[1] - - # Compute rotation difference: quat_inv(robot_quat) * ref_quat - robot_ee_quat_inv = quat_inv(robot_ee_quat) # (num_envs, num_ee_links, 4) - robot_ee_quat_inv = robot_ee_quat_inv.unsqueeze(1).expand( - -1, num_future_frames, -1, -1 - ) - - # Compute quaternion difference - ee_quat_diff = quat_mul( - robot_ee_quat_inv, ref_ee_quat - ) # (num_envs, num_future_frames, num_ee_links, 4) - - # Convert to 6D rotation representation (first two columns of rotation matrix) - mat = matrix_from_quat( - ee_quat_diff - ) # (num_envs, num_future_frames, num_ee_links, 3, 3) - ee_rot_6d = mat[..., :2].reshape(num_envs, num_future_frames, num_ee_links, 6) - - return ee_rot_6d.reshape(num_envs, -1) - - -def object_pos_delta( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future object position deltas. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future object position deltas (num_envs, num_future_frames * 3) - flattened object position deltas - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - object_pos_delta = command.command_object_pos_w_multi_future[ - :, :num_future_frames, : - ] - command.object_pos_w.unsqueeze(1) - return object_pos_delta.reshape(env.num_envs, -1) - - -def command_trajectory_progress( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """ - Normalized trajectory progress. - - Args: - env: The environment instance - command_name: Name of the tracking command term - - Returns: - Normalized trajectory progress (num_envs, 1) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - trajectory_progress = command.timestep.float() / max(command.num_timesteps - 1, 1) - return trajectory_progress.unsqueeze(-1) - - -def object_pose_delta( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Object body pose deltas in each current body frame. (num_envs, 7 * num_object_bodies).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - num_bodies = command.object_position_e.shape[1] - pos_delta, quat_delta = subtract_frame_transforms( - command.object_position_e.reshape(-1, 3), - command.object_orientation_e.reshape(-1, 4), - command.object_body_position_command_e.reshape(-1, 3), - command.object_body_wxyz_command_e.reshape(-1, 4), - ) - return torch.cat( - [ - pos_delta.reshape(env.num_envs, num_bodies, 3), - quat_delta.reshape(env.num_envs, num_bodies, 4), - ], - dim=-1, - ).reshape(env.num_envs, -1) - - -def wrist_position_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Wrist positions in body (pelvis) frame. (num_envs, 6) = [right(3), left(3)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - pelvis_pos_w = command.robot_anchor_pos_w - pelvis_quat_inv = quat_inv(command.robot_anchor_quat_w) - rot = matrix_from_quat(pelvis_quat_inv) - - def _to_body(pos_w: torch.Tensor) -> torch.Tensor: - delta = pos_w - pelvis_pos_w - return torch.bmm(rot, delta.unsqueeze(-1)).squeeze(-1) - - return torch.cat( - [ - _to_body(command.right_hand_wrist_position_e + env.scene.env_origins), - _to_body(command.left_hand_wrist_position_e + env.scene.env_origins), - ], - dim=-1, - ) - - -def wrist_orientation_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Wrist orientations relative to pelvis as 6D rotation. (num_envs, 12) = [right(6), left(6)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - pelvis_quat_inv = quat_inv(command.robot_anchor_quat_w) - - def _to_body_6d(wrist_quat_w: torch.Tensor) -> torch.Tensor: - rel_quat = quat_mul(pelvis_quat_inv, wrist_quat_w) - return matrix_from_quat(rel_quat)[..., :2].reshape(-1, 6) - - right_idx = 1 if command.robot_ee_quat_w.shape[1] > 1 else 0 - return torch.cat( - [ - _to_body_6d(command.robot_ee_quat_w[:, right_idx]), - _to_body_6d(command.robot_ee_quat_w[:, 0]), - ], - dim=-1, - ) - - -def wrist_velocity_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Wrist linear velocities in body (pelvis) frame. (num_envs, 6) = [right(3), left(3)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - robot = command.robot - pelvis_quat_inv = quat_inv(command.robot_anchor_quat_w) - rot = matrix_from_quat(pelvis_quat_inv) - - ee_ids = command.ee_link_ids or [] - if len(ee_ids) >= 2: - left_vel_w = robot.data.body_vel_w[:, ee_ids[0], :3] - right_vel_w = robot.data.body_vel_w[:, ee_ids[1], :3] - else: - return torch.zeros(env.num_envs, 6, device=env.device) - - right_vel_b = torch.bmm(rot, right_vel_w.unsqueeze(-1)).squeeze(-1) - left_vel_b = torch.bmm(rot, left_vel_w.unsqueeze(-1)).squeeze(-1) - return torch.cat([right_vel_b, left_vel_b], dim=-1) - - -def wrist_velocity_full_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Wrist linear+angular velocities in each wrist's own frame. (num_envs, 12) = [right(6), left(6)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - robot = command.robot - left_id = getattr(command, "_left_wrist_body_id", None) - right_id = getattr(command, "_right_wrist_body_id", None) - - def _per_wrist(body_id: int | None) -> torch.Tensor: - if body_id is None: - return torch.zeros(env.num_envs, 6, device=env.device) - quat = robot.data.body_quat_w[:, body_id] - lin_w = robot.data.body_lin_vel_w[:, body_id] - ang_w = robot.data.body_ang_vel_w[:, body_id] - return torch.cat( - [quat_apply_inverse(quat, lin_w), quat_apply_inverse(quat, ang_w)], - dim=-1, - ) - - return torch.cat([_per_wrist(right_id), _per_wrist(left_id)], dim=-1).float() - - -def object_position_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Object position in body (pelvis) frame. (num_envs, 3).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - pelvis_pos_w = command.robot_anchor_pos_w - pelvis_quat_inv = quat_inv(command.robot_anchor_quat_w) - rot = matrix_from_quat(pelvis_quat_inv) - obj_pos_w = command.object.data.root_pos_w - delta = obj_pos_w - pelvis_pos_w - return torch.bmm(rot, delta.unsqueeze(-1)).squeeze(-1) - - -def object_orientation_b( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Object orientation relative to pelvis as 6D rotation. (num_envs, 6).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - pelvis_quat_inv = quat_inv(command.robot_anchor_quat_w) - obj_quat_w = command.object.data.root_quat_w - rel_quat = quat_mul(pelvis_quat_inv, obj_quat_w) - return matrix_from_quat(rel_quat)[..., :2].reshape(env.num_envs, 6) - - -def object_pose_delta_6d( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Object pose delta (pos + 6D rot) in current object frame. Shape: (num_envs, 9).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - pos_delta, quat_delta = subtract_frame_transforms( - command.object_pos_w, - command.object_quat_w, - command.command_object_pos_w, - command.command_object_quat_w, - ) - rot_6d = matrix_from_quat(quat_delta)[..., :2].reshape(env.num_envs, 6) - return torch.cat([pos_delta, rot_6d], dim=-1) - - -def hand_object_transform_6d( - env: ManagerBasedRLEnv, frame_transform_cfg: Any, threshold: float = 10.0 -) -> torch.Tensor: - """Hand-object transform with 6D rotation. (num_envs, 9) = pos(3) + 6D rot(6). - - Zeroed when distance exceeds threshold. - """ - frame_transform = env.scene[frame_transform_cfg.name] - pos = frame_transform.data.target_pos_source # (E, 1, 3) - quat = frame_transform.data.target_quat_source # (E, 1, 4) - rot_6d = matrix_from_quat(quat)[..., :2].reshape(pos.shape[0], pos.shape[1], 6) - transform = torch.cat([pos, rot_6d], dim=-1) # (E, 1, 9) - - distance = torch.norm(pos, dim=-1) # (E, 1) - mask = (distance < threshold).unsqueeze(-1) - return torch.where(mask, transform, torch.zeros_like(transform)).reshape( - env.num_envs, -1 - ) - - -def action_history( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Past actions from the tracking command's action history buffer. - - Shape: (num_envs, history_len * action_dim). Zeroed on reset. - The action term must call command.update_action_history() each step. - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - return command.action_history - - -def contact_desired_positions_e( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Desired contact positions in env frame (left + right, flattened).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - left = command.left_hand_object_contact_command_positions_e - right = command.right_hand_object_contact_command_positions_e - return torch.cat( - [left.reshape(env.num_envs, -1), right.reshape(env.num_envs, -1)], dim=-1 - ) - - -def hand_object_reference_transform( - env: ManagerBasedRLEnv, - side: str, - command_name: str = "motion", - threshold: float = 10.0, -) -> torch.Tensor: - """Wrist pose expressed in the frame of the object body this hand tracks. - - Picks the commanded object body for this side when the scene has several - object bodies; reduces to the single object body otherwise. Zeroed when the - wrist is farther than ``threshold`` from that object. - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - if side == "left": - wrist_pos_e = command.left_hand_wrist_position_e - wrist_wxyz_e = command.left_hand_wrist_wxyz_e - elif side == "right": - wrist_pos_e = command.right_hand_wrist_position_e - wrist_wxyz_e = command.right_hand_wrist_wxyz_e - else: - raise ValueError(f"Unknown hand side: {side}") - - obj_pos_e, obj_wxyz_e = command.get_command_contact_object_position_orientation( - side - ) - pos_o, wxyz_o = subtract_frame_transforms( - obj_pos_e, - obj_wxyz_e, - wrist_pos_e, - wrist_wxyz_e, - ) - transform = torch.cat([pos_o, wxyz_o], dim=-1) - mask = (torch.norm(pos_o, dim=-1, keepdim=True) < threshold).expand_as(transform) - return torch.where(mask, transform, torch.zeros_like(transform)) - - -def wrist_position_e( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Wrist positions in env frame. (num_envs, 6) = [right(3), left(3)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_wrist_position_e, - command.left_hand_wrist_position_e, - ], - dim=-1, - ) - - -def wrist_wxyz_e(env: ManagerBasedRLEnv, command_name: str = "motion") -> torch.Tensor: - """Wrist quaternions in env frame. (num_envs, 8) = [right(4), left(4)].""" - command: TrackingCommand = env.command_manager.get_term(command_name) - return torch.cat( - [ - command.right_hand_wrist_wxyz_e, - command.left_hand_wrist_wxyz_e, - ], - dim=-1, - ) - - -def object_position_e( - env: ManagerBasedRLEnv, command_name: str = "motion" -) -> torch.Tensor: - """Object body positions in env frame. (num_envs, 3 * num_object_bodies).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - return command.object_position_e.reshape(env.num_envs, -1) - - -def object_wxyz_e(env: ManagerBasedRLEnv, command_name: str = "motion") -> torch.Tensor: - """Object body quaternions in env frame. (num_envs, 4 * num_object_bodies).""" - command: TrackingCommand = env.command_manager.get_term(command_name) - return command.object_orientation_e.reshape(env.num_envs, -1) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/rnd_observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/rnd_observations.py deleted file mode 100644 index 9c96bcab..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/rnd_observations.py +++ /dev/null @@ -1,49 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import SceneEntityCfg - - -def hand_object_transform( - env: ManagerBasedRLEnv, frame_transform_cfg: SceneEntityCfg, threshold: float = 0.10 -) -> torch.Tensor: - """ - Get the transform between the object and the hand. - - Args: - env: The environment instance - frame_transform_cfg: The frame transformer configuration - threshold: The threshold for the distance between the object and the hand - Returns: - Transform (num_envs, num_bodies, 7) - """ - frame_transform = env.scene[frame_transform_cfg.name] - pos_source = frame_transform.data.target_pos_source - quat_source = frame_transform.data.target_quat_source - transform = torch.cat([pos_source, quat_source], dim=-1) - - # check if distance between object and hand is less than threshold - distance = torch.norm(frame_transform.data.target_pos_source, dim=-1) - distance_mask = (distance < threshold).unsqueeze(-1) - filtered_transform = torch.where( - distance_mask, transform, torch.zeros_like(transform) - ) - - return filtered_transform.reshape(env.num_envs, -1) - - -def contact_force(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor: - """ - Get contact force from a contact sensor. - - Args: - env: The environment instance - sensor_cfg: The contact sensor configuration - Returns: - Contact force (num_envs, num_contact_points) - """ - contact_sensor = env.scene.sensors[sensor_cfg.name] - mean_contact_forces = contact_sensor.data.force_matrix_w_history.mean(dim=1) - - return mean_contact_forces.reshape(env.num_envs, -1) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_policy_observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_policy_observations.py deleted file mode 100644 index 4256ca2a..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_policy_observations.py +++ /dev/null @@ -1,139 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Observation functions for SONIC policy/decoder.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -import torch - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - from isaaclab.managers import SceneEntityCfg - -from robotic_grounding.tasks.v2p_whole_body.mdp.actions import SONICActionBase - - -def joint_pos( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg, - sonic_joints_only: bool = False, - action_name: str | None = None, -) -> torch.Tensor: - """Get current joint positions (absolute). - - Args: - env: The environment instance - asset_cfg: The asset configuration - sonic_joints_only: If True, filter to SONIC-controlled joints only (requires SONIC action term) - action_name: Name of the action term (required when sonic_joints_only=True) - - Returns: - Joint positions (num_envs, num_joints) - """ - asset = env.scene[asset_cfg.name] - joint_pos = asset.data.joint_pos - - if sonic_joints_only: - if action_name is None: - raise ValueError("action_name must be provided when sonic_joints_only=True") - action_term = env.action_manager.get_term(action_name) - if isinstance(action_term, SONICActionBase): - sonic_joint_ids = action_term.get_sonic_joint_ids() - joint_pos = joint_pos[:, sonic_joint_ids] - - return joint_pos - - -def joint_pos_rel( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg, - sonic_joints_only: bool = False, - action_name: str | None = None, -) -> torch.Tensor: - """Get current joint positions (relative to default). - - Args: - env: The environment instance - asset_cfg: The asset configuration - sonic_joints_only: If True, filter to SONIC-controlled joints only (requires SONIC action term) - action_name: Name of the action term (required when sonic_joints_only=True) - - Returns: - Joint positions (num_envs, num_joints) - """ - asset = env.scene[asset_cfg.name] - joint_pos = asset.data.joint_pos - asset.data.default_joint_pos - - if sonic_joints_only: - if action_name is None: - raise ValueError("action_name must be provided when sonic_joints_only=True") - action_term = env.action_manager.get_term(action_name) - if isinstance(action_term, SONICActionBase): - sonic_joint_ids = action_term.get_sonic_joint_ids() - joint_pos = joint_pos[:, sonic_joint_ids] - - return joint_pos - - -def joint_vel_rel( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg, - sonic_joints_only: bool = False, - action_name: str | None = None, -) -> torch.Tensor: - """Get current joint velocities. - - Args: - env: The environment instance - asset_cfg: The asset configuration - sonic_joints_only: If True, filter to SONIC-controlled joints only (requires SONIC action term) - action_name: Name of the action term (required when sonic_joints_only=True) - - Returns: - Joint velocities (num_envs, num_joints) - """ - asset = env.scene[asset_cfg.name] - joint_vel = asset.data.joint_vel - - if sonic_joints_only: - if action_name is None: - raise ValueError("action_name must be provided when sonic_joints_only=True") - action_term = env.action_manager.get_term(action_name) - if isinstance(action_term, SONICActionBase): - sonic_joint_ids = action_term.get_sonic_joint_ids() - joint_vel = joint_vel[:, sonic_joint_ids] - - return joint_vel - - -def last_action( - env: ManagerBasedRLEnv, - sonic_joints_only: bool = False, - action_name: str | None = None, -) -> torch.Tensor: - """Get last actions. - - Args: - env: The environment instance - sonic_joints_only: If True, return SONIC output actions only (requires SONIC action term) - If False, return raw actions from the action manager - action_name: Name of the action term (required when sonic_joints_only=True) - - Returns: - Last actions (num_envs, num_joints) - """ - if action_name is None: - action_name = "joint_pos" - - action_term = env.action_manager.get_term(action_name) - - if sonic_joints_only: - if isinstance(action_term, SONICActionBase): - return action_term.get_last_sonic_actions() - else: - # Fallback to raw actions if not a SONIC action term - return action_term.raw_actions - else: - return action_term.raw_actions diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_tokenizer_observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_tokenizer_observations.py deleted file mode 100644 index 670ff1d6..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/observations/sonic_tokenizer_observations.py +++ /dev/null @@ -1,166 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Observation functions for SONIC encoder/tokenizer.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -import torch - -if TYPE_CHECKING: - from isaaclab.envs import ManagerBasedRLEnv - -from robotic_grounding.tasks.v2p_whole_body.mdp.actions import SONICActionBase -from robotic_grounding.tasks.v2p_whole_body.mdp.commands import TrackingCommand - - -def encoder_mode(env: ManagerBasedRLEnv, command_name: str = "motion") -> torch.Tensor: - """Get encoder mode scalar index from tracking command. - - Returns: - Encoder mode selection (num_envs, 4) [0,0,0,0] - G1 mode active - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - return command.encoder_mode - - -def command_joint_pos( - env: ManagerBasedRLEnv, - command_name: str = "motion", - sonic_joints_only: bool = False, - action_name: str | None = None, - num_future_frames: int = 10, -) -> torch.Tensor: - """Get future joint positions. - - Args: - env: The environment instance - command_name: Name of the tracking command term - sonic_joints_only: If True, filter to SONIC-controlled joints only (requires SONIC action term) - action_name: Name of the action term (required when sonic_joints_only=True) - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future joint positions (num_envs, num_future_frames * num_joints) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - joint_pos = ( - command.command_joint_pos_multi_future - ) # (num_envs, num_future_frames, num_joints) - joint_pos = joint_pos[:, :num_future_frames, :] - - if sonic_joints_only: - if action_name is None: - raise ValueError("action_name must be provided when sonic_joints_only=True") - action_term = env.action_manager.get_term(action_name) - if isinstance(action_term, SONICActionBase): - sonic_joint_indices = action_term.get_sonic_joint_indices() - joint_pos = joint_pos[:, :, sonic_joint_indices] - - return joint_pos.reshape(env.num_envs, -1) - - -def command_joint_vel( - env: ManagerBasedRLEnv, - command_name: str = "motion", - sonic_joints_only: bool = False, - action_name: str | None = None, - num_future_frames: int = 10, -) -> torch.Tensor: - """Get future joint velocities. - - Args: - env: The environment instance - command_name: Name of the tracking command term - sonic_joints_only: If True, filter to SONIC-controlled joints only (requires SONIC action term) - action_name: Name of the action term (required when sonic_joints_only=True) - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future joint velocities (num_envs, num_future_frames * num_joints) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - joint_vel = ( - command.command_joint_vel_multi_future - ) # (num_envs, num_future_frames, num_joints) - joint_vel = joint_vel[:, :num_future_frames, :] - - if sonic_joints_only: - if action_name is None: - raise ValueError("action_name must be provided when sonic_joints_only=True") - action_term = env.action_manager.get_term(action_name) - if isinstance(action_term, SONICActionBase): - sonic_joint_indices = action_term.get_sonic_joint_indices() - joint_vel = joint_vel[:, :, sonic_joint_indices] - - return joint_vel.reshape(env.num_envs, -1) - - -def command_joint_pos_vel( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future joint positions and velocities. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future joint pos+vel (num_envs, num_future_frames * num_joints * 2) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - multi_future = ( - command.command_multi_future - ) # (num_envs, num_future_frames, num_joints * 2) - multi_future = multi_future[:, :num_future_frames, :] - return multi_future.reshape(env.num_envs, -1) - - -def motion_anchor_ori_b( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future orientation differences. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future orientation diffs (num_envs, num_future_frames * 6) - flattened 6D rotation - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - ori_diffs = ( - command.command_anchor_rot_diff_l_multi_future - ) # (num_envs, num_future_frames, 6) - ori_diffs = ori_diffs[:, :num_future_frames, :] - return ori_diffs.reshape(env.num_envs, -1) # Flatten - - -def command_z( - env: ManagerBasedRLEnv, command_name: str = "motion", num_future_frames: int = 10 -) -> torch.Tensor: - """Get future z (height) positions. - - Args: - env: The environment instance - command_name: Name of the tracking command term - num_future_frames: Number of future frames to include (default: 10) - - Returns: - Future z positions (num_envs, num_future_frames) - """ - command: TrackingCommand = env.command_manager.get_term(command_name) - z_positions = command.command_anchor_z_multi_future # (num_envs, num_future_frames) - return z_positions[:, :num_future_frames] - - -def encoder_padding(env: ManagerBasedRLEnv, dim: int) -> torch.Tensor: - """Zero padding for formatting tokenizer input. - - Returns: - Zero padding (num_envs, dim) - """ - return torch.zeros(env.num_envs, dim, device=env.device) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/README.md deleted file mode 100644 index cde2ecc5..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# Rewards - -## `tracking_rewards.py` — Motion tracking rewards - -| Function | Kernel | Description | -|----------|--------|-------------| -| `motion_global_anchor_position_error_exp` | exp(-err/std^2) | Root position tracking | -| `motion_global_anchor_orientation_error_exp` | exp(-err/std^2) | Root orientation tracking | -| `motion_ee_position_error_exp` | exp(-err/std^2) | EE position tracking (per-link max) | -| `motion_ee_orientation_error_exp` | exp(-err/std^2) | EE orientation tracking (per-link max) | -| `motion_joint_pos_error_exp` | exp(-err/std^2) | Joint position tracking (sum over joints) | -| `motion_object_position_error_exp` | exp(-err/std^2) | Object position tracking | -| `motion_object_orientation_error_exp` | exp(-err/std^2) | Object orientation tracking | -| `motion_object_lifted` | continuous | Height-based lifting progress | -| `motion_progress` | continuous | Trajectory progress relative to reset point | -| `motion_finger_joint_pos_gaussian_exp` | exp(-err/std^2) | Finger joint tracking from retargeted data (sum L+R, max 2.0) | -| `motion_hand_keypoints_gaussian_exp` | exp(-err/std^2) | Hand keypoint tracking from retargeted data (sum L+R, max 2.0) | -| `motion_contact_tracking_gaussian_exp` | exp(-chamfer/std^2) | Contact point Chamfer distance | -| `motion_contact_force_gaussian_exp` | exp(-err/std^2) | Contact force magnitude tracking | - -## `contact_rewards.py` — Wrench-based contact rewards - -All wrench rewards use per-body wrench space support functions computed by the tracking command. Contact data flows: sensor -> COM frame transform -> `compute_wrench_space` -> `compute_wrench_space_support_function`. - -| Function | Description | -|----------|-------------| -| `contact_wrench_support_reward` | Per-direction wrench support matching (command vs sim). Tolerance band, gated by both sides having support. | -| `unintended_contact_penalty` | Penalty for sim contact where command expects none. Binary + continuous magnitude. | -| `missed_contact_penalty` | Proportional penalty for missing expected contact directions per body. | -| `force_closure_reward` | Fraction of wrench basis directions with sim support, gated by binary contact labels. For use without retargeted contact data. | - -Note: wrench rewards currently compute per-hand independently. For bimanual grasps, both hands' contacts should be combined into a single wrench space per body (TODO). - -## `regularization_rewards.py` — Regularization - -| Function | Type | Description | -|----------|------|-------------| -| `body_acc_l2` | Class | Body linear + angular acceleration penalty (velocity history) | -| `body_ang_vel_l2` | Function | Body angular velocity L2 penalty | -| `mmd_similarity_reward` | Class | MMD between current pose distribution and expert motion dataset | diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/__init__.py deleted file mode 100644 index 427a3624..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from .regularization_rewards import * # noqa: F403 -from .tracking_rewards import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/contact_rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/contact_rewards.py deleted file mode 100644 index 04cbe1ae..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/contact_rewards.py +++ /dev/null @@ -1,139 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Contact and wrench support rewards for whole-body manipulation. - -These rewards use wrench space support functions computed by the tracking command -from retargeted contact data. The wrench space basis and friction cones are -precomputed at init; live supports are computed per step from contact sensor data. - -Utility functions (compute_wrench_space, sample_wrench_space_basis_scaled, etc.) -are in tasks.v2p.mdp.utils. -""" - -import torch -from isaaclab.envs import ManagerBasedEnv - -from robotic_grounding.tasks.v2p.mdp.utils_jit import ( - contact_wrench_support_reward_jit, - missed_contact_penalty_jit, - unintended_contact_penalty_jit, -) - - -def contact_wrench_support_reward( - env: ManagerBasedEnv, - command_name: str = "motion", - tolerance: float = 0.1, - var: float = 0.1, -) -> torch.Tensor: - """Per-direction contact wrench support reward. - - Evaluates each wrench basis direction independently. Directions where both - command and sim have support contribute exp(-loss/var), averaged over - active command directions per body, then over bodies and hands. - - Returns continuous value in [0, 1]. - """ - command = env.command_manager.get_term(command_name) - command.refresh_tensors() - return contact_wrench_support_reward_jit( - right_cmd_active=command._cached_right_wrench_cmd_active, - right_cur_active=command._cached_right_wrench_cur_active, - left_cmd_active=command._cached_left_wrench_cmd_active, - left_cur_active=command._cached_left_wrench_cur_active, - right_cmd_active_per_body=command._cached_right_wrench_cmd_active_per_body, - left_cmd_active_per_body=command._cached_left_wrench_cmd_active_per_body, - right_cmd_supports=command._cached_right_wrench_cmd_supports, - right_cur_supports=command.right_contact_wrench_supports, - left_cmd_supports=command._cached_left_wrench_cmd_supports, - left_cur_supports=command.left_contact_wrench_supports, - tolerance=tolerance, - var=var, - ) - - -def unintended_contact_penalty( - env: ManagerBasedEnv, - command_name: str = "motion", -) -> torch.Tensor: - """Penalty for contact where command expects none but sim has contact. - - Combines binary indicator (unintended contact exists) with continuous - penalty proportional to the unintended wrench support magnitude. - """ - command = env.command_manager.get_term(command_name) - command.refresh_tensors() - return unintended_contact_penalty_jit( - right_cmd_active_per_body=command._cached_right_wrench_cmd_active_per_body, - right_cur_active_per_body=command._cached_right_wrench_cur_active_per_body, - left_cmd_active_per_body=command._cached_left_wrench_cmd_active_per_body, - left_cur_active_per_body=command._cached_left_wrench_cur_active_per_body, - right_cur_supports=command.right_contact_wrench_supports, - left_cur_supports=command.left_contact_wrench_supports, - num_bodies=command.num_bodies, - ) - - -def missed_contact_penalty( - env: ManagerBasedEnv, - command_name: str = "motion", -) -> torch.Tensor: - """Proportional penalty for missing expected contact directions. - - For each body, computes fraction of expected wrench support directions - missing in sim. Averages over bodies with expected contact, then hands. - - Returns continuous value in [0, 1]. Zero when no contact expected. - """ - command = env.command_manager.get_term(command_name) - command.refresh_tensors() - return missed_contact_penalty_jit( - right_cmd_active=command._cached_right_wrench_cmd_active, - right_cur_active=command._cached_right_wrench_cur_active, - left_cmd_active=command._cached_left_wrench_cmd_active, - left_cur_active=command._cached_left_wrench_cur_active, - right_cmd_active_per_body=command._cached_right_wrench_cmd_active_per_body, - left_cmd_active_per_body=command._cached_left_wrench_cmd_active_per_body, - ) - - -def force_closure_reward( - env: ManagerBasedEnv, - command_name: str = "motion", - min_support: float = 0.01, -) -> torch.Tensor: - """Force closure reward gated by binary contact labels. - - When contact is expected, rewards fraction of wrench basis directions - with sim support above min_support. Averaged over active hands. - This is a proxy lower bound: support is evaluated per hand rather than by - combining both hands' contacts into a single wrench space per body. - """ - command = env.command_manager.get_term(command_name) - - def _hand_closure( - contact_active: torch.Tensor, cur_supports: torch.Tensor - ) -> tuple[torch.Tensor, torch.Tensor]: - """Returns (reward (E,), is_active (E,)).""" - # cur_supports: (E, B, M) — wrench support per body per basis direction - # Collapse bodies: max over bodies gives best support per direction - cur_max = cur_supports.amax(dim=1) # (E, M) - has_support = cur_max > min_support # (E, M) - fraction = has_support.float().mean(dim=-1) # (E,) in [0, 1] - - # Gate by binary label - is_active = contact_active > 0.5 # (E,) - return fraction * is_active.float(), is_active - - left_reward, left_active = _hand_closure( - command.left_hand_contact_active_command, - command.left_hand_contact_wrench_supports, - ) - right_reward, right_active = _hand_closure( - command.right_hand_contact_active_command, - command.right_hand_contact_wrench_supports, - ) - - n_hands = (left_active.float() + right_active.float()).clamp(min=1) - - return (left_reward + right_reward) / n_hands diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/regularization_rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/regularization_rewards.py deleted file mode 100644 index 77584e64..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/regularization_rewards.py +++ /dev/null @@ -1,213 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import torch - -# Import PyTorch Ignite MMD -from ignite.metrics import MaximumMeanDiscrepancy -from isaaclab.assets import Articulation -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.managers import ManagerTermBase, SceneEntityCfg -from isaaclab.managers.manager_term_cfg import RewardTermCfg - -from robotic_grounding.tasks.v2p_whole_body.utils import MotionDataset - - -class body_acc_l2(ManagerTermBase): # noqa: N801 - """ - Directly copied from agile.rl_env.mdp.rewards.aesthetic_rewards.py. - - Penalize body linear and angular accelerations using velocity history tracking (Isaac Gym style). - - This reward term computes world-frame accelerations for a specified body/link. - If no body_names is specified in asset_cfg, it defaults to using the root link. - - Usage: - # For root acceleration (default): - body_acc = RewTerm(func=body_acc_l2, weight=-0.01) - - # For a specific link: - torso_acc = RewTerm( - func=body_acc_l2, - weight=-0.01, - params={"asset_cfg": SceneEntityCfg("robot", body_names=["torso_link"])}, - ) - """ - - def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize velocity history buffer and resolve body index.""" - super().__init__(cfg, env) - - # Initialize velocity history buffer - # Shape: [num_envs, 6] where 6 = 3 (lin_vel) + 3 (ang_vel) - self.prev_body_vel = torch.zeros( - env.num_envs, 6, device=env.device, dtype=torch.float32 - ) - - # Flag to track if this is the first call (skip acceleration computation) - self.first_call = True - - # Resolve body index if body_names is provided - self._body_idx: int | None = None - asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) - if asset_cfg.body_names is not None: - asset: Articulation = env.scene[asset_cfg.name] - self._body_idx = asset.find_bodies(asset_cfg.body_names)[0][0] - - def __call__( - self, - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg | None = None, - ) -> torch.Tensor: - """Compute body acceleration penalty by tracking velocity changes in world frame.""" - if asset_cfg is None: - asset_cfg = SceneEntityCfg("robot") - # Extract the robot asset - robot: Articulation = env.scene[asset_cfg.name] - - # Get current velocities (both linear and angular) in world frame - if self._body_idx is not None: - # Use specified body velocities - current_lin_vel = robot.data.body_lin_vel_w[ - :, self._body_idx, : - ] # [num_envs, 3] - current_ang_vel = robot.data.body_ang_vel_w[ - :, self._body_idx, : - ] # [num_envs, 3] - else: - # Default to root velocities - current_lin_vel = robot.data.root_lin_vel_w # [num_envs, 3] - current_ang_vel = robot.data.root_ang_vel_w # [num_envs, 3] - - # Concatenate to form 6D velocity vector - current_body_vel = torch.cat( - [current_lin_vel, current_ang_vel], dim=-1 - ) # [num_envs, 6] - - if self.first_call: - # First call: initialize previous velocity and return zeros - self.prev_body_vel.copy_(current_body_vel) - self.first_call = False - return torch.zeros(env.num_envs, device=env.device) - - # Compute acceleration as velocity difference over timestep - body_acc = (current_body_vel - self.prev_body_vel) / env.step_dt - - # Update velocity history for next call - self.prev_body_vel.copy_(current_body_vel) - - # Compute L2 penalty on accelerations (sum of squared accelerations) - return torch.sum(torch.square(body_acc), dim=-1) - - -def body_ang_vel_l2( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg | None = None, -) -> torch.Tensor: - """ - Directly copied from agile.rl_env.mdp.rewards.aesthetic_rewards.py. - - Penalize body angular velocity using L2 norm. - - This reward penalizes high angular velocities of a specified body/link, - useful for reducing shaking/oscillations without affecting linear motion. - If no body_names is specified in asset_cfg, defaults to using the root link. - - Usage: - # For root angular velocity: - root_ang_vel = RewTerm(func=body_ang_vel_l2, weight=-0.01) - - # For a specific link (e.g., torso): - torso_ang_vel = RewTerm( - func=body_ang_vel_l2, - weight=-0.01, - params={"asset_cfg": SceneEntityCfg("robot", body_names=["torso_link"])}, - ) - - Args: - env: The environment. - asset_cfg: Asset configuration. Use body_names to specify a link, otherwise uses root. - - Returns: - L2 norm of the body's angular velocity (sum of squared components). - """ - if asset_cfg is None: - asset_cfg = SceneEntityCfg("robot") - # Extract the robot asset - robot: Articulation = env.scene[asset_cfg.name] - - # Get angular velocity based on whether body_names is specified - if asset_cfg.body_ids is not None and len(asset_cfg.body_ids) > 0: - # Use specified body angular velocity - body_idx = asset_cfg.body_ids[0] - ang_vel = robot.data.body_ang_vel_w[:, body_idx, :] # [num_envs, 3] - else: - # Default to root angular velocity - ang_vel = robot.data.root_ang_vel_w # [num_envs, 3] - - # Compute L2 penalty (sum of squared angular velocities) - return torch.sum(torch.square(ang_vel), dim=-1) - - -class mmd_similarity_reward(ManagerTermBase): # noqa: N801 - """Reward for encouraging natural robot pose distributions using Maximum Mean Discrepancy.""" - - def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv) -> None: - """Initialize the MMD similarity reward.""" - super().__init__(cfg, env) - - # Extract parameters - asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) - self.robot: Articulation = env.scene[asset_cfg.name] - dataset_path = cfg.params.get("dataset_path") - joint_order_file = cfg.params.get("joint_order_file", None) - self.variance = cfg.params.get( - "variance", 10.0 - ) # Higher variance for better discrimination - self.reward_scale = cfg.params.get("reward_scale", 1.0) - self.device = env.device - - if dataset_path is None: - raise ValueError("dataset_path must be provided for mmd_similarity_reward") - - # Load motion dataset - self.motion_dataset = MotionDataset( - dataset_path=dataset_path, - robot=self.robot, - joint_order_file=joint_order_file, - device=env.device, - ) - self.expert_joint_indices = self.motion_dataset.get_joint_indices_for_robot() - - def __call__( - self, - env: ManagerBasedRLEnv, - dataset_path: str, - joint_order_file: str | None = None, - variance: float = 1.0, - asset_cfg: SceneEntityCfg | None = None, - ) -> torch.Tensor: - """Compute the MMD-based motion distribution similarity reward.""" - if asset_cfg is None: - asset_cfg = SceneEntityCfg("robot") - # Get current robot joint positions (all environments) - robot_joint_pos = self.robot.data.joint_pos - agent_batch = robot_joint_pos[:, self.expert_joint_indices] - num_envs = agent_batch.shape[0] - - # Sample num_envs expert poses directly from dataset - expert_sample = self.motion_dataset.sample(num_envs) - - # Compute MMD between agent batch and expert sample - mmd_metric = MaximumMeanDiscrepancy(var=self.variance, device=self.device) - mmd_metric.update((agent_batch, expert_sample)) - mmd_value = mmd_metric.compute() - - # Convert to tensor if float - if not isinstance(mmd_value, torch.Tensor): - mmd_value = torch.tensor(mmd_value, device=self.device) - - # Convert MMD to reward - reward = torch.exp(-self.reward_scale * mmd_value) - - # Return same reward for all environments - return reward.expand(num_envs) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/tracking_rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/tracking_rewards.py deleted file mode 100644 index 9709fb8c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/rewards/tracking_rewards.py +++ /dev/null @@ -1,402 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import isaaclab.utils.math as math_utils -import torch -from isaaclab.envs import ManagerBasedEnv -from isaaclab.managers import ManagerTermBase -from isaaclab.managers.manager_term_cfg import RewardTermCfg -from isaaclab.utils.math import quat_error_magnitude - -from robotic_grounding.tasks.v2p.mdp.utils import chamfer_distance - - -def motion_global_anchor_position_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """ - Reward for tracking the global anchor position using exponential kernel. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - - Returns: - Reward tensor of shape (num_envs,). - """ - command = env.command_manager.get_term(command_name) - error = torch.sum( - torch.square(command.command_anchor_pos_w - command.robot_anchor_pos_w), dim=-1 - ) - return torch.exp(-error / std**2) - - -def motion_global_anchor_orientation_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """ - Reward for tracking the global anchor orientation using exponential kernel. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - - Returns: - Reward tensor of shape (num_envs,). - """ - command = env.command_manager.get_term(command_name) - error = ( - quat_error_magnitude(command.command_anchor_quat_w, command.robot_anchor_quat_w) - ** 2 - ) - return torch.exp(-error / std**2) - - -def motion_ee_position_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """ - Reward for tracking the EE position using exponential kernel. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - - Returns: - Reward tensor of shape (num_envs,). - """ - command = env.command_manager.get_term(command_name) - # command_ee_pos_w: (num_envs, num_ee_links, 3), robot_ee_pos_w: (num_envs, num_ee_links, 3) - num_envs = command.command_ee_pos_w.shape[0] - num_ee_links = command.command_ee_pos_w.shape[1] - - # Flatten to (num_envs * num_ee_links, 3) - cmd_pos_flat = command.command_ee_pos_w.reshape(-1, 3) - robot_pos_flat = command.robot_ee_pos_w.reshape(-1, 3) - - error_flat = torch.sum( - torch.square(cmd_pos_flat - robot_pos_flat), dim=-1 - ) # (num_envs * num_ee_links,) - error_per_ee = error_flat.reshape( - num_envs, num_ee_links - ) # (num_envs, num_ee_links) - error = torch.sum(error_per_ee, dim=-1) # (num_envs,) - sum over EE links - return torch.exp(-error / std**2) - - -def motion_ee_orientation_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """ - Reward for tracking the EE orientation using exponential kernel. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - - Returns: - Reward tensor of shape (num_envs,). - """ - command = env.command_manager.get_term(command_name) - # command_ee_quat_w: (num_envs, num_ee_links, 4), robot_ee_quat_w: (num_envs, num_ee_links, 4) - num_envs = command.command_ee_quat_w.shape[0] - num_ee_links = command.command_ee_quat_w.shape[1] - - # Reshape to (num_envs * num_ee_links, 4) for quat_error_magnitude - cmd_quat_flat = command.command_ee_quat_w.reshape(-1, 4) - robot_quat_flat = command.robot_ee_quat_w.reshape(-1, 4) - - error_flat = ( - quat_error_magnitude(cmd_quat_flat, robot_quat_flat) ** 2 - ) # (num_envs * num_ee_links,) - error_per_ee = error_flat.reshape( - num_envs, num_ee_links - ) # (num_envs, num_ee_links) - error = torch.sum(error_per_ee, dim=-1) # (num_envs,) - sum over EE links - return torch.exp(-error / std**2) - - -class motion_joint_pos_error_exp(ManagerTermBase): # noqa: N801 - """Reward for tracking joint positions using an exponential kernel.""" - - def __init__(self, cfg: RewardTermCfg, env: ManagerBasedEnv) -> None: - """Resolve optional joint filters once when the reward term is created.""" - super().__init__(cfg, env) - - self._command = env.command_manager.get_term(cfg.params["command_name"]) - joint_names: list[str] | None = cfg.params.get("joint_names") - self._joint_ids: torch.Tensor | None = None - - if joint_names is not None: - _, resolved_joint_names = self._command.robot.find_joints(joint_names) - tracked_joint_name_to_idx = { - name: idx for idx, name in enumerate(self._command._tracked_joint_names) - } - missing_joint_names = [ - name - for name in resolved_joint_names - if name not in tracked_joint_name_to_idx - ] - if missing_joint_names: - raise ValueError( - "Reward joint_names must be included in the tracking command joints. " - f"Missing joints: {missing_joint_names}" - ) - self._joint_ids = torch.tensor( - [tracked_joint_name_to_idx[name] for name in resolved_joint_names], - device=env.device, - dtype=torch.long, - ) - - def __call__( - self, - env: ManagerBasedEnv, - command_name: str, - std: float, - joint_names: list[str] | None = None, - ) -> torch.Tensor: - """ - Compute joint position tracking reward. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - joint_names: Optional list of joint names or name patterns to track. If not - provided, all joints tracked by the command are used. - - Returns: - Reward tensor of shape (num_envs,). - """ - del env, command_name, joint_names - command_joint_pos = self._command.command_joint_pos - robot_joint_pos = self._command.robot_joint_pos - if self._joint_ids is not None: - command_joint_pos = command_joint_pos[:, self._joint_ids] - robot_joint_pos = robot_joint_pos[:, self._joint_ids] - - error = torch.sum(torch.square(command_joint_pos - robot_joint_pos), dim=-1) - return torch.exp(-error / std**2) - - -def motion_object_position_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """ - Reward for tracking object position using exponential kernel. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - std: Standard deviation of the exponential kernel. - - Returns: - Reward tensor of shape (num_envs,). - """ - command = env.command_manager.get_term(command_name) - object_pos_w = env.scene["object"].data.root_pos_w - error = torch.sum(torch.square(object_pos_w - command.command_object_pos_w), dim=-1) - rew = torch.exp(-error / std**2) - - return rew - - -def motion_object_lifted( - env: ManagerBasedEnv, - command_name: str, -) -> torch.Tensor: - """ - Reward for object being lifted, but only when reference trajectory says it should be. - - Args: - env: The environment object. - command_name: Name of the tracking command term. - - Returns: - Reward tensor of shape (num_envs,) with 1.0 if lifted when should be, 0.0 otherwise. - """ - command = env.command_manager.get_term(command_name) - actual_object_height = env.scene["object"].data.root_pos_w[:, 2] - minimal_height = torch.min(command._object_pos_w[:, 2]) - - # Check if object is lifted - is_lifted = actual_object_height > minimal_height - - # Check if object should be lifted - ref_object_height = command.command_object_pos_w[:, 2] # Z coordinate - should_be_lifted = ref_object_height > minimal_height - - return (is_lifted & should_be_lifted).float() - - -def motion_object_orientation_error_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """Reward for tracking object orientation using exponential kernel.""" - command = env.command_manager.get_term(command_name) - object_quat_w = env.scene["object"].data.root_quat_w - error = quat_error_magnitude(object_quat_w, command.command_object_quat_w) ** 2 - return torch.exp(-error / std**2) - - -def motion_finger_joint_pos_gaussian_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """Finger joint tracking: exp(-||error||^2 / std^2). Returns sum L+R (max 2.0).""" - command = env.command_manager.get_term(command_name) - right_error = torch.sum( - torch.square( - command.right_hand_finger_joint_pos_command - - command.right_hand_finger_joint_pos - ), - dim=-1, - ) - left_error = torch.sum( - torch.square( - command.left_hand_finger_joint_pos_command - - command.left_hand_finger_joint_pos - ), - dim=-1, - ) - return torch.exp(-right_error / std**2) + torch.exp(-left_error / std**2) - - -def motion_progress(env: ManagerBasedEnv, command_name: str) -> torch.Tensor: - """Reward for tracking the progress of the motion. - - Progress is normalized relative to where each environment started from. - """ - command = env.command_manager.get_term(command_name) - # Compute progress relative to reset point - steps_taken = (command.timestep - command.reset_timestep).float() - steps_remaining = (command.num_timesteps - 1 - command.reset_timestep).float() - return steps_taken / steps_remaining.clamp(min=1.0) - - -def motion_hand_keypoints_gaussian_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """Hand keypoints tracking: exp(-||error||^2 / std^2). Returns sum L+R (max 2.0).""" - command = env.command_manager.get_term(command_name) - - left_kp_cmd = torch.cat( - [ - command.left_hand_wrist_pose_command_e[:, :3].unsqueeze(1), - command.left_hand_fingertip_position_command_e[..., :3], - ], - dim=1, - ) - right_kp_cmd = torch.cat( - [ - command.right_hand_wrist_pose_command_e[:, :3].unsqueeze(1), - command.right_hand_fingertip_position_command_e[..., :3], - ], - dim=1, - ) - left_kp = torch.cat( - [ - command.left_hand_wrist_position_e.unsqueeze(1), - command.left_hand_fingertip_position_e[..., :3], - ], - dim=1, - ) - right_kp = torch.cat( - [ - command.right_hand_wrist_position_e.unsqueeze(1), - command.right_hand_fingertip_position_e[..., :3], - ], - dim=1, - ) - - left_err = torch.sum(torch.square(left_kp_cmd - left_kp), dim=-1).sum(dim=-1) - right_err = torch.sum(torch.square(right_kp_cmd - right_kp), dim=-1).sum(dim=-1) - return torch.exp(-left_err / std**2) + torch.exp(-right_err / std**2) - - -def motion_contact_tracking_gaussian_exp( - env: ManagerBasedEnv, command_name: str, std: float, mask_zero_contact: bool = True -) -> torch.Tensor: - """Chamfer contact tracking: exp(-dist^2 / std^2). Returns sum L+R.""" - command = env.command_manager.get_term(command_name) - result = torch.zeros(env.num_envs, device=env.device) - - for side in ("right", "left"): - pos_e = getattr(command, f"{side}_hand_object_contact_positions_e") - pos_w = getattr(command, f"{side}_hand_object_contact_positions_w") - cmd_e = getattr(command, f"{side}_hand_object_contact_command_positions_e") - cmd_valid = getattr(command, f"retargeted_{side}_object_contact_is_valid")[ - command.timestep_counter - ] - - # Articulated objects expose one contact sensor per object body. Chamfer - # matching is hand-level, so flatten object bodies into one point cloud. - pos_e = pos_e.reshape(pos_e.shape[0], -1, 3) - valid = pos_w.reshape(pos_w.shape[0], -1, 3).norm(dim=-1) > 1e-5 - cmd_e = cmd_e.reshape(cmd_e.shape[0], -1, 3) - cmd_valid = cmd_valid.reshape(cmd_valid.shape[0], -1) - - dist = chamfer_distance(pos_e, cmd_e, valid, cmd_valid) - rew = torch.exp(-(dist**2) / std**2) - if mask_zero_contact: - both_zero = (valid.sum(dim=-1) == 0) & (cmd_valid.sum(dim=-1) == 0) - rew[both_zero] = 0.0 - result += rew - - return result - - -def motion_contact_force_gaussian_exp( - env: ManagerBasedEnv, command_name: str, std: float -) -> torch.Tensor: - """Contact force reward: exp(-force^2 / std^2). Mean over in-contact points.""" - command = env.command_manager.get_term(command_name) - - total_rew = torch.zeros(env.num_envs, device=env.device) - total_contacts = torch.zeros(env.num_envs, device=env.device) - - for side in ("right", "left"): - forces = ( - getattr(command, f"{side}_hand_object_contact_forces_w") - .norm(dim=1) - .norm(dim=-1) - ) - in_contact = forces > 1e-3 - total_rew += (in_contact * torch.exp(-(forces**2) / std**2)).sum(dim=-1) - total_contacts += in_contact.sum(dim=-1) - - return torch.nan_to_num(total_rew / total_contacts, nan=0.0) - - -def motion_object_keypoints_tracking_exp( - env: ManagerBasedEnv, - command_name: str, - var: float, -) -> torch.Tensor: - """Object keypoint tracking over all object bodies.""" - command = env.command_manager.get_term(command_name) - - object_pos = command.object_position_e.unsqueeze(2).expand(-1, -1, 6, -1) - object_quat = command.object_orientation_e.unsqueeze(2).expand(-1, -1, 6, -1) - object_keypoints, _ = math_utils.combine_frame_transforms( - object_pos, - object_quat, - command.KEYPOINT_VECS, - q12=None, - ) - - command_pos = command.object_body_position_command_e.unsqueeze(2).expand( - -1, -1, 6, -1 - ) - command_quat = command.object_body_wxyz_command_e.unsqueeze(2).expand(-1, -1, 6, -1) - command_keypoints, _ = math_utils.combine_frame_transforms( - command_pos, - command_quat, - command.KEYPOINT_VECS, - q12=None, - ) - - error = torch.sum(torch.square(object_keypoints - command_keypoints), dim=-1) - return torch.exp(-error / var).mean(dim=(-2, -1)) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/README.md b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/README.md deleted file mode 100644 index ab2aa6c9..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# Terminations - -## `terminations.py` - -| Function | Params | Description | -|----------|--------|-------------| -| `timestep_termination` | `command_name` | Terminate when trajectory end is reached | -| `anchor_pos_error` | `command_name`, `threshold` | Root position error exceeds threshold (meters) | -| `anchor_quat_error` | `command_name`, `threshold` | Root orientation error exceeds threshold (radians) | -| `ee_position_error` | `command_name`, `threshold` | Sum of squared EE position errors exceeds threshold | -| `ee_quat_error` | `command_name`, `threshold` | Sum of squared EE orientation errors exceeds threshold | -| `joint_pos_error` | `command_name`, `threshold` | Joint position L2 norm exceeds threshold | -| `object_pos_error` | `command_name`, `threshold` | Object position error exceeds threshold (meters) | -| `object_quat_error` | `command_name`, `threshold` | Object orientation error exceeds threshold (radians) | diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/__init__.py deleted file mode 100644 index ca00ea24..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Termination functions for SONIC environment.""" - -from .terminations import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/terminations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/terminations.py deleted file mode 100644 index 0dfee668..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/terminations/terminations.py +++ /dev/null @@ -1,160 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -from __future__ import annotations - -from typing import Any - -import torch -from isaaclab.envs import ManagerBasedRLEnv -from isaaclab.utils.math import quat_error_magnitude - -from robotic_grounding.tasks.v2p.mdp.terminations import ( - hand_wrist_away_from_trajectory as _v2p_hand_wrist_away_from_trajectory, -) - - -def _mask_freeze(command: Any, terminated: torch.Tensor) -> torch.Tensor: - """Suppress termination during the post-reset freeze period.""" - freeze_steps = getattr(command.cfg, "reset_freeze_steps", 0) - if freeze_steps > 0: - in_freeze = command.steps_since_last_reset <= freeze_steps - return terminated & ~in_freeze - return terminated - - -def timestep_termination( - env: ManagerBasedRLEnv, - command_name: str, -) -> torch.Tensor: - """Terminate when the trajectory is completed.""" - command = env.command_manager.get_term(command_name) - end_timestep = getattr( - command, "trajectory_end_timestep", command.num_timesteps - 1 - ) - return command.timestep >= end_timestep - - -def anchor_pos_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 0.7, -) -> torch.Tensor: - """Terminate when the anchor position is too far from the command.""" - command = env.command_manager.get_term(command_name) - error = torch.norm( - command.robot_anchor_pos_w - command.command_anchor_pos_w, dim=-1 - ) - return error > threshold - - -def anchor_quat_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 0.7, -) -> torch.Tensor: - """Terminate when the anchor orientation is too far from the command.""" - command = env.command_manager.get_term(command_name) - error = quat_error_magnitude( - command.robot_anchor_quat_w, command.command_anchor_quat_w - ) - return error > threshold - - -def ee_position_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 0.15, -) -> torch.Tensor: - """Terminate when either EE is too far from the command. Freeze-aware.""" - command = env.command_manager.get_term(command_name) - # Per-link error, terminate if ANY link exceeds threshold - error = torch.norm( - command.command_ee_pos_w - command.robot_ee_pos_w, dim=-1 - ) # (num_envs, num_ee) - terminated = error.max(dim=-1).values > threshold - return _mask_freeze(command, terminated) - - -def ee_quat_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 1.5, -) -> torch.Tensor: - """Terminate when either EE orientation is too far from the command. Freeze-aware.""" - command = env.command_manager.get_term(command_name) - cmd_flat = command.command_ee_quat_w.reshape(-1, 4) - robot_flat = command.robot_ee_quat_w.reshape(-1, 4) - error = quat_error_magnitude(cmd_flat, robot_flat).reshape(command.num_envs, -1) - terminated = error.max(dim=-1).values > threshold - return _mask_freeze(command, terminated) - - -def joint_pos_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 2.0, -) -> torch.Tensor: - """Terminate when joint positions are too far from the command. Freeze-aware.""" - command = env.command_manager.get_term(command_name) - error = torch.norm(command.robot_joint_pos - command.command_joint_pos, dim=-1) - terminated = error > threshold - return _mask_freeze(command, terminated) - - -def object_pos_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 0.1, -) -> torch.Tensor: - """Terminate when any object body position is too far from the command.""" - command = env.command_manager.get_term(command_name) - if hasattr(command, "object_body_position_command_e") and hasattr( - command, "object_position_e" - ): - error = torch.norm( - command.object_position_e - command.object_body_position_command_e, - dim=-1, - ) - terminated = (error > threshold).any(dim=-1) - else: - error = torch.norm(command.object_pos_w - command.command_object_pos_w, dim=-1) - terminated = error > threshold - return _mask_freeze(command, terminated) - - -def object_quat_error( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float = 0.5, -) -> torch.Tensor: - """Terminate when any object body orientation is too far from the command.""" - command = env.command_manager.get_term(command_name) - if hasattr(command, "object_body_wxyz_command_e") and hasattr( - command, "object_orientation_e" - ): - error = quat_error_magnitude( - command.object_orientation_e.reshape(-1, 4), - command.object_body_wxyz_command_e.reshape(-1, 4), - ).reshape(command.num_envs, -1) - terminated = (error > threshold).any(dim=-1) - else: - error = quat_error_magnitude( - command.object_quat_w, command.command_object_quat_w - ) - terminated = error > threshold - return _mask_freeze(command, terminated) - - -def hand_wrist_away_from_trajectory( - env: ManagerBasedRLEnv, - command_name: str, - threshold: float, -) -> torch.Tensor: - """V2P hands hand-away termination with whole-body reset-freeze masking.""" - command = env.command_manager.get_term(command_name) - terminated = _v2p_hand_wrist_away_from_trajectory( - env, - command_name=command_name, - threshold=threshold, - ) - return _mask_freeze(command, terminated) diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_commands.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_commands.py deleted file mode 100644 index 3253946c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_commands.py +++ /dev/null @@ -1,273 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for tracking command.""" - -import unittest -from typing import Any - -import torch -import yaml -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import MOTION_ASSET_DIR, SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEEEnvCfg - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestTrackingCommand(unittest.TestCase): - """Test suite for tracking command.""" - - env_cfg: Any - motion_file: str - env: Any - tracking_cmd: Any - expected_root_pos: torch.Tensor - expected_root_quat: torch.Tensor - expected_object_pos: torch.Tensor - expected_object_quat: torch.Tensor - - @classmethod - def setUpClass(cls) -> None: - """Set up the test environment once for all tests.""" - cls.env_cfg = G1SonicEEEnvCfg( - scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick_optimized.yaml" - ) - cls.env_cfg.commands.motion.object_pos_offset = [0.0, 0.0, 0.0] - cls.env_cfg.commands.motion.robot_anchor_pos_offset = [0.0, 0.0, 0.0] - cls.env_cfg.scene.num_envs = 2 - cls.motion_file = ( - f"{MOTION_ASSET_DIR}/object_pick_and_place_optimized_motion_with_ee.yaml" - ) - cls.env = ManagerBasedRLEnv(cfg=cls.env_cfg) - - # Get tracking command - cls.tracking_cmd = cls.env.command_manager.get_term("motion") - - # Load motion data to compare against - with open(cls.motion_file, "r") as f: - data = yaml.safe_load(f) - qpos_data = torch.tensor(data["qpos"]).to(cls.env.device) - object_pos_w = torch.tensor(data["object_position"]).to(cls.env.device) - object_quat_w = torch.tensor(data["object_wxyz"]).to(cls.env.device) - - # Extract components - cls.expected_root_pos = qpos_data[:, :3].float() - cls.expected_root_quat = qpos_data[:, 3:7].float() - cls.expected_object_pos = object_pos_w.float() - cls.expected_object_quat = object_quat_w.float() - - @classmethod - def tearDownClass(cls) -> None: - """Clean up after all tests.""" - cls.env.close() - - def test_object_pos_w(self) -> None: - """Test that object positions are correctly computed.""" - expected_shape = (self.env.num_envs, 3) - self.assertEqual( - self.tracking_cmd.command_object_pos_w.shape, - expected_shape, - f"Object positions should have shape {expected_shape}", - ) - - first_frame_object_pos = self.tracking_cmd.command_object_pos_w - expected_first_frame_object_pos = ( - self.expected_object_pos[0] + self.env.scene.env_origins - ) - self.assertTrue( - torch.allclose( - first_frame_object_pos, expected_first_frame_object_pos, atol=1e-5 - ), - f"First frame object positions should match. Max diff: {torch.abs(first_frame_object_pos - expected_first_frame_object_pos).max()}", - ) - - def test_object_quat_w(self) -> None: - """Test that object quaternions are correctly computed.""" - expected_shape = (self.env.num_envs, 4) - self.assertEqual( - self.tracking_cmd.command_object_quat_w.shape, - expected_shape, - f"Object quaternions should have shape {expected_shape}", - ) - - first_frame_object_quat = self.tracking_cmd.command_object_quat_w[0] - expected_first_frame_object_quat = self.expected_object_quat[0] - self.assertTrue( - torch.allclose( - first_frame_object_quat, expected_first_frame_object_quat, atol=1e-5 - ), - f"First frame object quaternions should match. Max diff: {torch.abs(first_frame_object_quat - expected_first_frame_object_quat).max()}", - ) - - def test_command_anchor_z_multi_future(self) -> None: - """Test that future z positions are correctly computed.""" - expected_shape = (self.env.num_envs, self.tracking_cmd.cfg.num_future_frames) - self.assertEqual( - self.tracking_cmd.command_anchor_z_multi_future.shape, - expected_shape, - f"Future z positions should have shape {expected_shape}", - ) - - first_frame_z = self.tracking_cmd.command_anchor_z_multi_future[0, 0] - expected_first_frame_z = self.expected_root_pos[0, 2] - self.assertTrue( - torch.allclose(first_frame_z, expected_first_frame_z, atol=1e-4), - f"First frame z position should match. Max diff: {torch.abs(first_frame_z - expected_first_frame_z).max()}", - ) - - def test_root_rot_dif_l_multi_future_shape(self) -> None: - """Test that future root rotation differences are correctly computed.""" - root_rot_dif = self.tracking_cmd.command_anchor_rot_diff_l_multi_future - expected_shape = (self.env.num_envs, self.tracking_cmd.cfg.num_future_frames, 6) - - self.assertEqual( - root_rot_dif.shape, - expected_shape, - f"Future root rotation differences should have shape {expected_shape}", - ) - - def test_joint_pos_multi_future_shape(self) -> None: - """Test that future joint positions have correct shape.""" - joint_pos_multi_future = self.tracking_cmd.command_joint_pos_multi_future - expected_shape = ( - self.env.num_envs, - self.tracking_cmd.cfg.num_future_frames, - len(self.env.scene["robot"].data.joint_names), - ) - - self.assertEqual( - joint_pos_multi_future.shape, - expected_shape, - f"Future joint positions should have shape {expected_shape}", - ) - - def test_joint_vel_multi_future_shape(self) -> None: - """Test that future joint velocities have correct shape.""" - joint_vel_multi_future = self.tracking_cmd.command_joint_vel_multi_future - expected_shape = ( - self.env.num_envs, - self.tracking_cmd.cfg.num_future_frames, - len(self.env.scene["robot"].data.joint_names), - ) - - self.assertEqual( - joint_vel_multi_future.shape, - expected_shape, - f"Future joint velocities should have shape {expected_shape}", - ) - - def test_ee_pos_multi_future_shape(self) -> None: - """Test that future EE positions have correct shape.""" - ee_pos_multi_future = self.tracking_cmd.command_ee_pos_w_multi_future - num_ee_links = len(self.tracking_cmd.cfg.ee_link_names) - expected_shape = ( - self.env.num_envs, - self.tracking_cmd.cfg.num_future_frames, - num_ee_links, - 3, - ) - - self.assertEqual( - ee_pos_multi_future.shape, - expected_shape, - f"Future EE positions should have shape {expected_shape}", - ) - - def test_ee_pos_w_shape(self) -> None: - """Test that current EE positions have correct shape.""" - ee_pos_w = self.tracking_cmd.command_ee_pos_w - num_ee_links = len(self.tracking_cmd.cfg.ee_link_names) - expected_shape = (self.env.num_envs, num_ee_links, 3) - - self.assertEqual( - ee_pos_w.shape, - expected_shape, - f"EE positions should have shape {expected_shape}", - ) - - def test_ee_quat_w_shape(self) -> None: - """Test that current EE quaternions have correct shape.""" - ee_quat_w = self.tracking_cmd.command_ee_quat_w - num_ee_links = len(self.tracking_cmd.cfg.ee_link_names) - expected_shape = (self.env.num_envs, num_ee_links, 4) - - self.assertEqual( - ee_quat_w.shape, - expected_shape, - f"EE quaternions should have shape {expected_shape}", - ) - - def test_update_command(self) -> None: - """Test that update_command correctly increments timestep.""" - self.tracking_cmd.timestep[:] = 0 - initial_timesteps = self.tracking_cmd.timestep.clone() - self.tracking_cmd._update_command() - expected_timesteps = initial_timesteps + 1 - self.assertTrue( - torch.all(self.tracking_cmd.timestep == expected_timesteps), - f"Timesteps should increment by 1. Expected: {expected_timesteps}, Got: {self.tracking_cmd.timestep}", - ) - - def test_update_command_wraps_at_end(self) -> None: - """Test that update_command resets timestep when reaching end of trajectory.""" - self.tracking_cmd.timestep[:] = self.tracking_cmd.num_timesteps - 1 - self.tracking_cmd._update_command() - self.assertTrue( - torch.all( - self.tracking_cmd.timestep == self.tracking_cmd.num_timesteps - 1 - ), - f"Timesteps should stay at end till reset. Got: {self.tracking_cmd.timestep}", - ) - - def test_update_metrics_populates_expected_metric_shapes(self) -> None: - """Test that update_metrics fills the expected per-env metric tensors.""" - self.tracking_cmd._update_metrics() - - expected_metric_names = [ - "anchor_position_error", - "anchor_wxyz_error", - "joint_pos_error", - "left_hand_wrist_position_error", - "left_hand_wrist_wxyz_error", - "left_hand_finger_joints_error", - "right_hand_wrist_position_error", - "right_hand_wrist_wxyz_error", - "right_hand_finger_joints_error", - "object_body_position_error", - "object_body_wxyz_error", - "virtual_object_controller_scale_factor", - ] - - for metric_name in expected_metric_names: - self.assertIn(metric_name, self.tracking_cmd.metrics) - metric = self.tracking_cmd.metrics[metric_name] - self.assertEqual( - metric.shape, - (self.env.num_envs,), - f"{metric_name} should be shape {(self.env.num_envs,)}", - ) - self.assertTrue( - torch.isfinite(metric).all(), - f"{metric_name} should contain only finite values", - ) - - def test_update_metrics_tracks_per_env_voc_scale(self) -> None: - """Test that VOC metric mirrors the currently applied per-env scale.""" - expected = torch.tensor([[0.25], [0.75]], device=self.env.device) - self.tracking_cmd.virtual_object_controller_scale_factor_per_env[:] = expected - - self.tracking_cmd._update_metrics() - - self.assertTrue( - torch.allclose( - self.tracking_cmd.metrics["virtual_object_controller_scale_factor"], - expected.squeeze(-1), - ), - "VOC metric should match per-env applied VOC scale", - ) - - -if __name__ == "__main__": - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_events.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_events.py deleted file mode 100644 index 0dba246c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_events.py +++ /dev/null @@ -1,104 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for environment events.""" - -import unittest -from typing import Any - -import h5py -import torch -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import MOTION_ASSET_DIR, SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEnvCfg - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestEvents(unittest.TestCase): - """Test suite for events.""" - - env_cfg: Any - motion_file: str - env: Any - - @classmethod - def setUpClass(cls) -> None: - """Set up the test environment once for all tests.""" - cls.env_cfg = G1SonicEnvCfg( - scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick.yaml" - ) - cls.env_cfg.scene.num_envs = 2 - cls.env_cfg.commands.motion.object_pos_offset = [0.0, 0.0, 0.0] - cls.env_cfg.commands.motion.robot_anchor_pos_offset = [0.0, 0.0, 0.0] - cls.env_cfg.events.reset_to_motion_start.params["trajectory_time_index"] = ( - 0, - 1, - ) - cls.motion_file = ( - f"{MOTION_ASSET_DIR}/apple_pick_and_place_retarget_motion_w_body.h5" - ) - cls.env = ManagerBasedRLEnv(cfg=cls.env_cfg) - - @classmethod - def tearDownClass(cls) -> None: - """Clean up after all tests.""" - cls.env.close() - - def test_reset_to_trajectory_start(self) -> None: - """Test that reset_robot_to_trajectory_start correctly positions the robot.""" - # Load the motion data to compare against - with h5py.File(self.motion_file, "r") as f: - qpos_data = torch.from_numpy(f["qpos"][()]).to(self.env.device) - - # Extract first frame data - expected_root_pos = qpos_data[0, :3] # (3,) - expected_root_quat = qpos_data[0, 3:7] # (4,) - - # Reset the environment - obs_dict, _ = self.env.reset() - - # Get tracking command - tracking_cmd = self.env.command_manager.get_term("motion") - robot = self.env.scene["robot"] - - # Tracking timestep should be 0 - self.assertTrue( - torch.all(tracking_cmd.timestep == 0), - f"Tracking timestep should be 0, got {tracking_cmd.timestep}", - ) - - # Root position should match first frame (plus env origins) - for env_id in range(self.env.num_envs): - expected_pos_w = expected_root_pos + self.env.scene.env_origins[env_id] - actual_pos_w = robot.data.root_pos_w[env_id] - - pos_diff = torch.abs(expected_pos_w - actual_pos_w) - self.assertTrue( - torch.all(pos_diff < 0.01), # 1cm tolerance - f"Env {env_id}: Root position mismatch. " - f"Expected: {expected_pos_w}, Got: {actual_pos_w}, Diff: {pos_diff}", - ) - - # Root orientation should match first frame - for env_id in range(self.env.num_envs): - expected_quat = expected_root_quat - actual_quat = robot.data.root_quat_w[env_id] - - # Check quaternion similarity (q and -q represent same rotation) - quat_diff = torch.min( - torch.abs(expected_quat - actual_quat), - torch.abs(expected_quat + actual_quat), - ) - - self.assertTrue( - torch.all(quat_diff < 0.01), - f"Env {env_id}: Root orientation mismatch. " - f"Expected: {expected_quat}, Got: {actual_quat}, Diff: {quat_diff}", - ) - - -if __name__ == "__main__": - # Run tests - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_observations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_observations.py deleted file mode 100644 index ab55495e..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_observations.py +++ /dev/null @@ -1,207 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for environment observations.""" - -import unittest -from typing import Any - -import torch -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import MOTION_ASSET_DIR, SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEEEnvCfg - from robotic_grounding.tasks.v2p_whole_body.mdp.observations import ( - policy_observations as obs, - ) - from robotic_grounding.tasks.v2p_whole_body.mdp.observations import ( - sonic_tokenizer_observations as tokenizer_obs, - ) - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestObservations(unittest.TestCase): - """Test suite for observations (including EE observations).""" - - env_cfg: Any - motion_file: str - env: Any - - @classmethod - def setUpClass(cls) -> None: - """Set up the test environment once for all tests.""" - cls.env_cfg = G1SonicEEEnvCfg( - scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick_optimized.yaml" - ) - cls.env_cfg.scene.num_envs = 2 - cls.motion_file = ( - f"{MOTION_ASSET_DIR}/object_pick_and_place_optimized_motion_with_ee.yaml" - ) - cls.env = ManagerBasedRLEnv(cfg=cls.env_cfg) - - @classmethod - def tearDownClass(cls) -> None: - """Clean up after all tests.""" - cls.env.close() - - def test_encoder_mode(self) -> None: - """Test that the encoder mode is correctly computed.""" - encoder_mode = tokenizer_obs.encoder_mode(self.env, "motion") - self.assertEqual( - encoder_mode.shape, - (self.env.num_envs, 4), - "Encoder mode should have shape (num_envs, 4)", - ) - expected = torch.zeros((self.env.num_envs, 4), device=self.env.device) - self.assertTrue( - torch.allclose(encoder_mode, expected), "Encoder mode should be all zeros" - ) - - def test_command_joint_pos(self) -> None: - """Test that the command joint positions are correctly computed.""" - command_joint_pos = tokenizer_obs.command_joint_pos(self.env, "motion") - num_joints = len(self.env.scene["robot"].data.joint_names) - expected_shape = ( - self.env.num_envs, - num_joints - * self.env.command_manager.get_term("motion").cfg.num_future_frames, - ) - self.assertEqual( - command_joint_pos.shape, - expected_shape, - f"Command joint positions should have shape {expected_shape}", - ) - - def test_command_joint_vel(self) -> None: - """Test that the command joint velocities are correctly computed.""" - command_joint_vel = tokenizer_obs.command_joint_vel(self.env, "motion") - num_joints = len(self.env.scene["robot"].data.joint_names) - expected_shape = ( - self.env.num_envs, - num_joints - * self.env.command_manager.get_term("motion").cfg.num_future_frames, - ) - self.assertEqual( - command_joint_vel.shape, - expected_shape, - f"Command joint velocities should have shape {expected_shape}", - ) - - def test_motion_anchor_ori_b(self) -> None: - """Test that the motion anchor orientation differences are correctly computed.""" - motion_anchor_ori_b = tokenizer_obs.motion_anchor_ori_b(self.env, "motion") - expected_shape = ( - self.env.num_envs, - 6 * self.env.command_manager.get_term("motion").cfg.num_future_frames, - ) - self.assertEqual( - motion_anchor_ori_b.shape, - expected_shape, - f"Motion anchor orientation differences should have shape {expected_shape}", - ) - - def test_encoder_padding(self) -> None: - """Test that the encoder padding is correctly computed.""" - encoder_padding = tokenizer_obs.encoder_padding(self.env, 17) - expected_shape = (self.env.num_envs, 17) - self.assertEqual( - encoder_padding.shape, - expected_shape, - f"Encoder padding should have shape {expected_shape}", - ) - - def test_motion_anchor_pos_b(self) -> None: - """Test that the motion anchor positions are correctly computed.""" - motion_anchor_pos_b = obs.motion_anchor_pos_b(self.env, "motion") - expected_shape = ( - self.env.num_envs, - 3 * self.env.command_manager.get_term("motion").cfg.num_future_frames, - ) - self.assertEqual( - motion_anchor_pos_b.shape, - expected_shape, - f"Motion anchor positions should have shape {expected_shape}", - ) - - def test_motion_joint_pos_delta(self) -> None: - """Test that the motion joint position deltas are correctly computed.""" - motion_joint_pos_delta = obs.motion_joint_pos_delta(self.env, "motion") - expected_shape = ( - self.env.num_envs, - self.env.command_manager.get_term("motion").cfg.num_future_frames - * len(self.env.scene["robot"].data.joint_names), - ) - self.assertEqual( - motion_joint_pos_delta.shape, - expected_shape, - f"Motion joint position deltas should have shape {expected_shape}", - ) - - def test_object_pos_delta(self) -> None: - """Test that the object position deltas are correctly computed.""" - object_pos_delta = obs.object_pos_delta(self.env, "motion") - expected_shape = ( - self.env.num_envs, - 3 * self.env.command_manager.get_term("motion").cfg.num_future_frames, - ) - self.assertEqual( - object_pos_delta.shape, - expected_shape, - f"Object position deltas should have shape {expected_shape}", - ) - - def test_command_trajectory_progress(self) -> None: - """Test that the command trajectory progress is correctly computed.""" - command_trajectory_progress = obs.command_trajectory_progress( - self.env, "motion" - ) - expected_shape = (self.env.num_envs, 1) - self.assertEqual( - command_trajectory_progress.shape, - expected_shape, - f"Command trajectory progress should have shape {expected_shape}", - ) - self.assertTrue( - torch.allclose( - command_trajectory_progress, - torch.zeros((self.env.num_envs, 1), device=self.env.device), - ), - "Command trajectory progress should be 0", - ) - - def test_motion_ee_pos_delta(self) -> None: - """Test that motion EE position deltas are correctly computed.""" - motion_ee_pos_delta = obs.motion_ee_pos_delta(self.env, "motion") - num_future_frames = self.env.command_manager.get_term( - "motion" - ).cfg.num_future_frames - num_ee_links = len( - self.env.command_manager.get_term("motion").cfg.ee_link_names - ) - expected_shape = (self.env.num_envs, num_future_frames * num_ee_links * 3) - self.assertEqual( - motion_ee_pos_delta.shape, - expected_shape, - f"Motion EE position deltas should have shape {expected_shape}", - ) - - def test_motion_ee_quat_delta(self) -> None: - """Test that motion EE quaternion deltas are correctly computed.""" - motion_ee_quat_delta = obs.motion_ee_quat_delta(self.env, "motion") - num_future_frames = self.env.command_manager.get_term( - "motion" - ).cfg.num_future_frames - num_ee_links = len( - self.env.command_manager.get_term("motion").cfg.ee_link_names - ) - expected_shape = (self.env.num_envs, num_future_frames * num_ee_links * 6) - self.assertEqual( - motion_ee_quat_delta.shape, - expected_shape, - f"Motion EE quaternion deltas should have shape {expected_shape}", - ) - - -if __name__ == "__main__": - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_rewards.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_rewards.py deleted file mode 100644 index ce7a6297..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_rewards.py +++ /dev/null @@ -1,207 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for tracking rewards.""" - -import unittest -from typing import Any - -import torch -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import MOTION_ASSET_DIR, SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEEEnvCfg - from robotic_grounding.tasks.v2p_whole_body.mdp.rewards import tracking_rewards - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestTrackingRewards(unittest.TestCase): - """Test suite for tracking rewards (including EE rewards).""" - - env_cfg: Any - motion_file: str - env: Any - tracking_cmd: Any - - @classmethod - def setUpClass(cls) -> None: - """Set up the test environment once for all tests.""" - cls.env_cfg = G1SonicEEEnvCfg( - scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick_optimized.yaml" - ) - cls.env_cfg.scene.num_envs = 2 - cls.motion_file = ( - f"{MOTION_ASSET_DIR}/object_pick_and_place_optimized_motion_with_ee.yaml" - ) - cls.env = ManagerBasedRLEnv(cfg=cls.env_cfg) - cls.tracking_cmd = cls.env.command_manager.get_term("motion") - - @classmethod - def tearDownClass(cls) -> None: - """Clean up after all tests.""" - cls.env.close() - - def test_motion_global_anchor_position_error_exp_zero_error(self) -> None: - """Test that anchor position reward is 1 when tracking error is 0.""" - self.tracking_cmd.robot_anchor_pos_w[:] = ( - self.tracking_cmd.command_anchor_pos_w.clone() - ) - reward = tracking_rewards.motion_global_anchor_position_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - expected = torch.ones(self.env.num_envs, device=self.env.device) - self.assertTrue( - torch.allclose(reward, expected, atol=1e-5), - f"Reward should be 1 when error is 0. Got: {reward}", - ) - - def test_motion_global_anchor_position_error_exp_nonzero_error(self) -> None: - """Test that anchor position reward is less than 1 when tracking error is nonzero.""" - self.tracking_cmd.robot_anchor_pos_w[:] = ( - self.tracking_cmd.command_anchor_pos_w.clone() - ) - self.tracking_cmd.robot_anchor_pos_w[:, 0] += 1.0 - reward = tracking_rewards.motion_global_anchor_position_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertTrue( - torch.all(reward < 1.0), - f"Reward should be less than 1 when error is nonzero. Got: {reward}", - ) - - def test_motion_global_anchor_orientation_error_exp_zero_error(self) -> None: - """Test that anchor orientation reward is 1 when tracking error is 0.""" - self.tracking_cmd.robot_anchor_quat_w[:] = ( - self.tracking_cmd.command_anchor_quat_w.clone() - ) - reward = tracking_rewards.motion_global_anchor_orientation_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - expected = torch.ones(self.env.num_envs, device=self.env.device) - self.assertTrue( - torch.allclose(reward, expected, atol=1e-5), - f"Reward should be 1 when error is 0. Got: {reward}", - ) - - def test_motion_global_anchor_orientation_error_exp_nonzero_error(self) -> None: - """Test that anchor orientation reward is less than 1 when tracking error is nonzero.""" - self.tracking_cmd.robot_anchor_quat_w[:] = torch.tensor( - [0.7071, 0.0, 0.0, 0.7071], device=self.env.device - ).repeat(self.env.num_envs, 1) - reward = tracking_rewards.motion_global_anchor_orientation_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertTrue( - torch.all(reward < 1.0), - f"Reward should be less than 1 when error is nonzero. Got: {reward}", - ) - - def test_motion_object_position_error_exp_zero_error(self) -> None: - """Test that object position reward is 1 when tracking error is 0.""" - self.env.scene["object"].data.root_pos_w[ - : - ] = self.tracking_cmd.command_object_pos_w.clone() - reward = tracking_rewards.motion_object_position_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - expected = torch.ones(self.env.num_envs, device=self.env.device) - self.assertTrue( - torch.allclose(reward, expected, atol=1e-5), - f"Reward should be 1 when error is 0. Got: {reward}", - ) - - def test_motion_object_position_error_exp_nonzero_error(self) -> None: - """Test that object position reward is less than 1 when tracking error is nonzero.""" - self.env.scene["object"].data.root_pos_w[ - : - ] = self.tracking_cmd.command_object_pos_w.clone() - self.env.scene["object"].data.root_pos_w[:, 0] += 1.0 - reward = tracking_rewards.motion_object_position_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertTrue( - torch.all(reward < 1.0), - f"Reward should be less than 1 when error is nonzero. Got: {reward}", - ) - - def test_motion_object_orientation_error_exp_zero_error(self) -> None: - """Test that object orientation reward is 1 when tracking error is 0.""" - self.env.scene["object"].data.root_quat_w[ - : - ] = self.tracking_cmd.command_object_quat_w.clone() - reward = tracking_rewards.motion_object_orientation_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - expected = torch.ones(self.env.num_envs, device=self.env.device) - self.assertTrue( - torch.allclose(reward, expected, atol=1e-5), - f"Reward should be 1 when error is 0. Got: {reward}", - ) - - def test_motion_object_orientation_error_exp_nonzero_error(self) -> None: - """Test that object orientation reward is less than 1 when tracking error is nonzero.""" - self.env.scene["object"].data.root_quat_w[:] = torch.tensor( - [0.7071, 0.0, 0.0, 0.7071], device=self.env.device - ).repeat(self.env.num_envs, 1) - reward = tracking_rewards.motion_object_orientation_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertTrue( - torch.all(reward < 1.0), - f"Reward should be less than 1 when error is nonzero. Got: {reward}", - ) - - def test_motion_ee_position_error_exp(self) -> None: - """Test EE position reward has correct shape and range.""" - reward = tracking_rewards.motion_ee_position_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - self.assertTrue( - torch.all(reward >= 0.0) and torch.all(reward <= 1.0), - f"Reward should be in [0, 1]. Got: {reward}", - ) - - def test_motion_ee_orientation_error_exp(self) -> None: - """Test EE orientation reward has correct shape and range.""" - reward = tracking_rewards.motion_ee_orientation_error_exp( - self.env, command_name="motion", std=1.0 - ) - self.assertEqual( - reward.shape, - (self.env.num_envs,), - f"Reward should have shape ({self.env.num_envs},)", - ) - self.assertTrue( - torch.all(reward >= 0.0) and torch.all(reward <= 1.0), - f"Reward should be in [0, 1]. Got: {reward}", - ) - - -if __name__ == "__main__": - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_terminations.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_terminations.py deleted file mode 100644 index 35e2bd41..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/mdp/tests/test_terminations.py +++ /dev/null @@ -1,101 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for terminations.""" - -import unittest -from typing import Any - -import torch -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import MOTION_ASSET_DIR, SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEEEnvCfg - from robotic_grounding.tasks.v2p_whole_body.mdp.terminations import ( - ee_position_error, - ee_quat_error, - timestep_termination, - ) - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestTerminations(unittest.TestCase): - """Test suite for terminations (including EE terminations).""" - - env_cfg: Any - motion_file: str - env: Any - tracking_cmd: Any - - @classmethod - def setUpClass(cls) -> None: - """Set up the test environment once for all tests.""" - cls.env_cfg = G1SonicEEEnvCfg( - scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick_optimized.yaml" - ) - cls.env_cfg.scene.num_envs = 2 - cls.motion_file = ( - f"{MOTION_ASSET_DIR}/object_pick_and_place_optimized_motion_with_ee.yaml" - ) - cls.env = ManagerBasedRLEnv(cfg=cls.env_cfg) - cls.tracking_cmd = cls.env.command_manager.get_term("motion") - - @classmethod - def tearDownClass(cls) -> None: - """Clean up after all tests.""" - cls.env.close() - - def test_timestep_termination(self) -> None: - """Test that the timestep termination is correctly computed.""" - termination = timestep_termination(self.env, "motion") - self.assertEqual( - termination.shape, - (self.env.num_envs,), - "Termination should have shape (num_envs,)", - ) - self.assertTrue(torch.all(termination == 0), "Termination should be 0") - - self.tracking_cmd.timestep += self.tracking_cmd.num_timesteps - termination = timestep_termination(self.env, "motion") - self.assertEqual( - termination.shape, - (self.env.num_envs,), - "Termination should have shape (num_envs,)", - ) - self.assertTrue(torch.all(termination == 1), "Termination should be 1") - - # Reset timestep for other tests - self.tracking_cmd.timestep[:] = 0 - - def test_ee_position_error(self) -> None: - """Test that EE position error termination has correct shape and dtype.""" - termination = ee_position_error(self.env, "motion", threshold=1.0) - self.assertEqual( - termination.shape, - (self.env.num_envs,), - f"Termination should have shape ({self.env.num_envs},)", - ) - self.assertEqual( - termination.dtype, - torch.bool, - f"Termination should be boolean. Got: {termination.dtype}", - ) - - def test_ee_quat_error(self) -> None: - """Test that EE quaternion error termination has correct shape and dtype.""" - termination = ee_quat_error(self.env, "motion", threshold=1.0) - self.assertEqual( - termination.shape, - (self.env.num_envs,), - f"Termination should have shape ({self.env.num_envs},)", - ) - self.assertEqual( - termination.dtype, - torch.bool, - f"Termination should be boolean. Got: {termination.dtype}", - ) - - -if __name__ == "__main__": - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/tests/test_sonic_g1_env.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/tests/test_sonic_g1_env.py deleted file mode 100644 index 79f5dc38..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/tests/test_sonic_g1_env.py +++ /dev/null @@ -1,121 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for SONIC environment instantiation.""" - -import unittest -from typing import Any - -import torch -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - from isaaclab.envs import ManagerBasedRLEnv - from robotic_grounding.assets import SCENE_CONFIG_DIR - from robotic_grounding.tasks.v2p_whole_body import G1SonicEnvCfg - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestSonicG1Env(unittest.TestCase): - """Test that SONIC G1 environment can be instantiated correctly.""" - - env: Any - cfg: Any - - @classmethod - def setUpClass(cls) -> None: - """Create one environment for all tests in this class.""" - cfg = G1SonicEnvCfg(scene_config_path=f"{SCENE_CONFIG_DIR}/apple_pick.yaml") - cfg.scene.num_envs = 2 - cfg.scene.env_spacing = 2.0 - cls.env = ManagerBasedRLEnv(cfg=cfg) - cls.cfg = cfg - - @classmethod - def tearDownClass(cls) -> None: - """Clean up the environment after all tests.""" - cls.env.close() - - def test_config_instantiation(self) -> None: - """Test that the SONIC configuration can be instantiated.""" - self.assertIsNotNone(self.cfg, "Configuration should be created") - self.assertIsNotNone(self.cfg.scene, "Scene configuration should exist") - self.assertIsNotNone(self.cfg.actions, "Actions configuration should exist") - self.assertIsNotNone( - self.cfg.observations, "Observations configuration should exist" - ) - - def test_environment_creation(self) -> None: - """Test that the SONIC environment can be created.""" - self.assertIsNotNone(self.env, "Environment should be created") - self.assertEqual(self.env.num_envs, 2, "Number of environments should match") - - def test_robot_in_scene(self) -> None: - """Test that the robot is present in the scene.""" - # Check if robot exists in scene - robot = self.env.scene["robot"] - self.assertIsNotNone(robot, "Robot should not be None") - - def test_action_space(self) -> None: - """Test that the action space is correctly defined.""" - # Check action space - self.assertIsNotNone(self.env.action_space, "Action space should exist") - self.assertGreater( - self.env.action_space.shape[-1], 0, "Action space should have dimensions" - ) - - def test_observation_space(self) -> None: - """Test that the observation space is correctly defined.""" - # Check observation space - self.assertIsNotNone( - self.env.observation_space, "Observation space should exist" - ) - - # Check observation groups - if hasattr(self.cfg.observations, "sonic_tokenizer"): - self.assertIn( - "sonic_tokenizer", - self.env.observation_space.keys(), - "Tokenizer observation group should exist", - ) - - if hasattr(self.cfg.observations, "sonic_policy"): - self.assertIn( - "sonic_policy", - self.env.observation_space.keys(), - "Policy observation group should exist", - ) - - def test_step(self) -> None: - """Test that the environment can step with actions.""" - # Reset first - obs, info = self.env.reset() - - # Create random actions - actions = torch.zeros( - self.env.num_envs, self.env.action_space.shape[-1], device=self.env.device - ) - - # Step the environment - obs, rewards, terminated, truncated, info = self.env.step(actions) - - # Check outputs - self.assertIsNotNone(obs, "Observations should not be None") - self.assertIsNotNone(rewards, "Rewards should not be None") - self.assertIsNotNone(terminated, "Terminated should not be None") - self.assertIsNotNone(truncated, "Truncated should not be None") - - # Check shapes - self.assertEqual( - rewards.shape[0], self.env.num_envs, "Rewards should match num_envs" - ) - self.assertEqual( - terminated.shape[0], self.env.num_envs, "Terminated should match num_envs" - ) - self.assertEqual( - truncated.shape[0], self.env.num_envs, "Truncated should match num_envs" - ) - - -if __name__ == "__main__": - # Run tests - unittest.main() diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/__init__.py deleted file mode 100644 index c1e43a9c..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Utility modules for robotic grounding.""" - -from robotic_grounding.tasks.v2p_whole_body.utils.motion_dataset import * # noqa: F403 -from robotic_grounding.tasks.v2p_whole_body.utils.wandb_video_uploader import * # noqa: F403 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/motion_dataset.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/motion_dataset.py deleted file mode 100644 index ae5c14ec..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/motion_dataset.py +++ /dev/null @@ -1,260 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Motion dataset utility for loading and managing motion data for MMD reward computation.""" - -from __future__ import annotations - -import csv -from pathlib import Path - -import h5py -import numpy as np -import torch -import yaml -from isaaclab.assets import Articulation - - -class MotionDataset: - """ - Utility class for loading and managing motion datasets. - - This class handles loading motion data from various formats (CSV, H5, YAML), - aligning joint ordering between file format and robot, and sampling states - for MMD reward computation. - """ - - def __init__( - self, - dataset_path: str, - robot: Articulation, - file_joint_names: list[str] | None = None, - joint_order_file: str | None = None, - include_root: bool = False, - device: str = "cuda", - ) -> None: - """Initialize the motion dataset. - - Args: - dataset_path: Path to the motion dataset (directory with CSV files or single H5/YAML file). - robot: The robot articulation for joint ordering reference. - file_joint_names: List of joint names in the order they appear in the file. - If None, will try to load from joint_order_file or assume IsaacLab order. - joint_order_file: Path to a file containing joint ordering (one joint per line). - include_root: Whether to include root position/orientation in the state. - device: Device to store the data on. - """ - self.dataset_path = Path(dataset_path) - self.robot = robot - self.include_root = include_root - self.device = device - - # Get robot joint names (excluding root) - self.robot_joint_names = robot.joint_names - - # Load file joint order - if file_joint_names is not None: - self.file_joint_names = file_joint_names - elif joint_order_file is not None: - self.file_joint_names = self._load_joint_order_file(joint_order_file) - else: - # Assume file is in robot order - self.file_joint_names = self.robot_joint_names - - # Compute reordering indices (file order -> robot order) - self.reorder_indices = self._compute_reorder_indices() - - # Load all motion data - self.motion_data = self._load_dataset() - - # Extract joint positions as the feature for MMD - self.joint_states = self._extract_joint_states() - - print( - f"[MotionDataset] Loaded {len(self.joint_states)} states from {self.dataset_path}" - ) - print(f"[MotionDataset] State dimension: {self.joint_states.shape[1]}") - - def _load_joint_order_file(self, filepath: str) -> list[str]: - """Load joint order from a text file (one joint per line).""" - joint_names = [] - with open(filepath, "r") as f: - for raw_line in f: - line = raw_line.strip() - if line and not line.startswith("root"): # Skip root line - joint_names.append(line) - return joint_names - - def _compute_reorder_indices(self) -> torch.Tensor | None: - """Compute indices to reorder from file joint order to robot joint order. - - Returns: - Tensor of indices or None if orders match. - """ - # Find common joints between file and robot - common_joints = [] - file_indices = [] - robot_indices = [] - - for i, joint_name in enumerate(self.robot_joint_names): - if joint_name in self.file_joint_names: - file_idx = self.file_joint_names.index(joint_name) - common_joints.append(joint_name) - file_indices.append(file_idx) - robot_indices.append(i) - - if len(common_joints) == 0: - raise ValueError( - f"No common joints found between file ({self.file_joint_names[:5]}...) " - f"and robot ({self.robot_joint_names[:5]}...)" - ) - - print(f"[MotionDataset] Found {len(common_joints)} common joints") - - # Store mapping info - self.common_joints = common_joints - self.file_indices = file_indices - self.robot_indices = robot_indices - - return torch.tensor(file_indices, dtype=torch.long, device=self.device) - - def _load_dataset(self) -> list[np.ndarray]: - """Load motion data from the dataset path. - - Returns: - List of motion arrays, each with shape (T, state_dim). - """ - motion_data = [] - - if self.dataset_path.is_dir(): - # Load all CSV files in directory - csv_files = sorted(self.dataset_path.glob("*.csv")) - for csv_file in csv_files: - data = self._load_csv(csv_file) - if data is not None and len(data) > 0: - motion_data.append(data) - elif self.dataset_path.suffix == ".h5": - data = self._load_h5(self.dataset_path) - if data is not None and len(data) > 0: - motion_data.append(data) - elif self.dataset_path.suffix in [".yaml", ".yml"]: - data = self._load_yaml(self.dataset_path) - if data is not None and len(data) > 0: - motion_data.append(data) - else: - raise ValueError(f"Unsupported dataset format: {self.dataset_path}") - - if len(motion_data) == 0: - raise ValueError(f"No motion data loaded from {self.dataset_path}") - - return motion_data - - def _load_csv(self, filepath: Path) -> np.ndarray | None: - """Load motion data from a CSV file. - - CSV format: root_pos(3), root_quat(4), joints(N) per row. - """ - try: - data = [] - with open(filepath, "r") as f: - reader = csv.reader(f) - for row in reader: - if row: # Skip empty rows - values = [float(v) for v in row] - data.append(values) - if data: - return np.array(data, dtype=np.float32) - except Exception as e: - print(f"[MotionDataset] Warning: Failed to load {filepath}: {e}") - return None - - def _load_h5(self, filepath: Path) -> np.ndarray | None: - """Load motion data from an H5 file.""" - try: - with h5py.File(filepath, "r") as f: - if "qpos" in f: - return np.array(f["qpos"], dtype=np.float32) - except Exception as e: - print(f"[MotionDataset] Warning: Failed to load {filepath}: {e}") - return None - - def _load_yaml(self, filepath: Path) -> np.ndarray | None: - """Load motion data from a YAML file.""" - try: - with open(filepath, "r") as f: - data = yaml.safe_load(f) - if "qpos" in data: - return np.array(data["qpos"], dtype=np.float32) - except Exception as e: - print(f"[MotionDataset] Warning: Failed to load {filepath}: {e}") - return None - - def _extract_joint_states(self) -> torch.Tensor: - """Extract joint position states from all motion data. - - Returns: - Tensor of shape (N, joint_dim) containing all joint states. - """ - all_states = [] - - for motion in self.motion_data: - # Motion format: [root_pos(3), root_quat(4), joints(...)] - # Extract joint positions (skip root: 7 values) - if motion.shape[1] > 7: - joint_pos = motion[:, 7:] # (T, num_file_joints) - - # Reorder to robot joint order using common joints - if self.reorder_indices is not None: - # Extract only the joints that exist in file - joint_pos_reordered = joint_pos[:, self.file_indices] - all_states.append(joint_pos_reordered) - else: - all_states.append(joint_pos) - - if len(all_states) == 0: - raise ValueError("No joint states extracted from motion data") - - # Concatenate all states - all_states = np.concatenate(all_states, axis=0) - return torch.tensor(all_states, dtype=torch.float32, device=self.device) - - def sample(self, num_samples: int) -> torch.Tensor: - """Sample a subset of motion states. - - Args: - num_samples: Number of samples to return. - - Returns: - Tensor of shape (num_samples, joint_dim). - """ - total_samples = len(self.joint_states) - if num_samples >= total_samples: - return self.joint_states - - indices = torch.randperm(total_samples, device=self.device)[:num_samples] - return self.joint_states[indices] - - def get_all_states(self) -> torch.Tensor: - """Get all motion states. - - Returns: - Tensor of shape (N, joint_dim). - """ - return self.joint_states - - def get_joint_indices_for_robot(self) -> list[int]: - """Get the robot joint indices that are available in the dataset. - - Returns: - List of robot joint indices that have corresponding data in the dataset. - """ - return self.robot_indices - - @property - def num_states(self) -> int: - """Return the total number of states in the dataset.""" - return len(self.joint_states) - - @property - def state_dim(self) -> int: - """Return the dimension of each state.""" - return self.joint_states.shape[1] diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/wandb_video_uploader.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/wandb_video_uploader.py deleted file mode 100644 index 865471a7..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tasks/v2p_whole_body/utils/wandb_video_uploader.py +++ /dev/null @@ -1,134 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Background video uploader for wandb integration.""" - -import glob -import os -import re -import threading -import time - - -class WandbVideoUploader: - """Background thread that monitors a folder and uploads new videos to wandb.""" - - def __init__( - self, - video_folder: str, - check_interval: float = 30.0, - num_steps_per_env: int = 24, - wandb_key: str = "train/video", - ) -> None: - """Initialize the video uploader. - - Args: - video_folder: Path to the folder containing videos. - check_interval: How often to check for new videos (in seconds). - num_steps_per_env: Number of env steps per training iteration (for step conversion). - wandb_key: W&B metric key to log videos under (e.g. "train/video" or "eval/video"). - """ - self.video_folder = video_folder - self.check_interval = check_interval - self.num_steps_per_env = num_steps_per_env - self.wandb_key = wandb_key - self.uploaded_videos: set = set() - self._stop_event = threading.Event() - self._thread: threading.Thread | None = None - self._metric_defined = False - - def start(self) -> None: - """Start the background upload thread.""" - self._thread = threading.Thread(target=self._upload_loop, daemon=True) - self._thread.start() - print(f"[INFO] Started wandb video uploader for: {self.video_folder}") - - def stop(self) -> None: - """Stop the background upload thread and upload any remaining videos.""" - self._stop_event.set() - if self._thread is not None: - self._thread.join(timeout=5.0) - # Final upload of any remaining videos - self._upload_new_videos() - print( - f"[INFO] Stopped wandb video uploader. Uploaded {len(self.uploaded_videos)} videos total." - ) - - def _upload_loop(self) -> None: - """Main loop that checks for and uploads new videos.""" - while not self._stop_event.is_set(): - self._upload_new_videos() - # Wait for the check interval, but check stop event more frequently - for _ in range(int(self.check_interval)): - if self._stop_event.is_set(): - break - time.sleep(1.0) - - def _upload_new_videos(self) -> None: - """Find and upload any new videos.""" - try: - import wandb # noqa: PLC0415 - - if wandb.run is None: - return - - # Define custom metric for videos (allows out-of-order step logging) - if not self._metric_defined: - step_key = self.wandb_key.replace("/video", "/video_step") - wandb.define_metric(step_key) - wandb.define_metric(self.wandb_key, step_metric=step_key) - self._metric_defined = True - - # Find all mp4 files - video_files = glob.glob(os.path.join(self.video_folder, "*.mp4")) - - for video_path in video_files: - if video_path in self.uploaded_videos: - continue - - # Check if the file is still being written (size changing) - try: - size1 = os.path.getsize(video_path) - time.sleep(0.5) - size2 = os.path.getsize(video_path) - if size1 != size2: - # File is still being written, skip for now - continue - except OSError: - continue - - video_name = os.path.basename(video_path) - - # Extract env step from filename (e.g., rl-video-step-4800.mp4) - match = re.search(r"step-(\d+)", video_name) - if match: - env_step = int(match.group(1)) - # Convert env step to training iteration - training_iter = env_step // self.num_steps_per_env - else: - training_iter = None - - # Rename to video_train_{training_iter}.mp4 convention - if training_iter is not None: - new_name = f"video_train_{training_iter}.mp4" - new_path = os.path.join(self.video_folder, new_name) - if new_path != video_path: - os.rename(video_path, new_path) - video_path = new_path # noqa: PLW2901 - video_name = new_name - - # Upload to wandb with the correct training iteration - try: - step_key = self.wandb_key.replace("/video", "/video_step") - log_data = {self.wandb_key: wandb.Video(video_path, format="mp4")} - if training_iter is not None: - log_data[step_key] = training_iter - wandb.log(log_data, commit=False) - self.uploaded_videos.add(video_path) - print( - f"[INFO] Uploaded video to wandb: {video_name} (iter={training_iter})" - ) - except Exception as e: - print(f"[WARNING] Failed to upload video {video_name}: {e}") - - except Exception as e: - print(f"[WARNING] Error in video upload loop: {e}") diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/__init__.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/__init__.py deleted file mode 100644 index 52a7a9da..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 diff --git a/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/utils.py b/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/utils.py deleted file mode 100644 index 072e8827..00000000 --- a/robotic_grounding/source/robotic_grounding/robotic_grounding/tests/utils.py +++ /dev/null @@ -1,14 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import unittest - -from isaaclab.app import AppLauncher - -# Launch the simulator in headless mode for unit tests -app_launcher = AppLauncher(headless=True) -simulation_app = app_launcher.app -APP_IS_READY = True - -if __name__ == "__main__": - unittest.main(exit=False) - simulation_app.close() diff --git a/robotic_grounding/source/robotic_grounding/setup.py b/robotic_grounding/source/robotic_grounding/setup.py deleted file mode 100644 index 694a3cc8..00000000 --- a/robotic_grounding/source/robotic_grounding/setup.py +++ /dev/null @@ -1,45 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Installation script for the 'robotic_grounding' python package.""" - -import os - -import toml -from setuptools import setup - -# Obtain the extension data from the extension.toml file -EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) -# Read the extension.toml file -EXTENSION_TOML_DATA = toml.load( - os.path.join(EXTENSION_PATH, "config", "extension.toml") -) - -# Minimum dependencies required prior to installation -# NOTE: pytorch3d is not listed here because it must be installed with --no-build-isolation -# (so the build sees the environment's torch). Install it separately or use the project Dockerfile. -INSTALL_REQUIRES = [ - # NOTE: Add dependencies - "psutil", -] - -# Installation operation -setup( - name="robotic_grounding", - packages=["robotic_grounding"], - author=EXTENSION_TOML_DATA["package"]["author"], - maintainer=EXTENSION_TOML_DATA["package"]["maintainer"], - url=EXTENSION_TOML_DATA["package"]["repository"], - version=EXTENSION_TOML_DATA["package"]["version"], - description=EXTENSION_TOML_DATA["package"]["description"], - keywords=EXTENSION_TOML_DATA["package"]["keywords"], - install_requires=INSTALL_REQUIRES, - license="Apache 2.0", - include_package_data=True, - python_requires=">=3.11,<3.12", - classifiers=[ - "Natural Language :: English", - "Programming Language :: Python :: 3.11", - "Isaac Sim :: 5.1.0", - ], - zip_safe=False, -) diff --git a/robotic_grounding/tests/fixtures/regenerate_whole_body_kin_baseline.py b/robotic_grounding/tests/fixtures/regenerate_whole_body_kin_baseline.py deleted file mode 100644 index 91e5038a..00000000 --- a/robotic_grounding/tests/fixtures/regenerate_whole_body_kin_baseline.py +++ /dev/null @@ -1,127 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Regenerate the WholeBodyKinematics numerical-baseline fixture. - -Captures the exact per-frame IK outputs (``q``, ``frame_pose``, -``frame_task_errors``, ``num_optimization_iterations``) that -``WholeBodyKinematics`` produces on a synthetic SOMA-shaped input. -The companion pytest ``tests/test_whole_body_kinematics_baseline.py`` -replays the same input and asserts allclose at ``atol=1e-10``. - -The committed ``whole_body_kin_baseline.npz`` was captured against -the pre-merge class layout; the post-merge unified class must -reproduce it bit-for-bit. Re-run this helper only when the -canonical IK output is intentionally changed (e.g. a config schema -bump that legitimately alters the numbers), and update the PR -description accordingly. - -Usage (inside the robotic-grounding-latest-gpu0 container): - python tests/fixtures/regenerate_whole_body_kin_baseline.py -""" - -from __future__ import annotations - -import sys -from pathlib import Path - -import numpy as np -from robotic_grounding.retarget.params import SOMA_JOINTS_ORDER -from robotic_grounding.retarget.robot_config import load_robot_config -from robotic_grounding.retarget.whole_body_kinematics import WholeBodyKinematics - -FIXTURE_PATH = Path(__file__).parent / "whole_body_kin_baseline.npz" -SEED = 42 -N_FRAMES = 5 -SCALE = 1.0 - - -def _build_synthetic_soma_input( - n_frames: int, n_joints: int, seed: int -) -> tuple[np.ndarray, np.ndarray]: - """Return (positions, wxyz) of shape (T, J, 3) and (T, J, 4). - - Synthetic input is sufficient for a numerical-regression baseline: - the test only cares that the same input produces the same output - before and after the refactor. We pick a deterministic shape - loosely resembling a humanoid (vertical spread along z) so the - IK runs realistic convergence iterations rather than terminating - in one step on a degenerate input. - """ - rng = np.random.default_rng(seed) - # Rough body shape: joints spread vertically from 0 to 1.7m, then - # spread laterally by a seeded offset. Hips at index 0 sits near - # the bottom (the IK uses the lowest joint as ground anchor). - base_z = np.linspace(0.0, 1.7, n_joints) - base_x = 0.15 * rng.standard_normal(n_joints) - base_y = 0.15 * rng.standard_normal(n_joints) - base_pose = np.stack([base_x, base_y, base_z], axis=-1) - base_pose[0] = np.array([0.0, 0.0, 0.05]) # Hips near ground - - # Per-frame perturbation: small drift around the base pose so the - # frame-task targets actually change between frames (exercises the - # qpos-threading and posture-task-q_prev paths). - drift = 0.02 * rng.standard_normal((n_frames, n_joints, 3)) - positions = base_pose[None, :, :] + drift - - # Quaternions: small random perturbations from identity (wxyz), - # normalized. - wxyz = np.tile(np.array([1.0, 0.0, 0.0, 0.0]), (n_frames, n_joints, 1)) - wxyz[..., 1:] += 0.05 * rng.standard_normal((n_frames, n_joints, 3)) - wxyz /= np.linalg.norm(wxyz, axis=-1, keepdims=True) - return positions, wxyz - - -def main() -> int: - """Capture the WholeBodyKinematics baseline fixture and print a sanity record.""" - n_joints = len(SOMA_JOINTS_ORDER) - positions, wxyz = _build_synthetic_soma_input( - n_frames=N_FRAMES, n_joints=n_joints, seed=SEED - ) - - config = load_robot_config("g1") - kin = WholeBodyKinematics(config=config) - - q = kin.robot.q0.copy() - q_per_frame: list[np.ndarray] = [] - frame_pose_per_frame: list[np.ndarray] = [] - frame_task_errors_per_frame: list[np.ndarray] = [] - num_iters_per_frame: list[int] = [] - for t in range(N_FRAMES): - result = kin.compute( - source_joints=positions[t], - source_joints_wxyz=wxyz[t], - source_to_robot_scale=SCALE, - qpos=q, - ) - q_per_frame.append(result["q"].copy()) - frame_pose_per_frame.append(np.asarray(result["frame_pose"]).copy()) - frame_task_errors_per_frame.append( - np.asarray(result["frame_task_errors"], dtype=np.float64).copy() - ) - num_iters_per_frame.append(int(result["num_optimization_iterations"])) - q = result["q"].copy() - - FIXTURE_PATH.parent.mkdir(parents=True, exist_ok=True) - np.savez_compressed( - FIXTURE_PATH, - positions=positions, - wxyz=wxyz, - q=np.stack(q_per_frame), - frame_pose=np.stack(frame_pose_per_frame), - frame_task_errors=np.stack(frame_task_errors_per_frame), - num_iters=np.asarray(num_iters_per_frame, dtype=np.int64), - scale=np.float64(SCALE), - seed=np.int64(SEED), - ) - - # Sanity record for the PR description. - print(f"Wrote {FIXTURE_PATH}") - print(f" n_frames={N_FRAMES} n_joints={n_joints} seed={SEED}") - print(f" q[0, :7] = {q_per_frame[0][:7]}") - print(f" frame_task_errors[0]= {frame_task_errors_per_frame[0]}") - print(f" num_iters_per_frame = {num_iters_per_frame}") - return 0 - - -if __name__ == "__main__": - sys.exit(main()) diff --git a/robotic_grounding/tests/fixtures/whole_body_kin_baseline.npz b/robotic_grounding/tests/fixtures/whole_body_kin_baseline.npz deleted file mode 100644 index 8b878485..00000000 --- a/robotic_grounding/tests/fixtures/whole_body_kin_baseline.npz +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:57e4ad7497641b362f3a6f688a33a151f266e68341c150eac4966a7f67ff8012 -size 34933 diff --git a/robotic_grounding/tests/test_load_asset.py b/robotic_grounding/tests/test_load_asset.py deleted file mode 100644 index c6211f24..00000000 --- a/robotic_grounding/tests/test_load_asset.py +++ /dev/null @@ -1,61 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -import unittest - -from robotic_grounding.tests.utils import APP_IS_READY - -if APP_IS_READY: - import isaaclab.sim as sim_utils - from isaaclab.assets import ArticulationCfg, RigidObjectCfg - from isaaclab.scene import InteractiveScene, InteractiveSceneCfg - from isaaclab.utils import configclass - from robotic_grounding.assets import OBJECTS_ASSET_DIR - - # Import the robot configurations - from robotic_grounding.assets.g1 import G1_CYLINDER_CFG - - -@configclass -class LoadAssetSceneCfg(InteractiveSceneCfg): - """Configuration for a scene with G1 robot.""" - - robot: ArticulationCfg = G1_CYLINDER_CFG.replace(prim_path="/World/Robot") - - object: RigidObjectCfg = RigidObjectCfg( - prim_path="/World/Object", - spawn=sim_utils.UsdFileCfg( - usd_path=f"{OBJECTS_ASSET_DIR}/apple/apple.usd", - ), - ) - - -@unittest.skipIf(not APP_IS_READY, "App is not ready") -class TestLoadAsset(unittest.TestCase): - """Test that robot assets can be instantiated correctly.""" - - def setUp(self) -> None: - """Set up a simulation context for each test.""" - self.sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01)) - - # Create scene with G1 robot and object - scene_cfg = LoadAssetSceneCfg(num_envs=1, env_spacing=2.0) - self.scene = InteractiveScene(scene_cfg) - - def tearDown(self) -> None: - """Clean up after each test.""" - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - def test_instantiate_g1_robot(self) -> None: - """Test that G1 robot can be instantiated as an Articulation.""" - robot = self.scene["robot"] - self.assertIsNotNone(robot, "Robot not found") - - def test_instantiate_object(self) -> None: - """Test that object can be instantiated as a RigidObject.""" - object = self.scene["object"] - self.assertIsNotNone(object, "Object not found") - - -if __name__ == "__main__": - unittest.main() diff --git a/robotic_grounding/tests/test_motion_schema.py b/robotic_grounding/tests/test_motion_schema.py deleted file mode 100644 index 54866012..00000000 --- a/robotic_grounding/tests/test_motion_schema.py +++ /dev/null @@ -1,673 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for the `motion_v1` unified motion schema. - -Covers: - U1. Schema round-trip with every optional group populated. - U2. Minimal single-robot file (only required groups) is loadable. - U3. `schema_version` mismatch raises `SchemaVersionMismatch`. - U4. `hand_sides`-indexed alignment for single/bimanual. - U5. Quaternion convention guard (wxyz vs xyzw). - U6. `ee_pose_w` shape invariant for E in {1, 2, 3}. - K1-K5. `motion_kind` validation: dual-hand round-trip, missing/empty/ - unknown kind, single-robot/dual-hand required-field enforcement, - per-side alignment. - -Run with pytest or as a script (the latter is nice inside the isaac-sim -container where pytest may not be available): - - pytest tests/test_motion_schema.py - python tests/test_motion_schema.py -""" - -from __future__ import annotations - -import tempfile -import traceback -from pathlib import Path -from typing import Any - -import pyarrow as pa -import pyarrow.parquet as pq -from robotic_grounding.motion_schema import ( - SCHEMA_VERSION, - MissingRequiredField, - MotionData, - SchemaVersionMismatch, - build_schema, - load_motion_data_parquet, - save_motion_parquet, -) -from robotic_grounding.motion_schema.writer import _row_dict - -# --------------------------------------------------------------------------- -# Test fixtures -# --------------------------------------------------------------------------- - - -def _identity_wxyz(t: int) -> list[list[float]]: - return [[1.0, 0.0, 0.0, 0.0] for _ in range(t)] - - -def _pose7_series(t: int, e: int) -> list[list[list[float]]]: - """Build a (T, E, 7) pose series centered at origin with identity rotation.""" - return [[[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0] for _ in range(e)] for _ in range(t)] - - -def _minimal_motion_data( - t: int = 4, - j: int = 5, - e: int = 2, - num_bodies: int = 1, -) -> MotionData: - """Construct a single-robot MotionData with only the REQUIRED fields populated.""" - return MotionData( - sequence_id="seq_unit_test", - robot_name="test_robot", - motion_kind="single_robot", - source_dataset="synthetic", - raw_motion_file="memory://unit_test", - fps=30.0, - coord_frame="robot_base_z_up", - robot_joint_names=[f"joint_{i}" for i in range(j)], - robot_root_position=[[0.0, 0.0, 0.8] for _ in range(t)], - robot_root_wxyz=_identity_wxyz(t), - robot_joint_positions=[[0.0 for _ in range(j)] for _ in range(t)], - ee_link_names=[f"ee_{i}" for i in range(e)], - ee_pose_w=_pose7_series(t, e), - object_body_names=[f"body_{i}" for i in range(num_bodies)], - object_body_position=[ - [[0.3, 0.0, 0.4] for _ in range(num_bodies)] for _ in range(t) - ], - object_body_wxyz=[ - [[1.0, 0.0, 0.0, 0.0] for _ in range(num_bodies)] for _ in range(t) - ], - ) - - -def _minimal_dual_hand_motion_data( - t: int = 4, - k: int = 3, - jf: int = 4, - num_bodies: int = 1, -) -> MotionData: - """Construct a dual-hand MotionData mirroring the Dex3 producer shape. - - Whole-body joint state is intentionally left empty; required validation - must succeed for `motion_kind="dual_hand"` regardless of `robot_*` fields. - """ - sides = ["left", "right"] - return MotionData( - sequence_id="seq_dual_hand", - robot_name="dex3", - motion_kind="dual_hand", - source_dataset="synthetic", - raw_motion_file="memory://unit_test", - fps=30.0, - coord_frame="robot_base_z_up", - ee_link_names=["left_wrist_link", "right_wrist_link"], - ee_pose_w=_pose7_series(t, 2), - object_body_names=[f"body_{i}" for i in range(num_bodies)], - object_body_position=[ - [[0.3, 0.0, 0.4] for _ in range(num_bodies)] for _ in range(t) - ], - object_body_wxyz=[ - [[1.0, 0.0, 0.0, 0.0] for _ in range(num_bodies)] for _ in range(t) - ], - hand_sides=sides, - hand_frame_names=[[f"{side}_frame_{i}" for i in range(k)] for side in sides], - hand_frames_w=[_pose7_series(t, k), _pose7_series(t, k)], - hand_finger_joint_names=[ - [f"{side}_fj_{i}" for i in range(jf)] for side in sides - ], - hand_finger_joints=[ - [[0.0 for _ in range(jf)] for _ in range(t)], - [[0.0 for _ in range(jf)] for _ in range(t)], - ], - ) - - -def _fully_populated_motion_data(t: int = 4) -> MotionData: - """Construct a MotionData with every optional group populated.""" - j = 3 - e = 2 - k = 3 # hand frames per side - jf = 4 # finger joints per side - n = 2 # contact links per side - - md = _minimal_motion_data(t=t, j=j, e=e, num_bodies=1) - md.source_dataset = "soma" - md.ee_link_names = ["left_wrist_yaw_link", "right_wrist_yaw_link"] - md.safe_object_name = "body_0" - md.object_name = "body_0" - md.safe_object_body_names = ["body_0"] - md.object_mesh_paths = ["mesh://0"] - md.object_urdf_paths = ["urdf://0"] - md.object_mesh_radius = [0.05] - md.object_articulation = [0.0 for _ in range(t)] - md.object_root_axis_angle = [[0.0, 0.0, 0.0] for _ in range(t)] - md.object_root_position = [[0.3, 0.0, 0.4] for _ in range(t)] - - # Hands - md.hand_sides = ["left", "right"] - md.hand_frame_names = [ - [f"{side}_frame_{i}" for i in range(k)] for side in md.hand_sides - ] - md.hand_frames_w = [ - _pose7_series(t, k), - _pose7_series(t, k), - ] - md.hand_finger_joint_names = [ - [f"{side}_fj_{i}" for i in range(jf)] for side in md.hand_sides - ] - md.hand_finger_joints = [ - [[0.1 for _ in range(jf)] for _ in range(t)], - [[0.2 for _ in range(jf)] for _ in range(t)], - ] - - # Contacts - md.hand_contact_link_names = [ - [f"{side}_contact_{i}" for i in range(n)] for side in md.hand_sides - ] - zero_vec3_series = [[[0.0, 0.0, 0.0] for _ in range(n)] for _ in range(t)] - zero_part_ids = [[0 for _ in range(n)] for _ in range(t)] - md.hand_link_contact_positions = [zero_vec3_series, zero_vec3_series] - md.hand_link_contact_normals = [zero_vec3_series, zero_vec3_series] - md.hand_object_contact_positions = [zero_vec3_series, zero_vec3_series] - md.hand_object_contact_normals = [zero_vec3_series, zero_vec3_series] - md.hand_object_contact_part_ids = [zero_part_ids, zero_part_ids] - md.hand_contact_active = [[0.0 for _ in range(t)] for _ in md.hand_sides] - - # Source + diagnostics - md.source_kind = "soma" - md.source_payload = b"\x00\x01\x02" - md.source_joint_names = ["n0", "n1"] - md.ik_error_per_frame = [0.0 for _ in range(t)] - md.ik_num_iterations = [1 for _ in range(t)] - md.frame_task_errors = [[0.0, 0.0] for _ in range(t)] - - return md - - -def _round_trip(md: MotionData, tmp_dir: Path) -> MotionData: - save_motion_parquet(md, root_path=str(tmp_dir)) - partition_dir = ( - tmp_dir / f"sequence_id={md.sequence_id}" / f"robot_name={md.robot_name}" - ) - return load_motion_data_parquet(str(partition_dir)) - - -# --------------------------------------------------------------------------- -# U1. Schema round-trip with every optional group populated -# --------------------------------------------------------------------------- - - -def test_u1_full_roundtrip(tmp_path: Path) -> None: - """Every optional group populated, save then load, compare key fields.""" - md = _fully_populated_motion_data() - loaded = _round_trip(md, tmp_path) - - assert loaded.schema_version == SCHEMA_VERSION - assert loaded.sequence_id == md.sequence_id - assert loaded.robot_name == md.robot_name - assert loaded.source_dataset == "soma" - assert abs(loaded.fps - md.fps) < 1e-6 - - # Robot state - assert loaded.robot_joint_names == md.robot_joint_names - t = len(md.robot_root_position) - j = len(md.robot_joint_names) - assert tuple(loaded.robot_root_position.shape) == (t, 3) - assert tuple(loaded.robot_root_wxyz.shape) == (t, 4) - assert tuple(loaded.robot_joint_positions.shape) == (t, j) - - # EE - assert loaded.ee_link_names == md.ee_link_names - assert loaded.ee_pose_w is not None - assert tuple(loaded.ee_pose_w.shape) == (4, 2, 7) - assert tuple(loaded.ee_pos_w.shape) == (4, 2, 3) - assert tuple(loaded.ee_quat_w.shape) == (4, 2, 4) - - # Object - assert loaded.object_body_names == md.object_body_names - assert loaded.object_mesh_paths == md.object_mesh_paths - assert tuple(loaded.object_body_position.shape) == (4, 1, 3) - assert tuple(loaded.object_body_wxyz.shape) == (4, 1, 4) - assert loaded.object_pos_w is not None - assert tuple(loaded.object_pos_w.shape) == (4, 3) - - # Hands — on-disk view - assert loaded.hand_sides == ["left", "right"] - assert loaded.hand_frame_names == md.hand_frame_names - assert len(loaded.hand_frames_w) == 2 - - # Hands — flattened view - assert loaded.left_hand_frames is not None - assert loaded.right_hand_frames is not None - assert tuple(loaded.left_hand_frames.shape) == (4, 3, 7) - assert loaded.left_hand_frame_names == md.hand_frame_names[0] - assert loaded.right_hand_frame_names == md.hand_frame_names[1] - assert tuple(loaded.left_finger_joints.shape) == (4, 4) - assert tuple(loaded.right_finger_joints.shape) == (4, 4) - - # Wrists derived from ee - assert loaded.left_wrist_position is not None - assert tuple(loaded.left_wrist_position.shape) == (4, 3) - assert tuple(loaded.right_wrist_wxyz.shape) == (4, 4) - - # Contacts - assert loaded.left_link_contact_positions is not None - assert loaded.right_object_contact_part_ids is not None - assert tuple(loaded.left_link_contact_positions.shape) == (4, 2, 3) - - # Source + diagnostics - assert loaded.source_kind == "soma" - assert loaded.source_payload == b"\x00\x01\x02" - assert loaded.ik_error_per_frame is not None - - -# --------------------------------------------------------------------------- -# U2. Minimal file — only required groups are populated -# --------------------------------------------------------------------------- - - -def test_u2_minimal_file_loads(tmp_path: Path) -> None: - """Round-trip a MotionData with no hands, no contacts, no source.""" - md = _minimal_motion_data() - loaded = _round_trip(md, tmp_path) - - assert loaded.schema_version == SCHEMA_VERSION - assert loaded.robot_root_position is not None - assert loaded.robot_joint_positions is not None - assert loaded.ee_pose_w is not None - - # Optional groups must all be None / empty on the flattened view. - for attr in ( - "left_wrist_position", - "left_wrist_wxyz", - "right_wrist_position", - "right_wrist_wxyz", - "left_hand_frames", - "right_hand_frames", - "left_finger_joints", - "right_finger_joints", - "left_link_contact_positions", - "right_link_contact_positions", - "left_object_contact_part_ids", - "left_hand_contact_active", - "ik_error_per_frame", - ): - assert getattr(loaded, attr) is None, f"expected {attr} to be None" - - assert loaded.hand_sides == [] - assert loaded.source_kind == "" - assert loaded.source_payload == b"" - - -# --------------------------------------------------------------------------- -# U3. schema_version enforcement -# --------------------------------------------------------------------------- - - -def _write_file_with_version(path: Path, version: str) -> Path: - """Build a parquet with a fake schema_version but otherwise valid columns.""" - md = _minimal_motion_data() - md.schema_version = version # bypass writer's forced SCHEMA_VERSION - # We have to build the row dict ourselves because save_motion_parquet - # always overwrites schema_version to SCHEMA_VERSION. - row = _row_dict(md) - row["schema_version"] = [version] - table = pa.Table.from_pydict(row, schema=build_schema()) - partition_dir = ( - path / f"sequence_id={md.sequence_id}" / f"robot_name={md.robot_name}" - ) - partition_dir.mkdir(parents=True, exist_ok=True) - file_path = partition_dir / "data.parquet" - pq.write_table(table, str(file_path)) - return file_path - - -def test_u3_version_mismatch_raises(tmp_path: Path) -> None: - """Reader raises SchemaVersionMismatch with an actionable message.""" - file_path = _write_file_with_version(tmp_path, version="motion_v0") - try: - load_motion_data_parquet(str(file_path)) - except SchemaVersionMismatch as exc: - assert "motion_v0" in str(exc) - assert SCHEMA_VERSION in str(exc) - assert str(file_path) in str(exc) - return - raise AssertionError("expected SchemaVersionMismatch to be raised") - - -def test_u3_missing_version_raises(tmp_path: Path) -> None: - """Reader raises if schema_version is empty string.""" - file_path = _write_file_with_version(tmp_path, version="") - try: - load_motion_data_parquet(str(file_path)) - except SchemaVersionMismatch: - return - raise AssertionError("expected SchemaVersionMismatch for empty version") - - -# --------------------------------------------------------------------------- -# U4. hand_sides alignment (single-hand right-only) -# --------------------------------------------------------------------------- - - -def test_u4_single_hand_right_only(tmp_path: Path) -> None: - """A right-only file populates right_* attributes and leaves left_* None.""" - t, k, jf, n = 4, 2, 3, 2 - md = _minimal_motion_data(t=t) - md.ee_link_names = ["right_wrist_yaw_link"] - md.ee_pose_w = _pose7_series(t, 1) - - md.hand_sides = ["right"] - md.hand_frame_names = [[f"right_frame_{i}" for i in range(k)]] - md.hand_frames_w = [_pose7_series(t, k)] - md.hand_finger_joint_names = [[f"right_fj_{i}" for i in range(jf)]] - md.hand_finger_joints = [[[0.0 for _ in range(jf)] for _ in range(t)]] - md.hand_contact_link_names = [[f"right_contact_{i}" for i in range(n)]] - zero_vec3 = [[[0.0, 0.0, 0.0] for _ in range(n)] for _ in range(t)] - md.hand_link_contact_positions = [zero_vec3] - md.hand_link_contact_normals = [zero_vec3] - md.hand_object_contact_positions = [zero_vec3] - md.hand_object_contact_normals = [zero_vec3] - md.hand_object_contact_part_ids = [[[0 for _ in range(n)] for _ in range(t)]] - md.hand_contact_active = [[0.0 for _ in range(t)]] - - loaded = _round_trip(md, tmp_path) - - # Right-side populated. - assert loaded.right_hand_frames is not None - assert tuple(loaded.right_hand_frames.shape) == (t, k, 7) - assert loaded.right_finger_joints is not None - assert tuple(loaded.right_finger_joints.shape) == (t, jf) - assert loaded.right_link_contact_positions is not None - assert loaded.right_hand_contact_active is not None - assert loaded.right_wrist_position is not None - - # Left-side strictly None (no blind [0, 1] indexing). - for attr in ( - "left_hand_frames", - "left_hand_frame_names", - "left_finger_joints", - "left_link_contact_positions", - "left_object_contact_positions", - "left_hand_contact_active", - "left_wrist_position", - "left_wrist_wxyz", - ): - assert ( - getattr(loaded, attr) is None - ), f"expected {attr} to be None in single-hand case" - - assert loaded.hand_sides == ["right"] - - -# --------------------------------------------------------------------------- -# U5. Quaternion convention guard -# --------------------------------------------------------------------------- - - -def test_u5_writer_rejects_xyzw(tmp_path: Path) -> None: - """Writer raises when quaternions look like xyzw instead of wxyz.""" - md = _minimal_motion_data() - # Build an xyzw-like series (real part last). This should fail the guard. - md.robot_root_wxyz = [[0.0, 0.0, 0.0, 1.0] for _ in range(4)] - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "wxyz" in str(exc) - return - raise AssertionError("expected ValueError when writing xyzw-ordered quaternions") - - -def test_u5_writer_accepts_plausible_rotations(tmp_path: Path) -> None: - """Writer accepts quaternions with moderate rotations (w still dominant enough).""" - md = _minimal_motion_data() - # 45deg about z axis: w=cos(22.5deg)~=0.924, z=sin(22.5deg)~=0.383. Still wxyz-ish. - w, z = 0.924, 0.383 - md.robot_root_wxyz = [[w, 0.0, 0.0, z] for _ in range(4)] - save_motion_parquet(md, root_path=str(tmp_path)) # should not raise - - -# --------------------------------------------------------------------------- -# U6. ee_pose_w shape invariant (E in {1, 2, 3}) -# --------------------------------------------------------------------------- - - -def test_u6_variable_num_ee(tmp_path: Path) -> None: - """ee_pose_w round-trips for E in {1, 2, 3} without reshape regressions.""" - for e in (1, 2, 3): - t = 3 - md = _minimal_motion_data(t=t, e=e) - md.ee_link_names = [f"ee_{i}" for i in range(e)] - md.ee_pose_w = _pose7_series(t, e) - sub_dir = tmp_path / f"e_{e}" - loaded = _round_trip(md, sub_dir) - assert tuple(loaded.ee_pose_w.shape) == (t, e, 7), f"E={e}" - assert tuple(loaded.ee_pos_w.shape) == (t, e, 3), f"E={e}" - assert tuple(loaded.ee_quat_w.shape) == (t, e, 4), f"E={e}" - - -# --------------------------------------------------------------------------- -# Missing-required-field behaviour -# --------------------------------------------------------------------------- - - -def test_missing_required_field_raises_on_write(tmp_path: Path) -> None: - """Writer fails fast with a pointer to the missing required field.""" - md = _minimal_motion_data() - md.ee_pose_w = None # common-required, removed - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "ee_pose_w" in str(exc) - return - raise AssertionError("expected ValueError from writer") - - -def test_missing_required_field_raises_on_read(tmp_path: Path) -> None: - """If a file lacks a required column at read time, reader raises.""" - md = _minimal_motion_data() - # Write via the writer but then truncate the column. - save_motion_parquet(md, root_path=str(tmp_path)) - # Rewrite with one required column cleared. - partition_dir = ( - tmp_path / f"sequence_id={md.sequence_id}" / f"robot_name={md.robot_name}" - ) - parquet_files = list(partition_dir.glob("*.parquet")) - assert len(parquet_files) == 1 - file_path = parquet_files[0] - table = pq.ParquetFile(str(file_path)).read() - pydict = table.to_pydict() - # pq.write_to_dataset strips partition columns from the file body; - # rebuild them here before writing the full-schema table back. - pydict["sequence_id"] = [md.sequence_id] - pydict["robot_name"] = [md.robot_name] - pydict["ee_pose_w"] = [None] - new_table = pa.Table.from_pydict(pydict, schema=build_schema()) - pq.write_table(new_table, str(file_path)) - - try: - load_motion_data_parquet(str(partition_dir)) - except MissingRequiredField as exc: - assert "ee_pose_w" in exc.missing - return - raise AssertionError("expected MissingRequiredField") - - -# --------------------------------------------------------------------------- -# K1-K5. motion_kind validation -# --------------------------------------------------------------------------- - - -def test_dual_hand_round_trip(tmp_path: Path) -> None: - """Dex3-style dual-hand file round-trips without whole-body joints.""" - md = _minimal_dual_hand_motion_data() - save_motion_parquet(md, root_path=str(tmp_path)) - partition_dir = ( - tmp_path / f"sequence_id={md.sequence_id}" / f"robot_name={md.robot_name}" - ) - loaded = load_motion_data_parquet(str(partition_dir)) - - assert loaded.motion_kind == "dual_hand" - assert loaded.robot_joint_names == [] - assert loaded.hand_sides == ["left", "right"] - assert loaded.left_hand_frames is not None - assert loaded.right_hand_frames is not None - assert loaded.left_finger_joints is not None - assert loaded.right_finger_joints is not None - # Whole-body fields stay None for dual-hand parquets. - assert loaded.robot_root_position is None - assert loaded.robot_joint_positions is None - - -def test_single_robot_missing_joints_raises(tmp_path: Path) -> None: - """`motion_kind=single_robot` with empty robot_joint_names raises.""" - md = _minimal_motion_data() - md.robot_joint_names = [] - md.robot_joint_positions = [] - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "robot_joint_names" in str(exc) - return - raise AssertionError("expected ValueError for single_robot without joints") - - -def test_dual_hand_missing_hand_sides_raises(tmp_path: Path) -> None: - """`motion_kind=dual_hand` without `hand_sides` raises on write.""" - md = _minimal_dual_hand_motion_data() - md.hand_sides = [] - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "hand_sides" in str(exc) - return - raise AssertionError("expected ValueError for dual_hand without hand_sides") - - -def test_dual_hand_misaligned_per_side_raises(tmp_path: Path) -> None: - """Per-side outer length must equal len(hand_sides).""" - md = _minimal_dual_hand_motion_data() - md.hand_finger_joints = [md.hand_finger_joints[0]] - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "hand_finger_joints" in str(exc) - assert "align" in str(exc) - return - raise AssertionError( - "expected ValueError for dual_hand with misaligned per-side data" - ) - - -def test_missing_motion_kind_raises_on_write(tmp_path: Path) -> None: - """Writer rejects a MotionData with empty motion_kind.""" - md = _minimal_motion_data() - md.motion_kind = "" - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "motion_kind" in str(exc) - return - raise AssertionError("expected ValueError for missing motion_kind on write") - - -def test_missing_motion_kind_raises_on_read(tmp_path: Path) -> None: - """Reader rejects a parquet whose motion_kind column is empty.""" - md = _minimal_motion_data() - save_motion_parquet(md, root_path=str(tmp_path)) - partition_dir = ( - tmp_path / f"sequence_id={md.sequence_id}" / f"robot_name={md.robot_name}" - ) - parquet_files = list(partition_dir.glob("*.parquet")) - assert len(parquet_files) == 1 - file_path = parquet_files[0] - table = pq.ParquetFile(str(file_path)).read() - pydict = table.to_pydict() - pydict["sequence_id"] = [md.sequence_id] - pydict["robot_name"] = [md.robot_name] - pydict["motion_kind"] = [""] - new_table = pa.Table.from_pydict(pydict, schema=build_schema()) - pq.write_table(new_table, str(file_path)) - - try: - load_motion_data_parquet(str(partition_dir)) - except MissingRequiredField as exc: - assert "motion_kind" in exc.missing - return - raise AssertionError("expected MissingRequiredField for empty motion_kind") - - -def test_unknown_motion_kind_raises_on_write(tmp_path: Path) -> None: - """Writer rejects an unrecognised motion_kind value.""" - md = _minimal_motion_data() - md.motion_kind = "quadruped" - try: - save_motion_parquet(md, root_path=str(tmp_path)) - except ValueError as exc: - assert "quadruped" in str(exc) or "Unknown motion_kind" in str(exc) - return - raise AssertionError("expected ValueError for unknown motion_kind") - - -# --------------------------------------------------------------------------- -# Script entry point -# --------------------------------------------------------------------------- - - -TESTS: list[tuple[str, Any]] = [ - ("U1 full round-trip", test_u1_full_roundtrip), - ("U2 minimal file loads", test_u2_minimal_file_loads), - ("U3 version mismatch raises", test_u3_version_mismatch_raises), - ("U3 missing version raises", test_u3_missing_version_raises), - ("U4 single-hand right only", test_u4_single_hand_right_only), - ("U5 writer rejects xyzw", test_u5_writer_rejects_xyzw), - ("U5 writer accepts rotations", test_u5_writer_accepts_plausible_rotations), - ("U6 variable num ee", test_u6_variable_num_ee), - ("missing required raises on write", test_missing_required_field_raises_on_write), - ("missing required raises on read", test_missing_required_field_raises_on_read), - ("K1 dual-hand round-trip", test_dual_hand_round_trip), - ("K2 single_robot missing joints raises", test_single_robot_missing_joints_raises), - ( - "K3 dual_hand missing hand_sides raises", - test_dual_hand_missing_hand_sides_raises, - ), - ( - "K4 dual_hand misaligned per-side raises", - test_dual_hand_misaligned_per_side_raises, - ), - ( - "K5 missing motion_kind raises on write", - test_missing_motion_kind_raises_on_write, - ), - ("K5 missing motion_kind raises on read", test_missing_motion_kind_raises_on_read), - ( - "K5 unknown motion_kind raises on write", - test_unknown_motion_kind_raises_on_write, - ), -] - - -def _run_as_script() -> int: - """Run all tests without pytest.""" - failures = 0 - for name, fn in TESTS: - with tempfile.TemporaryDirectory(prefix="motion_schema_test_") as tmp_dir: - print(f"[RUN] {name}", flush=True) - try: - fn(Path(tmp_dir)) - print(f"[PASS] {name}", flush=True) - except Exception: - print(f"[FAIL] {name}", flush=True) - traceback.print_exc() - failures += 1 - print(f"\n{len(TESTS) - failures}/{len(TESTS)} passed.", flush=True) - return 1 if failures else 0 - - -if __name__ == "__main__": - raise SystemExit(_run_as_script()) diff --git a/robotic_grounding/tests/test_motion_schema_parquet_integration.py b/robotic_grounding/tests/test_motion_schema_parquet_integration.py deleted file mode 100644 index f012a304..00000000 --- a/robotic_grounding/tests/test_motion_schema_parquet_integration.py +++ /dev/null @@ -1,190 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Integration test for the `motion_v1` parquet pipeline. - -Writes a motion_v1 parquet, loads it via SceneConfig + the unified reader, -and asserts the usual round-trip invariants. - -Usage (pytest): - pytest tests/test_motion_schema_parquet_integration.py - -Usage (direct): - python tests/test_motion_schema_parquet_integration.py -""" - -from __future__ import annotations - -import importlib.util -import sys -import tempfile -import traceback -from pathlib import Path - -from robotic_grounding.motion_schema import ( - SCHEMA_VERSION, - MotionData, - load_motion_data_parquet, - save_motion_parquet, -) - -try: - from robotic_grounding.tasks.scene_utils.scene_config import SceneConfig -except ModuleNotFoundError: - # Fallback for environments without IsaacLab/Omniverse modules where - # importing robotic_grounding.tasks triggers heavy runtime dependencies. - scene_config_path = ( - Path(__file__).resolve().parents[1] - / "source" - / "robotic_grounding" - / "robotic_grounding" - / "tasks" - / "scene_utils" - / "scene_config.py" - ) - spec = importlib.util.spec_from_file_location( - "scene_config_fallback", scene_config_path - ) - if spec is None or spec.loader is None: - raise ImportError( - f"Could not load SceneConfig from {scene_config_path}" - ) from None - scene_config_module = importlib.util.module_from_spec(spec) - sys.modules[spec.name] = scene_config_module - spec.loader.exec_module(scene_config_module) - SceneConfig = scene_config_module.SceneConfig - - -def _write_test_mesh(mesh_path: Path) -> None: - mesh_path.parent.mkdir(parents=True, exist_ok=True) - mesh_path.write_text( - "\n".join( - [ - "o object", - "v 0.0 0.0 0.0", - "v 0.1 0.0 0.0", - "v 0.0 0.1 0.0", - "f 1 2 3", - ] - ), - encoding="utf-8", - ) - - -def _write_test_urdf(urdf_path: Path, mesh_path: Path) -> None: - urdf_path.write_text( - f""" - - - - - - - - - - - - - - -""", - encoding="utf-8", - ) - - -def test_motion_v1_parquet_roundtrip_scene_config_and_loader(tmp_path: Path) -> None: - """Write motion_v1 parquet, load via SceneConfig + unified reader.""" - mesh_path = tmp_path / "object" / "textured_mesh.obj" - urdf_path = tmp_path / "object" / "textured_mesh.urdf" - _write_test_mesh(mesh_path) - _write_test_urdf(urdf_path, mesh_path) - - sequence_id = "seq_test_001" - robot_name = "g1" - t = 3 - md = MotionData( - sequence_id=sequence_id, - robot_name=robot_name, - motion_kind="single_robot", - source_dataset="soma", - raw_motion_file=str(tmp_path / "nova_params_opt.pt"), - fps=30.0, - coord_frame="robot_base_z_up", - robot_joint_names=["left_knee_joint", "right_knee_joint"], - robot_root_position=[[0.0, 0.0, 0.8] for _ in range(t)], - robot_root_wxyz=[[1.0, 0.0, 0.0, 0.0] for _ in range(t)], - robot_joint_positions=[[0.1, -0.1] for _ in range(t)], - ee_link_names=["left_hand_palm_link", "right_hand_palm_link"], - ee_pose_w=[ - [ - [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0], - [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0], - ] - for _ in range(t) - ], - object_name="seq_test_object", - safe_object_name="seq_test_object", - object_body_names=["object"], - safe_object_body_names=["object"], - object_mesh_paths=[str(mesh_path.resolve())], - object_urdf_paths=[str(urdf_path.resolve())], - object_mesh_radius=[0.1], - object_articulation=[0.0 for _ in range(t)], - object_root_axis_angle=[[0.0, 0.0, 0.0] for _ in range(t)], - object_root_position=[[0.2, 0.3, 0.4] for _ in range(t)], - object_body_position=[[[0.2, 0.3, 0.4]] for _ in range(t)], - object_body_wxyz=[[[1.0, 0.0, 0.0, 0.0]] for _ in range(t)], - ) - - output_dir = tmp_path / "motion_v1_processed" - save_motion_parquet(md, root_path=str(output_dir)) - - # Round-trip via the unified reader. - loaded = load_motion_data_parquet( - str(output_dir / f"sequence_id={sequence_id}" / f"robot_name={robot_name}") - ) - assert loaded.schema_version == SCHEMA_VERSION - assert loaded.object_body_names == ["object"] - assert loaded.object_mesh_paths == [str(mesh_path.resolve())] - assert loaded.object_urdf_paths == [str(urdf_path.resolve())] - assert loaded.robot_joint_names == md.robot_joint_names - assert tuple(loaded.robot_root_position.shape) == (t, 3) - assert tuple(loaded.robot_joint_positions.shape) == (t, 2) - - # SceneConfig must still build from the same partition directory. - partition_dir = ( - output_dir / f"sequence_id={sequence_id}" / f"robot_name={robot_name}" - ) - scene_cfg = SceneConfig.from_motion_file(str(partition_dir)) - assert scene_cfg.object_body_names == ["object"] - assert len(scene_cfg.scene_objects) == 1 - assert getattr(scene_cfg.scene_objects[0], "name", None) == "object" - assert getattr(scene_cfg.scene_objects[0], "usd_path", None) == str( - urdf_path.resolve() - ) - - -def _run_as_script() -> int: - """Run this test file directly without pytest.""" - test_name = "test_motion_v1_parquet_roundtrip_scene_config_and_loader" - print("=" * 72, flush=True) - print("Running motion_v1 parquet integration test", flush=True) - print(f"Test: {test_name}", flush=True) - print("=" * 72, flush=True) - try: - with tempfile.TemporaryDirectory(prefix="motion_v1_test_") as tmp_dir: - tmp_path = Path(tmp_dir) - print(f"[SETUP] Temporary directory: {tmp_path}", flush=True) - test_motion_v1_parquet_roundtrip_scene_config_and_loader(tmp_path) - print(f"[PASS] {test_name}", flush=True) - return 0 - except Exception: - print(f"[FAIL] {test_name}", flush=True) - print("-" * 72, flush=True) - traceback.print_exc() - print("-" * 72, flush=True) - return 1 - - -if __name__ == "__main__": - raise SystemExit(_run_as_script()) diff --git a/robotic_grounding/tests/test_reference_plane.py b/robotic_grounding/tests/test_reference_plane.py deleted file mode 100644 index 8e6233eb..00000000 --- a/robotic_grounding/tests/test_reference_plane.py +++ /dev/null @@ -1,108 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Unit tests for the generalized ``ReferencePlane`` (normal + offset).""" - -from __future__ import annotations - -import numpy as np -import pytest -from robotic_grounding.retarget.ground_alignment import ReferencePlane - - -def test_horizontal_factory_matches_legacy_signed_distance() -> None: - """`horizontal(z=z0)` produces the same signed_distance as the legacy form.""" - plane = ReferencePlane.horizontal(z=0.0) - pts = np.array([[0.0, 0.0, 0.0], [1.0, 2.0, 0.25], [3.0, 4.0, -0.10]]) - np.testing.assert_allclose(plane.signed_distance(pts), [0.0, 0.25, -0.10]) - - plane_z1 = ReferencePlane.horizontal(z=1.0) - np.testing.assert_allclose(plane_z1.signed_distance(pts), [-1.0, -0.75, -1.10]) - - -def test_inclined_signed_distance() -> None: - """A non-horizontal plane's signed_distance equals ``n . p + offset``.""" - # Plane through the origin tilted ~45 deg around the X axis: normal ~ (0, -1, 1). - plane = ReferencePlane(normal=(0.0, -1.0, 1.0), offset=0.0) - pt_on = np.array([0.0, 1.0, 1.0]) - np.testing.assert_allclose(plane.signed_distance(pt_on), 0.0, atol=1e-12) - pt_above = np.array([0.0, 0.0, 0.5]) - expected = float(np.dot(np.array([0.0, -1.0, 1.0]) / np.sqrt(2.0), pt_above)) - np.testing.assert_allclose(plane.signed_distance(pt_above), expected) - - -def test_vertical_offset_to_plane_horizontal_equivalence() -> None: - """For a horizontal plane, ``vertical_offset_to_plane = -signed_distance``.""" - plane = ReferencePlane.horizontal(z=0.5) - pts = np.array([[0.0, 0.0, 0.4], [0.0, 0.0, 1.5]]) - np.testing.assert_allclose( - plane.vertical_offset_to_plane(pts), -plane.signed_distance(pts) - ) - - -def test_vertical_offset_to_plane_inclined_uses_normal_z() -> None: - """On a tilted plane, dz = -signed_distance / normal_z.""" - plane = ReferencePlane(normal=(0.0, -1.0, 1.0), offset=0.0) - pts = np.array([[0.0, 0.0, 1.0], [0.0, 0.5, 1.0]]) - sd = plane.signed_distance(pts) - np.testing.assert_allclose( - plane.vertical_offset_to_plane(pts), -sd / plane.normal_z - ) - # Sanity: applying dz to Z should drive signed_distance to zero. - pts_corrected = pts.copy() - pts_corrected[:, 2] += plane.vertical_offset_to_plane(pts) - np.testing.assert_allclose(plane.signed_distance(pts_corrected), 0.0, atol=1e-12) - - -def test_normal_is_normalized() -> None: - """Constructor normalizes ``normal`` regardless of input magnitude.""" - plane = ReferencePlane(normal=(0.0, 0.0, 5.0), offset=-2.0) - np.testing.assert_allclose(plane.normal, (0.0, 0.0, 1.0)) - # Offset is rescaled so the plane equation is preserved: a point at z=2 - # remains on the plane. - pt = np.array([0.0, 0.0, 2.0]) - np.testing.assert_allclose(plane.signed_distance(pt), 0.0, atol=1e-12) - - -def test_orient_normal_so_normal_z_positive() -> None: - """A plane built with normal_z < 0 is flipped so normal_z > 0. - - Input: ``-z - 3 = 0`` -> plane lies at ``z = -3``. - After flip: ``z + 3 = 0`` -> still ``z = -3``. Both forms represent - the same plane; the canonical form has normal_z > 0. - """ - plane = ReferencePlane(normal=(0.0, 0.0, -1.0), offset=-3.0) - np.testing.assert_allclose(plane.normal, (0.0, 0.0, 1.0)) - np.testing.assert_allclose(plane.offset, 3.0) - pt = np.array([0.0, 0.0, -3.0]) - np.testing.assert_allclose(plane.signed_distance(pt), 0.0, atol=1e-12) - - -def test_near_vertical_plane_rejected() -> None: - """A near-horizontal-Z plane raises (invalid for foot anchoring).""" - with pytest.raises(ValueError): - ReferencePlane(normal=(1.0, 0.0, 0.0), offset=0.0) - - -def test_zero_normal_rejected() -> None: - """A zero-length normal raises.""" - with pytest.raises(ValueError): - ReferencePlane(normal=(0.0, 0.0, 0.0), offset=0.0) - - -def test_horizontal_signed_distance_supports_leading_batch_dims() -> None: - """Works for ``(T, K, 3)`` arrays without a reshape on the caller.""" - plane = ReferencePlane.horizontal(z=0.0) - pts = np.zeros((5, 2, 3), dtype=np.float64) - pts[:, 0, 2] = np.linspace(0.0, 0.4, 5) - pts[:, 1, 2] = 0.1 - d = plane.signed_distance(pts) - assert d.shape == (5, 2) - np.testing.assert_allclose(d[:, 0], np.linspace(0.0, 0.4, 5)) - np.testing.assert_allclose(d[:, 1], 0.1) - - -def test_signed_distance_rejects_wrong_trailing_dim() -> None: - """Non-3 last axis raises.""" - plane = ReferencePlane.horizontal() - with pytest.raises(ValueError): - plane.signed_distance(np.zeros((3, 2))) diff --git a/robotic_grounding/tests/test_replay_data.py b/robotic_grounding/tests/test_replay_data.py deleted file mode 100644 index 4187f682..00000000 --- a/robotic_grounding/tests/test_replay_data.py +++ /dev/null @@ -1,363 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Tests for the replay data adapter (schema detection, trajectory loading, joint reorder). - -These tests run without Isaac Lab / Omniverse — they only exercise the Parquet -adapter and joint-reorder logic. - -Usage (pytest): - pytest tests/test_replay_data.py -v - -Usage (direct): - python tests/test_replay_data.py -""" - -import importlib.util -import inspect -import sys -import tempfile -import traceback -from collections.abc import Callable -from pathlib import Path -from typing import Any - -import numpy as np -import torch -from robotic_grounding.motion_schema import MotionData, save_motion_parquet -from robotic_grounding.retarget.data_logger import ManoSharpaData - -# replay_data lives under robotic_grounding.tasks which transitively imports -# Isaac Lab / Omniverse. Use the same importlib fallback as the existing -# test_motion_schema_parquet_integration.py so the test runs without Omniverse. -_SCENE_UTILS_DIR = ( - Path(__file__).resolve().parents[1] - / "source" - / "robotic_grounding" - / "robotic_grounding" - / "tasks" - / "scene_utils" -) - - -def _load_module_directly(name: str, path: Path) -> Any: - """Load a single .py module without triggering __init__ chains.""" - spec = importlib.util.spec_from_file_location(name, path) - if spec is None or spec.loader is None: - raise ImportError(f"Could not load {name} from {path}") - mod = importlib.util.module_from_spec(spec) - sys.modules[name] = mod - spec.loader.exec_module(mod) - return mod - - -# scene_config must be loaded first because replay_data imports it. -_scene_config_mod = _load_module_directly( - "robotic_grounding.tasks.scene_utils.scene_config", - _SCENE_UTILS_DIR / "scene_config.py", -) -_replay_data_mod = _load_module_directly( - "robotic_grounding.tasks.scene_utils.replay_data", - _SCENE_UTILS_DIR / "replay_data.py", -) - -DualHandTrajectory = _replay_data_mod.DualHandTrajectory -SingleRobotTrajectory = _replay_data_mod.SingleRobotTrajectory -load_replay_trajectory = _replay_data_mod.load_replay_trajectory - - -def _build_joint_reorder( - parquet_names: list[str], sim_names: list[str] -) -> torch.Tensor | None: - """Inline copy of the reorder helper (avoids importing replay_motion which needs Isaac Lab).""" - if parquet_names == sim_names: - return None - sim_name_to_idx = {n: i for i, n in enumerate(sim_names)} - indices: list[int] = [] - for pq_name in parquet_names: - if pq_name not in sim_name_to_idx: - raise ValueError( - f"Parquet joint '{pq_name}' not found in spawned robot joints: {sim_names}" - ) - indices.append(sim_name_to_idx[pq_name]) - return torch.tensor(indices, dtype=torch.long) - - -# ============================================================ -# motion_v1 whole-body trajectory round-trip -# ============================================================ - - -def _write_motion_v1_g1_parquet(output_dir: Path) -> Path: - """Write a minimal motion_v1 whole-body parquet and return the partition dir.""" - seq_id = "replay_test_seq" - robot_name = "g1" - joint_names = ["joint_a", "joint_b"] - t = 5 - md = MotionData( - sequence_id=seq_id, - robot_name=robot_name, - motion_kind="single_robot", - source_dataset="soma", - raw_motion_file="fake.pt", - fps=30.0, - coord_frame="robot_base_z_up", - robot_joint_names=joint_names, - robot_root_position=[[0.0, 0.0, 0.1 * i] for i in range(t)], - robot_root_wxyz=[[1.0, 0.0, 0.0, 0.0] for _ in range(t)], - robot_joint_positions=[[0.1 * i, -0.1 * i] for i in range(t)], - ee_link_names=["left_hand_palm_link", "right_hand_palm_link"], - ee_pose_w=[ - [[0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.8, 1.0, 0.0, 0.0, 0.0]] - for _ in range(t) - ], - object_name="test_obj", - object_body_names=["test_obj"], - object_body_position=[[[0.2, 0.3, 0.4 + 0.01 * i]] for i in range(t)], - object_body_wxyz=[[[1.0, 0.0, 0.0, 0.0]] for _ in range(t)], - object_root_position=[[0.2, 0.3, 0.4 + 0.01 * i] for i in range(t)], - object_root_axis_angle=[[0.0, 0.0, 0.1 * i] for i in range(t)], - object_articulation=[0.0 for _ in range(t)], - ) - save_motion_parquet(md, root_path=str(output_dir)) - return output_dir / f"sequence_id={seq_id}" / f"robot_name={robot_name}" - - -def _write_motion_v1_dex3_parquet(output_dir: Path) -> Path: - """Write a minimal motion_v1 dual-hand parquet and return the partition dir.""" - seq_id = "replay_test_dex3" - robot_name = "dex3" - t = 4 - md = MotionData( - sequence_id=seq_id, - robot_name=robot_name, - motion_kind="dual_hand", - source_dataset="soma", - raw_motion_file="fake.pt", - fps=30.0, - coord_frame="robot_base_z_up", - ee_link_names=["left_wrist_link", "right_wrist_link"], - ee_pose_w=[ - [ - [0.1 * i, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0], - [-0.1 * i, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0], - ] - for i in range(t) - ], - object_name="test_obj", - object_body_names=["test_obj"], - object_body_position=[[[0.2, 0.3, 0.4 + 0.01 * i]] for i in range(t)], - object_body_wxyz=[[[1.0, 0.0, 0.0, 0.0]] for _ in range(t)], - object_root_position=[[0.2, 0.3, 0.4 + 0.01 * i] for i in range(t)], - object_root_axis_angle=[[0.0, 0.0, 0.1 * i] for i in range(t)], - object_articulation=[0.0 for _ in range(t)], - hand_sides=["left", "right"], - hand_frame_names=[["left_palm"], ["right_palm"]], - hand_frames_w=[ - [[[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]] for _ in range(t)], - [[[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]] for _ in range(t)], - ], - hand_finger_joint_names=[["left_j0"], ["right_j0"]], - hand_finger_joints=[ - [[0.1 * i] for i in range(t)], - [[-0.1 * i] for i in range(t)], - ], - ) - save_motion_parquet(md, root_path=str(output_dir)) - return output_dir / f"sequence_id={seq_id}" / f"robot_name={robot_name}" - - -def test_load_motion_v1_trajectory(tmp_path: Path) -> None: - """Load a motion_v1 whole-body parquet via the replay adapter.""" - partition_dir = _write_motion_v1_g1_parquet(tmp_path / "g1_data") - traj = load_replay_trajectory(str(partition_dir)) - - assert isinstance(traj, SingleRobotTrajectory) - assert traj.schema == "motion_v1" - assert traj.robot_layout == "single_robot" - assert traj.num_frames == 5 - assert traj.fps == 30.0 - assert traj.robot_joint_names == ["joint_a", "joint_b"] - assert traj.robot_root_position.shape == (5, 3) - assert traj.robot_root_wxyz.shape == (5, 4) - assert traj.robot_joint_positions.shape == (5, 2) - assert traj.object_traj is not None - assert traj.object_traj.root_position.shape == (5, 3) - assert traj.object_traj.root_wxyz.shape == (5, 4) - np.testing.assert_allclose(traj.robot_root_position[0, 2], 0.0, atol=1e-6) - np.testing.assert_allclose(traj.robot_root_position[4, 2], 0.4, atol=1e-6) - - -def test_load_motion_v1_dual_hand_trajectory(tmp_path: Path) -> None: - """Load a motion_v1 dual-hand (Dex3) parquet via the replay adapter.""" - partition_dir = _write_motion_v1_dex3_parquet(tmp_path / "dex3_data") - traj = load_replay_trajectory(str(partition_dir)) - - assert isinstance(traj, DualHandTrajectory) - assert traj.schema == "motion_v1" - assert traj.robot_layout == "dual_hand" - assert traj.num_frames == 4 - assert traj.fps == 30.0 - assert traj.wrist_orientation_format == "wxyz" - assert traj.left_wrist_position.shape == (4, 3) - assert traj.right_wrist_position.shape == (4, 3) - assert traj.left_wrist_orientation.shape == (4, 4) - assert traj.right_wrist_orientation.shape == (4, 4) - assert traj.left_finger_joints.shape == (4, 1) - assert traj.right_finger_joints.shape == (4, 1) - assert traj.object_traj is not None - assert traj.object_traj.root_position.shape == (4, 3) - - -# ============================================================ -# Sharpa trajectory round-trip -# ============================================================ - - -def _write_sharpa_parquet(output_dir: Path) -> Path: - """Write a minimal ManoSharpaData parquet and return the partition dir.""" - seq_id = "sharpa_test_seq" - robot_name = "sharpa_wave" - - data = ManoSharpaData( - sequence_id=seq_id, - raw_motion_file="fake.pt", - robot_name=robot_name, - fps=120.0, - mano_flat_hand_mean=True, - mano_center_idx=0, - mano_to_robot_scale=1.0, - mano_right_betas=[0.0] * 10, - mano_left_betas=[0.0] * 10, - mano_link_names=["palm"], - right_robot_finger_joint_names=["r_j0", "r_j1"], - right_robot_frame_names=["r_frame"], - right_robot_frame_task_names=["r_task"], - left_robot_finger_joint_names=["l_j0", "l_j1"], - left_robot_frame_names=["l_frame"], - left_robot_frame_task_names=["l_task"], - object_name="sharpa_obj", - safe_object_name="sharpa_obj", - object_body_names=[], - safe_object_body_names=[], - object_mesh_paths=[], - object_urdf_paths=[], - object_mesh_radius=[], - ) - - for i in range(3): - data.log_timestep( - mano_right_trans=[0.1 * i, 0.0, 0.0], - mano_right_global_orient=[0.0, 0.0, 0.0], - mano_right_finger_pose=[0.0] * 45, - mano_right_joints=[[0.0, 0.0, 0.0]] * 21, - mano_right_joints_wxyz=[[1.0, 0.0, 0.0, 0.0]] * 21, - mano_left_trans=[-0.1 * i, 0.0, 0.0], - mano_left_global_orient=[0.0, 0.0, 0.0], - mano_left_finger_pose=[0.0] * 45, - mano_left_joints=[[0.0, 0.0, 0.0]] * 21, - mano_left_joints_wxyz=[[1.0, 0.0, 0.0, 0.0]] * 21, - robot_right_wrist_position=[0.1 * i, 0.0, 0.3], - robot_right_wrist_wxyz=[1.0, 0.0, 0.0, 0.0], - robot_right_finger_joints=[0.0] * 22, - robot_right_frames=[[0.0] * 7] * 67, - robot_right_frame_task_errors=[0.0] * 11, - robot_right_num_optimization_iterations=1, - robot_left_wrist_position=[-0.1 * i, 0.0, 0.3], - robot_left_wrist_wxyz=[1.0, 0.0, 0.0, 0.0], - robot_left_finger_joints=[0.0] * 22, - robot_left_frames=[[0.0] * 7] * 67, - robot_left_frame_task_errors=[0.0] * 11, - robot_left_num_optimization_iterations=1, - object_articulation=0.0, - object_root_axis_angle=[0.0, 0.0, 0.0], - object_root_position=[0.0, 0.0, 0.5], - ) - - data.save_to_parquet(str(output_dir), partition_cols=["sequence_id", "robot_name"]) - return output_dir / f"sequence_id={seq_id}" / f"robot_name={robot_name}" - - -def test_load_sharpa_trajectory(tmp_path: Path) -> None: - """Load Sharpa parquet via replay adapter and verify dual-hand structure.""" - partition_dir = _write_sharpa_parquet(tmp_path / "sharpa_data") - traj = load_replay_trajectory(str(partition_dir)) - - assert isinstance(traj, DualHandTrajectory) - assert traj.schema == "mano_sharpa" - assert traj.robot_layout == "dual_hand" - assert traj.num_frames == 3 - assert traj.fps == 120.0 - assert traj.wrist_orientation_format == "wxyz" - assert traj.right_joint_names == ["r_j0", "r_j1"] - assert traj.left_joint_names == ["l_j0", "l_j1"] - assert traj.right_wrist_position.shape == (3, 3) - assert traj.left_wrist_position.shape == (3, 3) - assert traj.right_wrist_orientation.shape == (3, 4) - assert traj.right_finger_joints.shape == (3, 22) - assert traj.object_traj is not None - assert traj.object_traj.root_position.shape == (3, 3) - - -# ============================================================ -# Joint reorder -# ============================================================ - - -def test_build_joint_reorder_identity() -> None: - """Identical ordering returns None (no reorder needed).""" - names = ["a", "b", "c"] - assert _build_joint_reorder(names, names) is None - - -def test_build_joint_reorder_permutation() -> None: - """Permuted ordering returns correct index mapping.""" - parquet = ["c", "a", "b"] - sim = ["a", "b", "c"] - reorder = _build_joint_reorder(parquet, sim) - assert reorder is not None - expected = torch.tensor([2, 0, 1], dtype=torch.long) - assert torch.equal(reorder, expected) - - -# ============================================================ -# Script runner (no pytest) -# ============================================================ - -_ALL_TESTS: list[Callable[..., Any]] = [ - test_load_motion_v1_trajectory, - test_load_motion_v1_dual_hand_trajectory, - test_load_sharpa_trajectory, - test_build_joint_reorder_identity, - test_build_joint_reorder_permutation, -] - - -def _run_as_script() -> int: - """Run all tests directly without pytest.""" - print("=" * 72, flush=True) - print("Running replay_data adapter tests", flush=True) - print("=" * 72, flush=True) - passed = 0 - failed = 0 - for test_fn in _ALL_TESTS: - name = test_fn.__name__ - try: - with tempfile.TemporaryDirectory(prefix="replay_test_") as tmp_dir: - sig = inspect.signature(test_fn) - if "tmp_path" in sig.parameters: - test_fn(tmp_path=Path(tmp_dir)) - else: - test_fn() - print(f" [PASS] {name}", flush=True) - passed += 1 - except Exception: - print(f" [FAIL] {name}", flush=True) - traceback.print_exc() - failed += 1 - print("-" * 72, flush=True) - print(f"Results: {passed} passed, {failed} failed", flush=True) - return 1 if failed else 0 - - -if __name__ == "__main__": - raise SystemExit(_run_as_script()) diff --git a/robotic_grounding/tests/test_retarget_pipeline_e2e.py b/robotic_grounding/tests/test_retarget_pipeline_e2e.py deleted file mode 100644 index d99cdc5c..00000000 --- a/robotic_grounding/tests/test_retarget_pipeline_e2e.py +++ /dev/null @@ -1,467 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""End-to-end gate for the retarget pipeline. - -Runs every stage in ``workflow/retarget.yaml`` except ``load`` against a -committed ``{dataset}_loaded/`` fixture. The ``load`` stage needs raw -dataset data we can't commit (license + size), so the test starts from -the loaded parquet. - -Stages exercised (in ``workflow/retarget.yaml`` order): - 1.5 URDFs -> ``scripts/generate_rigid_urdfs.py`` - 2 process -> ``scripts/retarget/run_retarget.py`` - 3 reconstruct-> ``scripts/reconstruct_support_surfaces.py`` - 4 visualize -> ``scripts/retarget/vis_retargeted.py`` - 5 video -> ``scripts/rsl_rl/dummy_agent.py`` - -Coverage today: **taco only**. An ``arctic`` case will be added when an -``arctic_loaded/`` fixture lands in the repo. - -Requires Isaac Lab (``ISAACLAB_PATH``), GPU, and ``pxr``. -""" - -import os -import shutil -import subprocess -import tempfile -import unittest -from pathlib import Path - -import pyarrow.parquet as pq -import torch - - -class TestRetargetPipelineE2E(unittest.TestCase): - """End-to-end tests for retarget pipeline stages 1.5 through 5.""" - - project_root: Path - scripts_dir: Path - assets_dir: Path - isaaclab_path: str - isaaclab_script: str - # Lib dir to prepend to LD_LIBRARY_PATH so pinocchio imports; None when the - # Isaac Lab python already imports pinocchio cleanly (see - # ``_ensure_pinocchio_ld_path``). - _pinocchio_lib_dir: str | None = None - - @classmethod - def setUpClass(cls) -> None: - """Resolve paths and enforce Isaac Lab + GPU preconditions.""" - cls.project_root = Path(__file__).parent.parent.absolute() - cls.scripts_dir = cls.project_root / "scripts" - cls.assets_dir = ( - cls.project_root - / "source" - / "robotic_grounding" - / "robotic_grounding" - / "assets" - / "human_motion_data" - ) - - isaaclab_path = os.environ.get("ISAACLAB_PATH") - if not isaaclab_path: - raise unittest.SkipTest("ISAACLAB_PATH environment variable is not set") - isaaclab_script = os.path.join(isaaclab_path, "isaaclab.sh") - if not os.path.exists(isaaclab_script): - raise unittest.SkipTest(f"IsaacLab script not found at {isaaclab_script}") - cls.isaaclab_path = isaaclab_path - cls.isaaclab_script = isaaclab_script - - if not torch.cuda.is_available(): - raise unittest.SkipTest("CUDA not available — retarget E2E requires GPU") - - print(f"GPU available: {torch.cuda.get_device_name(0)}") - - cls._ensure_pinocchio_ld_path() - - @classmethod - def _ensure_pinocchio_ld_path(cls) -> None: - """Make ``import pinocchio`` work inside ``isaaclab.sh -p`` subprocesses. - - The Isaac Lab container ships pinocchio (pin 3.7.0) compiled against - urdfdom v4 / tinyxml2 v10, but the system cmeel wheels are v6 / v11, so - ``import pinocchio`` dies with:: - - ImportError: liburdfdom_sensor.so.4.0: cannot open shared object file - - ``scripts/retarget/process_soma_sequence.sh::setup_pinocchio_ld_path`` - fixes this for the OSMO workflow by installing the matching cmeel wheels - into a side prefix and prepending its ``lib/`` to ``LD_LIBRARY_PATH``. - This test invokes each stage directly via ``isaaclab.sh -p`` (not - through that script), so we replicate the fix here and stash the lib dir - on the class for ``_run`` to inject into every stage's environment. - - No-op (leaves ``_pinocchio_lib_dir`` None) when the Isaac Lab python - already imports pinocchio cleanly — e.g. on an image where the cmeel - soversion mismatch has been fixed at build time. - """ - cls._pinocchio_lib_dir = None - - def _pinocchio_imports() -> bool: - return ( - subprocess.run( - [cls.isaaclab_script, "-p", "-c", "import pinocchio"], - capture_output=True, - check=False, - ).returncode - == 0 - ) - - if _pinocchio_imports(): - print("pinocchio import OK (no LD_LIBRARY_PATH fix needed)") - return - - cache_dir = Path( - os.environ.get( - "PINOCCHIO_DEPS_PREFIX", - str(Path.home() / ".cache" / "robotic_grounding" / "pinocchio_deps"), - ) - ) - lib_dir = cache_dir / "cmeel.prefix" / "lib" - have_libs = (lib_dir / "liburdfdom_sensor.so.4.0").exists() and ( - lib_dir / "libtinyxml2.so.10" - ).exists() - if not have_libs: - print(f"Installing pinocchio v4/v10 cmeel deps to {cache_dir}") - cache_dir.mkdir(parents=True, exist_ok=True) - # ``--no-deps`` so pip doesn't pull cmeel-tinyxml2 v11 (urdfdom - # 4.0.1's metadata range otherwise resolves to v11; we need v10). - subprocess.run( - [ - cls.isaaclab_script, - "-p", - "-m", - "pip", - "install", - "--target", - str(cache_dir), - "--no-deps", - "cmeel-urdfdom==4.0.1", - "cmeel-tinyxml2==10.0.0", - ], - capture_output=True, - check=False, - ) - - cls._pinocchio_lib_dir = str(lib_dir) - print(f"pinocchio v4 deps: {lib_dir} (prepended to LD_LIBRARY_PATH)") - - check = subprocess.run( - [cls.isaaclab_script, "-p", "-c", "import pinocchio"], - capture_output=True, - text=True, - check=False, - env={ - **os.environ, - "LD_LIBRARY_PATH": f"{lib_dir}:{os.environ.get('LD_LIBRARY_PATH', '')}", - }, - ) - if check.returncode != 0: - print( - "WARNING: pinocchio still fails to import after the " - f"LD_LIBRARY_PATH fix; stages will hit the ImportError.\n" - f"{check.stderr[-1000:]}" - ) - - # ------------------------------------------------------------------ - # Subprocess helpers - # ------------------------------------------------------------------ - def _run( - self, - argv: list[str], - *, - stage: str, - timeout: int, - ) -> subprocess.CompletedProcess: - """Invoke a stage via ``isaaclab.sh -p``; fail the test on non-zero exit. - - Every retarget stage that ships in this repo is invoked in CI as - ``${ISAACLAB_PATH}/isaaclab.sh -p - - -""" - - -# ── HTTP Handler ────────────────────────────────────────────────────────────── - - -class Handler(BaseHTTPRequestHandler): - """HTTP request handler for the gallery server.""" - - def do_GET(self) -> None: - """Serve gallery HTML, dataset API, and static files.""" - raw_path = unquote(urlparse(self.path).path).lstrip("/") - - # Gallery root - if raw_path in ("", "index.html"): - self._send_text(GALLERY_HTML, "text/html; charset=utf-8") - return - - # Dataset API - if raw_path == "api/datasets": - self._send_json(discover_datasets()) - return - - # Static files — check _live/ mounts first, then DATA_DIR - file_path = None - for prefix, html_dir in _LIVE_MOUNTS.items(): - if raw_path == prefix or raw_path.startswith(prefix + "/"): - rel = raw_path[len(prefix) :].lstrip("/") - file_path = html_dir / rel if rel else html_dir - break - if file_path is None: - file_path = DATA_DIR / raw_path - if file_path.is_dir(): - file_path = file_path / "index.html" - if not file_path.is_file(): - self.send_error(404, f"Not found: {raw_path}") - return - - self._send_file(file_path) - - # ── helpers ── - - def _send_text(self, body: str, ctype: str) -> None: - data = body.encode("utf-8") - self.send_response(200) - self.send_header("Content-Type", ctype) - self.send_header("Content-Length", str(len(data))) - self.end_headers() - self.wfile.write(data) - - def _send_json(self, obj: Any) -> None: - data = json.dumps(obj, indent=2).encode("utf-8") - self.send_response(200) - self.send_header("Content-Type", "application/json") - self.send_header("Access-Control-Allow-Origin", "*") - self.send_header("Content-Length", str(len(data))) - self.end_headers() - self.wfile.write(data) - - def _send_file(self, file_path: Path) -> None: - mime = mimetypes.guess_type(str(file_path))[0] or "application/octet-stream" - size = file_path.stat().st_size - - # Immutable assets (hash in name) get a long cache lifetime - name = file_path.name - if re.search(r"-[A-Za-z0-9]{8,}\.(js|css|wasm)$", name): - cache = "public, max-age=31536000, immutable" - elif file_path.suffix in (".viser", ".mp4", ".hdr", ".ttf"): - cache = "public, max-age=604800" # recordings don't change; 7-day cache - else: - cache = "no-cache" - - # Range request — required for video seeking - range_hdr = self.headers.get("Range") - if range_hdr: - m = re.match(r"bytes=(\d+)-(\d*)", range_hdr) - if m: - start = int(m.group(1)) - end = int(m.group(2)) if m.group(2) else size - 1 - end = min(end, size - 1) - length = end - start + 1 - self.send_response(206) - self.send_header("Content-Type", mime) - self.send_header("Content-Range", f"bytes {start}-{end}/{size}") - self.send_header("Content-Length", str(length)) - self.send_header("Accept-Ranges", "bytes") - self.send_header("Cache-Control", cache) - self.end_headers() - self._stream(file_path, start, length) - return - - self.send_response(200) - self.send_header("Content-Type", mime) - self.send_header("Content-Length", str(size)) - self.send_header("Accept-Ranges", "bytes") - self.send_header("Cache-Control", cache) - self.end_headers() - self._stream(file_path, 0, size) - - def _stream(self, file_path: Path, offset: int, length: int) -> None: - try: - with open(file_path, "rb") as f: - f.seek(offset) - remaining = length - while remaining > 0: - chunk = f.read(min(CHUNK, remaining)) - if not chunk: - break - self.wfile.write(chunk) - remaining -= len(chunk) - except (BrokenPipeError, ConnectionResetError): - pass # client navigated away mid-transfer - - def log_message(self, fmt: str, *args: object) -> None: - """Suppress per-request noise.""" - - -# ── Entry point ─────────────────────────────────────────────────────────────── - - -def main() -> None: - """Parse CLI arguments and start the threaded HTTP server.""" - global DATA_DIR # noqa: PLW0603 - - parser = argparse.ArgumentParser(description="Robotics Visualizer Gallery Server") - parser.add_argument( - "--host", - default="0.0.0.0", - help="Bind address (default: 0.0.0.0 — all interfaces)", - ) - parser.add_argument("--port", type=int, default=8080, help="Port (default: 8080)") - parser.add_argument( - "--data-dir", - type=Path, - default=DATASETS_DIR, - help="Directory containing v2d_* dataset folders (default: visualizer/datasets/)", - ) - parser.add_argument( - "--html-dir", - type=Path, - action="append", - default=[], - metavar="DIR", - help="Extra html output dir (has recordings/ directly inside). " - "Pass multiple times to add a sidebar tab per dir, " - "e.g. --html-dir .../html_ours --html-dir .../html_baseline.", - ) - args = parser.parse_args() - - DATA_DIR = args.data_dir.expanduser().resolve() - if not DATA_DIR.is_dir(): - raise SystemExit(f"ERROR: --data-dir does not exist: {DATA_DIR}") - - for raw in args.html_dir: - p = raw.expanduser().resolve() - if not p.is_dir(): - raise SystemExit(f"ERROR: --html-dir does not exist: {p}") - # Pick a unique mount slug so two paths ending in the same leaf - # (e.g. ".../A/html_ours" and ".../B/html_ours") don't clobber each other. - for slug in ( - p.name, - f"{p.parent.name}_{p.name}", - hashlib.md5(str(p).encode(), usedforsecurity=False).hexdigest()[:8], - ): - prefix = f"_live/{slug}" - if prefix not in _LIVE_MOUNTS: - _LIVE_MOUNTS[prefix] = p - break - - datasets = discover_datasets() - total = sum(d["count"] for d in datasets) - - print(f"\n Robotics Visualizer → http://{args.host}:{args.port}") - print(f" data dir: {DATA_DIR}") - print(f" {len(datasets)} dataset(s) · {total} sequences total") - for d in datasets: - print(f" {d['name']:20s} {d['count']} sequences") - print("\n Press Ctrl+C to stop.\n") - - class ThreadedHTTPServer(ThreadingMixIn, HTTPServer): - daemon_threads = True - - server = ThreadedHTTPServer((args.host, args.port), Handler) - try: - server.serve_forever() - except KeyboardInterrupt: - print("\nStopped.") - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/visualizer/sync_visualizer_data.py b/robotic_grounding/visualizer/sync_visualizer_data.py deleted file mode 100644 index 4213dd47..00000000 --- a/robotic_grounding/visualizer/sync_visualizer_data.py +++ /dev/null @@ -1,372 +0,0 @@ -#!/usr/bin/env python3 -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -"""Download visualizer recordings from OSMO to local datasets dir. - -Syncs {name}_html/ trees (*.viser, *.mp4, viser-client/) from the OSMO -isaac bucket using `osmo dataset download`. Multiple datasets can be -downloaded in parallel with --jobs (use MAX to download all at once). -Ctrl+C terminates all running downloads immediately. - -Prerequisites: - - osmo CLI on PATH and authenticated - -Usage: - python robotic_grounding/visualizer/sync_visualizer_data.py - python robotic_grounding/visualizer/sync_visualizer_data.py --dataset arctic - python robotic_grounding/visualizer/sync_visualizer_data.py --dataset arctic h2o - python robotic_grounding/visualizer/sync_visualizer_data.py --jobs MAX - python robotic_grounding/visualizer/sync_visualizer_data.py --dry-run -""" - -import argparse -import os -import re -import shutil -import signal -import subprocess -import sys -import threading -from concurrent.futures import ThreadPoolExecutor, as_completed -from pathlib import Path - -from rich.progress import ( - BarColumn, - Progress, - TaskID, - TextColumn, - TimeRemainingColumn, -) - -# --------------------------------------------------------------------------- -# Constants -# --------------------------------------------------------------------------- -DATASETS = ("arctic", "h2o", "grab", "taco", "dexycb", "hot3d") - -LOCAL_DATASETS_DIR = Path(__file__).resolve().parent / "datasets" - -# Rich color per dataset (same order as DATASETS) -_RICH_COLORS = ("cyan", "green", "yellow", "magenta", "blue", "red") - -# Regex to parse a tqdm progress line emitted by osmo -# Matches: " 17%|███ | 181M/1.06G [00:05<00:22, 42.5MB/s, ...]" -_TQDM_RE = re.compile( - r"^\s*(\d+)%\|" # percentage - r".*?\|\s*" - r"([\d.]+\s*\S+)" # bytes done - r"/" - r"([\d.]+\s*\S+)" # bytes total - r"\s*\[" - r"[^,<]+" # elapsed - r" None: - """Kill every active osmo process group (osmo spawns ~64 child workers).""" - with _procs_lock: - for proc in _active_procs: - try: - os.killpg(os.getpgid(proc.pid), signal.SIGKILL) - except OSError: - try: - proc.kill() - except OSError: - pass - - -# --------------------------------------------------------------------------- -# Helpers -# --------------------------------------------------------------------------- -def _rich_color(name: str) -> str: - try: - return _RICH_COLORS[list(DATASETS).index(name)] - except ValueError: - return "white" - - -def _osmo_dataset(name: str) -> str: - if name == "arctic": - return "isaac/retargeted_arctic_exp_full" - elif name == "hot3d": - return "isaac/v2d_hot3d_retarget_exp_full" - elif name == "oakink2": - return "isaac/v2d_oakink2_retarget_exp_full" - - return f"isaac/v2d_{name}_retarget_exp_200" - - -def _local_dir(name: str) -> Path: - if name == "arctic": - return LOCAL_DATASETS_DIR / "retargeted_arctic_exp_full" - elif name == "hot3d": - return LOCAL_DATASETS_DIR / "v2d_hot3d_retarget_exp_full" - elif name == "oakink2": - return LOCAL_DATASETS_DIR / "v2d_oakink2_retarget_exp_full" - - return LOCAL_DATASETS_DIR / f"v2d_{name}_retarget_exp_200" - - -def _parse_tqdm(line: str) -> tuple[int, str, str, str] | None: - """Parse a tqdm progress line. - - Returns (pct, done, total, speed) or None if the line isn't a tqdm bar. - """ - m = _TQDM_RE.match(line) - if not m: - return None - pct = int(m.group(1)) - done = m.group(2).strip() - total = m.group(3).strip() - speed = (m.group(4) or "").strip() - return pct, done, total, speed - - -# --------------------------------------------------------------------------- -# Streaming output → rich progress -# --------------------------------------------------------------------------- -def _stream( - proc: subprocess.Popen, - name: str, - progress: Progress, - task_id: TaskID, -) -> None: - """Read proc stdout+stderr; update rich progress bars and print non-tqdm lines.""" - color = _rich_color(name) - buf = b"" - - assert proc.stdout is not None - while True: - chunk = proc.stdout.read(256) - if not chunk: - break - buf += chunk - - while True: - ni = buf.find(b"\n") - ri = buf.find(b"\r") - if ni == -1 and ri == -1: - break - if ni == -1 or (ri != -1 and ri < ni): - raw, buf = buf[:ri], buf[ri + 1 :] - else: - raw, buf = buf[:ni], buf[ni + 1 :] - - line = raw.decode(errors="replace").strip() - if not line: - continue - - parsed = _parse_tqdm(line) - if parsed: - pct, done, total, speed = parsed - label = f"[{color}]{name:<7}[/{color}]" - info = f"[dim]{done}/{total}" - if speed: - info += f" @ {speed}" - info += "[/dim]" - progress.update(task_id, completed=pct, description=f"{label} {info}") - else: - progress.console.print(f"[{color}]\\[{name}][/{color}] {line}") - - if buf.strip(): - line = buf.decode(errors="replace").strip() - progress.console.print( - f"[{_rich_color(name)}]\\[{name}][/{_rich_color(name)}] {line}" - ) - - -# --------------------------------------------------------------------------- -# Core sync -# --------------------------------------------------------------------------- -def sync( - name: str, - pattern: str | None, - dry_run: bool, - progress: Progress, -) -> int: - """Download {name}_html/ from OSMO. Returns osmo exit code.""" - if _shutdown.is_set(): - progress.console.print(f"[dim]\\[{name}] skipped (interrupted)[/dim]") - return 1 - - dest = _local_dir(name) - regex = rf"^{name}_html/.*{pattern}" if pattern else rf"^{name}_html/" - - # osmo ALWAYS creates a DATASET_NAME/ subdirectory inside whatever path it - # receives. So pass LOCAL_DATASETS_DIR (the parent); osmo then creates - # v2d_{name}_retarget_exp_200/ inside it — which is exactly dest. - # Delete dest first so osmo creates it fresh without nesting inside itself. - cmd = [ - "osmo", - "dataset", - "download", - _osmo_dataset(name), - str(LOCAL_DATASETS_DIR), - "--regex", - regex, - ] - - color = _rich_color(name) - progress.console.print( - f"[{color}]\\[{name}][/{color}] " f"{_osmo_dataset(name)} [dim]→[/dim] {dest}" - ) - - if dry_run: - progress.console.print( - f"[{color}]\\[{name}][/{color}] [dim][dry-run] {' '.join(cmd)}[/dim]" - ) - return 0 - - LOCAL_DATASETS_DIR.mkdir(parents=True, exist_ok=True) - if dest.exists(): - shutil.rmtree(dest) # remove so osmo creates dest/ fresh, not dest/dest/ - - task_id = progress.add_task( - f"[{color}]{name:<7}[/{color}] [dim]starting...[/dim]", - total=100, - ) - - proc = subprocess.Popen( - cmd, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - start_new_session=True, # own process group → killpg kills all workers - ) - with _procs_lock: - _active_procs.append(proc) - try: - _stream(proc, name, progress, task_id) - rc = proc.wait() - finally: - with _procs_lock: - try: - _active_procs.remove(proc) - except ValueError: - pass - - if rc == 0: - progress.update( - task_id, - completed=100, - description=f"[{color}]{name:<7}[/{color}] [green]done[/green]", - ) - elif not _shutdown.is_set(): - progress.update( - task_id, - description=f"[{color}]{name:<7}[/{color}] [red]failed (code {rc})[/red]", - ) - return rc - - -# --------------------------------------------------------------------------- -# CLI -# --------------------------------------------------------------------------- -def _parse_args() -> argparse.Namespace: - parser = argparse.ArgumentParser( - description="Download visualizer recordings from OSMO to local datasets dir.", - ) - parser.add_argument( - "--dataset", - nargs="+", - choices=[*DATASETS, "all"], - default=["all"], - help="Dataset(s) to sync, or 'all' (default).", - ) - parser.add_argument( - "--pattern", - type=str, - default=None, - help="Extra regex to narrow files within {name}_html/ (e.g. 'arctic_s01').", - ) - parser.add_argument( - "--jobs", - "-j", - type=str, - default="1", - metavar="N|MAX", - help="Parallel downloads: integer or MAX to download all datasets at once.", - ) - parser.add_argument( - "--dry-run", - action="store_true", - default=False, - help="Print the osmo command without running it.", - ) - return parser.parse_args() - - -def main() -> None: - """Parse CLI args and run sequential or parallel dataset downloads.""" - args = _parse_args() - datasets = list(DATASETS) if "all" in args.dataset else args.dataset - - jobs_str = args.jobs.upper() - n_jobs = len(datasets) if jobs_str == "MAX" else int(args.jobs) - - progress = Progress( - TextColumn("{task.description}", justify="left"), - BarColumn(bar_width=30), - TextColumn("[progress.percentage]{task.percentage:>3.0f}%"), - TimeRemainingColumn(), - refresh_per_second=10, - transient=False, - ) - - # ---- sequential path ----------------------------------------------- - if n_jobs == 1: - try: - with progress: - for name in datasets: - if _shutdown.is_set(): - break - rc = sync(name, args.pattern, args.dry_run, progress) - if rc != 0 and not _shutdown.is_set(): - sys.exit(rc) - except KeyboardInterrupt: - print("\nInterrupted — terminating download.") - _shutdown.set() - _terminate_all() - os._exit(130) - return - - # ---- parallel path ------------------------------------------------- - failed: list[str] = [] - pool = ThreadPoolExecutor(max_workers=n_jobs) - futures = { - pool.submit(sync, name, args.pattern, args.dry_run, progress): name - for name in datasets - } - try: - with progress: - for fut in as_completed(futures): - name = futures[fut] - try: - rc = fut.result() - except Exception as exc: - progress.console.print(f"[red]\\[{name}] EXCEPTION: {exc}[/red]") - failed.append(name) - else: - if rc != 0: - failed.append(name) - except KeyboardInterrupt: - print("\nInterrupted — terminating all downloads.") - _shutdown.set() - _terminate_all() - pool.shutdown(wait=False, cancel_futures=True) - os._exit(130) - - pool.shutdown(wait=True) - - if failed: - print(f"Failed datasets: {', '.join(failed)}", file=sys.stderr) - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/robotic_grounding/workflow/Dockerfile b/robotic_grounding/workflow/Dockerfile deleted file mode 100644 index 25d32cdf..00000000 --- a/robotic_grounding/workflow/Dockerfile +++ /dev/null @@ -1,143 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. - -############################################################## -# Base Stage -############################################################## - -FROM nvcr.io/nvidia/isaac-lab:2.3.1 AS base - -# Set working directory -WORKDIR /workspace/video_to_data/robotic_grounding - -RUN apt-get update && apt-get install -y \ - curl \ - tmux \ - htop \ - openssh-client \ - && rm -rf /var/lib/apt/lists/* - -############################################################## -# Install Python Dependencies (before source copy for caching) -############################################################## - -# Install general dependencies -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --upgrade pip -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - tensordict==0.8.3 \ - datasets==2.19.0 \ - wandb==0.22.2 \ - importlib_metadata \ - plotly \ - viser==1.0.15 \ - dearpygui \ - scipy \ - scikit-learn - -# GPL hand-model deps (chumpy + MANO layers) removed — the Stage-1 dataset load -# (raw -> loaded Parquet via hand FK) moved to reconstruction's -# v2d_task_library_loader. robotic_grounding consumes the loaded Parquet only. - -# Install py-soma-x for SOMA body model used by `scripts/retarget/soma_to_g1.py`. -# Assets are auto-downloaded from HuggingFace on first SOMALayer use; the -# robotic_grounding code instantiates SOMALayer with -# `data_root=BODY_MODELS_DIR / "soma"` so per-run assets land at a known path -# inside the bind-mounted repo. Pin via PyPI release. -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - py-soma-x - -# Install Retargeting dependencies. -# Pin the FULL pinocchio native closure to the last known-good versions. -# pinocchio (`pin`) bundles its native libs via cmeel; leaving them unpinned -# lets a rebuild drift cmeel soversions pinocchio wasn't built against, failing -# at runtime with e.g. "ImportError: liburdfdom_sensor.so.4.0 / libtinyxml2.so.10: -# cannot open shared object file". Pin pin's actual closure (pin -> coal, eigenpy, -# cmeel-*; cmeel-urdfdom -> cmeel-console-bridge, cmeel-tinyxml2). NOTE: include -# `coal` (not `hpp-fcl`) — pinocchio 3.7 depends on coal, and adding hpp-fcl 2.4.4 -# makes the resolve impossible (it wants cmeel-boost~=1.83 vs eigenpy's ~=1.87). -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - mujoco \ - judo-rai>=0.0.5 \ - pin==3.7.0 \ - pin-pink==4.2.0 \ - coal==3.0.1 \ - eigenpy==3.10.3 \ - cmeel==0.57.3 \ - cmeel-boost==1.87.0.1 \ - cmeel-urdfdom==4.0.1 \ - cmeel-console-bridge==1.0.2.3 \ - cmeel-tinyxml2==10.0.0 \ - cmeel-tinyxml==2.6.2.3 \ - cmeel-assimp==5.4.3.1 \ - cmeel-octomap==1.10.0 \ - cmeel-qhull==8.0.2.1 \ - cmeel-zlib==1.3.1 \ - usd-core \ - deprecation - -# Install QPSolvers -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir --upgrade \ - osqp \ - qpsolvers[clarabel,daqp,proxqp,scs,osqp]==4.9.0 - -# FIXME: pytorch3d is not installed with cuda, making the retarget distance_utils module not importable -# # Install pytorch3d (required for retarget distance_utils; --no-build-isolation so build sees Isaac Lab's torch) -# RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir --no-build-isolation \ -# "git+https://github.com/facebookresearch/pytorch3d.git" - -# Install Robotic Grounding dependencies -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - onnxruntime-gpu \ - imageio \ - imageio-ffmpeg \ - pyrender \ - pytorch-ignite \ - pytest \ - boto3 - -# Install numpy < 2.0.0 to avoid compatibility issues in Isaac Lab -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - "numpy<2.0.0" - -############################################################## -# Copy source code and scripts -############################################################## - -COPY pyproject.toml ./ -COPY README.md ./ - -COPY source/ ./source/ -COPY scripts/ ./scripts/ - -# Embed the third-party TACO object meshes the E2E tests need at the canonical -# asset path the pipeline reads from. The fixtures and their CC BY 4.0 LICENSE + -# NOTICE live under ci/assets/taco/meshes/ (© Yun Liu et al.); copy the whole dir -# so the license/attribution travels with the meshes in the image. -COPY ci/assets/taco/meshes/ ./source/robotic_grounding/robotic_grounding/assets/meshes/taco/ - -# Install the project in editable mode -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - -e ./source/robotic_grounding - -# Pre-generate the rigid URDFs for the embedded TACO meshes so the E2E tests that -# load processed sequences (test_train_e2e.py) find them without first running the -# URDF-generation stage. Mesh-only discovery (globs meshes/taco/*_cm.obj); no GPU. -RUN ${ISAACLAB_PATH}/isaaclab.sh -p scripts/generate_rigid_urdfs.py --dataset taco - -# Create wrapper script for python (for convenience in container shell) -RUN echo '#!/bin/bash' > /usr/local/bin/python && \ - echo "exec ${ISAACLAB_PATH}/isaaclab.sh -p \"\$@\"" >> /usr/local/bin/python && \ - chmod +x /usr/local/bin/python && \ - echo "alias python='${ISAACLAB_PATH}/isaaclab.sh -p'" >> /etc/skel/.bashrc && \ - echo "alias python='${ISAACLAB_PATH}/isaaclab.sh -p'" >> /root/.bashrc - -# # Verify pytorch3d is installed (build fails if not importable) -# RUN ${ISAACLAB_PATH}/isaaclab.sh -p -c "import pytorch3d; print('pytorch3d', pytorch3d.__version__)" - -# Set the default command -CMD ["/bin/bash"] diff --git a/robotic_grounding/workflow/Dockerfile.aarch64 b/robotic_grounding/workflow/Dockerfile.aarch64 deleted file mode 100644 index eddaa019..00000000 --- a/robotic_grounding/workflow/Dockerfile.aarch64 +++ /dev/null @@ -1,127 +0,0 @@ -# Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved. -# -# NVIDIA CORPORATION and its licensors retain all intellectual property -# and proprietary rights in and to this software, related documentation -# and any modifications thereto. Any use, reproduction, disclosure or -# distribution of this software and related documentation without an express -# license agreement from NVIDIA CORPORATION is strictly prohibited. -# -# aarch64 variant — three packages removed vs Dockerfile: -# dearpygui : no Linux aarch64 wheel on PyPI; not used in scripts/ or source/ -# usd-core : no Linux aarch64 wheel on PyPI; base image ships nvidia-srl-usd 2.0.0 (provides pxr) -# onnxruntime-gpu: no aarch64 wheel on PyPI; base image ships onnxruntime 1.24.4 with CUDAExecutionProvider - -############################################################## -# Base Stage -############################################################## - -FROM nvcr.io/nvidia/isaac-lab:2.3.1 AS base - -# Set working directory -WORKDIR /workspace/video_to_data/robotic_grounding - -RUN apt-get update && apt-get install -y \ - curl \ - tmux \ - htop \ - openssh-client \ - && rm -rf /var/lib/apt/lists/* - -############################################################## -# Install Python Dependencies (before source copy for caching) -############################################################## - -# Install general dependencies -# (dearpygui omitted — no aarch64 wheel; not used anywhere) -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --upgrade pip -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - tensordict==0.8.3 \ - datasets==2.19.0 \ - wandb==0.22.2 \ - importlib_metadata \ - plotly \ - viser==1.0.15 \ - scipy \ - scikit-learn - -# GPL hand-model deps (chumpy + MANO layers) removed — the Stage-1 dataset load -# (raw -> loaded Parquet via hand FK) moved to reconstruction's -# v2d_task_library_loader. robotic_grounding consumes the loaded Parquet only. - -# Install Retargeting dependencies -# (usd-core omitted — no aarch64 wheel; base image ships nvidia-srl-usd 2.0.0 which provides pxr) -# Pin the full pinocchio native closure — see workflow/Dockerfile for why -# (unpinned cmeel-* drift the soversions pinocchio links against; use coal not -# hpp-fcl to avoid the cmeel-boost resolve conflict). -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - mujoco \ - judo-rai>=0.0.5 \ - pin==3.7.0 \ - pin-pink==4.2.0 \ - coal==3.0.1 \ - eigenpy==3.10.3 \ - cmeel==0.57.3 \ - cmeel-boost==1.87.0.1 \ - cmeel-urdfdom==4.0.1 \ - cmeel-console-bridge==1.0.2.3 \ - cmeel-tinyxml2==10.0.0 \ - cmeel-tinyxml==2.6.2.3 \ - cmeel-assimp==5.4.3.1 \ - cmeel-octomap==1.10.0 \ - cmeel-qhull==8.0.2.1 \ - cmeel-zlib==1.3.1 \ - deprecation - -# Install QPSolvers -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir --upgrade \ - osqp \ - qpsolvers[clarabel,daqp,proxqp,scs,osqp]==4.9.0 - -# FIXME: pytorch3d is not installed with cuda, making the retarget distance_utils module not importable -# # Install pytorch3d (required for retarget distance_utils; --no-build-isolation so build sees Isaac Lab's torch) -# RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir --no-build-isolation \ -# "git+https://github.com/facebookresearch/pytorch3d.git" - -# Install Robotic Grounding dependencies -# onnxruntime-gpu has no aarch64 wheel. We install onnxruntime (CPU build) here so the package is -# explicitly present. The base image ships onnxruntime 1.24.4 with CUDAExecutionProvider built in, -# so pip will see it as already satisfied and skip the download — GPU inference still works. -# If the base image ever drops onnxruntime, this installs the CPU fallback instead. -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - onnxruntime \ - imageio \ - imageio-ffmpeg \ - pytorch-ignite \ - pytest \ - boto3 - -# Install numpy < 2.0.0 to avoid compatibility issues in Isaac Lab -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - "numpy<2.0.0" - -############################################################## -# Copy source code and scripts -############################################################## - -COPY pyproject.toml ./ -COPY README.md ./ - -COPY source/ ./source/ -COPY scripts/ ./scripts/ - -# Install the project in editable mode -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install --no-cache-dir \ - -e ./source/robotic_grounding - -# Create wrapper script for python (for convenience in container shell) -RUN echo '#!/bin/bash' > /usr/local/bin/python && \ - echo "exec ${ISAACLAB_PATH}/isaaclab.sh -p \"\$@\"" >> /usr/local/bin/python && \ - chmod +x /usr/local/bin/python && \ - echo "alias python='${ISAACLAB_PATH}/isaaclab.sh -p'" >> /etc/skel/.bashrc && \ - echo "alias python='${ISAACLAB_PATH}/isaaclab.sh -p'" >> /root/.bashrc - -# # Verify pytorch3d is installed (build fails if not importable) -# RUN ${ISAACLAB_PATH}/isaaclab.sh -p -c "import pytorch3d; print('pytorch3d', pytorch3d.__version__)" - -# Set the default command -CMD ["/bin/bash"] diff --git a/robotic_grounding/workflow/README.md b/robotic_grounding/workflow/README.md deleted file mode 100644 index a9108ae3..00000000 --- a/robotic_grounding/workflow/README.md +++ /dev/null @@ -1,101 +0,0 @@ -# OSMO Workflows - -Workflow definitions for running training, retargeting, and development environments on OSMO. See the [OSMO user guide](https://isaac-infrastructure.gitlab-master-pages.nvidia.com/osmo/release-6.0.x/user_guide/index.html) for platform details. - -**See also:** [data_pipeline.md](data_pipeline.md) for the end-to-end data flow (raw → retargeted → trained), [data_storage.md](data_storage.md) for storage layout and output-download commands. - -## Prerequisites - -### 1. Access - -- `isaac-amr` NGC group — open a ticket in `#swngc-help`. -- OSMO DLs `access-osmo` and `access-osmo-isaac-dev` via [DLRequest](https://dlrequest/) (ping `#osmo-support` to get approved). - -### 2. Configure NGC - -1. Install the [NGC CLI](https://docs.ngc.nvidia.com/cli/cmd.html) and [sign in](https://ngc.nvidia.com/signin). -2. Select `nvstaging` > `isaac-amr`, then **Setup** > **Create API Key**. -3. Configure and verify: - ```bash - ngc config set # use the API key - docker login nvcr.io # user: $oauthtoken, pass: - ngc user who # confirm read+write - ``` - -### 3. Configure OSMO - -Install the [OSMO CLI](https://isaac-infrastructure.gitlab-master-pages.nvidia.com/osmo/release-6.0.x/user_guide/getting_started/install/index.html) and set up [credentials](https://isaac-infrastructure.gitlab-master-pages.nvidia.com/osmo/release-6.0.x/user_guide/getting_started/credentials.html), including the [CSS setup](https://isaac-infrastructure.gitlab-master-pages.nvidia.com/osmo/release-6.0.x/user_guide/appendix/css/index.html#data-credentials-css) for storage access. - -Omni-auth credentials are generally not needed; if a workflow complains about them, remove the credentials block from the yaml or follow the [token guide](https://docs.omniverse.nvidia.com/nucleus/latest/config-and-info/api_tokens.html#token-generation). - -### 4. Configure W&B for training jobs - -OSMO training workflows log to W&B by default. Set `WANDB_API_KEY` in the shell that submits the workflow: - -```bash -export WANDB_API_KEY= -``` - -If W&B is not available, do not submit a cloud training run. Use local smoke tests with `--logger tensorboard` from the top-level README. - -## Submitting a Job - -`run_osmo.py` builds + pushes + submits in one command. Add `--image ` to skip rebuilding with an existing image, or `--dry-run` to preview. - -### Remote development - -```bash -python scripts/run_osmo.py --experiment-name --workflow-yaml workflow/dev_env.yaml - -# Once running: -osmo workflow port-forward dev-env --port 6000:22 -ssh root@localhost -p 6000 -``` - -### Training - -```bash -python scripts/run_osmo.py --experiment-name --workflow-yaml workflow/train.yaml -``` - -### Retargeting - -See [data_pipeline.md](data_pipeline.md) for what each stage does. Stages (`load`, `process`, `reconstruct`, `visualize`, `video`) can run together or individually. Outputs from one run are published as a single new version of the `v2d_{dataset}_retarget_exp_200` OSMO dataset — see [data_storage.md](data_storage.md) to pull them locally. - -```bash -# Full pipeline (works for any registered dataset: taco, arctic, oakink2, hot3d, h2o, grab, dexycb) -python scripts/run_osmo.py --experiment-name retarget- \ - --image nvcr.io/nvstaging/isaac-amr/robotic-grounding: \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset= - -# Run only one stage -python scripts/run_osmo.py --experiment-name retarget-- \ - --image nvcr.io/nvstaging/isaac-amr/robotic-grounding: \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset= --set stages= -``` - -#### Filtering sequences - -Use `sequence_pattern` (regex), `sequence_id` (exact), or `max_sequences` to pick a subset. `sequence_pattern` is applied as both an OSMO-input download regex and a Python-level filter. - -```bash -python scripts/run_osmo.py --experiment-name retarget-taco-screw \ - --image nvcr.io/nvstaging/isaac-amr/robotic-grounding: \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset=taco \ - --set 'sequence_pattern=.*(screw|skim_off|smear|stir).*' - -# Equivalent alternatives: -# --set sequence_id=taco_screw__screwdriver__toy_20231102_063 -# --set max_sequences=10 -``` - -## Managing Workflows - -```bash -osmo workflow list -osmo workflow logs -osmo workflow cancel -``` diff --git a/robotic_grounding/workflow/data_pipeline.md b/robotic_grounding/workflow/data_pipeline.md deleted file mode 100644 index 4da1505a..00000000 --- a/robotic_grounding/workflow/data_pipeline.md +++ /dev/null @@ -1,443 +0,0 @@ -# Data Pipeline: Raw Motion Data to RL Training - -This document describes how hand-object motion capture data flows through the -robotic_grounding pipeline, from raw dataset files to a trained RL policy. - -Two parquet formats are in use today: - -- **`motion_v1`** (`robotic_grounding.motion_schema`) — the unified whole-body / - bimanual format used by `soma_to_g1`, `arctic_to_dex3`, and the - `g1_planner` output. See - [../source/robotic_grounding/robotic_grounding/motion_schema/README.md](../source/robotic_grounding/robotic_grounding/motion_schema/README.md). -- **`ManoSharpaData`** — the legacy dual-hand V2P retarget pipeline described - below. Still used end-to-end by arctic/taco/oakink2/hot3d/h2o/grab/dexycb - loaders and the dual-hand v2p tracking command. Not on `motion_v1` yet. - -## Pipeline Overview - -``` -Raw Data on CSS (pkl/jsonl/csv/npy/...) - | - v Stage 1: Load (reconstruction v2d_{dataset}_load workflow; GPL MANO FK) -ManoSharpaData Parquet (MANO hands + object poses) --> {dataset}_loaded/ swift prefix - | - |-- Stage 1.5: Generate URDFs --> {dataset}_urdfs/*.urdf - | - v Stage 2: Retarget (IK) -ManoSharpaData Parquet (+ robot joint trajectories) - | - |-- Stage 3: Support surfaces --> reconstructed_stage/*.usda - |-- Stage 4: Visualize (optional) --> {dataset}_html/recordings/{seq}.viser (+ .mp4) - |-- Stage 5: Record video (optional) --> {dataset}_videos/{seq}.mp4 (Isaac Sim) - | - v (All stages' outputs written to the swift output_url prefix) -swift://…/human_motion_data/{dataset}/{dataset}_processed/ (+ _urdfs, reconstructed_stage, _html, _videos) - | - v Stage 6: sync_css_data.py / aws s3 sync -Local assets/human_motion_data/{dataset}/ - | - v Stage 7: RL Training (Isaac Sim) -Trained policy checkpoint -``` - -Stage 1 (Load) runs as a **separate workflow in the `reconstruction` repo** -(`v2d_{dataset}_load`): it carries the GPL-3.0 MANO/manotorch forward-kinematics -code, contained in its own image, and writes a `{dataset}_loaded` Parquet to a -swift prefix. Stages 1.5–5 are the robotic_grounding `retarget.yaml` workflow, -which **consumes** that `{dataset}_loaded` data (it is manotorch-free); any -subset can run via `--set stages=`. All `retarget.yaml` artifacts from one -run are written to the swift `output_url` prefix (see [data_storage.md](data_storage.md)). - -## Stage 1: Load - -**Purpose:** Parse raw dataset files into a standardized Parquet schema. - -> **Home:** Stage 1 lives in the **`reconstruction`** repo, not here. It uses -> MANO forward kinematics (manotorch, GPL-3.0), so it is contained in the -> `v2d_task_library_loader` image and run as the `v2d_{dataset}_load` OSMO -> workflow. robotic_grounding consumes the resulting `{dataset}_loaded` dataset -> and never imports manotorch. The description below documents the contract. - -**Module:** `reconstruction/modules/v2d_task_library_loader/lib/{dataset}_loader.py` -(dispatched via `lib/run_loader.py`, i.e. `python -m v2d.task_library_loader.lib.run_loader`) - -Each dataset stores motion data differently (pickles, JSONL, CSV, NPY). The -loader reads the raw format and writes a `ManoSharpaData` Parquet containing: - -- **MANO hand parameters** -- wrist position/orientation, 45-DOF finger pose, - shape betas, fitting error, per frame, per hand (left + right). -- **Object poses** -- per-body position and quaternion, per frame. -- **Object metadata** -- mesh file paths, URDF paths, body names, mesh radius. -- **Contact data** (optional) -- per-link contact positions and normals between - hand surface and object surface (16 MANO links per hand). - -The loader also handles dataset-specific preprocessing: - -- Coordinate frame transforms (Y-up to Z-up for Quest3, OakInk2). -- MANO PCA expansion (15 PCA coefficients to 45-DOF axis-angle for Hot3D). -- Timestamp deduplication (Hot3D multi-camera streams to ~30 Hz). -- Mesh scale normalization (centimeters to meters for TACO). - -**Output:** `human_motion_data/{dataset}/{dataset}_loaded/` partitioned by -`sequence_id` and `robot_name`. - -**In-container command** (what runs inside the loader image): -```bash -python -m v2d.task_library_loader.lib.run_loader \ - --dataset \ - --dataset_root \ - --object_model_root \ - --mesh_dir \ - --mano_model_dir \ - --output_dir \ - --device cuda:0 --save -``` - -**Run it locally** (host wrapper — mounts the inputs and runs the in-container -command above). Raw data lives under `//dataset/`; -`--object_assets_dir` is the root holding `urdfs//` + `meshes//`: - -```bash -python -m v2d.task_library_loader.docker.run_loader \ - --dataset arctic \ - --human_motion_data_dir \ - --object_assets_dir \ - --mano_model_dir \ - --output_dir \ - --max_sequences 2 --save -``` - -Omit `--object_assets_dir` for h2o/grab/dexycb (meshes come from the raw dataset). - -Or submit `reconstruction/workflows/task_library_load/osmo/load.yaml` on OSMO. - -## Stage 1.5: Generate Rigid URDFs - -**Purpose:** Create URDF files that Isaac Sim can load for each object mesh. - -**Script:** `scripts/generate_rigid_urdfs.py` - -Isaac Sim requires URDF (or USD) descriptions to spawn objects. This stage -converts raw object meshes (OBJ, GLB) into single-link rigid URDFs: - -1. Load source mesh via trimesh. -2. Export a clean visual STL (avoids Isaac Sim OBJ parser issues). -3. Generate a URDF with visual + collision geometry and default inertia. - -The mesh scale comes from the dataset registry (`mesh_vertex_scale`). This -stage is idempotent -- it skips objects that already have URDFs. Runs -automatically inside the `process` stage on OSMO. - -Locally, the source meshes are not committed — make sure they're in place first -(see [Object Assets](#object-assets-urdfs--meshes)). - -**Output:** `assets/urdfs/{dataset}/{object_id}_rigid.urdf` locally; in the -OSMO workflow these are copied to `{dataset}_urdfs/` in the published -dataset version. - -**Command:** -```bash -python scripts/generate_rigid_urdfs.py --dataset -``` - -## Stage 2: Retarget (IK) - -**Purpose:** Convert human MANO hand poses into robot joint trajectories. - -**Script:** `scripts/retarget/{dataset}_to_sharpa.py` (dispatched via `scripts/retarget/run_retarget.py`) - -For each frame in the loaded Parquet: - -1. Read MANO wrist position, orientation, and finger joint angles. -2. Run inverse kinematics to solve the Sharpa Wave robot's 22 finger joints - (per hand) to match MANO fingertip positions. -3. Compute 67 task-space reference "frames" (robot link poses) used for - reward computation during training. -4. Record IK error metrics and optimization iteration counts. - -The retarget script appends robot-specific fields (wrist pose, finger joints, -frames) to the existing Parquet alongside the original MANO and object data. - -**Output:** `human_motion_data/{dataset}/{dataset}_processed/` (same partition -structure, now includes SHARPA robot fields). - -**Command** (in-container — robotic_grounding image): -```bash -python scripts/retarget/run_retarget.py \ - --dataset --robot sharpa_wave \ - --input_dir \ - --output_dir \ - --device cuda:0 --save -``` - -**Run it locally** (override the image's Isaac Sim entrypoint — retarget is -pinocchio IK, no sim): - -```bash -docker run --rm --gpus all --entrypoint /bin/bash \ - -v "$PWD/..:/workspace/video_to_data" \ - -v :/data/loaded -v :/data/processed \ - -w /workspace/video_to_data/robotic_grounding \ - robotic-grounding: \ - -c "python scripts/retarget/run_retarget.py --dataset --robot sharpa_wave \ - --input_dir /data/loaded --output_dir /data/processed --device cuda:0 --save" -``` - -(Or `./workflow/run.sh start` then run the in-container command.) - -## Stage 3: Reconstruct Support Surfaces (optional) - -**Purpose:** Build collision meshes for surfaces that objects rest on. - -**Script:** `scripts/reconstruct_support_surfaces.py` - -Analyzes object still-poses across frames to identify the flat surface each -object contacts (e.g., a table). Generates a thin USD disk mesh at the -estimated support plane. These surfaces improve contact reward accuracy during -training by providing a physical ground for the objects. - -**Output:** `human_motion_data/{dataset}/reconstructed_stage/*.usda` - -**Command:** -```bash -python scripts/reconstruct_support_surfaces.py \ - --input_dir --dataset -``` - -## Stage 4: Visualize (optional) - -**Purpose:** Produce inspection artifacts alongside the retargeted data — -both interactive (`.viser`) and offline video (`.mp4`). Drives quality -reviews without downloading the full parquets locally. - -**Script:** `scripts/retarget/vis_retargeted.py` - -Running with `--save_html --save_mp4` writes, for every sequence: - -- `.viser` — web-based playback, shows MANO hands + Sharpa robot + - objects + support surfaces; open in `viser-client/?playbackPath=...`. -- `.mp4` — headless pyrender render of the same scene, auto-framed on - the object trajectory. No browser or Isaac Sim required. Useful for - quick QA at scale, embedding in reviews, or sanity-checking new datasets. - -Pyrender MP4s are skipped for OakInk2 (120 FPS × long sequences OOMs the -pod); Stage 5 already produces Isaac Sim MP4s for those. - -**Output:** `{dataset}_html/recordings/{seq}.viser` (+ `.mp4` where not -skipped) under the swift output prefix. - -## Stage 5: Record Isaac Sim Video (optional) - -**Purpose:** Record a playback MP4 of each retargeted sequence in Isaac Sim -with the Sharpa robot physically grasping the object — the closest thing -to a training-time render without actually training. Used for catching -regressions that don't show up in the MANO-only pyrender output (e.g. -collision-group misconfigurations, self-penetrations, physics blow-ups). - -**Script:** `scripts/rsl_rl/dummy_agent.py` (driven by the workflow's -`video` stage, not typically invoked directly) - -For each processed sequence the workflow spawns a single-env Isaac Sim -instance with `dummy_agent.py --record_video`, steps 300 frames of the -motion, and saves the MP4. Runs sequentially on one GPU — parallel Isaac -Sim startup contention (shader cache, EGL drivers, VRAM) made the sharded -version strictly slower in practice. - -**Output:** `{dataset}_videos/{sequence_id}.mp4` under the swift output prefix. - -## Stage 6: Sync Results Locally - -**Purpose:** Pull the workflow's swift outputs to the local repo so training -and local visualization scripts can read them. - -**Command:** `sync_css_data.py` / `aws s3 sync` — full details and -component-filter examples are in -[data_storage.md](data_storage.md#pulling-retarget-outputs-locally). - -Quick version: - -```bash -source scripts/setup_css_env.sh - -# Pull the pieces training needs (processed / loaded / support_surfaces) -python scripts/sync_css_data.py --dataset --component processed - -# Other components (urdfs/html/videos) via the aws CLI against the CSS endpoint -aws s3 sync \ - s3://datasets/v2d/human_motion_data//_urdfs/ \ - source/robotic_grounding/robotic_grounding/assets/human_motion_data//_urdfs/ \ - --endpoint-url ${CSS_ENDPOINT_URL} --region us-east-1 -``` - -## Stage 7: RL Training - -**Purpose:** Train a reinforcement learning policy to control the robot hand. - -**Script:** `scripts/rsl_rl/train.py` (runs inside Docker with Isaac Sim) - -### Scene Setup - -1. `SceneConfig` reads the processed Parquet, resolves object URDFs (via - object registry, parquet paths, or mesh-derived fallback), and loads - support surfaces if available. -2. Isaac Sim spawns the Sharpa Wave robot and objects in N parallel - environments. - -### Motion Command System - -3. The `CommandManager` loads retargeted motion data from the Parquet, - interpolates it to the simulation timestep, and provides per-step - target poses for both hands and all objects. -4. Contact tensors are initialized from Parquet data (or set to zero if the - dataset has no contact annotations). - -### Training Loop (PPO) - -5. Each simulation step: - - The command manager provides target robot joint positions, wrist poses, - and object poses from the motion trajectory. - - The policy network outputs joint position deltas. - - The simulation advances one physics step. - -6. Rewards measure how well the robot follows the reference motion: - - | Reward | What it tracks | - |--------|---------------| - | `hand_keypoints_tracking` | Wrist + fingertip position error | - | `hand_joint_pos_tracking` | Finger joint angle error | - | `object_keypoints_tracking` | Object position/orientation error | - | `contact_force_reward` | Matching reference contact patterns | - | `contact_wrench_support_reward` | Wrench-space contact quality | - | `action_rate_l2` / `action_l1` | Motion smoothness penalties | - - Contact-based rewards gracefully degrade to zero for datasets without - contact data (e.g., Hot3D). - -7. Episodes reset on timeout or when the hand/object drifts too far from - the reference trajectory. - -**Output:** Policy checkpoints in `logs/`. - -**Command:** -```bash -python scripts/rsl_rl/train.py \ - --task Sharpa-V2P-v0 \ - --headless \ - --motion_file /_processed//sharpa_wave \ - --num_envs 8 -``` - -## OSMO Workflow - -Stages 1.5 through 5 are submitted as a single OSMO workflow (`workflow/retarget.yaml`) running on the GPU cluster (Stage 1 Load runs separately in `reconstruction`). All artifacts from one run are written to the swift `output_url` prefix (`…/human_motion_data/{dataset}/`). See [README.md](README.md) for submission options and [data_storage.md](data_storage.md) for the output layout; `/osmo-retarget` skill covers usage patterns. - -```bash -python scripts/run_osmo.py \ - --experiment-name retarget- \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset= - -# Run a subset — e.g. skip the expensive video stage -python scripts/run_osmo.py \ - --experiment-name retarget--fast \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset= --set stages=process -``` - -## Dataset Inventory - -Candidates considered for Robot Grounding Task Library. Source: -[dataset-candidates spreadsheet](https://docs.google.com/spreadsheets/d/1aG_m4I1vuFdM5LkeHmSbhlF5zBxTRo-3Qu7TGThVt1M/edit). - -### In the pipeline - -Outputs live under the swift `…/human_motion_data//` prefix. - -| Dataset | Sequences | GT Fidelity | Hands | Notes | -|---------|-----------|-------------|-------|-------| -| **TACO** | 2,317 | High (NOKOV MoCap) | Bimanual | — | -| **Arctic** | 554 | Very High (Vicon MoCap) | Bimanual, articulated objects | — | -| **OakInk2** | 627 | High (OptiTrack MoCap) | Bimanual | Stage 4 MP4s skipped (long sequences OOM pyrender) | -| **HOT3D** | 294 | Very High (OptiTrack MoCap) | Bimanual | — | -| **H2O** | 137 | Medium-High (RGB-D opt.) | Bimanual | cam4 egocentric; known ~45° pitch (initial head tilt baked into PnP world frame) | -| **GRAB** | 1,335 | Very High (Vicon MoCap) | Bimanual + SMPL-X full body | — | -| **DexYCB** | 1,000 | Medium-High (multi-view RGB-D) | Single (per-session) | cam `932122062010`; master-frame gravity inferred per session | - -Current total: **~6,264 retargeted sequences** across 7 datasets and 150+ unique objects. - -### Next candidates (have MANO + tracked objects) - -Ordered by expected onboarding effort. - -| Dataset | Sequences | GT Fidelity | Notes | -|---------|-----------|-------------|-------| -| **HOI4D** | 4,000+ | Medium | 16 categories, rigid + articulated. Manual BBox annotation every 10 frames. Would roughly double the sequence count — biggest single-dataset gain available. | -| **FPHA** | 1,175 | Medium | 45 action categories. Older (2018), uses magnetic sensors (~2-3mm accuracy). Only 4/26 objects have meshes — most of the sequences would land without a mesh for training, limiting value. | - -### Not usable without extra preprocessing - -These datasets have MANO hands (or pseudo-labels) but no reliable object pose -ground truth, so they can't flow through the retarget pipeline as-is. - -| Dataset | Sequences | Why | -|---------|-----------|-----| -| HoloAssist | 2,200 | HaMeR pseudo-labels for hands, no object tracking | -| Taste-Rob | ~100K | Same — pseudo-labels + no object poses | -| EgoDex | 338K | ARKit skeleton only, no object tracking | -| ContactPose | 2,306 | Static grasps only (no temporal sequences) | - -## Adding a New Dataset - -See the `/add-dataset` skill or run through these steps: - -1. Add a `DatasetConfig` entry in `source/robotic_grounding/robotic_grounding/retarget/dataset_registry.py`. -2. Write a **loader** in the `reconstruction` repo at - `reconstruction/modules/v2d_task_library_loader/lib/_loader.py` (this is - the GPL/MANO stage) and register it in `lib/loader_registry.py`. -3. Write a **retarget** script here at `scripts/retarget/_to_sharpa.py`. - -Everything else (workflow dispatch, swift output publish, URDF generation, -training validation) is driven by the dataset registry automatically. - -## Validating Before Training - -Run the asset validator to catch missing URDFs or meshes before Isaac Sim -starts (saves the 45-second startup wait): - -```bash -python scripts/validate_training_assets.py --dataset -``` - -## Object Assets (URDFs + Meshes) - -Stage 1.5 (URDF gen), Stage 2 (retarget), Stage 3/4 (reconstruct/visualize), and -Stage 7 (training) load object geometry from: - -- `source/robotic_grounding/robotic_grounding/assets/meshes//` — object meshes -- `source/robotic_grounding/robotic_grounding/assets/urdfs//` — rigid URDFs - -**Obtaining them.** Download the dataset's object models from its original -distribution (per the dataset's own license/terms; see [Dataset Inventory](#dataset-inventory)), -copy the meshes into `assets/meshes//`, then generate the rigid URDFs: - -```bash -python scripts/generate_rigid_urdfs.py --dataset -``` - -For h2o/grab/dexycb the object meshes ship inside the raw dataset, so the loaders -read them from the raw tree — no separate mesh placement needed. - - -**NVIDIA-internal shortcut.** The prebuilt meshes + URDFs are mirrored on CSS; -pull them straight into place instead of re-downloading + regenerating: - -```bash -python scripts/fetch_object_assets.py --dataset # or --dataset all -``` - -(upload with `scripts/upload_object_assets.py`). OSMO retarget outputs also carry -a regenerated `{dataset}_urdfs/` tree. - - -> Still open: wire object-asset provisioning into the Docker image build / OSMO -> retarget so the cloud path doesn't rely on a committed copy. diff --git a/robotic_grounding/workflow/data_storage.md b/robotic_grounding/workflow/data_storage.md deleted file mode 100644 index 5d885edd..00000000 --- a/robotic_grounding/workflow/data_storage.md +++ /dev/null @@ -1,113 +0,0 @@ -# Task Library Data Storage - -All task-library data — raw inputs, the `{dataset}_loaded` Parquet, and the -retarget outputs — lives on the NVIDIA CSS (PDX) swift bucket under a single -per-dataset prefix: - -``` -swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/{dataset}/ -``` - -| | Where | Produced by | How it's accessed | -|---|---|---|---| -| **Raw inputs** (`dataset/`) | swift `…/{dataset}/dataset/` | uploaded once per dataset | auto-downloaded by the workflows; browse with `list_css_sequences.py` | -| **Loaded** (`{dataset}_loaded/`) | swift `…/{dataset}/{dataset}_loaded/` | **reconstruction** `v2d_{dataset}_load` workflow (MANO FK; GPL-contained) | consumed by `retarget.yaml`; pull with `sync_css_data.py` | -| **Retarget outputs** (`{dataset}_processed/`, `{dataset}_urdfs/`, `reconstructed_stage/`, `{dataset}_html/`, `{dataset}_videos/`, `{dataset}_quality.csv`) | swift `…/{dataset}/…` | `retarget.yaml` (writes to `output_url`) | pull with `sync_css_data.py` / `aws s3` | - -Object meshes + URDFs (arctic, taco, oakink2, hot3d) live on swift under -`…/{dataset}/object_assets/{meshes,urdfs}/{dataset}/` — **not committed** to the -repo. Fetch them locally with `scripts/fetch_object_assets.py --dataset ` -(upload with `scripts/upload_object_assets.py`). h2o/grab/dexycb keep meshes in -the raw dataset. Robot meshes (g1, sharpa_wave, …) remain committed under -`assets/meshes/`. - -## Why swift URLs (not OSMO datasets) - -Both workflows publish via swift `url:` outputs rather than versioned OSMO -`dataset:` outputs: - -- The OSMO **`isaac` dataset bucket is read-only** (`dataset:` writes are - rejected), so workflows write to the team's swift `AUTH_team-isaac` path - instead. -- It keeps the two stages **consistent** — the reconstruction load workflow - (`loaded_output_url`) and the RG retarget workflow (`output_url`) both write - swift prefixes with the same `…/{dataset}/…` layout. - -**Tradeoff — no automatic per-run versioning.** A swift `url:` output overwrites -the prefix in place; there's no rollback-by-version like an OSMO dataset gave. -For experiments, **override `output_url` to a scratch prefix** so you don't -clobber the canonical `{dataset}_processed/` (mirrors how the load workflow's -`loaded_output_url` defaults to canonical and is overridden for tests): - -```bash -python scripts/run_osmo.py --experiment-name retarget-taco-test \ - --workflow-yaml workflow/retarget.yaml \ - --set dataset=taco --set stages=process \ - --set output_url=swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/taco/scratch_taco_test/ -``` - -(`retarget.yaml`'s `dataset_suffix` is deprecated/unused now that output is a URL; -override `output_url` instead.) - -## Storage layout (swift) - -``` -swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/ - human_motion_data/ - {dataset}/ - dataset/ # Raw data (downloaded into each workflow run) - {dataset}_loaded/ # reconstruction load workflow (Parquet: MANO + object poses) - {dataset}_urdfs/ # Stage 1.5 (per-object rigid URDFs) - {dataset}_processed/ # Stage 2 (Parquet: IK-retargeted robot trajectories) - reconstructed_stage/ # Stage 3 (support-surface USDs; absent when every object is held) - {dataset}_html/ # Stage 4 (Viser recordings + pyrender MP4s) - {dataset}_videos/ # Stage 5 (Isaac Sim MP4s via dummy_agent) - {dataset}_quality.csv # Stage 6 (per-sequence quality metrics) -``` - -Supported datasets include `taco`, `arctic`, `oakink2`, `hot3d`, `h2o`, `dexycb`, `grab`. - -## Browsing available raw sequences on CSS - -`list_css_sequences.py` lists what's on the CSS input prefix — useful for picking -a sequence to target with `sequence_id` or `sequence_pattern`: - -```bash -# Set CSS credentials -source scripts/setup_css_env.sh - -# List all datasets -python scripts/list_css_sequences.py - -# List a specific dataset / filter by regex -python scripts/list_css_sequences.py --dataset taco -python scripts/list_css_sequences.py --dataset taco --pattern '.*screw.*' -``` - -## Pulling retarget outputs locally - -After a workflow completes, pull its swift outputs into the local repo so -training / visualization scripts find them under -`source/robotic_grounding/robotic_grounding/assets/human_motion_data/{dataset}/`. - -```bash -source scripts/setup_css_env.sh - -# Pull a component (loaded / processed / support_surfaces) -python scripts/sync_css_data.py --dataset taco --component processed - -# Or pull a prefix directly with the aws CLI (any component, incl. urdfs/html/videos) -aws s3 sync \ - s3://datasets/v2d/human_motion_data/taco/taco_processed/ \ - source/robotic_grounding/robotic_grounding/assets/human_motion_data/taco/taco_processed/ \ - --endpoint-url ${CSS_ENDPOINT_URL} --region us-east-1 -``` - -The downloaded layout matches what the training scripts expect — the -`motion_file` arg to `scripts/rsl_rl/train.py` resolves as -`{dataset}/{dataset}_processed/{sequence_id}/sharpa_wave` relative to -`HUMAN_MOTION_DATA_DIR`. - -> `sync_css_data.py` currently knows `loaded` / `processed` / `support_surfaces`; -> for `urdfs` / `html` / `videos` use the `aws s3 sync` form above until a -> `--component` is added. diff --git a/robotic_grounding/workflow/dev_env.yaml b/robotic_grounding/workflow/dev_env.yaml deleted file mode 100644 index c333d48f..00000000 --- a/robotic_grounding/workflow/dev_env.yaml +++ /dev/null @@ -1,77 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -workflow: - name: {{workflow_name}} - resources: - default: - cpu: 100 - gpu: 8 - memory: 800Gi - storage: 2000Gi - - tasks: - - name: dev-env - image: {{image}} - command: - - /bin/bash - args: - - /tmp/entry.sh - credentials: - sshd-keys: /tmp/.ssh - files: - - path: /tmp/setup_ssh.sh - contents: |- - set -ex - - # Install sshd - apt update - apt install -y openssh-server - - # Configure SSH to allow root login - sed -i 's/#PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config - sed -i 's/PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config - - # Setup SSH keys for root - mkdir -p /root/.ssh - chmod 700 /root/.ssh - - # Setup authorized keys to allow logging in with key - CLIENT_PUBLIC_KEY=$(cat /tmp/.ssh/client-public-key) - echo "$CLIENT_PUBLIC_KEY" > /root/.ssh/authorized_keys - chmod 600 /root/.ssh/authorized_keys - - # Setup sshd server private key so the client will recognize it - cp /tmp/.ssh/server-private-key /etc/ssh/ssh_host_ed25519_key - cp /tmp/.ssh/server-public-key /etc/ssh/ssh_host_ed25519_key.pub - chmod 600 /etc/ssh/ssh_host_ed25519_key - chmod 644 /etc/ssh/ssh_host_ed25519_key.pub - - # Start sshd - mkdir -p /run/sshd - /usr/sbin/sshd - - echo "SSH server started - root login enabled" - - - path: /tmp/entry.sh - contents: |- - set -ex - bash /tmp/setup_ssh.sh - sleep infinity - -default-values: - workflow_name: robotic_grounding_dev_env - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:latest diff --git a/robotic_grounding/workflow/retarget.yaml b/robotic_grounding/workflow/retarget.yaml deleted file mode 100644 index 0628b37c..00000000 --- a/robotic_grounding/workflow/retarget.yaml +++ /dev/null @@ -1,421 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -# Retarget pipeline: loaded Parquet -> retargeted Parquet -> support surfaces. -# -# The LOAD stage (raw dataset -> {dataset}_loaded Parquet via MANO FK) runs -# SEPARATELY in the reconstruction `v2d_task_library_loader` load workflow -# (manotorch/GPL-3.0, contained there); this workflow consumes its -# `{dataset}_loaded` output. -# -# Controls which stages run via the `stages` parameter: -# all - run every stage below (default) -# process - loaded Parquet -> retargeted Parquet (IK) + rigid URDFs -# reconstruct - loaded Parquet -> support surface USD -# visualize - retargeted Parquet -> offline Viser + robot-hand MP4s (pyrender) -# video - retargeted Parquet -> Isaac Lab MP4s with robot hands (dummy_agent) -# -# HOT3D-only behavior: -# Stage 1.6 (before retarget, on the loaded Parquet) splits ~90-second HOT3D -# sequences into atomic hand-object interaction clips. All subsequent stages -# (URDFs, retarget, visualize, video, quality filter) then operate on segments. -# Penetration filtering happens transparently in Stage 6 via the auto- -# discovered hand_penetration data_quality_check. -# -# Usage: -# # Full pipeline -# python scripts/run_osmo.py --experiment-name retarget-taco \ -# --image nvcr.io/nvstaging/isaac-amr/robotic-grounding:datasets \ -# --workflow-yaml workflow/retarget.yaml --set dataset=taco -# -# # (Load runs separately — see reconstruction/workflows/task_library_load/.) -# -# # Process only (IK retargeting) -# python scripts/run_osmo.py --experiment-name retarget-taco-process \ -# --workflow-yaml workflow/retarget.yaml --set dataset=taco --set stages=process -# -# # With filtering: -# python scripts/run_osmo.py --experiment-name retarget-taco-screw \ -# --image nvcr.io/nvstaging/isaac-amr/robotic-grounding:datasets \ -# --workflow-yaml workflow/retarget.yaml --set dataset=taco \ -# --set 'sequence_pattern=.*(screw|stir).*' - -workflow: - name: {{workflow_name}} - resources: - default: - cpu: 80 - gpu: 8 - memory: 256Gi - storage: 200Gi - - tasks: - - name: retarget - image: {{image}} - command: - - /bin/bash - args: - - /tmp/entry.sh - environment: - ACCEPT_EULA: Y - inputs: - # Consume the {{dataset}}_loaded Parquet produced by the reconstruction - # `v2d_task_library_loader` load workflow (raw -> MANO FK -> loaded). The raw - # dataset is no longer pulled here — every stage below reads the loaded / - # processed Parquet (Stage 1.5 reads baked object meshes). Fine-grained - # sequence filtering happens in the Python scripts via FILTER_ARGS below. - - url: {{input_url}} - regex: {{dataset}}_loaded/.* - outputs: - # Publish the whole {{output}} folder to a swift prefix, mirroring the - # reconstruction load workflow's url-based output. (Previously a versioned - # OSMO `dataset:`, but the isaac dataset bucket is read-only; swift writes - # via the team AUTH path still work.) The stage scripts write - # {{dataset}}_processed / {{dataset}}_urdfs / reconstructed_stage / … under - # {{output}}, which land beside the loaded data at output_url. For - # experiments, override output_url to a scratch prefix so you don't - # overwrite the canonical {{dataset}}_processed. - - url: {{output_url}} - files: - - path: /tmp/entry.sh - contents: |- - set -ex - OUTPUT={{output}} - STAGES={{stages}} - SHARDS={{shards}} - - # Symlink downloaded dataset into the expected path structure - export HUMAN_MOTION_DATA_DIR=/tmp/human_motion_data - mkdir -p ${HUMAN_MOTION_DATA_DIR} - ln -sfn {{input:0}} ${HUMAN_MOTION_DATA_DIR}/{{dataset}} - - FILTER_ARGS="" - if [ -n "{{sequence_pattern}}" ]; then - FILTER_ARGS="${FILTER_ARGS} --sequence_pattern '{{sequence_pattern}}'" - fi - if [ -n "{{sequence_id}}" ]; then - FILTER_ARGS="${FILTER_ARGS} --sequence_id {{sequence_id}}" - fi - if [ -n "{{max_sequences}}" ]; then - FILTER_ARGS="${FILTER_ARGS} --max_sequences {{max_sequences}}" - fi - - LOADED_DIR=${HUMAN_MOTION_DATA_DIR}/{{dataset}}/{{dataset}}_loaded - PROCESSED_DIR=${HUMAN_MOTION_DATA_DIR}/{{dataset}}/{{dataset}}_processed - RECONSTRUCTED_DIR=${HUMAN_MOTION_DATA_DIR}/{{dataset}}/reconstructed_stage - - # Wait on all background shard PIDs; tag any failure by shard index - # (not PID) so the log is easy to grep for "[ERROR] shard N", and - # propagate a non-zero exit so `set -e` aborts the stage (bash doesn't - # trigger set -e on background-job failures, hence manual checking). - wait_shards() { - local ec=0 - local shard=0 - local sc - for pid in "$@"; do - sc=0 - wait $pid || sc=$? - if [ $sc -ne 0 ]; then - echo "[ERROR] shard ${shard} (PID ${pid}) failed with exit ${sc}" >&2 - ec=$sc - fi - shard=$((shard+1)) - done - return $ec - } - - # Stage 1 (load: raw dataset -> {{dataset}}_loaded Parquet via MANO FK) now - # runs in the reconstruction `v2d_task_library_loader` load workflow. - # This workflow CONSUMES its output: - # ${LOADED_DIR} is populated from the {{dataset}}_loaded/ input (see - # `inputs:` above), not generated here. - - # Stage 1.6 (HOT3D only): segment long loaded sequences into atomic - # hand-object interaction clips bounded by contact events. The - # segmenter only consumes loader-stage fields (mano_*_tips_distance, - # mano_*_object_contact_part_ids), so it can run BEFORE retarget; - # every downstream stage (URDFs, retarget, visualize, dummy_agent, - # quality filter) then operates on atomic segments instead of the - # original ~90-second sequences. Penetration filtering is handled - # transparently in Stage 6 — ``hand_penetration.py`` is auto-discovered - # by ``data_assessor.py`` and wraps ``filter_penetrations.py``. - if [ "{{dataset}}" = "hot3d" ] && \ - { [ "${STAGES}" = "all" ] || [ "${STAGES}" = "process" ]; }; then - echo "=== Stage 1.6: HOT3D segmentation ===" - SEGMENTED_LOADED_DIR=${LOADED_DIR}_segmented - rm -rf "${SEGMENTED_LOADED_DIR}" - eval python scripts/segment_to_atomic.py \ - --input_dir ${LOADED_DIR} \ - --output_dir ${SEGMENTED_LOADED_DIR} \ - ${FILTER_ARGS} - seg_count=$(ls -1d ${SEGMENTED_LOADED_DIR}/sequence_id=* 2>/dev/null | wc -l) - full_count=$(ls -1d ${LOADED_DIR}/sequence_id=* 2>/dev/null | wc -l) - echo "=== Stage 1.6: ${seg_count} segments from ${full_count} full sequences ===" - # Publish the segmented loaded dir alongside the original full - # loaded dir so downstream consumers can pick either granularity. - cp -r ${SEGMENTED_LOADED_DIR} ${OUTPUT}/{{dataset}}_loaded_segmented - # Subsequent stages (URDFs, retarget, …) read from ${LOADED_DIR}; - # point that variable at the segmented dir so they see atomic clips. - LOADED_DIR=${SEGMENTED_LOADED_DIR} - # Drop --max_sequences so downstream stages process ALL segments - # (Stage 1's limit already constrained the inputs; segments are - # the real downstream units now). Sequence-id/pattern filters are - # preserved by re-deriving FILTER_ARGS from the templated vars. - FILTER_ARGS="" - if [ -n "{{sequence_pattern}}" ] && [ "{{sequence_pattern}}" != ".*" ]; then - FILTER_ARGS="${FILTER_ARGS} --sequence_pattern '{{sequence_pattern}}'" - fi - if [ -n "{{sequence_id}}" ]; then - FILTER_ARGS="${FILTER_ARGS} --sequence_id {{sequence_id}}" - fi - fi - - # Stage 1.5: Generate rigid URDFs (one-shot — not per-sequence) - if [ "${STAGES}" = "all" ] || [ "${STAGES}" = "process" ]; then - echo "=== Stage 1.5: Generating rigid URDFs for {{dataset}} ===" - python scripts/generate_rigid_urdfs.py --dataset {{dataset}} || true - URDF_SRC=source/robotic_grounding/robotic_grounding/assets/urdfs/{{dataset}} - if [ -d "${URDF_SRC}" ]; then - cp -r ${URDF_SRC} ${OUTPUT}/{{dataset}}_urdfs - fi - fi - - # Stage 2: IK retargeting -> retargeted Parquet (sharded) - if [ "${STAGES}" = "all" ] || [ "${STAGES}" = "process" ]; then - echo "=== Stage 2: Retargeting {{dataset}} (${SHARDS} shards) ===" - PIDS=() - for i in $(seq 0 $((SHARDS-1))); do - ( - export CUDA_VISIBLE_DEVICES=$i - eval python scripts/retarget/run_retarget.py \ - --dataset {{dataset}} \ - --input_dir ${LOADED_DIR} \ - --output_dir ${PROCESSED_DIR} \ - --device cuda:0 \ - --save ${FILTER_ARGS} \ - --shard_id $i --num_shards ${SHARDS} - ) & - PIDS+=($!) - done - wait_shards "${PIDS[@]}" - echo "=== Copying processed data to output ===" - cp -r ${PROCESSED_DIR} ${OUTPUT}/{{dataset}}_processed - fi - - # Stage 3: Reconstruct support surfaces -> USD (sharded) - if [ "${STAGES}" = "all" ] || [ "${STAGES}" = "reconstruct" ]; then - echo "=== Stage 3: Reconstructing support surfaces for {{dataset}} (${SHARDS} shards) ===" - PIDS=() - for i in $(seq 0 $((SHARDS-1))); do - ( - export CUDA_VISIBLE_DEVICES=$i - eval python scripts/reconstruct_support_surfaces.py \ - --input_dir ${LOADED_DIR} \ - --dataset {{dataset}} \ - ${FILTER_ARGS} \ - --shard_id $i --num_shards ${SHARDS} - ) & - PIDS+=($!) - done - wait_shards "${PIDS[@]}" - # Datasets where every object is held/grounded (e.g. h2o) never - # produce a reconstructed_stage dir — skip the copy instead of - # failing the whole workflow under `set -e`. - if [ -d "${RECONSTRUCTED_DIR}" ]; then - echo "=== Copying reconstructed data to output ===" - cp -r ${RECONSTRUCTED_DIR} ${OUTPUT}/reconstructed_stage - else - echo "=== No reconstructed stage to copy (no support surfaces for {{dataset}}) ===" - fi - fi - - # Stage 4: Viser + pyrender MP4s (sharded). - # Pyrender MP4s buffer every rendered frame in RAM (see - # ``_offline_video.py::save``), so datasets with very long sequences - # (oakink2 at 120 FPS ~ 4k frames/seq = ~11 GB per clip, × 8 shards - # running concurrently) OOM the pod. Stage 5's dummy_agent already - # produces Isaac Sim MP4s with the robot hands, so the pyrender - # MP4 is redundant there — we keep the .viser recordings, which - # are compact and let anyone replay interactively in a browser. - if [ "${STAGES}" = "all" ] || [ "${STAGES}" = "visualize" ]; then - echo "=== Stage 4: Generating Viser + MP4 visualizations for {{dataset}} (${SHARDS} shards) ===" - MP4_ARG="--save_mp4" - if [ "{{dataset}}" = "oakink2" ]; then - MP4_ARG="" - echo " (skipping pyrender MP4 for oakink2 — long sequences OOM the pod)" - fi - PIDS=() - for i in $(seq 0 $((SHARDS-1))); do - ( - export CUDA_VISIBLE_DEVICES=$i - eval python scripts/retarget/vis_retargeted.py \ - --input_dir ${PROCESSED_DIR} \ - --save_html \ - ${MP4_ARG} \ - --html_dir ${OUTPUT}/{{dataset}}_html \ - ${FILTER_ARGS} \ - --shard_id $i --num_shards ${SHARDS} - ) & - PIDS+=($!) - done - wait_shards "${PIDS[@]}" - fi - - # Stage 5: Isaac Lab MP4s via dummy_agent (robot + object). - # Experiment: shard across all GPUs to parallelize sequence rendering. - # A prior attempt went slower than sequential due to Isaac Sim startup - # contention (shader cache, EGL drivers, VRAM); revisit with current - # image + drivers and re-measure. Fall back to sequential by setting - # SHARDS=1 at the workflow level. - if [ "${STAGES}" = "all" ] || [ "${STAGES}" = "video" ]; then - echo "=== Stage 5: Recording scene videos for {{dataset}} (${SHARDS} shards) ===" - VIDEO_DIR=${OUTPUT}/{{dataset}}_videos - mkdir -p ${VIDEO_DIR} - - # Enumerate sequences once, then distribute across shards by index - # mod SHARDS so the work is roughly balanced. The per-sequence body - # is the same as the old sequential path — only the surrounding - # shard fan-out is new. - SEQ_DIRS=(${PROCESSED_DIR}/sequence_id=*/robot_name=*) - - PIDS=() - for shard in $(seq 0 $((SHARDS-1))); do - ( - export CUDA_VISIBLE_DEVICES=$shard - for idx in "${!SEQ_DIRS[@]}"; do - if [ $((idx % SHARDS)) -ne $shard ]; then - continue - fi - SEQ_DIR="${SEQ_DIRS[$idx]}" - [ -d "${SEQ_DIR}" ] || continue - # URL-decode the partition-directory name so oakink2's - # `++`-encoded IDs (sequence_id=scene_01__A007%2B%2Bseq__…) - # don't end up in file paths as literal `%2B`. `%2` in any - # path breaks gymnasium's RecordVideo wrapper, which does - # `msg % args` over its own log lines and treats `%2` as a - # format placeholder. - # - # We *don't* call out to `python -c` for this — in this image - # `python` is a wrapper around `isaaclab.sh -p` that prints - # `[INFO] Using python from: …` plus tset escape sequences on - # stdout before the real interpreter runs, so $(python -c …) - # captures that banner and poisons SEQ_ID. Pure-bash - # parameter expansion + `printf '%b'` does the decode without - # a subprocess. - RAW_ID=$(basename $(dirname "${SEQ_DIR}") | sed 's|sequence_id=||') - # shellcheck disable=SC2059 # intentional: %b interprets \xHH - SEQ_ID=$(printf '%b' "${RAW_ID//%/\\x}") - # Idempotent: skip sequences already rendered (partial resume). - [ -f "${VIDEO_DIR}/${SEQ_ID}.mp4" ] && continue - # Shard-scoped TMP dir avoids collisions when two shards pick - # up the same SEQ_ID by accident (shouldn't happen with modulo - # distribution, but cheap insurance if the array ever grows - # duplicates). - TMP_VID=/tmp/scene_video_shard${shard}_${SEQ_ID} - rm -rf ${TMP_VID} - # Wrap in `timeout --signal=KILL` — when Isaac Sim hits a CUDA - # assertion its Omniverse shutdown can deadlock, so `|| true` - # alone would leave the bash loop waiting forever. 5 min is - # plenty of headroom for ~1 min expected per-sequence work. - # Success sentinel goes IN the published tree, colocated - # with the parquet it vouches for. PROCESSED_DIR → OUTPUT - # was copied in Stage 2 (line 179) with the same - # sequence_id=…/robot_name=… layout, so a straight path - # rebase is enough. Stage 6 below keys off - # `.dummy_agent_ok` presence to decide kept vs. invalid. - REL_SEQ=${SEQ_DIR#${PROCESSED_DIR}/} - OUTPUT_SEQ_DIR=${OUTPUT}/{{dataset}}_processed/${REL_SEQ} - timeout --signal=KILL 300 python scripts/rsl_rl/dummy_agent.py \ - --task Sharpa-V2P-v0-Play \ - --motion_file ${SEQ_DIR} \ - --num_envs 1 --headless \ - --record_video --output_dir ${TMP_VID} --video_length 300 \ - --success_marker ${OUTPUT_SEQ_DIR}/.dummy_agent_ok || true - if [ -f "${TMP_VID}/rl-video-step-0.mp4" ]; then - mv ${TMP_VID}/rl-video-step-0.mp4 ${VIDEO_DIR}/${SEQ_ID}.mp4 - fi - rm -rf ${TMP_VID} - done - ) & - PIDS+=($!) - done - wait_shards "${PIDS[@]}" - - # Stage 6: Data quality filter. - # Runs `scripts/data_assessor.py` with auto-discovered checks under - # `scripts/data_quality_checks/`. The `dummy_agent_success` check - # reads the `.dummy_agent_ok` sentinel left behind by Stage 5; - # content checks (fitting_error, ik_task_error, sequence_length, - # tip_distance) read parquet columns. Any sequence failing at least - # one check is moved to `${dataset}_processed_invalid/`. The - # per-sequence metrics CSV is published alongside the data so - # downstream consumers can grep reasons for rejection. - # Fail-open: `|| true` on the assessor means an assessor bug ships - # all sequences as valid rather than dropping the whole run. - if [ -d "${OUTPUT}/{{dataset}}_processed" ]; then - echo "=== Stage 6: Data quality filter ===" - REJECT_FILE=/tmp/{{dataset}}_rejected.txt - QUALITY_CSV=${OUTPUT}/{{dataset}}_quality.csv - rm -f "${REJECT_FILE}" - python scripts/data_assessor.py \ - --input_dir ${OUTPUT}/{{dataset}}_processed \ - --reject --output_reject ${REJECT_FILE} \ - --output_csv ${QUALITY_CSV} || true - - INVALID_DIR=${OUTPUT}/{{dataset}}_processed_invalid - mkdir -p "${INVALID_DIR}" - moved=0 - if [ -s "${REJECT_FILE}" ]; then - while IFS= read -r SEQ_ID; do - [ -z "${SEQ_ID}" ] && continue - SRC="${OUTPUT}/{{dataset}}_processed/sequence_id=${SEQ_ID}" - [ -d "${SRC}" ] && mv "${SRC}" "${INVALID_DIR}/" && moved=$((moved+1)) - done < "${REJECT_FILE}" - fi - # Sentinels are workflow-internal bookkeeping; strip before OSMO - # publishes the dataset so the manifest stays clean. - find "${OUTPUT}/{{dataset}}_processed" -name .dummy_agent_ok -delete 2>/dev/null || true - find "${INVALID_DIR}" -name .dummy_agent_ok -delete 2>/dev/null || true - [ ${moved} -eq 0 ] && rmdir "${INVALID_DIR}" 2>/dev/null || true - echo "=== Stage 6: moved ${moved} invalid sequence(s); quality CSV: ${QUALITY_CSV} ===" - fi - fi - - echo "=== Done ===" - -default-values: - workflow_name: robotic_grounding_retarget - # TODO(public-release): internal image registry (nvcr.io/nvstaging) — replace before open-sourcing. - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:datasets - stages: all - dataset: arctic - # TODO(public-release): input_url/output_url point to internal NVIDIA storage - # (pdx.s8k.io / AUTH_team-isaac). Replace the swift host + bucket (or override - # via --set) before open-sourcing. - input_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/{{dataset}}/ - output_url: swift://pdx.s8k.io/AUTH_team-isaac/datasets/v2d/human_motion_data/{{dataset}}/ - sequence_pattern: ".*" - sequence_id: "" - max_sequences: "" - # Deprecated/unused: output is now a swift `url:` (see outputs above), not a - # versioned OSMO dataset, so this suffix no longer affects anything. Kept so - # existing `--set dataset_suffix=...` callers don't error. For experiment - # isolation, override output_url to a scratch prefix instead. - dataset_suffix: "" - # Sharding: each stage fans out into SHARDS parallel processes, each pinned - # to CUDA_VISIBLE_DEVICES=. Defaults to 8 to match the - # 8×L40 per-task resource request above. - shards: "8" diff --git a/robotic_grounding/workflow/run.sh b/robotic_grounding/workflow/run.sh deleted file mode 100755 index 50a32d6b..00000000 --- a/robotic_grounding/workflow/run.sh +++ /dev/null @@ -1,279 +0,0 @@ -#!/bin/bash - -# Script to manage robotic-grounding Docker container -# Usage: -# ./run.sh build [version] - Build x86_64 image (default version: latest) -# ./run.sh build-aarch64 [version] - Build aarch64 image (default version: latest) -# ./run.sh push [version] - Push x86_64 image to NVIDIA registry -# ./run.sh push-aarch64 [version] - Push aarch64 image to NVIDIA registry -# ./run.sh pull [version] - Pull x86_64 image from NVIDIA registry -# ./run.sh pull-aarch64 [version] - Pull aarch64 image from NVIDIA registry -# ./run.sh start [version] [gpu] - Start x86_64 container (default version: latest, gpu: 0) -# ./run.sh start-aarch64 [version] [gpu] - Start aarch64 container -# ./run.sh shell [version] [gpu] - Enter shell of a running container -# ./run.sh shell-aarch64 [version] [gpu] - Enter shell of a running aarch64 container -# ./run.sh exec [version] [gpu] -- - Run a command in a running container -# ./run.sh stop [version] [gpu] - Stop the running container -# ./run.sh stop-aarch64 [version] [gpu] - Stop the running aarch64 container - -set -e - -# Detect arch suffix from the subcommand — affects IMAGE_NAME and CONTAINER_NAME. -# e.g. start-aarch64 uses robotic-grounding:latest-aarch64 and a distinct container name. -ARCH_SUFFIX="" -CMD="$1" -if [[ "$CMD" == *-aarch64 ]]; then - ARCH_SUFFIX="-aarch64" - CMD="${CMD%-aarch64}" -fi - -VERSION=${2:-latest} -GPU_DEVICE=${3:-0} -IMAGE_NAME="robotic-grounding${ARCH_SUFFIX}:${VERSION}" -CONTAINER_NAME="robotic-grounding${ARCH_SUFFIX}-${VERSION}-gpu${GPU_DEVICE}" -NGC_LOCATION="nvcr.io/nvstaging/isaac-amr" - -case "$CMD" in - build) - echo "Building Docker image: ${IMAGE_NAME}" - cd "$(dirname "$0")/.." - - if [ -n "$ARCH_SUFFIX" ]; then - SETUP_OK=true - - if ! docker buildx version &>/dev/null; then - echo "" - echo "ERROR: docker buildx plugin is not installed." - echo " Fix: sudo apt-get install -y docker-buildx-plugin" - SETUP_OK=false - fi - - if ${SETUP_OK} && ! docker buildx ls 2>/dev/null | grep -q "multiarch"; then - echo "" - echo "WARNING: 'multiarch' buildx builder not found." - echo " Fix: docker buildx create --name multiarch --use" - echo " docker buildx inspect --bootstrap" - SETUP_OK=false - fi - - if ${SETUP_OK} && ! docker buildx ls 2>/dev/null | grep -q "linux/arm64"; then - echo "" - echo "WARNING: arm64 platform not available in buildx." - echo " Fix: docker run --privileged --rm tonistiigi/binfmt --install arm64" - SETUP_OK=false - fi - - if ! ${SETUP_OK}; then - echo "" - echo "One-time setup (run in order):" - echo " 1. sudo apt-get install -y docker-buildx-plugin" - echo " 2. docker run --privileged --rm tonistiigi/binfmt --install arm64" - echo " 3. docker buildx create --name multiarch --use" - echo " 4. docker buildx inspect --bootstrap" - exit 1 - fi - - docker buildx build --platform linux/arm64 --load \ - -t ${IMAGE_NAME} -f workflow/Dockerfile.aarch64 . - else - docker build -t ${IMAGE_NAME} -f workflow/Dockerfile . - fi - - echo "Build complete: ${IMAGE_NAME}" - ;; - - push) - echo "Pushing ${IMAGE_NAME} to NVIDIA registry..." - docker tag ${IMAGE_NAME} ${NGC_LOCATION}/${IMAGE_NAME} - docker push ${NGC_LOCATION}/${IMAGE_NAME} - echo "Push complete!" - echo "Removing local images to free disk space..." - docker rmi ${NGC_LOCATION}/${IMAGE_NAME} ${IMAGE_NAME} || true - ;; - - pull) - echo "Pulling ${NGC_LOCATION}/${IMAGE_NAME}..." - docker pull ${NGC_LOCATION}/${IMAGE_NAME} - docker tag ${NGC_LOCATION}/${IMAGE_NAME} ${IMAGE_NAME} - echo "Pull complete: ${IMAGE_NAME}" - ;; - - start) - echo "Starting container: ${CONTAINER_NAME} (image: ${IMAGE_NAME})" - - if docker ps --format '{{.Names}}' | grep -q "^${CONTAINER_NAME}$"; then - echo "Container is already running. Entering shell..." - else - echo "Creating and starting new container..." - cd "$(dirname "$0")/.." - xhost +local:docker > /dev/null 2>&1 || true - - SSH_AGENT_MOUNT="" - SSH_AGENT_ENV="" - if [ -n "${SSH_AUTH_SOCK}" ] && [ -S "${SSH_AUTH_SOCK}" ]; then - SSH_AGENT_MOUNT="-v ${SSH_AUTH_SOCK}:/ssh-agent" - SSH_AGENT_ENV="-e SSH_AUTH_SOCK=/ssh-agent" - else - echo "Warning: SSH agent socket not found; agent forwarding disabled." - fi - - # WANDB_API_KEY: use env if set (must be exported), else read from host home - WANDB_API_KEY_VALUE="${WANDB_API_KEY}" - if [ -z "${WANDB_API_KEY_VALUE}" ] && [ -f "${HOME}/.wand_api_key" ]; then - WANDB_API_KEY_VALUE=$(cat "${HOME}/.wand_api_key") - fi - WANDB_API_KEY_ENV="" - if [ -n "${WANDB_API_KEY_VALUE}" ]; then - WANDB_API_KEY_ENV="-e WANDB_API_KEY=${WANDB_API_KEY_VALUE}" - fi - - # Optional: overlay an external human_motion_data directory (e.g. from another repo). - # Set HUMAN_MOTION_DATA_DIR on the host to an absolute path before calling run.sh: - # HUMAN_MOTION_DATA_DIR=/path/to/human_motion_data ./workflow/run.sh start - DATA_MOUNT="" - CONTAINER_DATA_DIR="/workspace/video_to_data/robotic_grounding/source/robotic_grounding/robotic_grounding/assets/human_motion_data" - if [ -n "${HUMAN_MOTION_DATA_DIR}" ]; then - DATA_MOUNT="-v ${HUMAN_MOTION_DATA_DIR}:${CONTAINER_DATA_DIR}" - echo "Mounting external data: ${HUMAN_MOTION_DATA_DIR} → ${CONTAINER_DATA_DIR}" - fi - - # Build per-container passwd/group so the host UID has a name - # inside the container (avoids the "I have no name!" bash prompt - # and keeps tools that call getpwuid() happy: git, ssh, etc.). - # No secrets are written: passwords use the "x" placeholder and - # /etc/shadow is NOT mounted into the container. - HOST_UID="$(id -u)" - HOST_GID="$(id -g)" - HOST_USERNAME="$(id -un)" - HOST_GROUPNAME="$(id -gn)" - if ! [[ "${HOST_USERNAME}" =~ ^[a-z_][a-z0-9_-]*$ ]]; then - HOST_USERNAME="user" - fi - if ! [[ "${HOST_GROUPNAME}" =~ ^[a-z_][a-z0-9_-]*$ ]]; then - HOST_GROUPNAME="usergroup" - fi - CACHE_ROOT="${HOME:-/tmp}/.cache/robotic-grounding" - CONTAINER_PASSWD_DIR="${CACHE_ROOT}/${CONTAINER_NAME}" - mkdir -p "${CONTAINER_PASSWD_DIR}" - chmod 0700 "${CACHE_ROOT}" "${CONTAINER_PASSWD_DIR}" 2>/dev/null || true - umask 0022 - # Per-container shadowed /etc/passwd and /etc/group. We add an - # entry for the Isaac Sim image's owner (uid/gid 1234, name - # isaac-sim) so that --group-add 1234 below resolves to a real - # name inside the container; cosmetic but keeps tools that call - # getgrgid() happy. - cat > "${CONTAINER_PASSWD_DIR}/passwd" < "${CONTAINER_PASSWD_DIR}/group" </dev/null || true - [ "$1" = "--" ] && shift - if [ $# -eq 0 ]; then - echo "Usage: $0 exec${ARCH_SUFFIX} [version] [gpu] -- " - exit 1 - fi - - if ! docker ps --format '{{.Names}}' | grep -q "^${CONTAINER_NAME}$"; then - echo "Error: Container ${CONTAINER_NAME} is not running." - echo "Use './run.sh start${ARCH_SUFFIX}' to start the container first." - exit 1 - fi - - docker exec -it ${CONTAINER_NAME} "$@" - ;; - - stop) - echo "Stopping container: ${CONTAINER_NAME}" - - if ! docker ps --format '{{.Names}}' | grep -q "^${CONTAINER_NAME}$"; then - echo "Container ${CONTAINER_NAME} is not running." - exit 0 - fi - - docker stop ${CONTAINER_NAME} - echo "Container stopped successfully." - ;; - - *) - echo "Usage: $0 {build|push|pull|start|shell|exec|stop}[-aarch64] [version] [gpu]" - echo "" - echo " build [version] - Build x86_64 image (default: latest)" - echo " build-aarch64 [version] - Build aarch64 image (default: latest-aarch64)" - echo " push [version] - Push x86_64 image to NGC" - echo " push-aarch64 [version] - Push aarch64 image to NGC" - echo " pull [version] - Pull x86_64 image from NGC" - echo " pull-aarch64 [version] - Pull aarch64 image from NGC" - echo " start [version] [gpu] - Start x86_64 container (default gpu: 0)" - echo " start-aarch64 [version] [gpu] - Start aarch64 container" - echo " shell [version] [gpu] - Enter shell of a running container" - echo " shell-aarch64 [version] [gpu] - Enter shell of a running aarch64 container" - echo " exec [version] [gpu] -- - Run a command in a running container" - echo " stop [version] [gpu] - Stop the running container" - echo " stop-aarch64 [version] [gpu] - Stop the running aarch64 container" - exit 1 - ;; -esac diff --git a/robotic_grounding/workflow/setup_deps.sh b/robotic_grounding/workflow/setup_deps.sh deleted file mode 100644 index 9ac1812f..00000000 --- a/robotic_grounding/workflow/setup_deps.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env bash -set -euo pipefail - -sudo apt update -sudo apt install -y git-lfs pipx -pipx ensurepath -pipx install pre-commit -chmod +x "$(dirname "$0")/run.sh" - -echo "Done. You may need to restart your shell for pipx PATH changes." diff --git a/robotic_grounding/workflow/train.yaml b/robotic_grounding/workflow/train.yaml deleted file mode 100644 index 750aac71..00000000 --- a/robotic_grounding/workflow/train.yaml +++ /dev/null @@ -1,50 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# SPDX-License-Identifier: Apache-2.0 - -workflow: - name: {{workflow_name}} - resources: - default: - cpu: 8 - gpu: 1 - memory: 100Gi - storage: 200Gi - - tasks: - - name: train - image: {{image}} - command: - - /bin/bash - args: - - /tmp/entry.sh - environment: - ACCEPT_EULA: Y - OMNI_SERVER: omniverse://isaac-dev.ov.nvidia.com - credentials: - wandb: - WANDB_API_KEY: wandb_api_key - files: - - path: /tmp/entry.sh - contents: |- - set -ex - - python scripts/rsl_rl/train.py --task Sharpa-V2P-v0 --headless \ - --motion_file arctic_processed/arctic_s01_rigid_capsulemachine_grab_01/sharpa_wave \ - --run_name v2d_arctic_s01_rigid_capsulemachine_grab_01 - -default-values: - workflow_name: robotic_grounding_train - image: nvcr.io/nvstaging/isaac-amr/robotic-grounding:latest \ No newline at end of file diff --git a/video_ingestion_agent/docs/release_readiness.md b/video_ingestion_agent/docs/release_readiness.md index 64533525..c4a435e0 100644 --- a/video_ingestion_agent/docs/release_readiness.md +++ b/video_ingestion_agent/docs/release_readiness.md @@ -283,7 +283,7 @@ not part of the blocker pass. reuse rather than adding subfolder duplicates. - **GitHub Actions workflow.** The monorepo already follows a `_pipeline_ci.yml` convention - (`reconstruction_pipeline_ci.yml`, `robotic_grounding_ci.yml`). + (`reconstruction_pipeline_ci.yml`). Add `video_ingestion_agent_pipeline_ci.yml` running pre-commit + Sphinx build, scoped to `video_ingestion_agent/**` paths. The current `.gitlab-ci.yml` inside the subfolder can be removed once