Full Code of HorizonRobotics/HoloMotion for AI

master 75ca12750541 cached
249 files
2.8 MB
741.1k tokens
2254 symbols
1 requests
Download .txt
Showing preview only (2,964K chars total). Download the full file or copy to clipboard to get everything.
Repository: HorizonRobotics/HoloMotion
Branch: master
Commit: 75ca12750541
Files: 249
Total size: 2.8 MB

Directory structure:
gitextract_3fyqn965/

├── .gitattributes
├── .githooks/
│   ├── README.md
│   └── pre-commit
├── .gitignore
├── .gitlab-ci.yml
├── .gitmodules
├── LICENSE
├── Makefile
├── NOTICE
├── README.md
├── assets/
│   ├── robots/
│   │   └── unitree/
│   │       └── G1/
│   │           └── 29dof/
│   │               ├── g1_29dof.xml
│   │               ├── g1_29dof_rev_1_0.urdf
│   │               ├── g1_29dof_rev_1_0.xml
│   │               ├── g1_29dof_rev_1_0_s100.urdf
│   │               └── scene_29dof.xml
│   └── test_data/
│       └── motion_retargeting/
│           └── ACCAD/
│               └── Male1Walking_c3d/
│                   └── Walk_B10_-_Walk_turn_left_45_stageii.npz
├── deploy.env
├── deployment/
│   ├── deploy_environment.sh
│   ├── holomotion_teleop/
│   │   ├── holomotion_teleop_node.py
│   │   ├── holomotion_teleop_setup.md
│   │   └── setup_holomotion_teleop_x86_ubuntu2204.sh
│   └── unitree_g1_ros2_29dof/
│       ├── launch_holomotion_29dof.sh
│       ├── launch_holomotion_29dof_docker.sh
│       ├── src/
│       │   ├── CMakeLists.txt
│       │   ├── config/
│       │   │   └── g1_29dof_holomotion.yaml
│       │   ├── humanoid_policy/
│       │   │   ├── __init__.py
│       │   │   ├── holomotion_fk_root_only.py
│       │   │   ├── obs_builder/
│       │   │   │   ├── __init__.py
│       │   │   │   └── obs_builder.py
│       │   │   ├── policy_node_29dof.py
│       │   │   └── utils/
│       │   │       ├── __init__.py
│       │   │       ├── command_helper.py
│       │   │       ├── maths.py
│       │   │       ├── motor_crc.py
│       │   │       ├── remote_controller_filter.py
│       │   │       ├── rotation_helper.py
│       │   │       └── rotations.py
│       │   ├── include/
│       │   │   └── common/
│       │   │       ├── motor_crc.h
│       │   │       ├── motor_crc_hg.h
│       │   │       ├── ros2_sport_client.h
│       │   │       └── wireless_controller.h
│       │   ├── launch/
│       │   │   └── holomotion_29dof_launch.py
│       │   ├── models/
│       │   │   └── .gitkeep
│       │   ├── motion_data/
│       │   │   └── .gitkeep
│       │   ├── package.xml
│       │   ├── resource/
│       │   │   └── humanoid_control
│       │   ├── setup.cfg
│       │   ├── setup.py
│       │   └── src/
│       │       ├── common/
│       │       │   ├── motor_crc.cpp
│       │       │   ├── motor_crc_hg.cpp
│       │       │   ├── ros2_sport_client.cpp
│       │       │   └── wireless_controller.cpp
│       │       └── main_node.cpp
│       └── start_container.sh
├── docs/
│   ├── environment_setup.md
│   ├── evaluate_motion_tracking.md
│   ├── holomotion_motion_file_spec.md
│   ├── motion_retargeting.md
│   ├── mujoco_sim2sim.md
│   ├── realworld_deployment.md
│   ├── smpl_data_curation.md
│   └── train_motion_tracking.md
├── environments/
│   ├── environment_deploy.yaml
│   ├── environment_train_isaaclab_cu118.yaml
│   ├── environment_train_isaaclab_cu128.yaml
│   ├── requirements_base.txt
│   ├── requirements_deploy.txt
│   ├── requirements_torch_cu118.txt
│   ├── requirements_torch_cu128.txt
│   └── requirements_torch_cu130.txt
├── holomotion/
│   ├── config/
│   │   ├── algo/
│   │   │   ├── ppo.yaml
│   │   │   └── ppo_tf.yaml
│   │   ├── data_curation/
│   │   │   ├── joints2smpl.yaml
│   │   │   └── smplify_base.yaml
│   │   ├── env/
│   │   │   ├── domain_randomization/
│   │   │   │   ├── NO_domain_rand.yaml
│   │   │   │   ├── domain_rand_medium.yaml
│   │   │   │   ├── domain_rand_small.yaml
│   │   │   │   └── domain_rand_strong.yaml
│   │   │   ├── motion_tracking.yaml
│   │   │   ├── observations/
│   │   │   │   ├── motion_tracking/
│   │   │   │   │   ├── obs_motion_tracking_mlp.yaml
│   │   │   │   │   └── obs_motion_tracking_tf-moe.yaml
│   │   │   │   └── velocity_tracking/
│   │   │   │       └── obs_velocity_tracking.yaml
│   │   │   ├── rewards/
│   │   │   │   ├── motion_tracking/
│   │   │   │   │   └── rew_motion_tracking.yaml
│   │   │   │   └── velocity_tracking/
│   │   │   │       └── rew_velocity_tracking.yaml
│   │   │   ├── terminations/
│   │   │   │   ├── NO_termination.yaml
│   │   │   │   ├── termination_motion_tracking.yaml
│   │   │   │   └── termination_velocity_tracking.yaml
│   │   │   ├── terrain/
│   │   │   │   ├── isaaclab_plane.yaml
│   │   │   │   └── isaaclab_rough.yaml
│   │   │   └── velocity_tracking.yaml
│   │   ├── evaluation/
│   │   │   ├── eval_isaaclab.yaml
│   │   │   ├── eval_mujoco_sim2sim.yaml
│   │   │   └── eval_velocity_tracking.yaml
│   │   ├── modules/
│   │   │   ├── motion_tracking/
│   │   │   │   ├── motion_tracking_mlp.yaml
│   │   │   │   └── motion_tracking_tf-moe.yaml
│   │   │   └── velocity_tracking/
│   │   │       └── velocity_tracking_mlp.yaml
│   │   ├── motion_retargeting/
│   │   │   ├── gmr_to_holomotion.yaml
│   │   │   ├── holomotion_preprocess.yaml
│   │   │   ├── kinematic_filter.yaml
│   │   │   ├── pack_hdf5_database.yaml
│   │   │   ├── pack_hdf5_v2.yaml
│   │   │   └── unitree_G1_29dof_retargeting.yaml
│   │   ├── mujoco_eval/
│   │   │   └── sim2sim.yaml
│   │   ├── robot/
│   │   │   └── unitree/
│   │   │       └── G1/
│   │   │           └── 29dof/
│   │   │               ├── 29dof_training_isaaclab.yaml
│   │   │               └── 29dof_training_isaaclab_s100.yaml
│   │   └── training/
│   │       ├── motion_tracking/
│   │       │   ├── train_g1_29dof_motion_tracking_mlp.yaml
│   │       │   └── train_g1_29dof_motion_tracking_tf-moe.yaml
│   │       ├── train_base.yaml
│   │       └── velocity_tracking/
│   │           └── train_g1_29dof_velocity_tracking_mlp.yaml
│   ├── scripts/
│   │   ├── data_curation/
│   │   │   ├── convert_to_amass.sh
│   │   │   ├── filter_smpl_data.sh
│   │   │   ├── video_to_smpl_gvhmr.sh
│   │   │   └── visualize_smpl_npz.sh
│   │   ├── evaluation/
│   │   │   ├── calc_offline_eval_metrics.sh
│   │   │   ├── eval_motion_tracking.sh
│   │   │   ├── eval_mujoco_sim2sim.sh
│   │   │   ├── eval_velocity_tracking.sh
│   │   │   ├── mean_process_5metrics.py
│   │   │   └── multi_model_metrics_analysis.sh
│   │   ├── motion_retargeting/
│   │   │   ├── apply_gmr_motion_retarget_patch.sh
│   │   │   ├── pack_hdf5_v2.sh
│   │   │   ├── run_holomotion_preprocessing.sh
│   │   │   ├── run_kinematic_filter.sh
│   │   │   ├── run_motion_retargeting_gmr_bvh.sh
│   │   │   ├── run_motion_retargeting_gmr_smplx.sh
│   │   │   ├── run_motion_retargeting_gmr_to_holomotion.sh
│   │   │   └── run_motion_viz_mujoco.sh
│   │   └── training/
│   │       ├── train_motion_tracking.sh
│   │       └── train_velocity_tracking.sh
│   ├── src/
│   │   ├── algo/
│   │   │   ├── __init__.py
│   │   │   ├── algo_base.py
│   │   │   ├── algo_utils.py
│   │   │   ├── ppo.py
│   │   │   └── ppo_tf.py
│   │   ├── data_curation/
│   │   │   ├── .gitignore
│   │   │   ├── __init__.py
│   │   │   ├── data_smplify.py
│   │   │   ├── filter/
│   │   │   │   ├── filter.py
│   │   │   │   └── label_data.py
│   │   │   ├── smpl_npz_to_html.py
│   │   │   ├── smplify/
│   │   │   │   ├── __init__.py
│   │   │   │   ├── smplify_humanact12.py
│   │   │   │   ├── smplify_motionx.py
│   │   │   │   ├── smplify_omomo.py
│   │   │   │   └── smplify_zjumocap.py
│   │   │   ├── templates/
│   │   │   │   └── index_wooden_static.html
│   │   │   ├── video_to_smpl_gvhmr.py
│   │   │   ├── vison_mocap/
│   │   │   │   └── joints2smpl.py
│   │   │   └── visualize_smpl_npz.py
│   │   ├── env/
│   │   │   ├── __init__.py
│   │   │   ├── isaaclab_components/
│   │   │   │   ├── __init__.py
│   │   │   │   ├── isaaclab_actions.py
│   │   │   │   ├── isaaclab_curriculum.py
│   │   │   │   ├── isaaclab_domain_rand.py
│   │   │   │   ├── isaaclab_motion_tracking_command.py
│   │   │   │   ├── isaaclab_observation.py
│   │   │   │   ├── isaaclab_rewards.py
│   │   │   │   ├── isaaclab_scene.py
│   │   │   │   ├── isaaclab_simulator.py
│   │   │   │   ├── isaaclab_termination.py
│   │   │   │   ├── isaaclab_terrain.py
│   │   │   │   ├── isaaclab_utils.py
│   │   │   │   ├── isaaclab_velocity_tracking_command.py
│   │   │   │   └── unitree_actuators.py
│   │   │   ├── motion_tracking.py
│   │   │   └── velocity_tracking.py
│   │   ├── evaluation/
│   │   │   ├── __init__.py
│   │   │   ├── eval_motion_tracking.py
│   │   │   ├── eval_motion_tracking_single.py
│   │   │   ├── eval_mujoco_sim2sim.py
│   │   │   ├── eval_velocity_tracking.py
│   │   │   ├── find_worst_clips.py
│   │   │   ├── metrics.py
│   │   │   ├── multi_model_metrics_report.py
│   │   │   ├── obs/
│   │   │   │   ├── __init__.py
│   │   │   │   └── obs_builder.py
│   │   │   ├── ray_evaluator_actor.py
│   │   │   └── ray_metrics_postprocess.py
│   │   ├── modules/
│   │   │   ├── __init__.py
│   │   │   ├── agent_modules.py
│   │   │   └── network_modules.py
│   │   ├── motion_retargeting/
│   │   │   ├── __init__.py
│   │   │   ├── gmr_to_holomotion.py
│   │   │   ├── holomotion_fk.py
│   │   │   ├── holomotion_preprocess.py
│   │   │   ├── kinematic_filter.py
│   │   │   ├── pack_hdf5_v2.py
│   │   │   ├── reference_filtering.py
│   │   │   └── utils/
│   │   │       ├── __init__.py
│   │   │       ├── _schema.json
│   │   │       ├── rotation_conversions.py
│   │   │       ├── torch_humanoid_batch.py
│   │   │       └── visualize_with_mujoco.py
│   │   ├── training/
│   │   │   ├── __init__.py
│   │   │   ├── h5_dataloader.py
│   │   │   ├── reference_filter_export.py
│   │   │   └── train.py
│   │   └── utils/
│   │       ├── __init__.py
│   │       ├── config.py
│   │       ├── frame_utils.py
│   │       ├── isaac_utils/
│   │       │   ├── __init__.py
│   │       │   ├── maths.py
│   │       │   ├── rotations.py
│   │       │   └── setup.py
│   │       ├── onnx_export.py
│   │       ├── reference_prefix.py
│   │       ├── torch_utils.py
│   │       └── unitree_g1_actuator_calculator.py
│   └── tests/
│       └── __init__.py
├── pyproject.toml
├── tests/
│   ├── benchmark_legacy_onnx_attention.py
│   ├── benchmark_moe_router_export.py
│   ├── test_actor_export_config.py
│   ├── test_algo_base_iteration_logging.py
│   ├── test_build_quantization_dataset.py
│   ├── test_cache_curriculum_sampler.py
│   ├── test_domain_rand_config_builder.py
│   ├── test_eval_mujoco_action_delay.py
│   ├── test_eval_mujoco_action_ema.py
│   ├── test_eval_mujoco_contact_export.py
│   ├── test_eval_mujoco_s100_horizon_ptq.py
│   ├── test_eval_mujoco_use_gpu.py
│   ├── test_eval_onnx_io_dump.py
│   ├── test_evaluation_metrics.py
│   ├── test_isaaclab_termination.py
│   ├── test_mean_process_5metrics.py
│   ├── test_motion_cache_gather_state.py
│   ├── test_motion_cache_startup.py
│   ├── test_motion_tracking_command_reference_prefix.py
│   ├── test_motion_tracking_timing.py
│   ├── test_mujoco_filtered_ref_compat.py
│   ├── test_obs_norm_compile.py
│   ├── test_observation_frames.py
│   ├── test_onnx_attention_export.py
│   ├── test_onnx_export.py
│   ├── test_plot_moe_expert_heatmap.py
│   ├── test_plot_state_series.py
│   ├── test_ppo_checkpoint_sigma_override.py
│   ├── test_ppo_entropy_annealing.py
│   ├── test_ppo_symmetry_loss.py
│   ├── test_ppo_tf_aux_keybody.py
│   ├── test_ref_router_actor.py
│   ├── test_ref_router_seq_actor.py
│   ├── test_reference_filter_export.py
│   ├── test_reference_motion_config_wiring.py
│   ├── test_root_rel_rewards.py
│   ├── test_unitree_actuators.py
│   └── test_visualize_with_mujoco.py
└── train.env

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitattributes
================================================
*.ipynb filter=lfs diff=lfs merge=lfs -text
*.pt filter=lfs diff=lfs merge=lfs -text
*.obj filter=lfs diff=lfs merge=lfs -text
*.dae filter=lfs diff=lfs merge=lfs -text
*.onnx filter=lfs diff=lfs merge=lfs -text
*.pkl filter=lfs diff=lfs merge=lfs -text
*.npz filter=lfs diff=lfs merge=lfs -text
*.npy filter=lfs diff=lfs merge=lfs -text
assets/smplx filter=lfs diff=lfs merge=lfs -text
assets/smpl filter=lfs diff=lfs merge=lfs -text
assets/test_data filter=lfs diff=lfs merge=lfs -text
assets/media/*.png filter=lfs diff=lfs merge=lfs -text
assets/media/*.jpg filter=lfs diff=lfs merge=lfs -text
assets/media/*.jpeg filter=lfs diff=lfs merge=lfs -text
assets/videos/*.mp4 filter=lfs diff=lfs merge=lfs -text
*.gif filter=lfs diff=lfs merge=lfs -text
*.svg filter=lfs diff=lfs merge=lfs -text
*.png filter=lfs diff=lfs merge=lfs -text


================================================
FILE: .githooks/README.md
================================================
# Git hooks

Pre-commit runs [ruff](https://docs.astral.sh/ruff/) format on staged Python files (using `train.env` for the correct environment).

**Install ruff** in the holomotion train environment if it is absent:

```bash
conda activate holomotion_train
pip install ruff
```

Ruff is also listed in `environments/requirements_base.txt` if you install deps from there.

**Enable hooks** (run once from repo root):

```bash
git config core.hooksPath .githooks
```

Ensure the hook is executable: `chmod +x .githooks/pre-commit`

**Requirement:** Run `git commit` from a shell where conda is available so `train.env` can set `Train_CONDA_PREFIX`.

**Skip hook for one commit:** `git commit --no-verify`


================================================
FILE: .githooks/pre-commit
================================================
#!/bin/bash
# Run ruff format on staged Python files before commit.
# Use from holomotion repo root (standalone clone or submodule).
# Requires: run git commit from a shell where conda is available (so train.env can set Train_CONDA_PREFIX).

set -e
cd "$(git rev-parse --show-toplevel)"

mapfile -t staged_py < <(git diff --cached --name-only --diff-filter=ACM | grep '\.py$' || true)
if [ ${#staged_py[@]} -eq 0 ]; then
  exit 0
fi

# train.env sets Train_CONDA_PREFIX; conda must be on PATH when you run git commit
source train.env
"$Train_CONDA_PREFIX/bin/ruff" format --config pyproject.toml "${staged_py[@]}"
git add "${staged_py[@]}"
exit 0


================================================
FILE: .gitignore
================================================
# ignore logs and cache
logs/
logs_eval/
data/
outputs/
.archive/
tmp/

# ignore deployment bag_record, install, log
deployment/unitree_g1_ros2/bag_record/
deployment/unitree_g1_ros2/install/
deployment/unitree_g1_ros2/log/

# ignore data and outputs
data
data/
outputs/
build/
install/
log/
.DS_Store/
**.egg-info/
**.log
**.LOG

# ignore large files
*.log
*.pkl
*.pt
*.onnx
*.npy
*.npz
*.zip
*.tar.gz

*.obj
*.dae
*.STL

# ignore video, image, etc.
*.mp4
*.avi
*.mov
*.png
*.pdf

__pycache__/
*.pyc
*.egg-info

.agents/
.cursor/
.cursorignore
.vscode/

.*_cache/

**/usd/
assets/isaac/

not_for_commit/

thirdparties/smpl_models/

MUJOCO_LOG.TXT

# allow certain files
!deployment/unitree_g1_ros2/src/models/*.onnx
!deployment/unitree_g1_ros2/src/motion_data/*.pkl
!assets/smpl/*.pkl
!assets/smpl/*.npz
!assets/smplx/*.pkl
!assets/smplx/*.npz
!assets/test_data/**
!assets/media/**

# macOS system files
.DS_Store
**/.DS_Store


================================================
FILE: .gitlab-ci.yml
================================================

workflow:
  rules:
    - if: $CI_PIPELINE_SOURCE == 'merge_request_event'

job1:
  script:
    - echo "This job runs in merge request pipelines"


================================================
FILE: .gitmodules
================================================
[submodule "thirdparties/SMPLSim"]
path = thirdparties/SMPLSim
url = https://github.com/ZhengyiLuo/SMPLSim
branch = master

[submodule "thirdparties/joints2smpl"]
path = thirdparties/joints2smpl
url = https://github.com/wangsen1312/joints2smpl.git
branch = main

[submodule "thirdparties/omomo_release"]
path = thirdparties/omomo_release
url = https://github.com/lijiaman/omomo_release.git
branch = main

[submodule "thirdparties/unitree_ros"]
path = thirdparties/unitree_ros
url = https://github.com/unitreerobotics/unitree_ros
branch = master

[submodule "thirdparties/unitree_ros2"]
path = thirdparties/unitree_ros2
url = https://github.com/unitreerobotics/unitree_ros2
branch = master

[submodule "thirdparties/unitree_sdk2_python"]
path = thirdparties/unitree_sdk2_python
url = https://github.com/unitreerobotics/unitree_sdk2_python.git

[submodule "thirdparties/cyclonedds"]
path = thirdparties/cyclonedds
url = https://github.com/eclipse-cyclonedds/cyclonedds
branch = 0.10.2

[submodule "thirdparties/unitree_sdk2"]
path = thirdparties/unitree_sdk2
url = https://github.com/unitreerobotics/unitree_sdk2.git

[submodule "thirdparties/GMR"]
path = thirdparties/GMR
url = https://github.com/YanjieZe/GMR.git
branch = master
[submodule "thirdparties/HoloMotion_assets"]
path = thirdparties/HoloMotion_assets
url = https://huggingface.co/datasets/HorizonRobotics/HoloMotion_assets
[submodule "thirdparties/smplx"]
	path = thirdparties/smplx
	url = https://github.com/vchoutas/smplx
[submodule "thirdparties/GVHMR"]
	path = thirdparties/GVHMR
	url = https://github.com/zju3dv/GVHMR.git


================================================
FILE: LICENSE
================================================
                                 Apache License
                           Version 2.0, January 2004
                        http://www.apache.org/licenses/

   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION

   1. Definitions.

      "License" shall mean the terms and conditions for use, reproduction,
      and distribution as defined by Sections 1 through 9 of this document.

      "Licensor" shall mean the copyright owner or entity authorized by
      the copyright owner that is granting the License.

      "Legal Entity" shall mean the union of the acting entity and all
      other entities that control, are controlled by, or are under common
      control with that entity. For the purposes of this definition,
      "control" means (i) the power, direct or indirect, to cause the
      direction or management of such entity, whether by contract or
      otherwise, or (ii) ownership of fifty percent (50%) or more of the
      outstanding shares, or (iii) beneficial ownership of such entity.

      "You" (or "Your") shall mean an individual or Legal Entity
      exercising permissions granted by this License.

      "Source" form shall mean the preferred form for making modifications,
      including but not limited to software source code, documentation
      source, and configuration files.

      "Object" form shall mean any form resulting from mechanical
      transformation or translation of a Source form, including but
      not limited to compiled object code, generated documentation,
      and conversions to other media types.

      "Work" shall mean the work of authorship, whether in Source or
      Object form, made available under the License, as indicated by a
      copyright notice that is included in or attached to the work
      (an example is provided in the Appendix below).

      "Derivative Works" shall mean any work, whether in Source or Object
      form, that is based on (or derived from) the Work and for which the
      editorial revisions, annotations, elaborations, or other modifications
      represent, as a whole, an original work of authorship. For the purposes
      of this License, Derivative Works shall not include works that remain
      separable from, or merely link (or bind by name) to the interfaces of,
      the Work and Derivative Works thereof.

      "Contribution" shall mean any work of authorship, including
      the original version of the Work and any modifications or additions
      to that Work or Derivative Works thereof, that is intentionally
      submitted to Licensor for inclusion in the Work by the copyright owner
      or by an individual or Legal Entity authorized to submit on behalf of
      the copyright owner. For the purposes of this definition, "submitted"
      means any form of electronic, verbal, or written communication sent
      to the Licensor or its representatives, including but not limited to
      communication on electronic mailing lists, source code control systems,
      and issue tracking systems that are managed by, or on behalf of, the
      Licensor for the purpose of discussing and improving the Work, but
      excluding communication that is conspicuously marked or otherwise
      designated in writing by the copyright owner as "Not a Contribution."

      "Contributor" shall mean Licensor and any individual or Legal Entity
      on behalf of whom a Contribution has been received by Licensor and
      subsequently incorporated within the Work.

   2. Grant of Copyright License. Subject to the terms and conditions of
      this License, each Contributor hereby grants to You a perpetual,
      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
      copyright license to reproduce, prepare Derivative Works of,
      publicly display, publicly perform, sublicense, and distribute the
      Work and such Derivative Works in Source or Object form.

   3. Grant of Patent License. Subject to the terms and conditions of
      this License, each Contributor hereby grants to You a perpetual,
      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
      (except as stated in this section) patent license to make, have made,
      use, offer to sell, sell, import, and otherwise transfer the Work,
      where such license applies only to those patent claims licensable
      by such Contributor that are necessarily infringed by their
      Contribution(s) alone or by combination of their Contribution(s)
      with the Work to which such Contribution(s) was submitted. If You
      institute patent litigation against any entity (including a
      cross-claim or counterclaim in a lawsuit) alleging that the Work
      or a Contribution incorporated within the Work constitutes direct
      or contributory patent infringement, then any patent licenses
      granted to You under this License for that Work shall terminate
      as of the date such litigation is filed.

   4. Redistribution. You may reproduce and distribute copies of the
      Work or Derivative Works thereof in any medium, with or without
      modifications, and in Source or Object form, provided that You
      meet the following conditions:

      (a) You must give any other recipients of the Work or
          Derivative Works a copy of this License; and

      (b) You must cause any modified files to carry prominent notices
          stating that You changed the files; and

      (c) You must retain, in the Source form of any Derivative Works
          that You distribute, all copyright, patent, trademark, and
          attribution notices from the Source form of the Work,
          excluding those notices that do not pertain to any part of
          the Derivative Works; and

      (d) If the Work includes a "NOTICE" text file as part of its
          distribution, then any Derivative Works that You distribute must
          include a readable copy of the attribution notices contained
          within such NOTICE file, excluding those notices that do not
          pertain to any part of the Derivative Works, in at least one
          of the following places: within a NOTICE text file distributed
          as part of the Derivative Works; within the Source form or
          documentation, if provided along with the Derivative Works; or,
          within a display generated by the Derivative Works, if and
          wherever such third-party notices normally appear. The contents
          of the NOTICE file are for informational purposes only and
          do not modify the License. You may add Your own attribution
          notices within Derivative Works that You distribute, alongside
          or as an addendum to the NOTICE text from the Work, provided
          that such additional attribution notices cannot be construed
          as modifying the License.

      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 such Derivative Works as a whole, provided Your use,
      reproduction, and distribution of the Work otherwise complies with
      the conditions stated in this License.

   5. Submission of Contributions. Unless You explicitly state otherwise,
      any Contribution intentionally submitted for inclusion in the Work
      by You to the Licensor shall be under the terms and conditions of
      this License, without any additional terms or conditions.
      Notwithstanding the above, nothing herein shall supersede or modify
      the terms of any separate license agreement you may have executed
      with Licensor regarding such Contributions.

   6. Trademarks. This License does not grant permission to use the trade
      names, trademarks, service marks, or product names of the Licensor,
      except as required for reasonable and customary use in describing the
      origin of the Work and reproducing the content of the NOTICE file.

   7. Disclaimer of Warranty. Unless required by applicable law or
      agreed to in writing, Licensor provides the Work (and each
      Contributor provides its Contributions) 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 determining the
      appropriateness of using or redistributing the Work and assume any
      risks associated with Your exercise of permissions under this License.

   8. 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, shall any Contributor be
      liable to You for damages, including any direct, indirect, special,
      incidental, or consequential damages of any character arising as a
      result of this License or out of the use or inability to use the
      Work (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 such Contributor
      has been advised of the possibility of such damages.

   9. Accepting Warranty or Additional Liability. While redistributing
      the Work or Derivative Works thereof, You may choose to offer,
      and charge a fee for, acceptance of support, warranty, indemnity,
      or other liability obligations and/or rights consistent with this
      License. However, in accepting such obligations, You may act only
      on Your own behalf and on Your sole responsibility, not on behalf
      of any other Contributor, and only if You agree to indemnify,
      defend, and hold each Contributor harmless for any liability
      incurred by, or claims asserted against, such Contributor by reason
      of your accepting any such warranty or additional liability.

   END OF TERMS AND CONDITIONS

   APPENDIX: How to apply the Apache License to your work.

      To apply the Apache License to your work, attach the following
      boilerplate notice, with the fields enclosed by brackets "[]"
      replaced with your own identifying information. (Don't include
      the brackets!)  The text should be enclosed in the appropriate
      comment syntax for the file format. We also recommend that a
      file or class name and description of purpose be included on the
      same "printed page" as the copyright notice for easier
      identification within third-party archives.

   Copyright 2025 maiyue01.chen

   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.


================================================
FILE: Makefile
================================================
# Variables
PY_SRC := holomotion/  # Your Python code directory
RUFF := ruff  # Assumes Ruff is installed locally
PYTEST := pytest -v
TESTS := holomotion/tests/
COV := --cov=holomotion/src --cov-report=term-missing

# Directory to lint/format - can be overridden with DIR=path
DIR ?= holomotion/src

.PHONY: lint format check lint-dir format-dir

# Run Ruff linter on default directory
lint:
	@echo "Linting with Ruff..."
	@$(RUFF) check $(PY_SRC)

# Format code in default directory
format:
	@echo "Formatting with Ruff..."
	@$(RUFF) format $(PY_SRC)
	@$(RUFF) check --fix $(PY_SRC)  # Auto-fix lint errors

# Run Ruff linter on specific directory (with fallback)
lint-dir:
	@echo "Linting directory: $(DIR)"
	@$(RUFF) check $(DIR)

# Format code in specific directory (with fallback)
format-dir:
	@echo "Formatting directory: $(DIR)"
	@$(RUFF) format $(DIR)
	@$(RUFF) check --fix $(DIR)  # Auto-fix lint errors

# Strict check (for CI)
check:
	@$(RUFF) check $(PY_SRC) --exit-non-zero-on-fix

# Run all tests
test:
	$(PYTEST) $(TESTS)

================================================
FILE: NOTICE
================================================


=======================================================================
ASAP's MIT License
=======================================================================
Code derived from implementations in ASAP should mention its derivation
and reference the following license:

    MIT License

    Copyright (c) 2025 ASAP Team

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.



=======================================================================
omomo_release's MIT License
=======================================================================
Code derived from implementations in omomo_release should mention its derivation
and reference the following license:

   MIT License

   Copyright (c) 2023 Jiaman Li

   Permission is hereby granted, free of charge, to any person obtaining a copy
   of this software and associated documentation files (the "Software"), to deal
   in the Software without restriction, including without limitation the rights
   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   copies of the Software, and to permit persons to whom the Software is
   furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included in all
   copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   SOFTWARE.

=======================================================================
NVIDIA License
=======================================================================

   Copyright (c) 2020, 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.

================================================
FILE: README.md
================================================
<div align="center">

<img src="assets/media/holomotion_logo_text.png" alt="HoloMotion Logo" width="500"/>

---

[![Python](https://img.shields.io/badge/Python3.11-3776AB?logo=python&logoColor=fff)](#)
[![Ubuntu](https://img.shields.io/badge/Ubuntu22.04-E95420?logo=ubuntu&logoColor=white)](#)
[![License](https://img.shields.io/badge/License-Apache_2.0-green?logo=apache&logoColor=white)](./LICENSE)

[![Safari](https://img.shields.io/badge/Website-006CFF?logo=safari&logoColor=fff)](https://horizonrobotics.github.io/robot_lab/holomotion/)
[![HuggingFace](https://img.shields.io/badge/-HuggingFace-3B4252?style=flat&logo=huggingface&logoColor=)](https://huggingface.co/collections/HorizonRobotics/holomotion)
[![Ask DeepWiki](https://deepwiki.com/badge.svg)](https://deepwiki.com/HorizonRobotics/HoloMotion)
[![WeChat](https://img.shields.io/badge/Wechat-7BB32E?logo=wechat&logoColor=white)](https://horizonrobotics.feishu.cn/docx/Xs3cdEI8bo1EZuxUfzjckTgKn2c)

<!-- [![arXiv](https://img.shields.io/badge/arXiv-2025.00000-red?logo=arxiv&logoColor=white)](https://arxiv.org/abs/2025.00000) -->
<!-- [![arXiv](https://img.shields.io/badge/arXiv-2025.00000-red?logo=arxiv&logoColor=white)](https://arxiv.org/abs/2025.00000) -->

</div>

# HoloMotion: A Foundation Model for Whole-Body Humanoid Control

## NEWS
- [2026.04.04] The v1.2 version of HoloMotion has been released, we provide pre-trained motion tracking and velocity tracking models for the community to deploy directly.

- [2026.01.06] The v1.1 version of HoloMotion has been released, representing a major step forward toward a fully engineered, stable, and reproducible humanoid motion intelligence system.

- [2025.11.05] The v1.0 version of HoloMotion has been released, and the WeChat user group is now open! Please scan the [QR Code](https://horizonrobotics.feishu.cn/docx/Xs3cdEI8bo1EZuxUfzjckTgKn2c) to join.
<!-- <p align="center">
  <img width="150" height="230" src="assets/media/wechat_group_20251125.jpg" hspace="10">
</p> -->

## Pre-trained Models
- Motion Tracking Model: [Hugging Face](https://huggingface.co/HorizonRobotics/HoloMotion_v1.2/tree/main/holomotion_v1.2_motion_tracking_model)
- Velocity Tracking Model: [Hugging Face](https://huggingface.co/HorizonRobotics/HoloMotion_v1.2/tree/main/holomotion_v1.2_velocity_tracking_model)

Please read the doc for  [real-world deployment](docs/realworld_deployment.md) for more details on how to use the models. 

## Introduction

HoloMotion is a foundation model for humanoid robotics, designed to fullfill robust, real-time, and generalizable whole-body control.

Our framework provides an end-to-end solution, encompassing the entire workflow from data curation and motion retargeting to distributed model training, evaluation, and seamless deployment on physical hardware via ROS2. HoloMotion's modular architecture allows for flexible adaptation and extension, enabling researchers and developers to build and benchmark agents that can imitate, generalize, and master complex whole-body motions.

For those at the forefront of creating the next generation of humanoid robots, HoloMotion serves as a powerful, extensible, and open-source foundation for achieving whole-body control.

---

### 🛠️ Roadmap: Progress Toward Any Humanoid Control

We envision HoloMotion as a general-purpose foundation for humanoid motion and control. Its development is structured around four core generalization goals: Any Pose, Any Command, Any Terrain, and Any Embodiment. Each goal corresponds to a major version milestone.

| Version  | Target Capability | Description                                                                                                                         |
| -------- | ----------------- | ----------------------------------------------------------------------------------------------------------------------------------- |
| **v1.0** | 🔄 Any Pose       | Achieve robust tracking and imitation of diverse, whole-body human motions, forming the core of the imitation learning capability.  |
| **v2.0** | ⏳ Any Command    | Enable language- and task-conditioned motion generation, allowing for goal-directed and interactive behaviors.                      |
| **v3.0** | ⏳ Any Terrain    | Master adaptation to uneven, dynamic, and complex terrains, enhancing real-world operational robustness.                            |
| **v4.0** | ⏳ Any Embodiment | Generalize control policies across humanoids with varying morphologies and kinematics, achieving true embodiment-level abstraction. |

> Each stage builds on the previous one, moving from motion imitation to instruction following, terrain adaptation, and embodiment-level generalization.

## Pipeline Overview

```mermaid
flowchart LR
    A["🔧 1. Environment Setup<br/>Dependencies & conda"]

    subgraph dataFrame ["DATA"]
        B["📊 2. Dataset Preparation<br/>Download & curate"]
        C["🔄 3. Motion Retargeting<br/>Human to robot motion"]
        B --> C
    end

    subgraph modelFrame ["TRAIN & EVAL"]
        D["🧠 4. Model Training<br/>Train with HoloMotion"]
        E["📈 5. Evaluation<br/>Test & export"]
        D --> E
    end

    F["🚀 6. Deployment<br/>Deploy to robots"]

    A --> dataFrame
    dataFrame --> modelFrame
    modelFrame --> F

    classDef subgraphStyle fill:#f9f9f9,stroke:#333,stroke-width:2px,stroke-dasharray:5 5,rx:10,ry:10,font-size:16px,font-weight:bold
    classDef nodeStyle fill:#e1f5fe,stroke:#0277bd,stroke-width:2px,rx:10,ry:10

    class dataFrame,modelFrame subgraphStyle
    class A,B,C,D,E,F nodeStyle
```

## Quick Start

### 🔧 1. Environment Setup [[Doc](docs/environment_setup.md)]

Set up your development and deployment environments using Conda. This initial step ensures all dependencies are correctly configured for both training and real-world execution.

If you only intend to use our pretrained models, you can skip the training environment setup and proceed directly to configure the deployment environment. See the [real-world deployment documentation](docs/realworld_deployment.md) for details.

### 📊 2. Dataset Preparation [[Doc](docs/smpl_data_curation.md)]

Acquire and process large-scale motion datasets. Our tools help you curate high-quality data by converting it to the AMASS-compatible smpl format and filtering out anomalies using kinematic metrics.

### 🔄 3. Motion Retargeting [[Doc](docs/motion_retargeting.md)]

Translate human motion data into robot-specific kinematic data. Our pipeline leverages [GMR](https://github.com/YanjieZe/GMR) to map human movements onto your robot's morphology, producing optimized HDF5 datasets ready for high-speed, distributed training.

### 🧠 4. Model Training [[Doc](docs/train_motion_tracking.md)]

Train your foundation model using our reinforcement learning framework. HoloMotion supports versatile training tasks, including motion tracking and velocity tracking.

### 📈 5. Evaluation [[Doc](docs/evaluate_motion_tracking.md)]

Evaluate your trained policies in IsaacLab. Visualize performance, and export trained models in ONNX format for seamless deployment.

### 🚀 6. Real-world Deployment [[Doc](docs/realworld_deployment.md)]

Our ROS2 package facilitates the deployment of the exported ONNX models, enabling real-time control on hardware like the Unitree G1.

## Join Us

We are hiring full-time engineers, new graduates, and interns who are excited about humanoid robots, motion control, and embodied intelligence.
Send your resume by scanning the **WeChat** QR code below to get in touch with us.

<p align="center">
  <img width="420" height="150" src="assets/media/qr_codes.png" hspace="10">
</p>

## Citation

```
@software{HoloMotion,
  author = {Maiyue Chen, Kaihui Wang, Bo Zhang, Yi Ren, Zihao Zhu, Xihan Ma, Qijun Huang, Zhiyuan Yang, Yucheng Wang, Zhizhong Su},
  title = {HoloMotion: A Foundation Model for Whole-Body Humanoid Control},
  year = {2026},
  month = April,
  version = {1.2.0},
  url = {https://github.com/HorizonRobotics/HoloMotion},
  license = {Apache-2.0}
}
```

## License

This project is released under the **[Apache 2.0](https://img.shields.io/badge/license-Apache--2.0-blue.svg)** license.

## Acknowledgements

This project is built upon and inspired by several outstanding open source projects:

- [GMR](https://github.com/YanjieZe/GMR)
- [BeyondMimic](https://github.com/HybridRobotics/whole_body_tracking/tree/dcecabd8c24c68f59d143fdf8e3a670f420c972d)
- [ASAP](https://github.com/LeCAR-Lab/ASAP)
- [Humanoidverse](https://github.com/LeCAR-Lab/HumanoidVerse)
- [PHC](https://github.com/ZhengyiLuo/PHC?tab=readme-ov-file)
- [ProtoMotion](https://github.com/NVlabs/ProtoMotions/tree/main/protomotions)
- [Mink](https://github.com/kevinzakka/mink)
- [PBHC](https://github.com/TeleHuman/PBHC)


================================================
FILE: assets/robots/unitree/G1/29dof/g1_29dof.xml
================================================
<mujoco model="g1_29dof">
  <compiler angle="radian" meshdir="../meshes" />

  <default>
    <default class="torso_motor">
      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
    </default>
    <default class="leg_motor">
      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
    </default>
    <default class="ankle_motor">
      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
    </default>
    <default class="arm_motor">
      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
    </default>
    <default class="wrist_motor">
      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
    </default>
    
  </default>

  <asset>
    <mesh name="pelvis" file="pelvis.STL" />
    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL" />
    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL" />
    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL" />
    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL" />
    <mesh name="left_knee_link" file="left_knee_link.STL" />
    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL" />
    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL" />
    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL" />
    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL" />
    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL" />
    <mesh name="right_knee_link" file="right_knee_link.STL" />
    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL" />
    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL" />
    <mesh name="waist_yaw_link" file="waist_yaw_link.STL" />
    <mesh name="waist_roll_link" file="waist_roll_link.STL" />
    <mesh name="torso_link" file="torso_link.STL" />
    <mesh name="logo_link" file="logo_link.STL" />
    <mesh name="head_link" file="head_link.STL" />
    <mesh name="waist_support_link" file="waist_support_link.STL" />
    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL" />
    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL" />
    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL" />
    <mesh name="left_elbow_link" file="left_elbow_link.STL" />
    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL" />
    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL" />
    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL" />
    <mesh name="left_rubber_hand" file="left_rubber_hand.STL" />
    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL" />
    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL" />
    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL" />
    <mesh name="right_elbow_link" file="right_elbow_link.STL" />
    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL" />
    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL" />
    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL" />
    <mesh name="right_rubber_hand" file="right_rubber_hand.STL" />
  </asset>

  <worldbody>
    <body name="pelvis" pos="0 0 0.793">
      <site name="imu" size="0.01" pos="0.0 0.0 0.0" />
      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813"
        diaginertia="0.010549 0.0093089 0.0079184" />
      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false" />
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
        mesh="pelvis" />
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
        mesh="pelvis_contour_link" />
      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link" />
      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122"
          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
          actuatorfrcrange="-88 88" class="leg_motor" />
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
          mesh="left_hip_pitch_link" />
        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link" />
        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
          <inertial pos="0.029812 -0.001045 -0.087934"
            quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
            diaginertia="0.00254986 0.00241169 0.00148755" />
          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671"
            actuatorfrcrange="-88 88" class="leg_motor" />
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
            mesh="left_hip_roll_link" />
          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link" />
          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181"
              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
              actuatorfrcrange="-88 88" class="leg_motor" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
              mesh="left_hip_yaw_link" />
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link" />
            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734"
              quat="0.996179 0 0.0873386 0">
              <inertial pos="0.005457 0.003964 -0.12074"
                quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
                diaginertia="0.0113804 0.0112778 0.00146458" />
              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
                actuatorfrcrange="-139 139" class="leg_motor" />
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
                  range="-0.87267 0.5236" actuatorfrcrange="-50 50"  class="ankle_motor"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                  rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
                  <inertial pos="0.026505 0 -0.016425"
                    quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
                    diaginertia="0.00167218 0.0016161 0.000217621" />
                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                    rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link" />
                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122"
          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
          actuatorfrcrange="-88 88" class="leg_motor" />
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
          mesh="right_hip_pitch_link" />
        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link" />
        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793"
            mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755" />
          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236"
            actuatorfrcrange="-88 88" class="leg_motor" />
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
            mesh="right_hip_roll_link" />
          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link" />
          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598"
              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
              actuatorfrcrange="-88 88" class="leg_motor" />
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
              mesh="right_hip_yaw_link" />
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link" />
            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734"
              quat="0.996179 0 0.0873386 0">
              <inertial pos="0.005457 -0.003964 -0.12074"
                quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
                diaginertia="0.011374 0.0112843 0.00146452" />
              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
                actuatorfrcrange="-139 139" class="leg_motor" />
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                  rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
                  <inertial pos="0.026505 0 -0.016425"
                    quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
                    diaginertia="0.00167218 0.0016161 0.000217621" />
                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                    rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link" />
                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
      <body name="waist_yaw_link">
        <inertial pos="0.003964 0 0.018769" quat="-0.0178291 0.628464 0.0282471 0.777121"
          mass="0.244" diaginertia="0.000158561 0.000124229 9.67669e-05" />
        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618"
          actuatorfrcrange="-88 88" class="torso_motor"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
          mesh="waist_yaw_link" />
        <body name="waist_roll_link" pos="-0.0039635 0 0.035">
          <inertial pos="0 -0.000236 0.010111" quat="0.99979 0.020492 0 0" mass="0.047"
            diaginertia="7.515e-06 6.40206e-06 3.98394e-06" />
          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52"
            actuatorfrcrange="-50 50" class="torso_motor"/>
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
            mesh="waist_roll_link" />
          <body name="torso_link" pos="0 0 0.019">
            <inertial pos="0.00331658 0.000261533 0.179856"
              quat="0.999831 0.000376204 0.0179895 -0.00377704" mass="9.598"
              diaginertia="0.12407 0.111951 0.0325382" />
            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52"
              actuatorfrcrange="-50 50" class="torso_motor"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
              mesh="torso_link" />
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link" />
            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
              group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link" />
            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1"
              mesh="logo_link" />
            <geom pos="0.0039635 0 -0.054" type="mesh" contype="0" conaffinity="0" group="1"
              density="0" rgba="0.2 0.2 0.2 1" mesh="head_link" />
            <geom pos="0.0039635 0 -0.054" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link" />
            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
              group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_support_link" />
            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
              mesh="waist_support_link" />
            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.23778"
              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225"
                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link" />
              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0"
                type="cylinder" rgba="0.7 0.7 0.7 1" />
              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831"
                quat="0.990268 -0.139172 0 0">
                <inertial pos="-0.000227 0.00727 -0.063243"
                  quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
                  diaginertia="0.000691311 0.000618011 0.000388977" />
                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
                  range="-1.5882 2.2515" actuatorfrcrange="-25 25" class="arm_motor" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                  rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link" />
                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder"
                  rgba="0.7 0.7 0.7 1" />
                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
                  <inertial pos="0.010773 -0.002949 -0.072009"
                    quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734"
                    diaginertia="0.00106187 0.00103217 0.000400661" />
                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                    rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
                    <inertial pos="0.064956 0.004454 -0.010062"
                      quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
                      diaginertia="0.000443035 0.000421612 0.000259353" />
                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
                      actuatorfrcrange="-25 25" class="arm_motor" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                      rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
                      <inertial pos="0.0171394 0.000537591 4.8864e-07"
                        quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445"
                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                        rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
                        <inertial pos="0.0229999 -0.00111685 -0.00111658"
                          quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405"
                          diaginertia="0.000430353 0.000429873 0.000164648" />
                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                          rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
                          <inertial pos="0.0708244 0.000191745 0.00161742"
                            quat="0.510571 0.526295 0.468078 0.493188" mass="0.254576"
                            diaginertia="0.000646113 0.000559993 0.000147566" />
                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                            rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" contype="0"
                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
                            mesh="left_rubber_hand" />
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.23778"
              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152"
                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link" />
              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0"
                type="cylinder" rgba="0.7 0.7 0.7 1" />
              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831"
                quat="0.990268 0.139172 0 0">
                <inertial pos="-0.000227 -0.00727 -0.063243"
                  quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643"
                  diaginertia="0.000691311 0.000618011 0.000388977" />
                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
                  range="-2.2515 1.5882" actuatorfrcrange="-25 25" class="arm_motor" />
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                  rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link" />
                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder"
                  rgba="0.7 0.7 0.7 1" />
                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
                  <inertial pos="0.010773 0.002949 -0.072009"
                    quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734"
                    diaginertia="0.00106187 0.00103217 0.000400661" />
                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                    rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
                    <inertial pos="0.064956 -0.004454 -0.010062"
                      quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
                      diaginertia="0.000443035 0.000421612 0.000259353" />
                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
                      actuatorfrcrange="-25 25" class="arm_motor" />
                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                      rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
                      <inertial pos="0.0171394 -0.000537591 4.8864e-07"
                        quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445"
                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                        rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
                        <inertial pos="0.0229999 0.00111685 -0.00111658"
                          quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405"
                          diaginertia="0.000430353 0.000429873 0.000164648" />
                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                          rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
                          <inertial pos="0.0708244 -0.000191745 0.00161742"
                            quat="0.493188 0.468078 0.526295 0.510571" mass="0.254576"
                            diaginertia="0.000646113 0.000559993 0.000147566" />
                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
                            rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh" contype="0"
                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
                            mesh="right_rubber_hand" />
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <motor name="left_hip_pitch" joint="left_hip_pitch_joint" ctrlrange="-88 88" />
    <motor name="left_hip_roll" joint="left_hip_roll_joint" ctrlrange="-88 88" />
    <motor name="left_hip_yaw" joint="left_hip_yaw_joint" ctrlrange="-88 88" />
    <motor name="left_knee" joint="left_knee_joint" ctrlrange="-139 139" />
    <motor name="left_ankle_pitch" joint="left_ankle_pitch_joint" ctrlrange="-50 50" />
    <motor name="left_ankle_roll" joint="left_ankle_roll_joint" ctrlrange="-50 50" />

    <motor name="right_hip_pitch" joint="right_hip_pitch_joint" ctrlrange="-88 88" />
    <motor name="right_hip_roll" joint="right_hip_roll_joint" ctrlrange="-88 88" />
    <motor name="right_hip_yaw" joint="right_hip_yaw_joint" ctrlrange="-88 88" />
    <motor name="right_knee" joint="right_knee_joint" ctrlrange="-139 139" />
    <motor name="right_ankle_pitch" joint="right_ankle_pitch_joint" ctrlrange="-50 50" />
    <motor name="right_ankle_roll" joint="right_ankle_roll_joint" ctrlrange="-50 50" />

    <motor name="waist_yaw" joint="waist_yaw_joint" ctrlrange="-88 88" />
    <motor name="waist_roll" joint="waist_roll_joint" ctrlrange="-50 50" />
    <motor name="waist_pitch" joint="waist_pitch_joint" ctrlrange="-50 50" />

    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch_joint" ctrlrange="-25 25" />
    <motor name="left_shoulder_roll" joint="left_shoulder_roll_joint" ctrlrange="-25 25" />
    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw_joint" ctrlrange="-25 25" />
    <motor name="left_elbow" joint="left_elbow_joint" ctrlrange="-25 25" />
    <motor name="left_wrist_roll" joint="left_wrist_roll_joint" ctrlrange="-25 25" />
    <motor name="left_wrist_pitch" joint="left_wrist_pitch_joint" ctrlrange="-5 5" />
    <motor name="left_wrist_yaw" joint="left_wrist_yaw_joint" ctrlrange="-5 5" />

    <motor name="right_shoulder_pitch" joint="right_shoulder_pitch_joint" ctrlrange="-25 25" />
    <motor name="right_shoulder_roll" joint="right_shoulder_roll_joint" ctrlrange="-25 25" />
    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw_joint" ctrlrange="-25 25" />
    <motor name="right_elbow" joint="right_elbow_joint" ctrlrange="-25 25" />
    <motor name="right_wrist_roll" joint="right_wrist_roll_joint" ctrlrange="-25 25" />
    <motor name="right_wrist_pitch" joint="right_wrist_pitch_joint" ctrlrange="-5 5" />
    <motor name="right_wrist_yaw" joint="right_wrist_yaw_joint" ctrlrange="-5 5" />
  </actuator>

  <sensor>
    <jointpos name="left_hip_pitch_pos" joint="left_hip_pitch_joint" />
    <jointpos name="left_hip_roll_pos" joint="left_hip_roll_joint" />
    <jointpos name="left_hip_yaw_pos" joint="left_hip_yaw_joint" />
    <jointpos name="left_knee_pos" joint="left_knee_joint" />
    <jointpos name="left_ankle_pitch_pos" joint="left_ankle_pitch_joint" />
    <jointpos name="left_ankle_roll_pos" joint="left_ankle_roll_joint" />
    <jointpos name="right_hip_pitch_pos" joint="right_hip_pitch_joint" />
    <jointpos name="right_hip_roll_pos" joint="right_hip_roll_joint" />
    <jointpos name="right_hip_yaw_pos" joint="right_hip_yaw_joint" />
    <jointpos name="right_knee_pos" joint="right_knee_joint" />
    <jointpos name="right_ankle_pitch_pos" joint="right_ankle_pitch_joint" />
    <jointpos name="right_ankle_roll_pos" joint="right_ankle_roll_joint" />
    <jointpos name="waist_yaw_pos" joint="waist_yaw_joint" />
    <jointpos name="waist_roll_pos" joint="waist_roll_joint" />
    <jointpos name="waist_pitch_pos" joint="waist_pitch_joint" />
    <jointpos name="left_shoulder_pitch_pos" joint="left_shoulder_pitch_joint" />
    <jointpos name="left_shoulder_roll_pos" joint="left_shoulder_roll_joint" />
    <jointpos name="left_shoulder_yaw_pos" joint="left_shoulder_yaw_joint" />
    <jointpos name="left_elbow_pos" joint="left_elbow_joint" />
    <jointpos name="left_wrist_roll_pos" joint="left_wrist_roll_joint" />
    <jointpos name="left_wrist_pitch_pos" joint="left_wrist_pitch_joint" />
    <jointpos name="left_wrist_yaw_pos" joint="left_wrist_yaw_joint" />
    <jointpos name="right_shoulder_pitch_pos" joint="right_shoulder_pitch_joint" />
    <jointpos name="right_shoulder_roll_pos" joint="right_shoulder_roll_joint" />
    <jointpos name="right_shoulder_yaw_pos" joint="right_shoulder_yaw_joint" />
    <jointpos name="right_elbow_pos" joint="right_elbow_joint" />
    <jointpos name="right_wrist_roll_pos" joint="right_wrist_roll_joint" />
    <jointpos name="right_wrist_pitch_pos" joint="right_wrist_pitch_joint" />
    <jointpos name="right_wrist_yaw_pos" joint="right_wrist_yaw_joint" />

    <jointvel name="left_hip_pitch_vel" joint="left_hip_pitch_joint" />
    <jointvel name="left_hip_roll_vel" joint="left_hip_roll_joint" />
    <jointvel name="left_hip_yaw_vel" joint="left_hip_yaw_joint" />
    <jointvel name="left_knee_vel" joint="left_knee_joint" />
    <jointvel name="left_ankle_pitch_vel" joint="left_ankle_pitch_joint" />
    <jointvel name="left_ankle_roll_vel" joint="left_ankle_roll_joint" />
    <jointvel name="right_hip_pitch_vel" joint="right_hip_pitch_joint" />
    <jointvel name="right_hip_roll_vel" joint="right_hip_roll_joint" />
    <jointvel name="right_hip_yaw_vel" joint="right_hip_yaw_joint" />
    <jointvel name="right_knee_vel" joint="right_knee_joint" />
    <jointvel name="right_ankle_pitch_vel" joint="right_ankle_pitch_joint" />
    <jointvel name="right_ankle_roll_vel" joint="right_ankle_roll_joint" />
    <jointvel name="waist_yaw_vel" joint="waist_yaw_joint" />
    <jointvel name="waist_roll_vel" joint="waist_roll_joint" />
    <jointvel name="waist_pitch_vel" joint="waist_pitch_joint" />
    <jointvel name="left_shoulder_pitch_vel" joint="left_shoulder_pitch_joint" />
    <jointvel name="left_shoulder_roll_vel" joint="left_shoulder_roll_joint" />
    <jointvel name="left_shoulder_yaw_vel" joint="left_shoulder_yaw_joint" />
    <jointvel name="left_elbow_vel" joint="left_elbow_joint" />
    <jointvel name="left_wrist_roll_vel" joint="left_wrist_roll_joint" />
    <jointvel name="left_wrist_pitch_vel" joint="left_wrist_pitch_joint" />
    <jointvel name="left_wrist_yaw_vel" joint="left_wrist_yaw_joint" />
    <jointvel name="right_shoulder_pitch_vel" joint="right_shoulder_pitch_joint" />
    <jointvel name="right_shoulder_roll_vel" joint="right_shoulder_roll_joint" />
    <jointvel name="right_shoulder_yaw_vel" joint="right_shoulder_yaw_joint" />
    <jointvel name="right_elbow_vel" joint="right_elbow_joint" />
    <jointvel name="right_wrist_roll_vel" joint="right_wrist_roll_joint" />
    <jointvel name="right_wrist_pitch_vel" joint="right_wrist_pitch_joint" />
    <jointvel name="right_wrist_yaw_vel" joint="right_wrist_yaw_joint" />

    <jointactuatorfrc name="left_hip_pitch_torque" joint="left_hip_pitch_joint" />
    <jointactuatorfrc name="left_hip_roll_torque" joint="left_hip_roll_joint" />
    <jointactuatorfrc name="left_hip_yaw_torque" joint="left_hip_yaw_joint" />
    <jointactuatorfrc name="left_knee_torque" joint="left_knee_joint" />
    <jointactuatorfrc name="left_ankle_pitch_torque" joint="left_ankle_pitch_joint" />
    <jointactuatorfrc name="left_ankle_roll_torque" joint="left_ankle_roll_joint" />
    <jointactuatorfrc name="right_hip_pitch_torque" joint="right_hip_pitch_joint" />
    <jointactuatorfrc name="right_hip_roll_torque" joint="right_hip_roll_joint" />
    <jointactuatorfrc name="right_hip_yaw_torque" joint="right_hip_yaw_joint" />
    <jointactuatorfrc name="right_knee_torque" joint="right_knee_joint" />
    <jointactuatorfrc name="right_ankle_pitch_torque" joint="right_ankle_pitch_joint" />
    <jointactuatorfrc name="right_ankle_roll_torque" joint="right_ankle_roll_joint" />
    <jointactuatorfrc name="waist_yaw_torque" joint="waist_yaw_joint" />
    <jointactuatorfrc name="waist_roll_torque" joint="waist_roll_joint" />
    <jointactuatorfrc name="waist_pitch_torque" joint="waist_pitch_joint" />
    <jointactuatorfrc name="left_shoulder_pitch_torque" joint="left_shoulder_pitch_joint" />
    <jointactuatorfrc name="left_shoulder_roll_torque" joint="left_shoulder_roll_joint" />
    <jointactuatorfrc name="left_shoulder_yaw_torque" joint="left_shoulder_yaw_joint" />
    <jointactuatorfrc name="left_elbow_torque" joint="left_elbow_joint" />
    <jointactuatorfrc name="left_wrist_roll_torque" joint="left_wrist_roll_joint" />
    <jointactuatorfrc name="left_wrist_pitch_torque" joint="left_wrist_pitch_joint" />
    <jointactuatorfrc name="left_wrist_yaw_torque" joint="left_wrist_yaw_joint" />
    <jointactuatorfrc name="right_shoulder_pitch_torque" joint="right_shoulder_pitch_joint" />
    <jointactuatorfrc name="right_shoulder_roll_torque" joint="right_shoulder_roll_joint" />
    <jointactuatorfrc name="right_shoulder_yaw_torque" joint="right_shoulder_yaw_joint" />
    <jointactuatorfrc name="right_elbow_torque" joint="right_elbow_joint" />
    <jointactuatorfrc name="right_wrist_roll_torque" joint="right_wrist_roll_joint" />
    <jointactuatorfrc name="right_wrist_pitch_torque" joint="right_wrist_pitch_joint" />
    <jointactuatorfrc name="right_wrist_yaw_torque" joint="right_wrist_yaw_joint" />

    <framequat name="imu_quat" objtype="site" objname="imu" />
    <gyro name="imu_gyro" site="imu" />
    <accelerometer name="imu_acc" site="imu" />

    <framepos name="frame_pos" objtype="site" objname="imu" />
    <framelinvel name="frame_vel" objtype="site" objname="imu" />
  </sensor>

</mujoco>

================================================
FILE: assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0.urdf
================================================
<robot name="g1_29dof_rev_1_0">
  <material name="dark">
    <color rgba="0.2 0.2 0.2 1"/>
  </material>
  <material name="white">
    <color rgba="0.7 0.7 0.7 1"/>
  </material>

  <mujoco>
    <compiler meshdir="../meshes" discardvisual="false"/>
  </mujoco>

  <!-- [CAUTION] uncomment when convert to mujoco -->
  <!-- <link name="world"></link>
  <joint name="floating_base_joint" type="floating">
    <parent link="world"/>
    <child link="pelvis"/>
  </joint> -->

  <link name="pelvis">
    <inertial>
      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
      <mass value="3.813"/>
      <inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
  </link>
  <link name="pelvis_contour_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis_contour_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis_contour_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="pelvis_contour_joint" type="fixed">
    <parent link="pelvis"/>
    <child link="pelvis_contour_link"/>
  </joint>

  <!-- Legs -->
  <link name="left_hip_pitch_link">
    <inertial>
      <origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
      <mass value="1.35"/>
      <inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_pitch_joint" type="revolute">
    <origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="left_hip_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
  </joint>
  <link name="left_hip_roll_link">
    <inertial>
      <origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
      <mass value="1.52"/>
      <inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_roll_joint" type="revolute">
    <origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
    <parent link="left_hip_pitch_link"/>
    <child link="left_hip_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.5236" upper="2.9671" effort="139" velocity="20"/>
  </joint>
  <link name="left_hip_yaw_link">
    <inertial>
      <origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
      <mass value="1.702"/>
      <inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_yaw_joint" type="revolute">
    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
    <parent link="left_hip_roll_link"/>
    <child link="left_hip_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
  </joint>
  <link name="left_knee_link">
    <inertial>
      <origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
      <mass value="1.932"/>
      <inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_knee_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_knee_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_knee_joint" type="revolute">
    <origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
    <parent link="left_hip_yaw_link"/>
    <child link="left_knee_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
  </joint>
  <link name="left_ankle_pitch_link">
    <inertial>
      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
      <mass value="0.074"/>
      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_ankle_pitch_joint" type="revolute">
    <origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
    <parent link="left_knee_link"/>
    <child link="left_ankle_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/>
  </joint>
  <link name="left_ankle_roll_link">
    <inertial>
      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
      <mass value="0.608"/>
      <inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_roll_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_ankle_roll_joint" type="revolute">
    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
    <parent link="left_ankle_pitch_link"/>
    <child link="left_ankle_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/>
  </joint>
  <link name="right_hip_pitch_link">
    <inertial>
      <origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
      <mass value="1.35"/>
      <inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_pitch_joint" type="revolute">
    <origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="right_hip_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
  </joint>
  <link name="right_hip_roll_link">
    <inertial>
      <origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
      <mass value="1.52"/>
      <inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_roll_joint" type="revolute">
    <origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
    <parent link="right_hip_pitch_link"/>
    <child link="right_hip_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-2.9671" upper="0.5236" effort="139" velocity="20"/>
  </joint>
  <link name="right_hip_yaw_link">
    <inertial>
      <origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
      <mass value="1.702"/>
      <inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_yaw_joint" type="revolute">
    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
    <parent link="right_hip_roll_link"/>
    <child link="right_hip_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
  </joint>
  <link name="right_knee_link">
    <inertial>
      <origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
      <mass value="1.932"/>
      <inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_knee_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_knee_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_knee_joint" type="revolute">
    <origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
    <parent link="right_hip_yaw_link"/>
    <child link="right_knee_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
  </joint>
  <link name="right_ankle_pitch_link">
    <inertial>
      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
      <mass value="0.074"/>
      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_ankle_pitch_joint" type="revolute">
    <origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
    <parent link="right_knee_link"/>
    <child link="right_ankle_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/>
  </joint>
  <link name="right_ankle_roll_link">
    <inertial>
      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
      <mass value="0.608"/>
      <inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_roll_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_ankle_roll_joint" type="revolute">
    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
    <parent link="right_ankle_pitch_link"/>
    <child link="right_ankle_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/>
  </joint>

  <!-- Torso -->
  <link name="waist_yaw_link">
    <inertial>
      <origin xyz="0.003494 0.000233 0.018034" rpy="0 0 0"/>
      <mass value="0.214"/>
      <inertia ixx="0.00010673" ixy="2.703E-06" ixz="-7.631E-06" iyy="0.00010422" iyz="-2.01E-07" izz="0.0001625"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/waist_yaw_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <joint name="waist_yaw_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="waist_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
  </joint>
  <link name="waist_roll_link">
    <inertial>
      <origin xyz="0 2.3E-05 0" rpy="0 0 0"/>
      <mass value="0.086"/>
      <inertia ixx="7.079E-06" ixy="0" ixz="0" iyy="6.339E-06" iyz="0" izz="8.245E-06"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/waist_roll_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <joint name="waist_roll_joint" type="revolute">
    <origin xyz="-0.0039635 0 0.044" rpy="0 0 0"/>
    <parent link="waist_yaw_link"/>
    <child link="waist_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.52" upper="0.52" effort="35" velocity="30"/>
  </joint>
  <link name="torso_link">
    <inertial>
      <origin xyz="0.000931 0.000346 0.15082" rpy="0 0 0"/>
      <mass value="6.78"/>
      <inertia ixx="0.05905" ixy="3.3302E-05" ixz="-0.0017715" iyy="0.047014" iyz="-2.2399E-05" izz="0.025652"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/torso_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/torso_link_rev_1_0.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="waist_pitch_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="waist_roll_link"/>
    <child link="torso_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.52" upper="0.52" effort="35" velocity="30"/>
  </joint>

  <!-- LOGO -->
  <joint name="logo_joint" type="fixed">
    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="logo_link"/>
  </joint>
  <link name="logo_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/logo_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/logo_link.STL"/>
      </geometry>
    </collision>
  </link>

  <!-- Head -->
  <link name="head_link">
    <inertial>
      <origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
      <mass value="1.036"/>
      <inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/head_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/head_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_joint" type="fixed">
    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="head_link"/>
  </joint>


  <!-- IMU -->
  <link name="imu_in_torso"></link>
  <joint name="imu_in_torso_joint" type="fixed">
    <origin xyz="-0.03959 -0.00224 0.14792" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="imu_in_torso"/>
  </joint>

  <link name="imu_in_pelvis"></link>
  <joint name="imu_in_pelvis_joint" type="fixed">
    <origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="imu_in_pelvis"/>
  </joint>

  <!-- d435 -->
  <link name="d435_link"></link>
  <joint name="d435_joint" type="fixed">
    <origin xyz="0.0576235 0.01753 0.42987" rpy="0 0.8307767239493009 0"/>
    <parent link="torso_link"/>
    <child link="d435_link"/>
  </joint>

  <!-- mid360 -->
  <link name="mid360_link"></link>
  <joint name="mid360_joint" type="fixed">
    <origin xyz="0.0002835 0.00003 0.41618" rpy="0 0.04014257279586953 0"/>
    <parent link="torso_link"/>
    <child link="mid360_link"/>
  </joint>

  <!-- Arm -->
  <link name="left_shoulder_pitch_link">
    <inertial>
      <origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
      <mass value="0.718"/>
      <inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_pitch_joint" type="revolute">
    <origin xyz="0.0039563 0.10022 0.24778" rpy="0.27931 5.4949E-05 -0.00019159"/>
    <parent link="torso_link"/>
    <child link="left_shoulder_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
  </joint>
  <link name="left_shoulder_roll_link">
    <inertial>
      <origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
      <mass value="0.643"/>
      <inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_roll_joint" type="revolute">
    <origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
    <parent link="left_shoulder_pitch_link"/>
    <child link="left_shoulder_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
  </joint>
  <link name="left_shoulder_yaw_link">
    <inertial>
      <origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
      <mass value="0.734"/>
      <inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_yaw_joint" type="revolute">
    <origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
    <parent link="left_shoulder_roll_link"/>
    <child link="left_shoulder_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
  </joint>
  <link name="left_elbow_link">
    <inertial>
      <origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_elbow_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_elbow_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_elbow_joint" type="revolute">
    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
    <parent link="left_shoulder_yaw_link"/>
    <child link="left_elbow_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
  </joint>
  <joint name="left_wrist_roll_joint" type="revolute">
    <origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="left_elbow_link"/>
    <child link="left_wrist_roll_link"/>
    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
  </joint>
  <link name="left_wrist_roll_link">
    <inertial>
      <origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
      <mass value="0.08544498"/>
      <inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_wrist_pitch_joint" type="revolute">
    <origin xyz="0.038 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="left_wrist_roll_link"/>
    <child link="left_wrist_pitch_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="left_wrist_pitch_link">
    <inertial>
      <origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
      <mass value="0.48404956"/>
      <inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_wrist_yaw_joint" type="revolute">
    <origin xyz="0.046 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="left_wrist_pitch_link"/>
    <child link="left_wrist_yaw_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="left_wrist_yaw_link">
    <inertial>
      <origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
      <mass value="0.08457647"/>
      <inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hand_palm_joint" type="fixed">
    <origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
    <parent link="left_wrist_yaw_link"/>
    <child link="left_rubber_hand"/>
  </joint>
  <link name="left_rubber_hand">
    <inertial>
      <origin xyz="0.05361310808 -0.00295905240 0.00215413091" rpy="0 0 0"/>
      <mass value="0.170"/>
      <inertia ixx="0.00010099485234748" ixy="0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="0.00000330189743286" izz="0.00021894770413514"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_rubber_hand.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <link name="right_shoulder_pitch_link">
    <inertial>
      <origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
      <mass value="0.718"/>
      <inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_pitch_joint" type="revolute">
    <origin xyz="0.0039563 -0.10021 0.24778" rpy="-0.27931 5.4949E-05 0.00019159"/>
    <parent link="torso_link"/>
    <child link="right_shoulder_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
  </joint>
  <link name="right_shoulder_roll_link">
    <inertial>
      <origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
      <mass value="0.643"/>
      <inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_roll_joint" type="revolute">
    <origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
    <parent link="right_shoulder_pitch_link"/>
    <child link="right_shoulder_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
  </joint>
  <link name="right_shoulder_yaw_link">
    <inertial>
      <origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
      <mass value="0.734"/>
      <inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_yaw_joint" type="revolute">
    <origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
    <parent link="right_shoulder_roll_link"/>
    <child link="right_shoulder_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
  </joint>
  <link name="right_elbow_link">
    <inertial>
      <origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_elbow_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_elbow_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_elbow_joint" type="revolute">
    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
    <parent link="right_shoulder_yaw_link"/>
    <child link="right_elbow_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
  </joint>
  <joint name="right_wrist_roll_joint" type="revolute">
    <origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="right_elbow_link"/>
    <child link="right_wrist_roll_link"/>
    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
  </joint>
  <link name="right_wrist_roll_link">
    <inertial>
      <origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
      <mass value="0.08544498"/>
      <inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_wrist_pitch_joint" type="revolute">
    <origin xyz="0.038 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="right_wrist_roll_link"/>
    <child link="right_wrist_pitch_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="right_wrist_pitch_link">
    <inertial>
      <origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
      <mass value="0.48404956"/>
      <inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_wrist_yaw_joint" type="revolute">
    <origin xyz="0.046 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="right_wrist_pitch_link"/>
    <child link="right_wrist_yaw_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="right_wrist_yaw_link">
    <inertial>
      <origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
      <mass value="0.08457647"/>
      <inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hand_palm_joint" type="fixed">
    <origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
    <parent link="right_wrist_yaw_link"/>
    <child link="right_rubber_hand"/>
  </joint>
  <link name="right_rubber_hand">
    <inertial>
      <origin xyz="0.05361310808 0.00295905240 0.00215413091" rpy="0 0 0"/>
      <mass value="0.170"/>
      <inertia ixx="0.00010099485234748" ixy="-0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="-0.00000330189743286" izz="0.00021894770413514"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_rubber_hand.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
</robot>

================================================
FILE: assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0.xml
================================================
<mujoco model="g1_29dof_rev_1_0">
  <compiler angle="radian" meshdir="../meshes"/>

  <statistic meansize="0.144785" extent="1.23314" center="0.025392 2.0634e-05 -0.245975"/>
  <default>
    <joint damping="0.00" armature="0.001" frictionloss="0.03"/>
  </default>

  <asset>
    <mesh name="pelvis" file="pelvis.STL"/>
    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL"/>
    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL"/>
    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL"/>
    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL"/>
    <mesh name="left_knee_link" file="left_knee_link.STL"/>
    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL"/>
    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL"/>
    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL"/>
    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL"/>
    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL"/>
    <mesh name="right_knee_link" file="right_knee_link.STL"/>
    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL"/>
    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL"/>
    <mesh name="waist_yaw_link" file="waist_yaw_link_rev_1_0.STL"/>
    <mesh name="waist_roll_link" file="waist_roll_link_rev_1_0.STL"/>
    <mesh name="torso_link" file="torso_link_rev_1_0.STL"/>
    <mesh name="logo_link" file="logo_link.STL"/>
    <mesh name="head_link" file="head_link.STL"/>
    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL"/>
    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL"/>
    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL"/>
    <mesh name="left_elbow_link" file="left_elbow_link.STL"/>
    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL"/>
    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL"/>
    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL"/>
    <mesh name="left_rubber_hand" file="left_rubber_hand.STL"/>
    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL"/>
    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL"/>
    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL"/>
    <mesh name="right_elbow_link" file="right_elbow_link.STL"/>
    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL"/>
    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL"/>
    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL"/>
    <mesh name="right_rubber_hand" file="right_rubber_hand.STL"/>
  </asset>

  <worldbody>
    <body name="pelvis" pos="0 0 0.793">
      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813" diaginertia="0.010549 0.0093089 0.0079184"/>
      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false"/>
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="pelvis"/>
      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link"/>
      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link"/>
      <site name="imu_in_pelvis" size="0.01" pos="0.04525 0 -0.08339"/>
      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122" mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212"/>
        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link"/>
        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link"/>
        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
          <inertial pos="0.029812 -0.001045 -0.087934" quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755"/>
          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671" actuatorfrcrange="-139 139"/>
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link"/>
          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link"/>
          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181" mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139"/>
            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link"/>
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link"/>
            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
              <inertial pos="0.005457 0.003964 -0.12074" quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932" diaginertia="0.0113804 0.0112778 0.00146458"/>
              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_knee_link"/>
              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link"/>
              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link"/>
                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link"/>
                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
                  <inertial pos="0.026505 0 -0.016425" quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608" diaginertia="0.00167218 0.0016161 0.000217621"/>
                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link"/>
                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1"/>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122" mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212"/>
        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798" actuatorfrcrange="-88 88"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link"/>
        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link"/>
        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793" mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755"/>
          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236" actuatorfrcrange="-139 139"/>
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link"/>
          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link"/>
          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598" mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139"/>
            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576" actuatorfrcrange="-88 88"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link"/>
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link"/>
            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734" quat="0.996179 0 0.0873386 0">
              <inertial pos="0.005457 -0.003964 -0.12074" quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932" diaginertia="0.011374 0.0112843 0.00146452"/>
              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798" actuatorfrcrange="-139 139"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_knee_link"/>
              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link"/>
              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053" mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06"/>
                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.87267 0.5236" actuatorfrcrange="-50 50"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link"/>
                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link"/>
                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
                  <inertial pos="0.026505 0 -0.016425" quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608" diaginertia="0.00167218 0.0016161 0.000217621"/>
                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.2618 0.2618" actuatorfrcrange="-50 50"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link"/>
                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1"/>
                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1"/>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
      <body name="waist_yaw_link">
        <inertial pos="0.003494 0.000233 0.018034" quat="0.289697 0.591001 -0.337795 0.672821" mass="0.214" diaginertia="0.000163531 0.000107714 0.000102205"/>
        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-88 88"/>
        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_yaw_link"/>
        <body name="waist_roll_link" pos="-0.0039635 0 0.044">
          <inertial pos="0 2.3e-05 0" quat="0.5 0.5 -0.5 0.5" mass="0.086" diaginertia="8.245e-06 7.079e-06 6.339e-06"/>
          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_roll_link"/>
          <body name="torso_link">
            <inertial pos="0.00203158 0.000339683 0.184568" quat="0.999803 -6.03319e-05 0.0198256 0.00131986" mass="7.818" diaginertia="0.121847 0.109825 0.0273735"/>
            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52" actuatorfrcrange="-50 50"/>
            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="torso_link"/>
            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link"/>
            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link"/>
            <geom pos="0.0039635 0 -0.044" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1" mesh="logo_link"/>
            <geom pos="0.0039635 0 -0.044" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="head_link"/>
            <geom pos="0.0039635 0 -0.044" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link"/>
            <site name="imu_in_torso" size="0.01" pos="-0.03959 -0.00224 0.14792"/>
            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.24778" quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225" mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394"/>
              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link"/>
              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.7 0.7 0.7 1"/>
              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831" quat="0.990268 -0.139172 0 0">
                <inertial pos="-0.000227 0.00727 -0.063243" quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643" diaginertia="0.000691311 0.000618011 0.000388977"/>
                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-1.5882 2.2515" actuatorfrcrange="-25 25"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link"/>
                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder" rgba="0.7 0.7 0.7 1"/>
                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
                  <inertial pos="0.010773 -0.002949 -0.072009" quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link"/>
                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link"/>
                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
                    <inertial pos="0.064956 0.004454 -0.010062" quat="0.541765 0.636132 0.388821 0.388129" mass="0.6" diaginertia="0.000443035 0.000421612 0.000259353"/>
                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link"/>
                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link"/>
                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
                      <inertial pos="0.0171394 0.000537591 4.8864e-07" quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0" range="-1.97222 1.97222" actuatorfrcrange="-25 25"/>
                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link"/>
                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link"/>
                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
                        <inertial pos="0.0229999 -0.00111685 -0.00111658" quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.61443 1.61443" actuatorfrcrange="-5 5"/>
                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link"/>
                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link"/>
                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
                          <inertial pos="0.0708244 0.000191745 0.00161742" quat="0.510571 0.526295 0.468078 0.493188" mass="0.254576" diaginertia="0.000646113 0.000559993 0.000147566"/>
                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.61443 1.61443" actuatorfrcrange="-5 5"/>
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link"/>
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link"/>
                          <!-- <body name="left_rubber_hand" pos="0.0415 0.003 0"> -->
                            <!-- <inertial pos="0.05361310808 -0.00295905240 0.00215413091" quat="1 0 0 0" mass="0.170" diaginertia="0.00010099485234748 0.00028135871571621 0.00021894770413514"/> -->
                            <!-- <joint name="left_hand_palm_joint" pos="0 0 0" type="fixed"/> -->
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="left_rubber_hand"/>
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_rubber_hand"/>
                          <!-- </body> -->
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.24778" quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152" mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394"/>
              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0" range="-3.0892 2.6704" actuatorfrcrange="-25 25"/>
              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link"/>
              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.7 0.7 0.7 1"/>
              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831" quat="0.990268 0.139172 0 0">
                <inertial pos="-0.000227 -0.00727 -0.063243" quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643" diaginertia="0.000691311 0.000618011 0.000388977"/>
                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.2515 1.5882" actuatorfrcrange="-25 25"/>
                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link"/>
                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder" rgba="0.7 0.7 0.7 1"/>
                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
                  <inertial pos="0.010773 0.002949 -0.072009" quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734" diaginertia="0.00106187 0.00103217 0.000400661"/>
                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618" actuatorfrcrange="-25 25"/>
                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link"/>
                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link"/>
                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
                    <inertial pos="0.064956 -0.004454 -0.010062" quat="0.388129 0.388821 0.636132 0.541765" mass="0.6" diaginertia="0.000443035 0.000421612 0.000259353"/>
                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944" actuatorfrcrange="-25 25"/>
                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link"/>
                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link"/>
                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
                      <inertial pos="0.0171394 -0.000537591 4.8864e-07" quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445" diaginertia="5.48211e-05 4.96646e-05 3.57798e-05"/>
                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0" range="-1.97222 1.97222" actuatorfrcrange="-25 25"/>
                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link"/>
                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link"/>
                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
                        <inertial pos="0.0229999 0.00111685 -0.00111658" quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405" diaginertia="0.000430353 0.000429873 0.000164648"/>
                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-1.61443 1.61443" actuatorfrcrange="-5 5"/>
                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link"/>
                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link"/>
                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
                          <inertial pos="0.0708244 -0.000191745 0.00161742" quat="0.493188 0.468078 0.526295 0.510571" mass="0.254576" diaginertia="0.000646113 0.000559993 0.000147566"/>
                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-1.61443 1.61443" actuatorfrcrange="-5 5"/>
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link"/>
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link"/>
                          <!-- <body name="right_rubber_hand" pos="0.0415 -0.003 0"> -->
                            <!-- <inertial pos="0.05361310808 0.00295905240 0.00215413091" quat="1 0 0 0" mass="0.170" diaginertia="0.00010099485234748 0.00028135871571621 0.00021894770413514"/> -->
                            <!-- <joint name="right_hand_palm_joint" pos="0 0 0" type="fixed"/> -->
                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="right_rubber_hand"/>
                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_rubber_hand"/>
                          <!-- </body> -->
                        </body>
                      </body>
                    </body>
                  </body>
                </body>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>
  </worldbody>

  <actuator>
    <motor name="left_hip_pitch_joint" joint="left_hip_pitch_joint"/>
    <motor name="left_hip_roll_joint" joint="left_hip_roll_joint"/>
    <motor name="left_hip_yaw_joint" joint="left_hip_yaw_joint"/>
    <motor name="left_knee_joint" joint="left_knee_joint"/>
    <motor name="left_ankle_pitch_joint" joint="left_ankle_pitch_joint"/>
    <motor name="left_ankle_roll_joint" joint="left_ankle_roll_joint"/>
    <motor name="right_hip_pitch_joint" joint="right_hip_pitch_joint"/>
    <motor name="right_hip_roll_joint" joint="right_hip_roll_joint"/>
    <motor name="right_hip_yaw_joint" joint="right_hip_yaw_joint"/>
    <motor name="right_knee_joint" joint="right_knee_joint"/>
    <motor name="right_ankle_pitch_joint" joint="right_ankle_pitch_joint"/>
    <motor name="right_ankle_roll_joint" joint="right_ankle_roll_joint"/>
    <motor name="waist_yaw_joint" joint="waist_yaw_joint"/>
    <motor name="waist_roll_joint" joint="waist_roll_joint"/>
    <motor name="waist_pitch_joint" joint="waist_pitch_joint"/>
    <motor name="left_shoulder_pitch_joint" joint="left_shoulder_pitch_joint"/>
    <motor name="left_shoulder_roll_joint" joint="left_shoulder_roll_joint"/>
    <motor name="left_shoulder_yaw_joint" joint="left_shoulder_yaw_joint"/>
    <motor name="left_elbow_joint" joint="left_elbow_joint"/>
    <motor name="left_wrist_roll_joint" joint="left_wrist_roll_joint"/>
    <motor name="left_wrist_pitch_joint" joint="left_wrist_pitch_joint"/>
    <motor name="left_wrist_yaw_joint" joint="left_wrist_yaw_joint"/>
    <motor name="right_shoulder_pitch_joint" joint="right_shoulder_pitch_joint"/>
    <motor name="right_shoulder_roll_joint" joint="right_shoulder_roll_joint"/>
    <motor name="right_shoulder_yaw_joint" joint="right_shoulder_yaw_joint"/>
    <motor name="right_elbow_joint" joint="right_elbow_joint"/>
    <motor name="right_wrist_roll_joint" joint="right_wrist_roll_joint"/>
    <motor name="right_wrist_pitch_joint" joint="right_wrist_pitch_joint"/>
    <motor name="right_wrist_yaw_joint" joint="right_wrist_yaw_joint"/>
  </actuator>

  <sensor>
    <gyro name="imu-torso-angular-velocity" site="imu_in_torso" noise="5e-4" cutoff="34.9"/>
    <accelerometer name="imu-torso-linear-acceleration" site="imu_in_torso" noise="1e-2" cutoff="157"/>
    <gyro name="imu-pelvis-angular-velocity" site="imu_in_pelvis" noise="5e-4" cutoff="34.9"/>
    <accelerometer name="imu-pelvis-linear-acceleration" site="imu_in_pelvis" noise="1e-2" cutoff="157"/>
  </sensor>


  <!-- setup scene -->
  <statistic center="1.0 0.7 1.0" extent="0.8"/>
  <visual>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0.9 0.9 0.9"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="-140" elevation="-20"/>
  </visual>
  <asset>
    <texture type="skybox" builtin="flat" rgb1="0 0 0" rgb2="0 0 0" width="512" height="3072"/>
    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
  </asset>
  <worldbody>
    <light pos="1 0 3.5" dir="0 0 -1" directional="true"/>
    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
  </worldbody>
</mujoco>

================================================
FILE: assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0_s100.urdf
================================================
<robot name="g1_29dof_rev_1_0">
  <material name="dark">
    <color rgba="0.2 0.2 0.2 1"/>
  </material>
  <material name="white">
    <color rgba="0.7 0.7 0.7 1"/>
  </material>
  <material name="red">
    <color rgba="1.0 0.0 0.0 1"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 1.0 1"/>
  </material>
  <material name="green">
    <color rgba="0.0 1.0 0.0 1"/>
  </material>
  <material name="yellow">
    <color rgba="1.0 1.0 0.0 1"/>
  </material>
  <material name="purple">
    <color rgba="1.0 0.0 1.0 1"/>
  </material>

  <mujoco>
    <compiler meshdir="../meshes" discardvisual="false"/>
  </mujoco>

  <!-- [CAUTION] uncomment when convert to mujoco -->
  <!-- <link name="world"></link>
  <joint name="floating_base_joint" type="floating">
    <parent link="world"/>
    <child link="pelvis"/>
  </joint> -->

  <link name="pelvis">
    <inertial>
      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
      <mass value="3.813"/>
      <inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
  </link>
  <link name="pelvis_contour_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis_contour_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/pelvis_contour_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="pelvis_contour_joint" type="fixed">
    <parent link="pelvis"/>
    <child link="pelvis_contour_link"/>
  </joint>

  <!-- Legs -->
  <link name="left_hip_pitch_link">
    <inertial>
      <origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
      <mass value="1.35"/>
      <inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_pitch_joint" type="revolute">
    <origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="left_hip_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
  </joint>
  <link name="left_hip_roll_link">
    <inertial>
      <origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
      <mass value="1.52"/>
      <inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_roll_joint" type="revolute">
    <origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
    <parent link="left_hip_pitch_link"/>
    <child link="left_hip_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.5236" upper="2.9671" effort="139" velocity="20"/>
  </joint>
  <link name="left_hip_yaw_link">
    <inertial>
      <origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
      <mass value="1.702"/>
      <inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_hip_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hip_yaw_joint" type="revolute">
    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
    <parent link="left_hip_roll_link"/>
    <child link="left_hip_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
  </joint>
  <link name="left_knee_link">
    <inertial>
      <origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
      <mass value="1.932"/>
      <inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_knee_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_knee_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_knee_joint" type="revolute">
    <origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
    <parent link="left_hip_yaw_link"/>
    <child link="left_knee_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
  </joint>
  <link name="left_ankle_pitch_link">
    <inertial>
      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
      <mass value="0.074"/>
      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_ankle_pitch_joint" type="revolute">
    <origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
    <parent link="left_knee_link"/>
    <child link="left_ankle_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/>
  </joint>
  <link name="left_ankle_roll_link">
    <inertial>
      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
      <mass value="0.608"/>
      <inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_ankle_roll_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_ankle_roll_joint" type="revolute">
    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
    <parent link="left_ankle_pitch_link"/>
    <child link="left_ankle_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/>
  </joint>
  <link name="right_hip_pitch_link">
    <inertial>
      <origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
      <mass value="1.35"/>
      <inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_pitch_joint" type="revolute">
    <origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="right_hip_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
  </joint>
  <link name="right_hip_roll_link">
    <inertial>
      <origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
      <mass value="1.52"/>
      <inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_roll_joint" type="revolute">
    <origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
    <parent link="right_hip_pitch_link"/>
    <child link="right_hip_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-2.9671" upper="0.5236" effort="139" velocity="20"/>
  </joint>
  <link name="right_hip_yaw_link">
    <inertial>
      <origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
      <mass value="1.702"/>
      <inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_hip_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hip_yaw_joint" type="revolute">
    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
    <parent link="right_hip_roll_link"/>
    <child link="right_hip_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
  </joint>
  <link name="right_knee_link">
    <inertial>
      <origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
      <mass value="1.932"/>
      <inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_knee_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_knee_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_knee_joint" type="revolute">
    <origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
    <parent link="right_hip_yaw_link"/>
    <child link="right_knee_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
  </joint>
  <link name="right_ankle_pitch_link">
    <inertial>
      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
      <mass value="0.074"/>
      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_ankle_pitch_joint" type="revolute">
    <origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
    <parent link="right_knee_link"/>
    <child link="right_ankle_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.87267" upper="0.5236" effort="35" velocity="30"/>
  </joint>
  <link name="right_ankle_roll_link">
    <inertial>
      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
      <mass value="0.608"/>
      <inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_ankle_roll_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_ankle_roll_joint" type="revolute">
    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
    <parent link="right_ankle_pitch_link"/>
    <child link="right_ankle_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.2618" upper="0.2618" effort="35" velocity="30"/>
  </joint>

  <!-- Torso -->
  <link name="waist_yaw_link">
    <inertial>
      <origin xyz="0.003494 0.000233 0.018034" rpy="0 0 0"/>
      <mass value="0.214"/>
      <inertia ixx="0.00010673" ixy="2.703E-06" ixz="-7.631E-06" iyy="0.00010422" iyz="-2.01E-07" izz="0.0001625"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/waist_yaw_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <joint name="waist_yaw_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="waist_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
  </joint>
  <link name="waist_roll_link">
    <inertial>
      <origin xyz="0 2.3E-05 0" rpy="0 0 0"/>
      <mass value="0.086"/>
      <inertia ixx="7.079E-06" ixy="0" ixz="0" iyy="6.339E-06" iyz="0" izz="8.245E-06"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/waist_roll_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <joint name="waist_roll_joint" type="revolute">
    <origin xyz="-0.0039635 0 0.044" rpy="0 0 0"/>
    <parent link="waist_yaw_link"/>
    <child link="waist_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.52" upper="0.52" effort="35" velocity="30"/>
  </joint>
  <link name="torso_link">
    <inertial>
      <origin xyz="0.000931 0.000346 0.15082" rpy="0 0 0"/>
      <mass value="6.78"/>
      <inertia ixx="0.05905" ixy="3.3302E-05" ixz="-0.0017715" iyy="0.047014" iyz="-2.2399E-05" izz="0.025652"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/torso_link_rev_1_0.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/torso_link_rev_1_0.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="waist_pitch_joint" type="revolute">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="waist_roll_link"/>
    <child link="torso_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-0.52" upper="0.52" effort="35" velocity="30"/>
  </joint>

  <!-- LOGO -->
  <joint name="logo_joint" type="fixed">
    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="logo_link"/>
  </joint>
  <link name="logo_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/logo_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/logo_link.STL"/>
      </geometry>
    </collision>
  </link>

  <!-- Head -->
  <link name="head_link">
    <inertial>
      <origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
      <mass value="1.036"/>
      <inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/head_link.STL"/>
      </geometry>
      <material name="dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/head_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_joint" type="fixed">
    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="head_link"/>
  </joint>


  <!-- IMU -->
  <link name="imu_in_torso"></link>
  <joint name="imu_in_torso_joint" type="fixed">
    <origin xyz="-0.03959 -0.00224 0.14792" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="imu_in_torso"/>
  </joint>

  <link name="imu_in_pelvis"></link>
  <joint name="imu_in_pelvis_joint" type="fixed">
    <origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/>
    <parent link="pelvis"/>
    <child link="imu_in_pelvis"/>
  </joint>

  <!-- d435 -->
  <link name="d435_link"></link>
  <joint name="d435_joint" type="fixed">
    <origin xyz="0.0576235 0.01753 0.42987" rpy="0 0.8307767239493009 0"/>
    <parent link="torso_link"/>
    <child link="d435_link"/>
  </joint>

  <!-- mid360 -->
  <link name="mid360_link"></link>
  <joint name="mid360_joint" type="fixed">
    <origin xyz="0.0002835 0.00003 0.41618" rpy="0 0.04014257279586953 0"/>
    <parent link="torso_link"/>
    <child link="mid360_link"/>
  </joint>
    <!-- S100 Processor - Using STL -->
  <link name="shell_support">
    <inertial>
      <origin xyz="0.0 0.0 -0.0" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.000943" ixy="-2.1e-08" ixz="-2.70e-05"
              iyy="0.000991" iyz="1.8e-08"  izz="0.001600"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shell_support_s100_transformed.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="purple"/>
    </visual>
    <collision>
     <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shell_support_s100_transformed.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="s100_joint" type="fixed">
    <origin xyz="-0.09 -0.005 0.06" rpy="0 0 0"/>
    <parent link="torso_link"/>
    <child link="shell_support"/>
  </joint>
  <!-- Arm -->
  <link name="left_shoulder_pitch_link">
    <inertial>
      <origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
      <mass value="0.718"/>
      <inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_pitch_joint" type="revolute">
    <origin xyz="0.0039563 0.10022 0.24778" rpy="0.27931 5.4949E-05 -0.00019159"/>
    <parent link="torso_link"/>
    <child link="left_shoulder_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
  </joint>
  <link name="left_shoulder_roll_link">
    <inertial>
      <origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
      <mass value="0.643"/>
      <inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_roll_joint" type="revolute">
    <origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
    <parent link="left_shoulder_pitch_link"/>
    <child link="left_shoulder_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
  </joint>
  <link name="left_shoulder_yaw_link">
    <inertial>
      <origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
      <mass value="0.734"/>
      <inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_shoulder_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_shoulder_yaw_joint" type="revolute">
    <origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
    <parent link="left_shoulder_roll_link"/>
    <child link="left_shoulder_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
  </joint>
  <link name="left_elbow_link">
    <inertial>
      <origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_elbow_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_elbow_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_elbow_joint" type="revolute">
    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
    <parent link="left_shoulder_yaw_link"/>
    <child link="left_elbow_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
  </joint>
  <joint name="left_wrist_roll_joint" type="revolute">
    <origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="left_elbow_link"/>
    <child link="left_wrist_roll_link"/>
    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
  </joint>
  <link name="left_wrist_roll_link">
    <inertial>
      <origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
      <mass value="0.08544498"/>
      <inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_wrist_pitch_joint" type="revolute">
    <origin xyz="0.038 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="left_wrist_roll_link"/>
    <child link="left_wrist_pitch_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="left_wrist_pitch_link">
    <inertial>
      <origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
      <mass value="0.48404956"/>
      <inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_wrist_yaw_joint" type="revolute">
    <origin xyz="0.046 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="left_wrist_pitch_link"/>
    <child link="left_wrist_yaw_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="left_wrist_yaw_link">
    <inertial>
      <origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
      <mass value="0.08457647"/>
      <inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_wrist_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="left_hand_palm_joint" type="fixed">
    <origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
    <parent link="left_wrist_yaw_link"/>
    <child link="left_rubber_hand"/>
  </joint>
  <link name="left_rubber_hand">
    <inertial>
      <origin xyz="0.05361310808 -0.00295905240 0.00215413091" rpy="0 0 0"/>
      <mass value="0.170"/>
      <inertia ixx="0.00010099485234748" ixy="0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="0.00000330189743286" izz="0.00021894770413514"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/left_rubber_hand.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
  <link name="right_shoulder_pitch_link">
    <inertial>
      <origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
      <mass value="0.718"/>
      <inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.05"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_pitch_joint" type="revolute">
    <origin xyz="0.0039563 -0.10021 0.24778" rpy="-0.27931 5.4949E-05 0.00019159"/>
    <parent link="torso_link"/>
    <child link="right_shoulder_pitch_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
  </joint>
  <link name="right_shoulder_roll_link">
    <inertial>
      <origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
      <mass value="0.643"/>
      <inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_roll_joint" type="revolute">
    <origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
    <parent link="right_shoulder_pitch_link"/>
    <child link="right_shoulder_roll_link"/>
    <axis xyz="1 0 0"/>
    <limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
  </joint>
  <link name="right_shoulder_yaw_link">
    <inertial>
      <origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
      <mass value="0.734"/>
      <inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_shoulder_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_shoulder_yaw_joint" type="revolute">
    <origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
    <parent link="right_shoulder_roll_link"/>
    <child link="right_shoulder_yaw_link"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
  </joint>
  <link name="right_elbow_link">
    <inertial>
      <origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_elbow_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_elbow_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_elbow_joint" type="revolute">
    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
    <parent link="right_shoulder_yaw_link"/>
    <child link="right_elbow_link"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
  </joint>
  <joint name="right_wrist_roll_joint" type="revolute">
    <origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="right_elbow_link"/>
    <child link="right_wrist_roll_link"/>
    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
  </joint>
  <link name="right_wrist_roll_link">
    <inertial>
      <origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
      <mass value="0.08544498"/>
      <inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_roll_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_roll_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_wrist_pitch_joint" type="revolute">
    <origin xyz="0.038 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="right_wrist_roll_link"/>
    <child link="right_wrist_pitch_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="right_wrist_pitch_link">
    <inertial>
      <origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
      <mass value="0.48404956"/>
      <inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_pitch_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_pitch_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_wrist_yaw_joint" type="revolute">
    <origin xyz="0.046 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="right_wrist_pitch_link"/>
    <child link="right_wrist_yaw_link"/>
    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
  </joint>
  <link name="right_wrist_yaw_link">
    <inertial>
      <origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
      <mass value="0.08457647"/>
      <inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_yaw_link.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_wrist_yaw_link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="right_hand_palm_joint" type="fixed">
    <origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
    <parent link="right_wrist_yaw_link"/>
    <child link="right_rubber_hand"/>
  </joint>
  <link name="right_rubber_hand">
    <inertial>
      <origin xyz="0.05361310808 0.00295905240 0.00215413091" rpy="0 0 0"/>
      <mass value="0.170"/>
      <inertia ixx="0.00010099485234748" ixy="-0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="-0.00000330189743286" izz="0.00021894770413514"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="../meshes/right_rubber_hand.STL"/>
      </geometry>
      <material name="white"/>
    </visual>
  </link>
</robot>

================================================
FILE: assets/robots/unitree/G1/29dof/scene_29dof.xml
================================================
<mujoco model="g1_29dof scene">
  <include file="g1_29dof.xml"/>

  <!-- setup scene -->
  <statistic center="0.0 0.0 1.0" extent="0.8"/>
  <visual>
    <headlight diffuse="0.75 0.75 0.75" ambient="0.18 0.18 0.18" specular="0.95 0.95 0.95"/>
    <rgba haze="0.15 0.25 0.35 1"/>
    <global azimuth="-140" elevation="-20" offwidth="2080" offheight="1170"/>
  </visual>
  <asset>
     <texture type="skybox" builtin="gradient" rgb1="1 1 1" rgb2="1 1 1" width="800" height="800"/>
    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="1.0 1.0 1.0" rgb2="0.6 0.8 1.0" markrgb="0 0 0"
      width="300" height="300"/>
    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0"/>
    <texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0" width="100" height="100"/>
    <texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
    <texture name="texplane" builtin="checker" height="512" width="512" rgb1=".2 .3 .4" rgb2=".1 .15 .2" type="2d" />
    <material name="MatPlane" reflectance="0.5" shininess="0.01" specular="0.1" texrepeat="1 1" texture="texplane" texuniform="true" />
    <material name="geom" texture="texgeom" texuniform="true"/>
  </asset>
   <worldbody>
    <geom name="floor" size="0 0 0.01" type="plane" material="groundplane" contype="1" conaffinity="0" priority="1" condim="3"/>

        <light diffuse="0.65 0.65 0.65" pos="-3 -3 5" dir="3 3 -5" castshadow="true"/>

  </worldbody>
</mujoco>


================================================
FILE: assets/test_data/motion_retargeting/ACCAD/Male1Walking_c3d/Walk_B10_-_Walk_turn_left_45_stageii.npz
================================================
version https://git-lfs.github.com/spec/v1
oid sha256:738f96eb1767e281d78631ca697079adefb5f171d581acd622a27740f2503b4e
size 5876184


================================================
FILE: deploy.env
================================================
export CONDA_BASE=$(conda info --base)
export Deploy_CONDA_PREFIX="$CONDA_BASE/envs/holomotion_deploy"

export CUDA_HOME=$Deploy_CONDA_PREFIX
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$Deploy_CONDA_PREFIX/lib/
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$Deploy_CONDA_PREFIX/lib/stubs
export LIBRARY_PATH=$Deploy_CONDA_PREFIX/lib:$LIBRARY_PATH
export LIBRARY_PATH=$Deploy_CONDA_PREFIX/lib/stubs:$LIBRARY_PATH
export HYDRA_FULL_ERROR=1

echo "--------------------------------"
echo "Deploy_CONDA_PREFIX: $Deploy_CONDA_PREFIX"
echo "CUDA_HOME: $CUDA_HOME"
echo "LD_LIBRARY_PATH: $LD_LIBRARY_PATH"
echo "LIBRARY_PATH: $LIBRARY_PATH"
echo "HYDRA_FULL_ERROR: $HYDRA_FULL_ERROR"
echo "--------------------------------"

================================================
FILE: deployment/deploy_environment.sh
================================================
#!/bin/bash
##############################################################################
# HoloMotion Environment Deployment Script
#
# This script sets up the complete environment for HoloMotion humanoid robot
# system deployment. It handles:
# 1. Conda environment creation with all dependencies (CUDA, PyTorch, etc.)
# 2. Special dependencies (unitree_sdk2_python)  
# 3. ROS2 workspace compilation
#
# Prerequisites:
# - Anaconda/Miniconda installed
# - ROS2 Humble installed at /opt/ros/humble/
# - Unitree ROS2 SDK at ~/unitree_ros2/
#
# Usage:
#   chmod +x deploy_environment.sh
#   ./deploy_environment.sh [environment_name]
#
# Arguments:
#   environment_name: Optional. Name for the conda environment (default: holomotion_deploy)
#
# Examples:
#   ./deploy_environment.sh                    # Uses default name 'holomotion_deploy'
#   ./deploy_environment.sh my_robot_env      # Uses custom name 'my_robot_env'
#
# Author: HoloMotion Team
##############################################################################

set -e  # Exit on any error

# Parse command line arguments
ENV_NAME="${1:-holomotion_deploy}"

echo "🚀 Starting HoloMotion Environment Deployment..."
echo "📝 Environment name: $ENV_NAME"

# Get script directory
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
PROJECT_ROOT="$(dirname "$(dirname "$SCRIPT_DIR")")"

echo "📁 Project root: $PROJECT_ROOT"
echo "📁 Script directory: $SCRIPT_DIR"

# Step 1: Create conda environment with all dependencies
echo ""
echo "📦 Step 1: Creating conda environment with all dependencies..."
if conda env list | grep -q "^$ENV_NAME "; then
    echo "⚠️  Environment '$ENV_NAME' already exists. Removing it..."
    conda env remove -n "$ENV_NAME" -y
fi

echo "🔧 Creating new environment from environment_deploy.yaml..."
echo "   This will install: PyTorch (CUDA), NumPy, SciPy, ONNX Runtime, and all other dependencies..."
cd "$PROJECT_ROOT"
conda env create -f holomotion/environment_deploy.yaml -n "$ENV_NAME"

echo "✅ Conda environment with all dependencies created successfully!"

# Step 2: Install unitree_sdk2_python
echo ""
echo "📦 Step 2: Installing unitree_sdk2_python..."

# Function to run commands in conda environment
run_in_env() {
    conda run -n "$ENV_NAME" "$@"
}

echo "🔧 Installing unitree_sdk2_python..."
if [ ! -d "$HOME/unitree_sdk2_python" ]; then
    echo "📥 Cloning unitree_sdk2_python repository..."
    git clone https://github.com/unitreerobotics/unitree_sdk2_python.git "$HOME/unitree_sdk2_python"
fi

echo "🔧 Installing unitree_sdk2_python in development mode..."
cd "$HOME/unitree_sdk2_python"
run_in_env pip install -e .

echo "✅ unitree_sdk2_python installed successfully!"

# Step 3: Setup ROS2 workspace
echo ""
echo "📦 Step 3: Setting up ROS2 workspace..."

# Ensure conda environment is completely deactivated for ROS2 compilation
echo "🔧 Ensuring conda environment is completely deactivated..."

# Initialize conda for this script
eval "$(conda shell.bash hook)"

# Deactivate any active conda environments
while [[ "$CONDA_DEFAULT_ENV" != "" && "$CONDA_DEFAULT_ENV" != "base" ]]; do
    echo "  Deactivating conda environment: $CONDA_DEFAULT_ENV"
    conda deactivate
done

# If we're still in base environment, deactivate it too
if [[ "$CONDA_DEFAULT_ENV" == "base" ]]; then
    echo "  Deactivating base conda environment"
    conda deactivate
fi

echo "  ✅ Conda environment fully deactivated"

# Check ROS2 installation
if [ ! -f "/opt/ros/humble/setup.bash" ]; then
    echo "❌ ROS2 Humble not found at /opt/ros/humble/"
    echo "   Please install ROS2 Humble first: https://docs.ros.org/en/humble/Installation.html"
    exit 1
fi

# Check Unitree ROS2 SDK
if [ ! -f "$HOME/unitree_ros2/setup.sh" ]; then
    echo "❌ Unitree ROS2 SDK not found at ~/unitree_ros2/"
    echo "   Please install Unitree ROS2 SDK first"
    exit 1
fi

echo "🔧 Compiling ROS2 workspace..."
cd "$PROJECT_ROOT/holomotion/deployment/unitree_g1_ros2_29dof"

# Create necessary directories
echo "📁 Creating required directories..."
mkdir -p src/models
mkdir -p src/motion_data

# Clean previous build
rm -rf build install log

# Source ROS2 and Unitree setup
source /opt/ros/humble/setup.bash
source ~/unitree_ros2/setup.sh

# Build workspace
echo "🏗️  Building workspace with colcon..."
colcon build

echo "✅ ROS2 workspace compiled successfully!"

echo ""
echo "🎉 Deployment completed successfully!"
echo ""
echo "📋 Summary of installed packages:"
echo "   ✅ PyTorch 2.3.1 with CUDA 12.1 support"  
echo "   ✅ ONNX Runtime for neural network inference"
echo "   ✅ SMPLX for humanoid motion processing"
echo "   ✅ Scientific computing packages (NumPy, SciPy, etc.)"
echo "   ✅ Unitree SDK2 Python bindings"
echo "   ✅ ROS2 workspace compiled"
echo ""
echo "📋 To run the system:"
echo "1. Activate the conda environment:"
echo "   conda activate $ENV_NAME"
echo ""
echo "2. Launch the system:"
echo "   cd $PROJECT_ROOT/holomotion/deployment/unitree_g1_ros2_29dof"
echo "   bash launch_holomotion.sh"
echo ""
echo "✅ Environment '$ENV_NAME' setup complete!"
echo "🚀 Ready for robot deployment!"


================================================
FILE: deployment/holomotion_teleop/holomotion_teleop_node.py
================================================
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Single-process teleoperation pipeline.

This node reads raw Pico body tracking data from XRoboToolkit, converts it to
SMPL, applies GMR retargeting, and publishes a 65D observation vector to the
robot over ZMQ.

Data flow:
    xrobotoolkit_sdk (body_poses 24x7)
        -> body_poses_to_smpl_pose_trans
        -> SMPL_Parser / humanoid_fk
        -> GMR
        -> latest_obs(65)
        -> ZMQ PUB

Message format:
    [topic_bytes][1280-byte JSON header][binary payload]

Default payload fields:
    - latest_obs: (65,) float32
    - frame_index: (1,) int64
    - timestamp_realtime: (1,) float64
    - timestamp_monotonic: (1,) float64
    - timestamp_ns: (1,) int64
    - pico_dt: (1,) float32
    - pico_fps: (1,) float32
"""

from __future__ import annotations

import argparse
from collections import defaultdict
from dataclasses import dataclass
import json
import logging
import os
import subprocess
import sys
import threading
import time
import traceback
from types import SimpleNamespace
from typing import Any, Dict, Optional, Tuple

import numpy as np
import torch
import torch.nn.functional as F
import zmq
from scipy.spatial.transform import Rotation as R


FILE_DIR = os.path.dirname(os.path.abspath(__file__))
HOLOMOTION_ROOT_DIR = os.path.abspath(os.path.join(FILE_DIR, "..", ".."))
SMPL_ASSET_DIR = os.path.join(HOLOMOTION_ROOT_DIR, "assets", "smpl")
for extra_path in (
    FILE_DIR,
    os.path.join(FILE_DIR, "GMR"),
    os.path.join(FILE_DIR, "SMPLSim"),
):
    if extra_path not in sys.path:
        sys.path.insert(0, extra_path)


try:
    import xrobotoolkit_sdk as xrt
except ImportError:
    xrt = None

from third_party.GMR.general_motion_retargeting.motion_retarget import GeneralMotionRetargeting as GMR
from smpl_sim.smpllib.smpl_parser import SMPL_Parser


MIRROR_POSE = False
MIRROR_AXIS = "x"
HEADER_SIZE = 1280
OUT_TOPIC = b"obs65"

GMR_LR_SWAP_PAIRS = [
    ("left_hip", "right_hip"),
    ("left_knee", "right_knee"),
    ("left_foot", "right_foot"),
    ("left_shoulder", "right_shoulder"),
    ("left_elbow", "right_elbow"),
    ("left_wrist", "right_wrist"),
]

SMPL_PARENTS_24 = np.array(
    [-1, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 12, 13, 14, 16, 17, 18, 19, 20, 21],
    dtype=np.int32,
)


def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor:
    ret = torch.zeros_like(x)
    positive_mask = x > 0
    ret[positive_mask] = torch.sqrt(x[positive_mask])
    return ret


def matrix_to_quaternion(matrix: torch.Tensor) -> torch.Tensor:
    """
    Convert rotation matrices to quaternions in wxyz format.
    """
    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,
        )
    )

    quat_by_rijk = torch.stack(
        [
            torch.stack([q_abs[..., 0] ** 2, m21 - m12, m02 - m20, m10 - m01], dim=-1),
            torch.stack([m21 - m12, q_abs[..., 1] ** 2, m10 + m01, m02 + m20], dim=-1),
            torch.stack([m02 - m20, m10 + m01, q_abs[..., 2] ** 2, m12 + m21], dim=-1),
            torch.stack([m10 - m01, m20 + m02, m21 + m12, q_abs[..., 3] ** 2], dim=-1),
        ],
        dim=-2,
    )

    floor = torch.tensor(0.1, dtype=q_abs.dtype, device=q_abs.device)
    quat_candidates = quat_by_rijk / (2.0 * q_abs[..., None].max(floor))
    return quat_candidates[
        F.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5,
        :,
    ].reshape(batch_dim + (4,))


def axis_angle_to_matrix(axis_angle: torch.Tensor) -> torch.Tensor:
    """
    Convert axis-angle vectors to rotation matrices.
    Input shape: (..., 3)
    Output shape: (..., 3, 3)
    """
    orig_shape = axis_angle.shape[:-1]
    aa = axis_angle.reshape(-1, 3)

    theta = torch.linalg.norm(aa, dim=-1, keepdim=True)
    axis = aa / torch.clamp(theta, min=1e-8)

    x = axis[:, 0]
    y = axis[:, 1]
    z = axis[:, 2]
    zeros = torch.zeros_like(x)

    K = torch.stack(
        [
            zeros, -z, y,
            z, zeros, -x,
            -y, x, zeros,
        ],
        dim=-1,
    ).reshape(-1, 3, 3)

    eye = torch.eye(3, dtype=aa.dtype, device=aa.device).unsqueeze(0).expand(aa.shape[0], -1, -1)
    sin_theta = torch.sin(theta).unsqueeze(-1)
    cos_theta = torch.cos(theta).unsqueeze(-1)
    axis_outer = axis.unsqueeze(-1) @ axis.unsqueeze(-2)

    small = (theta.squeeze(-1) < 1e-8).unsqueeze(-1).unsqueeze(-1)
    rot = cos_theta * eye + (1.0 - cos_theta) * axis_outer + sin_theta * K
    rot = torch.where(small, eye, rot)
    return rot.reshape(orig_shape + (3, 3))


class Humanoid_Batch_V2:
    """
    Minimal per-frame SMPL kinematics helper used by this script only.
    Keeping it local avoids importing the much larger training/visualization module.
    """

    def __init__(self, device: torch.device = torch.device("cpu")):
        self.device = device
        self.smpl_24_parents = [
            -1, 0, 0, 0, 1, 2, 3,
            4, 5, 6, 7, 8, 9, 9,
            9, 12, 13, 14, 16, 17,
            18, 19, 20, 21,
        ]

    @staticmethod
    def _relative_link_position(joints_world: torch.Tensor, root_pos: torch.Tensor) -> torch.Tensor:
        return joints_world - root_pos.unsqueeze(0)

    def _relative_link_pose(self, full_pose_aa: torch.Tensor) -> torch.Tensor:
        joint_count = full_pose_aa.shape[0]
        assert joint_count == len(self.smpl_24_parents), (
            f"Joint count mismatch: {joint_count} vs {len(self.smpl_24_parents)}"
        )

        rotation_local = axis_angle_to_matrix(full_pose_aa)
        rotation_global = torch.empty_like(rotation_local)
        for joint_idx in range(joint_count):
            parent = self.smpl_24_parents[joint_idx]
            if parent == -1:
                rotation_global[joint_idx] = rotation_local[joint_idx]
            else:
                rotation_global[joint_idx] = rotation_global[parent] @ rotation_local[joint_idx]
        return rotation_global

    def step_per_frame(
        self,
        full_pose_aa: torch.Tensor,
        root_pos: torch.Tensor,
        joints: torch.Tensor,
    ) -> SimpleNamespace:
        global_joints_position = joints
        global_joints2root_pos = self._relative_link_position(joints[1:, :], root_pos)
        global_joints_rotation_mat = self._relative_link_pose(full_pose_aa)

        return SimpleNamespace(
            global_joints2root_pos=global_joints2root_pos,
            global_joints_rotation_mat=global_joints_rotation_mat,
            global_joints_position=global_joints_position,
        )


humanoid_fk = Humanoid_Batch_V2()


@dataclass
class PicoToSmplConfig:
    quat_scalar_first: bool = False
    apply_global_y_180: bool = True
    apply_root_rx90: bool = True
    root_align_degrees: float = 90.0
    root_align_axis: str = "x"


def body_poses_to_smpl_pose_trans(
    body_poses: np.ndarray,
    parents: np.ndarray = SMPL_PARENTS_24,
    cfg: Optional[PicoToSmplConfig] = None,
) -> Tuple[np.ndarray, np.ndarray]:
    if cfg is None:
        cfg = PicoToSmplConfig()

    body_poses = np.asarray(body_poses, dtype=np.float32)
    if body_poses.shape != (24, 7):
        raise ValueError(f"body_poses shape must be (24,7), got {body_poses.shape}")

    positions = body_poses[:, 0:3].astype(np.float32)
    qx, qy, qz, qw = body_poses[:, 3], body_poses[:, 4], body_poses[:, 5], body_poses[:, 6]
    global_quats_sfirst = np.stack([qw, qx, qy, qz], axis=1).astype(np.float32)
    global_rots = R.from_quat(global_quats_sfirst, scalar_first=True)

    if cfg.apply_global_y_180:
        global_rots = global_rots * R.from_euler("y", 180.0, degrees=True)

    local_rots = []
    for i in range(24):
        parent = int(parents[i])
        if parent == -1:
            local_rots.append(global_rots[i])
        else:
            local_rots.append(global_rots[parent].inv() * global_rots[i])

    pose_aa_24x3 = np.stack([rot.as_rotvec() for rot in local_rots], axis=0).astype(np.float32)
    trans = positions[0].astype(np.float32)

    if cfg.apply_root_rx90:
        rot_align = R.from_euler(cfg.root_align_axis, cfg.root_align_degrees, degrees=True).as_matrix().astype(
            np.float32
        )
        root_matrix = R.from_rotvec(pose_aa_24x3[0]).as_matrix().astype(np.float32)
        pose_aa_24x3[0] = R.from_matrix(rot_align @ root_matrix).as_rotvec().astype(np.float32)
        trans = (rot_align @ trans.reshape(3, 1)).reshape(3).astype(np.float32)

    return pose_aa_24x3, trans


def _mirror_matrix(mirror_axis: str) -> np.ndarray:
    if mirror_axis == "x":
        return np.diag([-1.0, 1.0, 1.0]).astype(np.float32)
    if mirror_axis == "y":
        return np.diag([1.0, -1.0, 1.0]).astype(np.float32)
    if mirror_axis == "z":
        return np.diag([1.0, 1.0, -1.0]).astype(np.float32)
    raise ValueError(f"mirror_axis must be one of x/y/z, got {mirror_axis}")


def safe_normalize_quat_wxyz(q: np.ndarray, eps: float = 1e-8) -> np.ndarray:
    q = np.asarray(q, dtype=np.float32).reshape(4,)
    n = float(np.linalg.norm(q))
    if n < eps:
        return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32)
    return (q / n).astype(np.float32)


def mirror_pos_and_quat_wxyz(pos: np.ndarray, quat_wxyz: np.ndarray, mirror_axis: str) -> Tuple[np.ndarray, np.ndarray]:
    pos = np.asarray(pos, dtype=np.float32).reshape(3,)
    q = safe_normalize_quat_wxyz(quat_wxyz)
    M = _mirror_matrix(mirror_axis)

    pos_m = (M @ pos).astype(np.float32)
    q_xyzw = np.array([q[1], q[2], q[3], q[0]], dtype=np.float32)
    rot_m = R.from_quat(q_xyzw).as_matrix().astype(np.float32)
    rot_m = (M @ rot_m @ M).astype(np.float32)
    q_m_xyzw = R.from_matrix(rot_m).as_quat().astype(np.float32)
    quat_m_wxyz = np.array([q_m_xyzw[3], q_m_xyzw[0], q_m_xyzw[1], q_m_xyzw[2]], dtype=np.float32)
    return pos_m, safe_normalize_quat_wxyz(quat_m_wxyz)


def mirror_and_swap_gmr_input(gmr_input: Dict[str, Any], mirror_axis: str = "x") -> Dict[str, Any]:
    mirrored: Dict[str, Any] = {}
    for key, (pos, quat) in gmr_input.items():
        mirrored[key] = mirror_pos_and_quat_wxyz(pos, quat, mirror_axis)

    out = dict(mirrored)
    for a, b in GMR_LR_SWAP_PAIRS:
        if a in out and b in out:
            out[a], out[b] = out[b], out[a]
    return out


def pack_numpy_message(payload: dict, topic: bytes = OUT_TOPIC, version: int = 1) -> bytes:
    fields = []
    binary_data = []

    for key, value in payload.items():
        if not isinstance(value, np.ndarray):
            continue
        if value.dtype == np.float32:
            dtype_str = "f32"
        elif value.dtype == np.float64:
            dtype_str = "f64"
        elif value.dtype == np.int32:
            dtype_str = "i32"
        elif value.dtype == np.int64:
            dtype_str = "i64"
        elif value.dtype == np.uint8:
            dtype_str = "u8"
        elif value.dtype == bool:
            dtype_str = "bool"
        else:
            dtype_str = "f32"
            value = value.astype(np.float32)

        if not value.flags["C_CONTIGUOUS"]:
            value = np.ascontiguousarray(value)
        if value.dtype.byteorder == ">":
            value = value.astype(value.dtype.newbyteorder("<"))

        fields.append({"name": key, "dtype": dtype_str, "shape": list(value.shape)})
        binary_data.append(value.tobytes())

    header = {"v": version, "endian": "le", "count": 1, "fields": fields}
    header_bytes = json.dumps(header, separators=(",", ":")).encode("utf-8")
    if len(header_bytes) > HEADER_SIZE:
        raise ValueError(f"Header too large: {len(header_bytes)} > {HEADER_SIZE}")
    header_bytes = header_bytes.ljust(HEADER_SIZE, b"\x00")
    return topic + header_bytes + b"".join(binary_data)


class PicoReader:
    def __init__(self):
        self._stop = threading.Event()
        self._thread = threading.Thread(target=self._run, daemon=True)
        self._last_stamp_ns = None
        self._fps_ema = 0.0
        self._latest = None
        self._lock = threading.Lock()

    def start(self):
        self._thread.start()

    def stop(self):
        self._stop.set()
        self._thread.join(timeout=1.0)

    def get_latest(self):
        with self._lock:
            return self._latest

    def _run(self):
        last_report = time.time()
        while not self._stop.is_set():
            if not xrt.is_body_data_available():
                time.sleep(0.001)
                continue

            stamp_ns = xrt.get_time_stamp_ns()
            prev_stamp_ns = self._last_stamp_ns
            if prev_stamp_ns is not None and stamp_ns == prev_stamp_ns:
                time.sleep(0.000001)
                continue

            device_dt = ((stamp_ns - prev_stamp_ns) * 1e-9) if prev_stamp_ns is not None else 0.0
            if device_dt > 0.0:
                inst_fps = 1.0 / device_dt
                self._fps_ema = inst_fps if self._fps_ema == 0.0 else (0.9 * self._fps_ema + 0.1 * inst_fps)
            self._last_stamp_ns = stamp_ns

            t_realtime = time.time()
            t_monotonic = time.monotonic()
            try:
                body_poses = xrt.get_body_joints_pose()
                body_poses_np = np.asarray(body_poses, dtype=np.float32)
                if body_poses_np.shape != (24, 7):
                    print(f"[PicoReader] WARNING: unexpected body_poses shape: {body_poses_np.shape}")

                sample = {
                    "body_poses_np": body_poses_np,
                    "timestamp_realtime": t_realtime,
                    "timestamp_monotonic": t_monotonic,
                    "timestamp_ns": int(stamp_ns),
                    "dt": float(device_dt),
                    "fps": float(self._fps_ema),
                }
                with self._lock:
                    self._latest = sample

                now = time.time()
                if now - last_report >= 5.0:
                    print(
                        f"[PicoReader] shape={body_poses_np.shape}, "
                        f"dt_ts={device_dt * 1000.0:.2f} ms, fps={self._fps_ema:.2f}"
                    )
                    last_report = now
            except Exception as exc:
                print(f"[PicoReader] read error: {exc}")


class ZmqObsSender:
    def __init__(self, uri: str, logger, topic: bytes = OUT_TOPIC, mode: str = "bind", conflate: bool = True):
        self.logger = logger
        self.topic = topic
        self._context = zmq.Context()
        self._socket = self._context.socket(zmq.PUB)
        self._socket.setsockopt(zmq.SNDHWM, 1)
        if conflate and hasattr(zmq, "CONFLATE"):
            self._socket.setsockopt(zmq.CONFLATE, 1)

        if mode == "bind":
            self._socket.bind(uri)
        elif mode == "connect":
            self._socket.connect(uri)
        else:
            raise ValueError("mode must be 'bind' or 'connect'")

        self._last_send_time = None
        self._send_freq_log = []
        self._frame_count = 0
        self.logger.info(f"[ZMQOut] sender ready: mode={mode}, uri={uri}, topic={topic.decode('utf-8')}")

    def send(self, latest_obs: np.ndarray, frame_index: int, sample_meta: dict):
        payload = {
            "latest_obs": np.asarray(latest_obs, dtype=np.float32),
            "frame_index": np.array([frame_index], dtype=np.int64),
            "timestamp_realtime": np.array([sample_meta["timestamp_realtime"]], dtype=np.float64),
            "timestamp_monotonic": np.array([sample_meta["timestamp_monotonic"]], dtype=np.float64),
            "timestamp_ns": np.array([sample_meta["timestamp_ns"]], dtype=np.int64),
            "pico_dt": np.array([sample_meta["dt"]], dtype=np.float32),
            "pico_fps": np.array([sample_meta["fps"]], dtype=np.float32),
        }
        packet = pack_numpy_message(payload, topic=self.topic)
        self._socket.send(packet)

        now = time.time()
        if self._last_send_time is not None:
            dt = now - self._last_send_time
            if dt > 0:
                self._send_freq_log.append(1.0 / dt)
                self._frame_count += 1
                if self._frame_count >= 50:
                    avg_freq = sum(self._send_freq_log) / len(self._send_freq_log)
                    self.logger.info(f"Average ZMQ send rate: {avg_freq:.2f} Hz")
                    self._send_freq_log.clear()
                    self._frame_count = 0
        self._last_send_time = now

    def stop(self):
        self._socket.close(0)
        self._context.term()
        self.logger.info("🛑 ZMQ obs sender stopped")


class VRNodeXRTPicoGMRZmqOut:
    def __init__(
        self,
        robot_zmq_uri: str,
        robot_zmq_mode: str = "bind",
        loop_hz: float = 55.0,
        timing_log_every: int = 100,
        save_obs_path: str = "",
    ):
        self.device = "cpu"
        logging.getLogger("websockets").setLevel(logging.WARNING)
        self.info(f"✅ VRNodeXRTPicoGMRZmqOut running on device={self.device}")
        self.info("starting xrt pico -> gmr -> robot zmq node")

        self.gmr = GMR(src_human="smplx", tgt_robot="unitree_g1")
        self.smpl_parser = SMPL_Parser(model_path=SMPL_ASSET_DIR, gender="neutral")
        if hasattr(self.smpl_parser, "to"):
            self.smpl_parser = self.smpl_parser.to(self.device)

        self.betas = torch.zeros(1, 10, device=self.device)
        self.gmr_input_data: Dict[str, Any] = {}
        self.prev_dof_pos = None
        self.lasttime = None
        self.timing_log_every = max(1, timing_log_every)
        self.save_obs_path = save_obs_path
        self.mirror_pose = MIRROR_POSE
        self.mirror_axis = MIRROR_AXIS
        self.tick_count = 0
        self.frame_index = 0
        self.timing_sums_ms = defaultdict(float)
        self.saved_obs = []
        self.latest_sample = None

        self.reader = PicoReader()
        self.reader.start()
        self.sender = ZmqObsSender(uri=robot_zmq_uri, logger=self, mode=robot_zmq_mode)
        self.start_loop(loop_hz)

    def info(self, msg): print(f"[INFO] {msg}")
    def error(self, msg): print(f"[ERROR] {msg}")
    def warning(self, msg): print(f"[WARN] {msg}")
    def debug(self, msg): print(f"[DEBUG] {msg}")

    def _accumulate_timing(self, name: str, start_time: float) -> float:
        elapsed_ms = (time.perf_counter() - start_time) * 1000.0
        self.timing_sums_ms[name] += elapsed_ms
        return elapsed_ms

    def _maybe_log_timing(self):
        if self.tick_count <= 0 or self.tick_count % self.timing_log_every != 0:
            return
        avg_parts = []
        for key in ("body_poses_to_smpl", "smpl_to_joints", "gmr_retarget", "postprocess_send", "tick_total"):
            if key in self.timing_sums_ms:
                avg_ms = self.timing_sums_ms[key] / self.timing_log_every
                avg_parts.append(f"{key}={avg_ms:.2f}ms")
        if avg_parts:
            self.info("[Timing] " + ", ".join(avg_parts))
        self.timing_sums_ms.clear()

    def process_smpl_pose_trans_to_gmr_input(self, smpl_pose_aa, smpl_trans) -> Dict[str, Any]:
        stage_start = time.perf_counter()
        if not isinstance(smpl_pose_aa, torch.Tensor):
            smpl_pose_aa = torch.tensor(smpl_pose_aa, dtype=torch.float32)
        if not isinstance(smpl_trans, torch.Tensor):
            smpl_trans = torch.tensor(smpl_trans, dtype=torch.float32)

        pose = smpl_pose_aa.to(self.device, dtype=torch.float32)
        trans = smpl_trans.to(self.device, dtype=torch.float32)
        if pose.ndim == 2:
            pose = pose.unsqueeze(0)
        if trans.ndim == 1:
            trans = trans.unsqueeze(0)

        verts, joints = self.smpl_parser.get_joints_verts(pose, self.betas, trans)
        # joints[..., 2] -= verts[0, :, 2].min().item()

        pose = pose.squeeze(0)
        trans = trans.squeeze(0)
        joints = joints.squeeze(0)
        motion_state = humanoid_fk.step_per_frame(pose, trans, joints)

        global_joints_position = motion_state.global_joints_position
        global_joints_rotation_mat = motion_state.global_joints_rotation_mat
        global_joints_qua_wxyz = matrix_to_quaternion(global_joints_rotation_mat)

        smpl_to_gmr = {
            "pelvis": 0,
            "spine3": 9,
            "left_hip": 1,
            "right_hip": 2,
            "left_knee": 4,
            "right_knee": 5,
            "left_foot": 10,
            "right_foot": 11,
            "left_shoulder": 16,
            "right_shoulder": 17,
            "left_elbow": 18,
            "right_elbow": 19,
            "left_wrist": 20,
            "right_wrist": 21,
        }

        gmr_input_data: Dict[str, Any] = {}
        for name, idx in smpl_to_gmr.items():
            pos = global_joints_position[idx].detach().cpu().numpy()
            quat = global_joints_qua_wxyz[idx].detach().cpu().numpy()
            gmr_input_data[name] = (pos, quat)

        if self.mirror_pose:
            gmr_input_data = mirror_and_swap_gmr_input(gmr_input_data, mirror_axis=self.mirror_axis)

        self._accumulate_timing("smpl_to_joints", stage_start)
        return gmr_input_data

    def process_xrt_frame_to_gmr_input(self, sample: dict):
        body_poses = np.asarray(sample["body_poses_np"], dtype=np.float32)
        if body_poses.shape != (24, 7):
            raise ValueError(f"[XRT] body_poses_np must have shape (24,7), got {body_poses.shape}")

        stage_start = time.perf_counter()
        pose_aa, trans = body_poses_to_smpl_pose_trans(
            body_poses,
            cfg=PicoToSmplConfig(
                apply_global_y_180=True,
                apply_root_rx90=True,
                root_align_axis="x",
                root_align_degrees=90.0,
            ),
        )
        self._accumulate_timing("body_poses_to_smpl", stage_start)
        self.gmr_input_data = self.process_smpl_pose_trans_to_gmr_input(pose_aa, trans)

    def process_gmr_output(self):
        stage_start = time.perf_counter()
        qpos = self.gmr.retarget(self.gmr_input_data)
        self._accumulate_timing("gmr_retarget", stage_start)

        stage_start = time.perf_counter()
        root_pos = qpos[:3]
        root_rot = qpos[3:7]
        dof_pos = qpos[7:]

        now = time.time()
        delta_time = 1 / 50 if self.lasttime is None else (now - self.lasttime)
        self.lasttime = now

        if self.prev_dof_pos is None:
            dof_vel = np.zeros_like(dof_pos, dtype=np.float32)
        else:
            dof_vel = (dof_pos - self.prev_dof_pos) / max(delta_time, 1e-6)
        self.prev_dof_pos = dof_pos.copy()

        latest_obs = np.concatenate([dof_pos, dof_vel, root_pos, root_rot], axis=0).astype(np.float32)
        self.publish_data(latest_obs)
        self.sender.send(latest_obs, self.frame_index, self.latest_sample)
        self.saved_obs.append(latest_obs.copy())
        self.frame_index += 1
        self._accumulate_timing("postprocess_send", stage_start)
        return latest_obs

    def publish_data(self, motion_state: np.ndarray):
        if motion_state.size != 65:
            self.error(f"Output dim {motion_state.size} != expected 65")
            return
        if np.isnan(motion_state).any():
            self.error("NaN detected")
            return

    def save_observations(self):
        if not self.save_obs_path:
            return
        if len(self.saved_obs) == 0:
            self.warning(f"[SaveObs] no observations to save: {self.save_obs_path}")
            return

        obs_array = np.stack(self.saved_obs, axis=0).astype(np.float32)
        save_dir = os.path.dirname(self.save_obs_path)
        if save_dir:
            os.makedirs(save_dir, exist_ok=True)

        if self.save_obs_path.endswith(".npy"):
            np.save(self.save_obs_path, obs_array)
        else:
            np.savez_compressed(
                self.save_obs_path,
                latest_obs=obs_array,
                columns=np.array(["dof_pos(29)", "dof_vel(29)", "root_pos(3)", "root_rot_wxyz(4)"], dtype=object),
            )
        self.info(f"[SaveObs] saved {obs_array.shape[0]} frames to {self.save_obs_path}")

    def start_loop(self, hz=50):
        self.info(f"Starting main loop at {hz} Hz")
        interval = 1.0 / hz

        def loop():
            next_time = time.time()
            while True:
                self._tick()
                next_time += interval
                sleep_time = next_time - time.time()
                if sleep_time > 0:
                    time.sleep(sleep_time)
                else:
                    next_time = time.time()

        threading.Thread(target=loop, daemon=True).start()

    def _tick(self):
        tick_start = time.perf_counter()
        sample = self.reader.get_latest()
        if sample is not None:
            try:
                self.latest_sample = sample
                self.process_xrt_frame_to_gmr_input(sample)
                self.process_gmr_output()
            except Exception as exc:
                self.error(f"[tick_error] {exc}")
                self.error(traceback.format_exc())
                return
        elif self.prev_dof_pos is not None:
            try:
                self.process_gmr_output()
            except Exception as exc:
                self.error(f"[tick_error] {exc}")
                self.error(traceback.format_exc())
                return

        self.tick_count += 1
        self._accumulate_timing("tick_total", tick_start)
        self._maybe_log_timing()

    def stop(self):
        self.reader.stop()
        self.sender.stop()
        self.save_observations()
        try:
            if xrt is not None and hasattr(xrt, "close"):
                xrt.close()
        except Exception:
            pass


def init_xrt(start_service: bool = True):
    if xrt is None:
        raise ImportError("XRoboToolkit SDK not available. Install xrobotoolkit_sdk first.")
    if start_service:
        subprocess.Popen(["bash", "/opt/apps/roboticsservice/runService.sh"])
    xrt.init()
    print("Waiting for body tracking data...")
    while not xrt.is_body_data_available():
        print("waiting for body data...")
        time.sleep(1)


def main():
    parser = argparse.ArgumentParser(description="XRT Pico -> GMR -> robot ZMQ(65D)")
    parser.add_argument("--robot-zmq-uri", default="tcp://*:6001", help="Robot-side ZMQ uri for 65D obs output")
    parser.add_argument("--robot-zmq-mode", default="bind", choices=["bind", "connect"])
    parser.add_argument("--hz", type=float, default=55.0, help="Main loop frequency / publish cap")
    parser.add_argument("--timing-log-every", type=int, default=200, help="Print average stage timing every N ticks")
    parser.add_argument("--save-obs-path", type=str, default="", help="Optional path to save emitted 65D observations")
    parser.add_argument("--skip-start-service", action="store_true", help="Do not auto-run /opt/apps/roboticsservice
Download .txt
gitextract_3fyqn965/

├── .gitattributes
├── .githooks/
│   ├── README.md
│   └── pre-commit
├── .gitignore
├── .gitlab-ci.yml
├── .gitmodules
├── LICENSE
├── Makefile
├── NOTICE
├── README.md
├── assets/
│   ├── robots/
│   │   └── unitree/
│   │       └── G1/
│   │           └── 29dof/
│   │               ├── g1_29dof.xml
│   │               ├── g1_29dof_rev_1_0.urdf
│   │               ├── g1_29dof_rev_1_0.xml
│   │               ├── g1_29dof_rev_1_0_s100.urdf
│   │               └── scene_29dof.xml
│   └── test_data/
│       └── motion_retargeting/
│           └── ACCAD/
│               └── Male1Walking_c3d/
│                   └── Walk_B10_-_Walk_turn_left_45_stageii.npz
├── deploy.env
├── deployment/
│   ├── deploy_environment.sh
│   ├── holomotion_teleop/
│   │   ├── holomotion_teleop_node.py
│   │   ├── holomotion_teleop_setup.md
│   │   └── setup_holomotion_teleop_x86_ubuntu2204.sh
│   └── unitree_g1_ros2_29dof/
│       ├── launch_holomotion_29dof.sh
│       ├── launch_holomotion_29dof_docker.sh
│       ├── src/
│       │   ├── CMakeLists.txt
│       │   ├── config/
│       │   │   └── g1_29dof_holomotion.yaml
│       │   ├── humanoid_policy/
│       │   │   ├── __init__.py
│       │   │   ├── holomotion_fk_root_only.py
│       │   │   ├── obs_builder/
│       │   │   │   ├── __init__.py
│       │   │   │   └── obs_builder.py
│       │   │   ├── policy_node_29dof.py
│       │   │   └── utils/
│       │   │       ├── __init__.py
│       │   │       ├── command_helper.py
│       │   │       ├── maths.py
│       │   │       ├── motor_crc.py
│       │   │       ├── remote_controller_filter.py
│       │   │       ├── rotation_helper.py
│       │   │       └── rotations.py
│       │   ├── include/
│       │   │   └── common/
│       │   │       ├── motor_crc.h
│       │   │       ├── motor_crc_hg.h
│       │   │       ├── ros2_sport_client.h
│       │   │       └── wireless_controller.h
│       │   ├── launch/
│       │   │   └── holomotion_29dof_launch.py
│       │   ├── models/
│       │   │   └── .gitkeep
│       │   ├── motion_data/
│       │   │   └── .gitkeep
│       │   ├── package.xml
│       │   ├── resource/
│       │   │   └── humanoid_control
│       │   ├── setup.cfg
│       │   ├── setup.py
│       │   └── src/
│       │       ├── common/
│       │       │   ├── motor_crc.cpp
│       │       │   ├── motor_crc_hg.cpp
│       │       │   ├── ros2_sport_client.cpp
│       │       │   └── wireless_controller.cpp
│       │       └── main_node.cpp
│       └── start_container.sh
├── docs/
│   ├── environment_setup.md
│   ├── evaluate_motion_tracking.md
│   ├── holomotion_motion_file_spec.md
│   ├── motion_retargeting.md
│   ├── mujoco_sim2sim.md
│   ├── realworld_deployment.md
│   ├── smpl_data_curation.md
│   └── train_motion_tracking.md
├── environments/
│   ├── environment_deploy.yaml
│   ├── environment_train_isaaclab_cu118.yaml
│   ├── environment_train_isaaclab_cu128.yaml
│   ├── requirements_base.txt
│   ├── requirements_deploy.txt
│   ├── requirements_torch_cu118.txt
│   ├── requirements_torch_cu128.txt
│   └── requirements_torch_cu130.txt
├── holomotion/
│   ├── config/
│   │   ├── algo/
│   │   │   ├── ppo.yaml
│   │   │   └── ppo_tf.yaml
│   │   ├── data_curation/
│   │   │   ├── joints2smpl.yaml
│   │   │   └── smplify_base.yaml
│   │   ├── env/
│   │   │   ├── domain_randomization/
│   │   │   │   ├── NO_domain_rand.yaml
│   │   │   │   ├── domain_rand_medium.yaml
│   │   │   │   ├── domain_rand_small.yaml
│   │   │   │   └── domain_rand_strong.yaml
│   │   │   ├── motion_tracking.yaml
│   │   │   ├── observations/
│   │   │   │   ├── motion_tracking/
│   │   │   │   │   ├── obs_motion_tracking_mlp.yaml
│   │   │   │   │   └── obs_motion_tracking_tf-moe.yaml
│   │   │   │   └── velocity_tracking/
│   │   │   │       └── obs_velocity_tracking.yaml
│   │   │   ├── rewards/
│   │   │   │   ├── motion_tracking/
│   │   │   │   │   └── rew_motion_tracking.yaml
│   │   │   │   └── velocity_tracking/
│   │   │   │       └── rew_velocity_tracking.yaml
│   │   │   ├── terminations/
│   │   │   │   ├── NO_termination.yaml
│   │   │   │   ├── termination_motion_tracking.yaml
│   │   │   │   └── termination_velocity_tracking.yaml
│   │   │   ├── terrain/
│   │   │   │   ├── isaaclab_plane.yaml
│   │   │   │   └── isaaclab_rough.yaml
│   │   │   └── velocity_tracking.yaml
│   │   ├── evaluation/
│   │   │   ├── eval_isaaclab.yaml
│   │   │   ├── eval_mujoco_sim2sim.yaml
│   │   │   └── eval_velocity_tracking.yaml
│   │   ├── modules/
│   │   │   ├── motion_tracking/
│   │   │   │   ├── motion_tracking_mlp.yaml
│   │   │   │   └── motion_tracking_tf-moe.yaml
│   │   │   └── velocity_tracking/
│   │   │       └── velocity_tracking_mlp.yaml
│   │   ├── motion_retargeting/
│   │   │   ├── gmr_to_holomotion.yaml
│   │   │   ├── holomotion_preprocess.yaml
│   │   │   ├── kinematic_filter.yaml
│   │   │   ├── pack_hdf5_database.yaml
│   │   │   ├── pack_hdf5_v2.yaml
│   │   │   └── unitree_G1_29dof_retargeting.yaml
│   │   ├── mujoco_eval/
│   │   │   └── sim2sim.yaml
│   │   ├── robot/
│   │   │   └── unitree/
│   │   │       └── G1/
│   │   │           └── 29dof/
│   │   │               ├── 29dof_training_isaaclab.yaml
│   │   │               └── 29dof_training_isaaclab_s100.yaml
│   │   └── training/
│   │       ├── motion_tracking/
│   │       │   ├── train_g1_29dof_motion_tracking_mlp.yaml
│   │       │   └── train_g1_29dof_motion_tracking_tf-moe.yaml
│   │       ├── train_base.yaml
│   │       └── velocity_tracking/
│   │           └── train_g1_29dof_velocity_tracking_mlp.yaml
│   ├── scripts/
│   │   ├── data_curation/
│   │   │   ├── convert_to_amass.sh
│   │   │   ├── filter_smpl_data.sh
│   │   │   ├── video_to_smpl_gvhmr.sh
│   │   │   └── visualize_smpl_npz.sh
│   │   ├── evaluation/
│   │   │   ├── calc_offline_eval_metrics.sh
│   │   │   ├── eval_motion_tracking.sh
│   │   │   ├── eval_mujoco_sim2sim.sh
│   │   │   ├── eval_velocity_tracking.sh
│   │   │   ├── mean_process_5metrics.py
│   │   │   └── multi_model_metrics_analysis.sh
│   │   ├── motion_retargeting/
│   │   │   ├── apply_gmr_motion_retarget_patch.sh
│   │   │   ├── pack_hdf5_v2.sh
│   │   │   ├── run_holomotion_preprocessing.sh
│   │   │   ├── run_kinematic_filter.sh
│   │   │   ├── run_motion_retargeting_gmr_bvh.sh
│   │   │   ├── run_motion_retargeting_gmr_smplx.sh
│   │   │   ├── run_motion_retargeting_gmr_to_holomotion.sh
│   │   │   └── run_motion_viz_mujoco.sh
│   │   └── training/
│   │       ├── train_motion_tracking.sh
│   │       └── train_velocity_tracking.sh
│   ├── src/
│   │   ├── algo/
│   │   │   ├── __init__.py
│   │   │   ├── algo_base.py
│   │   │   ├── algo_utils.py
│   │   │   ├── ppo.py
│   │   │   └── ppo_tf.py
│   │   ├── data_curation/
│   │   │   ├── .gitignore
│   │   │   ├── __init__.py
│   │   │   ├── data_smplify.py
│   │   │   ├── filter/
│   │   │   │   ├── filter.py
│   │   │   │   └── label_data.py
│   │   │   ├── smpl_npz_to_html.py
│   │   │   ├── smplify/
│   │   │   │   ├── __init__.py
│   │   │   │   ├── smplify_humanact12.py
│   │   │   │   ├── smplify_motionx.py
│   │   │   │   ├── smplify_omomo.py
│   │   │   │   └── smplify_zjumocap.py
│   │   │   ├── templates/
│   │   │   │   └── index_wooden_static.html
│   │   │   ├── video_to_smpl_gvhmr.py
│   │   │   ├── vison_mocap/
│   │   │   │   └── joints2smpl.py
│   │   │   └── visualize_smpl_npz.py
│   │   ├── env/
│   │   │   ├── __init__.py
│   │   │   ├── isaaclab_components/
│   │   │   │   ├── __init__.py
│   │   │   │   ├── isaaclab_actions.py
│   │   │   │   ├── isaaclab_curriculum.py
│   │   │   │   ├── isaaclab_domain_rand.py
│   │   │   │   ├── isaaclab_motion_tracking_command.py
│   │   │   │   ├── isaaclab_observation.py
│   │   │   │   ├── isaaclab_rewards.py
│   │   │   │   ├── isaaclab_scene.py
│   │   │   │   ├── isaaclab_simulator.py
│   │   │   │   ├── isaaclab_termination.py
│   │   │   │   ├── isaaclab_terrain.py
│   │   │   │   ├── isaaclab_utils.py
│   │   │   │   ├── isaaclab_velocity_tracking_command.py
│   │   │   │   └── unitree_actuators.py
│   │   │   ├── motion_tracking.py
│   │   │   └── velocity_tracking.py
│   │   ├── evaluation/
│   │   │   ├── __init__.py
│   │   │   ├── eval_motion_tracking.py
│   │   │   ├── eval_motion_tracking_single.py
│   │   │   ├── eval_mujoco_sim2sim.py
│   │   │   ├── eval_velocity_tracking.py
│   │   │   ├── find_worst_clips.py
│   │   │   ├── metrics.py
│   │   │   ├── multi_model_metrics_report.py
│   │   │   ├── obs/
│   │   │   │   ├── __init__.py
│   │   │   │   └── obs_builder.py
│   │   │   ├── ray_evaluator_actor.py
│   │   │   └── ray_metrics_postprocess.py
│   │   ├── modules/
│   │   │   ├── __init__.py
│   │   │   ├── agent_modules.py
│   │   │   └── network_modules.py
│   │   ├── motion_retargeting/
│   │   │   ├── __init__.py
│   │   │   ├── gmr_to_holomotion.py
│   │   │   ├── holomotion_fk.py
│   │   │   ├── holomotion_preprocess.py
│   │   │   ├── kinematic_filter.py
│   │   │   ├── pack_hdf5_v2.py
│   │   │   ├── reference_filtering.py
│   │   │   └── utils/
│   │   │       ├── __init__.py
│   │   │       ├── _schema.json
│   │   │       ├── rotation_conversions.py
│   │   │       ├── torch_humanoid_batch.py
│   │   │       └── visualize_with_mujoco.py
│   │   ├── training/
│   │   │   ├── __init__.py
│   │   │   ├── h5_dataloader.py
│   │   │   ├── reference_filter_export.py
│   │   │   └── train.py
│   │   └── utils/
│   │       ├── __init__.py
│   │       ├── config.py
│   │       ├── frame_utils.py
│   │       ├── isaac_utils/
│   │       │   ├── __init__.py
│   │       │   ├── maths.py
│   │       │   ├── rotations.py
│   │       │   └── setup.py
│   │       ├── onnx_export.py
│   │       ├── reference_prefix.py
│   │       ├── torch_utils.py
│   │       └── unitree_g1_actuator_calculator.py
│   └── tests/
│       └── __init__.py
├── pyproject.toml
├── tests/
│   ├── benchmark_legacy_onnx_attention.py
│   ├── benchmark_moe_router_export.py
│   ├── test_actor_export_config.py
│   ├── test_algo_base_iteration_logging.py
│   ├── test_build_quantization_dataset.py
│   ├── test_cache_curriculum_sampler.py
│   ├── test_domain_rand_config_builder.py
│   ├── test_eval_mujoco_action_delay.py
│   ├── test_eval_mujoco_action_ema.py
│   ├── test_eval_mujoco_contact_export.py
│   ├── test_eval_mujoco_s100_horizon_ptq.py
│   ├── test_eval_mujoco_use_gpu.py
│   ├── test_eval_onnx_io_dump.py
│   ├── test_evaluation_metrics.py
│   ├── test_isaaclab_termination.py
│   ├── test_mean_process_5metrics.py
│   ├── test_motion_cache_gather_state.py
│   ├── test_motion_cache_startup.py
│   ├── test_motion_tracking_command_reference_prefix.py
│   ├── test_motion_tracking_timing.py
│   ├── test_mujoco_filtered_ref_compat.py
│   ├── test_obs_norm_compile.py
│   ├── test_observation_frames.py
│   ├── test_onnx_attention_export.py
│   ├── test_onnx_export.py
│   ├── test_plot_moe_expert_heatmap.py
│   ├── test_plot_state_series.py
│   ├── test_ppo_checkpoint_sigma_override.py
│   ├── test_ppo_entropy_annealing.py
│   ├── test_ppo_symmetry_loss.py
│   ├── test_ppo_tf_aux_keybody.py
│   ├── test_ref_router_actor.py
│   ├── test_ref_router_seq_actor.py
│   ├── test_reference_filter_export.py
│   ├── test_reference_motion_config_wiring.py
│   ├── test_root_rel_rewards.py
│   ├── test_unitree_actuators.py
│   └── test_visualize_with_mujoco.py
└── train.env
Download .txt
Showing preview only (215K chars total). Download the full file or copy to clipboard to get everything.
SYMBOL INDEX (2254 symbols across 119 files)

FILE: deployment/holomotion_teleop/holomotion_teleop_node.py
  function _sqrt_positive_part (line 95) | def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor:
  function matrix_to_quaternion (line 102) | def matrix_to_quaternion(matrix: torch.Tensor) -> torch.Tensor:
  function axis_angle_to_matrix (line 144) | def axis_angle_to_matrix(axis_angle: torch.Tensor) -> torch.Tensor:
  class Humanoid_Batch_V2 (line 181) | class Humanoid_Batch_V2:
    method __init__ (line 187) | def __init__(self, device: torch.device = torch.device("cpu")):
    method _relative_link_position (line 197) | def _relative_link_position(joints_world: torch.Tensor, root_pos: torc...
    method _relative_link_pose (line 200) | def _relative_link_pose(self, full_pose_aa: torch.Tensor) -> torch.Ten...
    method step_per_frame (line 216) | def step_per_frame(
  class PicoToSmplConfig (line 237) | class PicoToSmplConfig:
  function body_poses_to_smpl_pose_trans (line 245) | def body_poses_to_smpl_pose_trans(
  function _mirror_matrix (line 287) | def _mirror_matrix(mirror_axis: str) -> np.ndarray:
  function safe_normalize_quat_wxyz (line 297) | def safe_normalize_quat_wxyz(q: np.ndarray, eps: float = 1e-8) -> np.nda...
  function mirror_pos_and_quat_wxyz (line 305) | def mirror_pos_and_quat_wxyz(pos: np.ndarray, quat_wxyz: np.ndarray, mir...
  function mirror_and_swap_gmr_input (line 319) | def mirror_and_swap_gmr_input(gmr_input: Dict[str, Any], mirror_axis: st...
  function pack_numpy_message (line 331) | def pack_numpy_message(payload: dict, topic: bytes = OUT_TOPIC, version:...
  class PicoReader (line 370) | class PicoReader:
    method __init__ (line 371) | def __init__(self):
    method start (line 379) | def start(self):
    method stop (line 382) | def stop(self):
    method get_latest (line 386) | def get_latest(self):
    method _run (line 390) | def _run(self):
  class ZmqObsSender (line 439) | class ZmqObsSender:
    method __init__ (line 440) | def __init__(self, uri: str, logger, topic: bytes = OUT_TOPIC, mode: s...
    method send (line 461) | def send(self, latest_obs: np.ndarray, frame_index: int, sample_meta: ...
    method stop (line 487) | def stop(self):
  class VRNodeXRTPicoGMRZmqOut (line 493) | class VRNodeXRTPicoGMRZmqOut:
    method __init__ (line 494) | def __init__(
    method info (line 531) | def info(self, msg): print(f"[INFO] {msg}")
    method error (line 532) | def error(self, msg): print(f"[ERROR] {msg}")
    method warning (line 533) | def warning(self, msg): print(f"[WARN] {msg}")
    method debug (line 534) | def debug(self, msg): print(f"[DEBUG] {msg}")
    method _accumulate_timing (line 536) | def _accumulate_timing(self, name: str, start_time: float) -> float:
    method _maybe_log_timing (line 541) | def _maybe_log_timing(self):
    method process_smpl_pose_trans_to_gmr_input (line 553) | def process_smpl_pose_trans_to_gmr_input(self, smpl_pose_aa, smpl_tran...
    method process_xrt_frame_to_gmr_input (line 608) | def process_xrt_frame_to_gmr_input(self, sample: dict):
    method process_gmr_output (line 626) | def process_gmr_output(self):
    method publish_data (line 654) | def publish_data(self, motion_state: np.ndarray):
    method save_observations (line 662) | def save_observations(self):
    method start_loop (line 684) | def start_loop(self, hz=50):
    method _tick (line 701) | def _tick(self):
    method stop (line 725) | def stop(self):
  function init_xrt (line 736) | def init_xrt(start_service: bool = True):
  function main (line 748) | def main():

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/holomotion_fk_root_only.py
  function _xyzw_to_wxyz (line 28) | def _xyzw_to_wxyz(q: np.ndarray) -> np.ndarray:
  function _wxyz_to_xyzw (line 32) | def _wxyz_to_xyzw(q: np.ndarray) -> np.ndarray:
  function _quat_conjugate_wxyz (line 36) | def _quat_conjugate_wxyz(q: np.ndarray) -> np.ndarray:
  function _quat_mul_wxyz (line 42) | def _quat_mul_wxyz(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
  function _standardize_quaternion_wxyz (line 62) | def _standardize_quaternion_wxyz(q: np.ndarray) -> np.ndarray:
  function _axis_angle_from_wxyz (line 66) | def _axis_angle_from_wxyz(q: np.ndarray) -> np.ndarray:
  function _grad_t (line 84) | def _grad_t(x: np.ndarray, dt: float) -> np.ndarray:
  class HoloMotionFKRootOnly (line 98) | class HoloMotionFKRootOnly(torch.nn.Module):
    method __init__ (line 105) | def __init__(
    method set_timing_logger (line 142) | def set_timing_logger(
    method _timing_ms (line 157) | def _timing_ms(self, t0: float) -> float:
    method _to_numpy (line 160) | def _to_numpy(self, x: torch.Tensor | np.ndarray) -> np.ndarray:
    method _to_output_tensor (line 171) | def _to_output_tensor(self, x: np.ndarray) -> torch.Tensor:
    method _get_gaussian_kernel (line 177) | def _get_gaussian_kernel(self, sigma: float) -> np.ndarray | None:
    method _gaussian_filter_time (line 193) | def _gaussian_filter_time(self, x: np.ndarray, kernel: np.ndarray | No...
    method _log_timing_message (line 205) | def _log_timing_message(self, message: str) -> None:
    method _record_timing (line 211) | def _record_timing(self, sample: Dict[str, float]) -> None:
    method forward (line 269) | def forward(

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/obs_builder/obs_builder.py
  function get_gravity_orientation (line 7) | def get_gravity_orientation(quaternion: np.ndarray) -> np.ndarray:
  class _CircularBuffer (line 28) | class _CircularBuffer:
    method __init__ (line 34) | def __init__(self, max_len: int, feat_dim: int):
    method buffer (line 48) | def buffer(self) -> torch.Tensor:
    method append (line 60) | def append(self, data: torch.Tensor) -> None:
  class PolicyObsBuilder (line 78) | class PolicyObsBuilder:
    method __init__ (line 90) | def __init__(
    method reset (line 124) | def reset(self) -> None:
    method _compute_term (line 130) | def _compute_term(
    method build_policy_obs (line 144) | def build_policy_obs(self) -> np.ndarray:

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/policy_node_29dof.py
  function _parse_cpu_affinity_str (line 51) | def _parse_cpu_affinity_str(s):
  function set_thread_cpu_affinity (line 64) | def set_thread_cpu_affinity(cpu_ids):
  function _decode_zmq_topic (line 109) | def _decode_zmq_topic(topic_value) -> bytes:
  function _coerce_config_bool (line 115) | def _coerce_config_bool(value, default: bool = False) -> bool:
  function _infer_onnx_dim (line 129) | def _infer_onnx_dim(dim, default: int = 1) -> int:
  function _infer_numpy_dtype_from_onnx_type (line 135) | def _infer_numpy_dtype_from_onnx_type(type_str: str):
  function unpack_numpy_message (line 150) | def unpack_numpy_message(packet: bytes, expected_topic: bytes | None = N...
  class LatestObsBuffer (line 192) | class LatestObsBuffer:
    method __init__ (line 195) | def __init__(self, max_queue_size: int = 20):
    method set (line 206) | def set(
    method get_with_age_and_delay (line 224) | def get_with_age_and_delay(self, max_age: float = 0.1, delay_steps: in...
    method get_queue_stats (line 256) | def get_queue_stats(self):
  class ZmqLatestObsSubscriber (line 272) | class ZmqLatestObsSubscriber:
    method __init__ (line 275) | def __init__(
    method _process_packet (line 300) | def _process_packet(self, packet: bytes):
    method _run (line 326) | def _run(self):
    method start (line 384) | def start(self):
    method stop (line 392) | def stop(self):
  class HoloMotionPolicyNode (line 400) | class HoloMotionPolicyNode(Node):
    method __init__ (line 431) | def __init__(self):
    method _is_vr_ready_for_motion (line 650) | def _is_vr_ready_for_motion(self) -> bool:
    method _init_keybody_indices_cache (line 667) | def _init_keybody_indices_cache(self):
    method _get_policy_atomic_obs_list (line 712) | def _get_policy_atomic_obs_list(self, config):
    method _find_actor_place_holder_ndim (line 862) | def _find_actor_place_holder_ndim(self):
    method _init_obs_buffers (line 875) | def _init_obs_buffers(self):
    method _reset_counter (line 931) | def _reset_counter(self):
    method _switch_to_velocity_mode (line 938) | def _switch_to_velocity_mode(self, reason: str = ""):
    method _is_button_pressed (line 961) | def _is_button_pressed(self, button_key):
    method load_policy (line 970) | def load_policy(self):
    method load_model_config (line 1137) | def load_model_config(self):
    method _load_motion_action_ema_filter_cfg (line 1195) | def _load_motion_action_ema_filter_cfg(self) -> None:
    method _reset_motion_action_ema_filter (line 1223) | def _reset_motion_action_ema_filter(self) -> None:
    method _apply_motion_action_ema_filter (line 1226) | def _apply_motion_action_ema_filter(
    method _build_dummy_input_from_onnx_node (line 1245) | def _build_dummy_input_from_onnx_node(self, node, fallback_last_dim: i...
    method _warmup_motion_policy (line 1257) | def _warmup_motion_policy(self, num_iters: int = 2) -> None:
    method update_config_parameters (line 1330) | def update_config_parameters(self):
    method load_motion_data (line 1372) | def load_motion_data(self):
    method _load_current_motion (line 1435) | def _load_current_motion(self):
    method _setup_subscribers (line 1477) | def _setup_subscribers(self):
    method _latest_obs_ros_callback (line 1502) | def _latest_obs_ros_callback(self, msg: Float32MultiArray):
    method _setup_publishers (line 1512) | def _setup_publishers(self):
    method _setup_timers (line 1542) | def _setup_timers(self):
    method _delayed_setup (line 1549) | def _delayed_setup(self):
    method _robot_state_callback (line 1563) | def _robot_state_callback(self, msg: String):
    method robot_root_rot_quat_wxyz (line 1586) | def robot_root_rot_quat_wxyz(self):
    method robot_root_ang_vel (line 1590) | def robot_root_ang_vel(self):
    method robot_dof_pos_by_name (line 1594) | def robot_dof_pos_by_name(self):
    method robot_dof_vel_by_name (line 1604) | def robot_dof_vel_by_name(self):
    method ref_motion_frame_idx (line 1614) | def ref_motion_frame_idx(self):
    method ref_dof_pos_raw (line 1618) | def ref_dof_pos_raw(self):
    method ref_dof_vel_raw (line 1630) | def ref_dof_vel_raw(self):
    method ref_dof_pos_onnx_order (line 1642) | def ref_dof_pos_onnx_order(self):
    method ref_dof_vel_onnx_order (line 1646) | def ref_dof_vel_onnx_order(self):
    method ref_root_pos_raw (line 1650) | def ref_root_pos_raw(self):
    method root_body_idx (line 1665) | def root_body_idx(self):
    method last_valid_ref_motion_frame_idx (line 1669) | def last_valid_ref_motion_frame_idx(self):
    method _xyzw_to_wxyz (line 1673) | def _xyzw_to_wxyz(self, q_xyzw: np.ndarray) -> np.ndarray:
    method _standardize_quaternion_wxyz (line 1683) | def _standardize_quaternion_wxyz(self, q_wxyz: np.ndarray) -> np.ndarray:
    method _quat_rotate_wxyz (line 1692) | def _quat_rotate_wxyz(self, q_wxyz: np.ndarray, v: np.ndarray) -> np.n...
    method _quat_rotate_inv_wxyz (line 1700) | def _quat_rotate_inv_wxyz(self, q_wxyz: np.ndarray, v: np.ndarray) -> ...
    method _quat_rotate_inv_wxyz_single (line 1708) | def _quat_rotate_inv_wxyz_single(
    method _get_future_frame_indices (line 1726) | def _get_future_frame_indices(self) -> np.ndarray:
    method _cache_fk_vr_for_obs (line 1736) | def _cache_fk_vr_for_obs(self):
    method _fill_vr_base_linvel_angvel_fut (line 1770) | def _fill_vr_base_linvel_angvel_fut(self):
    method _prepare_vr_fk_tensors (line 1795) | def _prepare_vr_fk_tensors(
    method _get_future_root_quat_wxyz (line 1832) | def _get_future_root_quat_wxyz(self) -> np.ndarray:
    method _get_ref_keybody_indices (line 1853) | def _get_ref_keybody_indices(self, term_name: str) -> np.ndarray:
    method _get_obs_actor_velocity_command (line 1862) | def _get_obs_actor_velocity_command(self):
    method _get_obs_actor_projected_gravity (line 1865) | def _get_obs_actor_projected_gravity(self):
    method _get_obs_actor_rel_robot_root_ang_vel (line 1868) | def _get_obs_actor_rel_robot_root_ang_vel(self):
    method _get_obs_actor_dof_pos (line 1871) | def _get_obs_actor_dof_pos(self):
    method _get_obs_actor_dof_vel (line 1874) | def _get_obs_actor_dof_vel(self):
    method _get_obs_actor_last_action (line 1877) | def _get_obs_actor_last_action(self):
    method _get_obs_actor_ref_gravity_projection_cur (line 1880) | def _get_obs_actor_ref_gravity_projection_cur(self):
    method _get_obs_actor_ref_gravity_projection_fut (line 1883) | def _get_obs_actor_ref_gravity_projection_fut(self):
    method _get_obs_actor_ref_base_linvel_cur (line 1886) | def _get_obs_actor_ref_base_linvel_cur(self):
    method _get_obs_actor_ref_base_linvel_fut (line 1889) | def _get_obs_actor_ref_base_linvel_fut(self):
    method _get_obs_actor_ref_base_angvel_cur (line 1892) | def _get_obs_actor_ref_base_angvel_cur(self):
    method _get_obs_actor_ref_base_angvel_fut (line 1895) | def _get_obs_actor_ref_base_angvel_fut(self):
    method _get_obs_actor_ref_dof_pos_cur (line 1898) | def _get_obs_actor_ref_dof_pos_cur(self):
    method _get_obs_actor_ref_dof_pos_fut (line 1901) | def _get_obs_actor_ref_dof_pos_fut(self):
    method _get_obs_actor_ref_root_height_cur (line 1904) | def _get_obs_actor_ref_root_height_cur(self):
    method _get_obs_actor_ref_root_height_fut (line 1907) | def _get_obs_actor_ref_root_height_fut(self):
    method _get_obs_actor_ref_keybody_rel_pos_cur (line 1910) | def _get_obs_actor_ref_keybody_rel_pos_cur(self):
    method _get_obs_actor_ref_keybody_rel_pos_fut (line 1913) | def _get_obs_actor_ref_keybody_rel_pos_fut(self):
    method _get_obs_velocity_command (line 1918) | def _get_obs_velocity_command(self):
    method _get_obs_projected_gravity (line 1928) | def _get_obs_projected_gravity(self):
    method _get_obs_rel_robot_root_ang_vel (line 1931) | def _get_obs_rel_robot_root_ang_vel(self):
    method _get_obs_dof_pos (line 1934) | def _get_obs_dof_pos(self):
    method _get_obs_dof_vel (line 1952) | def _get_obs_dof_vel(self):
    method _get_obs_last_action (line 1966) | def _get_obs_last_action(self):
    method _get_obs_ref_motion_states (line 1969) | def _get_obs_ref_motion_states(self):
    method _get_obs_ref_dof_pos_fut (line 1974) | def _get_obs_ref_dof_pos_fut(self):
    method _get_obs_ref_root_height_fut (line 2001) | def _get_obs_ref_root_height_fut(self):
    method _get_obs_ref_root_pos_fut (line 2019) | def _get_obs_ref_root_pos_fut(self):
    method _get_obs_ref_dof_pos_cur (line 2037) | def _get_obs_ref_dof_pos_cur(self):
    method _get_obs_ref_dof_vel_cur (line 2040) | def _get_obs_ref_dof_vel_cur(self):
    method _get_obs_ref_root_height_cur (line 2043) | def _get_obs_ref_root_height_cur(self):
    method _get_obs_ref_root_pos_cur (line 2050) | def _get_obs_ref_root_pos_cur(self):
    method _get_obs_ref_gravity_projection_cur (line 2053) | def _get_obs_ref_gravity_projection_cur(self):
    method _get_obs_ref_gravity_projection_fut (line 2072) | def _get_obs_ref_gravity_projection_fut(self):
    method _get_obs_ref_base_linvel_cur (line 2103) | def _get_obs_ref_base_linvel_cur(self):
    method _get_obs_ref_base_linvel_fut (line 2133) | def _get_obs_ref_base_linvel_fut(self):
    method _get_obs_ref_base_angvel_cur (line 2160) | def _get_obs_ref_base_angvel_cur(self):
    method _get_obs_ref_base_angvel_fut (line 2192) | def _get_obs_ref_base_angvel_fut(self):
    method _get_obs_ref_keybody_rel_pos_cur (line 2219) | def _get_obs_ref_keybody_rel_pos_cur(self):
    method _get_obs_ref_keybody_rel_pos_fut (line 2277) | def _get_obs_ref_keybody_rel_pos_fut(self):
    method _get_obs_place_holder (line 2340) | def _get_obs_place_holder(self):
    method _warmup_fk_for_vr (line 2345) | def _warmup_fk_for_vr(self):
    method _low_state_callback (line 2394) | def _low_state_callback(self, ls_msg: LowState):
    method run (line 2551) | def run(self):
    method _read_onnx_metadata (line 2636) | def _read_onnx_metadata(self, onnx_model_path: str) -> dict:
    method _store_external_latest_obs (line 2655) | def _store_external_latest_obs(self, arr: np.ndarray):
    method _poll_zmq_latest_obs (line 2715) | def _poll_zmq_latest_obs(self):
    method _publish_latest_obs (line 2774) | def _publish_latest_obs(self):
    method _apply_onnx_metadata (line 2785) | def _apply_onnx_metadata(self):
    method _build_dof_mappings (line 2815) | def _build_dof_mappings(self):
    method _publish_control_params (line 2921) | def _publish_control_params(self):
    method _publish_policy_mode (line 2953) | def _publish_policy_mode(self):
    method _timing_ms (line 2962) | def _timing_ms(self, t0: float) -> float:
    method _record_timing_sample (line 2965) | def _record_timing_sample(self, sample: dict):
    method _root_only_fk_has_required_keybodies (line 3020) | def _root_only_fk_has_required_keybodies(self, keybody_idxs: np.ndarra...
    method _run_without_profiling (line 3034) | def _run_without_profiling(self):
    method _process_and_publish_actions (line 3217) | def _process_and_publish_actions(self):
    method setup (line 3231) | def setup(self):
    method _init_fk (line 3252) | def _init_fk(self):
    method destroy_node (line 3295) | def destroy_node(self):
  function get_gravity_orientation (line 3304) | def get_gravity_orientation(quaternion: np.ndarray) -> np.ndarray:
  function main (line 3325) | def main():

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/command_helper.py
  class MotorMode (line 6) | class MotorMode:
  function create_damping_cmd (line 11) | def create_damping_cmd(cmd: Union[LowCmdGo, LowCmdHG]):
  function create_zero_cmd (line 21) | def create_zero_cmd(cmd: Union[LowCmdGo, LowCmdHG]):
  function init_cmd_hg (line 31) | def init_cmd_hg(cmd: LowCmdHG, mode_machine: int, mode_pr: int):
  function init_cmd_go (line 44) | def init_cmd_go(cmd: LowCmdGo, weak_motor: list):

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/maths.py
  function normalize (line 8) | def normalize(x, eps: float = 1e-9):
  function torch_rand_float (line 13) | def torch_rand_float(lower, upper, shape, device):
  function copysign (line 19) | def copysign(a, b):
  function set_seed (line 25) | def set_seed(seed, torch_deterministic=False):
  function to_torch (line 53) | def to_torch(x, dtype=torch.float, device="cuda:0", requires_grad=False):
  function quat_mul_legacy (line 60) | def quat_mul_legacy(
  function quat_mul (line 107) | def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
  function normalize (line 146) | def normalize(x, eps: float = 1e-9):
  function quat_apply (line 151) | def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor:
  function quat_apply_inverse (line 173) | def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.T...
  function quat_rotate (line 195) | def quat_rotate(q, v):
  function quat_rotate_inverse (line 212) | def quat_rotate_inverse(q, v):
  function quat_conjugate (line 229) | def quat_conjugate(a):
  function quat_unit (line 237) | def quat_unit(a):
  function quat_from_angle_axis (line 242) | def quat_from_angle_axis(angle, axis):
  function normalize_angle (line 250) | def normalize_angle(x):
  function get_basis_vector (line 255) | def get_basis_vector(q, v):
  function get_axis_params (line 259) | def get_axis_params(value, axis_idx, x_value=0.0, dtype=np.float64, n_di...
  function get_euler_xyz (line 278) | def get_euler_xyz(q: torch.Tensor) -> tuple:
  function quat_from_euler_xyz (line 312) | def quat_from_euler_xyz(roll, pitch, yaw):
  function torch_rand_float (line 328) | def torch_rand_float(lower, upper, shape, device):
  function torch_random_dir_2 (line 334) | def torch_random_dir_2(shape, device):
  function tensor_clamp (line 340) | def tensor_clamp(t, min_t, max_t):
  function scale (line 345) | def scale(x, lower, upper):
  function unscale (line 350) | def unscale(x, lower, upper):
  function unscale_np (line 354) | def unscale_np(x, lower, upper):
  function quat_to_angle_axis (line 359) | def quat_to_angle_axis(q):
  function angle_axis_to_exp_map (line 382) | def angle_axis_to_exp_map(angle, axis):
  function quat_to_exp_map (line 390) | def quat_to_exp_map(q):
  function slerp (line 399) | def slerp(q0, q1, t):
  function my_quat_rotate (line 425) | def my_quat_rotate(q, v):
  function calc_heading (line 442) | def calc_heading(q):
  function calc_heading_quat (line 456) | def calc_heading_quat(q):
  function calc_heading_quat_inv (line 469) | def calc_heading_quat_inv(q):
  function axis_angle_from_quat (line 482) | def axis_angle_from_quat(
  function quat_inv (line 547) | def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor:
  function xyzw_to_wxyz (line 563) | def xyzw_to_wxyz(q: torch.Tensor) -> torch.Tensor:
  function wxyz_to_xyzw (line 574) | def wxyz_to_xyzw(q: torch.Tensor) -> torch.Tensor:
  function quat_mul_wxyz (line 586) | def quat_mul_wxyz(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
  function subtract_frame_transforms (line 598) | def subtract_frame_transforms(
  function quat_normalize_wxyz (line 636) | def quat_normalize_wxyz(q_wxyz: torch.Tensor) -> torch.Tensor:
  function matrix_from_quat (line 650) | def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor:

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/motor_crc.py
  function crc32_core (line 6) | def crc32_core(data_array, length):
  function calc_crc (line 24) | def calc_crc(cmd) -> int:

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/remote_controller_filter.py
  class KeyMap (line 4) | class KeyMap:
  class RemoteController (line 23) | class RemoteController:
    method __init__ (line 24) | def __init__(self):
    method apply_filter_and_deadzone (line 52) | def apply_filter_and_deadzone(self, value, prev_value):
    method set (line 58) | def set(self, data):
    method get_velocity_commands (line 82) | def get_velocity_commands(self):

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/rotation_helper.py
  function get_gravity_orientation (line 5) | def get_gravity_orientation(quaternion):
  function transform_imu_data (line 20) | def transform_imu_data(waist_yaw, waist_yaw_omega, imu_quat, imu_omega):

FILE: deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/rotations.py
  function quat_unit (line 14) | def quat_unit(a):
  function quat_apply (line 19) | def quat_apply(a: Tensor, b: Tensor, w_last: bool) -> Tensor:
  function quat_apply_yaw (line 34) | def quat_apply_yaw(quat: Tensor, vec: Tensor, w_last: bool) -> Tensor:
  function wrap_to_pi (line 42) | def wrap_to_pi(angles):
  function quat_conjugate (line 49) | def quat_conjugate(a: Tensor, w_last: bool) -> Tensor:
  function quat_apply (line 59) | def quat_apply(a: Tensor, b: Tensor, w_last: bool) -> Tensor:
  function quat_rotate (line 74) | def quat_rotate(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_rotate_inverse (line 95) | def quat_rotate_inverse(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_angle_axis (line 116) | def quat_angle_axis(x: Tensor, w_last: bool) -> Tuple[Tensor, Tensor]:
  function quat_from_angle_axis (line 134) | def quat_from_angle_axis(angle: Tensor, axis: Tensor, w_last: bool) -> T...
  function vec_to_heading (line 145) | def vec_to_heading(h_vec):
  function heading_to_quat (line 151) | def heading_to_quat(h_theta, w_last: bool):
  function quat_axis (line 165) | def quat_axis(q: Tensor, axis: int, w_last: bool) -> Tensor:
  function normalize_angle (line 172) | def normalize_angle(x):
  function get_basis_vector (line 177) | def get_basis_vector(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_to_angle_axis (line 182) | def quat_to_angle_axis(q):
  function slerp (line 207) | def slerp(q0, q1, t):
  function angle_axis_to_exp_map (line 234) | def angle_axis_to_exp_map(angle, axis):
  function my_quat_rotate (line 243) | def my_quat_rotate(q, v):
  function calc_heading (line 260) | def calc_heading(q):
  function quat_to_exp_map (line 275) | def quat_to_exp_map(q):
  function calc_heading_quat (line 285) | def calc_heading_quat(q, w_last):
  function calc_heading_quat_inv (line 299) | def calc_heading_quat_inv(q, w_last):
  function quat_inverse (line 313) | def quat_inverse(x, w_last):
  function get_euler_xyz (line 322) | def get_euler_xyz(q: Tensor, w_last: bool) -> Tuple[Tensor, Tensor, Tens...
  function get_euler_xyz_in_tensor (line 357) | def get_euler_xyz_in_tensor(q):
  function quat_pos (line 389) | def quat_pos(x):
  function is_valid_quat (line 400) | def is_valid_quat(q):
  function quat_normalize (line 406) | def quat_normalize(q):
  function quat_mul (line 415) | def quat_mul(a, b, w_last: bool):
  function quat_mul_norm (line 446) | def quat_mul_norm(x, y, w_last):
  function quat_mul_norm (line 456) | def quat_mul_norm(x, y, w_last):
  function quat_identity (line 466) | def quat_identity(shape: List[int]):
  function quat_identity_like (line 477) | def quat_identity_like(x):
  function transform_from_rotation_translation (line 485) | def transform_from_rotation_translation(
  function transform_rotation (line 503) | def transform_rotation(x):
  function transform_translation (line 509) | def transform_translation(x):
  function transform_mul (line 515) | def transform_mul(x, y):
  function quaternion_to_matrix (line 533) | def quaternion_to_matrix(quaternions: torch.Tensor) -> torch.Tensor:
  function axis_angle_to_quaternion (line 565) | def axis_angle_to_quaternion(axis_angle: torch.Tensor) -> torch.Tensor:
  function wxyz_to_xyzw (line 599) | def wxyz_to_xyzw(quat):
  function xyzw_to_wxyz (line 604) | def xyzw_to_wxyz(quat):
  function matrix_to_quaternion (line 608) | def matrix_to_quaternion(matrix: torch.Tensor) -> torch.Tensor:
  function _sqrt_positive_part (line 672) | def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor:
  function quat_w_first (line 683) | def quat_w_first(rot):
  function quat_from_euler_xyz (line 689) | def quat_from_euler_xyz(roll, pitch, yaw):

FILE: deployment/unitree_g1_ros2_29dof/src/include/common/motor_crc.h
  type BmsCmd (line 39) | typedef struct
  type MotorCmd (line 47) | typedef struct
  type LowCmd (line 60) | typedef struct

FILE: deployment/unitree_g1_ros2_29dof/src/include/common/motor_crc_hg.h
  type MotorCmd (line 14) | typedef struct
  type LowCmd (line 25) | typedef struct

FILE: deployment/unitree_g1_ros2_29dof/src/include/common/ros2_sport_client.h
  type PathPoint (line 42) | typedef struct
  function class (line 53) | class SportClient

FILE: deployment/unitree_g1_ros2_29dof/src/include/common/wireless_controller.h
  function class (line 7) | class KeyMap {
  function class (line 27) | class RemoteController {

FILE: deployment/unitree_g1_ros2_29dof/src/launch/holomotion_29dof_launch.py
  function generate_launch_description (line 35) | def generate_launch_description():

FILE: deployment/unitree_g1_ros2_29dof/src/src/common/motor_crc.cpp
  function get_crc (line 4) | void get_crc(unitree_go::msg::LowCmd& msg)
  function crc32_core (line 49) | uint32_t crc32_core(uint32_t* ptr, uint32_t len)

FILE: deployment/unitree_g1_ros2_29dof/src/src/common/motor_crc_hg.cpp
  function get_crc (line 3) | void get_crc(unitree_hg::msg::LowCmd &msg)
  function crc32_core (line 28) | uint32_t crc32_core(uint32_t *ptr, uint32_t len)

FILE: deployment/unitree_g1_ros2_29dof/src/src/main_node.cpp
  type PRorAB (line 25) | enum PRorAB { PR = 0, AB = 1 }
  type RobotState (line 31) | enum class RobotState { ZERO_TORQUE, MOVE_TO_DEFAULT, EMERGENCY_STOP, PO...
  type EmergencyStopPhase (line 32) | enum class EmergencyStopPhase { DAMPING, DISABLE }
  class humanoid_controller (line 35) | class humanoid_controller : public rclcpp::Node {
    method humanoid_controller (line 37) | humanoid_controller() : Node("humanoid_controller") {
    method calculateExpectedTorque (line 134) | double calculateExpectedTorque(const std::string& dof_name, double q_d...
    method limitTorque (line 142) | std::pair<double, double> limitTorque(const std::string& dof_name, dou...
    method limitTorqueWithCustomGains (line 166) | std::pair<double, double> limitTorqueWithCustomGains(
    method loadConfig (line 193) | void loadConfig(const std::string &config_path) {
    method Control (line 327) | void Control() {
    method SendZeroTorqueCommand (line 472) | void SendZeroTorqueCommand() {
    method SendDefaultPositionCommand (line 486) | void SendDefaultPositionCommand() {
    method SendPolicyCommand (line 571) | void SendPolicyCommand() {
    method SendDampedEmergencyStop (line 622) | void SendDampedEmergencyStop() {
    method SendFinalEmergencyStop (line 640) | void SendFinalEmergencyStop() {
    method LowStateHandler (line 654) | void LowStateHandler(unitree_hg::msg::LowState::SharedPtr message) {
    method PolicyActionHandler (line 677) | void PolicyActionHandler(
    method KpsHandler (line 725) | void KpsHandler(const std_msgs::msg::Float32MultiArray::SharedPtr mess...
    method KdsHandler (line 749) | void KdsHandler(const std_msgs::msg::Float32MultiArray::SharedPtr mess...
    method clamp (line 773) | double clamp(double value, double low, double high) {
    method robotStateToString (line 781) | std::string robotStateToString(RobotState state) {
    method publishRobotState (line 796) | void publishRobotState() {
  function main (line 828) | int main(int argc, char **argv) {

FILE: holomotion/scripts/evaluation/mean_process_5metrics.py
  function get_dataset_name (line 73) | def get_dataset_name(motion_key):
  function process_data (line 88) | def process_data(folder_path):
  function highlight_best (line 165) | def highlight_best(val, best_val):
  function generate_report (line 178) | def generate_report(
  function _format_metric_values_for_cli (line 236) | def _format_metric_values_for_cli(sub_df: pd.DataFrame) -> pd.DataFrame:
  function _print_cli_tables (line 246) | def _print_cli_tables(df: pd.DataFrame, title: str, folder_path: str) ->...
  function generate_macro_mean_report_from_json_dir (line 302) | def generate_macro_mean_report_from_json_dir(folder_path: str) -> str:

FILE: holomotion/src/algo/algo_base.py
  class BaseOnpolicyRL (line 41) | class BaseOnpolicyRL:
    method __init__ (line 44) | def __init__(
    method _setup_accelerator (line 71) | def _setup_accelerator(self) -> None:
    method _setup_environment (line 208) | def _setup_environment(self) -> None:
    method _setup_configs (line 283) | def _setup_configs(self) -> None:
    method _setup_seeding (line 305) | def _setup_seeding(self) -> None:
    method _setup_data_buffers (line 323) | def _setup_data_buffers(self) -> None:
    method _setup_algo_components (line 353) | def _setup_algo_components(self) -> None:
    method _setup_models_and_optimizer (line 357) | def _setup_models_and_optimizer(self) -> None:
    method _build_storage (line 362) | def _build_storage(self, obs_td: TensorDict) -> Any:
    method _post_env_step_hook (line 366) | def _post_env_step_hook(
    method _post_update_hook (line 381) | def _post_update_hook(self, loss_dict: Dict[str, Any]) -> None:
    method _extra_checkpoint_state (line 385) | def _extra_checkpoint_state(self) -> Dict[str, Any]:
    method _load_extra_checkpoint_state (line 389) | def _load_extra_checkpoint_state(
    method _build_transition (line 395) | def _build_transition(
    method _post_iteration_hook (line 405) | def _post_iteration_hook(self, it: int) -> None:
    method _post_training_hook (line 408) | def _post_training_hook(self) -> None:
    method _release_cuda_cache (line 411) | def _release_cuda_cache(self) -> None:
    method _get_additional_log_metrics (line 415) | def _get_additional_log_metrics(self) -> Dict[str, Any]:
    method train_mode (line 418) | def train_mode(self) -> None:
    method _ensure_storage (line 422) | def _ensure_storage(self, obs_td: TensorDict) -> None:
    method _reset_rollout_forward_state (line 431) | def _reset_rollout_forward_state(self) -> None:
    method _rollout_forward (line 435) | def _rollout_forward(
    method _track_episode_stats (line 482) | def _track_episode_stats(
    method _compute_returns (line 510) | def _compute_returns(self, obs_td: TensorDict) -> None:
    method rollout_policy (line 533) | def rollout_policy(self, obs_td: TensorDict) -> TensorDict:
    method learn (line 550) | def learn(self):
    method process_env_step (line 620) | def process_env_step(
    method _wrap_obs_dict (line 651) | def _wrap_obs_dict(self, obs_dict: dict) -> TensorDict:
    method _clean_state_dict (line 660) | def _clean_state_dict(state_dict: Dict[str, Any]) -> Dict[str, Any]:
    method _load_model_state (line 677) | def _load_model_state(self, model, state_dict, *, strict: bool = True):
    method _resolve_model_file_path (line 692) | def _resolve_model_file_path(self, ckpt_path: str, model_name: str) ->...
    method _load_accelerate_model (line 703) | def _load_accelerate_model(
    method _aggregate_episode_log_metrics (line 722) | def _aggregate_episode_log_metrics(
    method _log_value_to_cpu_tensor (line 754) | def _log_value_to_cpu_tensor(value: Any) -> torch.Tensor | None:
    method _log_iteration (line 766) | def _log_iteration(
    method load (line 841) | def load(self, ckpt_path):
    method save (line 844) | def save(self, path, infos=None):

FILE: holomotion/src/algo/algo_utils.py
  class AlgoLogger (line 29) | class AlgoLogger:
    method __init__ (line 30) | def __init__(
    method _is_scalar_metric (line 42) | def _is_scalar_metric(value: Any) -> bool:
    method _to_scalar (line 50) | def _to_scalar(value: Any) -> float:
    method _format_console_value (line 56) | def _format_console_value(value: Any) -> str:
    method _build_console_log (line 71) | def _build_console_log(
    method log_iteration (line 105) | def log_iteration(
  class PpoTransition (line 139) | class PpoTransition:
  class PpoVelocityTransition (line 182) | class PpoVelocityTransition:
  class PpoAuxTransition (line 228) | class PpoAuxTransition:
  class RolloutStorage (line 291) | class RolloutStorage(nn.Module):
    method __init__ (line 294) | def __init__(
    method _resolve_shape (line 326) | def _resolve_shape(self, spec_shape, actions_shape) -> tuple:
    method _allocate_from_transition (line 340) | def _allocate_from_transition(
    method _to_storage_tensor (line 388) | def _to_storage_tensor(self, tensor: torch.Tensor) -> torch.Tensor:
    method add (line 400) | def add(self, transition: PpoTransition) -> None:
    method clear (line 425) | def clear(self) -> None:
    method compute_returns (line 428) | def compute_returns(
    method normalize_advantages_global (line 463) | def normalize_advantages_global(
    method normalize_advantages_global_by_move_mask (line 498) | def normalize_advantages_global_by_move_mask(
    method normalize_advantages_global_by_command (line 582) | def normalize_advantages_global_by_command(
    method iter_minibatches (line 600) | def iter_minibatches(
    method resolve_mini_batch_partition (line 634) | def resolve_mini_batch_partition(

FILE: holomotion/src/algo/ppo.py
  function _checkpoint_state_to_cpu (line 44) | def _checkpoint_state_to_cpu(value):
  class PPO (line 56) | class PPO(BaseOnpolicyRL):
    method _setup_configs (line 57) | def _setup_configs(self):
    method _resolve_num_mini_batches (line 234) | def _resolve_num_mini_batches(self, base_num_mini_batches: int) -> int:
    method _compute_lr_scale_factor (line 239) | def _compute_lr_scale_factor(self, lr_scale_cfg) -> float:
    method _symmetry_loss_active (line 277) | def _symmetry_loss_active(self) -> bool:
    method _omega_or_obj_to_dict (line 285) | def _omega_or_obj_to_dict(value):
    method _setup_symmetry (line 296) | def _setup_symmetry(self) -> None:
    method _extract_obs_mirror_metadata (line 342) | def _extract_obs_mirror_metadata(self) -> dict[str, dict]:
    method _get_actor_schema_terms (line 368) | def _get_actor_schema_terms(self) -> set[str]:
    method _build_obs_mirror_map (line 384) | def _build_obs_mirror_map(self) -> None:
    method _td_key_to_path (line 432) | def _td_key_to_path(key) -> str:
    method _mirror_actor_obs (line 437) | def _mirror_actor_obs(self, obs_td: TensorDict) -> TensorDict:
    method _mirror_env_action (line 462) | def _mirror_env_action(self, actions: torch.Tensor) -> torch.Tensor:
    method _compute_analytic_kl (line 483) | def _compute_analytic_kl(
    method _compute_clip_fraction (line 516) | def _compute_clip_fraction(
    method _compute_explained_variance (line 545) | def _compute_explained_variance(
    method _set_optimizer_learning_rates (line 591) | def _set_optimizer_learning_rates(self) -> None:
    method _validate_entropy_schedule_config (line 598) | def _validate_entropy_schedule_config(
    method _get_effective_entropy_coef (line 612) | def _get_effective_entropy_coef(self) -> float:
    method _apply_adaptive_lr (line 634) | def _apply_adaptive_lr(self, kl_signal: float | None) -> None:
    method _compute_windowed_kl_signal (line 666) | def _compute_windowed_kl_signal(
    method _should_early_stop_for_kl (line 674) | def _should_early_stop_for_kl(
    method _setup_data_buffers (line 690) | def _setup_data_buffers(self):
    method _build_optimizer_kwargs (line 702) | def _build_optimizer_kwargs(self, optimizer_class: type) -> dict:
    method _setup_models_and_optimizer (line 723) | def _setup_models_and_optimizer(self):
    method _build_storage (line 826) | def _build_storage(self, obs_td: TensorDict):
    method _build_transition (line 837) | def _build_transition(
    method _post_iteration_hook (line 896) | def _post_iteration_hook(self, it: int) -> None:
    method _post_training_hook (line 903) | def _post_training_hook(self) -> None:
    method _get_mean_policy_std (line 909) | def _get_mean_policy_std(self) -> torch.Tensor:
    method _maybe_override_loaded_actor_sigma (line 917) | def _maybe_override_loaded_actor_sigma(self) -> None:
    method _get_additional_log_metrics (line 945) | def _get_additional_log_metrics(self) -> dict[str, float]:
    method update (line 990) | def update(self):
    method load (line 1244) | def load(self, ckpt_path):
    method _restore_optimizer_state (line 1278) | def _restore_optimizer_state(
    method _optimizer_state_is_compatible (line 1309) | def _optimizer_state_is_compatible(
    method save (line 1340) | def save(self, path, infos=None):
    method offline_evaluate_policy (line 1375) | def offline_evaluate_policy(self, dump_npzs: bool = False):

FILE: holomotion/src/algo/ppo_tf.py
  class PPOTF (line 42) | class PPOTF(PPO):
    method _select_actor_wrapper_cls (line 46) | def _select_actor_wrapper_cls(actor_cfg: dict):
    method _summarize_moe_layer_stats (line 77) | def _summarize_moe_layer_stats(moe_layers) -> dict[str, float | None]:
    method _setup_configs (line 108) | def _setup_configs(self):
    method _unwrap_obs_schema (line 451) | def _unwrap_obs_schema(schema: dict | None) -> dict | None:
    method _schema_term_leaf_name (line 466) | def _schema_term_leaf_name(term: str) -> str:
    method _is_aux_command_term (line 470) | def _is_aux_command_term(cls, term: str, term_prefix: str) -> bool:
    method _build_aux_router_command_recon_schema (line 474) | def _build_aux_router_command_recon_schema(
    method _masked_aux_keybody_mse (line 497) | def _masked_aux_keybody_mse(
    method _masked_aux_mse (line 524) | def _masked_aux_mse(
    method _masked_adjacent_router_js (line 551) | def _masked_adjacent_router_js(
    method _masked_adjacent_router_normed_smooth_l1 (line 613) | def _masked_adjacent_router_normed_smooth_l1(
    method _masked_aux_gaussian_nll (line 678) | def _masked_aux_gaussian_nll(
    method _masked_aux_huber (line 724) | def _masked_aux_huber(
    method _compute_aux_router_future_recon_loss (line 753) | def _compute_aux_router_future_recon_loss(
    method _compute_routed_expert_orthogonal_loss (line 782) | def _compute_routed_expert_orthogonal_loss(
    method _root_relative_body_pos_from_mixed_position_frames (line 829) | def _root_relative_body_pos_from_mixed_position_frames(
    method _setup_models_and_optimizer (line 886) | def _setup_models_and_optimizer(self):
    method _setup_data_buffers (line 1148) | def _setup_data_buffers(self):
    method _build_transition (line 1218) | def _build_transition(
    method _build_storage (line 1393) | def _build_storage(self, obs_td: TensorDict):
    method _sample_iteration_future_masks (line 1415) | def _sample_iteration_future_masks(self) -> torch.Tensor | None:
    method _reset_rollout_forward_state (line 1469) | def _reset_rollout_forward_state(self) -> None:
    method _rollout_forward (line 1479) | def _rollout_forward(
    method process_env_step (line 1523) | def process_env_step(
    method _build_episode_causal_mask (line 1537) | def _build_episode_causal_mask(dones_seq: torch.Tensor) -> torch.Tensor:
    method _resolve_sequence_batch_partition (line 1548) | def _resolve_sequence_batch_partition(
    method _sequence_batches (line 1567) | def _sequence_batches(
    method update (line 1704) | def update(self):

FILE: holomotion/src/data_curation/data_smplify.py
  function ensure_dir (line 29) | def ensure_dir(path):
  function main (line 40) | def main():

FILE: holomotion/src/data_curation/filter/filter.py
  function checksitpose (line 24) | def checksitpose(
  function process_dataset (line 62) | def process_dataset(
  function jsonl_to_yaml (line 177) | def jsonl_to_yaml(jsonl_path, yaml_output_path):

FILE: holomotion/src/data_curation/filter/label_data.py
  function calc_max_xy_translation (line 29) | def calc_max_xy_translation(motion_data: dict):
  function calc_max_z_translation (line 42) | def calc_max_z_translation(motion_data: dict):
  function calc_max_velocity_scale (line 55) | def calc_max_velocity_scale(motion_data: dict, fps: float = 30):
  function calc_mean_velocity_scale (line 64) | def calc_mean_velocity_scale(motion_data: dict, fps: float = 30):
  function calc_std_velocity_scale (line 73) | def calc_std_velocity_scale(motion_data: dict, fps: float = 30):
  function calc_max_vxy_scale (line 82) | def calc_max_vxy_scale(motion_data: dict, fps: float = 30):
  function calc_std_accel (line 93) | def calc_std_accel(motion_data: dict, fps: float = 30.0) -> float:
  function calc_max_vz_scale (line 130) | def calc_max_vz_scale(motion_data: dict, fps: float = 30):
  function calc_vz_scale_with_direction (line 141) | def calc_vz_scale_with_direction(motion_data: dict, fps: float = 30):
  function beyond_upper_dof_limits (line 155) | def beyond_upper_dof_limits(
  class HyperParams (line 175) | class HyperParams:
  function label_data_with_metrics (line 203) | def label_data_with_metrics(data_folder, jsonl_path: str, parent_folder:...
  function main (line 285) | def main():

FILE: holomotion/src/data_curation/smpl_npz_to_html.py
  class SmplSequence (line 45) | class SmplSequence:
  function parse_args (line 55) | def parse_args() -> argparse.Namespace:
  function euler_fix_rot (line 99) | def euler_fix_rot(euler_deg=EULER_FIX_DEG, order=EULER_ORDER) -> R:
  function _require_key (line 104) | def _require_key(data: np.lib.npyio.NpzFile, key: str) -> np.ndarray:
  function load_npz (line 112) | def load_npz(path: Path) -> SmplSequence:
  function validate_sequence (line 130) | def validate_sequence(seq: SmplSequence, pose_joints: int) -> int:
  function build_smpl_frames (line 152) | def build_smpl_frames(
  function render_html (line 202) | def render_html(template_path: Path, frames: list, T: int, fps: float) -...
  function main (line 217) | def main(

FILE: holomotion/src/data_curation/smplify/smplify_humanact12.py
  function joints2smpl (line 41) | def joints2smpl(file_name, data_dir, save_dir):
  function humanact12_to_amass (line 169) | def humanact12_to_amass(data_dir, save_dir):

FILE: holomotion/src/data_curation/smplify/smplify_motionx.py
  function motionx_to_amass (line 23) | def motionx_to_amass(src_root, dst_root):

FILE: holomotion/src/data_curation/smplify/smplify_omomo.py
  class MyHandFootManipDataset (line 35) | class MyHandFootManipDataset(HandFootManipDataset):
    method __init__ (line 42) | def __init__(self, *args, **kwargs):
    method __getitem__ (line 56) | def __getitem__(self, index):
  function run_smplx_model (line 175) | def run_smplx_model(root_trans, aa_rot_rep, betas, gender, fname):
  function process_dataset (line 250) | def process_dataset(dl, dataset, target_folder, split_name: str):
  function omomo_to_amass (line 322) | def omomo_to_amass(data_root_folder, target_folder):

FILE: holomotion/src/data_curation/smplify/smplify_zjumocap.py
  function zju_single_to_amass (line 24) | def zju_single_to_amass(
  function zju_to_amass (line 94) | def zju_to_amass(input_dir, output_dir):

FILE: holomotion/src/data_curation/video_to_smpl_gvhmr.py
  function get_video_fps (line 76) | def get_video_fps(video_path: Path) -> float:
  function is_close_fps (line 85) | def is_close_fps(a: float, b: float, tol: float = 0.02) -> bool:
  function transcode_to_30fps_cfr (line 89) | def transcode_to_30fps_cfr(src: Path, dst: Path, crf: int) -> None:
  function parse_args_to_cfg (line 113) | def parse_args_to_cfg(args=None):
  function run_preprocess (line 208) | def run_preprocess(cfg):
  function load_data_dict (line 299) | def load_data_dict(cfg):
  function save_npz (line 329) | def save_npz(pred, save_path):
  function render_incam (line 362) | def render_incam(cfg):
  function render_global (line 413) | def render_global(cfg):

FILE: holomotion/src/data_curation/vison_mocap/joints2smpl.py
  function joints2smpl (line 40) | def joints2smpl(input_joints, save_name):

FILE: holomotion/src/data_curation/visualize_smpl_npz.py
  class AppConfig (line 112) | class AppConfig:
  function parse_args (line 125) | def parse_args() -> argparse.Namespace:
  function js_escape (line 170) | def js_escape(s: str) -> str:
  function ensure_exists (line 174) | def ensure_exists(path: Path, what: str) -> None:
  function is_port_available (line 179) | def is_port_available(port: int) -> bool:
  class StaticServer (line 192) | class StaticServer:
    method __init__ (line 193) | def __init__(self, root: Path, port: int):
    method start (line 198) | def start(self) -> None:
  class MakeVisRunner (line 211) | class MakeVisRunner:
    method __init__ (line 212) | def __init__(
    method run (line 224) | def run(self, npz_path: Path) -> None:
  function pick_npz_dialog (line 244) | def pick_npz_dialog(window) -> Optional[Path]:
  class UIAPI (line 261) | class UIAPI:
    method __init__ (line 262) | def __init__(self, window, cfg: AppConfig, runner: MakeVisRunner):
    method pick_and_generate (line 268) | def pick_and_generate(self) -> None:
    method auto_pick_once (line 300) | def auto_pick_once(self) -> None:
  function build_config (line 312) | def build_config(args: argparse.Namespace) -> AppConfig:
  function main (line 342) | def main() -> None:

FILE: holomotion/src/env/isaaclab_components/isaaclab_actions.py
  class ActionFunctions (line 22) | class ActionFunctions:
    method joint_position_action (line 26) | def joint_position_action(
    method joint_velocity_action (line 43) | def joint_velocity_action(
    method joint_effort_action (line 58) | def joint_effort_action(
  class ActionsCfg (line 74) | class ActionsCfg:
  function build_actions_config (line 80) | def build_actions_config(actions_config_dict: dict) -> ActionsCfg:

FILE: holomotion/src/env/isaaclab_components/isaaclab_curriculum.py
  function _completion_rate_curriculum_get_level (line 31) | def _completion_rate_curriculum_get_level(
  function lin_vel_cmd_levels (line 130) | def lin_vel_cmd_levels(
  function ang_vel_cmd_levels (line 164) | def ang_vel_cmd_levels(
  function robot_friction_range_by_completion_rate (line 192) | def robot_friction_range_by_completion_rate(
  function rigid_body_com_by_completion_rate (line 288) | def rigid_body_com_by_completion_rate(
  function default_dof_pos_bias_by_completion_rate (line 357) | def default_dof_pos_bias_by_completion_rate(
  function push_by_setting_velocity_range_by_completion_rate (line 424) | def push_by_setting_velocity_range_by_completion_rate(
  function randomize_actuator_gains_by_completion_rate (line 498) | def randomize_actuator_gains_by_completion_rate(
  function reward_term_weight_by_completion_rate (line 575) | def reward_term_weight_by_completion_rate(
  class CurriculumCfg (line 627) | class CurriculumCfg:
  function build_curriculum_config (line 631) | def build_curriculum_config(curriculum_config_dict: dict) -> CurriculumCfg:

FILE: holomotion/src/env/isaaclab_components/isaaclab_domain_rand.py
  class DomainRandFunctions (line 34) | class DomainRandFunctions:
    method _get_dr_default_dof_pos_bias (line 36) | def _get_dr_default_dof_pos_bias(
    method _get_dr_rigid_body_com (line 85) | def _get_dr_rigid_body_com(
    method _get_dr_rigid_body_material (line 102) | def _get_dr_rigid_body_material(
    method _get_dr_push_by_setting_velocity (line 130) | def _get_dr_push_by_setting_velocity(
    method _get_dr_randomize_actuator_gains (line 142) | def _get_dr_randomize_actuator_gains(
    method _get_dr_randomize_mass (line 167) | def _get_dr_randomize_mass(
  class EventsCfg (line 186) | class EventsCfg:
  function build_domain_rand_config (line 190) | def build_domain_rand_config(domain_rand_config_dict: dict) -> EventsCfg:

FILE: holomotion/src/env/isaaclab_components/isaaclab_motion_tracking_command.py
  class RefMotionCommand (line 92) | class RefMotionCommand(CommandTerm):
    method __init__ (line 95) | def __init__(
    method _init_tracking_config (line 113) | def _init_tracking_config(self, config):
    method _amp_filter_names_by_prefix (line 148) | def _amp_filter_names_by_prefix(
    method _amp_pick_first_name (line 158) | def _amp_pick_first_name(
    method _resolve_motion_cache_stage_device (line 167) | def _resolve_motion_cache_stage_device(
    method _init_motion_lib (line 216) | def _init_motion_lib(self):
    method setup_dumping_dir (line 669) | def setup_dumping_dir(self, log_dir: str):
    method set_runtime_distributed_context (line 699) | def set_runtime_distributed_context(
    method set_motion_cache_seed (line 705) | def set_motion_cache_seed(
    method close (line 712) | def close(self) -> None:
    method _init_per_env_cache (line 718) | def _init_per_env_cache(self):
    method _maybe_dump_sampled_motion_keys (line 743) | def _maybe_dump_sampled_motion_keys(self) -> None:
    method _init_robot_handle (line 797) | def _init_robot_handle(self):
    method _init_buffers (line 1017) | def _init_buffers(self):
    method _record_completion_rate_for_envs (line 1090) | def _record_completion_rate_for_envs(self, env_ids: torch.Tensor) -> N...
    method _reset_window_curriculum_stats (line 1144) | def _reset_window_curriculum_stats(self) -> None:
    method _build_window_curriculum_stats_from_current_batch (line 1150) | def _build_window_curriculum_stats_from_current_batch(
    method _update_cache_curriculum_state (line 1192) | def _update_cache_curriculum_state(
    method update_curriculum_reward_accumulators (line 1229) | def update_curriculum_reward_accumulators(
    method command (line 1251) | def command(
    method command_fut (line 1258) | def command_fut(
    method reset (line 1264) | def reset(
    method apply_cache_swap_if_pending_barrier (line 1289) | def apply_cache_swap_if_pending_barrier(self, accelerator=None) -> bool:
    method compute (line 1344) | def compute(self, dt: float):
    method _update_ref_motion_state (line 1354) | def _update_ref_motion_state(self):
    method _update_ref_motion_state_from_cache (line 1358) | def _update_ref_motion_state_from_cache(
    method _get_ref_state_array (line 1365) | def _get_ref_state_array(
    method get_ref_motion_filter_cutoff_hz_cur (line 1393) | def get_ref_motion_filter_cutoff_hz_cur(self) -> torch.Tensor:
    method _uniform_sample_ref_start_frames (line 1405) | def _uniform_sample_ref_start_frames(self, env_ids: torch.Tensor):
    method get_ref_motion_dof_pos_fut (line 1438) | def get_ref_motion_dof_pos_fut(
    method _get_immediate_next_ref_state_array (line 1445) | def _get_immediate_next_ref_state_array(
    method get_ref_motion_dof_vel_fut (line 1457) | def get_ref_motion_dof_vel_fut(
    method get_ref_motion_root_global_pos_fut (line 1464) | def get_ref_motion_root_global_pos_fut(
    method get_ref_motion_root_global_rot_quat_xyzw_fut (line 1471) | def get_ref_motion_root_global_rot_quat_xyzw_fut(
    method get_ref_motion_root_global_rot_quat_wxyz_fut (line 1477) | def get_ref_motion_root_global_rot_quat_wxyz_fut(
    method get_ref_motion_root_global_lin_vel_fut (line 1485) | def get_ref_motion_root_global_lin_vel_fut(
    method get_ref_motion_root_global_ang_vel_fut (line 1492) | def get_ref_motion_root_global_ang_vel_fut(
    method get_ref_motion_bodylink_global_pos_fut (line 1499) | def get_ref_motion_bodylink_global_pos_fut(
    method get_ref_motion_bodylink_rel_pos_cur (line 1509) | def get_ref_motion_bodylink_rel_pos_cur(
    method get_ref_motion_bodylink_rel_pos_fut (line 1533) | def get_ref_motion_bodylink_rel_pos_fut(
    method get_ref_motion_bodylink_global_rot_xyzw_fut (line 1557) | def get_ref_motion_bodylink_global_rot_xyzw_fut(
    method get_ref_motion_bodylink_global_lin_vel_fut (line 1564) | def get_ref_motion_bodylink_global_lin_vel_fut(
    method get_ref_motion_bodylink_global_ang_vel_fut (line 1571) | def get_ref_motion_bodylink_global_ang_vel_fut(
    method get_ref_motion_dof_pos_cur (line 1578) | def get_ref_motion_dof_pos_cur(
    method get_ref_motion_dof_pos_immediate_next (line 1585) | def get_ref_motion_dof_pos_immediate_next(
    method get_immediate_next_two_dof_pos (line 1592) | def get_immediate_next_two_dof_pos(
    method get_ref_motion_dof_pos_cur_urdf_order (line 1605) | def get_ref_motion_dof_pos_cur_urdf_order(
    method get_ref_motion_cur_heading_aligned_root_pos (line 1612) | def get_ref_motion_cur_heading_aligned_root_pos(
    method get_ref_motion_fut_heading_aligned_root_pos (line 1631) | def get_ref_motion_fut_heading_aligned_root_pos(
    method get_ref_motion_cur_heading_aligned_root_rot6d (line 1656) | def get_ref_motion_cur_heading_aligned_root_rot6d(
    method get_ref_motion_fut_heading_aligned_root_rot6d (line 1686) | def get_ref_motion_fut_heading_aligned_root_rot6d(
    method get_ref_motion_cur_heading_aligned_root_lin_vel (line 1721) | def get_ref_motion_cur_heading_aligned_root_lin_vel(
    method get_ref_motion_fut_heading_aligned_root_lin_vel (line 1740) | def get_ref_motion_fut_heading_aligned_root_lin_vel(
    method get_ref_motion_cur_heading_aligned_root_ang_vel (line 1763) | def get_ref_motion_cur_heading_aligned_root_ang_vel(
    method get_ref_motion_fut_heading_aligned_root_ang_vel (line 1782) | def get_ref_motion_fut_heading_aligned_root_ang_vel(
    method robot_dof_pos_cur_urdf_order (line 1806) | def robot_dof_pos_cur_urdf_order(self):
    method get_ref_motion_dof_vel_cur (line 1809) | def get_ref_motion_dof_vel_cur(
    method get_ref_motion_dof_vel_immediate_next (line 1816) | def get_ref_motion_dof_vel_immediate_next(
    method robot_dof_vel_cur_urdf_order (line 1824) | def robot_dof_vel_cur_urdf_order(self):
    method get_ref_motion_dof_vel_cur_urdf_order (line 1827) | def get_ref_motion_dof_vel_cur_urdf_order(
    method get_ref_motion_root_global_pos_cur (line 1834) | def get_ref_motion_root_global_pos_cur(
    method get_ref_motion_root_global_pos_immediate_next (line 1841) | def get_ref_motion_root_global_pos_immediate_next(
    method get_ref_motion_root_global_rot_quat_xyzw_cur (line 1848) | def get_ref_motion_root_global_rot_quat_xyzw_cur(
    method get_ref_motion_root_global_rot_quat_xyzw_immediate_next (line 1854) | def get_ref_motion_root_global_rot_quat_xyzw_immediate_next(
    method get_ref_motion_root_global_rot_quat_wxyz_cur (line 1860) | def get_ref_motion_root_global_rot_quat_wxyz_cur(
    method get_ref_motion_root_global_rot_quat_wxyz_immediate_next (line 1868) | def get_ref_motion_root_global_rot_quat_wxyz_immediate_next(
    method get_ref_motion_root_global_lin_vel_cur (line 1876) | def get_ref_motion_root_global_lin_vel_cur(
    method get_ref_motion_root_global_lin_vel_immediate_next (line 1883) | def get_ref_motion_root_global_lin_vel_immediate_next(
    method ref_motion_root_global_lin_vel_cur (line 1890) | def ref_motion_root_global_lin_vel_cur(self) -> torch.Tensor:
    method get_ref_motion_root_global_ang_vel_cur (line 1893) | def get_ref_motion_root_global_ang_vel_cur(
    method get_ref_motion_root_global_ang_vel_immediate_next (line 1900) | def get_ref_motion_root_global_ang_vel_immediate_next(
    method get_ref_motion_gravity_projection_cur (line 1906) | def get_ref_motion_gravity_projection_cur(
    method get_ref_motion_gravity_projection_immediate_next (line 1917) | def get_ref_motion_gravity_projection_immediate_next(
    method get_ref_motion_gravity_projection_fut (line 1929) | def get_ref_motion_gravity_projection_fut(
    method get_ref_motion_base_linvel_cur (line 1945) | def get_ref_motion_base_linvel_cur(
    method get_ref_motion_base_linvel_immediate_next (line 1960) | def get_ref_motion_base_linvel_immediate_next(
    method get_ref_motion_base_linvel_fut (line 1978) | def get_ref_motion_base_linvel_fut(
    method get_ref_motion_base_angvel_cur (line 1993) | def get_ref_motion_base_angvel_cur(
    method get_ref_motion_base_angvel_immediate_next (line 2008) | def get_ref_motion_base_angvel_immediate_next(
    method get_ref_motion_base_angvel_fut (line 2026) | def get_ref_motion_base_angvel_fut(
    method get_ref_motion_bodylink_global_pos_cur (line 2041) | def get_ref_motion_bodylink_global_pos_cur(
    method get_ref_motion_bodylink_global_pos_immediate_next (line 2051) | def get_ref_motion_bodylink_global_pos_immediate_next(
    method get_ref_motion_bodylink_global_pos_cur_urdf_order (line 2061) | def get_ref_motion_bodylink_global_pos_cur_urdf_order(
    method get_ref_motion_bodylink_global_rot_wxyz_cur (line 2068) | def get_ref_motion_bodylink_global_rot_wxyz_cur(
    method get_ref_motion_bodylink_global_rot_xyzw_cur (line 2077) | def get_ref_motion_bodylink_global_rot_xyzw_cur(
    method get_ref_motion_bodylink_global_rot_xyzw_immediate_next (line 2084) | def get_ref_motion_bodylink_global_rot_xyzw_immediate_next(
    method get_ref_motion_bodylink_global_rot_wxyz_immediate_next (line 2091) | def get_ref_motion_bodylink_global_rot_wxyz_immediate_next(
    method get_ref_motion_bodylink_global_rot_xyzw_cur_urdf_order (line 2100) | def get_ref_motion_bodylink_global_rot_xyzw_cur_urdf_order(
    method robot_bodylink_global_pos_cur_urdf_order (line 2108) | def robot_bodylink_global_pos_cur_urdf_order(self):
    method robot_bodylink_global_rot_wxyz_cur_urdf_order (line 2112) | def robot_bodylink_global_rot_wxyz_cur_urdf_order(self):
    method robot_bodylink_global_rot_xyzw_cur_urdf_order (line 2116) | def robot_bodylink_global_rot_xyzw_cur_urdf_order(self):
    method robot_bodylink_global_lin_vel_cur_urdf_order (line 2122) | def robot_bodylink_global_lin_vel_cur_urdf_order(self):
    method robot_bodylink_global_ang_vel_cur_urdf_order (line 2126) | def robot_bodylink_global_ang_vel_cur_urdf_order(self):
    method get_ref_motion_bodylink_global_lin_vel_cur (line 2129) | def get_ref_motion_bodylink_global_lin_vel_cur(
    method get_ref_motion_bodylink_global_lin_vel_immediate_next (line 2136) | def get_ref_motion_bodylink_global_lin_vel_immediate_next(
    method get_ref_motion_bodylink_global_lin_vel_cur_urdf_order (line 2143) | def get_ref_motion_bodylink_global_lin_vel_cur_urdf_order(
    method get_ref_motion_bodylink_global_ang_vel_cur (line 2150) | def get_ref_motion_bodylink_global_ang_vel_cur(
    method get_ref_motion_bodylink_global_ang_vel_immediate_next (line 2157) | def get_ref_motion_bodylink_global_ang_vel_immediate_next(
    method get_ref_motion_bodylink_global_ang_vel_cur_urdf_order (line 2164) | def get_ref_motion_bodylink_global_ang_vel_cur_urdf_order(
    method _build_amp_obs_from_ref_state (line 2171) | def _build_amp_obs_from_ref_state(
    method get_ref_motion_amp_obs_cur (line 2266) | def get_ref_motion_amp_obs_cur(
    method motion_end_mask (line 2273) | def motion_end_mask(self) -> torch.Tensor:
    method global_robot_anchor_pos_cur (line 2283) | def global_robot_anchor_pos_cur(self):
    method get_ref_motion_anchor_bodylink_global_pos_cur (line 2286) | def get_ref_motion_anchor_bodylink_global_pos_cur(
    method get_ref_motion_anchor_bodylink_global_rot_wxyz_cur (line 2293) | def get_ref_motion_anchor_bodylink_global_rot_wxyz_cur(
    method get_ref_motion_anchor_bodylink_global_pos_immediate_next (line 2300) | def get_ref_motion_anchor_bodylink_global_pos_immediate_next(
    method get_ref_motion_anchor_bodylink_global_rot_wxyz_immediate_next (line 2309) | def get_ref_motion_anchor_bodylink_global_rot_wxyz_immediate_next(
    method _get_obs_bydmmc_ref_motion (line 2318) | def _get_obs_bydmmc_ref_motion(
    method _get_obs_bydmmc_ref_motion_fut (line 2333) | def _get_obs_bydmmc_ref_motion_fut(
    method _get_obs_vr_ref_motion_states (line 2352) | def _get_obs_vr_ref_motion_states(
    method _get_obs_vr_ref_motion_fut (line 2372) | def _get_obs_vr_ref_motion_fut(
    method _get_obs_holomotion_rel_ref_motion_flat (line 2393) | def _get_obs_holomotion_rel_ref_motion_flat(
    method _resample_command (line 2540) | def _resample_command(self, env_ids: Sequence[int], eval=False):
    method _filter_env_ids_for_motion_task (line 2577) | def _filter_env_ids_for_motion_task(
    method _align_root_to_ref (line 2608) | def _align_root_to_ref(self, env_ids):
    method _align_dof_to_ref (line 2679) | def _align_dof_to_ref(self, env_ids):
    method force_realign_root_state_to_ref_no_perturb (line 2711) | def force_realign_root_state_to_ref_no_perturb(self, env_ids) -> None:
    method force_realign_dof_state_to_ref_no_perturb (line 2740) | def force_realign_dof_state_to_ref_no_perturb(self, env_ids) -> None:
    method force_realign_offline_eval_no_perturb (line 2766) | def force_realign_offline_eval_no_perturb(self, env_ids) -> None:
    method _update_command (line 2770) | def _update_command(self):
    method _resample_when_motion_end_cache (line 2794) | def _resample_when_motion_end_cache(self):
    method _update_metrics (line 2835) | def _update_metrics(self):
    method _update_mpjpe_metrics (line 2843) | def _update_mpjpe_metrics(self):
    method _update_mpkpe_metrics (line 2890) | def _update_mpkpe_metrics(self):
    method get_wholebody_mpjpe (line 2939) | def get_wholebody_mpjpe(
    method get_wholebody_mpkpe (line 2949) | def get_wholebody_mpkpe(
    method get_current_motion_keys (line 2959) | def get_current_motion_keys(
    method _set_debug_vis_impl (line 2974) | def _set_debug_vis_impl(self, debug_vis: bool):
    method setup_offline_eval_from_frame_zero (line 2989) | def setup_offline_eval_from_frame_zero(self):
    method setup_offline_eval_deterministic (line 3001) | def setup_offline_eval_deterministic(
    method _debug_vis_callback (line 3032) | def _debug_vis_callback(self, event):
  class MotionCommandCfg (line 3083) | class MotionCommandCfg(CommandTermCfg):
  class MoTrack_CommandsCfg (line 3136) | class MoTrack_CommandsCfg:
  function build_motion_tracking_commands_config (line 3140) | def build_motion_tracking_commands_config(command_config_dict: dict):

FILE: holomotion/src/env/isaaclab_components/isaaclab_observation.py
  function _build_noise_cfg (line 63) | def _build_noise_cfg(noise_cfg):
  class MirrorFunctions (line 89) | class MirrorFunctions:
    method mirror_dof (line 93) | def mirror_dof(
    method mirror_action (line 110) | def mirror_action(
    method mirror_vec3 (line 117) | def mirror_vec3(x: torch.Tensor) -> torch.Tensor:
    method mirror_axial_vec3 (line 129) | def mirror_axial_vec3(x: torch.Tensor) -> torch.Tensor:
    method mirror_velocity_command (line 141) | def mirror_velocity_command(x: torch.Tensor) -> torch.Tensor:
  class ObservationFunctions (line 159) | class ObservationFunctions:
    method _get_body_indices (line 168) | def _get_body_indices(
    method _slice_future_frames (line 194) | def _slice_future_frames(
    method _get_obs_head_pos_quat_vel (line 220) | def _get_obs_head_pos_quat_vel(
    method _get_obs_rel_headlink_lin_vel (line 309) | def _get_obs_rel_headlink_lin_vel(
    method _get_obs_rel_headlink_ang_vel (line 388) | def _get_obs_rel_headlink_ang_vel(
    method _get_obs_global_robot_root_pos (line 453) | def _get_obs_global_robot_root_pos(env: ManagerBasedRLEnv):
    method _get_obs_global_robot_root_rot_wxyz (line 462) | def _get_obs_global_robot_root_rot_wxyz(env: ManagerBasedRLEnv):
    method _get_obs_global_robot_root_rot_xyzw (line 467) | def _get_obs_global_robot_root_rot_xyzw(env: ManagerBasedRLEnv):
    method _get_obs_global_robot_root_rot_mat (line 474) | def _get_obs_global_robot_root_rot_mat(env: ManagerBasedRLEnv):
    method _get_obs_global_robot_root_lin_vel (line 481) | def _get_obs_global_robot_root_lin_vel(env: ManagerBasedRLEnv):
    method _get_obs_global_robot_root_ang_vel (line 486) | def _get_obs_global_robot_root_ang_vel(env: ManagerBasedRLEnv):
    method _get_obs_rel_robot_root_lin_vel (line 491) | def _get_obs_rel_robot_root_lin_vel(env: ManagerBasedRLEnv):
    method _get_obs_rel_robot_root_ang_vel (line 496) | def _get_obs_rel_robot_root_ang_vel(env: ManagerBasedRLEnv):
    method _get_obs_rel_anchor_lin_vel (line 501) | def _get_obs_rel_anchor_lin_vel(
    method _get_obs_projected_gravity (line 524) | def _get_obs_projected_gravity(
    method _get_obs_global_robot_root_yaw (line 547) | def _get_obs_global_robot_root_yaw(
    method _get_obs_robot_root_heading_aligned_quat (line 557) | def _get_obs_robot_root_heading_aligned_quat(
    method _get_obs_rel_robot_root_roll_pitch (line 577) | def _get_obs_rel_robot_root_roll_pitch(
    method _get_obs_global_robot_bodylink_pos (line 599) | def _get_obs_global_robot_bodylink_pos(
    method _get_obs_global_robot_bodylink_rot_wxyz (line 621) | def _get_obs_global_robot_bodylink_rot_wxyz(
    method _get_obs_global_robot_bodylink_rot_xyzw (line 635) | def _get_obs_global_robot_bodylink_rot_xyzw(
    method _get_obs_global_robot_bodylink_rot_mat (line 648) | def _get_obs_global_robot_bodylink_rot_mat(
    method _get_obs_global_robot_bodylink_lin_vel (line 666) | def _get_obs_global_robot_bodylink_lin_vel(
    method _get_obs_global_robot_bodylink_ang_vel (line 680) | def _get_obs_global_robot_bodylink_ang_vel(
    method _get_obs_root_rel_robot_bodylink_pos (line 694) | def _get_obs_root_rel_robot_bodylink_pos(
    method _get_obs_root_rel_robot_bodylink_rot_wxyz (line 721) | def _get_obs_root_rel_robot_bodylink_rot_wxyz(
    method _get_obs_root_rel_robot_bodylink_rot_xyzw (line 751) | def _get_obs_root_rel_robot_bodylink_rot_xyzw(
    method _get_obs_root_rel_robot_bodylink_rot_mat (line 764) | def _get_obs_root_rel_robot_bodylink_rot_mat(
    method _get_obs_root_rel_robot_bodylink_lin_vel (line 781) | def _get_obs_root_rel_robot_bodylink_lin_vel(
    method _get_obs_root_rel_robot_bodylink_ang_vel (line 816) | def _get_obs_root_rel_robot_bodylink_ang_vel(
    method _get_obs_global_robot_bodylink_pos_flat (line 852) | def _get_obs_global_robot_bodylink_pos_flat(
    method _get_obs_global_robot_bodylink_rot_wxyz_flat (line 866) | def _get_obs_global_robot_bodylink_rot_wxyz_flat(
    method _get_obs_global_robot_bodylink_rot_xyzw_flat (line 882) | def _get_obs_global_robot_bodylink_rot_xyzw_flat(
    method _get_obs_global_robot_bodylink_rot_mat_flat (line 898) | def _get_obs_global_robot_bodylink_rot_mat_flat(
    method _get_obs_global_robot_bodylink_lin_vel_flat (line 914) | def _get_obs_global_robot_bodylink_lin_vel_flat(
    method _get_obs_global_robot_bodylink_ang_vel_flat (line 930) | def _get_obs_global_robot_bodylink_ang_vel_flat(
    method _get_obs_root_rel_robot_bodylink_pos_flat (line 946) | def _get_obs_root_rel_robot_bodylink_pos_flat(
    method _get_obs_root_rel_robot_bodylink_rot_wxyz_flat (line 962) | def _get_obs_root_rel_robot_bodylink_rot_wxyz_flat(
    method _get_obs_root_rel_robot_bodylink_rot_xyzw_flat (line 978) | def _get_obs_root_rel_robot_bodylink_rot_xyzw_flat(
    method _get_obs_root_rel_robot_bodylink_rot_mat_flat (line 994) | def _get_obs_root_rel_robot_bodylink_rot_mat_flat(
    method _get_obs_root_rel_robot_bodylink_lin_vel_flat (line 1010) | def _get_obs_root_rel_robot_bodylink_lin_vel_flat(
    method _get_obs_root_rel_robot_bodylink_ang_vel_flat (line 1026) | def _get_obs_root_rel_robot_bodylink_ang_vel_flat(
    method _get_obs_dof_pos (line 1043) | def _get_obs_dof_pos(env: ManagerBasedRLEnv):
    method _get_obs_dof_vel (line 1048) | def _get_obs_dof_vel(env: ManagerBasedRLEnv):
    method _get_obs_last_actions (line 1053) | def _get_obs_last_actions(env: ManagerBasedRLEnv):
    method _get_obs_ref_motion_states (line 1059) | def _get_obs_ref_motion_states(
    method _get_obs_ref_motion_states_fut (line 1071) | def _get_obs_ref_motion_states_fut(
    method _get_obs_vr_ref_motion_states (line 1083) | def _get_obs_vr_ref_motion_states(
    method _get_obs_vr_ref_motion_states_fut (line 1092) | def _get_obs_vr_ref_motion_states_fut(
    method _get_obs_ref_dof_pos_cur (line 1102) | def _get_obs_ref_dof_pos_cur(
    method _get_obs_immediate_next_two_dof_pos (line 1112) | def _get_obs_immediate_next_two_dof_pos(
    method _get_obs_ref_motion_cur_heading_aligned_root_pos (line 1122) | def _get_obs_ref_motion_cur_heading_aligned_root_pos(
    method _get_obs_ref_motion_fut_heading_aligned_root_pos (line 1134) | def _get_obs_ref_motion_fut_heading_aligned_root_pos(
    method _get_obs_ref_motion_cur_heading_aligned_root_rot6d (line 1146) | def _get_obs_ref_motion_cur_heading_aligned_root_rot6d(
    method _get_obs_ref_motion_fut_heading_aligned_root_rot6d (line 1158) | def _get_obs_ref_motion_fut_heading_aligned_root_rot6d(
    method _get_obs_ref_motion_cur_heading_aligned_root_lin_vel (line 1170) | def _get_obs_ref_motion_cur_heading_aligned_root_lin_vel(
    method _get_obs_ref_motion_fut_heading_aligned_root_lin_vel (line 1182) | def _get_obs_ref_motion_fut_heading_aligned_root_lin_vel(
    method _get_obs_ref_motion_cur_heading_aligned_root_ang_vel (line 1194) | def _get_obs_ref_motion_cur_heading_aligned_root_ang_vel(
    method _get_obs_ref_motion_fut_heading_aligned_root_ang_vel (line 1206) | def _get_obs_ref_motion_fut_heading_aligned_root_ang_vel(
    method _get_obs_ref_dof_vel_cur (line 1218) | def _get_obs_ref_dof_vel_cur(
    method _get_obs_ref_motion_filter_cutoff_hz (line 1228) | def _get_obs_ref_motion_filter_cutoff_hz(
    method _get_obs_ref_root_height_cur (line 1237) | def _get_obs_ref_root_height_cur(
    method _get_obs_ref_dof_pos_fut (line 1253) | def _get_obs_ref_dof_pos_fut(
    method _get_obs_ref_gravity_projection_cur (line 1272) | def _get_obs_ref_gravity_projection_cur(
    method _get_obs_ref_gravity_projection_fut (line 1285) | def _get_obs_ref_gravity_projection_fut(
    method _get_obs_ref_base_linvel_cur (line 1304) | def _get_obs_ref_base_linvel_cur(
    method _get_obs_ref_base_linvel_fut (line 1315) | def _get_obs_ref_base_linvel_fut(
    method _get_obs_ref_base_angvel_cur (line 1332) | def _get_obs_ref_base_angvel_cur(
    method _get_obs_ref_keybody_rel_pos_cur (line 1343) | def _get_obs_ref_keybody_rel_pos_cur(
    method _get_obs_ref_keybody_rel_pos_fut (line 1365) | def _get_obs_ref_keybody_rel_pos_fut(
    method _get_obs_ref_base_angvel_fut (line 1393) | def _get_obs_ref_base_angvel_fut(
    method _get_obs_ref_dof_vel_fut (line 1410) | def _get_obs_ref_dof_vel_fut(
    method _get_obs_ref_root_height_fut (line 1430) | def _get_obs_ref_root_height_fut(
    method _get_obs_global_anchor_diff (line 1453) | def _get_obs_global_anchor_diff(
    method _get_obs_global_anchor_pos_diff (line 1497) | def _get_obs_global_anchor_pos_diff(
    method _get_obs_global_anchor_rot_diff (line 1534) | def _get_obs_global_anchor_rot_diff(
    method _get_obs_velocity_command (line 1572) | def _get_obs_velocity_command(
    method _get_obs_place_holder (line 1603) | def _get_obs_place_holder(env: ManagerBasedRLEnv, n_dim: int):
    method _get_obs_ref_headling_aligned_vel_cmd (line 1607) | def _get_obs_ref_headling_aligned_vel_cmd(
    method _get_obs_heading_aligned_root_ang_vel (line 1636) | def _get_obs_heading_aligned_root_ang_vel(env: ManagerBasedRLEnv):
  class ObservationsCfg (line 1651) | class ObservationsCfg:
  function build_observations_config (line 1655) | def build_observations_config(obs_config_dict: dict):

FILE: holomotion/src/env/isaaclab_components/isaaclab_rewards.py
  function _joint_ids_to_tensor (line 46) | def _joint_ids_to_tensor(
  function _select_effort_limit_vector (line 65) | def _select_effort_limit_vector(
  function key_dof_position_tracking_exp (line 132) | def key_dof_position_tracking_exp(
  function key_dof_velocity_tracking_exp (line 154) | def key_dof_velocity_tracking_exp(
  function motion_global_anchor_position_error_exp (line 176) | def motion_global_anchor_position_error_exp(
  function motion_global_anchor_orientation_error_exp (line 196) | def motion_global_anchor_orientation_error_exp(
  function motion_relative_body_position_error_exp (line 218) | def motion_relative_body_position_error_exp(
  function root_rel_keybodylink_pos_tracking_l2_exp (line 302) | def root_rel_keybodylink_pos_tracking_l2_exp(
  function motion_relative_body_orientation_error_exp (line 364) | def motion_relative_body_orientation_error_exp(
  function motion_global_body_linear_velocity_error_exp (line 432) | def motion_global_body_linear_velocity_error_exp(
  function motion_global_body_angular_velocity_error_exp (line 454) | def motion_global_body_angular_velocity_error_exp(
  function root_pos_xy_tracking_exp (line 476) | def root_pos_xy_tracking_exp(
  function root_rot_tracking_exp (line 495) | def root_rot_tracking_exp(
  function root_pos_rel_z_tracking_exp (line 517) | def root_pos_rel_z_tracking_exp(
  function root_lin_vel_tracking_l2_exp (line 533) | def root_lin_vel_tracking_l2_exp(
  function root_ang_vel_tracking_l2_exp (line 575) | def root_ang_vel_tracking_l2_exp(
  function root_rel_keybodylink_pos_tracking_l2_exp_bydmmc_style (line 617) | def root_rel_keybodylink_pos_tracking_l2_exp_bydmmc_style(
  function root_rel_keybodylink_rot_tracking_l2_exp (line 695) | def root_rel_keybodylink_rot_tracking_l2_exp(
  function root_rel_keybodylink_lin_vel_tracking_l2_exp (line 751) | def root_rel_keybodylink_lin_vel_tracking_l2_exp(
  function root_rel_keybodylink_ang_vel_tracking_l2_exp (line 861) | def root_rel_keybodylink_ang_vel_tracking_l2_exp(
  function global_keybodylink_lin_vel_tracking_l2_exp (line 926) | def global_keybodylink_lin_vel_tracking_l2_exp(
  function global_keybodylink_ang_vel_tracking_l2_exp (line 953) | def global_keybodylink_ang_vel_tracking_l2_exp(
  function feet_contact_time (line 981) | def feet_contact_time(
  function track_lin_vel_xy_yaw_frame_exp (line 997) | def track_lin_vel_xy_yaw_frame_exp(
  function feet_slide (line 1026) | def feet_slide(
  function feet_slide_ang_vel (line 1045) | def feet_slide_ang_vel(
  function foot_clearance_reward (line 1064) | def foot_clearance_reward(
  function feet_gait (line 1106) | def feet_gait(
  function joint_deviation_l2 (line 1152) | def joint_deviation_l2(
  function energy (line 1175) | def energy(
  function positive_work (line 1187) | def positive_work(
  class normed_positive_work (line 1206) | class normed_positive_work(ManagerTermBase):
    method __init__ (line 1209) | def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv):
    method _maybe_build_cache (line 1215) | def _maybe_build_cache(
    method __call__ (line 1245) | def __call__(
  class normed_torque_rate (line 1274) | class normed_torque_rate(ManagerTermBase):
    method __init__ (line 1277) | def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv):
    method reset (line 1287) | def reset(self, env_ids=None) -> None:
    method _maybe_build_cache (line 1299) | def _maybe_build_cache(
    method __call__ (line 1342) | def __call__(
  function track_stand_still_exp (line 1379) | def track_stand_still_exp(
  function stand_still_joint_deviation_l1 (line 1404) | def stand_still_joint_deviation_l1(
  function stand_still_action_rate (line 1430) | def stand_still_action_rate(
  function stand_still_dof_vel_l2 (line 1449) | def stand_still_dof_vel_l2(
  class action_acc (line 1467) | class action_acc(ManagerTermBase):
    method __init__ (line 1470) | def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv):
    method reset (line 1481) | def reset(self, env_ids=None) -> None:
    method _maybe_build_cache (line 1496) | def _maybe_build_cache(
    method __call__ (line 1528) | def __call__(self, env: ManagerBasedRLEnv) -> torch.Tensor:
  function feet_stumble (line 1572) | def feet_stumble(
  function feet_too_near (line 1588) | def feet_too_near(
  function feet_contact_without_cmd (line 1599) | def feet_contact_without_cmd(
  function torso_xy_ang_vel_l2_penalty (line 1621) | def torso_xy_ang_vel_l2_penalty(
  function torso_upright_l2_penalty (line 1658) | def torso_upright_l2_penalty(
  function torso_upright_l2_penalty_v2 (line 1696) | def torso_upright_l2_penalty_v2(
  function stand_still_torso_upright_exp_v2 (line 1739) | def stand_still_torso_upright_exp_v2(
  function torso_linacc_xy_l2_penalty (line 1773) | def torso_linacc_xy_l2_penalty(
  function track_lin_vel_xy_heading_aligned_frame_exp (line 1809) | def track_lin_vel_xy_heading_aligned_frame_exp(
  function track_lin_vel_xy_heading_aligned_frame_exp_v2 (line 1840) | def track_lin_vel_xy_heading_aligned_frame_exp_v2(
  function track_ang_vel_z_heading_aligned_frame_exp_v2 (line 1878) | def track_ang_vel_z_heading_aligned_frame_exp_v2(
  function track_ang_vel_z_heading_aligned_frame_exp (line 1912) | def track_ang_vel_z_heading_aligned_frame_exp(
  function smoothed_track_ang_vel_z_heading_aligned_frame_exp (line 1933) | def smoothed_track_ang_vel_z_heading_aligned_frame_exp(
  function feet_air_time (line 1963) | def feet_air_time(
  function feet_air_time_v2 (line 1989) | def feet_air_time_v2(
  function feet_air_time_v3 (line 2020) | def feet_air_time_v3(
  function feet_air_time_v4 (line 2055) | def feet_air_time_v4(
  function yaw_rate_only_movement_l2_penalty (line 2085) | def yaw_rate_only_movement_l2_penalty(
  function fly (line 2116) | def fly(
  function stand_still_torso_lin_vel_l2_penalty (line 2133) | def stand_still_torso_lin_vel_l2_penalty(
  function stand_still_torso_ang_vel_l2_penalty (line 2147) | def stand_still_torso_ang_vel_l2_penalty(
  function stand_still_torso_lin_vel_exp (line 2161) | def stand_still_torso_lin_vel_exp(
  function stand_still_torso_ang_vel_exp (line 2190) | def stand_still_torso_ang_vel_exp(
  function yaw_rate_only_movement_exp (line 2219) | def yaw_rate_only_movement_exp(
  function yaw_rate_only_hip_yaw_usage_exp (line 2249) | def yaw_rate_only_hip_yaw_usage_exp(
  class RewardsCfg (line 2301) | class RewardsCfg:
  class TaskGatedReward (line 2305) | class TaskGatedReward:
    method __init__ (line 2308) | def __init__(self, func, task_name: str):
    method __call__ (line 2313) | def __call__(self, env: ManagerBasedRLEnv, *args, **kwargs):
  function build_rewards_config (line 2337) | def build_rewards_config(reward_config_dict: dict):

FILE: holomotion/src/env/isaaclab_components/isaaclab_scene.py
  class SceneFunctions (line 43) | class SceneFunctions:
    method build_robot_config (line 47) | def build_robot_config(
    method build_lighting_config (line 203) | def build_lighting_config(
    method build_contact_sensor_config (line 229) | def build_contact_sensor_config(config: dict) -> ContactSensorCfg:
  class MotionTrackingSceneCfg (line 247) | class MotionTrackingSceneCfg(InteractiveSceneCfg):
  function build_scene_config (line 253) | def build_scene_config(
  function _cfg_to_kwargs (line 325) | def _cfg_to_kwargs(cfg: object) -> dict:
  function _build_unitree_actuator_cfg (line 333) | def _build_unitree_actuator_cfg(

FILE: holomotion/src/env/isaaclab_components/isaaclab_simulator.py
  function build_simulator_config (line 21) | def build_simulator_config(sim_config_dict: dict) -> SimulationCfg:

FILE: holomotion/src/env/isaaclab_components/isaaclab_termination.py
  function _list_supported_terminations (line 33) | def _list_supported_terminations() -> list[str]:
  function _resolve_termination_func (line 54) | def _resolve_termination_func(name: str):
  function global_bodylink_pos_far (line 69) | def global_bodylink_pos_far(
  function anchor_ref_z_far (line 102) | def anchor_ref_z_far(
  function ref_gravity_projection_far (line 119) | def ref_gravity_projection_far(
  function keybody_ref_pos_far (line 169) | def keybody_ref_pos_far(
  function keybody_ref_z_far (line 202) | def keybody_ref_z_far(
  function wholebody_mpjpe_far (line 235) | def wholebody_mpjpe_far(
  function motion_end (line 253) | def motion_end(
  class TerminationsCfg (line 269) | class TerminationsCfg:
  function build_terminations_config (line 273) | def build_terminations_config(

FILE: holomotion/src/env/isaaclab_components/isaaclab_terrain.py
  function _convert_range_like_params (line 36) | def _convert_range_like_params(params: dict) -> dict:
  function plane_terrain (line 54) | def plane_terrain(difficulty: float, cfg: HfTerrainBaseCfg) -> np.ndarray:
  class HfPlaneTerrainCfg (line 67) | class HfPlaneTerrainCfg(HfTerrainBaseCfg):
  class RandomSpawnTerrainImporter (line 73) | class RandomSpawnTerrainImporter(TerrainImporter):
    method _compute_env_origins_curriculum (line 80) | def _compute_env_origins_curriculum(
    method update_env_origins (line 166) | def update_env_origins(
  function build_terrain_config (line 214) | def build_terrain_config(

FILE: holomotion/src/env/isaaclab_components/isaaclab_utils.py
  function _get_dof_indices (line 36) | def _get_dof_indices(
  function _get_body_indices (line 52) | def _get_body_indices(
  function resolve_holo_config (line 79) | def resolve_holo_config(value):

FILE: holomotion/src/env/isaaclab_components/isaaclab_velocity_tracking_command.py
  class HoloMotionUniformVelocityCommand (line 40) | class HoloMotionUniformVelocityCommand(CommandTerm):
    method __init__ (line 61) | def __init__(
    method __str__ (line 106) | def __str__(self) -> str:
    method command (line 123) | def command(self) -> torch.Tensor:
    method _update_metrics (line 131) | def _update_metrics(self):
    method _resample_command (line 151) | def _resample_command(self, env_ids: Sequence[int]):
    method _update_command (line 178) | def _update_command(self):
    method _set_debug_vis_impl (line 208) | def _set_debug_vis_impl(self, debug_vis: bool):
    method _debug_vis_callback (line 230) | def _debug_vis_callback(self, event):
    method _resolve_xy_velocity_to_arrow (line 258) | def _resolve_xy_velocity_to_arrow(
  class HoloMotionUniformVelocityCommandCfg (line 283) | class HoloMotionUniformVelocityCommandCfg(CommandTermCfg):
    class Ranges (line 323) | class Ranges:
  class VelTrack_CommandsCfg (line 364) | class VelTrack_CommandsCfg:
  function _convert_ranges_dict_to_object (line 368) | def _convert_ranges_dict_to_object(
  function build_velocity_commands_config (line 383) | def build_velocity_commands_config(

FILE: holomotion/src/env/isaaclab_components/unitree_actuators.py
  class UnitreeActuator (line 38) | class UnitreeActuator(DelayedPDActuator):
    method __init__ (line 71) | def __init__(self, cfg: UnitreeActuatorCfg, *args, **kwargs):
    method compute (line 83) | def compute(
    method _clip_effort (line 107) | def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
    method _compute_effort_limit (line 121) | def _compute_effort_limit(self, max_effort):
  class UnitreeErfiActuator (line 127) | class UnitreeErfiActuator(UnitreeActuator):
    method __init__ (line 137) | def __init__(self, cfg: UnitreeErfiActuatorCfg, *args, **kwargs):
    method reset (line 164) | def reset(self, env_ids: Sequence[int] | slice | None):
    method compute (line 204) | def compute(
    method _filter_joint_position_action (line 236) | def _filter_joint_position_action(
    method _maybe_dump_ema_filter_debug_verification (line 273) | def _maybe_dump_ema_filter_debug_verification(
    method _maybe_dump_ema_filter_debug_skip (line 327) | def _maybe_dump_ema_filter_debug_skip(self, reason: str) -> None:
    method _maybe_dump_ema_filter_debug_payload (line 335) | def _maybe_dump_ema_filter_debug_payload(self, payload: dict) -> None:
    method _parse_bool_env (line 364) | def _parse_bool_env(name: str, default: bool) -> bool:
    method _env_ids_to_tensor (line 370) | def _env_ids_to_tensor(
    method _sample_uniform (line 379) | def _sample_uniform(
  class UnitreeActuatorCfg (line 386) | class UnitreeActuatorCfg(DelayedPDActuatorCfg):
  class UnitreeErfiActuatorCfg (line 416) | class UnitreeErfiActuatorCfg(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_M107_15 (line 453) | class UnitreeActuatorCfg_M107_15(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_M107_24 (line 463) | class UnitreeActuatorCfg_M107_24(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_Go2HV (line 473) | class UnitreeActuatorCfg_Go2HV(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_N7520_14p3 (line 481) | class UnitreeActuatorCfg_N7520_14p3(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_N7520_22p5 (line 500) | class UnitreeActuatorCfg_N7520_22p5(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_N5010_16 (line 519) | class UnitreeActuatorCfg_N5010_16(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_N5020_16 (line 534) | class UnitreeActuatorCfg_N5020_16(UnitreeActuatorCfg):
  class UnitreeActuatorCfg_W4010_25 (line 552) | class UnitreeActuatorCfg_W4010_25(UnitreeActuatorCfg):

FILE: holomotion/src/env/motion_tracking.py
  function _joint_ids_to_tensor (line 80) | def _joint_ids_to_tensor(
  function _select_effort_limit_vector (line 98) | def _select_effort_limit_vector(
  class MotionTrackingEnv (line 164) | class MotionTrackingEnv:
    method __init__ (line 175) | def __init__(
    method num_envs (line 216) | def num_envs(self):
    method device (line 220) | def device(self):
    method _init_isaaclab_env (line 223) | def _init_isaaclab_env(self):
    method _init_motion_tracking_components (line 467) | def _init_motion_tracking_components(self):
    method step (line 472) | def step(self, actor_state: dict):
    method _update_robot_metrics (line 482) | def _update_robot_metrics(self, infos: dict) -> None:
    method _update_completion_rate_stats (line 563) | def _update_completion_rate_stats(
    method reset_idx (line 603) | def reset_idx(self, env_ids: torch.Tensor):
    method reset_all (line 606) | def reset_all(self):
    method set_is_evaluating (line 611) | def set_is_evaluating(self):
    method seed (line 615) | def seed(self, seed: int):

FILE: holomotion/src/env/velocity_tracking.py
  class VelocityTrackingEnv (line 78) | class VelocityTrackingEnv:
    method __init__ (line 89) | def __init__(
    method num_envs (line 127) | def num_envs(self):
    method device (line 131) | def device(self):
    method _init_isaaclab_env (line 134) | def _init_isaaclab_env(self):
    method _init_motion_tracking_components (line 366) | def _init_motion_tracking_components(self):
    method step (line 369) | def step(self, actor_state: dict):
    method _update_completion_rate_stats (line 378) | def _update_completion_rate_stats(
    method reset_idx (line 415) | def reset_idx(self, env_ids: torch.Tensor):
    method reset_all (line 418) | def reset_all(self):
    method set_is_evaluating (line 423) | def set_is_evaluating(self):
    method seed (line 427) | def seed(self, seed: int):

FILE: holomotion/src/evaluation/eval_motion_tracking.py
  function find_checkpoints_to_evaluate (line 26) | def find_checkpoints_to_evaluate(
  function main (line 113) | def main(
  function parse_args (line 167) | def parse_args() -> argparse.Namespace:

FILE: holomotion/src/evaluation/eval_motion_tracking_single.py
  function load_training_config (line 31) | def load_training_config(
  function _infer_dataset_suffix (line 85) | def _infer_dataset_suffix(output_dir: str, checkpoint_path: str) -> str:
  function _checkpoint_sort_key (line 94) | def _checkpoint_sort_key(checkpoint_path: Path):
  function _normalize_ckpt_pt_names (line 101) | def _normalize_ckpt_pt_names(ckpt_pt_names) -> list[str]:
  function _resolve_export_ckpt_paths (line 125) | def _resolve_export_ckpt_paths(config: OmegaConf) -> list[Path]:
  function main (line 173) | def main(config: OmegaConf):

FILE: holomotion/src/evaluation/eval_mujoco_sim2sim.py
  function _coerce_config_bool (line 87) | def _coerce_config_bool(value, default: bool = False) -> bool:
  class OffscreenRenderer (line 102) | class OffscreenRenderer:
    method __init__ (line 105) | def __init__(
    method set_overlay_callback (line 140) | def set_overlay_callback(self, callback) -> None:
    method render (line 144) | def render(self, data) -> np.ndarray:
    method set_align_view (line 160) | def set_align_view(
    method close (line 187) | def close(self):
  class VelocityKeyboardHandler (line 191) | class VelocityKeyboardHandler:
    method __init__ (line 194) | def __init__(
    method start_listener (line 219) | def start_listener(self):
    method stop_listener (line 241) | def stop_listener(self):
    method get_velocity_command (line 247) | def get_velocity_command(self) -> np.ndarray:
    method _handle_key (line 256) | def _handle_key(self, char: str):
  class MujocoEvaluator (line 322) | class MujocoEvaluator:
    method __init__ (line 325) | def __init__(self, config):
    method _reset_onnx_io_dump_buffers (line 477) | def _reset_onnx_io_dump_buffers(self):
    method _get_action_ema_filter_cfg (line 483) | def _get_action_ema_filter_cfg(self) -> tuple[bool, float]:
    method _reset_action_ema_filter (line 500) | def _reset_action_ema_filter(self) -> None:
    method _apply_action_ema_filter (line 503) | def _apply_action_ema_filter(self, raw_actions: np.ndarray) -> np.ndar...
    method _get_action_delay_cfg (line 521) | def _get_action_delay_cfg(self) -> tuple[int, str]:
    method _sample_policy_action_delay_step (line 541) | def _sample_policy_action_delay_step(self) -> int:
    method _reset_action_delay_randomization (line 546) | def _reset_action_delay_randomization(self) -> None:
    method _apply_action_delay (line 560) | def _apply_action_delay(self, raw_actions: np.ndarray) -> np.ndarray:
    method _normalize_foot_geom_name_groups (line 588) | def _normalize_foot_geom_name_groups(raw_spec) -> list[list[str]]:
    method _normalize_foot_body_name_groups (line 622) | def _normalize_foot_body_name_groups(raw_spec) -> list[list[str]]:
    method _resolve_foot_geom_ids_from_geom_names (line 661) | def _resolve_foot_geom_ids_from_geom_names(
    method _resolve_foot_geom_ids_from_body_names (line 678) | def _resolve_foot_geom_ids_from_body_names(
    method _init_low_level_foot_contact_logging (line 716) | def _init_low_level_foot_contact_logging(self) -> None:
    method _record_low_level_foot_contact_sample (line 755) | def _record_low_level_foot_contact_sample(self) -> None:
    method _flatten_single_step_output (line 815) | def _flatten_single_step_output(values, *, dtype=None) -> np.ndarray:
    method _discover_policy_moe_outputs (line 823) | def _discover_policy_moe_outputs(self) -> None:
    method _get_stacked_moe_routing_tensors (line 856) | def _get_stacked_moe_routing_tensors(
    method _get_stacked_low_level_foot_contact_tensors (line 868) | def _get_stacked_low_level_foot_contact_tensors(
    method _record_onnx_io_frame (line 894) | def _record_onnx_io_frame(self, input_feed, output_names, onnx_output):
    method _stack_onnx_io_frames (line 916) | def _stack_onnx_io_frames(
    method save_onnx_io_dump (line 927) | def save_onnx_io_dump(self, output_path, meta_info):
    method _find_actor_place_holder_ndim (line 942) | def _find_actor_place_holder_ndim(self):
    method _get_actor_obs_term_params (line 954) | def _get_actor_obs_term_params(self, term_name: str) -> dict:
    method _get_ref_keybody_indices (line 966) | def _get_ref_keybody_indices(self, term_name: str) -> np.ndarray:
    method _to_plain_obs_cfg (line 992) | def _to_plain_obs_cfg(cfg):
    method _get_actor_obs_schema_terms (line 1003) | def _get_actor_obs_schema_terms(self) -> list[str]:
    method _get_actor_atomic_obs_entries (line 1020) | def _get_actor_atomic_obs_entries(self) -> list[tuple[str, str, dict]]:
    method _get_policy_atomic_obs_list (line 1066) | def _get_policy_atomic_obs_list(self):
    method _body_origin_world_velocity (line 1122) | def _body_origin_world_velocity(
    method _get_anchor_body_name (line 1140) | def _get_anchor_body_name(self) -> str:
    method _get_torso_body_name (line 1148) | def _get_torso_body_name(self) -> str:
    method ref_motion_frame_idx (line 1156) | def ref_motion_frame_idx(self):
    method anchor_body_idx (line 1160) | def anchor_body_idx(self) -> int:
    method root_body_idx (line 1166) | def root_body_idx(self) -> int:
    method torso_body_idx (line 1170) | def torso_body_idx(self) -> int:
    method robot_global_bodylink_pos (line 1174) | def robot_global_bodylink_pos(self):
    method robot_global_bodylink_rot (line 1190) | def robot_global_bodylink_rot(self):
    method robot_global_bodylink_lin_vel (line 1208) | def robot_global_bodylink_lin_vel(self):
    method robot_global_bodylink_ang_vel (line 1234) | def robot_global_bodylink_ang_vel(self):
    method robot_dof_pos (line 1261) | def robot_dof_pos(self):
    method robot_dof_vel (line 1267) | def robot_dof_vel(self):
    method _ensure_ref_to_sim_transform_rigid (line 1274) | def _ensure_ref_to_sim_transform_rigid(self):
    method _detect_command_mode (line 1391) | def _detect_command_mode(self) -> str:
    method _init_obs_buffers (line 1405) | def _init_obs_buffers(self):
    method load_policy (line 1415) | def load_policy(self):
    method _read_onnx_metadata (line 1533) | def _read_onnx_metadata(self) -> dict:
    method _apply_onnx_metadata (line 1567) | def _apply_onnx_metadata(self):
    method _build_dof_mappings (line 1576) | def _build_dof_mappings(self):
    method _normalize_filter_cutoff_hz (line 1606) | def _normalize_filter_cutoff_hz(raw_values, num_frames: int) -> np.nda...
    method load_motion_data (line 1635) | def load_motion_data(self):
    method load_mujoco_model (line 1855) | def load_mujoco_model(self):
    method _init_camera_config (line 1871) | def _init_camera_config(self):
    method _configure_viewer_camera (line 1905) | def _configure_viewer_camera(self, viewer):
    method _init_video_tools (line 1912) | def _init_video_tools(self, tag: str):
    method _dump_robot_augmented_npz (line 1959) | def _dump_robot_augmented_npz(self) -> None:
    method _close_video_tools (line 2087) | def _close_video_tools(self):
    method _update_camera_lookat (line 2097) | def _update_camera_lookat(self, cam):
    method _maybe_record_frame (line 2108) | def _maybe_record_frame(self):
    method _apply_control (line 2125) | def _apply_control(self, sleep: bool):
    method _compute_pd_torque_command_ref (line 2173) | def _compute_pd_torque_command_ref(self) -> np.ndarray:
    method _get_obs_ref_motion_states (line 2211) | def _get_obs_ref_motion_states(self):
    method _get_obs_ref_motion_states_fut (line 2225) | def _get_obs_ref_motion_states_fut(self):
    method _get_obs_ref_dof_pos_fut (line 2256) | def _get_obs_ref_dof_pos_fut(self):
    method _get_obs_ref_root_height_fut (line 2277) | def _get_obs_ref_root_height_fut(self):
    method _get_obs_ref_dof_pos_cur (line 2302) | def _get_obs_ref_dof_pos_cur(self):
    method _get_obs_ref_dof_vel_cur (line 2311) | def _get_obs_ref_dof_vel_cur(self):
    method _get_obs_ref_motion_filter_cutoff_hz (line 2320) | def _get_obs_ref_motion_filter_cutoff_hz(self):
    method _get_obs_ref_root_height_cur (line 2334) | def _get_obs_ref_root_height_cur(self):
    method _get_obs_ref_gravity_projection_cur (line 2341) | def _get_obs_ref_gravity_projection_cur(self):
    method _get_obs_ref_gravity_projection_fut (line 2358) | def _get_obs_ref_gravity_projection_fut(self):
    method _get_obs_ref_base_linvel_cur (line 2390) | def _get_obs_ref_base_linvel_cur(self):
    method _get_obs_ref_base_linvel_fut (line 2414) | def _get_obs_ref_base_linvel_fut(self):
    method _get_obs_ref_base_angvel_cur (line 2451) | def _get_obs_ref_base_angvel_cur(self):
    method _get_obs_ref_base_angvel_fut (line 2475) | def _get_obs_ref_base_angvel_fut(self):
    method _get_obs_ref_keybody_rel_pos_cur (line 2512) | def _get_obs_ref_keybody_rel_pos_cur(self):
    method _get_obs_ref_keybody_rel_pos_fut (line 2557) | def _get_obs_ref_keybody_rel_pos_fut(self):
    method _get_obs_place_holder (line 2616) | def _get_obs_place_holder(self):
    method _get_obs_vr_ref_motion_states (line 2619) | def _get_obs_vr_ref_motion_states(self):
    method _get_obs_vr_ref_motion_states_fut (line 2632) | def _get_obs_vr_ref_motion_states_fut(self):
    method _get_obs_rel_robot_root_ang_vel (line 2657) | def _get_obs_rel_robot_root_ang_vel(self):
    method _get_obs_last_action (line 2671) | def _get_obs_last_action(self):
    method _get_obs_velocity_command (line 2674) | def _get_obs_velocity_command(self):
    method _get_obs_actor_ref_headling_aligned_vel_cmd (line 2690) | def _get_obs_actor_ref_headling_aligned_vel_cmd(self):
    method _get_obs_actor_velocity_command (line 2694) | def _get_obs_actor_velocity_command(self):
    method _get_obs_actor_projected_gravity (line 2697) | def _get_obs_actor_projected_gravity(self):
    method _get_obs_actor_rel_robot_root_ang_vel (line 2700) | def _get_obs_actor_rel_robot_root_ang_vel(self):
    method _get_obs_actor_dof_pos (line 2703) | def _get_obs_actor_dof_pos(self):
    method _get_obs_actor_dof_vel (line 2706) | def _get_obs_actor_dof_vel(self):
    method _get_obs_actor_last_action (line 2709) | def _get_obs_actor_last_action(self):
    method _get_obs_actor_place_holder (line 2712) | def _get_obs_actor_place_holder(self):
    method _get_obs_actor_ref_dof_pos_fut (line 2715) | def _get_obs_actor_ref_dof_pos_fut(self):
    method _get_obs_actor_ref_dof_pos_cur (line 2718) | def _get_obs_actor_ref_dof_pos_cur(self):
    method _get_obs_actor_ref_motion_filter_cutoff_hz (line 2721) | def _get_obs_actor_ref_motion_filter_cutoff_hz(self):
    method _get_obs_actor_ref_root_height_fut (line 2724) | def _get_obs_actor_ref_root_height_fut(self):
    method _get_obs_actor_ref_root_height_cur (line 2727) | def _get_obs_actor_ref_root_height_cur(self):
    method _get_obs_actor_ref_gravity_projection_cur (line 2730) | def _get_obs_actor_ref_gravity_projection_cur(self):
    method _get_obs_actor_ref_gravity_projection_fut (line 2733) | def _get_obs_actor_ref_gravity_projection_fut(self):
    method _get_obs_actor_ref_base_linvel_cur (line 2736) | def _get_obs_actor_ref_base_linvel_cur(self):
    method _get_obs_actor_ref_base_linvel_fut (line 2739) | def _get_obs_actor_ref_base_linvel_fut(self):
    method _get_obs_actor_ref_base_angvel_cur (line 2742) | def _get_obs_actor_ref_base_angvel_cur(self):
    method _get_obs_actor_ref_base_angvel_fut (line 2745) | def _get_obs_actor_ref_base_angvel_fut(self):
    method _get_obs_actor_ref_keybody_rel_pos_cur (line 2748) | def _get_obs_actor_ref_keybody_rel_pos_cur(self):
    method _get_obs_actor_ref_keybody_rel_pos_fut (line 2751) | def _get_obs_actor_ref_keybody_rel_pos_fut(self):
    method _get_obs_global_anchor_diff (line 2754) | def _get_obs_global_anchor_diff(self):
    method _get_obs_global_anchor_pos_diff (line 2796) | def _get_obs_global_anchor_pos_diff(self):
    method _get_obs_global_anchor_rot_diff (line 2836) | def _get_obs_global_anchor_rot_diff(self):
    method _get_obs_global_bodylink_translation (line 2886) | def _get_obs_global_bodylink_translation(self) -> np.ndarray:
    method _get_obs_global_bodylink_rotation_quat (line 2895) | def _get_obs_global_bodylink_rotation_quat(self) -> np.ndarray:
    method _get_obs_global_bodylink_velocity (line 2905) | def _get_obs_global_bodylink_velocity(self) -> np.ndarray:
    method _get_obs_global_bodylink_angular_velocity (line 2912) | def _get_obs_global_bodylink_angular_velocity(self) -> np.ndarray:
    method ref_global_bodylink_pos (line 2920) | def ref_global_bodylink_pos(self) -> np.ndarray | None:
    method ref_global_bodylink_rot (line 2966) | def ref_global_bodylink_rot(self) -> np.ndarray | None:
    method _draw_ref_body_spheres_to_scene (line 3003) | def _draw_ref_body_spheres_to_scene(
    method _get_obs_rel_anchor_lin_vel (line 3038) | def _get_obs_rel_anchor_lin_vel(self):
    method _get_obs_projected_gravity (line 3055) | def _get_obs_projected_gravity(self):
    method _get_obs_dof_pos (line 3067) | def _get_obs_dof_pos(self):
    method _get_obs_dof_vel (line 3074) | def _get_obs_dof_vel(self):
    method _record_robot_states (line 3079) | def _record_robot_states(self) -> None:
    method load_specific_motion (line 3133) | def load_specific_motion(self, npz_path):
    method reset_state_teleport (line 3162) | def reset_state_teleport(self):
    method save_batch_result (line 3249) | def save_batch_result(self, output_path, meta_info):
    method setup (line 3322) | def setup(self):
    method _create_eval_progress_bar (line 3359) | def _create_eval_progress_bar(self, desc: str, max_steps: int):
    method _advance_eval_frame (line 3366) | def _advance_eval_frame(self, max_steps: int) -> bool:
    method _run_eval_step (line 3376) | def _run_eval_step(self, max_steps: int) -> bool:
    method _build_mjcf_dof_names (line 3384) | def _build_mjcf_dof_names(self):
    method _build_actuator_qpos_indices (line 3398) | def _build_actuator_qpos_indices(self):
    method _build_actuator_name_map (line 3407) | def _build_actuator_name_map(self):
    method _build_actuator_force_range_map (line 3425) | def _build_actuator_force_range_map(self):
    method run_simulation_unitree (line 3450) | def run_simulation_unitree(self):
    method run_simulation_unitree_headless (line 3535) | def run_simulation_unitree_headless(self):
    method run_simulation (line 3579) | def run_simulation(self):
    method _update_policy (line 3586) | def _update_policy(self):
  function _get_config_value (line 3705) | def _get_config_value(config_obj, key: str):
  function _normalize_ckpt_name_list (line 3712) | def _normalize_ckpt_name_list(ckpt_onnx_names):
  function _resolve_multi_ckpt_paths (line 3732) | def _resolve_multi_ckpt_paths(ckpt_onnx_root_dir, ckpt_onnx_names):
  function _resolve_eval_ckpt_paths (line 3789) | def _resolve_eval_ckpt_paths(config_obj):
  function _checkpoint_tag_from_path (line 3810) | def _checkpoint_tag_from_path(ckpt_path: Path) -> str:
  function _build_eval_output_dir (line 3817) | def _build_eval_output_dir(ckpt_path: Path, dataset_name: str) -> Path:
  function _build_onnx_io_dump_dir (line 3823) | def _build_onnx_io_dump_dir(output_dir: str | Path) -> Path:
  function _build_onnx_io_dump_path (line 3827) | def _build_onnx_io_dump_path(
  function _build_onnx_io_dump_readme_text (line 3834) | def _build_onnx_io_dump_readme_text() -> str:
  function write_onnx_io_dump_readme (line 3878) | def write_onnx_io_dump_readme(output_dir: str | Path) -> Path:
  function _allocate_actor_counts (line 3886) | def _allocate_actor_counts(num_checkpoints: int, total_actors: int):
  function _infer_step_from_ckpt_name (line 3896) | def _infer_step_from_ckpt_name(ckpt_name: str):
  function _read_total_macro_row (line 3906) | def _read_total_macro_row(tsv_path: Path):
  function _write_total_macro_summary_table (line 3918) | def _write_total_macro_summary_table(
  function process_config (line 4051) | def process_config(override_config):
  function _create_ray_evaluator (line 4124) | def _create_ray_evaluator(config_dict, model_type):
  function run_mujoco_sim2sim_eval (line 4148) | def run_mujoco_sim2sim_eval(override_config: OmegaConf):
  function main (line 4537) | def main(override_config: OmegaConf):

FILE: holomotion/src/evaluation/eval_velocity_tracking.py
  function load_training_config (line 29) | def load_training_config(
  function main (line 87) | def main(config: OmegaConf):

FILE: holomotion/src/evaluation/find_worst_clips.py
  function find_and_save_bad_clips (line 39) | def find_and_save_bad_clips(
  function main (line 88) | def main() -> None:

FILE: holomotion/src/evaluation/metrics.py
  function quat_inv (line 48) | def quat_inv(q):
  function quat_apply (line 52) | def quat_apply(q, v):
  function p_mpjpe (line 63) | def p_mpjpe(predicted: np.ndarray, target: np.ndarray) -> np.ndarray:
  function _parse_clip_len_from_name (line 123) | def _parse_clip_len_from_name(filename: str) -> Optional[int]:
  function _parse_metadata_entry (line 129) | def _parse_metadata_entry(raw_metadata) -> Dict[str, object]:
  function _extract_robot_control_dt (line 155) | def _extract_robot_control_dt(
  function _extract_low_level_contact_dt (line 175) | def _extract_low_level_contact_dt(
  function _aggregate_sample_metric_to_frames (line 200) | def _aggregate_sample_metric_to_frames(
  function _compute_torque_jump_series (line 220) | def _compute_torque_jump_series(
  function _safe_nanpercentile (line 241) | def _safe_nanpercentile(values: np.ndarray, q: float) -> float:
  function _safe_nanmean (line 249) | def _safe_nanmean(values: np.ndarray) -> float:
  function _safe_nanmedian (line 257) | def _safe_nanmedian(values: np.ndarray) -> float:
  function _compute_rolling_nanmean_max (line 265) | def _compute_rolling_nanmean_max(
  function _integrate_psd_band (line 291) | def _integrate_psd_band(
  function _compute_psd_high_frequency_ratio (line 313) | def _compute_psd_high_frequency_ratio(
  function _compute_torque_chatter_hf_ratio (line 354) | def _compute_torque_chatter_hf_ratio(
  function _compute_torso_roll_pitch_stability_metrics (line 376) | def _compute_torso_roll_pitch_stability_metrics(
  function _compute_expert_switching_js_div (line 411) | def _compute_expert_switching_js_div(
  function _compute_contact_stability_metrics (line 442) | def _compute_contact_stability_metrics(
  function _compute_clip_stability_summary (line 515) | def _compute_clip_stability_summary(
  function _compute_clip_torque_jump_summary (line 581) | def _compute_clip_torque_jump_summary(
  function _per_frame_metrics_from_npz (line 638) | def _per_frame_metrics_from_npz(
  function offline_evaluate_dumped_npzs (line 904) | def offline_evaluate_dumped_npzs(
  function parse_ckpt_and_dataset_from_eval_dirname (line 1284) | def parse_ckpt_and_dataset_from_eval_dirname(
  function run_evaluation (line 1313) | def run_evaluation(

FILE: holomotion/src/evaluation/multi_model_metrics_report.py
  class AnalysisReportGenerator (line 57) | class AnalysisReportGenerator:
    method __init__ (line 60) | def __init__(
    method run (line 89) | def run(self) -> None:
    method _load_and_prepare_data (line 112) | def _load_and_prepare_data(self) -> Optional[pd.DataFrame]:
    method _create_kde_plot (line 156) | def _create_kde_plot(self, metric: str, save_path: Path) -> None:
    method _create_matplotlib_radar_chart (line 195) | def _create_matplotlib_radar_chart(self) -> None:
    method _generate_markdown_report (line 333) | def _generate_markdown_report(self) -> str:
  function parse_args (line 426) | def parse_args() -> argparse.Namespace:

FILE: holomotion/src/evaluation/obs/obs_builder.py
  function get_gravity_orientation (line 24) | def get_gravity_orientation(quaternion: np.ndarray) -> np.ndarray:
  class _CircularBuffer (line 45) | class _CircularBuffer:
    method __init__ (line 51) | def __init__(self, max_len: int, feat_dim: int, device: str):
    method buffer (line 66) | def buffer(self) -> torch.Tensor:
    method append (line 78) | def append(self, data: torch.Tensor) -> None:
  class PolicyObsBuilder (line 96) | class PolicyObsBuilder:
    method __init__ (line 108) | def __init__(
    method reset (line 142) | def reset(self) -> None:
    method _compute_term (line 148) | def _compute_term(
    method build_policy_obs (line 162) | def build_policy_obs(self) -> np.ndarray:

FILE: holomotion/src/evaluation/ray_evaluator_actor.py
  class RayEvaluatorActor (line 30) | class RayEvaluatorActor:
    method __init__ (line 37) | def __init__(self, config_dict, output_dir):
    method run_clip (line 48) | def run_clip(self, file_path):
  function _load_ray_evaluator (line 83) | def _load_ray_evaluator(config_dict, model_type):

FILE: holomotion/src/evaluation/ray_metrics_postprocess.py
  function run_metrics_postprocess_job (line 28) | def run_metrics_postprocess_job(

FILE: holomotion/src/modules/agent_modules.py
  function _module_device (line 35) | def _module_device(module: nn.Module) -> torch.device:
  function _clone_module_for_cpu_export (line 43) | def _clone_module_for_cpu_export(module: nn.Module) -> nn.Module:
  class TensorDictAssembler (line 56) | class TensorDictAssembler(torch.nn.Module):
    method __init__ (line 57) | def __init__(self, schema_config: dict, *, output_mode: str = "flat"):
    method _get_from_data (line 82) | def _get_from_data(data: TensorDict, key: str):
    method _validate_to_seq (line 96) | def _validate_to_seq(
    method _validate_and_flatten (line 120) | def _validate_and_flatten(
    method forward (line 144) | def forward(self, data: TensorDict) -> torch.Tensor:
    method infer_output_dim (line 232) | def infer_output_dim(self, sample: TensorDict) -> int:
  class PPOActorOnnxModule (line 240) | class PPOActorOnnxModule(nn.Module):
    method __init__ (line 241) | def __init__(
    method forward (line 254) | def forward(self, obs: torch.Tensor) -> torch.Tensor:
  class PPOTFActorOnnxModule (line 265) | class PPOTFActorOnnxModule(nn.Module):
    method __init__ (line 266) | def __init__(
    method forward (line 279) | def forward(
  class PPOTFWoKVCacheActorOnnxModule (line 299) | class PPOTFWoKVCacheActorOnnxModule(nn.Module):
    method __init__ (line 300) | def __init__(
    method forward (line 313) | def forward(self, obs: torch.Tensor) -> torch.Tensor:
  class PPOCondTFActorOnnxModule (line 333) | class PPOCondTFActorOnnxModule(nn.Module):
    method __init__ (line 334) | def __init__(
    method forward (line 364) | def forward(
  class PPOActor (line 410) | class PPOActor(TensorDictModuleBase):
    method __init__ (line 411) | def __init__(
    method _process_module_config (line 554) | def _process_module_config(self, module_config_dict, num_actions):
    method _sigma_from_params (line 574) | def _sigma_from_params(self) -> torch.Tensor:
    method _normalize_actor_obs (line 579) | def _normalize_actor_obs(
    method _sigma_like (line 600) | def _sigma_like(self, like: torch.Tensor) -> torch.Tensor:
    method actor (line 617) | def actor(self):
    method flat_obs_dim (line 621) | def flat_obs_dim(self) -> int:
    method export_onnx (line 632) | def export_onnx(
    method forward (line 677) | def forward(
    method update_distribution (line 741) | def update_distribution(self, actor_obs):
    method override_sigma (line 749) | def override_sigma(self, sigma_override: float | torch.Tensor) -> None:
  class PPOCritic (line 778) | class PPOCritic(TensorDictModuleBase):
    method __init__ (line 779) | def __init__(
    method _normalize_critic_obs (line 884) | def _normalize_critic_obs(
    method forward (line 905) | def forward(
  class PPOTFActor (line 947) | class PPOTFActor(PPOActor):
    method __init__ (line 955) | def __init__(
    method _sigma_from_params (line 1001) | def _sigma_from_params(self) -> torch.Tensor:
    method reset_kv_cache (line 1007) | def reset_kv_cache(self, num_envs: int, device):
    method clear_env_cache (line 1011) | def clear_env_cache(self, env_ids: torch.Tensor):
    method onnx_past_key_values_shape (line 1015) | def onnx_past_key_values_shape(
    method onnx_moe_layer_indices (line 1032) | def onnx_moe_layer_indices(self) -> list[int]:
    method onnx_routing_output_names (line 1042) | def onnx_routing_output_names(self) -> list[str]:
    method _maybe_update_aux_router_future_recon_norm (line 1053) | def _maybe_update_aux_router_future_recon_norm(
    method export_onnx (line 1070) | def export_onnx(
    method update_distribution (line 1151) | def update_distribution(self, actor_obs):
    method forward (line 1162) | def forward(
    method sequence_forward_logp (line 1341) | def sequence_forward_logp(
  class PPOTFRefRouterActor (line 1512) | class PPOTFRefRouterActor(PPOTFActor):
    method _leaf_obs_name (line 1514) | def _leaf_obs_name(term: str) -> str:
    method _infer_flat_term_dim (line 1518) | def _infer_flat_term_dim(
    method infer_router_feature_indices (line 1554) | def infer_router_feature_indices(
    method __init__ (line 1591) | def __init__(
  class PPOTFRefRouterSeqActor (line 1640) | class PPOTFRefRouterSeqActor(PPOTFActor):
    method _leaf_obs_name (line 1663) | def _leaf_obs_name(term: str) -> str:
    method _infer_flat_term_dim (line 1667) | def _infer_flat_term_dim(
    method _validate_v2_aux_config (line 1702) | def _validate_v2_aux_config(cls, module_config_dict: dict) -> None:
    method _build_aux_router_future_recon_schema (line 1727) | def _build_aux_router_future_recon_schema(
    method _prepare_aux_router_future_recon (line 1759) | def _prepare_aux_router_future_recon(
    method _infer_shared_ref_layout (line 1784) | def _infer_shared_ref_layout(
    method __init__ (line 1891) | def __init__(
  class PPOTFRefRouterV3Actor (line 1967) | class PPOTFRefRouterV3Actor(PPOTFRefRouterSeqActor):
    method __init__ (line 1968) | def __init__(
  class PPOCondTFActor (line 2048) | class PPOCondTFActor(PPOTFActor):
    method __init__ (line 2051) | def __init__(
    method _infer_future_term_dims (line 2113) | def _infer_future_term_dims(self, obs_example: TensorDict) -> list[int]:
    method flat_obs_dim (line 2151) | def flat_obs_dim(self) -> int:
    method _normalize_state_obs (line 2160) | def _normalize_state_obs(
    method _assemble_state_future (line 2178) | def _assemble_state_future(
    method _split_flat_obs (line 2189) | def _split_flat_obs(
    method export_onnx (line 2218) | def export_onnx(
    method update_distribution (line 2276) | def update_distribution(self, actor_obs):
    method forward (line 2291) | def forward(
    method sequence_forward_logp_cond (line 2469) | def sequence_forward_logp_cond(

FILE: holomotion/src/modules/network_modules.py
  class EmpiricalNormalization (line 26) | class EmpiricalNormalization(nn.Module):
    method __init__ (line 29) | def __init__(
    method mean (line 90) | def mean(self):
    method std (line 94) | def std(self):
    method forward (line 97) | def forward(self, x):
    method normalize_only (line 111) | def normalize_only(self, x):
    method update (line 116) | def update(self, x):
    method inverse (line 147) | def inverse(self, y):
    method sync_stats_across_processes (line 150) | def sync_stats_across_processes(self, accelerator):
  class MLP (line 225) | class MLP(nn.Module):
    method __init__ (line 226) | def __init__(
    method forward (line 288) | def forward(self, x: torch.Tensor) -> torch.Tensor:
  class ConvMLP (line 303) | class ConvMLP(nn.Module):
    method __init__ (line 306) | def __init__(
    method forward (line 356) | def forward(self, hist_seq: torch.Tensor) -> torch.Tensor:
  class ReferenceMotionConvRouterEncoder (line 363) | class ReferenceMotionConvRouterEncoder(nn.Module):
    method __init__ (line 366) | def __init__(
    method forward (line 434) | def forward(self, seq: torch.Tensor) -> torch.Tensor:
  class SingleQueryAttentionPool (line 450) | class SingleQueryAttentionPool(nn.Module):
    method __init__ (line 451) | def __init__(self, d_model: int):
    method forward (line 460) | def forward(
  class GroupedMoETransformerPolicy (line 498) | class GroupedMoETransformerPolicy(nn.Module):
    method __init__ (line 516) | def __init__(
    method _load_from_state_dict (line 913) | def _load_from_state_dict(
    method _router_no_grad_context (line 962) | def _router_no_grad_context(self):
    method _apply_base_freeze_router_state (line 967) | def _apply_base_freeze_router_state(self) -> None:
    method _apply_freeze_router_state (line 972) | def _apply_freeze_router_state(self) -> None:
    method _set_cos_sin_cache (line 979) | def _set_cos_sin_cache(self, seq_len):
    method get_cos_sin (line 996) | def get_cos_sin(self, x, position_ids):
    method _init_last_moe_router_shift_state (line 1008) | def _init_last_moe_router_shift_state(self, num_envs: int, device) -> ...
    method _accumulate_last_moe_router_shift (line 1039) | def _accumulate_last_moe_router_shift(
    method get_last_moe_router_shift_stats (line 1085) | def get_last_moe_router_shift_stats(
    method reset_kv_cache (line 1095) | def reset_kv_cache(self, num_envs: int, device):
    method clear_env_cache (line 1123) | def clear_env_cache(self, env_ids: torch.Tensor | None):
    method set_collect_routing_stats (line 1156) | def set_collect_routing_stats(self, collect: bool) -> None:
    method reset_routing_stats (line 1165) | def reset_routing_stats(self) -> None:
    method clear_router_distribution_cache (line 1170) | def clear_router_distribution_cache(self) -> None:
    method _set_capture_router_distributions (line 1178) | def _set_capture_router_distributions(self, capture: bool) -> None:
    method _set_capture_router_features (line 1184) | def _set_capture_router_features(
    method apply_dynamic_bias_update_from_stats (line 1197) | def apply_dynamic_bias_update_from_stats(self) -> None:
    method _make_causal_mask (line 1202) | def _make_causal_mask(self, T: int, device) -> torch.Tensor:
    method _forward_layers_range (line 1206) | def _forward_layers_range(
    method _forward_layers (line 1332) | def _forward_layers(
    method _compute_router_hidden (line 1362) | def _compute_router_hidden(self, x: torch.Tensor) -> torch.Tensor | None:
    method sequence_mu (line 1365) | def sequence_mu(
    method sequence_hidden (line 1441) | def sequence_hidden(
    method _embed_future_tokens (line 1484) | def _embed_future_tokens(
    method sequence_mu_cond (line 1525) | def sequence_mu_cond(
    method predict_aux_from_pre_moe (line 1629) | def predict_aux_from_pre_moe(
    method predict_aux_router_command_from_router_features (line 1707) | def predict_aux_router_command_from_router_features(
    method update_aux_router_future_recon_normalizer (line 1726) | def update_aux_router_future_recon_normalizer(
    method normalize_aux_router_future_recon_target (line 1748) | def normalize_aux_router_future_recon_target(
    method predict_aux_router_future_recon_from_router_hidden (line 1771) | def predict_aux_router_future_recon_from_router_hidden(
    method single_step_mu_cond (line 1790) | def single_step_mu_cond(
    method forward (line 1914) | def forward(
    method single_step_mu (line 1930) | def single_step_mu(self, x: torch.Tensor) -> torch.Tensor:
    method _forward_inference_onnx (line 2012) | def _forward_inference_onnx(
  class ReferenceRoutedGroupedMoETransformerPolicy (line 2112) | class ReferenceRoutedGroupedMoETransformerPolicy(GroupedMoETransformerPo...
    method __init__ (line 2113) | def __init__(
    method _apply_freeze_router_state (line 2185) | def _apply_freeze_router_state(self) -> None:
    method _compute_router_hidden (line 2189) | def _compute_router_hidden(self, x: torch.Tensor) -> torch.Tensor | None:
    method _forward_inference_onnx_cond (line 2201) | def _forward_inference_onnx_cond(
  class ReferenceRoutedGroupedMoETransformerPolicyV2 (line 2281) | class ReferenceRoutedGroupedMoETransformerPolicyV2(
    method __init__ (line 2286) | def __init__(
    method _apply_freeze_router_state (line 2545) | def _apply_freeze_router_state(self) -> None:
    method _build_shared_ref_tokens (line 2556) | def _build_shared_ref_tokens(
    method _build_shared_ref_tokens_single_step (line 2574) | def _build_shared_ref_tokens_single_step(
    method _split_actor_ref_inputs (line 2604) | def _split_actor_ref_inputs(
    method _encode_future_tokens (line 2639) | def _encode_future_tokens(self, ref_fut_x: torch.Tensor) -> torch.Tensor:
    method _pool_router_context (line 2654) | def _pool_router_context(
    method _apply_actor_ref_film (line 2679) | def _apply_actor_ref_film(
    method _actor_film_gain (line 2695) | def _actor_film_gain(self) -> torch.Tensor:
    method _normalize_actor_film_delta (line 2700) | def _normalize_actor_film_delta(self, delta: torch.Tensor) -> torch.Te...
    method _ensure_internal_cache_device (line 2704) | def _ensure_internal_cache_device(
    method reset_kv_cache (line 2728) | def reset_kv_cache(self, num_envs: int, device):
    method clear_env_cache (line 2765) | def clear_env_cache(self, env_ids: torch.Tensor | None):
    method predict_aux_from_pre_moe (line 2801) | def predict_aux_from_pre_moe(
    method sequence_mu (line 2826) | def sequence_mu(
    method sequence_hidden (line 2911) | def sequence_hidden(
    method forward (line 2965) | def forward(
    method single_step_mu (line 2980) | def single_step_mu(self, x: torch.Tensor) -> torch.Tensor:
    method _forward_inference_onnx (line 3082) | def _forward_inference_onnx(
  class ReferenceRoutedGroupedMoETransformerPolicyV3 (line 3207) | class ReferenceRoutedGroupedMoETransformerPolicyV3(
    method __init__ (line 3212) | def __init__(
    method _apply_freeze_router_state (line 3406) | def _apply_freeze_router_state(self) -> None:
    method _build_router_ref_motion (line 3415) | def _build_router_ref_motion(
    method _build_shared_router_summary (line 3433) | def _build_shared_router_summary(
    method _build_router_h_per_layer (line 3440) | def _build_router_h_per_layer(
    method _build_ref_hist_hidden (line 3454) | def _build_ref_hist_hidden(
    method _build_ref_hist_hidden_single_step (line 3469) | def _build_ref_hist_hidden_single_step(
    method predict_aux_from_pre_moe (line 3496) | def predict_aux_from_pre_moe(
    method sequence_mu (line 3508) | def sequence_mu(
    method sequence_hidden (line 3584) | def sequence_hidden(
    method forward (line 3628) | def forward(
    method single_step_mu (line 3643) | def single_step_mu(self, x: torch.Tensor) -> torch.Tensor:
    method _forward_inference_onnx (line 3716) | def _forward_inference_onnx(
  class GroupedMoEBlock (line 3805) | class GroupedMoEBlock(nn.Module):
    method __init__ (line 3806) | def __init__(
    method reset_parameters (line 3994) | def reset_parameters(self) -> None:
    method _load_from_state_dict (line 3998) | def _load_from_state_dict(
    method _apply_freeze_router_state (line 4085) | def _apply_freeze_router_state(self) -> None:
    method reset_routing_stats (line 4088) | def reset_routing_stats(self) -> None:
    method accumulate_routing_stats (line 4103) | def accumulate_routing_stats(self, topk_idx: torch.Tensor) -> None:
    method _apply_bias_update_from_counts (line 4110) | def _apply_bias_update_from_counts(self, counts: torch.Tensor) -> None:
    method apply_bias_update_from_counts (line 4151) | def apply_bias_update_from_counts(self) -> None:
    method _update_routed_expert_stats_and_floor_loss (line 4157) | def _update_routed_expert_stats_and_floor_loss(
    method _compute_sparse_experts (line 4260) | def _compute_sparse_experts(
    method _compute_with_grouped_mm (line 4302) | def _compute_with_grouped_mm(
    method _compute_with_topk_selection (line 4328) | def _compute_with_topk_selection(
    method _compute_with_loop_fallback (line 4368) | def _compute_with_loop_fallback(
    method compute_moe_ffn (line 4391) | def compute_moe_ffn(
    method forward (line 4500) | def forward(
  class ModernTransformerBlock (line 4542) | class ModernTransformerBlock(nn.Module):
    method __init__ (line 4551) | def __init__(
    method forward (line 4598) | def forward(
  class DeepseekV3MLP (line 4642) | class DeepseekV3MLP(nn.Module):
    method __init__ (line 4645) | def __init__(self, hidden_size=None, intermediate_size=None):
    method forward (line 4662) | def forward(self, x: torch.Tensor) -> torch.Tensor:
  class RMSNorm (line 4669) | class RMSNorm(nn.Module):
    method __init__ (line 4672) | def __init__(self, dim: int, eps: float = 1e-6):
    method forward (line 4677) | def forward(self, x: torch.Tensor) -> torch.Tensor:
  class ModernAttention (line 4683) | class ModernAttention(nn.Module):
    method __init__ (line 4697) | def __init__(
    method forward (line 4749) | def forward(
    method forward_single_token (line 4823) | def forward_single_token(
  class ModernCrossAttention (line 4937) | class ModernCrossAttention(nn.Module):
    method __init__ (line 4940) | def __init__(
    method forward (line 4987) | def forward(
  function repeat_kv (line 5051) | def repeat_kv(hidden_states: torch.Tensor, n_rep: int) -> torch.Tensor:
  function export_safe_scaled_dot_product_attention (line 5074) | def export_safe_scaled_dot_product_attention(
  function rotate_half (line 5114) | def rotate_half(x):
  function apply_rotary_pos_emb (line 5121) | def apply_rotary_pos_emb(q, k, cos, sin, position_ids=None, unsqueeze_di...
  function _grouped_linear (line 5138) | def _grouped_linear(

FILE: holomotion/src/motion_retargeting/gmr_to_holomotion.py
  function quaternion_to_axis_angle (line 45) | def quaternion_to_axis_angle(q: torch.Tensor) -> torch.Tensor:
  function dof_to_pose_aa (line 58) | def dof_to_pose_aa(
  function load_any_pkl (line 91) | def load_any_pkl(p: Path):
  function unwrap_source (line 96) | def unwrap_source(obj) -> Dict[str, np.ndarray]:
  function make_motion_key (line 107) | def make_motion_key(p: Path, src_dir: Path) -> str:
  function key_to_filename (line 112) | def key_to_filename(key: str) -> str:
  function get_ref_schema (line 116) | def get_ref_schema(
  function infer_T (line 157) | def infer_T(src_inner: Dict[str, np.ndarray]) -> Optional[int]:
  function build_inner_from_source (line 177) | def build_inner_from_source(
  function to_torch (line 280) | def to_torch(tensor):
  function batch_interpolate_tensor (line 287) | def batch_interpolate_tensor(
  function fast_interpolate_motion (line 364) | def fast_interpolate_motion(motion_dict, source_fps, target_fps):
  function process_single_motion (line 406) | def process_single_motion(
  class InMemoryAlignedLoader (line 658) | class InMemoryAlignedLoader:
    method __init__ (line 661) | def __init__(self, mapping: Dict[str, Dict[str, object]]):
    method keys (line 664) | def keys(self) -> List[str]:
    method __len__ (line 667) | def __len__(self):
    method __getitem__ (line 670) | def __getitem__(self, k: str):
    method load (line 673) | def load(self, k: str):
    method get (line 676) | def get(self, k: str, default=None):
  function arrays_for_npz (line 680) | def arrays_for_npz(
  class MotionProcessorActor (line 712) | class MotionProcessorActor:
    method __init__ (line 717) | def __init__(
    method _dof_to_pose_aa_cached (line 728) | def _dof_to_pose_aa_cached(
    method process_pkl (line 750) | def process_pkl(
  function main (line 795) | def main(cfg: DictConfig) -> None:

FILE: holomotion/src/motion_retargeting/holomotion_fk.py
  class MJCFParser (line 31) | class MJCFParser:
    method __init__ (line 32) | def __init__(self, robot_file_path: str) -> None:
    method parse_vec (line 36) | def parse_vec(
    method _find_parent (line 49) | def _find_parent(
    method _select_include_children (line 59) | def _select_include_children(
    method _resolve_includes (line 73) | def _resolve_includes(cls, root: ETree.Element, base_dir: str) -> None:
    method load_root (line 96) | def load_root(self) -> ETree.Element:
    method parse (line 101) | def parse(
  class URDFParser (line 206) | class URDFParser:
    method __init__ (line 207) | def __init__(self, urdf_path: str) -> None:
    method _as_tf (line 211) | def _as_tf(
    method _load_chain (line 220) | def _load_chain(self) -> pk.Chain:
    method parse (line 225) | def parse(
  class HoloMotionFK (line 353) | class HoloMotionFK(torch.nn.Module):
    method __init__ (line 354) | def __init__(
    method forward (line 426) | def forward(
    method _forward_impl (line 532) | def _forward_impl(

FILE: holomotion/src/motion_retargeting/holomotion_preprocess.py
  function compute_slices (line 48) | def compute_slices(
  function _reshape_time_flat (line 66) | def _reshape_time_flat(a: np.ndarray) -> Tuple[np.ndarray, Tuple[int, .....
  function _butterworth_lowpass_smooth_time (line 72) | def _butterworth_lowpass_smooth_time(
  function _quat_normalize (line 98) | def _quat_normalize(q: np.ndarray) -> np.ndarray:
  function _quat_hemisphere_align (line 104) | def _quat_hemisphere_align(q: np.ndarray) -> np.ndarray:
  function _quat_conjugate (line 118) | def _quat_conjugate(q: np.ndarray) -> np.ndarray:
  function _quat_multiply (line 124) | def _quat_multiply(a: np.ndarray, b: np.ndarray) -> np.ndarray:
  function _finite_difference_time (line 135) | def _finite_difference_time(a: np.ndarray, dt: float) -> np.ndarray:
  function _angular_velocity_from_quat (line 148) | def _angular_velocity_from_quat(
  function butterworth_filter_ref_arrays (line 157) | def butterworth_filter_ref_arrays(
  function _summary (line 168) | def _summary(arr: np.ndarray) -> Dict[str, float]:
  function _ds_summary (line 190) | def _ds_summary(arr: np.ndarray) -> Dict[str, float]:
  function _interpolate_linear (line 212) | def _interpolate_linear(
  function _interpolate_quaternions_slerp (line 232) | def _interpolate_quaternions_slerp(
  function _extract_yaw_only_quat (line 259) | def _extract_yaw_only_quat(quat: np.ndarray) -> np.ndarray:
  function _dof_to_pose_aa (line 275) | def _dof_to_pose_aa(
  function _compute_fk_motion (line 310) | def _compute_fk_motion(
  class ProcessedClip (line 373) | class ProcessedClip:
  class HoloMotionPreprocessor (line 379) | class HoloMotionPreprocessor:
    method __init__ (line 387) | def __init__(
    method _resolve_pipeline (line 403) | def _resolve_pipeline(self, pipeline: Optional[List[str]]) -> List[str]:
    method process_clip (line 408) | def process_clip(self, clip: ProcessedClip) -> List[ProcessedClip]:
    method _apply_slicing (line 450) | def _apply_slicing(self, clip: ProcessedClip) -> List[ProcessedClip]:
    method _apply_filtering (line 516) | def _apply_filtering(self, clip: ProcessedClip) -> ProcessedClip:
    method _apply_filename_as_motionkey (line 538) | def _apply_filename_as_motionkey(
    method _apply_add_legacy_keys (line 560) | def _apply_add_legacy_keys(self, clip: ProcessedClip) -> ProcessedClip:
    method _apply_legacy_to_ref_keys (line 599) | def _apply_legacy_to_ref_keys(self, clip: ProcessedClip) -> ProcessedC...
    method _get_humanoid_fk (line 639) | def _get_humanoid_fk(self) -> HumanoidBatch:
    method _get_default_dof_pos (line 649) | def _get_default_dof_pos(self) -> np.ndarray:
    method _apply_add_padding (line 660) | def _apply_add_padding(self, clip: ProcessedClip) -> ProcessedClip:
    method process_npz_file (line 820) | def process_npz_file(self, npz_path: Path) -> List[ProcessedClip]:
    method run_on_directory (line 843) | def run_on_directory(
    method _run_on_directory_sequential (line 876) | def _run_on_directory_sequential(
    method _run_on_directory_ray (line 898) | def _run_on_directory_ray(
    method tag_directory (line 951) | def tag_directory(self, clips_dir: Path, tags_path: Path) -> None:
  class PreprocessorActor (line 1064) | class PreprocessorActor:
    method __init__ (line 1067) | def __init__(
    method process_npz_file (line 1086) | def process_npz_file(self, npz_path_str: str) -> List[ProcessedClip]:
  function main (line 1096) | def main(cfg: DictConfig) -> None:

FILE: holomotion/src/motion_retargeting/kinematic_filter.py
  function _eval_rule (line 30) | def _eval_rule(val: float, op: str, thr: float) -> bool:
  function _deep_get (line 46) | def _deep_get(container: Dict[str, Any], parts: List[str]) -> Optional[f...
  function _resolve_value (line 57) | def _resolve_value(
  function filter_with_schema (line 78) | def filter_with_schema(
  function _default_schema_path (line 131) | def _default_schema_path() -> Path:
  function run (line 144) | def run(
  function main (line 244) | def main(cfg: DictConfig) -> None:

FILE: holomotion/src/motion_retargeting/pack_hdf5_v2.py
  function _ensure_dir (line 32) | def _ensure_dir(path: str) -> None:
  class ArraySpec (line 37) | class ArraySpec:
  class ClipEntry (line 44) | class ClipEntry:
  class Hdf5ShardWriter (line 50) | class Hdf5ShardWriter:
    method __init__ (line 51) | def __init__(
    method append_motion (line 90) | def append_motion(
    method finalize (line 118) | def finalize(self) -> Dict[str, Any]:
  function _normalize_root_list (line 146) | def _normalize_root_list(value: Any) -> List[str]:
  function _discover_motion_entries (line 156) | def _discover_motion_entries(roots: List[str]) -> List[ClipEntry]:
  function _load_metadata_json (line 185) | def _load_metadata_json(npz_path: Path) -> Tuple[str, Dict[str, Any]]:
  function _cast_array (line 201) | def _cast_array(array: np.ndarray, name: str, npz_path: Path) -> np.ndar...
  function _discover_array_specs (line 220) | def _discover_array_specs(first_npz: Path) -> List[ArraySpec]:
  function _load_npz_arrays (line 253) | def _load_npz_arrays(
  function _relative_npz_path (line 310) | def _relative_npz_path(npz_path: Path, roots: List[str]) -> str:
  function _nan_array_names (line 322) | def _nan_array_names(arrays: Dict[str, np.ndarray]) -> List[str]:
  function _estimate_bytes_for_motion (line 333) | def _estimate_bytes_for_motion(npz_path: Path, mode: str) -> int:
  function main (line 369) | def main(cfg: OmegaConf) -> None:

FILE: holomotion/src/motion_retargeting/reference_filtering.py
  function _reshape_time_flat (line 27) | def _reshape_time_flat(a: np.ndarray) -> Tuple[np.ndarray, Tuple[int, .....
  function _butterworth_lowpass_smooth_time (line 33) | def _butterworth_lowpass_smooth_time(
  function _quat_normalize (line 59) | def _quat_normalize(q: np.ndarray) -> np.ndarray:
  function _quat_hemisphere_align (line 65) | def _quat_hemisphere_align(q: np.ndarray) -> np.ndarray:
  function _quat_conjugate (line 79) | def _quat_conjugate(q: np.ndarray) -> np.ndarray:
  function _quat_multiply (line 85) | def _quat_multiply(a: np.ndarray, b: np.ndarray) -> np.ndarray:
  function _finite_difference_time (line 96) | def _finite_difference_time(a: np.ndarray, dt: float) -> np.ndarray:
  function _angular_velocity_from_quat (line 109) | def _angular_velocity_from_quat(
  function butterworth_filter_ref_arrays (line 118) | def butterworth_filter_ref_arrays(
  function butterworth_filter_root_dof_arrays (line 162) | def butterworth_filter_root_dof_arrays(

FILE: holomotion/src/motion_retargeting/utils/rotation_conversions.py
  function wxyz_to_xyzw (line 13) | def wxyz_to_xyzw(quat):
  function xyzw_to_wxyz (line 17) | def xyzw_to_wxyz(quat):
  function quaternion_to_matrix (line 48) | def quaternion_to_matrix(quaternions: torch.Tensor) -> torch.Tensor:
  function _copysign (line 79) | def _copysign(a: torch.Tensor, b: torch.Tensor) -> torch.Tensor:
  function _sqrt_positive_part (line 99) | def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor:
  function matrix_to_quaternion (line 110) | def matrix_to_quaternion(matrix: torch.Tensor) -> torch.Tensor:
  function _axis_angle_rotation (line 176) | def _axis_angle_rotation(axis: str, angle: torch.Tensor) -> torch.Tensor:
  function euler_angles_to_matrix (line 206) | def euler_angles_to_matrix(
  function _angle_from_tan (line 239) | def _angle_from_tan(
  function _index_from_letter (line 272) | def _index_from_letter(letter: str) -> int:
  function matrix_to_euler_angles (line 282) | def matrix_to_euler_angles(
  function random_quaternions (line 326) | def random_quaternions(
  function random_rotations (line 353) | def random_rotations(
  function random_rotation (line 374) | def random_rotation(
  function standardize_quaternion (line 391) | def standardize_quaternion(quaternions: torch.Tensor) -> torch.Tensor:
  function quaternion_raw_multiply (line 407) | def quaternion_raw_multiply(a: torch.Tensor, b: torch.Tensor) -> torch.T...
  function quaternion_multiply (line 429) | def quaternion_multiply(a: torch.Tensor, b: torch.Tensor) -> torch.Tensor:
  function quaternion_invert (line 448) | def quaternion_invert(quaternion: torch.Tensor) -> torch.Tensor:
  function quaternion_apply (line 463) | def quaternion_apply(
  function axis_angle_to_matrix (line 489) | def axis_angle_to_matrix(axis_angle: torch.Tensor) -> torch.Tensor:
  function matrix_to_axis_angle (line 505) | def matrix_to_axis_angle(matrix: torch.Tensor) -> torch.Tensor:
  function axis_angle_to_quaternion (line 521) | def axis_angle_to_quaternion(axis_angle: torch.Tensor) -> torch.Tensor:
  function quaternion_to_axis_angle (line 554) | def quaternion_to_axis_angle(quaternions: torch.Tensor) -> torch.Tensor:
  function rotation_6d_to_matrix (line 585) | def rotation_6d_to_matrix(d6: torch.Tensor) -> torch.Tensor:
  function matrix_to_rotation_6d (line 611) | def matrix_to_rotation_6d(matrix: torch.Tensor) -> torch.Tensor:

FILE: holomotion/src/motion_retargeting/utils/torch_humanoid_batch.py
  class HumanoidBatch (line 53) | class HumanoidBatch:
    method __init__ (line 54) | def __init__(self, cfg, device=None):
    method from_mjcf (line 207) | def from_mjcf(self, path):
    method fk_batch (line 285) | def fk_batch(
    method convert_to_proper_kinematic (line 380) | def convert_to_proper_kinematic(self, return_dict):
    method forward_kinematics_batch (line 427) | def forward_kinematics_batch(
    method _compute_velocity (line 488) | def _compute_velocity(p, time_delta, guassian_filter=True):
    method _compute_angular_velocity (line 500) | def _compute_angular_velocity(r, time_delta: float, guassian_filter=Tr...
    method load_mesh (line 519) | def load_mesh(self):
    method mesh_fk (line 610) | def mesh_fk(self, pose=None, trans=None):
  function main (line 670) | def main(cfg: DictConfig):

FILE: holomotion/src/motion_retargeting/utils/visualize_with_mujoco.py
  class OffscreenRenderer (line 34) | class OffscreenRenderer:
    method __init__ (line 37) | def __init__(self, model, height, width):
    method render (line 66) | def render(
    method close (line 92) | def close(self):
  function _get_key_prefix_order (line 96) | def _get_key_prefix_order(cfg: DictConfig) -> List[str]:
  function _get_ref_key_prefix_order (line 121) | def _get_ref_key_prefix_order(cfg: DictConfig) -> List[str]:
  function _pick_with_prefixes (line 140) | def _pick_with_prefixes(
  function _resolve_visualization_arrays (line 164) | def _resolve_visualization_arrays(
  function _draw_body_spheres_to_scene (line 198) | def _draw_body_spheres_to_scene(
  function _load_npz_as_motion (line 234) | def _load_npz_as_motion(
  function _collect_all_npz (line 268) | def _collect_all_npz(
  function _infer_fps_from_meta (line 300) | def _infer_fps_from_meta(
  function _time_length (line 310) | def _time_length(*arrays) -> int:
  function process_single_motion_remote_npz (line 321) | def process_single_motion_remote_npz(
  class MotionRendererNPZ (line 423) | class MotionRendererNPZ:
    method process_single_motion (line 424) | def process_single_motion(
  function main (line 521) | def main(cfg: DictConfig) -> None:

FILE: holomotion/src/training/h5_dataloader.py
  function _cpu_only_dataloader_worker_init_fn (line 90) | def _cpu_only_dataloader_worker_init_fn(worker_id: int) -> None:
  function _allocate_batch_counts (line 96) | def _allocate_batch_counts(
  function _configure_weighted_bins (line 134) | def _configure_weighted_bins(
  function _collect_manifest_keys (line 300) | def _collect_manifest_keys(
  function _normalize_online_filter_cfg (line 336) | def _normalize_online_filter_cfg(
  function preview_weighted_bin_from_manifest (line 370) | def preview_weighted_bin_from_manifest(
  function preview_uniform_from_manifest (line 417) | def preview_uniform_from_manifest(
  function preview_sampling_from_cfg (line 585) | def preview_sampling_from_cfg(motion_cfg: Mapping[str, Any]) -> None:
  class _WorldFrameNormalizeTransform (line 650) | class _WorldFrameNormalizeTransform:
    method _apply_prefix (line 654) | def _apply_prefix(
    method __call__ (line 700) | def __call__(self, arrays: Dict[str, Tensor]) -> None:
  class _CpuFKTransform (line 755) | class _CpuFKTransform:
    method __init__ (line 758) | def __init__(self, robot_file_path: str) -> None:
    method __call__ (line 764) | def __call__(
  class MotionWindow (line 797) | class MotionWindow:
  class MotionClipSample (line 809) | class MotionClipSample:
  class ClipBatch (line 829) | class ClipBatch:
    method collate_fn (line 849) | def collate_fn(samples: List[MotionClipSample]) -> "ClipBatch":
  class Hdf5RootDofDataset (line 901) | class Hdf5RootDofDataset(Dataset[MotionClipSample]):
    method __init__ (line 904) | def __init__(
    method set_progress_counter (line 1046) | def set_progress_counter(self, counter: Optional[mp.Value]) -> None:
    method _normalize_motion_key (line 1050) | def _normalize_motion_key(value: Any) -> Optional[str]:
    method _build_motion_key_aliases (line 1063) | def _build_motion_key_aliases(
    method _enumerate_windows (line 1085) | def _enumerate_windows(self) -> List[MotionWindow]:
    method __len__ (line 1130) | def __len__(self) -> int:
    method _cast_motion_np (line 1134) | def _cast_motion_np(np_array: np.ndarray, name: str) -> Tensor:
    method _make_scalar_metadata_tensor (line 1151) | def _make_scalar_metadata_tensor(value: float, length: int) -> Tensor:
    method _sample_online_filter_cutoff_hz (line 1154) | def _sample_online_filter_cutoff_hz(self) -> float:
    method _add_online_filtered_reference_tensors (line 1167) | def _add_online_filtered_reference_tensors(
    method _derive_root_state_tensors (line 1193) | def _derive_root_state_tensors(
    method __getitem__ (line 1214) | def __getitem__(self, index: int) -> MotionClipSample:
    method _get_shard_handle (line 1294) | def _get_shard_handle(self, shard_index: int) -> h5py.File:
    method close (line 1335) | def close(self) -> None:
    method __del__ (line 1342) | def __del__(self) -> None:
  function _normalize_root_list (line 1346) | def _normalize_root_list(value: Any) -> List[str]:
  function build_motion_datasets_from_cfg (line 1354) | def build_motion_datasets_from_cfg(
  function _cache_collate_fn (line 1491) | def _cache_collate_fn(
  class InfiniteDistributedSampler (line 1508) | class InfiniteDistributedSampler(DistributedSampler):
    method __iter__ (line 1511) | def __iter__(self):
  class InfiniteRandomSampler (line 1520) | class InfiniteRandomSampler(Sampler[int]):
    method __init__ (line 1523) | def __init__(self, data_source: Dataset, seed: int = 0) -> None:
    method __iter__ (line 1528) | def __iter__(self):
    method __len__ (line 1538) | def __len__(self) -> int:
  class WeightedBinInfiniteSampler (line 1543) | class WeightedBinInfiniteSampler(Sampler[int]):
    method __init__ (line 1546) | def __init__(
    method __iter__ (line 1567) | def __iter__(self):
    method __len__ (line 1610) | def __len__(self) -> int:
  class PrioritizedInfiniteSampler (line 1614) | class PrioritizedInfiniteSampler(Sampler[int]):
    method __init__ (line 1617) | def __init__(
    method state_version (line 1680) | def state_version(self) -> int:
    method get_pool_statistics (line 1683) | def get_pool_statistics(self) -> Optional[Dict[str, float]]:
    method _aggregate_by_index (line 1689) | def _aggregate_by_index(
    method _pool_batch_sizes (line 1713) | def _pool_batch_sizes(self) -> Tuple[int, int]:
    method _priority_scores_for_indices (line 1721) | def _priority_scores_for_indices(self, indices: Tensor) -> Tensor:
    method _pool_metric_stats (line 1738) | def _pool_metric_stats(self) -> Dict[str, float]:
    method get_window_state_for_indices (line 1756) | def get_window_state_for_indices(
    method _rebuild_prioritized_pool (line 1796) | def _rebuild_prioritized_pool(self, candidate_indices: Tensor) -> None:
    method maybe_update_from_observations (line 1861) | def maybe_update_from_observations(
    method _reset_uniform_cycle (line 1983) | def _reset_uniform_cycle(self) -> None:
    method _next_uniform_index (line 2018) | def _next_uniform_index(self) -> int:
    method _sample_uniform_indices (line 2028) | def _sample_uniform_indices(
    method _sample_prioritized_indices (line 2058) | def _sample_prioritized_indices(
    method _sample_batch_indices (line 2069) | def _sample_batch_indices(self, generator: torch.Generator) -> Tensor:
    method get_scores_for_indices (line 2100) | def get_scores_for_indices(self, window_indices: Tensor) -> Tensor:
    method __iter__ (line 2109) | def __iter__(self):
    method __len__ (line 2125) | def __len__(self) -> int:
  class Hdf5MotionDataset (line 2129) | class Hdf5MotionDataset(Dataset[MotionClipSample]):
    method __init__ (line 2132) | def __init__(
    method set_progress_counter (line 2238) | def set_progress_counter(self, counter: Optional[mp.Value]) -> None:
    method _enumerate_windows (line 2241) | def _enumerate_windows(self) -> List[MotionWindow]:
    method __len__ (line 2288) | def __len__(self) -> int:
    method __getitem__ (line 2291) | def __getitem__(self, index: int) -> MotionClipSample:
    method _get_shard_handle (line 2367) | def _get_shard_handle(self, shard_index: int) -> h5py.File:
    method close (line 2407) | def close(self) -> None:
  class MotionClipBatchCache (line 2415) | class MotionClipBatchCache:
    method _infer_cuda_device_index (line 2419) | def _infer_cuda_device_index() -> int:
    method _normalize_stage_device (line 2429) | def _normalize_stage_device(
    method __init__ (line 2474) | def __init__(
    method current_batch (line 2584) | def current_batch(self) -> ClipBatch:
    method max_frame_length (line 2589) | def max_frame_length(self) -> int:
    method clip_count (line 2593) | def clip_count(self) -> int:
    method mode (line 2597) | def mode(self) -> str:
    method swap_index (line 2601) | def swap_index(self) -> int:
    method num_batches (line 2605) | def num_batches(self) -> int:
    method set_mode (line 2610) | def set_mode(self, mode: str) -> None:
    method set_seed (line 2619) | def set_seed(self, seed: int, *, reinitialize: bool = True) -> None:
    method advance (line 2625) | def advance(self) -> None:
    method enable_weighted_bin_sampling (line 2654) | def enable_weighted_bin_sampling(
    method enable_cache_curriculum_sampling (line 2734) | def enable_cache_curriculum_sampling(
    method _prepare_cache_curriculum_dump_dir (line 2780) | def _prepare_cache_curriculum_dump_dir(
    method set_cache_curriculum_dump_dir (line 2794) | def set_cache_curriculum_dump_dir(self, dump_dir: str) -> None:
    method update_cache_curriculum (line 2800) | def update_cache_curriculum(
    method _refresh_prefetched_batch (line 2825) | def _refresh_prefetched_batch(self) -> None:
    method _maybe_dump_cache_curriculum_scores_json (line 2830) | def _maybe_dump_cache_curriculum_scores_json(
    method cache_curriculum_scores_for_window_indices (line 2938) | def cache_curriculum_scores_for_window_indices(
    method cache_curriculum_pool_statistics (line 2952) | def cache_curriculum_pool_statistics(
    method sample_env_assignments (line 2959) | def sample_env_assignments(
    method _prepare_gather_indices (line 2996) | def _prepare_gather_indices(
    method gather_tensor (line 3028) | def gather_tensor(
    method lengths_for_indices (line 3049) | def lengths_for_indices(self, clip_indices: Tensor) -> Tensor:
    method motion_keys_for_indices (line 3053) | def motion_keys_for_indices(self, clip_indices: Tensor) -> List[str]:
    method window_indices_for_indices (line 3060) | def window_indices_for_indices(self, clip_indices: Tensor) -> Tensor:
    method _prime_buffers (line 3066) | def _prime_buffers(self) -> None:
    method _fetch_next_batch (line 3094) | def _fetch_next_batch(self) -> ClipBatch:
    method _load_next_batch (line 3109) | def _load_next_batch(self) -> ClipBatch:
    method _load_next_batch_raw (line 3114) | def _load_next_batch_raw(self) -> ClipBatch:
    method _load_next_batch_with_progress (line 3125) | def _load_next_batch_with_progress(self) -> ClipBatch:
    method _stage_batch_blocking (line 3166) | def _stage_batch_blocking(self, batch: ClipBatch) -> ClipBatch:
    method _stage_batch (line 3199) | def _stage_batch(
    method _build_iterator (line 3269) | def _build_iterator(
    method _build_dataloader (line 3280) | def _build_dataloader(self) -> None:
    method close (line 3417) | def close(self) -> None:
    method __del__ (line 3435) | def __del__(self) -> None:
    method _loader_timeout_seconds (line 3438) | def _loader_timeout_seconds(self) -> float:
    method _result_timeout (line 3443) | def _result_timeout(self) -> Optional[float]:
    method _should_use_batch_progress (line 3449) | def _should_use_batch_progress(self) -> bool:

FILE: holomotion/src/training/reference_filter_export.py
  function _to_numpy (line 37) | def _to_numpy(array_like) -> np.ndarray:
  function _require_tensor (line 43) | def _require_tensor(
  function _quat_xyzw_to_rpy (line 51) | def _quat_xyzw_to_rpy(quat_xyzw: np.ndarray) -> np.ndarray:
  function _write_npz (line 60) | def _write_npz(output_path: Path, payload: Mapping[str, np.ndarray]) -> ...
  function _plot_series_groups (line 64) | def _plot_series_groups(
  function _plot_dof_matrix (line 104) | def _plot_dof_matrix(
  function export_reference_filter_debug_artifacts (line 133) | def export_reference_filter_debug_artifacts(
  function _to_plain_sequence (line 373) | def _to_plain_sequence(values) -> list[str]:
  function export_reference_filter_artifacts_from_config (line 381) | def export_reference_filter_artifacts_from_config(config) -> Path:

FILE: holomotion/src/training/train.py
  function _resolve_mujoco_eval_onnx_names (line 34) | def _resolve_mujoco_eval_onnx_names(
  function _exec_mujoco_eval (line 76) | def _exec_mujoco_eval(eval_override_dict: dict) -> None:
  function _maybe_export_reference_filter_artifacts (line 102) | def _maybe_export_reference_filter_artifacts(config: OmegaConf) -> None:
  function main (line 117) | def main(config: OmegaConf):

FILE: holomotion/src/utils/config.py
  function setup_hydra_resolvers (line 28) | def setup_hydra_resolvers():
  function compile_config (line 66) | def compile_config(
  function compile_config_hf_accelerate (line 89) | def compile_config_hf_accelerate(
  function compile_config_devices (line 147) | def compile_config_devices(config):
  function compile_config_directories (line 187) | def compile_config_directories(config, eval: bool = False) -> None:

FILE: holomotion/src/utils/frame_utils.py
  function positions_world_to_env_frame (line 23) | def positions_world_to_env_frame(
  function root_relative_positions_from_env_frame (line 58) | def root_relative_positions_from_env_frame(
  function root_relative_positions_from_mixed_position_frames (line 108) | def root_relative_positions_from_mixed_position_frames(

FILE: holomotion/src/utils/isaac_utils/maths.py
  function normalize (line 28) | def normalize(x, eps: float = 1e-9):
  function torch_rand_float (line 33) | def torch_rand_float(lower, upper, shape, device):
  function copysign (line 39) | def copysign(a, b):
  function set_seed (line 45) | def set_seed(seed, torch_deterministic=False):

FILE: holomotion/src/utils/isaac_utils/rotations.py
  function quat_unit (line 34) | def quat_unit(a):
  function quat_apply (line 39) | def quat_apply(a: Tensor, b: Tensor, w_last: bool) -> Tensor:
  function quat_apply_yaw (line 54) | def quat_apply_yaw(quat: Tensor, vec: Tensor, w_last: bool) -> Tensor:
  function wrap_to_pi (line 62) | def wrap_to_pi(angles):
  function quat_conjugate (line 69) | def quat_conjugate(a: Tensor, w_last: bool) -> Tensor:
  function quat_apply (line 79) | def quat_apply(a: Tensor, b: Tensor, w_last: bool) -> Tensor:
  function quat_rotate (line 94) | def quat_rotate(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_rotate_inverse (line 115) | def quat_rotate_inverse(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_angle_axis (line 136) | def quat_angle_axis(x: Tensor, w_last: bool) -> Tuple[Tensor, Tensor]:
  function quat_from_angle_axis (line 153) | def quat_from_angle_axis(angle: Tensor, axis: Tensor, w_last: bool) -> T...
  function vec_to_heading (line 164) | def vec_to_heading(h_vec):
  function heading_to_quat (line 170) | def heading_to_quat(h_theta, w_last: bool):
  function quat_axis (line 184) | def quat_axis(q: Tensor, axis: int, w_last: bool) -> Tensor:
  function normalize_angle (line 191) | def normalize_angle(x):
  function get_basis_vector (line 196) | def get_basis_vector(q: Tensor, v: Tensor, w_last: bool) -> Tensor:
  function quat_to_angle_axis (line 201) | def quat_to_angle_axis(q):
  function slerp (line 226) | def slerp(q0, q1, t):
  function angle_axis_to_exp_map (line 253) | def angle_axis_to_exp_map(angle, axis):
  function my_quat_rotate (line 262) | def my_quat_rotate(q, v):
  function calc_heading (line 279) | def calc_heading(q):
  function quat_to_exp_map (line 294) | def quat_to_exp_map(q):
  function calc_heading_quat (line 304) | def calc_heading_quat(q, w_last):
  function calc_heading_quat_inv (line 318) | def calc_heading_quat_inv(q, w_last):
  function quat_inverse (line 332) | def quat_inverse(x, w_last):
  function get_euler_xyz (line 339) | def get_euler_xyz(q: Tensor, w_last: bool) -> Tuple[Tensor, Tensor, Tens...
  function get_euler_xyz_in_tensor (line 374) | def get_euler_xyz_in_tensor(q):
  function quat_pos (line 406) | def quat_pos(x):
  function is_valid_quat (line 415) | def is_valid_quat(q):
  function quat_normalize (line 421) | def quat_normalize(q):
  function quat_mul (line 428) | def quat_mul(a, b, w_last: bool):
  function quat_mul_norm (line 459) | def quat_mul_norm(x, y, w_last):
  function quat_mul_norm (line 468) | def quat_mul_norm(x, y, w_last):
  function quat_identity (line 477) | def quat_identity(shape: List[int]):
  function quat_identity_like (line 486) | def quat_identity_like(x):
  function transform_from_rotation_translation (line 492) | def transform_from_rotation_translation(
  function transform_rotation (line 508) | def transform_rotation(x):
  function transform_translation (line 514) | def transform_translation(x):
  function transform_mul (line 520) | def transform_mul(x, y):
  function quaternion_to_matrix (line 535) | def quaternion_to_matrix(
  function axis_angle_to_quaternion (line 577) | def axis_angle_to_quaternion(axis_angle: torch.Tensor) -> torch.Tensor:
  function wxyz_to_xyzw (line 611) | def wxyz_to_xyzw(quat):
  function xyzw_to_wxyz (line 616) | def xyzw_to_wxyz(quat):
  function matrix_to_quaternion (line 620) | def matrix_to_quaternion(matrix: torch.Tensor) -> torch.Tensor:
  function _sqrt_positive_part (line 684) | def _sqrt_positive_part(x: torch.Tensor) -> torch.Tensor:
  function quat_w_first (line 694) | def quat_w_first(rot):
  function quat_from_euler_xyz (line 700) | def quat_from_euler_xyz(roll, pitch, yaw):
  function remove_yaw_component (line 717) | def remove_yaw_component(

FILE: holomotion/src/utils/onnx_export.py
  function _list_to_csv_str (line 26) | def _list_to_csv_str(arr, *, decimals: int = 3, delimiter: str = ",") ->...
  function attach_onnx_metadata_holomotion (line 33) | def attach_onnx_metadata_holomotion(env, onnx_path: str) -> None:
  function export_policy_to_onnx (line 67) | def export_policy_to_onnx(

FILE: holomotion/src/utils/reference_prefix.py
  function resolve_reference_tensor_key (line 21) | def resolve_reference_tensor_key(

FILE: holomotion/src/utils/torch_utils.py
  function to_torch (line 15) | def to_torch(x, dtype=torch.float, device="cpu", requires_grad=False):
  function quat_mul (line 22) | def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
  function normalize (line 61) | def normalize(x, eps: float = 1e-9):
  function quat_apply (line 66) | def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor:
  function quat_apply_inverse (line 88) | def quat_apply_inverse(quat: torch.Tensor, vec: torch.Tensor) -> torch.T...
  function quat_rotate (line 110) | def quat_rotate(q, v):
  function quat_rotate_inverse (line 127) | def quat_rotate_inverse(q, v):
  function quat_conjugate (line 144) | def quat_conjugate(a):
  function quat_unit (line 152) | def quat_unit(a):
  function quat_from_angle_axis (line 157) | def quat_from_angle_axis(angle, axis):
  function normalize_angle (line 165) | def normalize_angle(x):
  function tf_inverse (line 170) | def tf_inverse(q, t):
  function tf_apply (line 176) | def tf_apply(q, t, v):
  function tf_vector (line 181) | def tf_vector(q, v):
  function tf_combine (line 186) | def tf_combine(q1, t1, q2, t2):
  function get_basis_vector (line 191) | def get_basis_vector(q, v):
  function get_axis_params (line 195) | def get_axis_params(value, axis_idx, x_value=0.0, dtype=np.float64, n_di...
  function copysign (line 208) | def copysign(a, b):
  function get_euler_xyz (line 214) | def get_euler_xyz(q: torch.Tensor) -> tuple:
  function quat_from_euler_xyz (line 248) | def quat_from_euler_xyz(roll, pitch, yaw):
  function torch_rand_float (line 264) | def torch_rand_float(lower, upper, shape, device):
  function torch_random_dir_2 (line 270) | def torch_random_dir_2(shape, device):
  function tensor_clamp (line 276) | def tensor_clamp(t, min_t, max_t):
  function scale (line 281) | def scale(x, lower, upper):
  function unscale (line 286) | def unscale(x, lower, upper):
  function unscale_np (line 290) | def unscale_np(x, lower, upper):
  function quat_to_angle_axis (line 295) | def quat_to_angle_axis(q):
  function angle_axis_to_exp_map (line 318) | def angle_axis_to_exp_map(angle, axis):
  function quat_to_exp_map (line 326) | def quat_to_exp_map(q):
  function slerp (line 335) | def slerp(q0, q1, t):
  function my_quat_rotate (line 361) | def my_quat_rotate(q, v):
  function calc_heading (line 378) | def calc_heading(q):
  function calc_heading_quat (line 392) | def calc_heading_quat(q):
  function calc_heading_quat_inv (line 405) | def calc_heading_quat_inv(q):
  function axis_angle_from_quat (line 418) | def axis_angle_from_quat(
  function quat_box_minus (line 483) | def quat_box_minus(
  function quat_error_magnitude (line 517) | def quat_error_magnitude(
  function quat_inv (line 537) | def quat_inv(q: torch.Tensor, eps: float = 1e-9) -> torch.Tensor:
  function xyzw_to_wxyz (line 553) | def xyzw_to_wxyz(q: torch.Tensor) -> torch.Tensor:
  function wxyz_to_xyzw (line 564) | def wxyz_to_xyzw(q: torch.Tensor) -> torch.Tensor:
  function quat_mul_wxyz (line 576) | def quat_mul_wxyz(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
  function rotate_vec_wxyz (line 588) | def rotate_vec_wxyz(q_wxyz: torch.Tensor, v: torch.Tensor) -> torch.Tensor:
  function rotate_vec_inv_wxyz (line 612) | def rotate_vec_inv_wxyz(q_wxyz: torch.Tensor, v: torch.Tensor) -> torch....
  function subtract_frame_transforms (line 636) | def subtract_frame_transforms(
  function quat_normalize_wxyz (line 674) | def quat_normalize_wxyz(q_wxyz: torch.Tensor) -> torch.Tensor:
  function matrix_from_quat (line 689) | def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor:
  function rot6d_from_quat (line 723) | def rot6d_from_quat(quaternions: torch.Tensor) -> torch.Tensor:
  function matrix_from_rot6d (line 741) | def matrix_from_rot6d(rot6d: torch.Tensor) -> torch.Tensor:
  function quat_from_matrix (line 770) | def quat_from_matrix(mat: torch.Tensor) -> torch.Tensor:
  function quat_from_rot6d (line 832) | def quat_from_rot6d(rot6d: torch.Tensor) -> torch.Tensor:
  function euler_xyz_from_quat (line 847) | def euler_xyz_from_quat(
  function yaw_quat (line 899) | def yaw_quat(quat: torch.Tensor) -> torch.Tensor:
  function standardize_quaternion (line 922) | def standardize_quaternion(quaternions: torch.Tensor) -> torch.Tensor:
  function gaussian_kernel1d (line 938) | def gaussian_kernel1d(
  function gaussian_filter1d (line 950) | def gaussian_filter1d(x: torch.Tensor, sigma: float, dim: int) -> torch....
  function smooth_time_series (line 965) | def smooth_time_series(
  function grad_t (line 979) | def grad_t(x: torch.Tensor, dt: float) -> torch.Tensor:
  function axis_angle_to_matrix (line 993) | def axis_angle_to_matrix(

FILE: holomotion/src/utils/unitree_g1_actuator_calculator.py
  class MotorFamily (line 27) | class MotorFamily:
  class JointSpec (line 40) | class JointSpec:
  function seconds_to_delay_steps (line 114) | def seconds_to_delay_steps(delay_seconds: float, physics_dt: float) -> int:
  function compute_pd_gains (line 218) | def compute_pd_gains(
  function fmt_float (line 227) | def fmt_float(x: float) -> str:
  function fmt_value (line 231) | def fmt_value(value: Any, indent: int = 0) -> str:
  function build_single_group_cfg (line 258) | def build_single_group_cfg(
  function render_single_group_cfg (line 327) | def render_single_group_cfg(
  function print_summary (line 372) | def print_summary(cfg: dict[str, Any]) -> None:
  function main (line 400) | def main() -> None:

FILE: tests/benchmark_legacy_onnx_attention.py
  class _RawAttentionModule (line 20) | class _RawAttentionModule(nn.Module):
    method forward (line 21) | def forward(
  class _SafeAttentionModule (line 33) | class _SafeAttentionModule(nn.Module):
    method forward (line 34) | def forward(
  function _export_model (line 51) | def _export_model(
  function _benchmark_session (line 71) | def _benchmark_session(
  function main (line 97) | def main() -> None:

FILE: tests/benchmark_moe_router_export.py
  function _extract_int_setting (line 8) | def _extract_int_setting(config_path: Path, key: str) -> int:
  function _load_b0310_shape_config (line 19) | def _load_b0310_shape_config() -> dict[str, int]:
  function _router_scores_training (line 38) | def _router_scores_training(
  function _router_scores_export_safe (line 56) | def _router_scores_export_safe(
  function _benchmark (line 72) | def _benchmark(
  function _run_case (line 96) | def _run_case(
  function main (line 143) | def main() -> None:

FILE: tests/test_actor_export_config.py
  class _DummyTFModule (line 37) | class _DummyTFModule(nn.Module):
    method __init__ (line 38) | def __init__(self):
    method forward (line 45) | def forward(
  class _DummyAttentionTFModule (line 54) | class _DummyAttentionTFModule(nn.Module):
    method __init__ (line 55) | def __init__(self):
    method forward (line 62) | def forward(
  class _RecordingDeviceModule (line 89) | class _RecordingDeviceModule(nn.Module):
    method __init__ (line 90) | def __init__(self):
    method to (line 95) | def to(self, device):
  function _make_minimal_real_transformer_actor (line 101) | def _make_minimal_real_transformer_actor(
  function _capture_moe_router_outputs (line 150) | def _capture_moe_router_outputs(
  function _make_minimal_ref_router_actor (line 201) | def _make_minimal_ref_router_actor() -> PPOTFRefRouterActor:
  function _make_ref_router_v2_obs_schema (line 238) | def _make_ref_router_v2_obs_schema() -> dict:
  function _make_ref_router_v2_obs (line 268) | def _make_ref_router_v2_obs(batch_size: list[int]) -> TensorDict:
  function _make_minimal_ref_router_v2_actor (line 296) | def _make_minimal_ref_router_v2_actor() -> PPOTFRefRouterSeqActor:
  function _make_minimal_ref_router_v3_actor (line 334) | def _make_minimal_ref_router_v3_actor() -> PPOTFRefRouterV3Actor:
  function test_export_policy_to_onnx_uses_opset_17 (line 370) | def test_export_policy_to_onnx_uses_opset_17(monkeypatch, tmp_path):
  function test_export_policy_to_onnx_restores_training_mode (line 410) | def test_export_policy_to_onnx_restores_training_mode(monkeypatch, tmp_p...
  function test_clone_module_for_cpu_export_does_not_move_live_module (line 466) | def test_clone_module_for_cpu_export_does_not_move_live_module(monkeypat...
  function test_ppotf_actor_export_uses_legacy_torchscript (line 482) | def test_ppotf_actor_export_uses_legacy_torchscript(monkeypatch, tmp_path):
  function test_ppotf_actor_export_onnx_avoids_isnan (line 511) | def test_ppotf_actor_export_onnx_avoids_isnan(tmp_path):
  function test_ppotf_real_transformer_export_onnx_avoids_isnan (line 534) | def test_ppotf_real_transformer_export_onnx_avoids_isnan(tmp_path):
  function test_ppotf_real_moe_transformer_export_reaches_router_ops (line 551) | def test_ppotf_real_moe_transformer_export_reaches_router_ops(tmp_path):
  function test_ppotf_real_moe_transformer_export_exposes_routing_outputs (line 573) | def test_ppotf_real_moe_transformer_export_exposes_routing_outputs(tmp_p...
  function test_ppotf_real_moe_transformer_export_dense_last_uses_actual_moe_indices (line 602) | def test_ppotf_real_moe_transformer_export_dense_last_uses_actual_moe_in...
  function test_ppotf_real_moe_transformer_export_avoids_reduce_log_sum_exp (line 634) | def test_ppotf_real_moe_transformer_export_avoids_reduce_log_sum_exp(
  function test_export_safe_moe_router_matches_training_scores_for_topk1 (line 658) | def test_export_safe_moe_router_matches_training_scores_for_topk1(monkey...
  function test_export_safe_moe_router_matches_training_scores_with_dynamic_bias (line 696) | def test_export_safe_moe_router_matches_training_scores_with_dynamic_bias(
  function test_grouped_moe_router_x_keeps_topk_when_main_input_changes (line 745) | def test_grouped_moe_router_x_keeps_topk_when_main_input_changes(monkeyp...
  function test_grouped_moe_router_x_changes_topk_when_router_input_changes (line 781) | def test_grouped_moe_router_x_changes_topk_when_router_input_changes(
  function test_ref_router_actor_export_keeps_single_obs_input_and_reaches_moe (line 818) | def test_ref_router_actor_export_keeps_single_obs_input_and_reaches_moe(
  function test_ref_router_v2_actor_export_keeps_single_obs_input_and_reaches_moe (line 839) | def test_ref_router_v2_actor_export_keeps_single_obs_input_and_reaches_moe(
  function test_ref_router_v3_actor_export_keeps_single_obs_input_and_reaches_moe (line 868) | def test_ref_router_v3_actor_export_keeps_single_obs_input_and_reaches_moe(
  function test_real_transformer_actor_export_supports_selected_expert_margin (line 897) | def test_real_transformer_actor_export_supports_selected_expert_margin(

FILE: tests/test_algo_base_iteration_logging.py
  function test_log_iteration_uses_checkpoint_start_for_total_iterations (line 10) | def test_log_iteration_uses_checkpoint_start_for_total_iterations():

FILE: tests/test_build_quantization_dataset.py
  function _load_module (line 5) | def _load_module():
  function test_allocate_sample_counts_normalizes_weights_and_matches_total (line 20) | def test_allocate_sample_counts_normalizes_weights_and_matches_total():
  function test_build_quantization_dataset_creates_symlinks (line 42) | def test_build_quantization_dataset_creates_symlinks(tmp_path):
  function test_build_quantization_dataset_caps_each_dataset_at_available_clips (line 76) | def test_build_quantization_dataset_caps_each_dataset_at_available_clips(

FILE: tests/test_cache_curriculum_sampler.py
  function _update_sampler (line 15) | def _update_sampler(
  function _update_sampler_subset (line 35) | def _update_sampler_subset(
  class _ChunkLimitedSampler (line 60) | class _ChunkLimitedSampler:
    method __init__ (line 61) | def __init__(self, *, max_query_size: int) -> None:
    method _checked_indices (line 66) | def _checked_indices(self, window_indices: torch.Tensor) -> torch.Tensor:
    method get_scores_for_indices (line 76) | def get_scores_for_indices(
    method get_window_state_for_indices (line 82) | def get_window_state_for_indices(
    method get_pool_statistics (line 97) | def get_pool_statistics(self) -> dict[str, float]:
  class _FakeTrainDataset (line 107) | class _FakeTrainDataset:
    method __init__ (line 108) | def __init__(self, windows: list[SimpleNamespace]) -> None:
    method __len__ (line 111) | def __len__(self) -> int:
    method close (line 114) | def close(self) -> None:
  function test_sampler_uses_configured_uniform_ratio_immediately (line 118) | def test_sampler_uses_configured_uniform_ratio_immediately():
  function test_completion_rate_relative_improvement_alone_drives_scores (line 129) | def test_completion_rate_relative_improvement_alone_drives_scores():
  function test_sampler_weights_progress_by_remaining_difficulty (line 149) | def test_sampler_weights_progress_by_remaining_difficulty():
  function test_sampler_tracks_cumulative_selection_counts (line 167) | def test_sampler_tracks_cumulative_selection_counts():
  function test_low_completion_plateau_drops_from_prioritized_replay (line 187) | def test_low_completion_plateau_drops_from_prioritized_replay():
  function test_prioritized_windows_persist_beyond_immediate_batch (line 211) | def test_prioritized_windows_persist_beyond_immediate_batch():
  function test_sampler_reports_pool_means_and_membership_churn (line 247) | def test_sampler_reports_pool_means_and_membership_churn():
  function test_sampler_hot_path_avoids_full_dataset_temporaries (line 280) | def test_sampler_hot_path_avoids_full_dataset_temporaries(monkeypatch):
  function test_ppo_logs_only_core_curriculum_metrics (line 337) | def test_ppo_logs_only_core_curriculum_metrics():
  function test_cache_curriculum_dumps_on_scheduled_swap_even_without_state_update (line 382) | def test_cache_curriculum_dumps_on_scheduled_swap_even_without_state_upd...
  function test_update_cache_curriculum_refreshes_prefetched_batch_when_state_changes (line 405) | def test_update_cache_curriculum_refreshes_prefetched_batch_when_state_c...
  function test_cache_curriculum_whole_window_dump_streams_rows_in_chunks (line 442) | def test_cache_curriculum_whole_window_dump_streams_rows_in_chunks(

FILE: tests/test_domain_rand_config_builder.py
  class _DummyEventTermCfg (line 17) | class _DummyEventTermCfg:
    method __init__ (line 18) | def __init__(self, **kwargs):
  class _DummySceneEntityCfg (line 22) | class _DummySceneEntityCfg:
    method __init__ (line 23) | def __init__(self, *args, **kwargs):
    method resolve (line 27) | def resolve(self, _scene):
  function _load_domain_rand_module (line 31) | def _load_domain_rand_module(monkeypatch):
  function test_build_domain_rand_config_skips_non_event_metadata (line 80) | def test_build_domain_rand_config_skips_non_event_metadata(monkeypatch):

FILE: tests/test_eval_mujoco_action_delay.py
  function test_action_delay_cfg_defaults_to_disabled_episode (line 9) | def test_action_delay_cfg_defaults_to_disabled_episode():
  function test_action_delay_cfg_rejects_invalid_delay_type (line 21) | def test_action_delay_cfg_rejects_invalid_delay_type():
  function test_apply_action_delay_passthrough_when_disabled (line 40) | def test_apply_action_delay_passthrough_when_disabled():
  function test_apply_action_delay_episode_reuses_single_sample (line 58) | def test_apply_action_delay_episode_reuses_single_sample(monkeypatch):
  function test_apply_action_delay_step_resamples_each_policy_step (line 86) | def test_apply_action_delay_step_resamples_each_policy_step(monkeypatch):

FILE: tests/test_eval_mujoco_action_ema.py
  function test_action_ema_filter_cfg_reads_erfi_settings (line 7) | def test_action_ema_filter_cfg_reads_erfi_settings():
  function test_action_ema_filter_defaults_to_disabled_for_non_erfi (line 29) | def test_action_ema_filter_defaults_to_disabled_for_non_erfi():
  function test_apply_action_ema_filter_uses_previous_filtered_action (line 51) | def test_apply_action_ema_filter_uses_previous_filtered_action():
  function test_reset_action_ema_filter_clears_state (line 70) | def test_reset_action_ema_filter_clears_state():

FILE: tests/test_eval_mujoco_contact_export.py
  function _build_export_evaluator (line 10) | def _build_export_evaluator(tmp_path: Path):
  function test_save_batch_result_exports_low_level_contact_traces (line 105) | def test_save_batch_result_exports_low_level_contact_traces(tmp_path: Pa...
  function test_dump_robot_augmented_npz_exports_low_level_contact_traces (line 127) | def test_dump_robot_augmented_npz_exports_low_level_contact_traces(
  function test_init_low_level_foot_contact_logging_falls_back_to_ankle_roll_bodies (line 152) | def test_init_low_level_foot_contact_logging_falls_back_to_ankle_roll_bo...

FILE: tests/test_eval_mujoco_s100_horizon_ptq.py
  class _FakeIoNode (line 14) | class _FakeIoNode:
    method __init__ (line 15) | def __init__(self, name, shape):
  function _make_value_info (line 20) | def _make_value_info(name, shape):
  function _make_fake_onnx_model (line 29) | def _make_fake_onnx_model():
  function _make_evaluator (line 45) | def _make_evaluator(model_path: Path, bc_path: Path | None = None):
  function test_load_policy_falls_back_to_horizon_quantized_bc_for_ptq_onnx (line 63) | def test_load_policy_falls_back_to_horizon_quantized_bc_for_ptq_onnx(
  function test_load_policy_resolves_common_horizon_runtime_artifact_names (line 127) | def test_load_policy_resolves_common_horizon_runtime_artifact_names(
  function test_load_policy_raises_original_error_when_ptq_fallback_bc_missing (line 178) | def test_load_policy_raises_original_error_when_ptq_fallback_bc_missing(
  function test_load_policy_keeps_standard_onnxruntime_path_for_regular_onnx (line 204) | def test_load_policy_keeps_standard_onnxruntime_path_for_regular_onnx(
  function test_load_policy_prefers_explicit_bc_path_for_inference_and_onnx_for_metadata (line 252) | def test_load_policy_prefers_explicit_bc_path_for_inference_and_onnx_for...
  function test_bc_runtime_run_normalizes_inputs_for_hbruntime (line 310) | def test_bc_runtime_run_normalizes_inputs_for_hbruntime(monkeypatch, tmp...
  function test_update_policy_raises_clear_error_before_runtime_on_obs_dim_mismatch (line 358) | def test_update_policy_raises_clear_error_before_runtime_on_obs_dim_mism...

FILE: tests/test_eval_mujoco_use_gpu.py
  class _FakeIoNode (line 11) | class _FakeIoNode:
    method __init__ (line 12) | def __init__(self, name, shape):
  function test_load_policy_treats_false_string_use_gpu_as_cpu (line 17) | def test_load_policy_treats_false_string_use_gpu_as_cpu(monkeypatch):
  function test_create_ray_evaluator_preserves_use_gpu_false (line 63) | def test_create_ray_evaluator_preserves_use_gpu_false(monkeypatch):
  function test_run_mujoco_sim2sim_eval_preserves_use_gpu_false (line 81) | def test_run_mujoco_sim2sim_eval_preserves_use_gpu_false(

FILE: tests/test_eval_onnx_io_dump.py
  class _Config (line 18) | class _Config(SimpleNamespace):
    method get (line 19) | def get(self, key, default=None):
  function test_save_onnx_io_dump_stacks_per_frame_inputs_and_outputs (line 23) | def test_save_onnx_io_dump_stacks_per_frame_inputs_and_outputs(tmp_path):
  function test_write_onnx_io_dump_readme_creates_chinese_loading_instructions (line 83) | def test_write_onnx_io_dump_readme_creates_chinese_loading_instructions(
  function test_save_batch_result_persists_low_level_torque_dump_and_dt (line 95) | def test_save_batch_result_persists_low_level_torque_dump_and_dt(tmp_path):
  function test_save_batch_result_persists_moe_routing_tensors (line 147) | def test_save_batch_result_persists_moe_routing_tensors(tmp_path):
  function test_dump_robot_augmented_npz_persists_moe_routing_tensors (line 207) | def test_dump_robot_augmented_npz_persists_moe_routing_tensors(tmp_path):
  function test_ray_actor_run_clip_overwrites_existing_outputs_and_sidecar (line 265) | def test_ray_actor_run_clip_overwrites_existing_outputs_and_sidecar(tmp_...
  function test_ray_actor_skips_sidecar_for_non_default_model_type (line 325) | def test_ray_actor_skips_sidecar_for_non_default_model_type(tmp_path):
  function test_ray_actor_treats_empty_model_type_as_default_holomotion (line 369) | def test_ray_actor_treats_empty_model_type_as_default_holomotion(tmp_path):
  function test_ray_actor_init_uses_configured_evaluator_module (line 414) | def test_ray_actor_init_uses_configured_evaluator_module(

FILE: tests/test_evaluation_metrics.py
  function _make_eval_data (line 14) | def _make_eval_data(
  function test_per_frame_metrics_include_torque_jump_diagnostics (line 102) | def test_per_frame_metrics_include_torque_jump_diagnostics():
  function test_offline_evaluate_dumped_npzs_exports_torque_jump_summary_metrics (line 142) | def test_offline_evaluate_dumped_npzs_exports_torque_jump_summary_metrics(
  function test_compute_clip_stability_summary_detects_chatter_and_support_events (line 215) | def test_compute_clip_stability_summary_detects_chatter_and_support_even...
  function test_compute_clip_stability_summary_reports_expert_switching_js_div (line 320) | def test_compute_clip_stability_summary_reports_expert_switching_js_div():
  function test_offline_evaluate_dumped_npzs_reports_nan_contact_metrics_for_legacy_npz (line 364) | def test_offline_evaluate_dumped_npzs_reports_nan_contact_metrics_for_le...

FILE: tests/test_isaaclab_termination.py
  class _Scene (line 27) | class _Scene(SimpleNamespace):
    method __getitem__ (line 28) | def __getitem__(self, key):
  function _load_isaaclab_termination_module (line 32) | def _load_isaaclab_termination_module(module_name: str):
  function test_wholebody_mpjpe_far_flags_envs_above_mean_error_threshold (line 115) | def test_wholebody_mpjpe_far_flags_envs_above_mean_error_threshold():
  function test_wholebody_mpjpe_far_uses_immediate_next_reference (line 142) | def test_wholebody_mpjpe_far_uses_immediate_next_reference():
  function test_keybody_ref_pos_far_uses_immediate_next_reference (line 164) | def test_keybody_ref_pos_far_uses_immediate_next_reference():
  function test_ref_gravity_projection_far_uses_immediate_next_reference (line 200) | def test_ref_gravity_projection_far_uses_immediate_next_reference():
  function test_build_terminations_config_registers_wholebody_mpjpe_far (line 237) | def test_build_terminations_config_registers_wholebody_mpjpe_far():
  function test_build_terminations_config_resolves_native_isaaclab_termination (line 258) | def test_build_terminations_config_resolves_native_isaaclab_termination():
  function test_build_terminations_config_raises_on_unknown_termination (line 279) | def test_build_terminations_config_raises_on_unknown_termination():

FILE: tests/test_mean_process_5metrics.py
  function test_macro_report_appends_torque_jump_columns_to_legacy_tables (line 34) | def test_macro_report_appends_torque_jump_columns_to_legacy_tables(

FILE: tests/test_motion_cache_gather_state.py
  function _expected_field (line 21) | def _expected_field(
  class MotionCacheGatherStateTests (line 36) | class MotionCacheGatherStateTests(unittest.TestCase):
    method test_normalize_online_filter_cfg_includes_velocity_smoothing_sigmas (line 37) | def test_normalize_online_filter_cfg_includes_velocity_smoothing_sigmas(
    method test_normalize_online_filter_cfg_uses_fk_sigma_fallback_defaults (line 58) | def test_normalize_online_filter_cfg_uses_fk_sigma_fallback_defaults(s...
    method test_build_motion_datasets_from_cfg_passes_fk_sigma_fallback (line 67) | def test_build_motion_datasets_from_cfg_passes_fk_sigma_fallback(self):
    method test_build_motion_datasets_from_cfg_defaults_fk_sigma_fallback (line 95) | def test_build_motion_datasets_from_cfg_defaults_fk_sigma_fallback(self):
    method test_gather_tensor_returns_expected_values (line 122) | def test_gather_tensor_returns_expected_values(self):
    method test_gather_tensor_reflects_updated_indices_without_cached_state (line 182) | def test_gather_tensor_reflects_updated_indices_without_cached_state(s...
    method test_cpu_fk_transform_forwards_explicit_vel_smoothing_sigma (line 236) | def test_cpu_fk_transform_forwards_explicit_vel_smoothing_sigma(self):
    method test_cpu_fk_transform_defaults_vel_smoothing_sigma_to_two (line 265) | def test_cpu_fk_transform_defaults_vel_smoothing_sigma_to_two(self):
    method test_hdf5_v2_sample_exposes_zero_cutoff_metadata_when_disabled (line 289) | def test_hdf5_v2_sample_exposes_zero_cutoff_metadata_when_disabled(self):
    method test_hdf5_v2_sample_exposes_sampled_cutoff_metadata (line 300) | def test_hdf5_v2_sample_exposes_sampled_cutoff_metadata(self):
    method test_hdf5_v2_sample_generates_filtered_reference_family (line 314) | def test_hdf5_v2_sample_generates_filtered_reference_family(self):
    method test_hdf5_v2_sample_uses_split_fk_smoothing_sigmas (line 336) | def test_hdf5_v2_sample_uses_split_fk_smoothing_sigmas(self):
    method test_hdf5_v2_sample_skips_filtered_reference_family_when_disabled (line 353) | def test_hdf5_v2_sample_skips_filtered_reference_family_when_disabled(
    method _make_stub_root_dof_dataset (line 368) | def _make_stub_root_dof_dataset(

FILE: tests/test_motion_cache_startup.py
  class _FakeDataset (line 15) | class _FakeDataset:
    method __init__ (line 16) | def __init__(self, length: int = 8) -> None:
    method __len__ (line 21) | def __len__(self) -> int:
    method __getitem__ (line 24) | def __getitem__(self, index: int):
    method set_progress_counter (line 27) | def set_progress_counter(self, counter) -> None:
    method close (line 30) | def close(self) -> None:
  class MotionCacheStartupTests (line 34) | class MotionCacheStartupTests(unittest.TestCase):
    method test_motion_cache_uses_explicit_constructor_seed (line 35) | def test_motion_cache_uses_explicit_constructor_seed(self):
    method test_setup_seeding_does_not_reinitialize_motion_cache (line 55) | def test_setup_seeding_does_not_reinitialize_motion_cache(self):
    method test_motion_cache_passes_loader_timeout_to_dataloader (line 79) | def test_motion_cache_passes_loader_timeout_to_dataloader(self):
    method test_motion_cache_disables_progress_bar_in_distributed_runs (line 104) | def test_motion_cache_disables_progress_bar_in_distributed_runs(self):
    method test_motion_cache_keeps_progress_bar_for_local_runs (line 126) | def test_motion_cache_keeps_progress_bar_for_local_runs(self):
    method test_motion_cache_requires_positive_loader_timeout (line 148) | def test_motion_cache_requires_positive_loader_timeout(self):

FILE: tests/test_motion_tracking_command_reference_prefix.py
  class MotionTrackingCommandReferencePrefixTests (line 11) | class MotionTrackingCommandReferencePrefixTests(unittest.TestCase):
    method test_ft_ref_prefix_uses_filtered_tensor_when_present (line 12) | def test_ft_ref_prefix_uses_filtered_tensor_when_present(self):
    method test_ft_ref_prefix_requires_filtered_tensor (line 21) | def test_ft_ref_prefix_requires_filtered_tensor(self):
    method test_ref_prefix_falls_back_to_unprefixed_tensor (line 29) | def test_ref_prefix_falls_back_to_unprefixed_tensor(self):
    method test_ref_prefix_prefers_prefixed_tensor_when_present (line 38) | def test_ref_prefix_prefers_prefixed_tensor_when_present(self):

FILE: tests/test_motion_tracking_timing.py
  class _DummyConfig (line 19) | class _DummyConfig:
    method __init__ (line 20) | def __init__(self, *args, **kwargs):
  function _install_fake_motion_command_deps (line 25) | def _install_fake_motion_command_deps(monkeypatch):
  function _load_motion_command_module (line 167) | def _load_motion_command_module(monkeypatch):
  function test_immediate_next_reference_getters_use_slot_one (line 179) | def test_immediate_next_reference_getters_use_slot_one(monkeypatch):
  function test_update_command_skips_just_reset_envs (line 224) | def test_update_command_skips_just_reset_envs(monkeypatch):
  function test_update_command_resumes_advancing_after_reset_step (line 246) | def test_update_command_resumes_advancing_after_reset_step(monkeypatch):
  function test_mpjpe_metrics_use_immediate_next_reference (line 268) | def test_mpjpe_metrics_use_immediate_next_reference(monkeypatch):
  function test_mpkpe_metrics_use_immediate_next_reference (line 298) | def test_mpkpe_metrics_use_immediate_next_reference(monkeypatch):

FILE: tests/test_mujoco_filtered_ref_compat.py
  function _make_minimal_motion_npz (line 34) | def _make_minimal_motion_npz(path: Path, *, include_cutoff: bool) -> None:
  function test_policy_obs_list_accepts_shared_cutoff_term (line 53) | def test_policy_obs_list_accepts_shared_cutoff_term():
  function test_cutoff_obs_getters_use_current_frame_and_default_zero (line 68) | def test_cutoff_obs_getters_use_current_frame_and_default_zero():
  function test_policy_obs_list_v2_uses_only_actor_schema_terms (line 84) | def test_policy_obs_list_v2_uses_only_actor_schema_terms():
  function test_load_specific_motion_loads_cutoff_metadata_with_zero_fallback (line 101) | def test_load_specific_motion_loads_cutoff_metadata_with_zero_fallback():

FILE: tests/test_obs_norm_compile.py
  function _make_actor_with_obs_norm (line 7) | def _make_actor_with_obs_norm(obs_dim: int = 16) -> PPOTFActor:
  function test_obs_norm_update_is_not_captured_by_dynamo (line 16) | def test_obs_norm_update_is_not_captured_by_dynamo():

FILE: tests/test_observation_frames.py
  class _DummyConfig (line 19) | class _DummyConfig:
    method __init__ (line 20) | def __init__(self, *args, **kwargs):
  class _Scene (line 25) | class _Scene(SimpleNamespace):
    method __getitem__ (line 26) | def __getitem__(self, key):
  function _identity_quat (line 30) | def _identity_quat(*shape: int) -> torch.Tensor:
  function _load_observation_module (line 36) | def _load_observation_module(monkeypatch):
  function test_ref_future_observations_can_limit_num_frames (line 145) | def test_ref_future_observations_can_limit_num_frames(monkeypatch):
  function _make_env (line 239) | def _make_env():
  function test_global_robot_bodylink_pos_is_in_environment_frame (line 264) | def test_global_robot_bodylink_pos_is_in_environment_frame(monkeypatch):
  function test_root_rel_robot_bodylink_pos_uses_consistent_env_frame (line 276) | def test_root_rel_robot_bodylink_pos_uses_consistent_env_frame(monkeypat...
  function test_global_anchor_pos_diff_uses_environment_frame_consistently (line 294) | def test_global_anchor_pos_diff_uses_environment_frame_consistently(
  function test_build_additive_uniform_noise_cfg_supports_optional_z_override (line 307) | def test_build_additive_uniform_noise_cfg_supports_optional_z_override(
  function test_build_additive_uniform_noise_cfg_keeps_scalar_bounds_without_z_override (line 330) | def test_build_additive_uniform_noise_cfg_keeps_scalar_bounds_without_z_...

FILE: tests/test_onnx_attention_export.py
  class _ExportAttentionModule (line 19) | class _ExportAttentionModule(nn.Module):
    method forward (line 20) | def forward(
  class _ExportCausalAttentionModule (line 37) | class _ExportCausalAttentionModule(nn.Module):
    method forward (line 38) | def forward(
  function _export_model (line 54) | def _export_model(
  function _export_op_types (line 72) | def _export_op_types(
  function _run_onnx (line 84) | def _run_onnx(
  function test_export_safe_attention_uses_native_bool_mask_outside_export (line 104) | def test_export_safe_attention_uses_native_bool_mask_outside_export(
  function test_export_safe_attention_matches_sdpa_for_valid_masks (line 151) | def test_export_safe_attention_matches_sdpa_for_valid_masks():
  function test_legacy_attention_export_avoids_isnan (line 184) | def test_legacy_attention_export_avoids_isnan():
  function test_legacy_attention_export_ort_matches_pytorch_for_future_mask (line 203) | def test_legacy_attention_export_ort_matches_pytorch_for_future_mask():
  function test_legacy_attention_export_ort_matches_pytorch_for_causal_path (line 238) | def test_legacy_attention_export_ort_matches_pytorch_for_causal_path():
  function test_legacy_attention_export_ort_matches_pytorch_for_kv_mask (line 265) | def test_legacy_attention_export_ort_matches_pytorch_for_kv_mask():

FILE: tests/test_onnx_export.py
  class _FakeEntry (line 7) | class _FakeEntry:
    method __init__ (line 8) | def __init__(self):
  class _FakeTensor (line 13) | class _FakeTensor:
    method __init__ (line 14) | def __init__(self, values):
    method __getitem__ (line 17) | def __getitem__(self, index):
    method cpu (line 20) | def cpu(self):
    method tolist (line 23) | def tolist(self):
  function test_attach_onnx_metadata_uses_default_joint_gains (line 27) | def test_attach_onnx_metadata_uses_default_joint_gains(monkeypatch):

FILE: tests/test_plot_moe_expert_heatmap.py
  function _load_plot_moe_expert_heatmap_module (line 14) | def _load_plot_moe_expert_heatmap_module():
  function _write_eval_npz (line 24) | def _write_eval_npz(path: Path) -> None:
  function test_plot_dump_exports_moe_heatmap_pdf (line 58) | def test_plot_dump_exports_moe_heatmap_pdf(tmp_path):
  function test_selected_expert_weights_are_renormalized_within_selected_ids (line 74) | def test_selected_expert_weights_are_renormalized_within_selected_ids():
  function test_selected_expert_heatmap_only_colors_activated_experts (line 108) | def test_selected_expert_heatmap_only_colors_activated_experts():
  function test_collect_npz_paths_recursively_sorts_directory_entries (line 148) | def test_collect_npz_paths_recursively_sorts_directory_entries(tmp_path):
  function test_plot_input_path_directory_generates_all_heatmaps_with_tqdm (line 163) | def test_plot_input_path_directory_generates_all_heatmaps_with_tqdm(
  function test_plot_dump_requires_2d_robot_dof_torque (line 219) | def test_plot_dump_requires_2d_robot_dof_torque(tmp_path):

FILE: tests/test_plot_state_series.py
  function _load_plot_state_series_module (line 13) | def _load_plot_state_series_module():
  function test_plot_dump_exports_time_matched_scalar_series (line 23) | def test_plot_dump_exports_time_matched_scalar_series(tmp_path):

FILE: tests/test_ppo_checkpoint_sigma_override.py
  class _DummyActor (line 10) | class _DummyActor(nn.Module):
    method __init__ (line 11) | def __init__(self, events: list[str]):
    method override_sigma (line 17) | def override_sigma(self, sigma_override):
  function test_ppo_load_reapplies_sigma_override_after_checkpoint_restore (line 22) | def test_ppo_load_reapplies_sigma_override_after_checkpoint_restore():
  function test_ppo_load_skips_optimizer_restore_during_offline_eval (line 80) | def test_ppo_load_skips_optimizer_restore_during_offline_eval():
  function test_ppo_load_skips_incompatible_optimizer_state_restore (line 119) | def test_ppo_load_skips_incompatible_optimizer_state_restore():
  function test_checkpoint_state_to_cpu_moves_nested_tensors (line 177) | def test_checkpoint_state_to_cpu_moves_nested_tensors():

FILE: tests/test_ppo_entropy_annealing.py
  function _build_entropy_algo (line 13) | def _build_entropy_algo(
  function test_entropy_coef_is_constant_when_annealing_disabled (line 32) | def test_entropy_coef_is_constant_when_annealing_disabled():
  function test_entropy_coef_decays_and_respects_resumed_total_iterations (line 44) | def test_entropy_coef_decays_and_respects_resumed_total_iterations():
  function test_entropy_coef_clamps_to_zero_at_and_after_zero_point (line 58) | def test_entropy_coef_clamps_to_zero_at_and_after_zero_point():
  function test_validate_entropy_schedule_config_rejects_invalid_values (line 82) | def test_validate_entropy_schedule_config_rejects_invalid_values(
  function test_learn_sets_current_iteration_before_each_update (line 95) | def test_learn_sets_current_iteration_before_each_update():

FILE: tests/test_ppo_symmetry_loss.py
  class _DummyAccelerator (line 14) | class _DummyAccelerator:
    method autocast (line 15) | def autocast(self):
    method backward (line 18) | def backward(self, loss):
    method clip_grad_norm_ (line 21) | def clip_grad_norm_(self, parameters, max_norm):
    method reduce (line 24) | def reduce(self, tensor, reduction="mean"):
  class _DummyActor (line 28) | class _DummyActor(nn.Module):
    method __init__ (line 29) | def __init__(self, num_actions: int, mirror_offset: float):
    method forward (line 35) | def forward(
  class _DummyCritic (line 62) | class _DummyCritic(nn.Module):
    method __init__ (line 63) | def __init__(self):
    method forward (line 67) | def forward(self, obs_td: TensorDict, *, update_obs_norm: bool = True):
  class _SingleBatchStorage (line 75) | class _SingleBatchStorage:
    method __init__ (line 76) | def __init__(self, batch):
    method iter_minibatches (line 86) | def iter_minibatches(self, num_mini_batches: int, num_epochs: int):
    method clear (line 90) | def clear(self):
  function _install_mirror_stub (line 94) | def _install_mirror_stub():
  function test_setup_symmetry_builds_expected_dof_permutation_and_signs (line 149) | def test_setup_symmetry_builds_expected_dof_permutation_and_signs():
  function test_mirror_actor_obs_uses_slash_qualified_actor_terms_only (line 232) | def test_mirror_actor_obs_uses_slash_qualified_actor_terms_only():
  function test_update_reports_symmetry_loss_only_for_velocity_tracking (line 276) | def test_update_reports_symmetry_loss_only_for_velocity_tracking():

FILE: tests/test_ppo_tf_aux_keybody.py
  function _make_aux_policy (line 23) | def _make_aux_policy(
  function _make_aux_actor (line 71) | def _make_aux_actor() -> PPOTFActor:
  function _make_aux_command_policy (line 92) | def _make_aux_command_policy(
  function _make_temporal_aux_actor (line 134) | def _make_temporal_aux_actor() -> PPOTFActor:
  function _make_temporal_only_aux_actor (line 155) | def _make_temporal_only_aux_actor() -> PPOTFActor:
  function test_rollout_storage_allocates_ref_and_robot_keybody_targets (line 161) | def test_rollout_storage_allocates_ref_and_robot_keybody_targets():
  function test_grouped_moe_policy_returns_keybody_position_predictions (line 187) | def test_grouped_moe_policy_returns_keybody_position_predictions():
  function test_grouped_moe_policy_omits_denoise_predictions_when_weights_zero (line 205) | def test_grouped_moe_policy_omits_denoise_predictions_when_weights_zero():
  function test_ppotf_actor_sequence_logp_emits_actor_facing_dof_denoise_keys (line 220) | def test_ppotf_actor_sequence_logp_emits_actor_facing_dof_denoise_keys():
  function test_grouped_moe_policy_default_layout_keeps_dense_first_and_moe_tail (line 244) | def test_grouped_moe_policy_default_layout_keeps_dense_first_and_moe_tai...
  function test_grouped_moe_policy_dense_layer_at_last_keeps_only_middle_layers_moe (line 258) | def test_grouped_moe_policy_dense_layer_at_last_keeps_only_middle_layers...
  function test_grouped_moe_policy_dense_layer_at_last_allows_shallow_fully_dense (line 283) | def test_grouped_moe_policy_dense_layer_at_last_allows_shallow_fully_den...
  function test_grouped_moe_policy_command_recon_uses_live_router_features (line 298) | def test_grouped_moe_policy_command_recon_uses_live_router_features():
  function test_grouped_moe_policy_freeze_router_detaches_router_features_and_params (line 329) | def test_grouped_moe_policy_freeze_router_detaches_router_features_and_p...
  function test_grouped_moe_policy_loads_legacy_aux_command_recon_head_keys (line 363) | def test_grouped_moe_policy_loads_legacy_aux_command_recon_head_keys():
  function test_grouped_moe_policy_ignores_legacy_aux_command_recon_head_keys_when_disabled (line 389) | def test_grouped_moe_policy_ignores_legacy_aux_command_recon_head_keys_w...
  function test_grouped_moe_policy_clears_router_cache_before_deepcopy (line 412) | def test_grouped_moe_policy_clears_router_cache_before_deepcopy():
  function test_grouped_moe_block_tracks_least_utilized_expert_stats (line 443) | def test_grouped_moe_block_tracks_least_utilized_expert_stats():
  function test_grouped_moe_block_tracks_dead_expert_margin_to_topk_loss (line 471) | def test_grouped_moe_block_tracks_dead_expert_margin_to_topk_loss():
  function test_grouped_moe_block_tracks_selected_expert_margin_to_unselected (line 524) | def test_grouped_moe_block_tracks_selected_expert_margin_to_unselected():
  function test_ppotf_summarize_moe_layer_stats_includes_least_utilized_metrics (line 581) | def test_ppotf_summarize_moe_layer_stats_includes_least_utilized_metrics():
  function test_compute_routed_expert_orthogonal_loss_uses_active_experts_only (line 613) | def test_compute_routed_expert_orthogonal_loss_uses_active_experts_only():
  function test_compute_routed_expert_orthogonal_loss_returns_zero_below_two_active (line 654) | def test_compute_routed_expert_orthogonal_loss_returns_zero_below_two_ac...
  function test_ppotf_actor_sequence_logp_emits_router_features_for_aux_router_losses (line 679) | def test_ppotf_actor_sequence_logp_emits_router_features_for_aux_router_...
  function test_ppotf_actor_sequence_logp_emits_only_router_features_for_temporal_only_aux (line 703) | def test_ppotf_actor_sequence_logp_emits_only_router_features_for_tempor...
  function test_masked_adjacent_router_js_averages_only_valid_adjacent_tokens (line 727) | def test_masked_adjacent_router_js_averages_only_valid_adjacent_tokens():
  function test_masked_adjacent_router_normed_smooth_l1_averages_only_valid_adjacent_tokens (line 765) | def test_masked_adjacent_router_normed_smooth_l1_averages_only_valid_adj...
  function test_masked_aux_keybody_mse_averages_only_valid_tokens (line 808) | def test_masked_aux_keybody_mse_averages_only_valid_tokens():
  function test_masked_aux_huber_averages_only_valid_tokens (line 819) | def test_masked_aux_huber_averages_only_valid_tokens():
  function test_setup_configs_rejects_router_aux_terms_outside_motion_tracking (line 835) | def test_setup_configs_rejects_router_aux_terms_outside_motion_tracking():
  function test_setup_configs_rejects_unknown_router_switch_penalty_metric (line 851) | def test_setup_configs_rejects_unknown_router_switch_penalty_metric():
  function test_setup_configs_reads_dead_expert_margin_to_topk_only (line 873) | def test_setup_configs_reads_dead_expert_margin_to_topk_only():
  function test_setup_configs_reads_selected_expert_margin_to_unselected (line 901) | def test_setup_configs_reads_selected_expert_margin_to_unselected():
  function test_setup_configs_reads_aux_router_future_recon (line 943) | def test_setup_configs_reads_aux_router_future_recon():
  function test_setup_configs_reads_router_expert_orthogonal (line 972) | def test_setup_configs_reads_router_expert_orthogonal():
  function test_setup_configs_rejects_router_expert_orthogonal_without_dead_margin (line 996) | def test_setup_configs_rejects_router_expert_orthogonal_without_dead_mar...
  function test_build_transition_uses_filtered_residual_targets_for_denoise_outputs (line 1014) | def test_build_transition_uses_filtered_residual_targets_for_denoise_out...
  function test_build_transition_rejects_mismatched_denoise_dof_target_shape (lin
Condensed preview — 249 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (3,051K chars).
[
  {
    "path": ".gitattributes",
    "chars": 836,
    "preview": "*.ipynb filter=lfs diff=lfs merge=lfs -text\n*.pt filter=lfs diff=lfs merge=lfs -text\n*.obj filter=lfs diff=lfs merge=lfs"
  },
  {
    "path": ".githooks/README.md",
    "chars": 703,
    "preview": "# Git hooks\n\nPre-commit runs [ruff](https://docs.astral.sh/ruff/) format on staged Python files (using `train.env` for t"
  },
  {
    "path": ".githooks/pre-commit",
    "chars": 647,
    "preview": "#!/bin/bash\n# Run ruff format on staged Python files before commit.\n# Use from holomotion repo root (standalone clone or"
  },
  {
    "path": ".gitignore",
    "chars": 928,
    "preview": "# ignore logs and cache\nlogs/\nlogs_eval/\ndata/\noutputs/\n.archive/\ntmp/\n\n# ignore deployment bag_record, install, log\ndep"
  },
  {
    "path": ".gitlab-ci.yml",
    "chars": 146,
    "preview": "\nworkflow:\n  rules:\n    - if: $CI_PIPELINE_SOURCE == 'merge_request_event'\n\njob1:\n  script:\n    - echo \"This job runs in"
  },
  {
    "path": ".gitmodules",
    "chars": 1588,
    "preview": "[submodule \"thirdparties/SMPLSim\"]\npath = thirdparties/SMPLSim\nurl = https://github.com/ZhengyiLuo/SMPLSim\nbranch = mast"
  },
  {
    "path": "LICENSE",
    "chars": 11343,
    "preview": "                                 Apache License\n                           Version 2.0, January 2004\n                   "
  },
  {
    "path": "Makefile",
    "chars": 1036,
    "preview": "# Variables\nPY_SRC := holomotion/  # Your Python code directory\nRUFF := ruff  # Assumes Ruff is installed locally\nPYTEST"
  },
  {
    "path": "NOTICE",
    "chars": 3395,
    "preview": "\n\n=======================================================================\nASAP's MIT License\n==========================="
  },
  {
    "path": "README.md",
    "chars": 8755,
    "preview": "<div align=\"center\">\n\n<img src=\"assets/media/holomotion_logo_text.png\" alt=\"HoloMotion Logo\" width=\"500\"/>\n\n---\n\n[![Pyth"
  },
  {
    "path": "assets/robots/unitree/G1/29dof/g1_29dof.xml",
    "chars": 35827,
    "preview": "<mujoco model=\"g1_29dof\">\n  <compiler angle=\"radian\" meshdir=\"../meshes\" />\n\n  <default>\n    <default class=\"torso_motor"
  },
  {
    "path": "assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0.urdf",
    "chars": 34567,
    "preview": "<robot name=\"g1_29dof_rev_1_0\">\r\n  <material name=\"dark\">\r\n    <color rgba=\"0.2 0.2 0.2 1\"/>\r\n  </material>\r\n  <material"
  },
  {
    "path": "assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0.xml",
    "chars": 26975,
    "preview": "<mujoco model=\"g1_29dof_rev_1_0\">\n  <compiler angle=\"radian\" meshdir=\"../meshes\"/>\n\n  <statistic meansize=\"0.144785\" ext"
  },
  {
    "path": "assets/robots/unitree/G1/29dof/g1_29dof_rev_1_0_s100.urdf",
    "chars": 35881,
    "preview": "<robot name=\"g1_29dof_rev_1_0\">\r\n  <material name=\"dark\">\r\n    <color rgba=\"0.2 0.2 0.2 1\"/>\r\n  </material>\r\n  <material"
  },
  {
    "path": "assets/robots/unitree/G1/29dof/scene_29dof.xml",
    "chars": 1597,
    "preview": "<mujoco model=\"g1_29dof scene\">\n  <include file=\"g1_29dof.xml\"/>\n\n  <!-- setup scene -->\n  <statistic center=\"0.0 0.0 1."
  },
  {
    "path": "assets/test_data/motion_retargeting/ACCAD/Male1Walking_c3d/Walk_B10_-_Walk_turn_left_45_stageii.npz",
    "chars": 132,
    "preview": "version https://git-lfs.github.com/spec/v1\noid sha256:738f96eb1767e281d78631ca697079adefb5f171d581acd622a27740f2503b4e\ns"
  },
  {
    "path": "deploy.env",
    "chars": 706,
    "preview": "export CONDA_BASE=$(conda info --base)\nexport Deploy_CONDA_PREFIX=\"$CONDA_BASE/envs/holomotion_deploy\"\n\nexport CUDA_HOME"
  },
  {
    "path": "deployment/deploy_environment.sh",
    "chars": 5088,
    "preview": "#!/bin/bash\n##############################################################################\n# HoloMotion Environment Depl"
  },
  {
    "path": "deployment/holomotion_teleop/holomotion_teleop_node.py",
    "chars": 27797,
    "preview": "#!/usr/bin/env python3\n# -*- coding: utf-8 -*-\n\"\"\"\nSingle-process teleoperation pipeline.\n\nThis node reads raw Pico body"
  },
  {
    "path": "deployment/holomotion_teleop/holomotion_teleop_setup.md",
    "chars": 6808,
    "preview": "# Holomotion Teleop\n\nSingle-process pipeline for:\n\n`PICO / XRoboToolkit -> SMPL conversion -> GMR retargeting -> robot Z"
  },
  {
    "path": "deployment/holomotion_teleop/setup_holomotion_teleop_x86_ubuntu2204.sh",
    "chars": 7839,
    "preview": "#!/usr/bin/env bash\nset -euo pipefail\n\n# One-click setup script for the holomotion teleoperation environment.\n#\n# This s"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/launch_holomotion_29dof.sh",
    "chars": 2117,
    "preview": "#!/bin/bash\n\n##############################################################################\n# HoloMotion Deployment Laun"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/launch_holomotion_29dof_docker.sh",
    "chars": 2516,
    "preview": "#!/bin/bash\n\n##############################################################################\n# HoloMotion Deployment Laun"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/CMakeLists.txt",
    "chars": 1897,
    "preview": "cmake_minimum_required(VERSION 3.8)\nproject(humanoid_control)\n\n# Default to C99\nif(NOT CMAKE_C_STANDARD)\n  set(CMAKE_C_S"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/config/g1_29dof_holomotion.yaml",
    "chars": 8370,
    "preview": "device: \"cuda\"\npolicy_freq: 50 # Hz\ncontrol_freq: 500 # Hz\nlowstate_topic: \"/lowstate\"\naction_topic: \"/humanoid/action\"\n"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/__init__.py",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/holomotion_fk_root_only.py",
    "chars": 13312,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/obs_builder/__init__.py",
    "chars": 137,
    "preview": "from .obs_builder import PolicyObsBuilder, get_gravity_orientation\n\n__all__ = [\n    \"PolicyObsBuilder\",\n    \"get_gravity"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/obs_builder/obs_builder.py",
    "chars": 7088,
    "preview": "import numpy as np\nimport torch\n\nfrom typing import Dict, List, Sequence, Any, Optional\n\n\ndef get_gravity_orientation(qu"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/policy_node_29dof.py",
    "chars": 147637,
    "preview": "#! /your_dir/miniconda3/envs/holomotion_deploy/bin/python\n\"\"\"\nHoloMotion Policy Node\n\nThis module implements the main po"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/__init__.py",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/command_helper.py",
    "chars": 1745,
    "preview": "from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ as LowCmdGo\nfrom unitree_sdk2py.idl.unitree_hg.msg.dds_ impor"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/maths.py",
    "chars": 20209,
    "preview": "import torch\nimport numpy as np\nimport random\nimport os\n\n\n@torch.jit.script\ndef normalize(x, eps: float = 1e-9):\n    ret"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/motor_crc.py",
    "chars": 1498,
    "preview": "import struct\nimport numpy as np\nfrom ctypes import Structure, c_uint8, c_float, c_uint32, Array\n\n\ndef crc32_core(data_a"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/remote_controller_filter.py",
    "chars": 3121,
    "preview": "import struct\n\n\nclass KeyMap:\n    R1 = 0\n    L1 = 1\n    start = 2\n    select = 3\n    R2 = 4\n    L2 = 5\n    F1 = 6\n    F2"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/rotation_helper.py",
    "chars": 855,
    "preview": "import numpy as np\nfrom scipy.spatial.transform import Rotation as R\n\n\ndef get_gravity_orientation(quaternion):\n    qw ="
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/humanoid_policy/utils/rotations.py",
    "chars": 19105,
    "preview": "import torch\nfrom torch import Tensor\nimport torch.nn.functional as F\nfrom humanoid_policy.utils.maths import (\n    norm"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/include/common/motor_crc.h",
    "chars": 1921,
    "preview": "/*****************************************************************\n Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rig"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/include/common/motor_crc_hg.h",
    "chars": 1029,
    "preview": "/*****************************************************************\n Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rig"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/include/common/ros2_sport_client.h",
    "chars": 5305,
    "preview": "#ifndef _ROS2_SPORT_CLIENT_\n#define _ROS2_SPORT_CLIENT_\n#include<iostream>\n#include \"nlohmann/json.hpp\"\n#include \"unitre"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/include/common/wireless_controller.h",
    "chars": 949,
    "preview": "#pragma once\n\n#include \"unitree_go/msg/wireless_controller.hpp\"\n#include <array>\n#include <cstring>\n\nclass KeyMap {\npubl"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/launch/holomotion_29dof_launch.py",
    "chars": 6323,
    "preview": "\"\"\"\nHoloMotion ROS2 Launch Configuration\n\nThis module defines the ROS2 launch configuration for the HoloMotion humanoid "
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/models/.gitkeep",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/motion_data/.gitkeep",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/package.xml",
    "chars": 1165,
    "preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/resource/humanoid_control",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/setup.cfg",
    "chars": 101,
    "preview": "[develop]\nscript_dir=$base/lib/humanoid_control\n[install]\ninstall_scripts=$base/lib/humanoid_control\n"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/setup.py",
    "chars": 1218,
    "preview": "from setuptools import setup, find_packages\nimport os\n\n\npackage_name = \"humanoid_control\"\n\ndata_files = [\n    (\n        "
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/src/common/motor_crc.cpp",
    "chars": 1789,
    "preview": "#include \"motor_crc.h\"\n\n\nvoid get_crc(unitree_go::msg::LowCmd& msg)\n{\n    LowCmd raw{};\n    memcpy(&raw.head[0], &msg.he"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/src/common/motor_crc_hg.cpp",
    "chars": 1355,
    "preview": "#include \"motor_crc_hg.h\"\n\nvoid get_crc(unitree_hg::msg::LowCmd &msg)\n{\n    LowCmd raw{};\n\n    raw.modePr = msg.mode_pr;"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/src/common/ros2_sport_client.cpp",
    "chars": 5111,
    "preview": "#include \"ros2_sport_client.h\"\n\nvoid SportClient::Damp(unitree_api::msg::Request &req)\n{\n    req.header.identity.api_id "
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/src/common/wireless_controller.cpp",
    "chars": 1995,
    "preview": "#include \"common/wireless_controller.h\"\n#include <cstring>\n\n// Define static constants\nconst int KeyMap::R1 = 0;\nconst i"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/src/src/main_node.cpp",
    "chars": 33455,
    "preview": "/**\n * This example demonstrates how to use ROS2 to send low-level motor commands of\n *unitree g1 robot 29 dof\n **/\n #in"
  },
  {
    "path": "deployment/unitree_g1_ros2_29dof/start_container.sh",
    "chars": 1694,
    "preview": "#!/bin/bash\n\ndocker kill holomotion_orin_deploy\ndocker rm holomotion_orin_deploy\necho \"Old holomotion_orin_deploy contai"
  },
  {
    "path": "docs/environment_setup.md",
    "chars": 3003,
    "preview": "# Environment Setup\n\n## Step 1: Setup Conda\n\nThis project uses conda to manage Python environments. We recommend using ["
  },
  {
    "path": "docs/evaluate_motion_tracking.md",
    "chars": 2268,
    "preview": "## Evaluate the Motion Tracking Model\n\nAfter training for a while and saving model checkpoints, it is necessary to run t"
  },
  {
    "path": "docs/holomotion_motion_file_spec.md",
    "chars": 3679,
    "preview": "## HoloMotion NPZ Format — Keys and Values\n\nThis document lists the exact keys saved in a HoloMotion NPZ and their value"
  },
  {
    "path": "docs/motion_retargeting.md",
    "chars": 2333,
    "preview": "# Motion Retargeting\n\nTransform human motion data into robot-compatible joint trajectories for following training. We su"
  },
  {
    "path": "docs/mujoco_sim2sim.md",
    "chars": 533,
    "preview": "# Sim2Sim Verification\n\nAfter generating the ONNX file from the evaluation stage, you can verify the performance of your"
  },
  {
    "path": "docs/realworld_deployment.md",
    "chars": 16899,
    "preview": "# HoloMotion Deployment Guide\n\nThis guide describes how to set up the deployment environment and run the trained policy "
  },
  {
    "path": "docs/smpl_data_curation.md",
    "chars": 7515,
    "preview": "# Dataset Preparation Guide\n\nThis guide describes the workflow and setup for preparing datasets to train the motion trac"
  },
  {
    "path": "docs/train_motion_tracking.md",
    "chars": 4263,
    "preview": "## Train the Motion Tracking Model\n\nAfter completing motion retargeting, you can train a motion tracking model with Holo"
  },
  {
    "path": "environments/environment_deploy.yaml",
    "chars": 710,
    "preview": "name: holomotion_deploy\nchannels:\n  - pytorch\n  - nvidia  \n  - conda-forge\n  - defaults\n\ndependencies:\n  # Python runtim"
  },
  {
    "path": "environments/environment_train_isaaclab_cu118.yaml",
    "chars": 231,
    "preview": "name: holomotion_train\nchannels:\n  - conda-forge\ndependencies:\n  - python=3.11\n  - pip\n  - mesalib\n  - pip:\n      - -r r"
  },
  {
    "path": "environments/environment_train_isaaclab_cu128.yaml",
    "chars": 231,
    "preview": "name: holomotion_train\nchannels:\n  - conda-forge\ndependencies:\n  - python=3.11\n  - pip\n  - mesalib\n  - pip:\n      - -r r"
  },
  {
    "path": "environments/requirements_base.txt",
    "chars": 601,
    "preview": "isaacsim[all,extscache]==5.0.0\nisaaclab[isaacsim,all]==2.2.0\n\n\nsetuptools\nwheel\nnumpy==1.26.0\nsmplx==0.1.28\nhydra-core=="
  },
  {
    "path": "environments/requirements_deploy.txt",
    "chars": 584,
    "preview": "# Machine Learning Runtime  \nonnxruntime-gpu\n\n# SMPL/SMPLX support\nsmplx==0.1.28\n\n# Configuration management\nhydra-core="
  },
  {
    "path": "environments/requirements_torch_cu118.txt",
    "chars": 151,
    "preview": "--extra-index-url https://download.pytorch.org/whl/cu118\n--extra-index-url https://pypi.nvidia.com\n\ntorch==2.7.0\ntorchvi"
  },
  {
    "path": "environments/requirements_torch_cu128.txt",
    "chars": 171,
    "preview": "--extra-index-url https://download.pytorch.org/whl/cu128\n--extra-index-url https://pypi.nvidia.com\n\ntorch==2.10.0+cu128\n"
  },
  {
    "path": "environments/requirements_torch_cu130.txt",
    "chars": 171,
    "preview": "--extra-index-url https://download.pytorch.org/whl/cu130\n--extra-index-url https://pypi.nvidia.com\n\ntorch==2.10.0+cu130\n"
  },
  {
    "path": "holomotion/config/algo/ppo.yaml",
    "chars": 2638,
    "preview": "# @package _global_\n\nalgo:\n  _target_: holomotion.src.algo.ppo.PPO\n  _recursive_: false\n  config:\n    # --- General Sett"
  },
  {
    "path": "holomotion/config/algo/ppo_tf.yaml",
    "chars": 1896,
    "preview": "# @package _global_\n\ndefaults:\n  - ppo\n\nalgo:\n  _target_: holomotion.src.algo.ppo_tf.PPOTF\n  config:\n    num_steps_per_e"
  },
  {
    "path": "holomotion/config/data_curation/joints2smpl.yaml",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "holomotion/config/data_curation/smplify_base.yaml",
    "chars": 0,
    "preview": ""
  },
  {
    "path": "holomotion/config/env/domain_randomization/NO_domain_rand.yaml",
    "chars": 1645,
    "preview": "# @package _global_\n\ndomain_rand:\n  action_delay:\n    enabled: false\n\n  erfi:\n    enabled: false\n\n  motion_init_perturb:"
  },
  {
    "path": "holomotion/config/env/domain_randomization/domain_rand_medium.yaml",
    "chars": 3126,
    "preview": "# @package _global_\n\ndomain_rand:\n  action_delay:\n    enabled: true\n    min_delay: 0\n    max_delay: 2\n\n  erfi:\n    enabl"
  },
  {
    "path": "holomotion/config/env/domain_randomization/domain_rand_small.yaml",
    "chars": 3131,
    "preview": "# @package _global_\n\ndomain_rand:\n  action_delay:\n    enabled: false\n    min_delay: 0\n    max_delay: 0\n\n  erfi:\n    enab"
  },
  {
    "path": "holomotion/config/env/domain_randomization/domain_rand_strong.yaml",
    "chars": 3125,
    "preview": "# @package _global_\n\ndomain_rand:\n  action_delay:\n    enabled: true\n    min_delay: 0\n    max_delay: 4\n\n  erfi:\n    enabl"
  },
  {
    "path": "holomotion/config/env/motion_tracking.yaml",
    "chars": 5865,
    "preview": "# @package _global_\n\nenv:\n  _target_: holomotion.src.env.motion_tracking.MotionTrackingEnv\n  _recursive_: False\n  config"
  },
  {
    "path": "holomotion/config/env/observations/motion_tracking/obs_motion_tracking_mlp.yaml",
    "chars": 11735,
    "preview": "# @package _global_\n\nobs:\n  context_length: 32\n  n_fut_frames: 10\n  target_fps: 50\n  actor_obs_prefix: \"ref_\"\n  critic_o"
  },
  {
    "path": "holomotion/config/env/observations/motion_tracking/obs_motion_tracking_tf-moe.yaml",
    "chars": 11734,
    "preview": "# @package _global_\n\nobs:\n  context_length: 1\n  n_fut_frames: 10\n  target_fps: 50\n  actor_obs_prefix: \"ref_\"\n  critic_ob"
  },
  {
    "path": "holomotion/config/env/observations/velocity_tracking/obs_velocity_tracking.yaml",
    "chars": 2922,
    "preview": "# @package _global_\n\nobs:\n  context_length: 8\n  n_fut_frames: 0\n  target_fps: 50\n\n  obs_groups:\n    unified:\n      atomi"
  },
  {
    "path": "holomotion/config/env/rewards/motion_tracking/rew_motion_tracking.yaml",
    "chars": 1637,
    "preview": "# @package _global_\n\nrewards:\n  _config:\n    reward_prefix: \"ref_\"\n\n  is_alive:\n    weight: 0.5\n    params: {}\n\n  root_p"
  },
  {
    "path": "holomotion/config/env/rewards/velocity_tracking/rew_velocity_tracking.yaml",
    "chars": 3602,
    "preview": "# @package _global_\n\nrewards:\n  stand_still_action_rate:\n    weight: -1.0\n    params:\n      command_name: base_velocity\n"
  },
  {
    "path": "holomotion/config/env/terminations/NO_termination.yaml",
    "chars": 38,
    "preview": "# @package _global_\n\nterminations: {}\n"
  },
  {
    "path": "holomotion/config/env/terminations/termination_motion_tracking.yaml",
    "chars": 608,
    "preview": "# @package _global_\n\nterminations:\n\n  time_out:\n    time_out: true\n\n  ref_gravity_projection_far:\n    params:\n      thre"
  },
  {
    "path": "holomotion/config/env/terminations/termination_velocity_tracking.yaml",
    "chars": 189,
    "preview": "# @package _global_\n\nterminations:\n  time_out:\n    time_out: true\n\n  root_height_below_minimum:\n    params:\n      minimu"
  },
  {
    "path": "holomotion/config/env/terrain/isaaclab_plane.yaml",
    "chars": 1255,
    "preview": "# @package _global_\n\n# Simple flat terrain generated via IsaacLab height-field TerrainGenerator.\n# Uses random-uniform h"
  },
  {
    "path": "holomotion/config/env/terrain/isaaclab_rough.yaml",
    "chars": 1479,
    "preview": "# @package _global_\n\n# Rough height-field terrain for locomotion training.\n# Uses random-uniform height field to create "
  },
  {
    "path": "holomotion/config/env/velocity_tracking.yaml",
    "chars": 2053,
    "preview": "# @package _global_\n\nenv:\n  _target_: holomotion.src.env.velocity_tracking.VelocityTrackingEnv\n  _recursive_: False\n  co"
  },
  {
    "path": "holomotion/config/evaluation/eval_isaaclab.yaml",
    "chars": 2597,
    "preview": "# @package _global_\n\ndefaults:\n  - /robot: unitree/G1/29dof/29dof_training_isaaclab\n  - /env: motion_tracking\n  - /env/t"
  },
  {
    "path": "holomotion/config/evaluation/eval_mujoco_sim2sim.yaml",
    "chars": 2683,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n\n# Evaluation toggles\nheadless: false # true to run without GUI\nrecord_video: "
  },
  {
    "path": "holomotion/config/evaluation/eval_velocity_tracking.yaml",
    "chars": 2365,
    "preview": "# @package _global_\n\ndefaults:\n  - /robot: unitree/G1/29dof/29dof_training_isaaclab\n  - /env: velocity_tracking\n  - /env"
  },
  {
    "path": "holomotion/config/modules/motion_tracking/motion_tracking_mlp.yaml",
    "chars": 3560,
    "preview": "# @package _global_\n\nmodules:\n  actor:\n    type: MLP\n\n    hidden_norm: none\n    layer_config:\n      hidden_dims:\n       "
  },
  {
    "path": "holomotion/config/modules/motion_tracking/motion_tracking_tf-moe.yaml",
    "chars": 4427,
    "preview": "# @package _global_\n\nmodules:\n  actor:\n    type: ReferenceRoutedGroupedMoETransformerPolicy\n\n    use_checkpointing: fals"
  },
  {
    "path": "holomotion/config/modules/velocity_tracking/velocity_tracking_mlp.yaml",
    "chars": 2107,
    "preview": "# @package _global_\n\nmodules:\n  actor:\n    type: MLP\n    fix_sigma: false\n    noise_std_type: scalar\n    obs_norm:\n     "
  },
  {
    "path": "holomotion/config/motion_retargeting/gmr_to_holomotion.yaml",
    "chars": 1146,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n\nhydra:\n  job:\n    chdir: false\n\nio:\n  src_dir: ???\n  robot_config: ???\n  out_"
  },
  {
    "path": "holomotion/config/motion_retargeting/holomotion_preprocess.yaml",
    "chars": 929,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n\nhydra:\n  job:\n    chdir: false\n\nio:\n  src_root: ???\n  out_root: ???\n\npreproce"
  },
  {
    "path": "holomotion/config/motion_retargeting/kinematic_filter.yaml",
    "chars": 574,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n\nhydra:\n  job:\n    chdir: false\n\nio:\n  dataset_root: \"\" # absolute path to dat"
  },
  {
    "path": "holomotion/config/motion_retargeting/pack_hdf5_database.yaml",
    "chars": 1135,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n  - /robot: unitree/G1/29dof/29dof_training_isaaclab\n\nhydra:\n  job:\n    chdir:"
  },
  {
    "path": "holomotion/config/motion_retargeting/pack_hdf5_v2.yaml",
    "chars": 370,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n  - /robot: unitree/G1/29dof/29dof_training_isaaclab\n\nhydra:\n  job:\n    chdir:"
  },
  {
    "path": "holomotion/config/motion_retargeting/unitree_G1_29dof_retargeting.yaml",
    "chars": 436,
    "preview": "robot:\n  humanoid_type: unitree/G1/29dof\n\n  asset:\n    smpl_dir: \"assets/smpl\"\n    assetRoot: \"./\"\n    assetFileName: \"a"
  },
  {
    "path": "holomotion/config/mujoco_eval/sim2sim.yaml",
    "chars": 706,
    "preview": "# @package _group_\n\ndefaults:\n  - _self_\n\nenabled: false\nmodel_type: \"holomotion\"\n\n# Evaluation toggles\nheadless: false\n"
  },
  {
    "path": "holomotion/config/robot/unitree/G1/29dof/29dof_training_isaaclab.yaml",
    "chars": 15598,
    "preview": "# @package _global_\n\nrobot:\n  humanoid_type: unitree/G1/29dof\n\n  dof_obs_size: 29\n  actions_dim: 29\n  num_bodies: 30\n  n"
  },
  {
    "path": "holomotion/config/robot/unitree/G1/29dof/29dof_training_isaaclab_s100.yaml",
    "chars": 177,
    "preview": "# @package _global_\n\ndefaults:\n  - unitree/G1/29dof/29dof_training_isaaclab\n\nrobot:\n  asset:\n    urdf_file: \"assets/robo"
  },
  {
    "path": "holomotion/config/training/motion_tracking/train_g1_29dof_motion_tracking_mlp.yaml",
    "chars": 562,
    "preview": "# @package _global_\n\ndefaults:\n  - /training: train_base\n  - /algo: ppo\n  - /robot: unitree/G1/29dof/29dof_training_isaa"
  },
  {
    "path": "holomotion/config/training/motion_tracking/train_g1_29dof_motion_tracking_tf-moe.yaml",
    "chars": 570,
    "preview": "# @package _global_\n\ndefaults:\n  - /training: train_base\n  - /algo: ppo_tf\n  - /robot: unitree/G1/29dof/29dof_training_i"
  },
  {
    "path": "holomotion/config/training/train_base.yaml",
    "chars": 450,
    "preview": "# @package _global_\n\ndefaults:\n  - _self_\n  - /mujoco_eval: sim2sim\n\nproject_name: ???\nexperiment_name: ???\n\nnum_envs: ?"
  },
  {
    "path": "holomotion/config/training/velocity_tracking/train_g1_29dof_velocity_tracking_mlp.yaml",
    "chars": 1186,
    "preview": "# @package _global_\n\ndefaults:\n  - /training: train_base\n  - /algo: ppo\n  - /robot: unitree/G1/29dof/29dof_training_isaa"
  },
  {
    "path": "holomotion/scripts/data_curation/convert_to_amass.sh",
    "chars": 237,
    "preview": "source train.env\n\n# 默认原始数据路径\nDATA_ROOT=\"./data/raw_datasets\"\n\n# 如果传入参数就覆盖默认\nif [ ! -z \"$1\" ]; then\n    DATA_ROOT=\"$1\"\nfi"
  },
  {
    "path": "holomotion/scripts/data_curation/filter_smpl_data.sh",
    "chars": 1349,
    "preview": "source train.env\n\n# default json lisy\ndefault_jsonl_list=(\"humanact12\" \"MotionX\" \"OMOMO\" \"ZJU_Mocap\" \"amass\")\njsonl_list"
  },
  {
    "path": "holomotion/scripts/data_curation/video_to_smpl_gvhmr.sh",
    "chars": 919,
    "preview": "export CONDA_BASE=$(conda info --base)\nexport Train_CONDA_PREFIX=\"$CONDA_BASE/envs/gvhmr\"\n\nvideo_folder_root=\"holomotion"
  },
  {
    "path": "holomotion/scripts/data_curation/visualize_smpl_npz.sh",
    "chars": 178,
    "preview": "export CONDA_BASE=$(conda info --base)\nexport Train_CONDA_PREFIX=\"$CONDA_BASE/envs/gvhmr\"\n\n$Train_CONDA_PREFIX/bin/pytho"
  },
  {
    "path": "holomotion/scripts/evaluation/calc_offline_eval_metrics.sh",
    "chars": 1112,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/evaluation/eval_motion_tracking.sh",
    "chars": 1350,
    "preview": "#!/bin/bash\n\n# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under "
  },
  {
    "path": "holomotion/scripts/evaluation/eval_mujoco_sim2sim.sh",
    "chars": 1602,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/evaluation/eval_velocity_tracking.sh",
    "chars": 1210,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/evaluation/mean_process_5metrics.py",
    "chars": 9907,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/evaluation/multi_model_metrics_analysis.sh",
    "chars": 841,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/apply_gmr_motion_retarget_patch.sh",
    "chars": 12246,
    "preview": "#!/usr/bin/env bash\n# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/pack_hdf5_v2.sh",
    "chars": 1041,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_holomotion_preprocessing.sh",
    "chars": 1226,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_kinematic_filter.sh",
    "chars": 859,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_motion_retargeting_gmr_bvh.sh",
    "chars": 1065,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_motion_retargeting_gmr_smplx.sh",
    "chars": 1063,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_motion_retargeting_gmr_to_holomotion.sh",
    "chars": 1299,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/motion_retargeting/run_motion_viz_mujoco.sh",
    "chars": 1062,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/training/train_motion_tracking.sh",
    "chars": 1435,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/scripts/training/train_velocity_tracking.sh",
    "chars": 1284,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/algo/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/algo/algo_base.py",
    "chars": 30645,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/algo/algo_utils.py",
    "chars": 23050,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/algo/ppo.py",
    "chars": 79995,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/algo/ppo_tf.py",
    "chars": 107076,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/.gitignore",
    "chars": 12,
    "preview": "_generated/\n"
  },
  {
    "path": "holomotion/src/data_curation/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/data_smplify.py",
    "chars": 3140,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/filter/filter.py",
    "chars": 8116,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/filter/label_data.py",
    "chars": 11817,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smpl_npz_to_html.py",
    "chars": 7311,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smplify/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smplify/smplify_humanact12.py",
    "chars": 5568,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smplify/smplify_motionx.py",
    "chars": 3512,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smplify/smplify_omomo.py",
    "chars": 12909,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/smplify/smplify_zjumocap.py",
    "chars": 4320,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/templates/index_wooden_static.html",
    "chars": 26303,
    "preview": "<!--\n# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apac"
  },
  {
    "path": "holomotion/src/data_curation/video_to_smpl_gvhmr.py",
    "chars": 20541,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/vison_mocap/joints2smpl.py",
    "chars": 4941,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/data_curation/visualize_smpl_npz.py",
    "chars": 10565,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/__init__.py",
    "chars": 1899,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_actions.py",
    "chars": 3175,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_curriculum.py",
    "chars": 23849,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_domain_rand.py",
    "chars": 7190,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_motion_tracking_command.py",
    "chars": 120072,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_observation.py",
    "chars": 68126,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_rewards.py",
    "chars": 85200,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_scene.py",
    "chars": 23372,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_simulator.py",
    "chars": 1514,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_termination.py",
    "chars": 9205,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_terrain.py",
    "chars": 21963,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_utils.py",
    "chars": 3443,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/isaaclab_velocity_tracking_command.py",
    "chars": 16266,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/isaaclab_components/unitree_actuators.py",
    "chars": 18921,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/motion_tracking.py",
    "chars": 23324,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/env/velocity_tracking.py",
    "chars": 15290,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/eval_motion_tracking.py",
    "chars": 6086,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/eval_motion_tracking_single.py",
    "chars": 11911,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/eval_mujoco_sim2sim.py",
    "chars": 177967,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/eval_velocity_tracking.py",
    "chars": 5427,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/find_worst_clips.py",
    "chars": 3046,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/metrics.py",
    "chars": 52033,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/multi_model_metrics_report.py",
    "chars": 15807,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/obs/__init__.py",
    "chars": 137,
    "preview": "from .obs_builder import PolicyObsBuilder, get_gravity_orientation\n\n__all__ = [\n    \"PolicyObsBuilder\",\n    \"get_gravity"
  },
  {
    "path": "holomotion/src/evaluation/obs/obs_builder.py",
    "chars": 7933,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/ray_evaluator_actor.py",
    "chars": 3355,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/evaluation/ray_metrics_postprocess.py",
    "chars": 2629,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/modules/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/modules/agent_modules.py",
    "chars": 98896,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/modules/network_modules.py",
    "chars": 199211,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/gmr_to_holomotion.py",
    "chars": 35919,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/holomotion_fk.py",
    "chars": 33625,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/holomotion_preprocess.py",
    "chars": 43171,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/kinematic_filter.py",
    "chars": 8935,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/pack_hdf5_v2.py",
    "chars": 21066,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/reference_filtering.py",
    "chars": 6384,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/utils/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/utils/_schema.json",
    "chars": 730,
    "preview": "{\n  \"schema\": {\n    \"root_trans_offset\": {\n      \"shape\": [\n        682,\n        3\n      ],\n      \"dtype\": \"float64\"\n   "
  },
  {
    "path": "holomotion/src/motion_retargeting/utils/rotation_conversions.py",
    "chars": 20091,
    "preview": "# Copyright (c) Facebook, Inc. and its affiliates.\n# All rights reserved.\n#\n# This source code is licensed under the BSD"
  },
  {
    "path": "holomotion/src/motion_retargeting/utils/torch_humanoid_batch.py",
    "chars": 25328,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/motion_retargeting/utils/visualize_with_mujoco.py",
    "chars": 19967,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/training/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/training/h5_dataloader.py",
    "chars": 128512,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/training/reference_filter_export.py",
    "chars": 13304,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/training/train.py",
    "chars": 6322,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/utils/__init__.py",
    "chars": 636,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/utils/config.py",
    "chars": 7149,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  },
  {
    "path": "holomotion/src/utils/frame_utils.py",
    "chars": 4658,
    "preview": "# Project HoloMotion\n#\n# Copyright (c) 2024-2026 Horizon Robotics. All Rights Reserved.\n#\n# Licensed under the Apache Li"
  }
]

// ... and 49 more files (download for full content)

About this extraction

This page contains the full source code of the HorizonRobotics/HoloMotion GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 249 files (2.8 MB), approximately 741.1k tokens, and a symbol index with 2254 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.

Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.

Copied to clipboard!