Tinghong-Ye22 commited on
Commit
cfcd06e
·
verified ·
1 Parent(s): 1611afb

Add files using upload-large-folder tool

Browse files
Files changed (50) hide show
  1. .gitignore +114 -0
  2. CLAUDE.md +114 -0
  3. DOC.md +23 -0
  4. LICENSE +7 -0
  5. README.md +554 -0
  6. TEST_MOTIONS.md +17 -0
  7. amass_to_robot.sh +21 -0
  8. assets/optitrack.png +0 -0
  9. assets/pal_talos/LICENSE +176 -0
  10. assets/pal_talos/assets/gripper/base_link_collision.stl +0 -0
  11. assets/pal_talos/assets/gripper/fingertip.stl +0 -0
  12. assets/pal_talos/assets/gripper/fingertip_collision.stl +0 -0
  13. assets/pal_talos/assets/gripper/gripper_motor_double_collision.stl +0 -0
  14. assets/pal_talos/assets/gripper/gripper_motor_single_collision.stl +0 -0
  15. assets/pal_talos/assets/gripper/gripper_simple_collision.stl +0 -0
  16. assets/pal_talos/assets/gripper/inner_double.stl +0 -0
  17. assets/pal_talos/assets/gripper/inner_double_collision.stl +0 -0
  18. assets/pal_talos/assets/gripper/inner_single.stl +0 -0
  19. assets/pal_talos/assets/gripper/inner_single_collision.stl +0 -0
  20. assets/pal_talos/assets/head/head_1_collision.stl +0 -0
  21. assets/pal_talos/assets/head/head_2_default_collision.stl +0 -0
  22. assets/pal_talos/assets/torso/torso_2_collision.stl +0 -0
  23. assets/pal_talos/talos.xml +508 -0
  24. check_phmr_pkl.py +17 -0
  25. phmr_to_robot.sh +45 -0
  26. setup.py +32 -0
  27. third_party/poselib/__init__.py +3 -0
  28. third_party/poselib/core/__init__.py +3 -0
  29. third_party/poselib/core/backend/__init__.py +3 -0
  30. third_party/poselib/core/backend/abstract.py +121 -0
  31. third_party/poselib/core/backend/logger.py +13 -0
  32. third_party/poselib/core/rotation3d.py +443 -0
  33. third_party/poselib/core/tensor_utils.py +36 -0
  34. third_party/poselib/core/tests/__init__.py +0 -0
  35. third_party/poselib/core/tests/test_rotation.py +55 -0
  36. third_party/poselib/fbx_importer.py +43 -0
  37. third_party/poselib/skeleton/__init__.py +0 -0
  38. third_party/poselib/skeleton/backend/__init__.py +0 -0
  39. third_party/poselib/skeleton/backend/fbx/__init__.py +0 -0
  40. third_party/poselib/skeleton/backend/fbx/fbx_backend.py +245 -0
  41. third_party/poselib/skeleton/backend/fbx/fbx_read_wrapper.py +32 -0
  42. third_party/poselib/skeleton/skeleton3d.py +1421 -0
  43. third_party/poselib/visualization/__init__.py +0 -0
  44. third_party/poselib/visualization/common.py +182 -0
  45. third_party/poselib/visualization/core.py +71 -0
  46. third_party/poselib/visualization/plt_plotter.py +395 -0
  47. third_party/poselib/visualization/simple_plotter_tasks.py +185 -0
  48. third_party/poselib/visualization/skeleton_plotter_tasks.py +187 -0
  49. third_party/poselib/visualization/tests/__init__.py +0 -0
  50. third_party/poselib/visualization/tests/test_plotter.py +16 -0
.gitignore ADDED
@@ -0,0 +1,114 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Python
2
+ *.egg-info/
3
+ __pycache__/
4
+ *.pyc
5
+ *.pyo
6
+ *.pyd
7
+ .Python
8
+ env/
9
+ pip-log.txt
10
+ pip-delete-this-directory.txt
11
+ .tox/
12
+ .coverage
13
+ .coverage.*
14
+ .cache
15
+ nosetests.xml
16
+ coverage.xml
17
+ *.cover
18
+ *.log
19
+ .git
20
+ .mypy_cache
21
+ .pytest_cache
22
+ .hypothesis
23
+
24
+ # IDEs
25
+ .vscode/
26
+ .idea/
27
+ *.swp
28
+ *.swo
29
+ *~
30
+
31
+ # OS
32
+ .DS_Store
33
+ .DS_Store?
34
+ ._*
35
+ .Spotlight-V100
36
+ .Trashes
37
+ ehthumbs.db
38
+ Thumbs.db
39
+
40
+ # Data directories
41
+ data/
42
+ data_record/
43
+ Download/
44
+ assets/body_models/
45
+
46
+ # Media files
47
+ *.mp4
48
+ *.avi
49
+ *.mov
50
+ *.wmv
51
+ *.flv
52
+ *.webm
53
+
54
+ # Archives
55
+ *.zip
56
+ *.tar
57
+ *.tar.gz
58
+ *.rar
59
+ *.7z
60
+
61
+ # Model files
62
+ smplx/
63
+ body_models/
64
+ *.pkl
65
+ *.pth
66
+ *.pt
67
+ *.ckpt
68
+
69
+ # Logs
70
+ MUJOCO_LOG.TXT
71
+ *.log
72
+
73
+ # Session files
74
+ session*.json
75
+
76
+ # Temporary files
77
+ *.tmp
78
+ *.temp
79
+ .tmp/
80
+ .temp/
81
+
82
+ # Build artifacts
83
+ build/
84
+ dist/
85
+ *.egg-info/
86
+
87
+ # Environment files
88
+ .env
89
+ .env.local
90
+ .env.*.local
91
+
92
+ # Jupyter
93
+ .ipynb_checkpoints/
94
+
95
+ # Cache
96
+ .cache/
97
+ *.cache
98
+
99
+ videos/
100
+ PythonClient/
101
+
102
+ TEST_MOTIONS_MY.md
103
+
104
+ tmp/
105
+
106
+ motion_data/
107
+
108
+ GVHMR/
109
+
110
+ batch_twist1_to_csv.py
111
+
112
+ yaml_to_flat_csv.py
113
+
114
+ compute_rotation.py
CLAUDE.md ADDED
@@ -0,0 +1,114 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # CLAUDE.md
2
+
3
+ This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
4
+
5
+ ## Installation and Setup
6
+
7
+ This is a Python package for motion retargeting to humanoid robots. Install in development mode:
8
+
9
+ ```bash
10
+ conda create -n gmr python=3.10 -y
11
+ conda activate gmr
12
+ pip install -e .
13
+ conda install -c conda-forge libstdcxx-ng -y
14
+ ```
15
+
16
+ ## Code Architecture
17
+
18
+ ### Core Components
19
+
20
+ - **`GeneralMotionRetargeting`** (`general_motion_retargeting/motion_retarget.py`): Main class for motion retargeting using inverse kinematics (IK) solver built on mink/mujoco
21
+ - **`KinematicsModel`** (`general_motion_retargeting/kinematics_model.py`): Handles robot kinematics calculations
22
+ - **`RobotMotionViewer`** (`general_motion_retargeting/robot_motion_viewer.py`): MuJoCo-based visualization for robot motions
23
+ - **Configuration System** (`general_motion_retargeting/params.py`): Simplified robot definitions and IK config mappings - cleaned to focus on core supported robots
24
+
25
+ ### Data Flow
26
+
27
+ 1. **Human Motion Input**: SMPL-X (AMASS/OMOMO) or BVH (LAFAN1) format
28
+ 2. **Motion Format**: Each frame = dict of (human_body_name, 3D translation + rotation)
29
+ 3. **Robot Output**: Tuple of (base_translation, base_rotation, joint_positions)
30
+ 4. **IK Configs**: JSON files in `general_motion_retargeting/ik_configs/` define human-to-robot body mappings
31
+
32
+ ### Supported Robots
33
+
34
+ Core robot models in `assets/` directory:
35
+ - Unitree G1 (`unitree_g1`) - 29 DOF humanoid
36
+ - Booster T1 (`booster_t1`) - Full-body humanoid
37
+ - Booster K1 (`booster_k1`) - 22 DOF humanoid
38
+ - Stanford ToddlerBot (`stanford_toddy`) - Research humanoid
39
+ - Fourier N1 (`fourier_n1`) - Commercial humanoid
40
+ - ENGINEAI PM01 (`engineai_pm01`) - Industrial humanoid
41
+ - Kuavo S45 (`kuavo_s45`) - 28 DOF humanoid
42
+ - HighTorque Hi (`hightorque_hi`) - 25 DOF humanoid
43
+ - Galaxea R1 Pro (`galaxea_r1pro`) - 24 DOF wheeled humanoid
44
+
45
+ Additional models retained in ROBOT_BASE_DICT for compatibility:
46
+ - `unitree_g1_with_hands` (43 DOF with dexterous hands)
47
+ - `dex31_left_hand`, `dex31_right_hand` (hand components)
48
+
49
+ ## Common Commands
50
+
51
+ ### Single Motion Retargeting
52
+ ```bash
53
+ # SMPL-X to robot
54
+ python scripts/smplx_to_robot.py --smplx_file <path> --robot <robot_name> --save_path <output.pkl>
55
+
56
+ # BVH to robot
57
+ python scripts/bvh_to_robot.py --bvh_file <path> --robot <robot_name> --save_path <output.pkl>
58
+ ```
59
+
60
+ ### Batch Processing
61
+ ```bash
62
+ # Process datasets
63
+ python scripts/smplx_to_robot_dataset.py
64
+ python scripts/bvh_to_robot_dataset.py
65
+ ```
66
+
67
+ ### Visualization
68
+ ```bash
69
+ # Visualize saved robot motion
70
+ python scripts/vis_robot_motion.py --robot <robot_name> --robot_motion_path <path.pkl>
71
+ ```
72
+
73
+ Add `--record_video --video_path <output.mp4>` to any visualization command to record video.
74
+
75
+ ## Key Technical Details
76
+
77
+ - **IK Solver**: Uses mink library with configurable solver (default: "daqp") and damping (default: 5e-1)
78
+ - **Human Height Scaling**: Automatic scaling based on `actual_human_height` parameter vs config assumptions
79
+ - **Real-time Performance**: Optimized for 60-70 FPS on high-end CPUs for teleoperation use cases
80
+ - **Body Model Dependencies**: Requires SMPL-X body models in `assets/body_models/smplx/`
81
+
82
+ ## File Organization
83
+
84
+ - `scripts/`: Entry point scripts for different retargeting workflows
85
+ - `general_motion_retargeting/`: Core library code
86
+ - `assets/`: Robot models (MuJoCo XML) and body models (SMPL-X)
87
+ - `general_motion_retargeting/ik_configs/`: JSON configuration files for human-to-robot body mappings:
88
+ - SMPL-X configs: `smplx_to_{g1,t1,k1,toddy,n1,pm01,kuavo,hi,r1pro}.json`
89
+ - BVH configs: `bvh_to_{g1,t1,toddy,n1,pm01}.json`
90
+ - FBX configs: `fbx_to_g1.json`
91
+
92
+ ## Project Status & Features
93
+
94
+ **Current State**: Production-ready motion retargeting system with extensive robot support
95
+
96
+ **Key Capabilities**:
97
+ - **Multi-format Input**: SMPL-X (AMASS/OMOMO), BVH (LAFAN1), FBX (OptiTrack)
98
+ - **Real-time Performance**: 60-70 FPS on high-end hardware for teleoperation
99
+ - **9 Robot Models**: From research platforms to commercial humanoids
100
+ - **Robust IK**: Mink-based solver with automatic human height scaling
101
+ - **Visualization**: MuJoCo-based viewer with video recording capabilities
102
+ - **Batch Processing**: Dataset-level retargeting workflows
103
+
104
+ **Use Cases**:
105
+ - Real-time whole-body teleoperation (see [TWIST](https://github.com/YanjieZe/TWIST))
106
+ - RL policy training data generation
107
+ - Motion capture to robot deployment
108
+ - Cross-platform humanoid motion transfer
109
+
110
+ **Recent Additions** (2025):
111
+ - Booster K1 support (9th robot)
112
+ - Dexterous hand integration (G1 + Dex31)
113
+ - Wheeled humanoid support (Galaxea R1 Pro)
114
+ - Enhanced OptiTrack real-time streaming
DOC.md ADDED
@@ -0,0 +1,23 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # IK Config
2
+ In our ik config such as `smplx_to_g1.json`, you might find following params. I add annotations here for your understanding.
3
+ ```json
4
+ "ik_match_table1": {
5
+ "pelvis": [ # robot's body name
6
+ "pelvis", # corresponding human body name, here we are using "pelvis" as example
7
+ 100, # weight to track 3D positions (xyz)
8
+ 10, # weight to track 3D rotations
9
+ [
10
+ 0.0, # x offset added to human body "pelvis" x
11
+ 0.0, # y offset added to human body "pelvis" y
12
+ 0.0 # z offset added to human body "pelvis" z
13
+ ],
14
+ [
15
+ # the rotation (represented as quaternion) applied to human body "pelvis". the order follows scalar first (wxyz)
16
+ 0.5,
17
+ -0.5,
18
+ -0.5,
19
+ -0.5
20
+ ]
21
+ ],
22
+ ...
23
+ ```
LICENSE ADDED
@@ -0,0 +1,7 @@
 
 
 
 
 
 
 
 
1
+ Copyright 2025 Yanjieze
2
+
3
+ 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:
4
+
5
+ The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
6
+
7
+ 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.
README.md ADDED
@@ -0,0 +1,554 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # GMR: General Motion Retargeting
2
+
3
+ <a href="https://arxiv.org/abs/2505.02833">
4
+ <img src="https://img.shields.io/badge/paper-arXiv%3A2505.02833-b31b1b.svg" alt="arXiv Paper"/>
5
+ </a> <a href="https://arxiv.org/abs/2510.02252">
6
+ <img src="https://img.shields.io/badge/paper-arXiv%3A2510.02252-b31b1b.svg" alt="arXiv Paper"/>
7
+ </a> <a href="https://opensource.org/licenses/MIT">
8
+ <img src="https://img.shields.io/badge/License-MIT-yellow.svg" alt="License: MIT"/>
9
+ </a> <a href="https://github.com/YanjieZe/GMR/releases">
10
+ <img src="https://img.shields.io/badge/version-0.2.0-blue.svg" alt="Version"/>
11
+ </a> <a href="https://x.com/ZeYanjie/status/1952446745696469334">
12
+ <img src="https://img.shields.io/badge/twitter-ZeYanjie-blue.svg" alt="Twitter"/>
13
+ </a> <a href="https://yanjieze.github.io/humanoid-foundation/#GMR">
14
+ <img src="https://img.shields.io/badge/blog-GMR-blue.svg" alt="Blog"/>
15
+ </a> <a href="https://www.bilibili.com/video/BV1p1nazeEzC/?share_source=copy_web&vd_source=c76e3ab14ac3f7219a9006b96b4b0f76">
16
+ <img src="https://img.shields.io/badge/tutorial-BILIBILI-blue.svg" alt="Blog"/>
17
+ </a>
18
+
19
+ ![Banner for GMR](./assets/GMR.png)
20
+
21
+ ![GMR](./assets/GMR_pipeline.png)
22
+
23
+ #### Key features of GMR:
24
+ - Real-time high-quality retargeting, unlock the potential of real-time whole-body teleoperation, i.e., [TWIST](https://github.com/YanjieZe/TWIST).
25
+ - Carefully tuned for good performance of RL tracking policies.
26
+ - Support multiple humanoid robots and multiple human motion data formats (See our table below).
27
+
28
+ > [!NOTE]
29
+ > If you want this repo to support a new robot or a new human motion data format, send the robot files (`.xml`, `.urdf`, and meshes) / human motion data to <a href="mailto:lastyanjieze@gmail.com">Yanjie Ze</a> or create an issue, we will support it as soon as possible. And please make sure the robot files you sent can be open-sourced in this repo.
30
+
31
+ This repo is licensed under the [MIT License](LICENSE).
32
+
33
+
34
+ # News & Updates
35
+ - **2026-01-21:** GMR now supports [Xsens](https://www.xsens.com/) BVH offline data.
36
+ - **2026-01-12:** GMR now supports [Fourier GR3](https://www.fftai.com/), the 17th humanoid robot in the repo.
37
+ - **2025-12-02:** GMR now supports [TWIST2](https://yanjieze.com/TWIST2), which utilizes [XRoboToolkit SDK](https://github.com/XR-Robotics/XRoboToolkit-PC-Service).
38
+ - **2025-11-17:** To join our community for discussions, you can add my WeChat contact [QR Code](https://yanjieze.com/TWIST2/images/my_wechat.jpg) with info like "[GMR] [Your Name] [Your Affiliation]".
39
+ - **2025-11-08:** [MimicKit] from Jason Peng now supports GMR format. Check [here](https://github.com/xbpeng/MimicKit/tree/main/tools/gmr_to_mimickit).
40
+ - **2025-10-15:** Now supporting [PAL Robotics' Talos](https://pal-robotics.com/robot/talos/), the 15th humanoid robot.
41
+ - **2025-10-14:** GMR now supports [Nokov](https://www.nokov.com/) BVH data.
42
+ - **2025-10-14:** Add a doc on ik config. See [DOC.md](DOC.md)
43
+ - **2025-10-09:** Check [TWIST](https://github.com/YanjieZe/TWIST) open-sourced code for RL motion tracking.
44
+ - **2025-10-02:** Tech report for GMR is now on [arXiv](https://arxiv.org/abs/2510.02252).
45
+ - **2025-10-01:** GMR now supports converting GMR pickle files to CSV (for beyondmimic), check `scripts/batch_gmr_pkl_to_csv.py`.
46
+ - **2025-09-25:** An introduction on GMR is available on [Bilibili](https://www.bilibili.com/video/BV1p1nazeEzC/?share_source=copy_web&vd_source=c76e3ab14ac3f7219a9006b96b4b0f76).
47
+ - **2025-09-16:** GMR now supports to use [GVHMR](https://github.com/zju3dv/GVHMR) for extracting human pose from **monocular video** and retargeting to robot.
48
+ - **2025-09-12:** GMR now supports [Tienkung](https://github.com/Open-X-Humanoid/TienKung-Lab), the 14th humanoid robot in the repo.
49
+ - **2025-08-30:** GMR now supports [Unitree H1 2](https://www.unitree.com/cn/h1) and [PND Adam Lite](https://pndbotics.com/), the 12th and 13th humanoid robots in the repo.
50
+ - **2025-08-28:** GMR now supports [Booster T1](https://www.boosterobotics.com/) for both 23dof and 29dof.
51
+ - **2025-08-28:** GMR now supports using exported offline FBX motion data from [OptiTrack](https://www.optitrack.com/).
52
+ - **2025-08-27:** GMR now supports [Berkeley Humanoid Lite](https://github.com/HybridRobotics/Berkeley-Humanoid-Lite-Assets), the 11th humanoid robot in the repo.
53
+ - **2025-08-24:** GMR now supports [Unitree H1](https://www.unitree.com/h1/), the 10th humanoid robot in the repo.
54
+ - **2025-08-24:** GMR now supports velocity limits for the robot motors, `use_velocity_limit=True` by default in `GeneralMotionRetargeting` class (and we use 3*pi as the velocity limit by default); we also add printing of robot DoF/Body/Motor names and their IDs by default, and you can access them via `robot_dof_names`, `robot_body_names`, and `robot_motor_names` attributes.
55
+ - **2025-08-10:** GMR now supports [Booster K1](https://www.boosterobotics.com/), the 9th robot in the repo.
56
+ - **2025-08-09:** GMR now supports *Unitree G1 with Dex31 hands*.
57
+ - **2025-08-07:** GMR now supports [Galexea R1 Pro](https://galaxea-dynamics.com/) (this is a wheeled humanoid robot!) and [KUAVO](https://www.kuavo.ai/), the 7th and 8th humanoid robots in the repo.
58
+ - **2025-08-06:** GMR now supports [HighTorque Hi](https://www.hightorquerobotics.com/hi/), the 6th humanoid robot in the repo.
59
+ - **2025-08-04:** Initial release of GMR. Check our [twitter post](https://x.com/ZeYanjie/status/1952446745696469334).
60
+
61
+ ## Demos
62
+
63
+ <table>
64
+ <tr>
65
+ <td align="center" width="20%">
66
+ <b>Demo 1</b><br>
67
+ Retargeting LAFAN1 dancing motion to 5 robots.<br>
68
+ <video src="https://github.com/user-attachments/assets/23566fa5-6335-46b9-957b-4b26aed11b9e" width="200" controls></video>
69
+ </td>
70
+ <td align="center" width="20%">
71
+ <b>Demo 2</b><br>
72
+ Galexea R1 Pro robot (view 1).<br>
73
+ <video src="https://github.com/user-attachments/assets/903ed0b0-0ac5-4226-8f82-5a88631e9b7c" width="200" controls></video>
74
+ </td>
75
+ <td align="center" width="20%">
76
+ <b>Demo 3</b><br>
77
+ Galexea R1 Pro robot (view 2).<br>
78
+ <video src="https://github.com/user-attachments/assets/deea0e64-f1c6-41bc-8661-351682006d5d" width="200" controls></video>
79
+ </td>
80
+ <td align="center" width="20%">
81
+ <b>Demo 4</b><br>
82
+ Switching robots by changing one argument.<br>
83
+ <video src="https://github.com/user-attachments/assets/03f10902-c541-40b1-8104-715a5759fd5e" width="200" controls></video>
84
+ </td>
85
+ <td align="center" width="20%">
86
+ <b>Demo 5</b><br>
87
+ HighTorque robot doing a twist dance.<br>
88
+ <video src="https://github.com/user-attachments/assets/1d3e663b-f29e-41b1-8e15-5c0deb6a4a5c" width="200" controls></video>
89
+ </td>
90
+ </tr>
91
+
92
+ <tr>
93
+ <td align="center">
94
+ <b>Demo 6</b><br>
95
+ Kuavo robot picking up a box.<br>
96
+ <video src="https://github.com/user-attachments/assets/02fc8f41-c363-484b-a329-4f4e83ed5b80" width="200" controls></video>
97
+ </td>
98
+ <td align="center">
99
+ <b>Demo 7</b><br>
100
+ Unitree H1 robot doing a ChaCha dance.<br>
101
+ <video src="https://github.com/user-attachments/assets/28ee6f0f-be30-42bb-8543-cf1152d97724" width="200" controls></video>
102
+ </td>
103
+ <td align="center">
104
+ <b>Demo 8</b><br>
105
+ Booster T1 robot jumping (view 1).<br>
106
+ <video src="https://github.com/user-attachments/assets/2c75a146-e28f-4327-930f-5281bfc2ca9c" width="200" controls></video>
107
+ </td>
108
+ <td align="center">
109
+ <b>Demo 9</b><br>
110
+ Booster T1 robot jumping (view 2).<br>
111
+ <video src="https://github.com/user-attachments/assets/ff10c7ef-4357-4789-9219-23c6db8dba6d" width="200" controls></video>
112
+ </td>
113
+ <td align="center">
114
+ <b>Demo 10</b><br>
115
+ Unitree H1-2 robot jumping.<br>
116
+ <video src="https://github.com/user-attachments/assets/2382d8ce-7902-432f-ab45-348a11eeb312" width="200" controls></video>
117
+ </td>
118
+ </tr>
119
+
120
+ <tr>
121
+ <td align="center">
122
+ <b>Demo 11</b><br>
123
+ PND Adam Lite robot.<br>
124
+ <video src="https://github.com/user-attachments/assets/a8ef1409-88f1-4393-9cd0-d2b14216d2a4" width="200" controls></video>
125
+ </td>
126
+ <td align="center">
127
+ <b>Demo 12</b><br>
128
+ Tienkung robot walking.<br>
129
+ <video src="https://github.com/user-attachments/assets/7a775ecc-4254-450c-a3eb-49e843b8e331" width="200" controls></video>
130
+ </td>
131
+ <td align="center">
132
+ <b>Demo 13</b><br>
133
+ Extracting human pose (GVHMR + GMR).<br>
134
+ <a href="https://www.bilibili.com/video/BV1Tnpmz9EaE">▶ Watch on Bilibili</a>
135
+ </td>
136
+ <td align="center">
137
+ <b>Demo 14</b><br>
138
+ PAL Robotics’ Talos robot fighting.<br>
139
+ <video src="https://github.com/user-attachments/assets/3ec0bf80-80c1-4181-a623-dc2b072c2ca2" width="200" controls></video>
140
+ </td>
141
+ <td align="center">
142
+ <b>Demo 15</b><br>
143
+ (Optional placeholder if you add a new one later!)<br>
144
+ <i>Coming soon...</i>
145
+ </td>
146
+ </tr>
147
+ </table>
148
+
149
+
150
+ ## Supported Robots and Data Formats
151
+
152
+
153
+
154
+ | Assigned ID | Robot/Data Format | Robot DoF | SMPLX ([AMASS](https://amass.is.tue.mpg.de/), [OMOMO](https://github.com/lijiaman/omomo_release)) | BVH [LAFAN1](https://github.com/ubisoft/ubisoft-laforge-animation-dataset)| FBX ([OptiTrack](https://www.optitrack.com/)) | BVH [Nokov](https://www.nokov.com/) | PICO ([XRoboToolkit](https://github.com/XR-Robotics/XRoboToolkit-PC-Service)) | More formats coming soon |
155
+ | --- | --- | --- | --- | --- | --- | --- | --- | --- |
156
+ | 0 | Unitree G1 `unitree_g1` | Leg (2\*6) + Waist (3) + Arm (2\*7) = 29 | ✅ | ✅ | ✅ | ✅ | ✅ |
157
+ | 1 | Unitree G1 with Hands `unitree_g1_with_hands` | Leg (2\*6) + Waist (3) + Arm (2\*7) + Hand (2\*7) = 43 | ✅ | ✅ | ✅ | TBD | TBD |
158
+ | 2 | Unitree H1 `unitree_h1` | Leg (2\*5) + Waist (1) + Arm (2\*4) = 19 | ✅ | TBD | TBD | TBD | TBD |
159
+ | 3 | Unitree H1 2 `unitree_h1_2` | Leg (2\*6) + Waist (1) + Arm (2\*7) = 27 | ✅ | TBD | TBD | TBD | TBD |
160
+ | 4 | Booster T1 `booster_t1` | TBD | ✅ | TBD | TBD | TBD |
161
+ | 5 | Booster T1 29dof `booster_t1_29dof` | TBD | ✅ | ✅ | TBD | TBD |
162
+ | 6 | Booster K1 `booster_k1` | Neck (2) + Arm (2\*4) + Leg (2\*6) = 22 | ✅ | TBD | TBD | TBD |
163
+ | 7 | Stanford ToddlerBot `stanford_toddy` | TBD | ✅ | ✅ | TBD | TBD |
164
+ | 8 | Fourier N1 `fourier_n1` | TBD | ✅ | ✅ | TBD | TBD |
165
+ | 9 | ENGINEAI PM01 `engineai_pm01` | TBD | ✅ | ✅ | TBD | TBD |
166
+ | 10 | HighTorque Hi `hightorque_hi` | Head (2) + Arm (2\*5) + Waist (1) + Leg (2\*6) = 25 | ✅ | TBD | TBD | TBD |
167
+ | 11 | Galaxea R1 Pro `galaxea_r1pro` (this is a wheeled robot!) | Base (6) + Torso (4) + Arm (2*7) = 24 | ✅ | TBD | TBD | TBD |
168
+ | 12 | Kuavo `kuavo_s45` | Head (2) + Arm (2\*7) + Leg (2\*6) = 28 | ✅ | TBD | TBD | TBD |
169
+ | 13 | Berkeley Humanoid Lite `berkeley_humanoid_lite` (need further tuning) | Leg (2\*6) + Arm (2\*5) = 22 | ✅ | TBD | TBD | TBD |
170
+ | 14 | PND Adam Lite `pnd_adam_lite` | Leg (2\*6) + Waist (3) + Arm (2\*5) = 25 | ✅ | TBD | TBD | TBD |
171
+ | 15 | Tienkung `tienkung` | Leg (2\*6) + Arm (2\*4) = 20 | ✅ | TBD | TBD | TBD |
172
+ | 16 | PAL Robotics' Talos `pal_talos` | Head (2) + Arm (2\*7) + Waist (2) + Leg (2\*6) = 30 | ✅ | TBD | TBD | TBD |
173
+ | 17 | Fourier GR3 `fourier_gr3` | Head (2) + Arm (2\*7) + Waist (3) + Leg (2\*6) = 31 | ✅ | TBD | TBD | TBD |
174
+ | More robots coming soon ! |
175
+ | 18 | AgiBot A2 `agibot_a2` | TBD | TBD | TBD | TBD | TBD |
176
+ | 19 | OpenLoong `openloong` | TBD | TBD | TBD | TBD | TBD |
177
+
178
+
179
+
180
+
181
+ ## Installation
182
+
183
+ > [!NOTE]
184
+ > The code is tested on Ubuntu 22.04/20.04.
185
+
186
+ First create your conda environment:
187
+
188
+ ```bash
189
+ conda create -n gmr python=3.10 -y
190
+ conda activate gmr
191
+ ```
192
+
193
+ Then, install GMR:
194
+
195
+ ```bash
196
+ pip install -e .
197
+ ```
198
+
199
+ After installing SMPLX, change `ext` in `smplx/body_models.py` from `npz` to `pkl` if you are using SMPL-X pkl files.
200
+
201
+ And to resolve some possible rendering issues:
202
+
203
+ ```bash
204
+ conda install -c conda-forge libstdcxx-ng -y
205
+ ```
206
+
207
+ ## Data Preparation
208
+
209
+ [[SMPLX](https://github.com/vchoutas/smplx) body model] download SMPL-X body models to `assets/body_models` from [SMPL-X](https://smpl-x.is.tue.mpg.de/) and then structure as follows:
210
+ ```bash
211
+ - assets/body_models/smplx/
212
+ -- SMPLX_NEUTRAL.pkl
213
+ -- SMPLX_FEMALE.pkl
214
+ -- SMPLX_MALE.pkl
215
+ ```
216
+
217
+ [[AMASS](https://amass.is.tue.mpg.de/) motion data] download raw SMPL-X data to any folder you want from [AMASS](https://amass.is.tue.mpg.de/). NOTE: Do not download SMPL+H data.
218
+
219
+ [[OMOMO](https://github.com/lijiaman/omomo_release) motion data] download raw OMOMO data to any folder you want from [this google drive file](https://drive.google.com/file/d/1tZVqLB7II0whI-Qjz-z-AU3ponSEyAmm/view?usp=sharing). And process the data into the SMPL-X format using `scripts/convert_omomo_to_smplx.py`.
220
+
221
+ [[LAFAN1](https://github.com/ubisoft/ubisoft-laforge-animation-dataset) motion data] download raw LAFAN1 bvh files from [the official repo](https://github.com/ubisoft/ubisoft-laforge-animation-dataset), i.e., [lafan1.zip](https://github.com/ubisoft/ubisoft-laforge-animation-dataset/blob/master/lafan1/lafan1.zip).
222
+
223
+
224
+ ## Human/Robot Motion Data Formulation
225
+
226
+ To better use this library, you can first have an understanding of the human motion data we use and the robot motion data we obtain.
227
+
228
+ Each frame of **human motion data** is formulated as a dict of (human_body_name, 3d global translation + global rotation). The rotation is usually represented as quaternion (with wxyz order by default, to align with mujoco).
229
+
230
+ Each frame of **robot motion data** can be understood as a tuple of (robot_base_translation, robot_base_rotation, robot_joint_positions).
231
+
232
+ ## Usage
233
+
234
+ ### [NEW] PICO Streaming to Robot (TWIST2)
235
+
236
+ Install PICO SDK:
237
+ 1. On your PICO, install PICO SDK: see [here](https://github.com/XR-Robotics/XRoboToolkit-Unity-Client/releases/).
238
+ 2. On your own PC,
239
+ - Download [deb package for ubuntu 22.04](https://github.com/XR-Robotics/XRoboToolkit-PC-Service/releases/download/v1.0.0/XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb), or build from the [repo source](https://github.com/XR-Robotics/XRoboToolkit-PC-Service).
240
+ - To install, use command
241
+ ```bash
242
+ sudo dpkg -i XRoboToolkit_PC_Service_1.0.0_ubuntu_22.04_amd64.deb
243
+ ```
244
+ then you should see `xrobotoolkit-pc-service` in your APPs. remember to start this app before you do teleopperation.
245
+ - Build PICO PC Service SDK and Python SDK for PICO streaming:
246
+ ```bash
247
+ conda activate gmr
248
+
249
+ git clone https://github.com/YanjieZe/XRoboToolkit-PC-Service-Pybind.git
250
+ cd XRoboToolkit-PC-Service-Pybind
251
+
252
+ mkdir -p tmp
253
+ cd tmp
254
+ git clone https://github.com/XR-Robotics/XRoboToolkit-PC-Service.git
255
+ cd XRoboToolkit-PC-Service/RoboticsService/PXREARobotSDK
256
+ bash build.sh
257
+ cd ../../../..
258
+
259
+
260
+ mkdir -p lib
261
+ mkdir -p include
262
+ cp tmp/XRoboToolkit-PC-Service/RoboticsService/PXREARobotSDK/PXREARobotSDK.h include/
263
+ cp -r tmp/XRoboToolkit-PC-Service/RoboticsService/PXREARobotSDK/nlohmann include/nlohmann/
264
+ cp tmp/XRoboToolkit-PC-Service/RoboticsService/PXREARobotSDK/build/libPXREARobotSDK.so lib/
265
+ # rm -rf tmp
266
+
267
+ # Build the project
268
+ conda install -c conda-forge pybind11
269
+ pip uninstall -y xrobotoolkit_sdk
270
+ python setup.py install
271
+ ```
272
+
273
+ You should be all set!
274
+
275
+ To try it, check [this script from TWIST2](https://github.com/amazon-far/TWIST2/blob/master/teleop.sh):
276
+ ```bash
277
+ bash teleop.sh
278
+ ```
279
+ You should be able to see the retargeted robot motion in a mujoco window.
280
+
281
+ ### Retargeting from SMPL-X (AMASS, OMOMO) to Robot
282
+
283
+ > [!NOTE]
284
+ > NOTE: after install SMPL-X, change `ext` in `smplx/body_models.py` from `npz` to `pkl` if you are using SMPL-X pkl files.
285
+
286
+ Retarget a single motion:
287
+
288
+ ```bash
289
+ python scripts/smplx_to_robot.py --smplx_file <path_to_smplx_data> --robot <path_to_robot_data> --save_path <path_to_save_robot_data.pkl> --rate_limit
290
+ ```
291
+
292
+ By default you should see the visualization of the retargeted robot motion in a mujoco window.
293
+ If you want to record video, add `--record_video` and `--video_path <your_video_path,mp4>`.
294
+
295
+ - `--rate_limit` is used to limit the rate of the retargeted robot motion to keep the same as the human motion. If you want it as fast as possible, remove `--rate_limit`.
296
+
297
+ Retarget a folder of motions:
298
+
299
+ ```bash
300
+ python scripts/smplx_to_robot_dataset.py --src_folder <path_to_dir_of_smplx_data> --tgt_folder <path_to_dir_to_save_robot_data> --robot <robot_name>
301
+ ```
302
+
303
+ By default there is no visualization for batch retargeting.
304
+
305
+ ### Retargeting from GVHMR to Robot
306
+
307
+ First, install GVHMR by following [their official instructions](https://github.com/zju3dv/GVHMR/blob/main/docs/INSTALL.md).
308
+
309
+ And run their demo that can extract human pose from monocular video:
310
+
311
+ ```bash
312
+ cd path/to/GVHMR
313
+ python tools/demo/demo.py --video=docs/example_video/tennis.mp4 -s
314
+ ```
315
+
316
+ Then you should obtain the saved human pose data in `GVHMR/outputs/demo/tennis/hmr4d_results.pt`.
317
+
318
+ Then, run the command below to retarget the extracted human pose data to your robot:
319
+
320
+ ```bash
321
+ python scripts/gvhmr_to_robot.py --gvhmr_pred_file <path_to_hmr4d_results.pt> --robot unitree_g1 --record_video
322
+ ```
323
+
324
+
325
+
326
+ ## Retargeting from BVH (LAFAN1, Nokov) to Robot
327
+
328
+ Retarget a single motion:
329
+
330
+ ```bash
331
+ # single motion
332
+ python scripts/bvh_to_robot.py --bvh_file <path_to_bvh_data> --robot <path_to_robot_data> --save_path <path_to_save_robot_data.pkl> --rate_limit --format <format>
333
+ ```
334
+
335
+ By default you should see the visualization of the retargeted robot motion in a mujoco window.
336
+ - `--rate_limit` is used to limit the rate of the retargeted robot motion to keep the same as the human motion. If you want it as fast as possible, remove `--rate_limit`.
337
+ - `--format` is used to specify the format of the BVH data. Supported formats are `lafan1` and `nokov`.
338
+
339
+
340
+ Retarget a folder of motions:
341
+
342
+ ```bash
343
+ python scripts/bvh_to_robot_dataset.py --src_folder <path_to_dir_of_bvh_data> --tgt_folder <path_to_dir_to_save_robot_data> --robot <robot_name>
344
+ ```
345
+
346
+ By default there is no visualization for batch retargeting.
347
+
348
+
349
+
350
+ ## Retargeting from BVH (Xsens) to Robot
351
+
352
+ #### Visualize bvh data using mujoco:
353
+
354
+ Install PyQt6:
355
+ ```bash
356
+ pip install PyQt6 PyQt6-Qt6 PyQt6-sip
357
+ ```
358
+
359
+
360
+ ```bash
361
+ python general_motion_retargeting/utils/xsens_vendor/mujoco_xsens_bvh_view.py \
362
+ --bvh_file <path_to_dir_of_bvh_data> \
363
+ --scale <displacement scaling size> \
364
+ --reset_to_zero
365
+ ```
366
+ like
367
+ ```bash
368
+ python general_motion_retargeting/utils/xsens_vendor/mujoco_xsens_bvh_view.py \
369
+ --scale 0.01 \
370
+ --bvh_file assets/xsens_bvh_test/251021_04_boxing_120Hz_cm_3DsMax.bvh \
371
+ --reset_to_zero
372
+ ```
373
+
374
+ - `--start` is used to specify the initial processing frame. If no input is given, processing will start from the first frame by default.
375
+
376
+ - `--end` is used to specify the final processing frame. If not input, it will be processed by default to the last frame.
377
+
378
+ - `--reset_to_zero` is used to reset the displacement and Z-axis rotation to zero.This function, when used in combination with `--start`, will set the data to the initial zero position very well.Because sometimes the first one or two frames of some datasets differ too much from the subsequent data, these data need to be discarded.
379
+
380
+ - `--scale` is used to set the scaling value of the displacement, which depends on the relationship between the unit used for the displacement in the dataset and the meter.
381
+
382
+ - ##### Before using it, you must install PyQt6. `pip install PyQt6`
383
+ - ##### Upon executing this command, a UI interface will be launched, enabling you to adjust the angle values for each channel in the x, y, and z directions of every joint. After completing the adjustments, click the `"Apply and Preview"` button, which will generate an `offset.json` file locally and perform a MuJoCo visualization playback of the BVH file. When running `xsens_bvh_to_robot.py`, it will read the data from this JSON file. Therefore, you need to execute `mujoco_xsens_bvh_view.py` prior to using `xsens_bvh_to_robot.py` for motion retargeting, to ensure that the `offset.json` file exists locally.
384
+
385
+ #### Retarget a single motion:
386
+ ```bash
387
+ # single motion
388
+ python scripts/xsens_bvh_to_robot.py \
389
+ --bvh_file <path_to_bvh_data> \
390
+ --robot <path_to_robot_data> \
391
+ --save_path <path_to_save_robot_data.pkl> \
392
+ --rate_limit \
393
+ --start <number of the first frame> \
394
+ --scale <displacement scaling size> \
395
+ --reset_to_zero \
396
+ --bvh_format <exported bvh format>
397
+ ```
398
+ like
399
+ ```bash
400
+ python scripts/xsens_bvh_to_robot.py \
401
+ --robot unitree_h1_2 \
402
+ --scale 0.01 \
403
+ --reset_to_zero \
404
+ --bvh_format 3DSM \
405
+ --bvh_file assets/xsens_bvh_test/251021_04_boxing_120Hz_cm_3DsMax.bvh \
406
+ --save_path retargeting_data/h1/251021_04_boxing_120Hz_cm_3DsMax.pkl
407
+ ```
408
+ ##### By default you should see the visualization of the retargeted robot motion in a mujoco window.
409
+ - `--rate_limit` is used to limit the rate of the retargeted robot motion to keep the same as the human motion. If you want it as fast as possible, remove `--rate_limit`.
410
+
411
+ - `--start` is used to specify the initial processing frame. If no input is given, processing will start from the first frame by default.
412
+
413
+ - `--end` is used to specify the final processing frame. If not input, it will be processed by default to the last frame.
414
+
415
+ - `--reset_to_zero` is used to reset the displacement and Z-axis rotation to zero.This function, when used in combination with `--start`, will set the data to the initial zero position very well.Because sometimes the first one or two frames of some datasets differ too much from the subsequent data, these data need to be discarded.
416
+
417
+ - `--scale` is used to set the scaling value of the displacement, which depends on the relationship between the unit used for the displacement in the dataset and the meter.
418
+
419
+ ##### !!!!!!!!!!!!!!!!!! ATTENTION !!!!!!!!!!!!!!!!!!!!
420
+ - `--bvh_format` is used to set the format of the bvh being parsed. In the Xsens MVN software, BVH files in three formats can be exported. There will be some differences among BVH files in different formats. Here I recommend using the 3D Studio Max format.(In fact, I have not yet completed the parsing of data in other formats.)
421
+
422
+ - The exported pkl file will represent quaternions in the `wxyz` format. ^ _ ^
423
+
424
+
425
+ ### Retargeting from FBX (OptiTrack) to Robot
426
+
427
+ #### Offline FBX Files
428
+
429
+ Retarget a single motion:
430
+
431
+ 1. Install `fbx_sdk` by following [these instructions](https://github.com/nv-tlabs/ASE/tree/main/ase/poselib#importing-from-fbx) and [these instructions](https://github.com/nv-tlabs/ASE/issues/61#issuecomment-2670315114). You will probably need a new conda environment for this.
432
+
433
+ 2. Activate the conda environment where you installed `fbx_sdk`.
434
+ Use the following command to extract motion data from your `.fbx` file:
435
+
436
+ ```bash
437
+ cd third_party
438
+ python poselib/fbx_importer.py --input <path_to_fbx_file.fbx> --output <path_to_save_motion_data.pkl> --root-joint <root_joint_name> --fps <fps>
439
+ ```
440
+
441
+ 3. Then, run the command below to retarget the extracted motion data to your robot:
442
+
443
+ ```bash
444
+ conda activate gmr
445
+ # single motion
446
+ python scripts/fbx_offline_to_robot.py --motion_file <path_to_saved_motion_data.pkl> --robot <path_to_robot_data> --save_path <path_to_save_robot_data.pkl> --rate_limit
447
+ ```
448
+
449
+ By default you should see the visualization of the retargeted robot motion in a mujoco window.
450
+
451
+ - `--rate_limit` is used to limit the rate of the retargeted robot motion to keep the same as the human motion. If you want it as fast as possible, remove `--rate_limit`.
452
+
453
+ #### Online Streaming
454
+
455
+ We provide the script to use OptiTrack MoCap data for real-time streaming and retargeting.
456
+
457
+ Usually you will have two computers, one is the server that installed with Motive (Desktop APP for OptiTrack) and the other is the client that installed with GMR.
458
+
459
+ Find the server ip (the computer that installed with Motive) and client ip (your computer). Set the streaming as follows:
460
+
461
+ ![OptiTrack Streaming](./assets/optitrack.png)
462
+
463
+ And then run:
464
+
465
+ ```bash
466
+ python scripts/optitrack_to_robot.py --server_ip <server_ip> --client_ip <client_ip> --use_multicast False --robot unitree_g1
467
+ ```
468
+
469
+ You should see the visualization of the retargeted robot motion in a mujoco window.
470
+
471
+ ### Visualize saved robot motion
472
+
473
+ Visualize a single motions:
474
+
475
+ ```bash
476
+ python scripts/vis_robot_motion.py --robot <robot_name> --robot_motion_path <path_to_save_robot_data.pkl>
477
+ ```
478
+
479
+ If you want to record video, add `--record_video` and `--video_path <your_video_path,mp4>`.
480
+
481
+ Visualize a folder of motions:
482
+
483
+ ```bash
484
+ python scripts/vis_robot_motion_dataset.py --robot <robot_name> --robot_motion_folder <path_to_save_robot_data_folder>
485
+ ```
486
+
487
+ After launching the MuJoCo visualization window and clicking on it, you can use the following keyboard controls::
488
+ * `[`: play the previous motion
489
+ * `]`: play the next motion
490
+ * `space`: toggle play/pause
491
+
492
+ ## Speed Benchmark
493
+
494
+ | CPU | Retargeting Speed |
495
+ | --- | --- |
496
+ | AMD Ryzen Threadripper 7960X 24-Cores | 60~70 FPS |
497
+ | 13th Gen Intel Core i9-13900K 24-Cores | 35~45 FPS |
498
+ | TBD | TBD |
499
+
500
+ ## Citation
501
+
502
+ If you find our code useful, please consider citing our related papers:
503
+
504
+ ```bibtex
505
+ @article{joao2025gmr,
506
+ title={Retargeting Matters: General Motion Retargeting for Humanoid Motion Tracking},
507
+ author= {Joao Pedro Araujo and Yanjie Ze and Pei Xu and Jiajun Wu and C. Karen Liu},
508
+ year= {2025},
509
+ journal= {arXiv preprint arXiv:2510.02252}
510
+ }
511
+ ```
512
+
513
+ ```bibtex
514
+ @article{ze2025twist,
515
+ title={TWIST: Teleoperated Whole-Body Imitation System},
516
+ author= {Yanjie Ze and Zixuan Chen and João Pedro Araújo and Zi-ang Cao and Xue Bin Peng and Jiajun Wu and C. Karen Liu},
517
+ year= {2025},
518
+ journal= {arXiv preprint arXiv:2505.02833}
519
+ }
520
+ ```
521
+
522
+ and this github repo:
523
+
524
+ ```bibtex
525
+ @software{ze2025gmr,
526
+ title={GMR: General Motion Retargeting},
527
+ author= {Yanjie Ze and João Pedro Araújo and Jiajun Wu and C. Karen Liu},
528
+ year= {2025},
529
+ url= {https://github.com/YanjieZe/GMR},
530
+ note= {GitHub repository}
531
+ }
532
+ ```
533
+
534
+ ## Known Issues
535
+
536
+ Designing a single config for all different humans is not trivial. We observe some motions might have bad retargeting results. If you observe some bad results, please let us know! We now have a collection of such motions in [TEST_MOTIONS.md](TEST_MOTIONS.md).
537
+
538
+ ## Acknowledgement
539
+
540
+ Our IK solver is built upon [mink](https://github.com/kevinzakka/mink) and [mujoco](https://github.com/google-deepmind/mujoco). Our visualization is built upon [mujoco](https://github.com/google-deepmind/mujoco). The human motion data we try includes [AMASS](https://amass.is.tue.mpg.de/), [OMOMO](https://github.com/lijiaman/omomo_release), and [LAFAN1](https://github.com/ubisoft/ubisoft-laforge-animation-dataset).
541
+
542
+ The original robot models can be found at the following locations:
543
+
544
+ * [Berkley Humanoid Lite](https://github.com/HybridRobotics/Berkeley-Humanoid-Lite-Assets): CC-BY-SA-4.0 license
545
+ * [Booster K1](https://www.boosterobotics.com/)
546
+ * [Booster T1](https://booster.feishu.cn/wiki/UvowwBes1iNvvUkoeeVc3p5wnUg) ([English](https://booster.feishu.cn/wiki/DtFgwVXYxiBT8BksUPjcOwG4n4f))
547
+ * [EngineAI PM01](https://github.com/engineai-robotics/engineai_ros2_workspace): [Link to file](https://github.com/engineai-robotics/engineai_ros2_workspace/blob/community/src/simulation/mujoco/assets/resource)
548
+ * [Fourier N1](https://github.com/FFTAI/Wiki-GRx-Gym): [Link to file](https://github.com/FFTAI/Wiki-GRx-Gym/tree/FourierN1/legged_gym/resources/robots/N1)
549
+ * [Galaxea R1 Pro](https://galaxea-dynamics.com/): MIT license
550
+ * [HighToqure Hi](https://www.hightorquerobotics.com/hi/)
551
+ * [LEJU Kuavo S45](https://gitee.com/leju-robot/kuavo-ros-opensource/blob/master/LICENSE): MIT license
552
+ * [PAL Robotics' Talos](https://github.com/google-deepmind/mujoco_menagerie): [Link to file](https://github.com/google-deepmind/mujoco_menagerie/tree/main/pal_talos)
553
+ * [Toddlerbot](https://github.com/hshi74/toddlerbot): [Link to file](https://github.com/hshi74/toddlerbot/tree/main/toddlerbot/descriptions/toddlerbot_active)
554
+ * [Unitree G1](https://github.com/unitreerobotics/unitree_ros): [Link to file](https://github.com/unitreerobotics/unitree_ros/tree/master/robots/g1_description)
TEST_MOTIONS.md ADDED
@@ -0,0 +1,17 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ # Test Motions
2
+
3
+ We list some test motions here that are not easy for retargeting (i.e., sometimes giving bad results).
4
+ The [x] means the motion is retargeted successfully.
5
+
6
+
7
+ # AMASS
8
+
9
+ - [x] BMLrub_rub081_0031_rom_stageii.npz
10
+ - [x] KIT_3_walk_6m_straight_line04_stageii.npz
11
+ - [ ] DanceDB/20120807_CliodelaVara/Clio_Maleviziotikos_stageii.npz
12
+ - [ ] DanceDB/20130216_AnnaCharalambous/Anna_Curiosity_C3D_stageii.npz (Body jittering)
13
+ - [ ] CMU/111/111_21_stageii.npz (Should be laying on the ground facing right. The right arm is wristed on G1)
14
+
15
+ # LAFAN1
16
+
17
+ - [x] dance1_subject2.bvh
amass_to_robot.sh ADDED
@@ -0,0 +1,21 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ cd /cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/code/GMR
2
+ pkill -f monitor.py
3
+ conda activate /cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/envs/gmr
4
+
5
+ # 如果要保存视频,需要先apt install -y xvfb
6
+ # export PYOPENGL_PLATFORM="egl"
7
+ # export MUJOCO_GL="egl"
8
+ # xvfb-run -a python scripts/smplx_to_robot.py \
9
+ # --smplx_file "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/code/AMASS/ACCAD/Female1General_c3d/A2_-_Sway_stageii.npz" \
10
+ # --robot "unitree_g1" \
11
+ # --save_path "outputs/ACCAD/Female1General_c3d/A2_-_Sway_stageii.pkl" \
12
+ # --rate_limit \
13
+ # --record_video
14
+
15
+ # 如果不打算可视化
16
+ python scripts/smplx_to_robot.py \
17
+ --smplx_file "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/code/AMASS/ACCAD/Female1General_c3d/A2_-_Sway_stageii.npz" \
18
+ --robot "unitree_g1" \
19
+ --save_path "outputs/ACCAD/Female1General_c3d/A2_-_Sway_stageii_cpy.pkl" \
20
+ --rate_limit \
21
+ --headless
assets/optitrack.png ADDED
assets/pal_talos/LICENSE ADDED
@@ -0,0 +1,176 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ Apache License
2
+ Version 2.0, January 2004
3
+ http://www.apache.org/licenses/
4
+
5
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
6
+
7
+ 1. Definitions.
8
+
9
+ "License" shall mean the terms and conditions for use, reproduction,
10
+ and distribution as defined by Sections 1 through 9 of this document.
11
+
12
+ "Licensor" shall mean the copyright owner or entity authorized by
13
+ the copyright owner that is granting the License.
14
+
15
+ "Legal Entity" shall mean the union of the acting entity and all
16
+ other entities that control, are controlled by, or are under common
17
+ control with that entity. For the purposes of this definition,
18
+ "control" means (i) the power, direct or indirect, to cause the
19
+ direction or management of such entity, whether by contract or
20
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
21
+ outstanding shares, or (iii) beneficial ownership of such entity.
22
+
23
+ "You" (or "Your") shall mean an individual or Legal Entity
24
+ exercising permissions granted by this License.
25
+
26
+ "Source" form shall mean the preferred form for making modifications,
27
+ including but not limited to software source code, documentation
28
+ source, and configuration files.
29
+
30
+ "Object" form shall mean any form resulting from mechanical
31
+ transformation or translation of a Source form, including but
32
+ not limited to compiled object code, generated documentation,
33
+ and conversions to other media types.
34
+
35
+ "Work" shall mean the work of authorship, whether in Source or
36
+ Object form, made available under the License, as indicated by a
37
+ copyright notice that is included in or attached to the work
38
+ (an example is provided in the Appendix below).
39
+
40
+ "Derivative Works" shall mean any work, whether in Source or Object
41
+ form, that is based on (or derived from) the Work and for which the
42
+ editorial revisions, annotations, elaborations, or other modifications
43
+ represent, as a whole, an original work of authorship. For the purposes
44
+ of this License, Derivative Works shall not include works that remain
45
+ separable from, or merely link (or bind by name) to the interfaces of,
46
+ the Work and Derivative Works thereof.
47
+
48
+ "Contribution" shall mean any work of authorship, including
49
+ the original version of the Work and any modifications or additions
50
+ to that Work or Derivative Works thereof, that is intentionally
51
+ submitted to Licensor for inclusion in the Work by the copyright owner
52
+ or by an individual or Legal Entity authorized to submit on behalf of
53
+ the copyright owner. For the purposes of this definition, "submitted"
54
+ means any form of electronic, verbal, or written communication sent
55
+ to the Licensor or its representatives, including but not limited to
56
+ communication on electronic mailing lists, source code control systems,
57
+ and issue tracking systems that are managed by, or on behalf of, the
58
+ Licensor for the purpose of discussing and improving the Work, but
59
+ excluding communication that is conspicuously marked or otherwise
60
+ designated in writing by the copyright owner as "Not a Contribution."
61
+
62
+ "Contributor" shall mean Licensor and any individual or Legal Entity
63
+ on behalf of whom a Contribution has been received by Licensor and
64
+ subsequently incorporated within the Work.
65
+
66
+ 2. Grant of Copyright License. Subject to the terms and conditions of
67
+ this License, each Contributor hereby grants to You a perpetual,
68
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
69
+ copyright license to reproduce, prepare Derivative Works of,
70
+ publicly display, publicly perform, sublicense, and distribute the
71
+ Work and such Derivative Works in Source or Object form.
72
+
73
+ 3. Grant of Patent License. Subject to the terms and conditions of
74
+ this License, each Contributor hereby grants to You a perpetual,
75
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
76
+ (except as stated in this section) patent license to make, have made,
77
+ use, offer to sell, sell, import, and otherwise transfer the Work,
78
+ where such license applies only to those patent claims licensable
79
+ by such Contributor that are necessarily infringed by their
80
+ Contribution(s) alone or by combination of their Contribution(s)
81
+ with the Work to which such Contribution(s) was submitted. If You
82
+ institute patent litigation against any entity (including a
83
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
84
+ or a Contribution incorporated within the Work constitutes direct
85
+ or contributory patent infringement, then any patent licenses
86
+ granted to You under this License for that Work shall terminate
87
+ as of the date such litigation is filed.
88
+
89
+ 4. Redistribution. You may reproduce and distribute copies of the
90
+ Work or Derivative Works thereof in any medium, with or without
91
+ modifications, and in Source or Object form, provided that You
92
+ meet the following conditions:
93
+
94
+ (a) You must give any other recipients of the Work or
95
+ Derivative Works a copy of this License; and
96
+
97
+ (b) You must cause any modified files to carry prominent notices
98
+ stating that You changed the files; and
99
+
100
+ (c) You must retain, in the Source form of any Derivative Works
101
+ that You distribute, all copyright, patent, trademark, and
102
+ attribution notices from the Source form of the Work,
103
+ excluding those notices that do not pertain to any part of
104
+ the Derivative Works; and
105
+
106
+ (d) If the Work includes a "NOTICE" text file as part of its
107
+ distribution, then any Derivative Works that You distribute must
108
+ include a readable copy of the attribution notices contained
109
+ within such NOTICE file, excluding those notices that do not
110
+ pertain to any part of the Derivative Works, in at least one
111
+ of the following places: within a NOTICE text file distributed
112
+ as part of the Derivative Works; within the Source form or
113
+ documentation, if provided along with the Derivative Works; or,
114
+ within a display generated by the Derivative Works, if and
115
+ wherever such third-party notices normally appear. The contents
116
+ of the NOTICE file are for informational purposes only and
117
+ do not modify the License. You may add Your own attribution
118
+ notices within Derivative Works that You distribute, alongside
119
+ or as an addendum to the NOTICE text from the Work, provided
120
+ that such additional attribution notices cannot be construed
121
+ as modifying the License.
122
+
123
+ You may add Your own copyright statement to Your modifications and
124
+ may provide additional or different license terms and conditions
125
+ for use, reproduction, or distribution of Your modifications, or
126
+ for any such Derivative Works as a whole, provided Your use,
127
+ reproduction, and distribution of the Work otherwise complies with
128
+ the conditions stated in this License.
129
+
130
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
131
+ any Contribution intentionally submitted for inclusion in the Work
132
+ by You to the Licensor shall be under the terms and conditions of
133
+ this License, without any additional terms or conditions.
134
+ Notwithstanding the above, nothing herein shall supersede or modify
135
+ the terms of any separate license agreement you may have executed
136
+ with Licensor regarding such Contributions.
137
+
138
+ 6. Trademarks. This License does not grant permission to use the trade
139
+ names, trademarks, service marks, or product names of the Licensor,
140
+ except as required for reasonable and customary use in describing the
141
+ origin of the Work and reproducing the content of the NOTICE file.
142
+
143
+ 7. Disclaimer of Warranty. Unless required by applicable law or
144
+ agreed to in writing, Licensor provides the Work (and each
145
+ Contributor provides its Contributions) on an "AS IS" BASIS,
146
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147
+ implied, including, without limitation, any warranties or conditions
148
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149
+ PARTICULAR PURPOSE. You are solely responsible for determining the
150
+ appropriateness of using or redistributing the Work and assume any
151
+ risks associated with Your exercise of permissions under this License.
152
+
153
+ 8. Limitation of Liability. In no event and under no legal theory,
154
+ whether in tort (including negligence), contract, or otherwise,
155
+ unless required by applicable law (such as deliberate and grossly
156
+ negligent acts) or agreed to in writing, shall any Contributor be
157
+ liable to You for damages, including any direct, indirect, special,
158
+ incidental, or consequential damages of any character arising as a
159
+ result of this License or out of the use or inability to use the
160
+ Work (including but not limited to damages for loss of goodwill,
161
+ work stoppage, computer failure or malfunction, or any and all
162
+ other commercial damages or losses), even if such Contributor
163
+ has been advised of the possibility of such damages.
164
+
165
+ 9. Accepting Warranty or Additional Liability. While redistributing
166
+ the Work or Derivative Works thereof, You may choose to offer,
167
+ and charge a fee for, acceptance of support, warranty, indemnity,
168
+ or other liability obligations and/or rights consistent with this
169
+ License. However, in accepting such obligations, You may act only
170
+ on Your own behalf and on Your sole responsibility, not on behalf
171
+ of any other Contributor, and only if You agree to indemnify,
172
+ defend, and hold each Contributor harmless for any liability
173
+ incurred by, or claims asserted against, such Contributor by reason
174
+ of your accepting any such warranty or additional liability.
175
+
176
+ END OF TERMS AND CONDITIONS
assets/pal_talos/assets/gripper/base_link_collision.stl ADDED
Binary file (18.3 kB). View file
 
assets/pal_talos/assets/gripper/fingertip.stl ADDED
Binary file (21.5 kB). View file
 
assets/pal_talos/assets/gripper/fingertip_collision.stl ADDED
Binary file (5.18 kB). View file
 
assets/pal_talos/assets/gripper/gripper_motor_double_collision.stl ADDED
Binary file (12.6 kB). View file
 
assets/pal_talos/assets/gripper/gripper_motor_single_collision.stl ADDED
Binary file (12.6 kB). View file
 
assets/pal_talos/assets/gripper/gripper_simple_collision.stl ADDED
Binary file (15.9 kB). View file
 
assets/pal_talos/assets/gripper/inner_double.stl ADDED
Binary file (54.3 kB). View file
 
assets/pal_talos/assets/gripper/inner_double_collision.stl ADDED
Binary file (9.08 kB). View file
 
assets/pal_talos/assets/gripper/inner_single.stl ADDED
Binary file (54.4 kB). View file
 
assets/pal_talos/assets/gripper/inner_single_collision.stl ADDED
Binary file (7.58 kB). View file
 
assets/pal_talos/assets/head/head_1_collision.stl ADDED
Binary file (12.9 kB). View file
 
assets/pal_talos/assets/head/head_2_default_collision.stl ADDED
Binary file (65.4 kB). View file
 
assets/pal_talos/assets/torso/torso_2_collision.stl ADDED
Binary file (34 kB). View file
 
assets/pal_talos/talos.xml ADDED
@@ -0,0 +1,508 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ <!-- derived file from https://github.com/google-deepmind/mujoco_menagerie/tree/main/pal_talos -->
2
+ <mujoco model="talos position scene">
3
+ <compiler angle="radian" meshdir="assets/"/>
4
+
5
+ <option timestep="0.001" integrator="implicitfast"/>
6
+
7
+ <size nkey="1"/>
8
+
9
+ <visual>
10
+ <global azimuth="160" elevation="-10"/>
11
+ <headlight ambient="0.3 0.3 0.3" diffuse="0.6 0.6 0.6" specular="0 0 0"/>
12
+ <rgba haze="0.15 0.25 0.35 1"/>
13
+ </visual>
14
+
15
+ <statistic extent="1.5" center="0 0 0.9"/>
16
+
17
+ <default>
18
+ <default class="talos">
19
+ <joint damping="1" frictionloss="1"/>
20
+ <geom type="mesh" solref="0.001"/>
21
+ <site size="0.01 0.005 0.005" group="5" rgba="1 0 0 1"/>
22
+ <default class="visual">
23
+ <geom contype="0" conaffinity="0" group="2" density="0"/>
24
+ </default>
25
+ <default class="collision">
26
+ <geom group="3"/>
27
+ </default>
28
+ </default>
29
+ <default class="position">
30
+ <general biastype="affine" biasprm="0 -1 1"/>
31
+ </default>
32
+ </default>
33
+
34
+ <asset>
35
+ <texture type="skybox" colorspace="auto" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
36
+ <texture type="2d" colorspace="auto" 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"/>
37
+ <material name="black" rgba="0.1 0.1 0.1 1"/>
38
+ <material name="orange" rgba="1 0.5 0 1"/>
39
+ <material name="white" rgba="0.9 0.9 0.9 1"/>
40
+ <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
41
+ <mesh name="torso_1" content_type="model/stl" file="torso/torso_1.stl"/>
42
+ <mesh name="torso_2" content_type="model/stl" file="torso/torso_2.stl"/>
43
+ <mesh name="torso_2_collision" content_type="model/stl" file="torso/torso_2_collision.stl"/>
44
+ <mesh name="base_link" content_type="model/stl" file="torso/base_link.stl"/>
45
+ <mesh name="base_link_collision" content_type="model/stl" file="torso/base_link_collision.stl"/>
46
+ <mesh name="head_1" content_type="model/stl" file="head/head_1.stl"/>
47
+ <mesh name="head_1_collision" content_type="model/stl" file="head/head_1_collision.stl"/>
48
+ <mesh name="head_2_default" content_type="model/stl" file="head/head_2_default.stl"/>
49
+ <mesh name="head_2_default_collision" content_type="model/stl" file="head/head_2_default_collision.stl"/>
50
+ <mesh name="orbbec" content_type="model/stl" file="sensors/orbbec/orbbec.stl"/>
51
+ <mesh name="l_arm_1" content_type="model/stl" file="arm/arm_1.stl"/>
52
+ <mesh name="l_arm_1c" content_type="model/stl" file="arm/arm_1_collision.stl"/>
53
+ <mesh name="l_arm_2" content_type="model/stl" file="arm/arm_2.stl"/>
54
+ <mesh name="l_arm_2c" content_type="model/stl" file="arm/arm_2_collision.stl"/>
55
+ <mesh name="l_arm_3" content_type="model/stl" file="arm/arm_3.stl"/>
56
+ <mesh name="l_arm_3c" content_type="model/stl" file="arm/arm_3_collision.stl"/>
57
+ <mesh name="l_arm_4" content_type="model/stl" file="arm/arm_4.stl"/>
58
+ <mesh name="l_arm_4c" content_type="model/stl" file="arm/arm_4_collision.stl"/>
59
+ <mesh name="l_arm_5" content_type="model/stl" file="arm/arm_5.stl"/>
60
+ <mesh name="l_arm_5c" content_type="model/stl" file="arm/arm_5_collision.stl"/>
61
+ <mesh name="l_arm_6" content_type="model/stl" file="arm/arm_6.stl"/>
62
+ <mesh name="l_arm_6c" content_type="model/stl" file="arm/arm_6_collision.stl"/>
63
+ <mesh name="l_arm_7" content_type="model/stl" file="arm/arm_7.stl"/>
64
+ <mesh name="l_arm_7c" content_type="model/stl" file="arm/arm_7_collision.stl"/>
65
+ <mesh name="r_arm_1" content_type="model/stl" file="arm/arm_1.stl" scale="1 -1 1"/>
66
+ <mesh name="r_arm_1c" content_type="model/stl" file="arm/arm_1_collision.stl" scale="1 -1 1"/>
67
+ <mesh name="r_arm_2" content_type="model/stl" file="arm/arm_2.stl" scale="1 -1 1"/>
68
+ <mesh name="r_arm_2c" content_type="model/stl" file="arm/arm_2_collision.stl" scale="1 -1 1"/>
69
+ <mesh name="r_arm_3" content_type="model/stl" file="arm/arm_3.stl" scale="1 -1 1"/>
70
+ <mesh name="r_arm_3c" content_type="model/stl" file="arm/arm_3_collision.stl" scale="1 -1 1"/>
71
+ <mesh name="r_arm_4" content_type="model/stl" file="arm/arm_4.stl" scale="1 -1 1"/>
72
+ <mesh name="r_arm_4c" content_type="model/stl" file="arm/arm_4_collision.stl" scale="1 -1 1"/>
73
+ <mesh name="r_arm_5" content_type="model/stl" file="arm/arm_5.stl" scale="1 -1 1"/>
74
+ <mesh name="r_arm_5c" content_type="model/stl" file="arm/arm_5_collision.stl" scale="1 -1 1"/>
75
+ <mesh name="r_arm_6" content_type="model/stl" file="arm/arm_6.stl" scale="1 -1 1"/>
76
+ <mesh name="r_arm_6c" content_type="model/stl" file="arm/arm_6_collision.stl" scale="1 -1 1"/>
77
+ <mesh name="r_arm_7" content_type="model/stl" file="arm/arm_7.stl" scale="1 -1 1"/>
78
+ <mesh name="r_arm_7c" content_type="model/stl" file="arm/arm_7_collision.stl" scale="1 -1 1"/>
79
+ <!-- <mesh name="gripper_base_link" content_type="model/stl" file="gripper/base_link.stl"/>
80
+ <mesh name="gripper_base_link_collision" content_type="model/stl" file="gripper/base_link_collision.stl"/>
81
+ <mesh name="fingertip" content_type="model/stl" file="gripper/fingertip.stl"/>
82
+ <mesh name="fingertip_collision" content_type="model/stl" file="gripper/fingertip_collision.stl"/>
83
+ <mesh name="gripper_motor_double" content_type="model/stl" file="gripper/gripper_motor_double.stl"/>
84
+ <mesh name="gripper_motor_double_collision" content_type="model/stl" file="gripper/gripper_motor_double_collision.stl"/>
85
+ <mesh name="gripper_motor_single" content_type="model/stl" file="gripper/gripper_motor_single.stl"/>
86
+ <mesh name="gripper_motor_single_collision" content_type="model/stl" file="gripper/gripper_motor_single_collision.stl"/>
87
+ <mesh name="inner_double" content_type="model/stl" file="gripper/inner_double.stl"/>
88
+ <mesh name="inner_double_collision" content_type="model/stl" file="gripper/inner_double_collision.stl"/>
89
+ <mesh name="inner_single" content_type="model/stl" file="gripper/inner_single.stl"/>
90
+ <mesh name="inner_single_collision" content_type="model/stl" file="gripper/inner_single_collision.stl"/> -->
91
+ <mesh name="l_hip_z_lo_res" content_type="model/stl" file="v2/hip_z_lo_res.stl"/>
92
+ <mesh name="l_hip_z_collision" content_type="model/stl" file="v2/hip_z_collision.stl"/>
93
+ <mesh name="l_hip_x_lo_res" content_type="model/stl" file="v2/hip_x_lo_res.stl"/>
94
+ <mesh name="l_hip_x_collision" content_type="model/stl" file="v2/hip_x_collision.stl"/>
95
+ <mesh name="l_hip_y_lo_res" content_type="model/stl" file="v2/hip_y_lo_res.stl"/>
96
+ <mesh name="l_hip_y_collision" content_type="model/stl" file="v2/hip_y_collision.stl"/>
97
+ <mesh name="l_knee_lo_res" content_type="model/stl" file="v2/knee_lo_res.stl"/>
98
+ <mesh name="l_knee_collision" content_type="model/stl" file="v2/knee_collision.stl"/>
99
+ <mesh name="l_ankle_Y_lo_res" content_type="model/stl" file="v2/ankle_Y_lo_res.stl"/>
100
+ <mesh name="l_ankle_Y_collision" content_type="model/stl" file="v2/ankle_Y_collision.stl"/>
101
+ <mesh name="l_ankle_X_lo_res" content_type="model/stl" file="v2/ankle_X_lo_res.stl"/>
102
+ <mesh name="l_ankle_X_collision" content_type="model/stl" file="v2/ankle_X_collision.stl" scale="1 1 0.88"/>
103
+ <mesh name="r_hip_z_lo_res" content_type="model/stl" file="v2/hip_z_lo_res.stl" scale="1 -1 1"/>
104
+ <mesh name="r_hip_z_collision" content_type="model/stl" file="v2/hip_z_collision.stl" scale="1 -1 1"/>
105
+ <mesh name="r_hip_x_lo_res" content_type="model/stl" file="v2/hip_x_lo_res.stl" scale="1 -1 1"/>
106
+ <mesh name="r_hip_x_collision" content_type="model/stl" file="v2/hip_x_collision.stl" scale="1 -1 1"/>
107
+ <mesh name="r_hip_y_lo_res" content_type="model/stl" file="v2/hip_y_lo_res.stl" scale="1 -1 1"/>
108
+ <mesh name="r_hip_y_collision" content_type="model/stl" file="v2/hip_y_collision.stl" scale="1 -1 1"/>
109
+ <mesh name="r_knee_lo_res" content_type="model/stl" file="v2/knee_lo_res.stl" scale="1 -1 1"/>
110
+ <mesh name="r_knee_collision" content_type="model/stl" file="v2/knee_collision.stl" scale="1 -1 1"/>
111
+ <mesh name="r_ankle_Y_lo_res" content_type="model/stl" file="v2/ankle_Y_lo_res.stl" scale="1 -1 1"/>
112
+ <mesh name="r_ankle_Y_collision" content_type="model/stl" file="v2/ankle_Y_collision.stl" scale="1 -1 1"/>
113
+ <mesh name="r_ankle_X_lo_res" content_type="model/stl" file="v2/ankle_X_lo_res.stl" scale="1 -1 1"/>
114
+ <mesh name="r_ankle_X_collision" content_type="model/stl" file="v2/ankle_X_collision.stl" scale="1 -1 0.88"/>
115
+ </asset>
116
+
117
+ <worldbody>
118
+ <geom name="floor" size="0 0 0.05" type="plane" material="groundplane"/>
119
+ <light name="spotlight" target="base_link" pos="1 0 20" dir="0 0 -1" mode="targetbody"/>
120
+ <body name="base_link" childclass="talos" pos="0 0 1.1">
121
+ <inertial pos="-0.0570942 0.00153054 -0.0762521" quat="0.528705 0.532893 -0.46798 0.466359" mass="15.3628" diaginertia="0.23241 0.200537 0.0841093"/>
122
+ <joint name="reference" type="free" damping="0" frictionloss="0"/>
123
+ <geom class="visual" material="white" mesh="base_link"/>
124
+ <geom class="collision" material="white" mesh="base_link_collision"/>
125
+ <body name="torso_1_link" pos="0 0 0.0722">
126
+ <inertial pos="0.00078223 3.528e-05 -0.0178246" quat="0.703744 0.704233 0.0663452 -0.0663278" mass="2.29466" diaginertia="0.00639091 0.00622386 0.00410256"/>
127
+ <joint name="torso_1_joint" pos="0 0 0" axis="0 0 1" range="-1.25664 1.25664" actuatorfrcrange="-200 200" frictionloss="2"/>
128
+ <geom class="visual" material="white" mesh="torso_1"/>
129
+ <geom class="collision" material="white" mesh="torso_1"/>
130
+ <body name="torso_2_link">
131
+ <inertial pos="-0.0463 -0.000989647 0.145241" quat="0.704021 0.708473 0.0363665 -0.0331367" mass="16.984" diaginertia="0.444971 0.320913 0.299991"/>
132
+ <joint name="torso_2_joint" pos="0 0 0" axis="0 1 0" range="-0.226893 0.733038" actuatorfrcrange="-200 200" frictionloss="2"/>
133
+ <geom class="visual" material="white" mesh="torso_2"/>
134
+ <geom class="collision" material="white" mesh="torso_2_collision"/>
135
+ <geom class="visual" size="0.005 0.005 0.005" pos="0.04925 0 0.078" quat="0 0.707107 0.707107 0" type="box" material="black"/>
136
+ <body name="head_1_link" pos="0 0 0.316">
137
+ <inertial pos="-0.00157211 -0.00157919 0.0217577" quat="0.679576 0.71147 -0.126684 0.126246" mass="0.73746" diaginertia="0.0022809 0.00202202 0.00110971"/>
138
+ <joint name="head_1_joint" pos="0 0 0" axis="0 1 0" range="-0.20944 0.785398" actuatorfrcrange="-8 8" damping="0.5"/>
139
+ <geom class="visual" material="black" mesh="head_1"/>
140
+ <geom class="collision" material="black" mesh="head_1_collision"/>
141
+ <body name="head_2_link">
142
+ <inertial pos="0.0108233 0.000630364 0.142045" quat="0.584927 -0.0294601 -0.0201927 0.810299" mass="1.46795" diaginertia="0.0110721 0.0109715 0.00581517"/>
143
+ <joint name="head_2_joint" pos="0 0 0" axis="0 0 1" range="-1.309 1.309" actuatorfrcrange="-4 4" damping="0.5"/>
144
+ <geom class="visual" material="white" mesh="head_2_default"/>
145
+ <geom class="collision" material="white" mesh="head_2_default_collision"/>
146
+ <geom class="visual" pos="0.0621 0.0375 0.1832" quat="1 0 0 0" material="black" mesh="orbbec"/>
147
+ <site name="head" pos="0 0 0.2"/>
148
+ </body>
149
+ </body>
150
+ <body name="arm_left_1_link" pos="0 0.1575 0.232">
151
+ <inertial pos="-0.0002762 0.100602 0.0443742" quat="0.509187 0.520076 -0.492016 0.477671" mass="2.71457" diaginertia="0.0135919 0.012374 0.00418528"/>
152
+ <joint name="arm_left_1_joint" pos="0 0 0" axis="0 0 1" range="-1.5708 0.785398" actuatorfrcrange="-100 100"/>
153
+ <geom class="visual" material="black" mesh="l_arm_1"/>
154
+ <geom class="collision" material="black" mesh="l_arm_1c"/>
155
+ <body name="arm_left_2_link" pos="0.00493 0.1365 0.04673">
156
+ <inertial pos="0.0143883 0.00092938 -0.0868427" quat="0.981914 0.00964503 0.164999 0.0923476" mass="2.42509" diaginertia="0.0140925 0.0138001 0.00368288"/>
157
+ <joint name="arm_left_2_joint" pos="0 0 0" axis="1 0 0" range="-0.001 2.87107" actuatorfrcrange="-100 100"/>
158
+ <geom class="visual" material="black" mesh="l_arm_2"/>
159
+ <geom class="collision" material="black" mesh="l_arm_2c"/>
160
+ <body name="arm_left_3_link">
161
+ <inertial pos="0.0136084 0.0124162 -0.2499" quat="0.991718 0.0691072 -0.105592 -0.0238848" mass="2.20874" diaginertia="0.00735601 0.00699198 0.0036592"/>
162
+ <joint name="arm_left_3_joint" pos="0 0 0" axis="0 0 1" range="-2.42601 2.42601" actuatorfrcrange="-70 70"/>
163
+ <geom class="visual" material="black" mesh="l_arm_3"/>
164
+ <geom class="collision" material="black" mesh="l_arm_3c"/>
165
+ <body name="arm_left_4_link" pos="0.02 0 -0.2782">
166
+ <inertial pos="-0.00810404 -0.0194433 -0.0332924" quat="0.428586 -0.11894 0.057285 0.893804" mass="0.815675" diaginertia="0.00326211 0.002924 0.00111543"/>
167
+ <joint name="arm_left_4_joint" pos="0 0 0" axis="0 1 0" range="-2.23402 0.001" actuatorfrcrange="-70 70"/>
168
+ <geom class="visual" material="black" mesh="l_arm_4"/>
169
+ <geom class="collision" material="black" mesh="l_arm_4c"/>
170
+ <body name="arm_left_5_link" pos="-0.02 0 -0.2643">
171
+ <inertial pos="-6e-05 0.003262 0.079625" quat="0.711712 -0.019748 0.0131536 0.70207" mass="1.87792" diaginertia="0.004373 0.003495 0.002278"/>
172
+ <joint name="arm_left_5_joint" pos="0 0 0" axis="0 0 1" range="-2.51327 2.51327" actuatorfrcrange="-20 20"/>
173
+ <geom class="visual" material="white" mesh="l_arm_5"/>
174
+ <geom class="collision" material="white" mesh="l_arm_5c"/>
175
+ <body name="arm_left_6_link">
176
+ <inertial pos="2.1e-05 -0.001965 -0.000591" quat="0.0104356 0.706678 0.0122672 0.707352" mass="0.40931" diaginertia="0.000154 0.000141 0.000107"/>
177
+ <joint name="arm_left_6_joint" pos="0 0 0" axis="1 0 0" range="-1.37008 1.37008" actuatorfrcrange="-8 8"/>
178
+ <geom class="visual" material="white" mesh="l_arm_6"/>
179
+ <geom class="collision" material="white" mesh="l_arm_6c"/>
180
+ <body name="arm_left_7_link">
181
+ <inertial pos="0.00215393 0.00361697 -0.0837948" quat="0.966304 0.0192407 0.0500561 -0.251755" mass="1.04198" diaginertia="0.00273016 0.00272405 0.0010495"/>
182
+ <joint name="arm_left_7_joint" pos="0 0 0" axis="0 1 0" range="-0.680678 0.680678" actuatorfrcrange="-8 8"/>
183
+ <geom class="visual" material="black" mesh="l_arm_7"/>
184
+ <geom class="collision" material="black" mesh="l_arm_7c"/>
185
+ <geom class="visual" size="0.0225 0.00785" pos="0 0 -0.051" quat="-0.707107 0 0 -0.707107" type="cylinder" material="white"/>
186
+ <geom class="collision" size="0.0225 0.00785" pos="0 0 -0.051" quat="-0.707107 0 0 -0.707107" type="cylinder" material="white"/>
187
+ <geom class="visual" size="0.025 0.004875" pos="0 0 -0.063725" type="cylinder" material="black"/>
188
+ <geom class="collision" size="0.025 0.004875" pos="0 0 -0.063725" type="cylinder" material="black"/>
189
+ <!-- <geom class="visual" pos="0 0 -0.092475" quat="1 0 0 0" material="black" mesh="gripper_base_link"/>
190
+ <geom class="collision" pos="0 0 -0.092475" quat="1 0 0 0" material="black" mesh="gripper_base_link_collision"/>
191
+ <body name="gripper_left_motor_double_link" pos="0 0.02025 -0.122475">
192
+ <inertial pos="0.019654 0.018572 -0.011998" quat="0.434706 0.588708 -0.0217956 0.68116" mass="0.134356" diaginertia="0.000272 0.000221 8e-05"/>
193
+ <joint name="gripper_left_joint" pos="0 0 0" axis="1 0 0" range="-0.959931 0"/>
194
+ <geom class="visual" material="orange" mesh="gripper_motor_double"/>
195
+ <geom class="collision" material="orange" mesh="gripper_motor_double_collision"/>
196
+ </body>
197
+ <body name="gripper_left_inner_double_link" pos="0 0.00525 -0.148455">
198
+ <inertial pos="-0.013283 0.036852 -0.023153" quat="0.312568 0.633813 0.365244 0.605953" mass="0.087986" diaginertia="0.000144 8.89999e-05 7.4e-05"/>
199
+ <joint name="gripper_left_inner_double_joint" pos="0 0 0" axis="1 0 0" range="-1.0472 0"/>
200
+ <geom class="visual" material="orange" mesh="inner_double"/>
201
+ <geom class="collision" material="orange" mesh="inner_double_collision"/>
202
+ <body name="gripper_left_fingertip_1_link" pos="0.032 0.04589 -0.06553">
203
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
204
+ <joint name="gripper_left_fingertip_1_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
205
+ <geom class="visual" material="black" mesh="fingertip"/>
206
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
207
+ </body>
208
+ <body name="gripper_left_fingertip_2_link" pos="-0.032 0.04589 -0.06553">
209
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
210
+ <joint name="gripper_left_fingertip_2_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
211
+ <geom class="visual" material="black" mesh="fingertip"/>
212
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
213
+ </body>
214
+ </body>
215
+ <body name="gripper_left_motor_single_link" pos="0 -0.02025 -0.122475">
216
+ <inertial pos="0.025237 -0.011231 -0.008158" quat="-0.429079 0.588757 0.0530926 0.682962" mass="0.107923" diaginertia="0.000171 0.000142 5.1e-05"/>
217
+ <joint name="gripper_left_motor_single_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
218
+ <geom class="visual" material="orange" mesh="gripper_motor_single"/>
219
+ <geom class="collision" material="orange" mesh="gripper_motor_single_collision"/>
220
+ </body>
221
+ <body name="gripper_left_inner_single_link" pos="0 -0.00525 -0.148455">
222
+ <inertial pos="0 -0.034565 -0.021412" quat="0.672196 -0.216723 0.212111 0.67542" mass="0.047177" diaginertia="4.3e-05 4.2e-05 1.1e-05"/>
223
+ <joint name="gripper_left_inner_single_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
224
+ <geom class="visual" material="orange" mesh="inner_single"/>
225
+ <geom class="collision" material="orange" mesh="inner_single_collision"/>
226
+ <body name="gripper_left_fingertip_3_link" pos="0 -0.04589 -0.06553" quat="0 0 0 1">
227
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
228
+ <joint name="gripper_left_fingertip_3_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
229
+ <geom class="visual" material="black" mesh="fingertip"/>
230
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
231
+ <site name="left_palm" pos="0 -0.05 0"/>
232
+ </body>
233
+ </body> -->
234
+ </body>
235
+ </body>
236
+ </body>
237
+ </body>
238
+ </body>
239
+ </body>
240
+ </body>
241
+ <body name="arm_right_1_link" pos="0 -0.1575 0.232">
242
+ <inertial pos="-0.0002762 -0.100602 0.0443742" quat="0.520076 0.509187 -0.477671 0.492016" mass="2.71457" diaginertia="0.0135919 0.012374 0.00418528"/>
243
+ <joint name="arm_right_1_joint" pos="0 0 0" axis="0 0 1" range="-0.785398 1.5708" actuatorfrcrange="-100 100"/>
244
+ <geom class="visual" material="black" mesh="r_arm_1"/>
245
+ <geom class="collision" material="black" mesh="r_arm_1c"/>
246
+ <body name="arm_right_2_link" pos="0.00493 -0.1365 0.04673">
247
+ <inertial pos="0.0143883 -0.00092938 -0.0868427" quat="0.981914 -0.00964503 0.164999 -0.0923476" mass="2.42509" diaginertia="0.0140925 0.0138001 0.00368288"/>
248
+ <joint name="arm_right_2_joint" pos="0 0 0" axis="1 0 0" range="-2.87107 0.001" actuatorfrcrange="-100 100"/>
249
+ <geom class="visual" material="black" mesh="r_arm_2"/>
250
+ <geom class="collision" material="black" mesh="r_arm_2c"/>
251
+ <body name="arm_right_3_link">
252
+ <inertial pos="0.0136084 -0.0124162 -0.2499" quat="0.991718 -0.0691072 -0.105592 0.0238848" mass="2.20874" diaginertia="0.00735601 0.00699198 0.0036592"/>
253
+ <joint name="arm_right_3_joint" pos="0 0 0" axis="0 0 1" range="-2.42601 2.42601" actuatorfrcrange="-70 70"/>
254
+ <geom class="visual" material="black" mesh="r_arm_3"/>
255
+ <geom class="collision" material="black" mesh="r_arm_3c"/>
256
+ <body name="arm_right_4_link" pos="0.02 0 -0.2782">
257
+ <inertial pos="-0.00810404 -0.0194433 -0.0332924" quat="0.428586 -0.11894 0.057285 0.893804" mass="0.815675" diaginertia="0.00326211 0.002924 0.00111543"/>
258
+ <joint name="arm_right_4_joint" pos="0 0 0" axis="0 1 0" range="-2.23402 0.001" actuatorfrcrange="-70 70"/>
259
+ <geom class="visual" material="black" mesh="r_arm_4"/>
260
+ <geom class="collision" material="black" mesh="r_arm_4c"/>
261
+ <body name="arm_right_5_link" pos="-0.02 0 -0.2643">
262
+ <inertial pos="-6e-05 -0.003262 0.079625" quat="0.70207 0.0131536 -0.019748 0.711712" mass="1.87792" diaginertia="0.004373 0.003495 0.002278"/>
263
+ <joint name="arm_right_5_joint" pos="0 0 0" axis="0 0 1" range="-2.51327 2.51327" actuatorfrcrange="-20 20"/>
264
+ <geom class="visual" material="white" mesh="r_arm_5"/>
265
+ <geom class="collision" material="white" mesh="r_arm_5c"/>
266
+ <body name="arm_right_6_link">
267
+ <inertial pos="2.1e-05 0.001965 -0.000591" quat="-0.0104356 0.706678 -0.0122672 0.707352" mass="0.40931" diaginertia="0.000154 0.000141 0.000107"/>
268
+ <joint name="arm_right_6_joint" pos="0 0 0" axis="1 0 0" range="-1.37008 1.37008" actuatorfrcrange="-8 8"/>
269
+ <geom class="visual" material="white" mesh="r_arm_6"/>
270
+ <geom class="collision" material="white" mesh="r_arm_6c"/>
271
+ <body name="arm_right_7_link">
272
+ <inertial pos="0.0023011 -0.00361697 -0.0837948" quat="0.927123 -0.0149278 0.0465372 0.371557" mass="1.04198" diaginertia="0.00273231 0.00271796 0.00105208"/>
273
+ <joint name="arm_right_7_joint" pos="0 0 0" axis="0 1 0" range="-0.680678 0.680678" actuatorfrcrange="-8 8"/>
274
+ <geom class="visual" material="black" mesh="r_arm_7"/>
275
+ <geom class="collision" material="black" mesh="r_arm_7c"/>
276
+ <geom class="visual" size="0.0225 0.00785" pos="0 0 -0.051" quat="0.707107 0 0 -0.707107" type="cylinder" material="white"/>
277
+ <geom class="collision" size="0.0225 0.00785" pos="0 0 -0.051" quat="0.707107 0 0 -0.707107" type="cylinder" material="white"/>
278
+ <geom class="visual" size="0.025 0.004875" pos="0 0 -0.063725" quat="0 0 0 -1" type="cylinder" material="black"/>
279
+ <geom class="collision" size="0.025 0.004875" pos="0 0 -0.063725" quat="0 0 0 -1" type="cylinder" material="black"/>
280
+ <!-- <geom class="visual" pos="0 0 -0.092475" quat="0 0 0 -1" material="black" mesh="gripper_base_link"/>
281
+ <geom class="collision" pos="0 0 -0.092475" quat="0 0 0 -1" material="black" mesh="gripper_base_link_collision"/>
282
+ <body name="gripper_right_motor_double_link" pos="0 -0.02025 -0.122475" quat="0 0 0 -1">
283
+ <inertial pos="0.019654 0.018572 -0.011998" quat="0.434706 0.588708 -0.0217956 0.68116" mass="0.134356" diaginertia="0.000272 0.000221 8e-05"/>
284
+ <joint name="gripper_right_joint" pos="0 0 0" axis="1 0 0" range="-0.959931 0"/>
285
+ <geom class="visual" material="orange" mesh="gripper_motor_double"/>
286
+ <geom class="collision" material="orange" mesh="gripper_motor_double_collision"/>
287
+ </body>
288
+ <body name="gripper_right_inner_double_link" pos="0 -0.00525 -0.148455" quat="0 0 0 -1">
289
+ <inertial pos="-0.013283 0.036852 -0.023153" quat="0.312568 0.633813 0.365244 0.605953" mass="0.087986" diaginertia="0.000144 8.89999e-05 7.4e-05"/>
290
+ <joint name="gripper_right_inner_double_joint" pos="0 0 0" axis="1 0 0" range="-1.0472 0"/>
291
+ <geom class="visual" material="orange" mesh="inner_double"/>
292
+ <geom class="collision" material="orange" mesh="inner_double_collision"/>
293
+ <body name="gripper_right_fingertip_1_link" pos="0.032 0.04589 -0.06553">
294
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
295
+ <joint name="gripper_right_fingertip_1_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
296
+ <geom class="visual" material="black" mesh="fingertip"/>
297
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
298
+ </body>
299
+ <body name="gripper_right_fingertip_2_link" pos="-0.032 0.04589 -0.06553">
300
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
301
+ <joint name="gripper_right_fingertip_2_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
302
+ <geom class="visual" material="black" mesh="fingertip"/>
303
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
304
+ <site name="right_palm" pos="0 -0.05 0"/>
305
+ </body>
306
+ </body>
307
+ <body name="gripper_right_motor_single_link" pos="0 0.02025 -0.122475" quat="0 0 0 -1">
308
+ <inertial pos="0.025237 -0.011231 -0.008158" quat="-0.429079 0.588757 0.0530926 0.682962" mass="0.107923" diaginertia="0.000171 0.000142 5.1e-05"/>
309
+ <joint name="gripper_right_motor_single_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
310
+ <geom class="visual" material="orange" mesh="gripper_motor_single"/>
311
+ <geom class="collision" material="orange" mesh="gripper_motor_single_collision"/>
312
+ </body>
313
+ <body name="gripper_right_inner_single_link" pos="0 0.00525 -0.148455" quat="0 0 0 -1">
314
+ <inertial pos="0 -0.034565 -0.021412" quat="0.672196 -0.216723 0.212111 0.67542" mass="0.047177" diaginertia="4.3e-05 4.2e-05 1.1e-05"/>
315
+ <joint name="gripper_right_inner_single_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
316
+ <geom class="visual" material="orange" mesh="inner_single"/>
317
+ <geom class="collision" material="orange" mesh="inner_single_collision"/>
318
+ <body name="gripper_right_fingertip_3_link" pos="0 -0.04589 -0.06553" quat="0 0 0 1">
319
+ <inertial pos="0 0.004604 -0.002537" quat="0.70316 -0.074603 0.074603 0.70316" mass="0.026301" diaginertia="8.99999e-06 8e-06 2e-06"/>
320
+ <joint name="gripper_right_fingertip_3_joint" pos="0 0 0" axis="1 0 0" range="0 1.0472"/>
321
+ <geom class="visual" material="black" mesh="fingertip"/>
322
+ <geom class="collision" material="black" mesh="fingertip_collision"/>
323
+ </body>
324
+ </body> -->
325
+ </body>
326
+ </body>
327
+ </body>
328
+ </body>
329
+ </body>
330
+ </body>
331
+ </body>
332
+ </body>
333
+ </body>
334
+ <body name="leg_left_1_link" pos="-0.02 0.085 -0.27105">
335
+ <inertial pos="0.0847284 0.109974 0.017059" quat="0.522748 -0.852371 -0.00673577 0.012351" mass="2.23501" diaginertia="0.0105013 0.0076913 0.00473271"/>
336
+ <joint name="leg_left_1_joint" pos="0 0 0" axis="0 0 1" range="-0.349066 1.5708" actuatorfrcrange="-100 100"/>
337
+ <geom class="visual" material="black" mesh="l_hip_z_lo_res"/>
338
+ <geom class="collision" material="black" mesh="l_hip_z_collision"/>
339
+ <body name="leg_left_2_link">
340
+ <inertial pos="-0.00704703 0.0259266 0.00273385" quat="0.712294 0.701399 -0.00472331 -0.0255805" mass="1.49095" diaginertia="0.00497732 0.00474343 0.00189918"/>
341
+ <joint name="leg_left_2_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 0.5236" actuatorfrcrange="-160 160"/>
342
+ <geom class="visual" material="black" mesh="l_hip_x_lo_res"/>
343
+ <geom class="collision" material="black" mesh="l_hip_x_collision"/>
344
+ <body name="leg_left_3_link">
345
+ <inertial pos="0.0058523 0.0636967 -0.183396" quat="0.995121 -0.0974306 -0.00559068 0.0145098" mass="6.23987" diaginertia="0.153409 0.140791 0.0256712"/>
346
+ <joint name="leg_left_3_joint" pos="0 0 0" axis="0 1 0" range="-2.095 0.7" actuatorfrcrange="-160 160"/>
347
+ <geom class="visual" material="black" mesh="l_hip_y_lo_res"/>
348
+ <geom class="collision" material="black" mesh="l_hip_y_collision"/>
349
+ <body name="leg_left_4_link" pos="0 0 -0.38">
350
+ <inertial pos="0.0131772 0.0291751 -0.115946" quat="0.999357 0.00773677 -0.0135107 0.0322952" mass="3.75995" diaginertia="0.0429266 0.0359662 0.013621"/>
351
+ <joint name="leg_left_4_joint" pos="0 0 0" axis="0 1 0" range="0 2.618" actuatorfrcrange="-400 400"/>
352
+ <geom class="visual" material="black" mesh="l_knee_lo_res"/>
353
+ <geom class="collision" material="black" mesh="l_knee_collision"/>
354
+ <body name="leg_left_5_link" pos="0 0 -0.325">
355
+ <inertial pos="-0.0140084 0.0418006 0.0382019" quat="0.94386 -0.249123 0.00319491 0.216924" mass="1.29096" diaginertia="0.0118206 0.010046 0.00352112"/>
356
+ <joint name="leg_left_5_joint" pos="0 0 0" axis="0 1 0" range="-1.27 0.68" actuatorfrcrange="-160 160"/>
357
+ <geom class="visual" rgba="0.7 0.7 0.7 1" mesh="l_ankle_Y_lo_res"/>
358
+ <geom class="collision" rgba="0.7 0.7 0.7 1" mesh="l_ankle_Y_collision"/>
359
+ <body name="leg_left_6_link">
360
+ <inertial pos="-0.0201699 -0.000510665 -0.0602836" quat="0.343485 0.610901 0.618186 0.3559" mass="1.61177" diaginertia="0.00773279 0.006503 0.00387932"/>
361
+ <joint name="leg_left_6_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 0.5236" actuatorfrcrange="-100 100"/>
362
+ <geom class="visual" rgba="0.7 0.7 0.7 1" mesh="l_ankle_X_lo_res"/>
363
+ <geom class="collision" size="0.1 0.06 0.006" pos="-0.005 0 -0.1" type="box" contype="6"/>
364
+ <site name="left_foot" pos="0 0 -0.1"/>
365
+ </body>
366
+ </body>
367
+ </body>
368
+ </body>
369
+ </body>
370
+ </body>
371
+ <body name="leg_right_1_link" pos="-0.02 -0.085 -0.27105">
372
+ <inertial pos="0.0847284 -0.109974 0.017059" quat="0.522748 0.852371 -0.00673577 0.012351" mass="2.23501" diaginertia="0.0105013 0.0076913 0.00473271"/>
373
+ <joint name="leg_right_1_joint" pos="0 0 0" axis="0 0 1" range="-1.5708 0.349066" actuatorfrcrange="-100 100"/>
374
+ <geom class="visual" material="black" mesh="r_hip_z_lo_res"/>
375
+ <geom class="collision" material="black" mesh="r_hip_z_collision"/>
376
+ <body name="leg_right_2_link">
377
+ <inertial pos="-0.00704703 -0.0259266 0.00273385" quat="0.701399 0.712294 0.0255805 0.00472331" mass="1.49095" diaginertia="0.00497732 0.00474343 0.00189918"/>
378
+ <joint name="leg_right_2_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 0.5236" actuatorfrcrange="-160 160"/>
379
+ <geom class="visual" material="black" mesh="r_hip_x_lo_res"/>
380
+ <geom class="collision" material="black" mesh="r_hip_x_collision"/>
381
+ <body name="leg_right_3_link">
382
+ <inertial pos="0.0058523 -0.0636967 -0.183396" quat="0.995121 0.0974306 -0.00559068 -0.0145098" mass="6.23987" diaginertia="0.153409 0.140791 0.0256712"/>
383
+ <joint name="leg_right_3_joint" pos="0 0 0" axis="0 1 0" range="-2.095 0.7" actuatorfrcrange="-160 160"/>
384
+ <geom class="visual" material="black" mesh="r_hip_y_lo_res"/>
385
+ <geom class="collision" material="black" mesh="r_hip_y_collision"/>
386
+ <body name="leg_right_4_link" pos="0 0 -0.38">
387
+ <inertial pos="0.0131772 -0.0291751 -0.115946" quat="0.999357 -0.00773677 -0.0135107 -0.0322952" mass="3.75995" diaginertia="0.0429266 0.0359662 0.013621"/>
388
+ <joint name="leg_right_4_joint" pos="0 0 0" axis="0 1 0" range="0 2.618" actuatorfrcrange="-400 400"/>
389
+ <geom class="visual" material="black" mesh="r_knee_lo_res"/>
390
+ <geom class="collision" material="black" mesh="r_knee_collision"/>
391
+ <body name="leg_right_5_link" pos="0 0 -0.325">
392
+ <inertial pos="-0.0140084 -0.0418006 0.0382019" quat="0.94386 0.249123 0.00319491 -0.216924" mass="1.29096" diaginertia="0.0118206 0.010046 0.00352112"/>
393
+ <joint name="leg_right_5_joint" pos="0 0 0" axis="0 1 0" range="-1.27 0.68" actuatorfrcrange="-160 160"/>
394
+ <geom class="visual" rgba="0.7 0.7 0.7 1" mesh="r_ankle_Y_lo_res"/>
395
+ <geom class="collision" rgba="0.7 0.7 0.7 1" mesh="r_ankle_Y_collision"/>
396
+ <body name="leg_right_6_link">
397
+ <inertial pos="-0.0201699 0.000510665 -0.0602836" quat="0.3559 0.618186 0.610901 0.343485" mass="1.61177" diaginertia="0.00773279 0.006503 0.00387932"/>
398
+ <joint name="leg_right_6_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 0.5236" actuatorfrcrange="-100 100"/>
399
+ <geom class="visual" rgba="0.7 0.7 0.7 1" mesh="r_ankle_X_lo_res"/>
400
+ <geom class="collision" size="0.1 0.06 0.006" pos="-0.005 0 -0.1" type="box" contype="6"/>
401
+ <site name="right_foot" pos="0 0 -0.1"/>
402
+ </body>
403
+ </body>
404
+ </body>
405
+ </body>
406
+ </body>
407
+ </body>
408
+ </body>
409
+ </worldbody>
410
+
411
+ <contact>
412
+ <exclude body1="base_link" body2="torso_2_link"/>
413
+ <exclude body1="base_link" body2="leg_left_3_link"/>
414
+ <exclude body1="base_link" body2="leg_right_3_link"/>
415
+ <exclude body1="arm_left_4_link" body2="arm_left_7_link"/>
416
+ <exclude body1="arm_left_5_link" body2="arm_left_7_link"/>
417
+ <exclude body1="arm_left_6_link" body2="arm_left_7_link"/>
418
+ <!-- <exclude body1="arm_left_7_link" body2="gripper_left_motor_double_link"/>
419
+ <exclude body1="arm_left_7_link" body2="gripper_left_inner_double_link"/>
420
+ <exclude body1="arm_left_7_link" body2="gripper_left_motor_single_link"/>
421
+ <exclude body1="arm_left_7_link" body2="gripper_left_inner_single_link"/>
422
+ <exclude body1="gripper_left_motor_double_link" body2="gripper_left_inner_double_link"/>
423
+ <exclude body1="gripper_left_motor_double_link" body2="gripper_left_fingertip_2_link"/>
424
+ <exclude body1="gripper_left_motor_double_link" body2="gripper_left_motor_single_link"/>
425
+ <exclude body1="gripper_left_motor_double_link" body2="gripper_left_inner_single_link"/>
426
+ <exclude body1="gripper_left_inner_double_link" body2="gripper_left_motor_single_link"/>
427
+ <exclude body1="gripper_left_fingertip_1_link" body2="gripper_left_fingertip_2_link"/>
428
+ <exclude body1="gripper_left_fingertip_1_link" body2="gripper_left_fingertip_3_link"/>
429
+ <exclude body1="gripper_left_fingertip_2_link" body2="gripper_left_fingertip_3_link"/>
430
+ <exclude body1="gripper_left_motor_single_link" body2="gripper_left_inner_single_link"/>
431
+ <exclude body1="gripper_left_motor_single_link" body2="gripper_left_fingertip_3_link"/>
432
+ <exclude body1="gripper_left_motor_single_link" body2="leg_left_4_link"/> -->
433
+ <exclude body1="arm_right_4_link" body2="arm_right_7_link"/>
434
+ <exclude body1="arm_right_5_link" body2="arm_right_7_link"/>
435
+ <exclude body1="arm_right_6_link" body2="arm_right_7_link"/>
436
+ <!-- <exclude body1="arm_right_7_link" body2="gripper_right_motor_double_link"/>
437
+ <exclude body1="arm_right_7_link" body2="gripper_right_inner_double_link"/>
438
+ <exclude body1="arm_right_7_link" body2="gripper_right_motor_single_link"/>
439
+ <exclude body1="arm_right_7_link" body2="gripper_right_inner_single_link"/>
440
+ <exclude body1="gripper_right_motor_double_link" body2="gripper_right_inner_double_link"/>
441
+ <exclude body1="gripper_right_motor_double_link" body2="gripper_right_fingertip_2_link"/>
442
+ <exclude body1="gripper_right_motor_double_link" body2="gripper_right_motor_single_link"/>
443
+ <exclude body1="gripper_right_motor_double_link" body2="gripper_right_inner_single_link"/>
444
+ <exclude body1="gripper_right_inner_double_link" body2="gripper_right_motor_single_link"/>
445
+ <exclude body1="gripper_right_fingertip_1_link" body2="gripper_right_fingertip_2_link"/>
446
+ <exclude body1="gripper_right_fingertip_1_link" body2="gripper_right_fingertip_3_link"/>
447
+ <exclude body1="gripper_right_fingertip_2_link" body2="gripper_right_fingertip_3_link"/>
448
+ <exclude body1="gripper_right_motor_single_link" body2="gripper_right_inner_single_link"/>
449
+ <exclude body1="gripper_right_motor_single_link" body2="gripper_right_fingertip_3_link"/>
450
+ <exclude body1="gripper_right_motor_single_link" body2="leg_right_4_link"/> -->
451
+ <exclude body1="leg_left_1_link" body2="leg_left_3_link"/>
452
+ <exclude body1="leg_left_2_link" body2="leg_left_3_link"/>
453
+ <exclude body1="leg_left_3_link" body2="leg_left_4_link"/>
454
+ <exclude body1="leg_left_4_link" body2="leg_left_6_link"/>
455
+ <exclude body1="leg_right_1_link" body2="leg_right_3_link"/>
456
+ <exclude body1="leg_right_2_link" body2="leg_right_3_link"/>
457
+ <exclude body1="leg_right_3_link" body2="leg_right_4_link"/>
458
+ <exclude body1="leg_right_4_link" body2="leg_right_6_link"/>
459
+ </contact>
460
+
461
+ <!-- <equality>
462
+ <joint joint1="gripper_left_motor_single_joint" joint2="gripper_left_joint" polycoef="0 -1 0 0 0" solref="0.01"/>
463
+ <joint joint1="gripper_left_joint" joint2="gripper_left_inner_double_joint" polycoef="0 1 0 0 0" solref="0.01"/>
464
+ <joint joint1="gripper_left_inner_single_joint" joint2="gripper_left_inner_double_joint" polycoef="0 -1 0 0 0" solref="0.01"/>
465
+ <joint joint1="gripper_right_motor_single_joint" joint2="gripper_right_joint" polycoef="0 -1 0 0 0" solref="0.01"/>
466
+ <joint joint1="gripper_right_joint" joint2="gripper_right_inner_double_joint" polycoef="0 1 0 0 0" solref="0.01"/>
467
+ <joint joint1="gripper_right_inner_single_joint" joint2="gripper_right_inner_double_joint" polycoef="0 -1 0 0 0" solref="0.01"/>
468
+ </equality> -->
469
+
470
+ <actuator>
471
+ <general name="torso_1_joint_position" class="position" joint="torso_1_joint" ctrlrange="-1.25664 1.25664" gainprm="2000" biasprm="0 -2000 -139.858"/>
472
+ <general name="torso_2_joint_position" class="position" joint="torso_2_joint" ctrlrange="-0.226893 0.733038" gainprm="500" biasprm="0 -500 -68.9009"/>
473
+ <general name="head_1_joint_position" class="position" joint="head_1_joint" ctrlrange="-0.20944 0.785398" gainprm="200" biasprm="0 -200 -5.81825"/>
474
+ <general name="head_2_joint_position" class="position" joint="head_2_joint" ctrlrange="-1.309 1.309" gainprm="1000" biasprm="0 -1000 -4.90471"/>
475
+ <general name="arm_left_1_joint_position" class="position" joint="arm_left_1_joint" ctrlrange="-1.5708 0.785398" gainprm="100" biasprm="0 -100 -9.76649"/>
476
+ <general name="arm_left_2_joint_position" class="position" joint="arm_left_2_joint" ctrlrange="0.00872665 2.87107" gainprm="100" biasprm="0 -100 -23.8597"/>
477
+ <general name="arm_left_3_joint_position" class="position" joint="arm_left_3_joint" ctrlrange="-2.42601 2.42601" gainprm="100" biasprm="0 -100 -2.13267"/>
478
+ <general name="arm_left_4_joint_position" class="position" joint="arm_left_4_joint" ctrlrange="-2.23402 -0.00349066" gainprm="100" biasprm="0 -100 -11.2117"/>
479
+ <general name="arm_left_5_joint_position" class="position" joint="arm_left_5_joint" ctrlrange="-2.51327 2.51327" gainprm="100" biasprm="0 -100 -1.42145"/>
480
+ <general name="arm_left_6_joint_position" class="position" joint="arm_left_6_joint" ctrlrange="-1.37008 1.37008" gainprm="100" biasprm="0 -100 -3.04811"/>
481
+ <general name="arm_left_7_joint_position" class="position" joint="arm_left_7_joint" ctrlrange="-0.680678 0.680678" gainprm="100" biasprm="0 -100 -3.01031"/>
482
+ <!-- <general name="gripper_left_joint_position" class="position" joint="gripper_left_joint" ctrlrange="-0.959931 0" forcerange="-10 10" gainprm="1000" biasprm="0 -1000 -0.895504"/> -->
483
+ <general name="arm_right_1_joint_position" class="position" joint="arm_right_1_joint" ctrlrange="-0.785398 1.5708" gainprm="100" biasprm="0 -100 -9.9406"/>
484
+ <general name="arm_right_2_joint_position" class="position" joint="arm_right_2_joint" ctrlrange="-2.87107 -0.00872665" gainprm="100" biasprm="0 -100 -23.8597"/>
485
+ <general name="arm_right_3_joint_position" class="position" joint="arm_right_3_joint" ctrlrange="-2.42601 2.42601" gainprm="100" biasprm="0 -100 -2.13267"/>
486
+ <general name="arm_right_4_joint_position" class="position" joint="arm_right_4_joint" ctrlrange="-2.23402 -0.00349066" gainprm="100" biasprm="0 -100 -11.2176"/>
487
+ <general name="arm_right_5_joint_position" class="position" joint="arm_right_5_joint" ctrlrange="-2.51327 2.51327" gainprm="100" biasprm="0 -100 -1.42145"/>
488
+ <general name="arm_right_6_joint_position" class="position" joint="arm_right_6_joint" ctrlrange="-1.37008 1.37008" gainprm="100" biasprm="0 -100 -3.04811"/>
489
+ <general name="arm_right_7_joint_position" class="position" joint="arm_right_7_joint" ctrlrange="-0.680678 0.680678" gainprm="100" biasprm="0 -100 -3.01031"/>
490
+ <!-- <general name="gripper_right_joint_position" class="position" joint="gripper_right_joint" ctrlrange="-0.959931 0" forcerange="-10 10" gainprm="1000" biasprm="0 -1000 -0.895504"/> -->
491
+ <general name="leg_left_1_joint_position" class="position" joint="leg_left_1_joint" ctrlrange="-0.349066 1.5708" gainprm="1000" biasprm="0 -1000 -23.9228"/>
492
+ <general name="leg_left_2_joint_position" class="position" joint="leg_left_2_joint" ctrlrange="-0.5236 0.5236" gainprm="1000" biasprm="0 -1000 -107.733"/>
493
+ <general name="leg_left_3_joint_position" class="position" joint="leg_left_3_joint" ctrlrange="-2.095 0.7" gainprm="1000" biasprm="0 -1000 -106.649"/>
494
+ <general name="leg_left_4_joint_position" class="position" joint="leg_left_4_joint" ctrlrange="0 2.618" gainprm="1000" biasprm="0 -1000 -42.434"/>
495
+ <general name="leg_left_5_joint_position" class="position" joint="leg_left_5_joint" ctrlrange="-1.27 0.68" gainprm="10000" biasprm="0 -10000 -31.7939"/>
496
+ <general name="leg_left_6_joint_position" class="position" joint="leg_left_6_joint" ctrlrange="-0.5236 0.5236" gainprm="10000" biasprm="0 -10000 -20.4185"/>
497
+ <general name="leg_right_1_joint_position" class="position" joint="leg_right_1_joint" ctrlrange="-1.5708 0.349066" gainprm="1000" biasprm="0 -1000 -23.9229"/>
498
+ <general name="leg_right_2_joint_position" class="position" joint="leg_right_2_joint" ctrlrange="-0.5236 0.5236" gainprm="1000" biasprm="0 -1000 -107.733"/>
499
+ <general name="leg_right_3_joint_position" class="position" joint="leg_right_3_joint" ctrlrange="-2.095 0.7" gainprm="1000" biasprm="0 -1000 -106.649"/>
500
+ <general name="leg_right_4_joint_position" class="position" joint="leg_right_4_joint" ctrlrange="0 2.618" gainprm="1000" biasprm="0 -1000 -42.434"/>
501
+ <general name="leg_right_5_joint_position" class="position" joint="leg_right_5_joint" ctrlrange="-1.27 0.68" gainprm="10000" biasprm="0 -10000 -31.7939"/>
502
+ <general name="leg_right_6_joint_position" class="position" joint="leg_right_6_joint" ctrlrange="-0.5236 0.5236" gainprm="10000" biasprm="0 -10000 -20.4185"/>
503
+ </actuator>
504
+
505
+ <!-- <keyframe>
506
+ <key name="walk_pose" qpos="0 0 1.025 1 0 0 0 0 0.15 0 0 0.3 0.4 -0.5 -1.5 0 0 0 0 -0.4 0 0 0 0 0 -0.3 -0.4 0.5 -1.5 0 0 0 0 -0.4 0 0 0 0 0 0 0 -0.4 0.8 -0.4 0 0 0 -0.4 0.8 -0.4 0" ctrl="0 0.15 0 0 0.3 0.4 -0.5 -1.5 0 0 0 -0.4 -0.3 -0.4 0.5 -1.5 0 0 0 -0.4 0 0 -0.4 0.8 -0.4 0 0 0 -0.4 0.8 -0.4 0"/>
507
+ </keyframe> -->
508
+ </mujoco>
check_phmr_pkl.py ADDED
@@ -0,0 +1,17 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import joblib
2
+
3
+ path = "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/data/motion_examples/human_motion/0000685.pkl"
4
+ data = joblib.load(path)
5
+
6
+ print(data.keys()) # dict_keys(['camera', 'people', 'timings', 'has_tracks', 'has_hps_cam', 'has_hps_world', 'has_slam', 'has_hands', 'has_2d_kpts', 'has_post_opt', 'spec_calib', 'contact_joint_ids', 'camera_world'])
7
+ print(data['people'].keys()) # dict_keys([1]),这里代表只有一个人,如果[1, 2]则两个人
8
+ print(data['people'][1].keys()) # dict_keys(['track_id', 'bbox_format', 'frames', 'bboxes', 'detected', 'smplx_pose', 'smplx_transl', 'smplx_betas', 'smplx_cam', 'smplx_world'])
9
+ print(data['people'][1]['smplx_pose'].shape) # (349, 22, 3, 3)
10
+ print(data['people'][1]['smplx_transl'].shape) # (349, 3)
11
+ print(data['people'][1]['smplx_betas'].shape) # (349, 10)
12
+ print(data['people'][1]['smplx_world'].keys()) # dict_keys(['pose', 'shape', 'trans'])
13
+ print(data['people'][1]['smplx_world']['pose'].shape) # (349, 165), 55*3,轴角格式
14
+
15
+
16
+ breakpoint()
17
+ # print(data)
phmr_to_robot.sh ADDED
@@ -0,0 +1,45 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ cd /cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/code/GMR
2
+ pkill -f monitor.py
3
+ conda activate /cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/envs/gmr
4
+
5
+ # 如果要保存视频,需要先apt install -y xvfb
6
+ # export PYOPENGL_PLATFORM="egl"
7
+ # export MUJOCO_GL="egl"
8
+ # xvfb-run -a python scripts/phmr_to_robot.py \
9
+ # --phmr_pred_file "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/data/motion_examples/human_motion/0000052.pkl" \
10
+ # --robot "unitree_g1" \
11
+ # --save_path "outputs/motion_examples/human_motion/0000052.pkl" \
12
+ # --rate_limit \
13
+ # --record_video
14
+
15
+ # 如果不打算可视化
16
+ # 单个
17
+ # python scripts/phmr_to_robot.py \
18
+ # --phmr_pred_file "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/data/motion_examples/human_motion/0000052.pkl" \
19
+ # --robot "unitree_g1" \
20
+ # --save_path "outputs/motion_examples/human_motion/0000052.pkl" \
21
+ # --rate_limit \
22
+ # --headless
23
+
24
+ # 批量
25
+ # export PYOPENGL_PLATFORM="egl"
26
+ # export MUJOCO_GL="egl"
27
+ # xvfb-run -a python scripts/phmr_to_robot_dataset.py \
28
+ # --jsonl_path "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/data/filtered_motion_examples/filtered_pkl.jsonl" \
29
+ # --robot "unitree_g1" \
30
+ # --save_dir "outputs/motion_examples/human_motion" \
31
+ # --rate_limit \
32
+ # --record_video \
33
+ # --st 100 \
34
+ # --ed 300
35
+
36
+ export PYOPENGL_PLATFORM="egl"
37
+ export MUJOCO_GL="egl"
38
+ xvfb-run -a python scripts/phmr_to_robot_dataset.py \
39
+ --jsonl_path "/cpfs01/thscc/sharestorage/AIGC-06-01/Hithink_VideoGen_Group/user_workspace/buwendong/user/data/filtered_motion_examples/filtered_pkl.jsonl" \
40
+ --robot "unitree_g1" \
41
+ --save_dir "outputs/motion_examples/human_motion" \
42
+ --rate_limit \
43
+ --record_video \
44
+ --st 300 \
45
+ --ed 484
setup.py ADDED
@@ -0,0 +1,32 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from setuptools import setup, find_packages
2
+
3
+ setup(
4
+ name = 'general_motion_retargeting',
5
+ packages = find_packages(),
6
+ author="Yanjie Ze",
7
+ author_email="lastyanjieze@gmail.com",
8
+ description="General Motion Retargeting (GMR) for Humanoid Robots",
9
+ long_description=open("README.md").read(),
10
+ long_description_content_type="text/markdown",
11
+ url="https://github.com/YanjieZe/GMR",
12
+ license="MIT",
13
+ version="0.2.0",
14
+ install_requires=[
15
+ "loop_rate_limiters",
16
+ "mink",
17
+ "mujoco",
18
+ "numpy",
19
+ "scipy",
20
+ "qpsolvers[proxqp]",
21
+ "rich",
22
+ "tqdm",
23
+ "opencv-python",
24
+ "natsort",
25
+ "psutil",
26
+ "smplx @ git+https://github.com/vchoutas/smplx",
27
+ "protobuf",
28
+ "redis[hiredis]",
29
+ "imageio[ffmpeg]",
30
+ ],
31
+ python_requires='>=3.10',
32
+ )
third_party/poselib/__init__.py ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ __version__ = "0.0.1"
2
+
3
+ from .core import *
third_party/poselib/core/__init__.py ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ from .tensor_utils import *
2
+ from .rotation3d import *
3
+ from .backend import Serializable, logger
third_party/poselib/core/backend/__init__.py ADDED
@@ -0,0 +1,3 @@
 
 
 
 
1
+ from .abstract import Serializable
2
+
3
+ from .logger import logger
third_party/poselib/core/backend/abstract.py ADDED
@@ -0,0 +1,121 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from abc import ABCMeta, abstractmethod, abstractclassmethod
2
+ from collections import OrderedDict
3
+ import json
4
+
5
+ import numpy as np
6
+ import os
7
+
8
+ TENSOR_CLASS = {}
9
+
10
+
11
+ def register(name):
12
+ global TENSOR_CLASS
13
+
14
+ def core(tensor_cls):
15
+ TENSOR_CLASS[name] = tensor_cls
16
+ return tensor_cls
17
+
18
+ return core
19
+
20
+
21
+ def _get_cls(name):
22
+ global TENSOR_CLASS
23
+ return TENSOR_CLASS[name]
24
+
25
+
26
+ class NumpyEncoder(json.JSONEncoder):
27
+ """ Special json encoder for numpy types """
28
+
29
+ def default(self, obj):
30
+ if isinstance(
31
+ obj,
32
+ (
33
+ np.int_,
34
+ np.intc,
35
+ np.intp,
36
+ np.int8,
37
+ np.int16,
38
+ np.int32,
39
+ np.int64,
40
+ np.uint8,
41
+ np.uint16,
42
+ np.uint32,
43
+ np.uint64,
44
+ ),
45
+ ):
46
+ return int(obj)
47
+ elif isinstance(obj, (np.float_, np.float16, np.float32, np.float64)):
48
+ return float(obj)
49
+ elif isinstance(obj, (np.ndarray,)):
50
+ return dict(__ndarray__=obj.tolist(), dtype=str(obj.dtype), shape=obj.shape)
51
+ return json.JSONEncoder.default(self, obj)
52
+
53
+
54
+ def json_numpy_obj_hook(dct):
55
+ if isinstance(dct, dict) and "__ndarray__" in dct:
56
+ data = np.asarray(dct["__ndarray__"], dtype=dct["dtype"])
57
+ return data.reshape(dct["shape"])
58
+ return dct
59
+
60
+
61
+ class Serializable:
62
+ """ Implementation to read/write to file.
63
+ All class the is inherited from this class needs to implement to_dict() and
64
+ from_dict()
65
+ """
66
+
67
+ @abstractclassmethod
68
+ def from_dict(cls, dict_repr, *args, **kwargs):
69
+ """ Read the object from an ordered dictionary
70
+
71
+ :param dict_repr: the ordered dictionary that is used to construct the object
72
+ :type dict_repr: OrderedDict
73
+ :param args, kwargs: the arguments that need to be passed into from_dict()
74
+ :type args, kwargs: additional arguments
75
+ """
76
+ pass
77
+
78
+ @abstractmethod
79
+ def to_dict(self):
80
+ """ Construct an ordered dictionary from the object
81
+
82
+ :rtype: OrderedDict
83
+ """
84
+ pass
85
+
86
+ @classmethod
87
+ def from_file(cls, path, *args, **kwargs):
88
+ """ Read the object from a file (either .npy or .json)
89
+
90
+ :param path: path of the file
91
+ :type path: string
92
+ :param args, kwargs: the arguments that need to be passed into from_dict()
93
+ :type args, kwargs: additional arguments
94
+ """
95
+ if path.endswith(".json"):
96
+ with open(path, "r") as f:
97
+ d = json.load(f, object_hook=json_numpy_obj_hook)
98
+ elif path.endswith(".npy"):
99
+ d = np.load(path, allow_pickle=True).item()
100
+ else:
101
+ assert False, "failed to load {} from {}".format(cls.__name__, path)
102
+ assert d["__name__"] == cls.__name__, "the file belongs to {}, not {}".format(
103
+ d["__name__"], cls.__name__
104
+ )
105
+ return cls.from_dict(d, *args, **kwargs)
106
+
107
+ def to_file(self, path: str) -> None:
108
+ """ Write the object to a file (either .npy or .json)
109
+
110
+ :param path: path of the file
111
+ :type path: string
112
+ """
113
+ if os.path.dirname(path) != "" and not os.path.exists(os.path.dirname(path)):
114
+ os.makedirs(os.path.dirname(path))
115
+ d = self.to_dict()
116
+ d["__name__"] = self.__class__.__name__
117
+ if path.endswith(".json"):
118
+ with open(path, "w") as f:
119
+ json.dump(d, f, cls=NumpyEncoder, indent=4)
120
+ elif path.endswith(".npy"):
121
+ np.save(path, d)
third_party/poselib/core/backend/logger.py ADDED
@@ -0,0 +1,13 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import logging
2
+
3
+ logger = logging.getLogger("poselib")
4
+ logger.setLevel(logging.INFO)
5
+
6
+ if not len(logger.handlers):
7
+ formatter = logging.Formatter(
8
+ fmt="%(asctime)-15s - %(levelname)s - %(module)s - %(message)s"
9
+ )
10
+ handler = logging.StreamHandler()
11
+ handler.setFormatter(formatter)
12
+ logger.addHandler(handler)
13
+ logger.info("logger initialized")
third_party/poselib/core/rotation3d.py ADDED
@@ -0,0 +1,443 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from typing import List, Optional
2
+
3
+ import math
4
+ import torch
5
+
6
+
7
+ @torch.jit.script
8
+ def quat_mul(a, b):
9
+ """
10
+ quaternion multiplication
11
+ """
12
+ x1, y1, z1, w1 = a[..., 0], a[..., 1], a[..., 2], a[..., 3]
13
+ x2, y2, z2, w2 = b[..., 0], b[..., 1], b[..., 2], b[..., 3]
14
+
15
+ w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
16
+ x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
17
+ y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
18
+ z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
19
+
20
+ return torch.stack([x, y, z, w], dim=-1)
21
+
22
+
23
+ @torch.jit.script
24
+ def quat_pos(x):
25
+ """
26
+ make all the real part of the quaternion positive
27
+ """
28
+ q = x
29
+ z = (q[..., 3:] < 0).float()
30
+ q = (1 - 2 * z) * q
31
+ return q
32
+
33
+
34
+ @torch.jit.script
35
+ def quat_abs(x):
36
+ """
37
+ quaternion norm (unit quaternion represents a 3D rotation, which has norm of 1)
38
+ """
39
+ x = x.norm(p=2, dim=-1)
40
+ return x
41
+
42
+
43
+ @torch.jit.script
44
+ def quat_unit(x):
45
+ """
46
+ normalized quaternion with norm of 1
47
+ """
48
+ norm = quat_abs(x).unsqueeze(-1)
49
+ return x / (norm.clamp(min=1e-9))
50
+
51
+
52
+ @torch.jit.script
53
+ def quat_conjugate(x):
54
+ """
55
+ quaternion with its imaginary part negated
56
+ """
57
+ return torch.cat([-x[..., :3], x[..., 3:]], dim=-1)
58
+
59
+
60
+ @torch.jit.script
61
+ def quat_real(x):
62
+ """
63
+ real component of the quaternion
64
+ """
65
+ return x[..., 3]
66
+
67
+
68
+ @torch.jit.script
69
+ def quat_imaginary(x):
70
+ """
71
+ imaginary components of the quaternion
72
+ """
73
+ return x[..., :3]
74
+
75
+
76
+ @torch.jit.script
77
+ def quat_norm_check(x):
78
+ """
79
+ verify that a quaternion has norm 1
80
+ """
81
+ assert bool(
82
+ (abs(x.norm(p=2, dim=-1) - 1) < 1e-3).all()
83
+ ), "the quaternion is has non-1 norm: {}".format(abs(x.norm(p=2, dim=-1) - 1))
84
+ assert bool((x[..., 3] >= 0).all()), "the quaternion has negative real part"
85
+
86
+
87
+ @torch.jit.script
88
+ def quat_normalize(q):
89
+ """
90
+ Construct 3D rotation from quaternion (the quaternion needs not to be normalized).
91
+ """
92
+ q = quat_unit(quat_pos(q)) # normalized to positive and unit quaternion
93
+ return q
94
+
95
+
96
+ @torch.jit.script
97
+ def quat_from_xyz(xyz):
98
+ """
99
+ Construct 3D rotation from the imaginary component
100
+ """
101
+ w = (1.0 - xyz.norm()).unsqueeze(-1)
102
+ assert bool((w >= 0).all()), "xyz has its norm greater than 1"
103
+ return torch.cat([xyz, w], dim=-1)
104
+
105
+
106
+ @torch.jit.script
107
+ def quat_identity(shape: List[int]):
108
+ """
109
+ Construct 3D identity rotation given shape
110
+ """
111
+ w = torch.ones(shape + [1])
112
+ xyz = torch.zeros(shape + [3])
113
+ q = torch.cat([xyz, w], dim=-1)
114
+ return quat_normalize(q)
115
+
116
+
117
+ @torch.jit.script
118
+ def quat_from_angle_axis(angle, axis, degree: bool = False):
119
+ """ Create a 3D rotation from angle and axis of rotation. The rotation is counter-clockwise
120
+ along the axis.
121
+
122
+ The rotation can be interpreted as a_R_b where frame "b" is the new frame that
123
+ gets rotated counter-clockwise along the axis from frame "a"
124
+
125
+ :param angle: angle of rotation
126
+ :type angle: Tensor
127
+ :param axis: axis of rotation
128
+ :type axis: Tensor
129
+ :param degree: put True here if the angle is given by degree
130
+ :type degree: bool, optional, default=False
131
+ """
132
+ if degree:
133
+ angle = angle / 180.0 * math.pi
134
+ theta = (angle / 2).unsqueeze(-1)
135
+ axis = axis / (axis.norm(p=2, dim=-1, keepdim=True).clamp(min=1e-9))
136
+ xyz = axis * theta.sin()
137
+ w = theta.cos()
138
+ return quat_normalize(torch.cat([xyz, w], dim=-1))
139
+
140
+
141
+ @torch.jit.script
142
+ def quat_from_rotation_matrix(m):
143
+ """
144
+ Construct a 3D rotation from a valid 3x3 rotation matrices.
145
+ Reference can be found here:
146
+ http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche52.html
147
+
148
+ :param m: 3x3 orthogonal rotation matrices.
149
+ :type m: Tensor
150
+
151
+ :rtype: Tensor
152
+ """
153
+ m = m.unsqueeze(0)
154
+ diag0 = m[..., 0, 0]
155
+ diag1 = m[..., 1, 1]
156
+ diag2 = m[..., 2, 2]
157
+
158
+ # Math stuff.
159
+ w = (((diag0 + diag1 + diag2 + 1.0) / 4.0).clamp(0.0, None)) ** 0.5
160
+ x = (((diag0 - diag1 - diag2 + 1.0) / 4.0).clamp(0.0, None)) ** 0.5
161
+ y = (((-diag0 + diag1 - diag2 + 1.0) / 4.0).clamp(0.0, None)) ** 0.5
162
+ z = (((-diag0 - diag1 + diag2 + 1.0) / 4.0).clamp(0.0, None)) ** 0.5
163
+
164
+ # Only modify quaternions where w > x, y, z.
165
+ c0 = (w >= x) & (w >= y) & (w >= z)
166
+ x[c0] *= (m[..., 2, 1][c0] - m[..., 1, 2][c0]).sign()
167
+ y[c0] *= (m[..., 0, 2][c0] - m[..., 2, 0][c0]).sign()
168
+ z[c0] *= (m[..., 1, 0][c0] - m[..., 0, 1][c0]).sign()
169
+
170
+ # Only modify quaternions where x > w, y, z
171
+ c1 = (x >= w) & (x >= y) & (x >= z)
172
+ w[c1] *= (m[..., 2, 1][c1] - m[..., 1, 2][c1]).sign()
173
+ y[c1] *= (m[..., 1, 0][c1] + m[..., 0, 1][c1]).sign()
174
+ z[c1] *= (m[..., 0, 2][c1] + m[..., 2, 0][c1]).sign()
175
+
176
+ # Only modify quaternions where y > w, x, z.
177
+ c2 = (y >= w) & (y >= x) & (y >= z)
178
+ w[c2] *= (m[..., 0, 2][c2] - m[..., 2, 0][c2]).sign()
179
+ x[c2] *= (m[..., 1, 0][c2] + m[..., 0, 1][c2]).sign()
180
+ z[c2] *= (m[..., 2, 1][c2] + m[..., 1, 2][c2]).sign()
181
+
182
+ # Only modify quaternions where z > w, x, y.
183
+ c3 = (z >= w) & (z >= x) & (z >= y)
184
+ w[c3] *= (m[..., 1, 0][c3] - m[..., 0, 1][c3]).sign()
185
+ x[c3] *= (m[..., 2, 0][c3] + m[..., 0, 2][c3]).sign()
186
+ y[c3] *= (m[..., 2, 1][c3] + m[..., 1, 2][c3]).sign()
187
+
188
+ return quat_normalize(torch.stack([x, y, z, w], dim=-1)).squeeze(0)
189
+
190
+
191
+ @torch.jit.script
192
+ def quat_mul_norm(x, y):
193
+ """
194
+ Combine two set of 3D rotations together using \**\* operator. The shape needs to be
195
+ broadcastable
196
+ """
197
+ return quat_normalize(quat_mul(x, y))
198
+
199
+
200
+ @torch.jit.script
201
+ def quat_rotate(rot, vec):
202
+ """
203
+ Rotate a 3D vector with the 3D rotation
204
+ """
205
+ other_q = torch.cat([vec, torch.zeros_like(vec[..., :1])], dim=-1)
206
+ return quat_imaginary(quat_mul(quat_mul(rot, other_q), quat_conjugate(rot)))
207
+
208
+
209
+ @torch.jit.script
210
+ def quat_inverse(x):
211
+ """
212
+ The inverse of the rotation
213
+ """
214
+ return quat_conjugate(x)
215
+
216
+
217
+ @torch.jit.script
218
+ def quat_identity_like(x):
219
+ """
220
+ Construct identity 3D rotation with the same shape
221
+ """
222
+ return quat_identity(x.shape[:-1])
223
+
224
+
225
+ @torch.jit.script
226
+ def quat_angle_axis(x):
227
+ """
228
+ The (angle, axis) representation of the rotation. The axis is normalized to unit length.
229
+ The angle is guaranteed to be between [0, pi].
230
+ """
231
+ s = 2 * (x[..., 3] ** 2) - 1
232
+ angle = s.clamp(-1, 1).arccos() # just to be safe
233
+ axis = x[..., :3]
234
+ axis /= axis.norm(p=2, dim=-1, keepdim=True).clamp(min=1e-9)
235
+ return angle, axis
236
+
237
+
238
+ @torch.jit.script
239
+ def quat_yaw_rotation(x, z_up: bool = True):
240
+ """
241
+ Yaw rotation (rotation along z-axis)
242
+ """
243
+ q = x
244
+ if z_up:
245
+ q = torch.cat([torch.zeros_like(q[..., 0:2]), q[..., 2:3], q[..., 3:]], dim=-1)
246
+ else:
247
+ q = torch.cat(
248
+ [
249
+ torch.zeros_like(q[..., 0:1]),
250
+ q[..., 1:2],
251
+ torch.zeros_like(q[..., 2:3]),
252
+ q[..., 3:4],
253
+ ],
254
+ dim=-1,
255
+ )
256
+ return quat_normalize(q)
257
+
258
+
259
+ @torch.jit.script
260
+ def transform_from_rotation_translation(
261
+ r: Optional[torch.Tensor] = None, t: Optional[torch.Tensor] = None
262
+ ):
263
+ """
264
+ Construct a transform from a quaternion and 3D translation. Only one of them can be None.
265
+ """
266
+ assert r is not None or t is not None, "rotation and translation can't be all None"
267
+ if r is None:
268
+ assert t is not None
269
+ r = quat_identity(list(t.shape))
270
+ if t is None:
271
+ t = torch.zeros(list(r.shape) + [3])
272
+ return torch.cat([r, t], dim=-1)
273
+
274
+
275
+ @torch.jit.script
276
+ def transform_identity(shape: List[int]):
277
+ """
278
+ Identity transformation with given shape
279
+ """
280
+ r = quat_identity(shape)
281
+ t = torch.zeros(shape + [3])
282
+ return transform_from_rotation_translation(r, t)
283
+
284
+
285
+
286
+ @torch.jit.script
287
+ def transform_rotation(x):
288
+ """Get rotation from transform"""
289
+ return x[..., :4]
290
+
291
+
292
+ @torch.jit.script
293
+ def transform_translation(x):
294
+ """Get translation from transform"""
295
+ return x[..., 4:]
296
+
297
+
298
+ @torch.jit.script
299
+ def transform_inverse(x):
300
+ """
301
+ Inverse transformation
302
+ """
303
+ inv_so3 = quat_inverse(transform_rotation(x))
304
+ return transform_from_rotation_translation(
305
+ r=inv_so3, t=quat_rotate(inv_so3, -transform_translation(x))
306
+ )
307
+
308
+
309
+ @torch.jit.script
310
+ def transform_identity_like(x):
311
+ """
312
+ identity transformation with the same shape
313
+ """
314
+ return transform_identity(x.shape)
315
+
316
+
317
+ @torch.jit.script
318
+ def transform_mul(x, y):
319
+ """
320
+ Combine two transformation together
321
+ """
322
+ z = transform_from_rotation_translation(
323
+ r=quat_mul_norm(transform_rotation(x), transform_rotation(y)),
324
+ t=quat_rotate(transform_rotation(x), transform_translation(y))
325
+ + transform_translation(x),
326
+ )
327
+ return z
328
+
329
+
330
+ @torch.jit.script
331
+ def transform_apply(rot, vec):
332
+ """
333
+ Transform a 3D vector
334
+ """
335
+ assert isinstance(vec, torch.Tensor)
336
+ return quat_rotate(transform_rotation(rot), vec) + transform_translation(rot)
337
+
338
+
339
+ @torch.jit.script
340
+ def rot_matrix_det(x):
341
+ """
342
+ Return the determinant of the 3x3 matrix. The shape of the tensor will be as same as the
343
+ shape of the matrix
344
+ """
345
+ a, b, c = x[..., 0, 0], x[..., 0, 1], x[..., 0, 2]
346
+ d, e, f = x[..., 1, 0], x[..., 1, 1], x[..., 1, 2]
347
+ g, h, i = x[..., 2, 0], x[..., 2, 1], x[..., 2, 2]
348
+ t1 = a * (e * i - f * h)
349
+ t2 = b * (d * i - f * g)
350
+ t3 = c * (d * h - e * g)
351
+ return t1 - t2 + t3
352
+
353
+
354
+ @torch.jit.script
355
+ def rot_matrix_integrity_check(x):
356
+ """
357
+ Verify that a rotation matrix has a determinant of one and is orthogonal
358
+ """
359
+ det = rot_matrix_det(x)
360
+ assert bool((abs(det - 1) < 1e-3).all()), "the matrix has non-one determinant"
361
+ rtr = x @ x.permute(torch.arange(x.dim() - 2), -1, -2)
362
+ rtr_gt = rtr.zeros_like()
363
+ rtr_gt[..., 0, 0] = 1
364
+ rtr_gt[..., 1, 1] = 1
365
+ rtr_gt[..., 2, 2] = 1
366
+ assert bool(((rtr - rtr_gt) < 1e-3).all()), "the matrix is not orthogonal"
367
+
368
+
369
+ @torch.jit.script
370
+ def rot_matrix_from_quaternion(q):
371
+ """
372
+ Construct rotation matrix from quaternion
373
+ """
374
+ # Shortcuts for individual elements (using wikipedia's convention)
375
+ qi, qj, qk, qr = q[..., 0], q[..., 1], q[..., 2], q[..., 3]
376
+
377
+ # Set individual elements
378
+ R00 = 1.0 - 2.0 * (qj ** 2 + qk ** 2)
379
+ R01 = 2 * (qi * qj - qk * qr)
380
+ R02 = 2 * (qi * qk + qj * qr)
381
+ R10 = 2 * (qi * qj + qk * qr)
382
+ R11 = 1.0 - 2.0 * (qi ** 2 + qk ** 2)
383
+ R12 = 2 * (qj * qk - qi * qr)
384
+ R20 = 2 * (qi * qk - qj * qr)
385
+ R21 = 2 * (qj * qk + qi * qr)
386
+ R22 = 1.0 - 2.0 * (qi ** 2 + qj ** 2)
387
+
388
+ R0 = torch.stack([R00, R01, R02], dim=-1)
389
+ R1 = torch.stack([R10, R11, R12], dim=-1)
390
+ R2 = torch.stack([R20, R21, R22], dim=-1)
391
+
392
+ R = torch.stack([R0, R1, R2], dim=-2)
393
+
394
+ return R
395
+
396
+
397
+ @torch.jit.script
398
+ def euclidean_to_rotation_matrix(x):
399
+ """
400
+ Get the rotation matrix on the top-left corner of a Euclidean transformation matrix
401
+ """
402
+ return x[..., :3, :3]
403
+
404
+
405
+ @torch.jit.script
406
+ def euclidean_integrity_check(x):
407
+ euclidean_to_rotation_matrix(x) # check 3d-rotation matrix
408
+ assert bool((x[..., 3, :3] == 0).all()), "the last row is illegal"
409
+ assert bool((x[..., 3, 3] == 1).all()), "the last row is illegal"
410
+
411
+
412
+ @torch.jit.script
413
+ def euclidean_translation(x):
414
+ """
415
+ Get the translation vector located at the last column of the matrix
416
+ """
417
+ return x[..., :3, 3]
418
+
419
+
420
+ @torch.jit.script
421
+ def euclidean_inverse(x):
422
+ """
423
+ Compute the matrix that represents the inverse rotation
424
+ """
425
+ s = x.zeros_like()
426
+ irot = quat_inverse(quat_from_rotation_matrix(x))
427
+ s[..., :3, :3] = irot
428
+ s[..., :3, 4] = quat_rotate(irot, -euclidean_translation(x))
429
+ return s
430
+
431
+
432
+ @torch.jit.script
433
+ def euclidean_to_transform(transformation_matrix):
434
+ """
435
+ Construct a transform from a Euclidean transformation matrix
436
+ """
437
+ return transform_from_rotation_translation(
438
+ r=quat_from_rotation_matrix(
439
+ m=euclidean_to_rotation_matrix(transformation_matrix)
440
+ ),
441
+ t=euclidean_translation(transformation_matrix),
442
+ )
443
+
third_party/poselib/core/tensor_utils.py ADDED
@@ -0,0 +1,36 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from collections import OrderedDict
2
+ from .backend import Serializable
3
+ import torch
4
+
5
+
6
+ class TensorUtils(Serializable):
7
+ @classmethod
8
+ def from_dict(cls, dict_repr, *args, **kwargs):
9
+ """ Read the object from an ordered dictionary
10
+
11
+ :param dict_repr: the ordered dictionary that is used to construct the object
12
+ :type dict_repr: OrderedDict
13
+ :param kwargs: the arguments that need to be passed into from_dict()
14
+ :type kwargs: additional arguments
15
+ """
16
+ return torch.from_numpy(dict_repr["arr"].astype(dict_repr["context"]["dtype"]))
17
+
18
+ def to_dict(self):
19
+ """ Construct an ordered dictionary from the object
20
+
21
+ :rtype: OrderedDict
22
+ """
23
+ return NotImplemented
24
+
25
+ def tensor_to_dict(x):
26
+ """ Construct an ordered dictionary from the object
27
+
28
+ :rtype: OrderedDict
29
+ """
30
+ x_np = x.numpy()
31
+ return {
32
+ "arr": x_np,
33
+ "context": {
34
+ "dtype": x_np.dtype.name
35
+ }
36
+ }
third_party/poselib/core/tests/__init__.py ADDED
File without changes
third_party/poselib/core/tests/test_rotation.py ADDED
@@ -0,0 +1,55 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from ..rotation3d import *
2
+ import numpy as np
3
+ import torch
4
+
5
+ q = torch.from_numpy(np.array([[0, 1, 2, 3], [-2, 3, -1, 5]], dtype=np.float32))
6
+ print("q", q)
7
+ r = quat_normalize(q)
8
+ x = torch.from_numpy(np.array([[1, 0, 0], [0, -1, 0]], dtype=np.float32))
9
+ print(r)
10
+ print(quat_rotate(r, x))
11
+
12
+ angle = torch.from_numpy(np.array(np.random.rand() * 10.0, dtype=np.float32))
13
+ axis = torch.from_numpy(
14
+ np.array([1, np.random.rand() * 10.0, np.random.rand() * 10.0], dtype=np.float32),
15
+ )
16
+
17
+ print(repr(angle))
18
+ print(repr(axis))
19
+
20
+ rot = quat_from_angle_axis(angle, axis)
21
+ x = torch.from_numpy(np.random.rand(5, 6, 3))
22
+ y = quat_rotate(quat_inverse(rot), quat_rotate(rot, x))
23
+ print(x.numpy())
24
+ print(y.numpy())
25
+ assert np.allclose(x.numpy(), y.numpy())
26
+
27
+ m = torch.from_numpy(np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]], dtype=np.float32))
28
+ r = quat_from_rotation_matrix(m)
29
+ t = torch.from_numpy(np.array([0, 1, 0], dtype=np.float32))
30
+ se3 = transform_from_rotation_translation(r=r, t=t)
31
+ print(se3)
32
+ print(transform_apply(se3, t))
33
+
34
+ rot = quat_from_angle_axis(
35
+ torch.from_numpy(np.array([45, -54], dtype=np.float32)),
36
+ torch.from_numpy(np.array([[1, 0, 0], [0, 1, 0]], dtype=np.float32)),
37
+ degree=True,
38
+ )
39
+ trans = torch.from_numpy(np.array([[1, 1, 0], [1, 1, 0]], dtype=np.float32))
40
+ transform = transform_from_rotation_translation(r=rot, t=trans)
41
+
42
+ t = transform_mul(transform, transform_inverse(transform))
43
+ gt = np.zeros((2, 7))
44
+ gt[:, 0] = 1.0
45
+ print(t.numpy())
46
+ print(gt)
47
+ # assert np.allclose(t.numpy(), gt)
48
+
49
+ transform2 = torch.from_numpy(
50
+ np.array(
51
+ [[1, 0, 0, 1], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]], dtype=np.float32
52
+ ),
53
+ )
54
+ transform2 = euclidean_to_transform(transform2)
55
+ print(transform2)
third_party/poselib/fbx_importer.py ADDED
@@ -0,0 +1,43 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import os
2
+ import json
3
+ import argparse
4
+ import sys
5
+
6
+ # Add the current directory to Python path to find poselib module
7
+ sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
8
+
9
+ from poselib.skeleton.skeleton3d import SkeletonMotion
10
+
11
+ def main():
12
+ # Set up argument parser
13
+ parser = argparse.ArgumentParser(description='Import FBX file and convert to PoseLib format')
14
+ parser.add_argument('--input', '-i', required=True, help='Input FBX file path')
15
+ parser.add_argument('--output', '-o', required=True, help='Output file path for the converted motion')
16
+ parser.add_argument('--root-joint', '-r', default='Hips', help='Root joint name (default: Hips)')
17
+ parser.add_argument('--fps', '-f', type=int, default=120, help='FPS for the motion (default: 120)')
18
+
19
+ args = parser.parse_args()
20
+
21
+ # Check if input file exists
22
+ if not os.path.exists(args.input):
23
+ print(f"Error: Input file '{args.input}' does not exist")
24
+ return
25
+
26
+ # Create output directory if it doesn't exist
27
+ output_dir = os.path.dirname(args.output)
28
+ if output_dir and not os.path.exists(output_dir):
29
+ os.makedirs(output_dir)
30
+
31
+ # Import fbx file
32
+ motion = SkeletonMotion.from_fbx(
33
+ fbx_file_path=args.input,
34
+ root_joint=args.root_joint,
35
+ fps=args.fps
36
+ )
37
+
38
+ # Save motion in the specified format
39
+ motion.to_retarget_motion_file(args.output)
40
+ print(f"Successfully converted '{args.input}' to '{args.output}'")
41
+
42
+ if __name__ == "__main__":
43
+ main()
third_party/poselib/skeleton/__init__.py ADDED
File without changes
third_party/poselib/skeleton/backend/__init__.py ADDED
File without changes
third_party/poselib/skeleton/backend/fbx/__init__.py ADDED
File without changes
third_party/poselib/skeleton/backend/fbx/fbx_backend.py ADDED
@@ -0,0 +1,245 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ This script reads an fbx file and returns the joint names, parents, and transforms.
3
+
4
+ NOTE: It requires the Python FBX package to be installed.
5
+ """
6
+
7
+ import sys
8
+
9
+ import numpy as np
10
+
11
+ try:
12
+ import fbx
13
+ import FbxCommon
14
+ except ImportError as e:
15
+ print("Error: FBX library failed to load - importing FBX data will not succeed. Message: {}".format(e))
16
+ print("FBX tools must be installed from https://help.autodesk.com/view/FBX/2020/ENU/?guid=FBX_Developer_Help_scripting_with_python_fbx_installing_python_fbx_html")
17
+
18
+
19
+ def fbx_to_npy(file_name_in, root_joint_name, fps):
20
+ """
21
+ This function reads in an fbx file, and saves the relevant info to a numpy array
22
+
23
+ Fbx files have a series of animation curves, each of which has animations at different
24
+ times. This script assumes that for mocap data, there is only one animation curve that
25
+ contains all the joints. Otherwise it is unclear how to read in the data.
26
+
27
+ If this condition isn't met, then the method throws an error
28
+
29
+ :param file_name_in: str, file path in. Should be .fbx file
30
+ :return: nothing, it just writes a file.
31
+ """
32
+
33
+ # Create the fbx scene object and load the .fbx file
34
+ fbx_sdk_manager, fbx_scene = FbxCommon.InitializeSdkObjects()
35
+ FbxCommon.LoadScene(fbx_sdk_manager, fbx_scene, file_name_in)
36
+
37
+ """
38
+ To read in the animation, we must find the root node of the skeleton.
39
+
40
+ Unfortunately fbx files can have "scene parents" and other parts of the tree that are
41
+ not joints
42
+
43
+ As a crude fix, this reader just takes and finds the first thing which has an
44
+ animation curve attached
45
+ """
46
+
47
+ search_root = (root_joint_name is None or root_joint_name == "")
48
+
49
+ # Get the root node of the skeleton, which is the child of the scene's root node
50
+ possible_root_nodes = [fbx_scene.GetRootNode()]
51
+ found_root_node = False
52
+ max_key_count = 0
53
+ root_joint = None
54
+ while len(possible_root_nodes) > 0:
55
+ joint = possible_root_nodes.pop(0)
56
+ if not search_root:
57
+ if root_joint_name in joint.GetName():
58
+ root_joint = joint
59
+ try:
60
+ curve = _get_animation_curve(joint, fbx_scene)
61
+ except RuntimeError:
62
+ curve = None
63
+ if curve is not None:
64
+ key_count = curve.KeyGetCount()
65
+ if key_count > max_key_count:
66
+ found_root_node = True
67
+ max_key_count = key_count
68
+ root_curve = curve
69
+ if search_root and not root_joint:
70
+ root_joint = joint
71
+ for child_index in range(joint.GetChildCount()):
72
+ possible_root_nodes.append(joint.GetChild(child_index))
73
+ if not found_root_node:
74
+ raise RuntimeError("No root joint found!! Exiting")
75
+
76
+ joint_list, joint_names, parents = _get_skeleton(root_joint)
77
+
78
+ """
79
+ Read in the transformation matrices of the animation, taking the scaling into account
80
+ """
81
+
82
+ anim_range, frame_count, frame_rate = _get_frame_count(fbx_scene)
83
+
84
+ local_transforms = []
85
+ #for frame in range(frame_count):
86
+ time_sec = anim_range.GetStart().GetSecondDouble()
87
+ time_range_sec = anim_range.GetStop().GetSecondDouble() - time_sec
88
+ fbx_fps = frame_count / time_range_sec
89
+ if fps != 120:
90
+ fbx_fps = fps
91
+ print("FPS: ", fbx_fps)
92
+ while time_sec < anim_range.GetStop().GetSecondDouble():
93
+ fbx_time = fbx.FbxTime()
94
+ fbx_time.SetSecondDouble(time_sec)
95
+ fbx_time = fbx_time.GetFramedTime()
96
+ transforms_current_frame = []
97
+
98
+ # Fbx has a unique time object which you need
99
+ #fbx_time = root_curve.KeyGetTime(frame)
100
+ for joint in joint_list:
101
+ arr = np.array(_recursive_to_list(joint.EvaluateLocalTransform(fbx_time)))
102
+ scales = np.array(_recursive_to_list(joint.EvaluateLocalScaling(fbx_time)))
103
+ if not np.allclose(scales[0:3], scales[0]):
104
+ raise ValueError(
105
+ "Different X, Y and Z scaling. Unsure how this should be handled. "
106
+ "To solve this, look at this link and try to upgrade the script "
107
+ "http://help.autodesk.com/view/FBX/2017/ENU/?guid=__files_GUID_10CDD"
108
+ "63C_79C1_4F2D_BB28_AD2BE65A02ED_htm"
109
+ )
110
+ # Adjust the array for scaling
111
+ arr /= scales[0]
112
+ arr[3, 3] = 1.0
113
+ transforms_current_frame.append(arr)
114
+ local_transforms.append(transforms_current_frame)
115
+
116
+ time_sec += (1.0/fbx_fps)
117
+
118
+ local_transforms = np.array(local_transforms)
119
+ print("Frame Count: ", len(local_transforms))
120
+
121
+ return joint_names, parents, local_transforms, fbx_fps
122
+
123
+ def _get_frame_count(fbx_scene):
124
+ # Get the animation stacks and layers, in order to pull off animation curves later
125
+ num_anim_stacks = fbx_scene.GetSrcObjectCount(
126
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimStack.ClassId)
127
+ )
128
+ # if num_anim_stacks != 1:
129
+ # raise RuntimeError(
130
+ # "More than one animation stack was found. "
131
+ # "This script must be modified to handle this case. Exiting"
132
+ # )
133
+ if num_anim_stacks > 1:
134
+ index = 1
135
+ else:
136
+ index = 0
137
+ anim_stack = fbx_scene.GetSrcObject(
138
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimStack.ClassId), index
139
+ )
140
+
141
+ anim_range = anim_stack.GetLocalTimeSpan()
142
+ duration = anim_range.GetDuration()
143
+ fps = duration.GetFrameRate(duration.GetGlobalTimeMode())
144
+ frame_count = duration.GetFrameCount(True)
145
+
146
+ return anim_range, frame_count, fps
147
+
148
+ def _get_animation_curve(joint, fbx_scene):
149
+ # Get the animation stacks and layers, in order to pull off animation curves later
150
+ num_anim_stacks = fbx_scene.GetSrcObjectCount(
151
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimStack.ClassId)
152
+ )
153
+ # if num_anim_stacks != 1:
154
+ # raise RuntimeError(
155
+ # "More than one animation stack was found. "
156
+ # "This script must be modified to handle this case. Exiting"
157
+ # )
158
+ if num_anim_stacks > 1:
159
+ index = 1
160
+ else:
161
+ index = 0
162
+ anim_stack = fbx_scene.GetSrcObject(
163
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimStack.ClassId), index
164
+ )
165
+
166
+ num_anim_layers = anim_stack.GetSrcObjectCount(
167
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimLayer.ClassId)
168
+ )
169
+ if num_anim_layers != 1:
170
+ raise RuntimeError(
171
+ "More than one animation layer was found. "
172
+ "This script must be modified to handle this case. Exiting"
173
+ )
174
+ animation_layer = anim_stack.GetSrcObject(
175
+ FbxCommon.FbxCriteria.ObjectType(FbxCommon.FbxAnimLayer.ClassId), 0
176
+ )
177
+
178
+ def _check_longest_curve(curve, max_curve_key_count):
179
+ longest_curve = None
180
+ if curve and curve.KeyGetCount() > max_curve_key_count[0]:
181
+ max_curve_key_count[0] = curve.KeyGetCount()
182
+ return True
183
+
184
+ return False
185
+
186
+ max_curve_key_count = [0]
187
+ longest_curve = None
188
+ for c in ["X", "Y", "Z"]:
189
+ curve = joint.LclTranslation.GetCurve(
190
+ animation_layer, c
191
+ ) # sample curve for translation
192
+ if _check_longest_curve(curve, max_curve_key_count):
193
+ longest_curve = curve
194
+
195
+ curve = joint.LclRotation.GetCurve(
196
+ animation_layer, "X"
197
+ )
198
+ if _check_longest_curve(curve, max_curve_key_count):
199
+ longest_curve = curve
200
+
201
+ return longest_curve
202
+
203
+
204
+ def _get_skeleton(root_joint):
205
+
206
+ # Do a depth first search of the skeleton to extract all the joints
207
+ joint_list = [root_joint]
208
+ joint_names = [root_joint.GetName()]
209
+ parents = [-1] # -1 means no parent
210
+
211
+ def append_children(joint, pos):
212
+ """
213
+ Depth first search function
214
+ :param joint: joint item in the fbx
215
+ :param pos: position of current element (for parenting)
216
+ :return: Nothing
217
+ """
218
+ for child_index in range(joint.GetChildCount()):
219
+ child = joint.GetChild(child_index)
220
+ joint_list.append(child)
221
+ joint_names.append(child.GetName())
222
+ parents.append(pos)
223
+ append_children(child, len(parents) - 1)
224
+
225
+ append_children(root_joint, 0)
226
+ return joint_list, joint_names, parents
227
+
228
+
229
+ def _recursive_to_list(array):
230
+ """
231
+ Takes some iterable that might contain iterables and converts it to a list of lists
232
+ [of lists... etc]
233
+
234
+ Mainly used for converting the strange fbx wrappers for c++ arrays into python lists
235
+ :param array: array to be converted
236
+ :return: array converted to lists
237
+ """
238
+ try:
239
+ return float(array)
240
+ except TypeError:
241
+ return [_recursive_to_list(a) for a in array]
242
+
243
+
244
+ def parse_fbx(file_name_in, root_joint_name, fps):
245
+ return fbx_to_npy(file_name_in, root_joint_name, fps)
third_party/poselib/skeleton/backend/fbx/fbx_read_wrapper.py ADDED
@@ -0,0 +1,32 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ Script that reads in fbx files from python
3
+
4
+ This requires a configs file, which contains the command necessary to switch conda
5
+ environments to run the fbx reading script from python
6
+ """
7
+
8
+ from ....core import logger
9
+
10
+ import inspect
11
+ import os
12
+
13
+ import numpy as np
14
+
15
+ from .fbx_backend import parse_fbx
16
+
17
+
18
+ def fbx_to_array(fbx_file_path, root_joint, fps):
19
+ """
20
+ Reads an fbx file to an array.
21
+
22
+ :param fbx_file_path: str, file path to fbx
23
+ :return: tuple with joint_names, parents, transforms, frame time
24
+ """
25
+
26
+ # Ensure the file path is valid
27
+ fbx_file_path = os.path.abspath(fbx_file_path)
28
+ assert os.path.exists(fbx_file_path)
29
+
30
+ # Parse FBX file
31
+ joint_names, parents, local_transforms, fbx_fps = parse_fbx(fbx_file_path, root_joint, fps)
32
+ return joint_names, parents, local_transforms, fbx_fps
third_party/poselib/skeleton/skeleton3d.py ADDED
@@ -0,0 +1,1421 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import os
2
+ import xml.etree.ElementTree as ET
3
+ from collections import OrderedDict
4
+ from typing import List, Optional, Type, Dict
5
+
6
+ import numpy as np
7
+ import pickle
8
+ import torch
9
+
10
+ from ..core import *
11
+ from .backend.fbx.fbx_read_wrapper import fbx_to_array
12
+ import scipy.ndimage.filters as filters
13
+
14
+
15
+ class SkeletonTree(Serializable):
16
+ """
17
+ A skeleton tree gives a complete description of a rigid skeleton. It describes a tree structure
18
+ over a list of nodes with their names indicated by strings. Each edge in the tree has a local
19
+ translation associated with it which describes the distance between the two nodes that it
20
+ connects.
21
+
22
+ Basic Usage:
23
+ >>> t = SkeletonTree.from_mjcf(SkeletonTree.__example_mjcf_path__)
24
+ >>> t
25
+ SkeletonTree(
26
+ node_names=['torso', 'front_left_leg', 'aux_1', 'front_left_foot', 'front_right_leg', 'aux_2', 'front_right_foot', 'left_back_leg', 'aux_3', 'left_back_foot', 'right_back_leg', 'aux_4', 'right_back_foot'],
27
+ parent_indices=tensor([-1, 0, 1, 2, 0, 4, 5, 0, 7, 8, 0, 10, 11]),
28
+ local_translation=tensor([[ 0.0000, 0.0000, 0.7500],
29
+ [ 0.0000, 0.0000, 0.0000],
30
+ [ 0.2000, 0.2000, 0.0000],
31
+ [ 0.2000, 0.2000, 0.0000],
32
+ [ 0.0000, 0.0000, 0.0000],
33
+ [-0.2000, 0.2000, 0.0000],
34
+ [-0.2000, 0.2000, 0.0000],
35
+ [ 0.0000, 0.0000, 0.0000],
36
+ [-0.2000, -0.2000, 0.0000],
37
+ [-0.2000, -0.2000, 0.0000],
38
+ [ 0.0000, 0.0000, 0.0000],
39
+ [ 0.2000, -0.2000, 0.0000],
40
+ [ 0.2000, -0.2000, 0.0000]])
41
+ )
42
+ >>> t.node_names
43
+ ['torso', 'front_left_leg', 'aux_1', 'front_left_foot', 'front_right_leg', 'aux_2', 'front_right_foot', 'left_back_leg', 'aux_3', 'left_back_foot', 'right_back_leg', 'aux_4', 'right_back_foot']
44
+ >>> t.parent_indices
45
+ tensor([-1, 0, 1, 2, 0, 4, 5, 0, 7, 8, 0, 10, 11])
46
+ >>> t.local_translation
47
+ tensor([[ 0.0000, 0.0000, 0.7500],
48
+ [ 0.0000, 0.0000, 0.0000],
49
+ [ 0.2000, 0.2000, 0.0000],
50
+ [ 0.2000, 0.2000, 0.0000],
51
+ [ 0.0000, 0.0000, 0.0000],
52
+ [-0.2000, 0.2000, 0.0000],
53
+ [-0.2000, 0.2000, 0.0000],
54
+ [ 0.0000, 0.0000, 0.0000],
55
+ [-0.2000, -0.2000, 0.0000],
56
+ [-0.2000, -0.2000, 0.0000],
57
+ [ 0.0000, 0.0000, 0.0000],
58
+ [ 0.2000, -0.2000, 0.0000],
59
+ [ 0.2000, -0.2000, 0.0000]])
60
+ >>> t.parent_of('front_left_leg')
61
+ 'torso'
62
+ >>> t.index('front_right_foot')
63
+ 6
64
+ >>> t[2]
65
+ 'aux_1'
66
+ """
67
+
68
+ __example_mjcf_path__ = os.path.join(
69
+ os.path.dirname(os.path.realpath(__file__)), "tests/ant.xml"
70
+ )
71
+
72
+ def __init__(self, node_names, parent_indices, local_translation):
73
+ """
74
+ :param node_names: a list of names for each tree node
75
+ :type node_names: List[str]
76
+ :param parent_indices: an int32-typed tensor that represents the edge to its parent.\
77
+ -1 represents the root node
78
+ :type parent_indices: Tensor
79
+ :param local_translation: a 3d vector that gives local translation information
80
+ :type local_translation: Tensor
81
+ """
82
+ ln, lp, ll = len(node_names), len(parent_indices), len(local_translation)
83
+ assert len(set((ln, lp, ll))) == 1
84
+ self._node_names = node_names
85
+ self._parent_indices = parent_indices.long()
86
+ self._local_translation = local_translation
87
+ self._node_indices = {self.node_names[i]: i for i in range(len(self))}
88
+
89
+ def __len__(self):
90
+ """ number of nodes in the skeleton tree """
91
+ return len(self.node_names)
92
+
93
+ def __iter__(self):
94
+ """ iterator that iterate through the name of each node """
95
+ yield from self.node_names
96
+
97
+ def __getitem__(self, item):
98
+ """ get the name of the node given the index """
99
+ return self.node_names[item]
100
+
101
+ def __repr__(self):
102
+ return (
103
+ "SkeletonTree(\n node_names={},\n parent_indices={},"
104
+ "\n local_translation={}\n)".format(
105
+ self._indent(repr(self.node_names)),
106
+ self._indent(repr(self.parent_indices)),
107
+ self._indent(repr(self.local_translation)),
108
+ )
109
+ )
110
+
111
+ def _indent(self, s):
112
+ return "\n ".join(s.split("\n"))
113
+
114
+ @property
115
+ def node_names(self):
116
+ return self._node_names
117
+
118
+ @property
119
+ def parent_indices(self):
120
+ return self._parent_indices
121
+
122
+ @property
123
+ def local_translation(self):
124
+ return self._local_translation
125
+
126
+ @property
127
+ def num_joints(self):
128
+ """ number of nodes in the skeleton tree """
129
+ return len(self)
130
+
131
+ @classmethod
132
+ def from_dict(cls, dict_repr, *args, **kwargs):
133
+ return cls(
134
+ list(map(str, dict_repr["node_names"])),
135
+ TensorUtils.from_dict(dict_repr["parent_indices"], *args, **kwargs),
136
+ TensorUtils.from_dict(dict_repr["local_translation"], *args, **kwargs),
137
+ )
138
+
139
+ def to_dict(self):
140
+ return OrderedDict(
141
+ [
142
+ ("node_names", self.node_names),
143
+ ("parent_indices", tensor_to_dict(self.parent_indices)),
144
+ ("local_translation", tensor_to_dict(self.local_translation)),
145
+ ]
146
+ )
147
+
148
+ @classmethod
149
+ def from_mjcf(cls, path: str) -> "SkeletonTree":
150
+ """
151
+ Parses a mujoco xml scene description file and returns a Skeleton Tree.
152
+ We use the model attribute at the root as the name of the tree.
153
+
154
+ :param path:
155
+ :type path: string
156
+ :return: The skeleton tree constructed from the mjcf file
157
+ :rtype: SkeletonTree
158
+ """
159
+ tree = ET.parse(path)
160
+ xml_doc_root = tree.getroot()
161
+ xml_world_body = xml_doc_root.find("worldbody")
162
+ if xml_world_body is None:
163
+ raise ValueError("MJCF parsed incorrectly please verify it.")
164
+ # assume this is the root
165
+ xml_body_root = xml_world_body.find("body")
166
+ if xml_body_root is None:
167
+ raise ValueError("MJCF parsed incorrectly please verify it.")
168
+
169
+ node_names = []
170
+ parent_indices = []
171
+ local_translation = []
172
+
173
+ # recursively adding all nodes into the skel_tree
174
+ def _add_xml_node(xml_node, parent_index, node_index):
175
+ node_name = xml_node.attrib.get("name")
176
+ # parse the local translation into float list
177
+ pos = np.fromstring(xml_node.attrib.get("pos"), dtype=float, sep=" ")
178
+ node_names.append(node_name)
179
+ parent_indices.append(parent_index)
180
+ local_translation.append(pos)
181
+ curr_index = node_index
182
+ node_index += 1
183
+ for next_node in xml_node.findall("body"):
184
+ node_index = _add_xml_node(next_node, curr_index, node_index)
185
+ return node_index
186
+
187
+ _add_xml_node(xml_body_root, -1, 0)
188
+
189
+ return cls(
190
+ node_names,
191
+ torch.from_numpy(np.array(parent_indices, dtype=np.int32)),
192
+ torch.from_numpy(np.array(local_translation, dtype=np.float32)),
193
+ )
194
+
195
+ def parent_of(self, node_name):
196
+ """ get the name of the parent of the given node
197
+
198
+ :param node_name: the name of the node
199
+ :type node_name: string
200
+ :rtype: string
201
+ """
202
+ return self[int(self.parent_indices[self.index(node_name)].item())]
203
+
204
+ def index(self, node_name):
205
+ """ get the index of the node
206
+
207
+ :param node_name: the name of the node
208
+ :type node_name: string
209
+ :rtype: int
210
+ """
211
+ return self._node_indices[node_name]
212
+
213
+ def drop_nodes_by_names(
214
+ self, node_names: List[str], pairwise_translation=None
215
+ ) -> "SkeletonTree":
216
+ new_length = len(self) - len(node_names)
217
+ new_node_names = []
218
+ new_local_translation = torch.zeros(
219
+ new_length, 3, dtype=self.local_translation.dtype
220
+ )
221
+ new_parent_indices = torch.zeros(new_length, dtype=self.parent_indices.dtype)
222
+ parent_indices = self.parent_indices.numpy()
223
+ new_node_indices: dict = {}
224
+ new_node_index = 0
225
+ for node_index in range(len(self)):
226
+ if self[node_index] in node_names:
227
+ continue
228
+ tb_node_index = parent_indices[node_index]
229
+ if tb_node_index != -1:
230
+ local_translation = self.local_translation[node_index, :]
231
+ while tb_node_index != -1 and self[tb_node_index] in node_names:
232
+ local_translation += self.local_translation[tb_node_index, :]
233
+ tb_node_index = parent_indices[tb_node_index]
234
+ assert tb_node_index != -1, "the root node cannot be dropped"
235
+
236
+ if pairwise_translation is not None:
237
+ local_translation = pairwise_translation[
238
+ tb_node_index, node_index, :
239
+ ]
240
+ else:
241
+ local_translation = self.local_translation[node_index, :]
242
+
243
+ new_node_names.append(self[node_index])
244
+ new_local_translation[new_node_index, :] = local_translation
245
+ if tb_node_index == -1:
246
+ new_parent_indices[new_node_index] = -1
247
+ else:
248
+ new_parent_indices[new_node_index] = new_node_indices[
249
+ self[tb_node_index]
250
+ ]
251
+ new_node_indices[self[node_index]] = new_node_index
252
+ new_node_index += 1
253
+
254
+ return SkeletonTree(new_node_names, new_parent_indices, new_local_translation)
255
+
256
+ def keep_nodes_by_names(
257
+ self, node_names: List[str], pairwise_translation=None
258
+ ) -> "SkeletonTree":
259
+ nodes_to_drop = list(filter(lambda x: x not in node_names, self))
260
+ return self.drop_nodes_by_names(nodes_to_drop, pairwise_translation)
261
+
262
+
263
+ class SkeletonState(Serializable):
264
+ """
265
+ A skeleton state contains all the information needed to describe a static state of a skeleton.
266
+ It requires a skeleton tree, local/global rotation at each joint and the root translation.
267
+
268
+ Example:
269
+ >>> t = SkeletonTree.from_mjcf(SkeletonTree.__example_mjcf_path__)
270
+ >>> zero_pose = SkeletonState.zero_pose(t)
271
+ >>> plot_skeleton_state(zero_pose) # can be imported from `.visualization.common`
272
+ [plot of the ant at zero pose
273
+ >>> local_rotation = zero_pose.local_rotation.clone()
274
+ >>> local_rotation[2] = torch.tensor([0, 0, 1, 0])
275
+ >>> new_pose = SkeletonState.from_rotation_and_root_translation(
276
+ ... skeleton_tree=t,
277
+ ... r=local_rotation,
278
+ ... t=zero_pose.root_translation,
279
+ ... is_local=True
280
+ ... )
281
+ >>> new_pose.local_rotation
282
+ tensor([[0., 0., 0., 1.],
283
+ [0., 0., 0., 1.],
284
+ [0., 1., 0., 0.],
285
+ [0., 0., 0., 1.],
286
+ [0., 0., 0., 1.],
287
+ [0., 0., 0., 1.],
288
+ [0., 0., 0., 1.],
289
+ [0., 0., 0., 1.],
290
+ [0., 0., 0., 1.],
291
+ [0., 0., 0., 1.],
292
+ [0., 0., 0., 1.],
293
+ [0., 0., 0., 1.],
294
+ [0., 0., 0., 1.]])
295
+ >>> plot_skeleton_state(new_pose) # you should be able to see one of ant's leg is bent
296
+ [plot of the ant with the new pose
297
+ >>> new_pose.global_rotation # the local rotation is propagated to the global rotation at joint #3
298
+ tensor([[0., 0., 0., 1.],
299
+ [0., 0., 0., 1.],
300
+ [0., 1., 0., 0.],
301
+ [0., 1., 0., 0.],
302
+ [0., 0., 0., 1.],
303
+ [0., 0., 0., 1.],
304
+ [0., 0., 0., 1.],
305
+ [0., 0., 0., 1.],
306
+ [0., 0., 0., 1.],
307
+ [0., 0., 0., 1.],
308
+ [0., 0., 0., 1.],
309
+ [0., 0., 0., 1.],
310
+ [0., 0., 0., 1.]])
311
+
312
+ Global/Local Representation (cont. from the previous example)
313
+ >>> new_pose.is_local
314
+ True
315
+ >>> new_pose.tensor # this will return the local rotation followed by the root translation
316
+ tensor([0., 0., 0., 1., 0., 0., 0., 1., 0., 1., 0., 0., 0., 0., 0., 1., 0., 0.,
317
+ 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1.,
318
+ 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0.,
319
+ 0.])
320
+ >>> new_pose.tensor.shape # 4 * 13 (joint rotation) + 3 (root translatio
321
+ torch.Size([55])
322
+ >>> new_pose.global_repr().is_local
323
+ False
324
+ >>> new_pose.global_repr().tensor # this will return the global rotation followed by the root translation instead
325
+ tensor([0., 0., 0., 1., 0., 0., 0., 1., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0.,
326
+ 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1.,
327
+ 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0.,
328
+ 0.])
329
+ >>> new_pose.global_repr().tensor.shape # 4 * 13 (joint rotation) + 3 (root translation
330
+ torch.Size([55])
331
+ """
332
+
333
+ def __init__(self, tensor_backend, skeleton_tree, is_local):
334
+ self._skeleton_tree = skeleton_tree
335
+ self._is_local = is_local
336
+ self.tensor = tensor_backend.clone()
337
+
338
+ def __len__(self):
339
+ return self.tensor.shape[0]
340
+
341
+ @property
342
+ def rotation(self):
343
+ if not hasattr(self, "_rotation"):
344
+ self._rotation = self.tensor[..., : self.num_joints * 4].reshape(
345
+ *(self.tensor.shape[:-1] + (self.num_joints, 4))
346
+ )
347
+ return self._rotation
348
+
349
+ @property
350
+ def _local_rotation(self):
351
+ if self._is_local:
352
+ return self.rotation
353
+ else:
354
+ return None
355
+
356
+ @property
357
+ def _global_rotation(self):
358
+ if not self._is_local:
359
+ return self.rotation
360
+ else:
361
+ return None
362
+
363
+ @property
364
+ def is_local(self):
365
+ """ is the rotation represented in local frame?
366
+
367
+ :rtype: bool
368
+ """
369
+ return self._is_local
370
+
371
+ @property
372
+ def invariant_property(self):
373
+ return {"skeleton_tree": self.skeleton_tree, "is_local": self.is_local}
374
+
375
+ @property
376
+ def num_joints(self):
377
+ """ number of joints in the skeleton tree
378
+
379
+ :rtype: int
380
+ """
381
+ return self.skeleton_tree.num_joints
382
+
383
+ @property
384
+ def skeleton_tree(self):
385
+ """ skeleton tree
386
+
387
+ :rtype: SkeletonTree
388
+ """
389
+ return self._skeleton_tree
390
+
391
+ @property
392
+ def root_translation(self):
393
+ """ root translation
394
+
395
+ :rtype: Tensor
396
+ """
397
+ if not hasattr(self, "_root_translation"):
398
+ self._root_translation = self.tensor[
399
+ ..., self.num_joints * 4 : self.num_joints * 4 + 3
400
+ ]
401
+ return self._root_translation
402
+
403
+ @property
404
+ def global_transformation(self):
405
+ """ global transformation of each joint (transform from joint frame to global frame) """
406
+ if not hasattr(self, "_global_transformation"):
407
+ local_transformation = self.local_transformation
408
+ global_transformation = []
409
+ parent_indices = self.skeleton_tree.parent_indices.numpy()
410
+ # global_transformation = local_transformation.identity_like()
411
+ for node_index in range(len(self.skeleton_tree)):
412
+ parent_index = parent_indices[node_index]
413
+ if parent_index == -1:
414
+ global_transformation.append(
415
+ local_transformation[..., node_index, :]
416
+ )
417
+ else:
418
+ global_transformation.append(
419
+ transform_mul(
420
+ global_transformation[parent_index],
421
+ local_transformation[..., node_index, :],
422
+ )
423
+ )
424
+ self._global_transformation = torch.stack(global_transformation, axis=-2)
425
+ return self._global_transformation
426
+
427
+ @property
428
+ def global_rotation(self):
429
+ """ global rotation of each joint (rotation matrix to rotate from joint's F.O.R to global
430
+ F.O.R) """
431
+ if self._global_rotation is None:
432
+ if not hasattr(self, "_comp_global_rotation"):
433
+ self._comp_global_rotation = transform_rotation(
434
+ self.global_transformation
435
+ )
436
+ return self._comp_global_rotation
437
+ else:
438
+ return self._global_rotation
439
+
440
+ @property
441
+ def global_translation(self):
442
+ """ global translation of each joint """
443
+ if not hasattr(self, "_global_translation"):
444
+ self._global_translation = transform_translation(self.global_transformation)
445
+ return self._global_translation
446
+
447
+ @property
448
+ def global_translation_xy(self):
449
+ """ global translation in xy """
450
+ trans_xy_data = self.global_translation.zeros_like()
451
+ trans_xy_data[..., 0:2] = self.global_translation[..., 0:2]
452
+ return trans_xy_data
453
+
454
+ @property
455
+ def global_translation_xz(self):
456
+ """ global translation in xz """
457
+ trans_xz_data = self.global_translation.zeros_like()
458
+ trans_xz_data[..., 0:1] = self.global_translation[..., 0:1]
459
+ trans_xz_data[..., 2:3] = self.global_translation[..., 2:3]
460
+ return trans_xz_data
461
+
462
+ @property
463
+ def local_rotation(self):
464
+ """ the rotation from child frame to parent frame given in the order of child nodes appeared
465
+ in `.skeleton_tree.node_names` """
466
+ if self._local_rotation is None:
467
+ if not hasattr(self, "_comp_local_rotation"):
468
+ local_rotation = quat_identity_like(self.global_rotation)
469
+ for node_index in range(len(self.skeleton_tree)):
470
+ parent_index = self.skeleton_tree.parent_indices[node_index]
471
+ if parent_index == -1:
472
+ local_rotation[..., node_index, :] = self.global_rotation[
473
+ ..., node_index, :
474
+ ]
475
+ else:
476
+ local_rotation[..., node_index, :] = quat_mul_norm(
477
+ quat_inverse(self.global_rotation[..., parent_index, :]),
478
+ self.global_rotation[..., node_index, :],
479
+ )
480
+ self._comp_local_rotation = local_rotation
481
+ return self._comp_local_rotation
482
+ else:
483
+ return self._local_rotation
484
+
485
+ @property
486
+ def local_transformation(self):
487
+ """ local translation + local rotation. It describes the transformation from child frame to
488
+ parent frame given in the order of child nodes appeared in `.skeleton_tree.node_names` """
489
+ if not hasattr(self, "_local_transformation"):
490
+ self._local_transformation = transform_from_rotation_translation(
491
+ r=self.local_rotation, t=self.local_translation
492
+ )
493
+ return self._local_transformation
494
+
495
+ @property
496
+ def local_translation(self):
497
+ """ local translation of the skeleton state. It is identical to the local translation in
498
+ `.skeleton_tree.local_translation` except the root translation. The root translation is
499
+ identical to `.root_translation` """
500
+ if not hasattr(self, "_local_translation"):
501
+ broadcast_shape = (
502
+ tuple(self.tensor.shape[:-1])
503
+ + (len(self.skeleton_tree),)
504
+ + tuple(self.skeleton_tree.local_translation.shape[-1:])
505
+ )
506
+ local_translation = self.skeleton_tree.local_translation.broadcast_to(
507
+ *broadcast_shape
508
+ ).clone()
509
+ local_translation[..., 0, :] = self.root_translation
510
+ self._local_translation = local_translation
511
+ return self._local_translation
512
+
513
+ # Root Properties
514
+ @property
515
+ def root_translation_xy(self):
516
+ """ root translation on xy """
517
+ if not hasattr(self, "_root_translation_xy"):
518
+ self._root_translation_xy = self.global_translation_xy[..., 0, :]
519
+ return self._root_translation_xy
520
+
521
+ @property
522
+ def global_root_rotation(self):
523
+ """ root rotation """
524
+ if not hasattr(self, "_global_root_rotation"):
525
+ self._global_root_rotation = self.global_rotation[..., 0, :]
526
+ return self._global_root_rotation
527
+
528
+ @property
529
+ def global_root_yaw_rotation(self):
530
+ """ root yaw rotation """
531
+ if not hasattr(self, "_global_root_yaw_rotation"):
532
+ self._global_root_yaw_rotation = self.global_root_rotation.yaw_rotation()
533
+ return self._global_root_yaw_rotation
534
+
535
+ # Properties relative to root
536
+ @property
537
+ def local_translation_to_root(self):
538
+ """ The 3D translation from joint frame to the root frame. """
539
+ if not hasattr(self, "_local_translation_to_root"):
540
+ self._local_translation_to_root = (
541
+ self.global_translation - self.root_translation.unsqueeze(-1)
542
+ )
543
+ return self._local_translation_to_root
544
+
545
+ @property
546
+ def local_rotation_to_root(self):
547
+ """ The 3D rotation from joint frame to the root frame. It is equivalent to
548
+ The root_R_world * world_R_node """
549
+ return (
550
+ quat_inverse(self.global_root_rotation).unsqueeze(-1) * self.global_rotation
551
+ )
552
+
553
+ def compute_forward_vector(
554
+ self,
555
+ left_shoulder_index,
556
+ right_shoulder_index,
557
+ left_hip_index,
558
+ right_hip_index,
559
+ gaussian_filter_width=20,
560
+ ):
561
+ """ Computes forward vector based on cross product of the up vector with
562
+ average of the right->left shoulder and hip vectors """
563
+ global_positions = self.global_translation
564
+ # Perpendicular to the forward direction.
565
+ # Uses the shoulders and hips to find this.
566
+ side_direction = (
567
+ global_positions[:, left_shoulder_index].numpy()
568
+ - global_positions[:, right_shoulder_index].numpy()
569
+ + global_positions[:, left_hip_index].numpy()
570
+ - global_positions[:, right_hip_index].numpy()
571
+ )
572
+ side_direction = (
573
+ side_direction
574
+ / np.sqrt((side_direction ** 2).sum(axis=-1))[..., np.newaxis]
575
+ )
576
+
577
+ # Forward direction obtained by crossing with the up direction.
578
+ forward_direction = np.cross(side_direction, np.array([[0, 1, 0]]))
579
+
580
+ # Smooth the forward direction with a Gaussian.
581
+ # Axis 0 is the time/frame axis.
582
+ forward_direction = filters.gaussian_filter1d(
583
+ forward_direction, gaussian_filter_width, axis=0, mode="nearest"
584
+ )
585
+ forward_direction = (
586
+ forward_direction
587
+ / np.sqrt((forward_direction ** 2).sum(axis=-1))[..., np.newaxis]
588
+ )
589
+
590
+ return torch.from_numpy(forward_direction)
591
+
592
+ @staticmethod
593
+ def _to_state_vector(rot, rt):
594
+ state_shape = rot.shape[:-2]
595
+ vr = rot.reshape(*(state_shape + (-1,)))
596
+ vt = rt.broadcast_to(*state_shape + rt.shape[-1:]).reshape(
597
+ *(state_shape + (-1,))
598
+ )
599
+ v = torch.cat([vr, vt], axis=-1)
600
+ return v
601
+
602
+ @classmethod
603
+ def from_dict(
604
+ cls: Type["SkeletonState"], dict_repr: OrderedDict, *args, **kwargs
605
+ ) -> "SkeletonState":
606
+ rot = TensorUtils.from_dict(dict_repr["rotation"], *args, **kwargs)
607
+ rt = TensorUtils.from_dict(dict_repr["root_translation"], *args, **kwargs)
608
+ return cls(
609
+ SkeletonState._to_state_vector(rot, rt),
610
+ SkeletonTree.from_dict(dict_repr["skeleton_tree"], *args, **kwargs),
611
+ dict_repr["is_local"],
612
+ )
613
+
614
+ def to_dict(self) -> OrderedDict:
615
+ return OrderedDict(
616
+ [
617
+ ("rotation", tensor_to_dict(self.rotation)),
618
+ ("root_translation", tensor_to_dict(self.root_translation)),
619
+ ("skeleton_tree", self.skeleton_tree.to_dict()),
620
+ ("is_local", self.is_local),
621
+ ]
622
+ )
623
+
624
+ def to_retarget_motion_file(self, path: str) -> None:
625
+ """
626
+ Save the motion for the retargeting.
627
+ """
628
+ global_positions = quat_rotate(
629
+ torch.tensor([0.70711, 0, 0, 0.70711]),
630
+ self.global_translation
631
+ ).detach().cpu().numpy() / 100 # cm -> m
632
+ global_quaternions = quat_mul_norm(
633
+ torch.tensor([0.70711, 0, 0, 0.70711]), self.global_rotation
634
+ ).detach().cpu().numpy() # y-up -> z-up
635
+ joint_names = self.skeleton_tree.node_names
636
+
637
+ num_frames = global_positions.shape[0]
638
+ num_joints = len(joint_names)
639
+ data = []
640
+
641
+ for frame in range(num_frames):
642
+ motion = {}
643
+ for i in range(num_joints):
644
+ motion[joint_names[i].split('_')[1]] = [
645
+ global_positions[frame, i].tolist(),
646
+ global_quaternions[frame, i, [3, 0, 1, 2]].tolist()
647
+ ]
648
+ data.append(motion)
649
+
650
+ with open(path, "wb") as f:
651
+ pickle.dump(data, f)
652
+
653
+ @classmethod
654
+ def from_rotation_and_root_translation(cls, skeleton_tree, r, t, is_local=True):
655
+ """
656
+ Construct a skeleton state from rotation and root translation
657
+
658
+ :param skeleton_tree: the skeleton tree
659
+ :type skeleton_tree: SkeletonTree
660
+ :param r: rotation (either global or local)
661
+ :type r: Tensor
662
+ :param t: root translation
663
+ :type t: Tensor
664
+ :param is_local: to indicate that whether the rotation is local or global
665
+ :type is_local: bool, optional, default=True
666
+ """
667
+ assert (
668
+ r.dim() > 0
669
+ ), "the rotation needs to have at least 1 dimension (dim = {})".format(r.dim)
670
+ return cls(
671
+ SkeletonState._to_state_vector(r, t),
672
+ skeleton_tree=skeleton_tree,
673
+ is_local=is_local,
674
+ )
675
+
676
+ @classmethod
677
+ def zero_pose(cls, skeleton_tree):
678
+ """
679
+ Construct a zero-pose skeleton state from the skeleton tree by assuming that all the local
680
+ rotation is 0 and root translation is also 0.
681
+
682
+ :param skeleton_tree: the skeleton tree as the rigid body
683
+ :type skeleton_tree: SkeletonTree
684
+ """
685
+ return cls.from_rotation_and_root_translation(
686
+ skeleton_tree=skeleton_tree,
687
+ r=quat_identity([skeleton_tree.num_joints]),
688
+ t=torch.zeros(3, dtype=skeleton_tree.local_translation.dtype),
689
+ is_local=True,
690
+ )
691
+
692
+ def local_repr(self):
693
+ """
694
+ Convert the skeleton state into local representation. This will only affects the values of
695
+ .tensor. If the skeleton state already has `is_local=True`. This method will do nothing.
696
+
697
+ :rtype: SkeletonState
698
+ """
699
+ if self.is_local:
700
+ return self
701
+ return SkeletonState.from_rotation_and_root_translation(
702
+ self.skeleton_tree,
703
+ r=self.local_rotation,
704
+ t=self.root_translation,
705
+ is_local=True,
706
+ )
707
+
708
+ def global_repr(self):
709
+ """
710
+ Convert the skeleton state into global representation. This will only affects the values of
711
+ .tensor. If the skeleton state already has `is_local=False`. This method will do nothing.
712
+
713
+ :rtype: SkeletonState
714
+ """
715
+ if not self.is_local:
716
+ return self
717
+ return SkeletonState.from_rotation_and_root_translation(
718
+ self.skeleton_tree,
719
+ r=self.global_rotation,
720
+ t=self.root_translation,
721
+ is_local=False,
722
+ )
723
+
724
+ def _get_pairwise_average_translation(self):
725
+ global_transform_inv = transform_inverse(self.global_transformation)
726
+ p1 = global_transform_inv.unsqueeze(-2)
727
+ p2 = self.global_transformation.unsqueeze(-3)
728
+
729
+ pairwise_translation = (
730
+ transform_translation(transform_mul(p1, p2))
731
+ .reshape(-1, len(self.skeleton_tree), len(self.skeleton_tree), 3)
732
+ .mean(axis=0)
733
+ )
734
+ return pairwise_translation
735
+
736
+ def _transfer_to(self, new_skeleton_tree: SkeletonTree):
737
+ old_indices = list(map(self.skeleton_tree.index, new_skeleton_tree))
738
+ return SkeletonState.from_rotation_and_root_translation(
739
+ new_skeleton_tree,
740
+ r=self.global_rotation[..., old_indices, :],
741
+ t=self.root_translation,
742
+ is_local=False,
743
+ )
744
+
745
+ def drop_nodes_by_names(
746
+ self, node_names: List[str], estimate_local_translation_from_states: bool = True
747
+ ) -> "SkeletonState":
748
+ """
749
+ Drop a list of nodes from the skeleton and re-compute the local rotation to match the
750
+ original joint position as much as possible.
751
+
752
+ :param node_names: a list node names that specifies the nodes need to be dropped
753
+ :type node_names: List of strings
754
+ :param estimate_local_translation_from_states: the boolean indicator that specifies whether\
755
+ or not to re-estimate the local translation from the states (avg.)
756
+ :type estimate_local_translation_from_states: boolean
757
+ :rtype: SkeletonState
758
+ """
759
+ if estimate_local_translation_from_states:
760
+ pairwise_translation = self._get_pairwise_average_translation()
761
+ else:
762
+ pairwise_translation = None
763
+ new_skeleton_tree = self.skeleton_tree.drop_nodes_by_names(
764
+ node_names, pairwise_translation
765
+ )
766
+ return self._transfer_to(new_skeleton_tree)
767
+
768
+ def keep_nodes_by_names(
769
+ self, node_names: List[str], estimate_local_translation_from_states: bool = True
770
+ ) -> "SkeletonState":
771
+ """
772
+ Keep a list of nodes and drop all other nodes from the skeleton and re-compute the local
773
+ rotation to match the original joint position as much as possible.
774
+
775
+ :param node_names: a list node names that specifies the nodes need to be dropped
776
+ :type node_names: List of strings
777
+ :param estimate_local_translation_from_states: the boolean indicator that specifies whether\
778
+ or not to re-estimate the local translation from the states (avg.)
779
+ :type estimate_local_translation_from_states: boolean
780
+ :rtype: SkeletonState
781
+ """
782
+ return self.drop_nodes_by_names(
783
+ list(filter(lambda x: (x not in node_names), self)),
784
+ estimate_local_translation_from_states,
785
+ )
786
+
787
+ def _remapped_to(
788
+ self, joint_mapping: Dict[str, str], target_skeleton_tree: SkeletonTree
789
+ ):
790
+ joint_mapping_inv = {target: source for source, target in joint_mapping.items()}
791
+ reduced_target_skeleton_tree = target_skeleton_tree.keep_nodes_by_names(
792
+ list(joint_mapping_inv)
793
+ )
794
+ n_joints = (
795
+ len(joint_mapping),
796
+ len(self.skeleton_tree),
797
+ len(reduced_target_skeleton_tree),
798
+ )
799
+ assert (
800
+ len(set(n_joints)) == 1
801
+ ), "the joint mapping is not consistent with the skeleton trees"
802
+ source_indices = list(
803
+ map(
804
+ lambda x: self.skeleton_tree.index(joint_mapping_inv[x]),
805
+ reduced_target_skeleton_tree,
806
+ )
807
+ )
808
+ target_local_rotation = self.local_rotation[..., source_indices, :]
809
+ return SkeletonState.from_rotation_and_root_translation(
810
+ skeleton_tree=reduced_target_skeleton_tree,
811
+ r=target_local_rotation,
812
+ t=self.root_translation,
813
+ is_local=True,
814
+ )
815
+
816
+ def retarget_to(
817
+ self,
818
+ joint_mapping: Dict[str, str],
819
+ source_tpose_local_rotation,
820
+ source_tpose_root_translation: np.ndarray,
821
+ target_skeleton_tree: SkeletonTree,
822
+ target_tpose_local_rotation,
823
+ target_tpose_root_translation: np.ndarray,
824
+ rotation_to_target_skeleton,
825
+ scale_to_target_skeleton: float,
826
+ z_up: bool = True,
827
+ ) -> "SkeletonState":
828
+ """
829
+ Retarget the skeleton state to a target skeleton tree. This is a naive retarget
830
+ implementation with rough approximations. The function follows the procedures below.
831
+
832
+ Steps:
833
+ 1. Drop the joints from the source (self) that do not belong to the joint mapping\
834
+ with an implementation that is similar to "keep_nodes_by_names()" - take a\
835
+ look at the function doc for more details (same for source_tpose)
836
+
837
+ 2. Rotate the source state and the source tpose by "rotation_to_target_skeleton"\
838
+ to align the source with the target orientation
839
+
840
+ 3. Extract the root translation and normalize it to match the scale of the target\
841
+ skeleton
842
+
843
+ 4. Extract the global rotation from source state relative to source tpose and\
844
+ re-apply the relative rotation to the target tpose to construct the global\
845
+ rotation after retargetting
846
+
847
+ 5. Combine the computed global rotation and the root translation from 3 and 4 to\
848
+ complete the retargeting.
849
+
850
+ 6. Make feet on the ground (global translation z)
851
+
852
+ :param joint_mapping: a dictionary of that maps the joint node from the source skeleton to \
853
+ the target skeleton
854
+ :type joint_mapping: Dict[str, str]
855
+
856
+ :param source_tpose_local_rotation: the local rotation of the source skeleton
857
+ :type source_tpose_local_rotation: Tensor
858
+
859
+ :param source_tpose_root_translation: the root translation of the source tpose
860
+ :type source_tpose_root_translation: np.ndarray
861
+
862
+ :param target_skeleton_tree: the target skeleton tree
863
+ :type target_skeleton_tree: SkeletonTree
864
+
865
+ :param target_tpose_local_rotation: the local rotation of the target skeleton
866
+ :type target_tpose_local_rotation: Tensor
867
+
868
+ :param target_tpose_root_translation: the root translation of the target tpose
869
+ :type target_tpose_root_translation: Tensor
870
+
871
+ :param rotation_to_target_skeleton: the rotation that needs to be applied to the source\
872
+ skeleton to align with the target skeleton. Essentially the rotation is t_R_s, where t is\
873
+ the frame of reference of the target skeleton and s is the frame of reference of the source\
874
+ skeleton
875
+ :type rotation_to_target_skeleton: Tensor
876
+ :param scale_to_target_skeleton: the factor that needs to be multiplied from source\
877
+ skeleton to target skeleton (unit in distance). For example, to go from `cm` to `m`, the \
878
+ factor needs to be 0.01.
879
+ :type scale_to_target_skeleton: float
880
+ :rtype: SkeletonState
881
+ """
882
+
883
+ # STEP 0: Preprocess
884
+ source_tpose = SkeletonState.from_rotation_and_root_translation(
885
+ skeleton_tree=self.skeleton_tree,
886
+ r=source_tpose_local_rotation,
887
+ t=source_tpose_root_translation,
888
+ is_local=True,
889
+ )
890
+ target_tpose = SkeletonState.from_rotation_and_root_translation(
891
+ skeleton_tree=target_skeleton_tree,
892
+ r=target_tpose_local_rotation,
893
+ t=target_tpose_root_translation,
894
+ is_local=True,
895
+ )
896
+
897
+ # STEP 1: Drop the irrelevant joints
898
+ pairwise_translation = self._get_pairwise_average_translation()
899
+ node_names = list(joint_mapping)
900
+ new_skeleton_tree = self.skeleton_tree.keep_nodes_by_names(
901
+ node_names, pairwise_translation
902
+ )
903
+
904
+ # TODO: combine the following steps before STEP 3
905
+ source_tpose = source_tpose._transfer_to(new_skeleton_tree)
906
+ source_state = self._transfer_to(new_skeleton_tree)
907
+
908
+ source_tpose = source_tpose._remapped_to(joint_mapping, target_skeleton_tree)
909
+ source_state = source_state._remapped_to(joint_mapping, target_skeleton_tree)
910
+
911
+ # STEP 2: Rotate the source to align with the target
912
+ new_local_rotation = source_tpose.local_rotation.clone()
913
+ new_local_rotation[..., 0, :] = quat_mul_norm(
914
+ rotation_to_target_skeleton, source_tpose.local_rotation[..., 0, :]
915
+ )
916
+
917
+ source_tpose = SkeletonState.from_rotation_and_root_translation(
918
+ skeleton_tree=source_tpose.skeleton_tree,
919
+ r=new_local_rotation,
920
+ t=quat_rotate(rotation_to_target_skeleton, source_tpose.root_translation),
921
+ is_local=True,
922
+ )
923
+
924
+ new_local_rotation = source_state.local_rotation.clone()
925
+ new_local_rotation[..., 0, :] = quat_mul_norm(
926
+ rotation_to_target_skeleton, source_state.local_rotation[..., 0, :]
927
+ )
928
+ source_state = SkeletonState.from_rotation_and_root_translation(
929
+ skeleton_tree=source_state.skeleton_tree,
930
+ r=new_local_rotation,
931
+ t=quat_rotate(rotation_to_target_skeleton, source_state.root_translation),
932
+ is_local=True,
933
+ )
934
+
935
+ # STEP 3: Normalize to match the target scale
936
+ root_translation_diff = (
937
+ source_state.root_translation - source_tpose.root_translation
938
+ ) * scale_to_target_skeleton
939
+ # STEP 4: the global rotation from source state relative to source tpose and
940
+ # re-apply to the target
941
+ current_skeleton_tree = source_state.skeleton_tree
942
+ target_tpose_global_rotation = source_state.global_rotation[0, :].clone()
943
+ for current_index, name in enumerate(current_skeleton_tree):
944
+ if name in target_tpose.skeleton_tree:
945
+ target_tpose_global_rotation[
946
+ current_index, :
947
+ ] = target_tpose.global_rotation[
948
+ target_tpose.skeleton_tree.index(name), :
949
+ ]
950
+
951
+ global_rotation_diff = quat_mul_norm(
952
+ source_state.global_rotation, quat_inverse(source_tpose.global_rotation)
953
+ )
954
+ new_global_rotation = quat_mul_norm(
955
+ global_rotation_diff, target_tpose_global_rotation
956
+ )
957
+
958
+ # STEP 5: Putting 3 and 4 together
959
+ current_skeleton_tree = source_state.skeleton_tree
960
+ shape = source_state.global_rotation.shape[:-1]
961
+ shape = shape[:-1] + target_tpose.global_rotation.shape[-2:-1]
962
+ new_global_rotation_output = quat_identity(shape)
963
+ for current_index, name in enumerate(target_skeleton_tree):
964
+ while name not in current_skeleton_tree:
965
+ name = target_skeleton_tree.parent_of(name)
966
+ parent_index = current_skeleton_tree.index(name)
967
+ new_global_rotation_output[:, current_index, :] = new_global_rotation[
968
+ :, parent_index, :
969
+ ]
970
+
971
+ source_state = SkeletonState.from_rotation_and_root_translation(
972
+ skeleton_tree=target_skeleton_tree,
973
+ r=new_global_rotation_output,
974
+ t=target_tpose.root_translation + root_translation_diff,
975
+ is_local=False,
976
+ ).local_repr()
977
+
978
+ return source_state
979
+
980
+ def retarget_to_by_tpose(
981
+ self,
982
+ joint_mapping: Dict[str, str],
983
+ source_tpose: "SkeletonState",
984
+ target_tpose: "SkeletonState",
985
+ rotation_to_target_skeleton,
986
+ scale_to_target_skeleton: float,
987
+ ) -> "SkeletonState":
988
+ """
989
+ Retarget the skeleton state to a target skeleton tree. This is a naive retarget
990
+ implementation with rough approximations. See the method `retarget_to()` for more information
991
+
992
+ :param joint_mapping: a dictionary of that maps the joint node from the source skeleton to \
993
+ the target skeleton
994
+ :type joint_mapping: Dict[str, str]
995
+
996
+ :param source_tpose: t-pose of the source skeleton
997
+ :type source_tpose: SkeletonState
998
+
999
+ :param target_tpose: t-pose of the target skeleton
1000
+ :type target_tpose: SkeletonState
1001
+
1002
+ :param rotation_to_target_skeleton: the rotation that needs to be applied to the source\
1003
+ skeleton to align with the target skeleton. Essentially the rotation is t_R_s, where t is\
1004
+ the frame of reference of the target skeleton and s is the frame of reference of the source\
1005
+ skeleton
1006
+ :type rotation_to_target_skeleton: Tensor
1007
+ :param scale_to_target_skeleton: the factor that needs to be multiplied from source\
1008
+ skeleton to target skeleton (unit in distance). For example, to go from `cm` to `m`, the \
1009
+ factor needs to be 0.01.
1010
+ :type scale_to_target_skeleton: float
1011
+ :rtype: SkeletonState
1012
+ """
1013
+ assert (
1014
+ len(source_tpose.shape) == 0 and len(target_tpose.shape) == 0
1015
+ ), "the retargeting script currently doesn't support vectorized operations"
1016
+ return self.retarget_to(
1017
+ joint_mapping,
1018
+ source_tpose.local_rotation,
1019
+ source_tpose.root_translation,
1020
+ target_tpose.skeleton_tree,
1021
+ target_tpose.local_rotation,
1022
+ target_tpose.root_translation,
1023
+ rotation_to_target_skeleton,
1024
+ scale_to_target_skeleton,
1025
+ )
1026
+
1027
+
1028
+ class SkeletonMotion(SkeletonState):
1029
+ def __init__(self, tensor_backend, skeleton_tree, is_local, fps, *args, **kwargs):
1030
+ self._fps = fps
1031
+ super().__init__(tensor_backend, skeleton_tree, is_local, *args, **kwargs)
1032
+
1033
+ def clone(self):
1034
+ return SkeletonMotion(
1035
+ self.tensor.clone(), self.skeleton_tree, self._is_local, self._fps
1036
+ )
1037
+
1038
+ @property
1039
+ def invariant_property(self):
1040
+ return {
1041
+ "skeleton_tree": self.skeleton_tree,
1042
+ "is_local": self.is_local,
1043
+ "fps": self.fps,
1044
+ }
1045
+
1046
+ @property
1047
+ def global_velocity(self):
1048
+ """ global velocity """
1049
+ curr_index = self.num_joints * 4 + 3
1050
+ return self.tensor[..., curr_index : curr_index + self.num_joints * 3].reshape(
1051
+ *(self.tensor.shape[:-1] + (self.num_joints, 3))
1052
+ )
1053
+
1054
+ @property
1055
+ def global_angular_velocity(self):
1056
+ """ global angular velocity """
1057
+ curr_index = self.num_joints * 7 + 3
1058
+ return self.tensor[..., curr_index : curr_index + self.num_joints * 3].reshape(
1059
+ *(self.tensor.shape[:-1] + (self.num_joints, 3))
1060
+ )
1061
+
1062
+ @property
1063
+ def fps(self):
1064
+ """ number of frames per second """
1065
+ return self._fps
1066
+
1067
+ @property
1068
+ def time_delta(self):
1069
+ """ time between two adjacent frames """
1070
+ return 1.0 / self.fps
1071
+
1072
+ @property
1073
+ def global_root_velocity(self):
1074
+ """ global root velocity """
1075
+ return self.global_velocity[..., 0, :]
1076
+
1077
+ @property
1078
+ def global_root_angular_velocity(self):
1079
+ """ global root angular velocity """
1080
+ return self.global_angular_velocity[..., 0, :]
1081
+
1082
+ @classmethod
1083
+ def from_state_vector_and_velocity(
1084
+ cls,
1085
+ skeleton_tree,
1086
+ state_vector,
1087
+ global_velocity,
1088
+ global_angular_velocity,
1089
+ is_local,
1090
+ fps,
1091
+ ):
1092
+ """
1093
+ Construct a skeleton motion from a skeleton state vector, global velocity and angular
1094
+ velocity at each joint.
1095
+
1096
+ :param skeleton_tree: the skeleton tree that the motion is based on
1097
+ :type skeleton_tree: SkeletonTree
1098
+ :param state_vector: the state vector from the skeleton state by `.tensor`
1099
+ :type state_vector: Tensor
1100
+ :param global_velocity: the global velocity at each joint
1101
+ :type global_velocity: Tensor
1102
+ :param global_angular_velocity: the global angular velocity at each joint
1103
+ :type global_angular_velocity: Tensor
1104
+ :param is_local: if the rotation ins the state vector is given in local frame
1105
+ :type is_local: boolean
1106
+ :param fps: number of frames per second
1107
+ :type fps: int
1108
+
1109
+ :rtype: SkeletonMotion
1110
+ """
1111
+ state_shape = state_vector.shape[:-1]
1112
+ v = global_velocity.reshape(*(state_shape + (-1,)))
1113
+ av = global_angular_velocity.reshape(*(state_shape + (-1,)))
1114
+ new_state_vector = torch.cat([state_vector, v, av], axis=-1)
1115
+ return cls(
1116
+ new_state_vector, skeleton_tree=skeleton_tree, is_local=is_local, fps=fps,
1117
+ )
1118
+
1119
+ @classmethod
1120
+ def from_skeleton_state(
1121
+ cls: Type["SkeletonMotion"], skeleton_state: SkeletonState, fps: int
1122
+ ):
1123
+ """
1124
+ Construct a skeleton motion from a skeleton state. The velocities are estimated using second
1125
+ order gaussian filter along the last axis. The skeleton state must have at least .dim >= 1
1126
+
1127
+ :param skeleton_state: the skeleton state that the motion is based on
1128
+ :type skeleton_state: SkeletonState
1129
+ :param fps: number of frames per second
1130
+ :type fps: int
1131
+
1132
+ :rtype: SkeletonMotion
1133
+ """
1134
+ assert (
1135
+ type(skeleton_state) == SkeletonState
1136
+ ), "expected type of {}, got {}".format(SkeletonState, type(skeleton_state))
1137
+ global_velocity = SkeletonMotion._compute_velocity(
1138
+ p=skeleton_state.global_translation, time_delta=1 / fps
1139
+ )
1140
+ global_angular_velocity = SkeletonMotion._compute_angular_velocity(
1141
+ r=skeleton_state.global_rotation, time_delta=1 / fps
1142
+ )
1143
+ return cls.from_state_vector_and_velocity(
1144
+ skeleton_tree=skeleton_state.skeleton_tree,
1145
+ state_vector=skeleton_state.tensor,
1146
+ global_velocity=global_velocity,
1147
+ global_angular_velocity=global_angular_velocity,
1148
+ is_local=skeleton_state.is_local,
1149
+ fps=fps,
1150
+ )
1151
+
1152
+ @staticmethod
1153
+ def _to_state_vector(rot, rt, vel, avel):
1154
+ state_shape = rot.shape[:-2]
1155
+ skeleton_state_v = SkeletonState._to_state_vector(rot, rt)
1156
+ v = vel.reshape(*(state_shape + (-1,)))
1157
+ av = avel.reshape(*(state_shape + (-1,)))
1158
+ skeleton_motion_v = torch.cat([skeleton_state_v, v, av], axis=-1)
1159
+ return skeleton_motion_v
1160
+
1161
+ @classmethod
1162
+ def from_dict(
1163
+ cls: Type["SkeletonMotion"], dict_repr: OrderedDict, *args, **kwargs
1164
+ ) -> "SkeletonMotion":
1165
+ rot = TensorUtils.from_dict(dict_repr["rotation"], *args, **kwargs)
1166
+ rt = TensorUtils.from_dict(dict_repr["root_translation"], *args, **kwargs)
1167
+ vel = TensorUtils.from_dict(dict_repr["global_velocity"], *args, **kwargs)
1168
+ avel = TensorUtils.from_dict(
1169
+ dict_repr["global_angular_velocity"], *args, **kwargs
1170
+ )
1171
+ return cls(
1172
+ SkeletonMotion._to_state_vector(rot, rt, vel, avel),
1173
+ skeleton_tree=SkeletonTree.from_dict(
1174
+ dict_repr["skeleton_tree"], *args, **kwargs
1175
+ ),
1176
+ is_local=dict_repr["is_local"],
1177
+ fps=dict_repr["fps"],
1178
+ )
1179
+
1180
+ def to_dict(self) -> OrderedDict:
1181
+ return OrderedDict(
1182
+ [
1183
+ ("rotation", tensor_to_dict(self.rotation)),
1184
+ ("root_translation", tensor_to_dict(self.root_translation)),
1185
+ ("global_velocity", tensor_to_dict(self.global_velocity)),
1186
+ ("global_angular_velocity", tensor_to_dict(self.global_angular_velocity)),
1187
+ ("skeleton_tree", self.skeleton_tree.to_dict()),
1188
+ ("is_local", self.is_local),
1189
+ ("fps", self.fps),
1190
+ ]
1191
+ )
1192
+
1193
+ @classmethod
1194
+ def from_fbx(
1195
+ cls: Type["SkeletonMotion"],
1196
+ fbx_file_path,
1197
+ skeleton_tree=None,
1198
+ is_local=True,
1199
+ fps=120,
1200
+ root_joint="",
1201
+ root_trans_index=0,
1202
+ *args,
1203
+ **kwargs,
1204
+ ) -> "SkeletonMotion":
1205
+ """
1206
+ Construct a skeleton motion from a fbx file (TODO - generalize this). If the skeleton tree
1207
+ is not given, it will use the first frame of the mocap to construct the skeleton tree.
1208
+
1209
+ :param fbx_file_path: the path of the fbx file
1210
+ :type fbx_file_path: string
1211
+ :param fbx_configs: the configuration in terms of {"tmp_path": ..., "fbx_py27_path": ...}
1212
+ :type fbx_configs: dict
1213
+ :param skeleton_tree: the optional skeleton tree that the rotation will be applied to
1214
+ :type skeleton_tree: SkeletonTree, optional
1215
+ :param is_local: the state vector uses local or global rotation as the representation
1216
+ :type is_local: bool, optional, default=True
1217
+ :param fps: FPS of the FBX animation
1218
+ :type fps: int, optional, default=120
1219
+ :param root_joint: the name of the root joint for the skeleton
1220
+ :type root_joint: string, optional, default="" or the first node in the FBX scene with animation data
1221
+ :param root_trans_index: index of joint to extract root transform from
1222
+ :type root_trans_index: int, optional, default=0 or the root joint in the parsed skeleton
1223
+ :rtype: SkeletonMotion
1224
+ """
1225
+ joint_names, joint_parents, transforms, fps = fbx_to_array(
1226
+ fbx_file_path, root_joint, fps
1227
+ )
1228
+ # swap the last two axis to match the convention
1229
+ local_transform = euclidean_to_transform(
1230
+ transformation_matrix=torch.from_numpy(
1231
+ np.swapaxes(np.array(transforms), -1, -2),
1232
+ ).float()
1233
+ )
1234
+ local_rotation = transform_rotation(local_transform)
1235
+ root_translation = transform_translation(local_transform)[..., root_trans_index, :]
1236
+ joint_parents = torch.from_numpy(np.array(joint_parents)).int()
1237
+
1238
+ if skeleton_tree is None:
1239
+ local_translation = transform_translation(local_transform).reshape(
1240
+ -1, len(joint_parents), 3
1241
+ )[0]
1242
+ skeleton_tree = SkeletonTree(joint_names, joint_parents, local_translation)
1243
+ skeleton_state = SkeletonState.from_rotation_and_root_translation(
1244
+ skeleton_tree, r=local_rotation, t=root_translation, is_local=True
1245
+ )
1246
+ if not is_local:
1247
+ skeleton_state = skeleton_state.global_repr()
1248
+ return cls.from_skeleton_state(
1249
+ skeleton_state=skeleton_state, fps=fps
1250
+ )
1251
+
1252
+ @staticmethod
1253
+ def _compute_velocity(p, time_delta, guassian_filter=True):
1254
+ velocity = torch.from_numpy(
1255
+ filters.gaussian_filter1d(
1256
+ np.gradient(p.numpy(), axis=-3), 2, axis=-3, mode="nearest"
1257
+ )
1258
+ / time_delta,
1259
+ )
1260
+ return velocity
1261
+
1262
+ @staticmethod
1263
+ def _compute_angular_velocity(r, time_delta: float, guassian_filter=True):
1264
+ # assume the second last dimension is the time axis
1265
+ diff_quat_data = quat_identity_like(r)
1266
+ diff_quat_data[..., :-1, :, :] = quat_mul_norm(
1267
+ r[..., 1:, :, :], quat_inverse(r[..., :-1, :, :])
1268
+ )
1269
+ diff_angle, diff_axis = quat_angle_axis(diff_quat_data)
1270
+ angular_velocity = diff_axis * diff_angle.unsqueeze(-1) / time_delta
1271
+ angular_velocity = torch.from_numpy(
1272
+ filters.gaussian_filter1d(
1273
+ angular_velocity.numpy(), 2, axis=-3, mode="nearest"
1274
+ ),
1275
+ )
1276
+ return angular_velocity
1277
+
1278
+ def crop(self, start: int, end: int, fps: Optional[int] = None):
1279
+ """
1280
+ Crop the motion along its last axis. This is equivalent to performing a slicing on the
1281
+ object with [..., start: end: skip_every] where skip_every = old_fps / fps. Note that the
1282
+ new fps provided must be a factor of the original fps.
1283
+
1284
+ :param start: the beginning frame index
1285
+ :type start: int
1286
+ :param end: the ending frame index
1287
+ :type end: int
1288
+ :param fps: number of frames per second in the output (if not given the original fps will be used)
1289
+ :type fps: int, optional
1290
+ :rtype: SkeletonMotion
1291
+ """
1292
+ if fps is None:
1293
+ new_fps = int(self.fps)
1294
+ old_fps = int(self.fps)
1295
+ else:
1296
+ new_fps = int(fps)
1297
+ old_fps = int(self.fps)
1298
+ assert old_fps % fps == 0, (
1299
+ "the resampling doesn't support fps with non-integer division "
1300
+ "from the original fps: {} => {}".format(old_fps, fps)
1301
+ )
1302
+ skip_every = old_fps // new_fps
1303
+ return SkeletonMotion.from_skeleton_state(
1304
+ SkeletonState.from_rotation_and_root_translation(
1305
+ skeleton_tree=self.skeleton_tree,
1306
+ t=self.root_translation[start:end:skip_every],
1307
+ r=self.local_rotation[start:end:skip_every],
1308
+ is_local=True
1309
+ ),
1310
+ fps=self.fps
1311
+ )
1312
+
1313
+ def retarget_to(
1314
+ self,
1315
+ joint_mapping: Dict[str, str],
1316
+ source_tpose_local_rotation,
1317
+ source_tpose_root_translation: np.ndarray,
1318
+ target_skeleton_tree: "SkeletonTree",
1319
+ target_tpose_local_rotation,
1320
+ target_tpose_root_translation: np.ndarray,
1321
+ rotation_to_target_skeleton,
1322
+ scale_to_target_skeleton: float,
1323
+ z_up: bool = True,
1324
+ ) -> "SkeletonMotion":
1325
+ """
1326
+ Same as the one in :class:`SkeletonState`. This method discards all velocity information before
1327
+ retargeting and re-estimate the velocity after the retargeting. The same fps is used in the
1328
+ new retargetted motion.
1329
+
1330
+ :param joint_mapping: a dictionary of that maps the joint node from the source skeleton to \
1331
+ the target skeleton
1332
+ :type joint_mapping: Dict[str, str]
1333
+
1334
+ :param source_tpose_local_rotation: the local rotation of the source skeleton
1335
+ :type source_tpose_local_rotation: Tensor
1336
+
1337
+ :param source_tpose_root_translation: the root translation of the source tpose
1338
+ :type source_tpose_root_translation: np.ndarray
1339
+
1340
+ :param target_skeleton_tree: the target skeleton tree
1341
+ :type target_skeleton_tree: SkeletonTree
1342
+
1343
+ :param target_tpose_local_rotation: the local rotation of the target skeleton
1344
+ :type target_tpose_local_rotation: Tensor
1345
+
1346
+ :param target_tpose_root_translation: the root translation of the target tpose
1347
+ :type target_tpose_root_translation: Tensor
1348
+
1349
+ :param rotation_to_target_skeleton: the rotation that needs to be applied to the source\
1350
+ skeleton to align with the target skeleton. Essentially the rotation is t_R_s, where t is\
1351
+ the frame of reference of the target skeleton and s is the frame of reference of the source\
1352
+ skeleton
1353
+ :type rotation_to_target_skeleton: Tensor
1354
+ :param scale_to_target_skeleton: the factor that needs to be multiplied from source\
1355
+ skeleton to target skeleton (unit in distance). For example, to go from `cm` to `m`, the \
1356
+ factor needs to be 0.01.
1357
+ :type scale_to_target_skeleton: float
1358
+ :rtype: SkeletonMotion
1359
+ """
1360
+ return SkeletonMotion.from_skeleton_state(
1361
+ super().retarget_to(
1362
+ joint_mapping,
1363
+ source_tpose_local_rotation,
1364
+ source_tpose_root_translation,
1365
+ target_skeleton_tree,
1366
+ target_tpose_local_rotation,
1367
+ target_tpose_root_translation,
1368
+ rotation_to_target_skeleton,
1369
+ scale_to_target_skeleton,
1370
+ z_up,
1371
+ ),
1372
+ self.fps,
1373
+ )
1374
+
1375
+ def retarget_to_by_tpose(
1376
+ self,
1377
+ joint_mapping: Dict[str, str],
1378
+ source_tpose: "SkeletonState",
1379
+ target_tpose: "SkeletonState",
1380
+ rotation_to_target_skeleton,
1381
+ scale_to_target_skeleton: float,
1382
+ z_up: bool = True,
1383
+ ) -> "SkeletonMotion":
1384
+ """
1385
+ Same as the one in :class:`SkeletonState`. This method discards all velocity information before
1386
+ retargeting and re-estimate the velocity after the retargeting. The same fps is used in the
1387
+ new retargetted motion.
1388
+
1389
+ :param joint_mapping: a dictionary of that maps the joint node from the source skeleton to \
1390
+ the target skeleton
1391
+ :type joint_mapping: Dict[str, str]
1392
+
1393
+ :param source_tpose: t-pose of the source skeleton
1394
+ :type source_tpose: SkeletonState
1395
+
1396
+ :param target_tpose: t-pose of the target skeleton
1397
+ :type target_tpose: SkeletonState
1398
+
1399
+ :param rotation_to_target_skeleton: the rotation that needs to be applied to the source\
1400
+ skeleton to align with the target skeleton. Essentially the rotation is t_R_s, where t is\
1401
+ the frame of reference of the target skeleton and s is the frame of reference of the source\
1402
+ skeleton
1403
+ :type rotation_to_target_skeleton: Tensor
1404
+ :param scale_to_target_skeleton: the factor that needs to be multiplied from source\
1405
+ skeleton to target skeleton (unit in distance). For example, to go from `cm` to `m`, the \
1406
+ factor needs to be 0.01.
1407
+ :type scale_to_target_skeleton: float
1408
+ :rtype: SkeletonMotion
1409
+ """
1410
+ return self.retarget_to(
1411
+ joint_mapping,
1412
+ source_tpose.local_rotation,
1413
+ source_tpose.root_translation,
1414
+ target_tpose.skeleton_tree,
1415
+ target_tpose.local_rotation,
1416
+ target_tpose.root_translation,
1417
+ rotation_to_target_skeleton,
1418
+ scale_to_target_skeleton,
1419
+ z_up,
1420
+ )
1421
+
third_party/poselib/visualization/__init__.py ADDED
File without changes
third_party/poselib/visualization/common.py ADDED
@@ -0,0 +1,182 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import os
2
+
3
+ from ..core import logger
4
+ from .plt_plotter import Matplotlib3DPlotter
5
+ from .skeleton_plotter_tasks import Draw3DSkeletonMotion, Draw3DSkeletonState
6
+
7
+
8
+ def plot_skeleton_state(skeleton_state, task_name=""):
9
+ """
10
+ Visualize a skeleton state
11
+
12
+ :param skeleton_state:
13
+ :param task_name:
14
+ :type skeleton_state: SkeletonState
15
+ :type task_name: string, optional
16
+ """
17
+ logger.info("plotting {}".format(task_name))
18
+ task = Draw3DSkeletonState(task_name=task_name, skeleton_state=skeleton_state)
19
+ plotter = Matplotlib3DPlotter(task)
20
+ plotter.show()
21
+
22
+
23
+ def plot_skeleton_states(skeleton_state, skip_n=1, task_name=""):
24
+ """
25
+ Visualize a sequence of skeleton state. The dimension of the skeleton state must be 1
26
+
27
+ :param skeleton_state:
28
+ :param task_name:
29
+ :type skeleton_state: SkeletonState
30
+ :type task_name: string, optional
31
+ """
32
+ logger.info("plotting {} motion".format(task_name))
33
+ assert len(skeleton_state.shape) == 1, "the state must have only one dimension"
34
+ task = Draw3DSkeletonState(task_name=task_name, skeleton_state=skeleton_state[0])
35
+ plotter = Matplotlib3DPlotter(task)
36
+ for frame_id in range(skeleton_state.shape[0]):
37
+ if frame_id % skip_n != 0:
38
+ continue
39
+ task.update(skeleton_state[frame_id])
40
+ plotter.update()
41
+ plotter.show()
42
+
43
+
44
+ def plot_skeleton_motion(skeleton_motion, skip_n=1, task_name=""):
45
+ """
46
+ Visualize a skeleton motion along its first dimension.
47
+
48
+ :param skeleton_motion:
49
+ :param task_name:
50
+ :type skeleton_motion: SkeletonMotion
51
+ :type task_name: string, optional
52
+ """
53
+ logger.info("plotting {} motion".format(task_name))
54
+ task = Draw3DSkeletonMotion(
55
+ task_name=task_name, skeleton_motion=skeleton_motion, frame_index=0
56
+ )
57
+ plotter = Matplotlib3DPlotter(task)
58
+ for frame_id in range(len(skeleton_motion)):
59
+ if frame_id % skip_n != 0:
60
+ continue
61
+ task.update(frame_id)
62
+ plotter.update()
63
+ plotter.show()
64
+
65
+
66
+ def plot_skeleton_motion_interactive_base(skeleton_motion, task_name=""):
67
+ class PlotParams:
68
+ def __init__(self, total_num_frames):
69
+ self.current_frame = 0
70
+ self.playing = False
71
+ self.looping = False
72
+ self.confirmed = False
73
+ self.playback_speed = 4
74
+ self.total_num_frames = total_num_frames
75
+
76
+ def sync(self, other):
77
+ self.current_frame = other.current_frame
78
+ self.playing = other.playing
79
+ self.looping = other.current_frame
80
+ self.confirmed = other.confirmed
81
+ self.playback_speed = other.playback_speed
82
+ self.total_num_frames = other.total_num_frames
83
+
84
+ task = Draw3DSkeletonMotion(
85
+ task_name=task_name, skeleton_motion=skeleton_motion, frame_index=0
86
+ )
87
+ plotter = Matplotlib3DPlotter(task)
88
+
89
+ plot_params = PlotParams(total_num_frames=len(skeleton_motion))
90
+ print("Entered interactive plot - press 'n' to quit, 'h' for a list of commands")
91
+
92
+ def press(event):
93
+ if event.key == "x":
94
+ plot_params.playing = not plot_params.playing
95
+ elif event.key == "z":
96
+ plot_params.current_frame = plot_params.current_frame - 1
97
+ elif event.key == "c":
98
+ plot_params.current_frame = plot_params.current_frame + 1
99
+ elif event.key == "a":
100
+ plot_params.current_frame = plot_params.current_frame - 20
101
+ elif event.key == "d":
102
+ plot_params.current_frame = plot_params.current_frame + 20
103
+ elif event.key == "w":
104
+ plot_params.looping = not plot_params.looping
105
+ print("Looping: {}".format(plot_params.looping))
106
+ elif event.key == "v":
107
+ plot_params.playback_speed *= 2
108
+ print("playback speed: {}".format(plot_params.playback_speed))
109
+ elif event.key == "b":
110
+ if plot_params.playback_speed != 1:
111
+ plot_params.playback_speed //= 2
112
+ print("playback speed: {}".format(plot_params.playback_speed))
113
+ elif event.key == "n":
114
+ plot_params.confirmed = True
115
+ elif event.key == "h":
116
+ rows, columns = os.popen("stty size", "r").read().split()
117
+ columns = int(columns)
118
+ print("=" * columns)
119
+ print("x: play/pause")
120
+ print("z: previous frame")
121
+ print("c: next frame")
122
+ print("a: jump 10 frames back")
123
+ print("d: jump 10 frames forward")
124
+ print("w: looping/non-looping")
125
+ print("v: double speed (this can be applied multiple times)")
126
+ print("b: half speed (this can be applied multiple times)")
127
+ print("n: quit")
128
+ print("h: help")
129
+ print("=" * columns)
130
+
131
+ print(
132
+ 'current frame index: {}/{} (press "n" to quit)'.format(
133
+ plot_params.current_frame, plot_params.total_num_frames - 1
134
+ )
135
+ )
136
+
137
+ plotter.fig.canvas.mpl_connect("key_press_event", press)
138
+ while True:
139
+ reset_trail = False
140
+ if plot_params.confirmed:
141
+ break
142
+ if plot_params.playing:
143
+ plot_params.current_frame += plot_params.playback_speed
144
+ if plot_params.current_frame >= plot_params.total_num_frames:
145
+ if plot_params.looping:
146
+ plot_params.current_frame %= plot_params.total_num_frames
147
+ reset_trail = True
148
+ else:
149
+ plot_params.current_frame = plot_params.total_num_frames - 1
150
+ if plot_params.current_frame < 0:
151
+ if plot_params.looping:
152
+ plot_params.current_frame %= plot_params.total_num_frames
153
+ reset_trail = True
154
+ else:
155
+ plot_params.current_frame = 0
156
+ yield plot_params
157
+ task.update(plot_params.current_frame, reset_trail)
158
+ plotter.update()
159
+
160
+
161
+ def plot_skeleton_motion_interactive(skeleton_motion, task_name=""):
162
+ """
163
+ Visualize a skeleton motion along its first dimension interactively.
164
+
165
+ :param skeleton_motion:
166
+ :param task_name:
167
+ :type skeleton_motion: SkeletonMotion
168
+ :type task_name: string, optional
169
+ """
170
+ for _ in plot_skeleton_motion_interactive_base(skeleton_motion, task_name):
171
+ pass
172
+
173
+
174
+ def plot_skeleton_motion_interactive_multiple(*callables, sync=True):
175
+ for _ in zip(*callables):
176
+ if sync:
177
+ for p1, p2 in zip(_[:-1], _[1:]):
178
+ p2.sync(p1)
179
+
180
+
181
+ # def plot_skeleton_motion_interactive_multiple_same(skeleton_motions, task_name=""):
182
+
third_party/poselib/visualization/core.py ADDED
@@ -0,0 +1,71 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ The base abstract classes for plotter and the plotting tasks. It describes how the plotter
3
+ deals with the tasks in the general cases
4
+ """
5
+ from typing import List
6
+
7
+
8
+ class BasePlotterTask(object):
9
+ _task_name: str # unique name of the task
10
+ _task_type: str # type of the task is used to identify which callable
11
+
12
+ def __init__(self, task_name: str, task_type: str) -> None:
13
+ self._task_name = task_name
14
+ self._task_type = task_type
15
+
16
+ @property
17
+ def task_name(self):
18
+ return self._task_name
19
+
20
+ @property
21
+ def task_type(self):
22
+ return self._task_type
23
+
24
+ def get_scoped_name(self, name):
25
+ return self._task_name + "/" + name
26
+
27
+ def __iter__(self):
28
+ """Should override this function to return a list of task primitives
29
+ """
30
+ raise NotImplementedError
31
+
32
+
33
+ class BasePlotterTasks(object):
34
+ def __init__(self, tasks) -> None:
35
+ self._tasks = tasks
36
+
37
+ def __iter__(self):
38
+ for task in self._tasks:
39
+ yield from task
40
+
41
+
42
+ class BasePlotter(object):
43
+ """An abstract plotter which deals with a plotting task. The children class needs to implement
44
+ the functions to create/update the objects according to the task given
45
+ """
46
+
47
+ _task_primitives: List[BasePlotterTask]
48
+
49
+ def __init__(self, task: BasePlotterTask) -> None:
50
+ self._task_primitives = []
51
+ self.create(task)
52
+
53
+ @property
54
+ def task_primitives(self):
55
+ return self._task_primitives
56
+
57
+ def create(self, task: BasePlotterTask) -> None:
58
+ """Create more task primitives from a task for the plotter"""
59
+ new_task_primitives = list(task) # get all task primitives
60
+ self._task_primitives += new_task_primitives # append them
61
+ self._create_impl(new_task_primitives)
62
+
63
+ def update(self) -> None:
64
+ """Update the plotter for any updates in the task primitives"""
65
+ self._update_impl(self._task_primitives)
66
+
67
+ def _update_impl(self, task_list: List[BasePlotterTask]) -> None:
68
+ raise NotImplementedError
69
+
70
+ def _create_impl(self, task_list: List[BasePlotterTask]) -> None:
71
+ raise NotImplementedError
third_party/poselib/visualization/plt_plotter.py ADDED
@@ -0,0 +1,395 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ The matplotlib plotter implementation for all the primitive tasks (in our case: lines and
3
+ dots)
4
+ """
5
+ from typing import Any, Callable, Dict, List
6
+
7
+ import matplotlib.pyplot as plt
8
+ import mpl_toolkits.mplot3d.axes3d as p3
9
+
10
+ import numpy as np
11
+
12
+ from .core import BasePlotter, BasePlotterTask
13
+
14
+
15
+ class Matplotlib2DPlotter(BasePlotter):
16
+ _fig: plt.figure # plt figure
17
+ _ax: plt.axis # plt axis
18
+ # stores artist objects for each task (task name as the key)
19
+ _artist_cache: Dict[str, Any]
20
+ # callables for each task primitives
21
+ _create_impl_callables: Dict[str, Callable]
22
+ _update_impl_callables: Dict[str, Callable]
23
+
24
+ def __init__(self, task: "BasePlotterTask") -> None:
25
+ fig, ax = plt.subplots()
26
+ self._fig = fig
27
+ self._ax = ax
28
+ self._artist_cache = {}
29
+
30
+ self._create_impl_callables = {
31
+ "Draw2DLines": self._lines_create_impl,
32
+ "Draw2DDots": self._dots_create_impl,
33
+ "Draw2DTrail": self._trail_create_impl,
34
+ }
35
+ self._update_impl_callables = {
36
+ "Draw2DLines": self._lines_update_impl,
37
+ "Draw2DDots": self._dots_update_impl,
38
+ "Draw2DTrail": self._trail_update_impl,
39
+ }
40
+ self._init_lim()
41
+ super().__init__(task)
42
+
43
+ @property
44
+ def ax(self):
45
+ return self._ax
46
+
47
+ @property
48
+ def fig(self):
49
+ return self._fig
50
+
51
+ def show(self):
52
+ plt.show()
53
+
54
+ def _min(self, x, y):
55
+ if x is None:
56
+ return y
57
+ if y is None:
58
+ return x
59
+ return min(x, y)
60
+
61
+ def _max(self, x, y):
62
+ if x is None:
63
+ return y
64
+ if y is None:
65
+ return x
66
+ return max(x, y)
67
+
68
+ def _init_lim(self):
69
+ self._curr_x_min = None
70
+ self._curr_y_min = None
71
+ self._curr_x_max = None
72
+ self._curr_y_max = None
73
+
74
+ def _update_lim(self, xs, ys):
75
+ self._curr_x_min = self._min(np.min(xs), self._curr_x_min)
76
+ self._curr_y_min = self._min(np.min(ys), self._curr_y_min)
77
+ self._curr_x_max = self._max(np.max(xs), self._curr_x_max)
78
+ self._curr_y_max = self._max(np.max(ys), self._curr_y_max)
79
+
80
+ def _set_lim(self):
81
+ if not (
82
+ self._curr_x_min is None
83
+ or self._curr_x_max is None
84
+ or self._curr_y_min is None
85
+ or self._curr_y_max is None
86
+ ):
87
+ self._ax.set_xlim(self._curr_x_min, self._curr_x_max)
88
+ self._ax.set_ylim(self._curr_y_min, self._curr_y_max)
89
+ self._init_lim()
90
+
91
+ @staticmethod
92
+ def _lines_extract_xy_impl(index, lines_task):
93
+ return lines_task[index, :, 0], lines_task[index, :, 1]
94
+
95
+ @staticmethod
96
+ def _trail_extract_xy_impl(index, trail_task):
97
+ return (trail_task[index : index + 2, 0], trail_task[index : index + 2, 1])
98
+
99
+ def _lines_create_impl(self, lines_task):
100
+ color = lines_task.color
101
+ self._artist_cache[lines_task.task_name] = [
102
+ self._ax.plot(
103
+ *Matplotlib2DPlotter._lines_extract_xy_impl(i, lines_task),
104
+ color=color,
105
+ linewidth=lines_task.line_width,
106
+ alpha=lines_task.alpha
107
+ )[0]
108
+ for i in range(len(lines_task))
109
+ ]
110
+
111
+ def _lines_update_impl(self, lines_task):
112
+ lines_artists = self._artist_cache[lines_task.task_name]
113
+ for i in range(len(lines_task)):
114
+ artist = lines_artists[i]
115
+ xs, ys = Matplotlib2DPlotter._lines_extract_xy_impl(i, lines_task)
116
+ artist.set_data(xs, ys)
117
+ if lines_task.influence_lim:
118
+ self._update_lim(xs, ys)
119
+
120
+ def _dots_create_impl(self, dots_task):
121
+ color = dots_task.color
122
+ self._artist_cache[dots_task.task_name] = self._ax.plot(
123
+ dots_task[:, 0],
124
+ dots_task[:, 1],
125
+ c=color,
126
+ linestyle="",
127
+ marker=".",
128
+ markersize=dots_task.marker_size,
129
+ alpha=dots_task.alpha,
130
+ )[0]
131
+
132
+ def _dots_update_impl(self, dots_task):
133
+ dots_artist = self._artist_cache[dots_task.task_name]
134
+ dots_artist.set_data(dots_task[:, 0], dots_task[:, 1])
135
+ if dots_task.influence_lim:
136
+ self._update_lim(dots_task[:, 0], dots_task[:, 1])
137
+
138
+ def _trail_create_impl(self, trail_task):
139
+ color = trail_task.color
140
+ trail_length = len(trail_task) - 1
141
+ self._artist_cache[trail_task.task_name] = [
142
+ self._ax.plot(
143
+ *Matplotlib2DPlotter._trail_extract_xy_impl(i, trail_task),
144
+ color=trail_task.color,
145
+ linewidth=trail_task.line_width,
146
+ alpha=trail_task.alpha * (1.0 - i / (trail_length - 1))
147
+ )[0]
148
+ for i in range(trail_length)
149
+ ]
150
+
151
+ def _trail_update_impl(self, trail_task):
152
+ trails_artists = self._artist_cache[trail_task.task_name]
153
+ for i in range(len(trail_task) - 1):
154
+ artist = trails_artists[i]
155
+ xs, ys = Matplotlib2DPlotter._trail_extract_xy_impl(i, trail_task)
156
+ artist.set_data(xs, ys)
157
+ if trail_task.influence_lim:
158
+ self._update_lim(xs, ys)
159
+
160
+ def _create_impl(self, task_list):
161
+ for task in task_list:
162
+ self._create_impl_callables[task.task_type](task)
163
+ self._draw()
164
+
165
+ def _update_impl(self, task_list):
166
+ for task in task_list:
167
+ self._update_impl_callables[task.task_type](task)
168
+ self._draw()
169
+
170
+ def _set_aspect_equal_2d(self, zero_centered=True):
171
+ xlim = self._ax.get_xlim()
172
+ ylim = self._ax.get_ylim()
173
+
174
+ if not zero_centered:
175
+ xmean = np.mean(xlim)
176
+ ymean = np.mean(ylim)
177
+ else:
178
+ xmean = 0
179
+ ymean = 0
180
+
181
+ plot_radius = max(
182
+ [
183
+ abs(lim - mean_)
184
+ for lims, mean_ in ((xlim, xmean), (ylim, ymean))
185
+ for lim in lims
186
+ ]
187
+ )
188
+
189
+ self._ax.set_xlim([xmean - plot_radius, xmean + plot_radius])
190
+ self._ax.set_ylim([ymean - plot_radius, ymean + plot_radius])
191
+
192
+ def _draw(self):
193
+ self._set_lim()
194
+ self._set_aspect_equal_2d()
195
+ self._fig.canvas.draw()
196
+ self._fig.canvas.flush_events()
197
+ plt.pause(0.00001)
198
+
199
+
200
+ class Matplotlib3DPlotter(BasePlotter):
201
+ _fig: plt.figure # plt figure
202
+ _ax: p3.Axes3D # plt 3d axis
203
+ # stores artist objects for each task (task name as the key)
204
+ _artist_cache: Dict[str, Any]
205
+ # callables for each task primitives
206
+ _create_impl_callables: Dict[str, Callable]
207
+ _update_impl_callables: Dict[str, Callable]
208
+
209
+ def __init__(self, task: "BasePlotterTask") -> None:
210
+ self._fig = plt.figure()
211
+ self._ax = p3.Axes3D(self._fig)
212
+ self._artist_cache = {}
213
+
214
+ self._create_impl_callables = {
215
+ "Draw3DLines": self._lines_create_impl,
216
+ "Draw3DDots": self._dots_create_impl,
217
+ "Draw3DTrail": self._trail_create_impl,
218
+ }
219
+ self._update_impl_callables = {
220
+ "Draw3DLines": self._lines_update_impl,
221
+ "Draw3DDots": self._dots_update_impl,
222
+ "Draw3DTrail": self._trail_update_impl,
223
+ }
224
+ self._init_lim()
225
+ super().__init__(task)
226
+
227
+ @property
228
+ def ax(self):
229
+ return self._ax
230
+
231
+ @property
232
+ def fig(self):
233
+ return self._fig
234
+
235
+ def show(self):
236
+ plt.show()
237
+
238
+ def _min(self, x, y):
239
+ if x is None:
240
+ return y
241
+ if y is None:
242
+ return x
243
+ return min(x, y)
244
+
245
+ def _max(self, x, y):
246
+ if x is None:
247
+ return y
248
+ if y is None:
249
+ return x
250
+ return max(x, y)
251
+
252
+ def _init_lim(self):
253
+ self._curr_x_min = None
254
+ self._curr_y_min = None
255
+ self._curr_z_min = None
256
+ self._curr_x_max = None
257
+ self._curr_y_max = None
258
+ self._curr_z_max = None
259
+
260
+ def _update_lim(self, xs, ys, zs):
261
+ self._curr_x_min = self._min(np.min(xs), self._curr_x_min)
262
+ self._curr_y_min = self._min(np.min(ys), self._curr_y_min)
263
+ self._curr_z_min = self._min(np.min(zs), self._curr_z_min)
264
+ self._curr_x_max = self._max(np.max(xs), self._curr_x_max)
265
+ self._curr_y_max = self._max(np.max(ys), self._curr_y_max)
266
+ self._curr_z_max = self._max(np.max(zs), self._curr_z_max)
267
+
268
+ def _set_lim(self):
269
+ if not (
270
+ self._curr_x_min is None
271
+ or self._curr_x_max is None
272
+ or self._curr_y_min is None
273
+ or self._curr_y_max is None
274
+ or self._curr_z_min is None
275
+ or self._curr_z_max is None
276
+ ):
277
+ self._ax.set_xlim3d(self._curr_x_min, self._curr_x_max)
278
+ self._ax.set_ylim3d(self._curr_y_min, self._curr_y_max)
279
+ self._ax.set_zlim3d(self._curr_z_min, self._curr_z_max)
280
+ self._init_lim()
281
+
282
+ @staticmethod
283
+ def _lines_extract_xyz_impl(index, lines_task):
284
+ return lines_task[index, :, 0], lines_task[index, :, 1], lines_task[index, :, 2]
285
+
286
+ @staticmethod
287
+ def _trail_extract_xyz_impl(index, trail_task):
288
+ return (
289
+ trail_task[index : index + 2, 0],
290
+ trail_task[index : index + 2, 1],
291
+ trail_task[index : index + 2, 2],
292
+ )
293
+
294
+ def _lines_create_impl(self, lines_task):
295
+ color = lines_task.color
296
+ self._artist_cache[lines_task.task_name] = [
297
+ self._ax.plot(
298
+ *Matplotlib3DPlotter._lines_extract_xyz_impl(i, lines_task),
299
+ color=color,
300
+ linewidth=lines_task.line_width,
301
+ alpha=lines_task.alpha
302
+ )[0]
303
+ for i in range(len(lines_task))
304
+ ]
305
+
306
+ def _lines_update_impl(self, lines_task):
307
+ lines_artists = self._artist_cache[lines_task.task_name]
308
+ for i in range(len(lines_task)):
309
+ artist = lines_artists[i]
310
+ xs, ys, zs = Matplotlib3DPlotter._lines_extract_xyz_impl(i, lines_task)
311
+ artist.set_data(xs, ys)
312
+ artist.set_3d_properties(zs)
313
+ if lines_task.influence_lim:
314
+ self._update_lim(xs, ys, zs)
315
+
316
+ def _dots_create_impl(self, dots_task):
317
+ color = dots_task.color
318
+ self._artist_cache[dots_task.task_name] = self._ax.plot(
319
+ dots_task[:, 0],
320
+ dots_task[:, 1],
321
+ dots_task[:, 2],
322
+ c=color,
323
+ linestyle="",
324
+ marker=".",
325
+ markersize=dots_task.marker_size,
326
+ alpha=dots_task.alpha,
327
+ )[0]
328
+
329
+ def _dots_update_impl(self, dots_task):
330
+ dots_artist = self._artist_cache[dots_task.task_name]
331
+ dots_artist.set_data(dots_task[:, 0], dots_task[:, 1])
332
+ dots_artist.set_3d_properties(dots_task[:, 2])
333
+ if dots_task.influence_lim:
334
+ self._update_lim(dots_task[:, 0], dots_task[:, 1], dots_task[:, 2])
335
+
336
+ def _trail_create_impl(self, trail_task):
337
+ color = trail_task.color
338
+ trail_length = len(trail_task) - 1
339
+ self._artist_cache[trail_task.task_name] = [
340
+ self._ax.plot(
341
+ *Matplotlib3DPlotter._trail_extract_xyz_impl(i, trail_task),
342
+ color=trail_task.color,
343
+ linewidth=trail_task.line_width,
344
+ alpha=trail_task.alpha * (1.0 - i / (trail_length - 1))
345
+ )[0]
346
+ for i in range(trail_length)
347
+ ]
348
+
349
+ def _trail_update_impl(self, trail_task):
350
+ trails_artists = self._artist_cache[trail_task.task_name]
351
+ for i in range(len(trail_task) - 1):
352
+ artist = trails_artists[i]
353
+ xs, ys, zs = Matplotlib3DPlotter._trail_extract_xyz_impl(i, trail_task)
354
+ artist.set_data(xs, ys)
355
+ artist.set_3d_properties(zs)
356
+ if trail_task.influence_lim:
357
+ self._update_lim(xs, ys, zs)
358
+
359
+ def _create_impl(self, task_list):
360
+ for task in task_list:
361
+ self._create_impl_callables[task.task_type](task)
362
+ self._draw()
363
+
364
+ def _update_impl(self, task_list):
365
+ for task in task_list:
366
+ self._update_impl_callables[task.task_type](task)
367
+ self._draw()
368
+
369
+ def _set_aspect_equal_3d(self):
370
+ xlim = self._ax.get_xlim3d()
371
+ ylim = self._ax.get_ylim3d()
372
+ zlim = self._ax.get_zlim3d()
373
+
374
+ xmean = np.mean(xlim)
375
+ ymean = np.mean(ylim)
376
+ zmean = np.mean(zlim)
377
+
378
+ plot_radius = max(
379
+ [
380
+ abs(lim - mean_)
381
+ for lims, mean_ in ((xlim, xmean), (ylim, ymean), (zlim, zmean))
382
+ for lim in lims
383
+ ]
384
+ )
385
+
386
+ self._ax.set_xlim3d([xmean - plot_radius, xmean + plot_radius])
387
+ self._ax.set_ylim3d([ymean - plot_radius, ymean + plot_radius])
388
+ self._ax.set_zlim3d([zmean - plot_radius, zmean + plot_radius])
389
+
390
+ def _draw(self):
391
+ self._set_lim()
392
+ self._set_aspect_equal_3d()
393
+ self._fig.canvas.draw()
394
+ self._fig.canvas.flush_events()
395
+ plt.pause(0.00001)
third_party/poselib/visualization/simple_plotter_tasks.py ADDED
@@ -0,0 +1,185 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ This is where all the task primitives are defined
3
+ """
4
+ import numpy as np
5
+
6
+ from .core import BasePlotterTask
7
+
8
+
9
+ class DrawXDLines(BasePlotterTask):
10
+ _lines: np.ndarray
11
+ _color: str
12
+ _line_width: int
13
+ _alpha: float
14
+ _influence_lim: bool
15
+
16
+ def __init__(
17
+ self,
18
+ task_name: str,
19
+ lines: np.ndarray,
20
+ color: str = "blue",
21
+ line_width: int = 2,
22
+ alpha: float = 1.0,
23
+ influence_lim: bool = True,
24
+ ) -> None:
25
+ super().__init__(task_name=task_name, task_type=self.__class__.__name__)
26
+ self._color = color
27
+ self._line_width = line_width
28
+ self._alpha = alpha
29
+ self._influence_lim = influence_lim
30
+ self.update(lines)
31
+
32
+ @property
33
+ def influence_lim(self) -> bool:
34
+ return self._influence_lim
35
+
36
+ @property
37
+ def raw_data(self):
38
+ return self._lines
39
+
40
+ @property
41
+ def color(self):
42
+ return self._color
43
+
44
+ @property
45
+ def line_width(self):
46
+ return self._line_width
47
+
48
+ @property
49
+ def alpha(self):
50
+ return self._alpha
51
+
52
+ @property
53
+ def dim(self):
54
+ raise NotImplementedError
55
+
56
+ @property
57
+ def name(self):
58
+ return "{}DLines".format(self.dim)
59
+
60
+ def update(self, lines):
61
+ self._lines = np.array(lines)
62
+ shape = self._lines.shape
63
+ assert shape[-1] == self.dim and shape[-2] == 2 and len(shape) == 3
64
+
65
+ def __getitem__(self, index):
66
+ return self._lines[index]
67
+
68
+ def __len__(self):
69
+ return self._lines.shape[0]
70
+
71
+ def __iter__(self):
72
+ yield self
73
+
74
+
75
+ class DrawXDDots(BasePlotterTask):
76
+ _dots: np.ndarray
77
+ _color: str
78
+ _marker_size: int
79
+ _alpha: float
80
+ _influence_lim: bool
81
+
82
+ def __init__(
83
+ self,
84
+ task_name: str,
85
+ dots: np.ndarray,
86
+ color: str = "blue",
87
+ marker_size: int = 10,
88
+ alpha: float = 1.0,
89
+ influence_lim: bool = True,
90
+ ) -> None:
91
+ super().__init__(task_name=task_name, task_type=self.__class__.__name__)
92
+ self._color = color
93
+ self._marker_size = marker_size
94
+ self._alpha = alpha
95
+ self._influence_lim = influence_lim
96
+ self.update(dots)
97
+
98
+ def update(self, dots):
99
+ self._dots = np.array(dots)
100
+ shape = self._dots.shape
101
+ assert shape[-1] == self.dim and len(shape) == 2
102
+
103
+ def __getitem__(self, index):
104
+ return self._dots[index]
105
+
106
+ def __len__(self):
107
+ return self._dots.shape[0]
108
+
109
+ def __iter__(self):
110
+ yield self
111
+
112
+ @property
113
+ def influence_lim(self) -> bool:
114
+ return self._influence_lim
115
+
116
+ @property
117
+ def raw_data(self):
118
+ return self._dots
119
+
120
+ @property
121
+ def color(self):
122
+ return self._color
123
+
124
+ @property
125
+ def marker_size(self):
126
+ return self._marker_size
127
+
128
+ @property
129
+ def alpha(self):
130
+ return self._alpha
131
+
132
+ @property
133
+ def dim(self):
134
+ raise NotImplementedError
135
+
136
+ @property
137
+ def name(self):
138
+ return "{}DDots".format(self.dim)
139
+
140
+
141
+ class DrawXDTrail(DrawXDDots):
142
+ @property
143
+ def line_width(self):
144
+ return self.marker_size
145
+
146
+ @property
147
+ def name(self):
148
+ return "{}DTrail".format(self.dim)
149
+
150
+
151
+ class Draw2DLines(DrawXDLines):
152
+ @property
153
+ def dim(self):
154
+ return 2
155
+
156
+
157
+ class Draw3DLines(DrawXDLines):
158
+ @property
159
+ def dim(self):
160
+ return 3
161
+
162
+
163
+ class Draw2DDots(DrawXDDots):
164
+ @property
165
+ def dim(self):
166
+ return 2
167
+
168
+
169
+ class Draw3DDots(DrawXDDots):
170
+ @property
171
+ def dim(self):
172
+ return 3
173
+
174
+
175
+ class Draw2DTrail(DrawXDTrail):
176
+ @property
177
+ def dim(self):
178
+ return 2
179
+
180
+
181
+ class Draw3DTrail(DrawXDTrail):
182
+ @property
183
+ def dim(self):
184
+ return 3
185
+
third_party/poselib/visualization/skeleton_plotter_tasks.py ADDED
@@ -0,0 +1,187 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ """
2
+ This is where all skeleton related complex tasks are defined (skeleton state and skeleton
3
+ motion)
4
+ """
5
+ import numpy as np
6
+
7
+ from .core import BasePlotterTask
8
+ from .simple_plotter_tasks import Draw3DDots, Draw3DLines, Draw3DTrail
9
+
10
+
11
+ class Draw3DSkeletonState(BasePlotterTask):
12
+ _lines_task: Draw3DLines # sub-task for drawing lines
13
+ _dots_task: Draw3DDots # sub-task for drawing dots
14
+
15
+ def __init__(
16
+ self,
17
+ task_name: str,
18
+ skeleton_state,
19
+ joints_color: str = "red",
20
+ lines_color: str = "blue",
21
+ alpha=1.0,
22
+ ) -> None:
23
+ super().__init__(task_name=task_name, task_type="3DSkeletonState")
24
+ lines, dots = Draw3DSkeletonState._get_lines_and_dots(skeleton_state)
25
+ self._lines_task = Draw3DLines(
26
+ self.get_scoped_name("bodies"), lines, joints_color, alpha=alpha
27
+ )
28
+ self._dots_task = Draw3DDots(
29
+ self.get_scoped_name("joints"), dots, lines_color, alpha=alpha
30
+ )
31
+
32
+ @property
33
+ def name(self):
34
+ return "3DSkeleton"
35
+
36
+ def update(self, skeleton_state) -> None:
37
+ self._update(*Draw3DSkeletonState._get_lines_and_dots(skeleton_state))
38
+
39
+ @staticmethod
40
+ def _get_lines_and_dots(skeleton_state):
41
+ """Get all the lines and dots needed to draw the skeleton state
42
+ """
43
+ assert (
44
+ len(skeleton_state.tensor.shape) == 1
45
+ ), "the state has to be zero dimensional"
46
+ dots = skeleton_state.global_translation.numpy()
47
+ skeleton_tree = skeleton_state.skeleton_tree
48
+ parent_indices = skeleton_tree.parent_indices.numpy()
49
+ lines = []
50
+ for node_index in range(len(skeleton_tree)):
51
+ parent_index = parent_indices[node_index]
52
+ if parent_index != -1:
53
+ lines.append([dots[node_index], dots[parent_index]])
54
+ lines = np.array(lines)
55
+ return lines, dots
56
+
57
+ def _update(self, lines, dots) -> None:
58
+ self._lines_task.update(lines)
59
+ self._dots_task.update(dots)
60
+
61
+ def __iter__(self):
62
+ yield from self._lines_task
63
+ yield from self._dots_task
64
+
65
+
66
+ class Draw3DSkeletonMotion(BasePlotterTask):
67
+ def __init__(
68
+ self,
69
+ task_name: str,
70
+ skeleton_motion,
71
+ frame_index=None,
72
+ joints_color="red",
73
+ lines_color="blue",
74
+ velocity_color="green",
75
+ angular_velocity_color="purple",
76
+ trail_color="black",
77
+ trail_length=10,
78
+ alpha=1.0,
79
+ ) -> None:
80
+ super().__init__(task_name=task_name, task_type="3DSkeletonMotion")
81
+ self._trail_length = trail_length
82
+ self._skeleton_motion = skeleton_motion
83
+ # if frame_index is None:
84
+ curr_skeleton_motion = self._skeleton_motion.clone()
85
+ if frame_index is not None:
86
+ curr_skeleton_motion.tensor = self._skeleton_motion.tensor[frame_index, :]
87
+ # else:
88
+ # curr_skeleton_motion = self._skeleton_motion[frame_index, :]
89
+ self._skeleton_state_task = Draw3DSkeletonState(
90
+ self.get_scoped_name("skeleton_state"),
91
+ curr_skeleton_motion,
92
+ joints_color=joints_color,
93
+ lines_color=lines_color,
94
+ alpha=alpha,
95
+ )
96
+ vel_lines, avel_lines = Draw3DSkeletonMotion._get_vel_and_avel(
97
+ curr_skeleton_motion
98
+ )
99
+ self._com_pos = curr_skeleton_motion.root_translation.numpy()[
100
+ np.newaxis, ...
101
+ ].repeat(trail_length, axis=0)
102
+ self._vel_task = Draw3DLines(
103
+ self.get_scoped_name("velocity"),
104
+ vel_lines,
105
+ velocity_color,
106
+ influence_lim=False,
107
+ alpha=alpha,
108
+ )
109
+ self._avel_task = Draw3DLines(
110
+ self.get_scoped_name("angular_velocity"),
111
+ avel_lines,
112
+ angular_velocity_color,
113
+ influence_lim=False,
114
+ alpha=alpha,
115
+ )
116
+ self._com_trail_task = Draw3DTrail(
117
+ self.get_scoped_name("com_trail"),
118
+ self._com_pos,
119
+ trail_color,
120
+ marker_size=2,
121
+ influence_lim=True,
122
+ alpha=alpha,
123
+ )
124
+
125
+ @property
126
+ def name(self):
127
+ return "3DSkeletonMotion"
128
+
129
+ def update(self, frame_index=None, reset_trail=False, skeleton_motion=None) -> None:
130
+ if skeleton_motion is not None:
131
+ self._skeleton_motion = skeleton_motion
132
+
133
+ curr_skeleton_motion = self._skeleton_motion.clone()
134
+ if frame_index is not None:
135
+ curr_skeleton_motion.tensor = curr_skeleton_motion.tensor[frame_index, :]
136
+ if reset_trail:
137
+ self._com_pos = curr_skeleton_motion.root_translation.numpy()[
138
+ np.newaxis, ...
139
+ ].repeat(self._trail_length, axis=0)
140
+ else:
141
+ self._com_pos = np.concatenate(
142
+ (
143
+ curr_skeleton_motion.root_translation.numpy()[np.newaxis, ...],
144
+ self._com_pos[:-1],
145
+ ),
146
+ axis=0,
147
+ )
148
+ self._skeleton_state_task.update(curr_skeleton_motion)
149
+ self._com_trail_task.update(self._com_pos)
150
+ self._update(*Draw3DSkeletonMotion._get_vel_and_avel(curr_skeleton_motion))
151
+
152
+ @staticmethod
153
+ def _get_vel_and_avel(skeleton_motion):
154
+ """Get all the velocity and angular velocity lines
155
+ """
156
+ pos = skeleton_motion.global_translation.numpy()
157
+ vel = skeleton_motion.global_velocity.numpy()
158
+ avel = skeleton_motion.global_angular_velocity.numpy()
159
+
160
+ vel_lines = np.stack((pos, pos + vel * 0.02), axis=1)
161
+ avel_lines = np.stack((pos, pos + avel * 0.01), axis=1)
162
+ return vel_lines, avel_lines
163
+
164
+ def _update(self, vel_lines, avel_lines) -> None:
165
+ self._vel_task.update(vel_lines)
166
+ self._avel_task.update(avel_lines)
167
+
168
+ def __iter__(self):
169
+ yield from self._skeleton_state_task
170
+ yield from self._vel_task
171
+ yield from self._avel_task
172
+ yield from self._com_trail_task
173
+
174
+
175
+ class Draw3DSkeletonMotions(BasePlotterTask):
176
+ def __init__(self, skeleton_motion_tasks) -> None:
177
+ self._skeleton_motion_tasks = skeleton_motion_tasks
178
+
179
+ @property
180
+ def name(self):
181
+ return "3DSkeletonMotions"
182
+
183
+ def update(self, frame_index) -> None:
184
+ list(map(lambda x: x.update(frame_index), self._skeleton_motion_tasks))
185
+
186
+ def __iter__(self):
187
+ yield from self._skeleton_state_tasks
third_party/poselib/visualization/tests/__init__.py ADDED
File without changes
third_party/poselib/visualization/tests/test_plotter.py ADDED
@@ -0,0 +1,16 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ from typing import cast
2
+
3
+ import matplotlib.pyplot as plt
4
+ import numpy as np
5
+
6
+ from ..core import BasePlotterTask, BasePlotterTasks
7
+ from ..plt_plotter import Matplotlib3DPlotter
8
+ from ..simple_plotter_tasks import Draw3DDots, Draw3DLines
9
+
10
+ task = Draw3DLines(task_name="test",
11
+ lines=np.array([[[0, 0, 0], [0, 0, 1]], [[0, 1, 1], [0, 1, 0]]]), color="blue")
12
+ task2 = Draw3DDots(task_name="test2",
13
+ dots=np.array([[0, 0, 0], [0, 0, 1], [0, 1, 1], [0, 1, 0]]), color="red")
14
+ task3 = BasePlotterTasks([task, task2])
15
+ plotter = Matplotlib3DPlotter(cast(BasePlotterTask, task3))
16
+ plt.show()