Add files using upload-large-folder tool
Browse files- .gitignore +114 -0
- CLAUDE.md +114 -0
- DOC.md +23 -0
- LICENSE +7 -0
- README.md +554 -0
- TEST_MOTIONS.md +17 -0
- amass_to_robot.sh +21 -0
- assets/optitrack.png +0 -0
- assets/pal_talos/LICENSE +176 -0
- assets/pal_talos/assets/gripper/base_link_collision.stl +0 -0
- assets/pal_talos/assets/gripper/fingertip.stl +0 -0
- assets/pal_talos/assets/gripper/fingertip_collision.stl +0 -0
- assets/pal_talos/assets/gripper/gripper_motor_double_collision.stl +0 -0
- assets/pal_talos/assets/gripper/gripper_motor_single_collision.stl +0 -0
- assets/pal_talos/assets/gripper/gripper_simple_collision.stl +0 -0
- assets/pal_talos/assets/gripper/inner_double.stl +0 -0
- assets/pal_talos/assets/gripper/inner_double_collision.stl +0 -0
- assets/pal_talos/assets/gripper/inner_single.stl +0 -0
- assets/pal_talos/assets/gripper/inner_single_collision.stl +0 -0
- assets/pal_talos/assets/head/head_1_collision.stl +0 -0
- assets/pal_talos/assets/head/head_2_default_collision.stl +0 -0
- assets/pal_talos/assets/torso/torso_2_collision.stl +0 -0
- assets/pal_talos/talos.xml +508 -0
- check_phmr_pkl.py +17 -0
- phmr_to_robot.sh +45 -0
- setup.py +32 -0
- third_party/poselib/__init__.py +3 -0
- third_party/poselib/core/__init__.py +3 -0
- third_party/poselib/core/backend/__init__.py +3 -0
- third_party/poselib/core/backend/abstract.py +121 -0
- third_party/poselib/core/backend/logger.py +13 -0
- third_party/poselib/core/rotation3d.py +443 -0
- third_party/poselib/core/tensor_utils.py +36 -0
- third_party/poselib/core/tests/__init__.py +0 -0
- third_party/poselib/core/tests/test_rotation.py +55 -0
- third_party/poselib/fbx_importer.py +43 -0
- third_party/poselib/skeleton/__init__.py +0 -0
- third_party/poselib/skeleton/backend/__init__.py +0 -0
- third_party/poselib/skeleton/backend/fbx/__init__.py +0 -0
- third_party/poselib/skeleton/backend/fbx/fbx_backend.py +245 -0
- third_party/poselib/skeleton/backend/fbx/fbx_read_wrapper.py +32 -0
- third_party/poselib/skeleton/skeleton3d.py +1421 -0
- third_party/poselib/visualization/__init__.py +0 -0
- third_party/poselib/visualization/common.py +182 -0
- third_party/poselib/visualization/core.py +71 -0
- third_party/poselib/visualization/plt_plotter.py +395 -0
- third_party/poselib/visualization/simple_plotter_tasks.py +185 -0
- third_party/poselib/visualization/skeleton_plotter_tasks.py +187 -0
- third_party/poselib/visualization/tests/__init__.py +0 -0
- 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 |
+

|
| 20 |
+
|
| 21 |
+

|
| 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 |
+

|
| 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()
|