test

.gitattributes

@@ -86,25 +86,3 @@ assets/meshes/waist_constraint_R.STL filter=lfs diff=lfs merge=lfs -text
 assets/meshes/waist_support_link.STL filter=lfs diff=lfs merge=lfs -text
 assets/meshes/waist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text
 assets/meshes/waist_yaw_link_rev_1_0.STL filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/shoulder__2.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/shoulder__3.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/shoulder__4.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/shoulder__5.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/shoulder.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist__2.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist__3.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist__4.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist__5.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist__6.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_left/wrist.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/shoulder__2.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/shoulder__3.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/shoulder__4.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/shoulder__5.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/shoulder.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist__2.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist__3.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist__4.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist__5.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist__6.stl filter=lfs diff=lfs merge=lfs -text
-assets/meshes_exo_right/wrist.stl filter=lfs diff=lfs merge=lfs -text

CAMERA_README.md

@@ -0,0 +1,256 @@
+# Camera System for MuJoCo G1 Simulator
+
+## Overview
+
+The simulator has two cameras defined:
+
+### 1. **`head_camera`** - Robot Ego-View
+- **Location**: Attached to `torso_link` body
+- **Position**: `[0.06, 0.0, 0.45]` relative to torso (6cm forward, 45cm up)
+- **Orientation**: `euler="0 -0.8 -1.57"` (facing forward, slightly tilted down)
+- **FOV**: 90 degrees
+- **Purpose**: First-person view from the robot's perspective (like a head-mounted camera)
+
+### 2. **`global_view`** - Third-Person View  
+- **Location**: Fixed in world coordinates
+- **Position**: `[2.910, -5.040, 3.860]` (behind and above the robot)
+- **Purpose**: External observer view for visualization
+
+## How Camera Publishing Works
+
+The camera system uses a **zero-copy architecture** with three components:
+
+```
+┌─────────────────────┐
+│   MuJoCo Simulator  │
+│  (Main Process)     │
+│                     │
+│  1. Render cameras  │
+│  2. Copy to shmem   │──┐
+└─────────────────────┘  │
+                         │ Shared Memory
+                         │ (fast IPC)
+                    ┌────▼────────────────┐
+                    │  Image Publisher    │
+                    │  (Subprocess)       │
+                    │                     │
+                    │  3. Encode images   │
+                    │  4. ZMQ publish     │
+                    └─────────┬───────────┘
+                              │
+                              │ TCP (ZMQ)
+                              │ port 5555
+                              ▼
+                    ┌─────────────────────┐
+                    │  Your Policy/Client │
+                    │  (Subscribe)        │
+                    └─────────────────────┘
+```
+
+### Key Technologies:
+- **MuJoCo Renderer**: Captures RGB images from virtual cameras
+- **Shared Memory (`multiprocessing.shared_memory`)**: Zero-copy transfer between processes
+- **ZMQ (ZeroMQ)**: Network socket for publishing images (TCP)
+- **No ROS2 required!** Pure Python multiprocessing
+
+## Usage
+
+### Basic Simulation (No Camera Publishing)
+```bash
+python run_sim.py
+```
+
+### With Camera Publishing
+```bash
+# Publish head camera on default port 5555
+python run_sim.py --publish-images
+
+# Publish multiple cameras
+python run_sim.py --publish-images --cameras head_camera global_view
+
+# Custom port
+python run_sim.py --publish-images --camera-port 6000
+```
+
+### Viewing Camera Streams
+
+In a **separate terminal**, run the camera viewer:
+
+```bash
+# Basic usage (default: localhost:5555)
+python view_cameras.py
+
+# Custom host/port
+python view_cameras.py --host 192.168.1.100 --port 6000
+
+# Save images to directory
+python view_cameras.py --save ./camera_recordings
+
+# Adjust display rate
+python view_cameras.py --fps 60
+```
+
+**Keyboard Controls:**
+- `q`: Quit viewer
+- `s`: Save snapshot of current frame
+
+**Example Workflow:**
+```bash
+# Terminal 1: Start simulator with camera publishing
+python run_sim.py --publish-images
+
+# Terminal 2: View the camera feed
+python view_cameras.py
+```
+
+### Receiving Images in Your Code
+
+```python
+import zmq
+import numpy as np
+from gr00t_wbc.control.sensor.sensor_server import ImageMessageSchema
+
+# Connect to camera publisher
+context = zmq.Context()
+socket = context.socket(zmq.SUB)
+socket.connect("tcp://localhost:5555")
+socket.setsockopt(zmq.SUBSCRIBE, b"")  # Subscribe to all messages
+
+while True:
+    # Receive serialized image data
+    data = socket.recv_pyobj()
+    
+    # Decode images
+    if "head_camera" in data:
+        # Decode image (returns numpy array HxWx3 uint8)
+        img = decode_image(data["head_camera"])
+        
+        # Use image for your policy
+        process_observation(img)
+```
+
+## Camera Configuration
+
+Edit `config.yaml` to change camera settings:
+
+```yaml
+IMAGE_DT: 0.033333  # Publishing rate (30 Hz)
+ENABLE_OFFSCREEN: false  # Enable for camera rendering
+MP_START_METHOD: "spawn"  # Multiprocessing method
+```
+
+Or programmatically in `run_sim.py`:
+
+```python
+camera_configs = {
+    "head_camera": {
+        "height": 480,
+        "width": 640
+    },
+    "custom_camera": {
+        "height": 224,
+        "width": 224
+    }
+}
+```
+
+## Adding Custom Cameras
+
+Edit `assets/g1_29dof_with_hand.xml` or `assets/scene_43dof.xml`:
+
+```xml
+<!-- Camera attached to robot body -->
+<body name="torso_link" pos="0 0 0.019">
+  <camera name="my_camera" pos="0.1 0.0 0.5" euler="0 0 0" fovy="60"/>
+</body>
+
+<!-- Camera in world coordinates -->
+<worldbody>
+  <camera name="side_view" pos="0 -3.0 1.5" xyaxes="1 0 0 0 0.5 0.866"/>
+</worldbody>
+```
+
+Then publish it:
+```bash
+python run_sim.py --publish-images --cameras my_camera
+```
+
+## Performance Notes
+
+- **Rendering overhead**: ~5-10ms per camera per frame @ 640x480
+- **Publishing overhead**: ~2-3ms for encoding + network
+- Image publishing runs in **separate subprocess** to not block simulation
+- Uses **shared memory** for fast inter-process image transfer
+- Target: 30 FPS camera publishing while maintaining 500 Hz simulation
+
+## Troubleshooting
+
+### No images received?
+1. Check if offscreen rendering is enabled (`--publish-images` flag)
+2. Verify ZMQ port is not blocked
+3. Check camera exists in scene XML
+
+### Images are delayed?
+- Reduce `IMAGE_DT` in config
+- Lower camera resolution
+- Use fewer cameras
+
+### "Camera not found" error?
+- Verify camera name in XML matches config
+- Check XML syntax is valid
+- Ensure MuJoCo model loads successfully
+
+## Quick Reference
+
+### File Structure
+```
+mujoco_sim_g1/
+├── run_sim.py              # Simulator launcher
+├── view_cameras.py         # Camera viewer (this file!)
+├── config.yaml             # Simulator config
+├── assets/
+│   ├── scene_43dof.xml     # Scene with global_view camera
+│   └── g1_29dof_with_hand.xml  # Robot model with head_camera
+└── sim/
+    ├── base_sim.py         # MuJoCo environment
+    ├── sensor_utils.py     # ZMQ camera server/client
+    └── image_publish_utils.py  # Multiprocessing image publisher
+```
+
+### Camera Definitions
+
+Edit these files to modify cameras:
+
+**`assets/g1_29dof_with_hand.xml`** - Robot-attached cameras:
+```xml
+<body name="torso_link" pos="0 0 0.019">
+  <camera name="head_camera" pos="0.06 0.0 0.45" euler="0 -0.8 -1.57" fovy="90"/>
+</body>
+```
+
+**`assets/scene_43dof.xml`** - World-frame cameras:
+```xml
+<worldbody>
+  <camera name="global_view" pos="2.910 -5.040 3.860" xyaxes="0.866 0.500 0.000 -0.250 0.433 0.866" fovy="45"/>
+</worldbody>
+```
+
+### Complete Example
+
+```bash
+# Terminal 1: Start simulator with camera publishing
+cd mujoco_sim_g1
+python run_sim.py --publish-images --cameras head_camera global_view
+
+# Terminal 2: View cameras in real-time
+python view_cameras.py
+
+# Terminal 3: Use in your policy (Python code)
+from sim.sensor_utils import SensorClient, ImageUtils
+client = SensorClient()
+client.start_client("localhost", 5555)
+data = client.receive_message()
+img = ImageUtils.decode_image(data["head_camera"])
+# img is now numpy array (H, W, 3) in BGR format
+```
+

README.md

@@ -1,5 +1,167 @@
-# MuJoCo Sim for Unitree G1
+# Standalone MuJoCo Simulator for Unitree G1
 
-Standalone MuJoCo physics simulator for the Unitree G1 robot, adapted from gr00t_wbc. Currently supports G1_29dof.
+This is a standalone MuJoCo physics simulator for the Unitree G1 robot, extracted from the GR00T-WholeBodyControl repository.
+
+## Features
+
+- **Physics Simulation**: Runs G1 robot in MuJoCo at 500Hz (2ms timestep)
+- **DDS Communication**: Uses Unitree SDK2 DDS for robot communication
+- **Compatible**: Works with existing `unitree_g1.py` control code via DDS
+- **Visualization**: Real-time 3D visualization of robot motion
+
+## Directory Structure
+
+```
+mujoco_sim_g1/
+├── requirements.txt          # Python dependencies
+├── run_sim.py               # Main launcher script
+├── config.yaml              # Simulation configuration
+├── sim/                     # Simulation modules
+│   ├── base_sim.py
+│   ├── simulator_factory.py
+│   ├── unitree_sdk2py_bridge.py
+│   └── ...
+└── assets/                  # Robot models
+    ├── g1_29dof_with_hand.xml
+    └── meshes/*.STL
+```
+
+## Installation
+
+### 1. Create Virtual Environment (Recommended)
+
+```bash
+cd mujoco_sim_g1
+python3 -m venv venv
+source venv/bin/activate  # On Windows: venv\Scripts\activate
+```
+
+### 2. Install Dependencies
+
+```bash
+pip install -r requirements.txt
+```
+
+**Note**: If you encounter issues with `rclpy`, you can comment out the ROS2 imports in `sim/base_sim.py` (lines 11, 609-617) if you don't need camera publishing.
+
+## Usage
+
+### Basic Usage
+
+```bash
+# Activate environment
+source venv/bin/activate
+
+# Run the simulator
+python run_sim.py
+```
+
+The simulator will:
+1. Load the G1 robot model
+2. Initialize DDS communication on domain 0
+3. Open a MuJoCo visualization window
+4. Start listening for motor commands via DDS
+
+### Running with Your Robot Control Code
+
+Once the simulator is running, you can control it from another terminal:
+
+```bash
+# In another terminal
+cd /home/yope/Documents/lerobot
+conda activate unitree_lerobot
+
+# Run your existing control code
+python test_locomotion_minimal.py
+```
+
+Your `unitree_g1.py` code will automatically connect to the simulator via DDS!
+
+## Configuration
+
+Edit `config.yaml` to customize:
+
+- **SIMULATE_DT**: Simulation timestep (default: 0.002s = 500Hz)
+- **DOMAIN_ID**: DDS domain ID (default: 0)
+- **ENABLE_ONSCREEN**: Show visualization (default: true)
+- **USE_JOYSTICK**: Enable gamepad control (default: false)
+- **ROBOT_SCENE**: Path to MuJoCo XML scene
+
+### PD Gains
+
+The simulator uses the following PD gains (matching NVIDIA GR00T):
+
+**Legs (indices 0-11):**
+- Hip joints: KP=150, KD=2
+- Knee joints: KP=300, KD=4
+- Ankle joints: KP=40, KD=2
+
+**Waist (indices 12-14):**
+- All waist joints: KP=250, KD=5
+
+**Arms (indices 15-28):**
+- Shoulders: KP=100, KD=2-5
+- Elbows/Wrists: KP=20-40, KD=1-2
+
+## Troubleshooting
+
+### ImportError: cannot import name 'ChannelFactoryInitialize'
+
+```bash
+pip install --upgrade unitree-sdk2py
+```
+
+### ROS2/rclpy errors
+
+If you don't need camera publishing, edit `sim/base_sim.py`:
+- Comment out line 11: `import rclpy`
+- Comment out lines 609-617 (ROS2 initialization)
+
+### Meshes not found
+
+Make sure mesh paths in `assets/g1_29dof_with_hand.xml` are relative:
+```xml
+<mesh file="meshes/pelvis.STL"/>  <!-- Not absolute path -->
+```
+
+### Robot falls immediately
+
+Check that:
+1. PD gains match NVIDIA's values (see config.yaml)
+2. Velocity command scaling is correct (ang_vel_scale=0.25, cmd_scale=[2.0, 2.0, 0.25])
+3. Observations for missing joints are zeroed out (indices 12, 14, 20, 21, 27, 28)
+
+## Technical Details
+
+### Communication
+
+The simulator publishes:
+- **`rt/lowstate`**: Robot state (joint positions, velocities, IMU, etc.)
+- **`rt/wirelesscontroller`**: Wireless remote controller state (if joystick enabled)
+
+The simulator subscribes to:
+- **`rt/lowcmd`**: Motor commands (position, velocity, torque, KP, KD)
+
+### Coordinate Frames
+
+- **World frame**: Z-up
+- **Joint ordering**: 29 DOF (12 legs + 3 waist + 14 arms)
+- **IMU**: Quaternion in [w, x, y, z] format
+
+### Performance
+
+- Simulation runs at ~500Hz (2ms timestep)
+- Viewer updates at ~50Hz (20ms)
+- Typical CPU usage: 20-40% on single core
+
+## Files from GR00T-WholeBodyControl
+
+This standalone simulator was extracted from:
+- `gr00t_wbc/control/envs/g1/sim/` (simulation modules)
+- `gr00t_wbc/control/robot_model/model_data/g1/` (robot model files)
+- `gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml` (configuration)
+
+## License
+
+Follows the license of the original GR00T-WholeBodyControl repository.
 
-set use joystick to 1 to control the robot 

assets/exo_left.urdf

@@ -1,347 +0,0 @@
-<?xml version="1.0" ?>
-<!-- Generated using onshape-to-robot -->
-<!-- Onshape https://cad.onshape.com/documents/30647899d0efca800f6a0b61/w/6367c32608b1610b986f4220/e/82448bde0644faba4511c9d2 -->
-<robot name="my-robot">
-  <link name="base_link"/>
-  <joint name="base_to_shoulder_pitch" type="fixed">
-    <origin xyz="-0.600072 -0.15 0.3178" rpy="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="shoulder_pitch"/>
-  </joint>
-  <!-- Link shoulder_pitch -->
-  <link name="shoulder_pitch">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder -->
-    <visual>
-      <origin xyz="-0.319917 -0.188041 -0.0739695" rpy="1.73941e-11 -2.77556e-17 -3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder.stl"/>
-      </geometry>
-      <material name="shoulder_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.319917 -0.188041 -0.0739695" rpy="1.73941e-11 -2.77556e-17 -3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part shoulder_2 -->
-    <visual>
-      <origin xyz="-0.319917 -0.188041 -0.0739695" rpy="1.73941e-11 1.7367e-11 3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__2.stl"/>
-      </geometry>
-      <material name="shoulder_2_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.319917 -0.188041 -0.0739695" rpy="1.73941e-11 1.7367e-11 3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__2.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link shoulder_yaw -->
-  <link name="shoulder_yaw">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_3 -->
-    <visual>
-      <origin xyz="0.337111 0.085895 0.174004" rpy="1.5708 4.85145e-12 -5.68355e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__3.stl"/>
-      </geometry>
-      <material name="shoulder_3_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.337111 0.085895 0.174004" rpy="1.5708 4.85145e-12 -5.68355e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__3.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link shoulder_roll -->
-  <link name="shoulder_roll">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_4 -->
-    <visual>
-      <origin xyz="0.188947 -0.0970719 0.361641" rpy="-1.62911 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__4.stl"/>
-      </geometry>
-      <material name="shoulder_4_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.188947 -0.0970719 0.361641" rpy="-1.62911 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__4.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link elbow_flex -->
-  <link name="elbow_flex">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_5 -->
-    <visual>
-      <origin xyz="0.358661 0.193894 -0.0611064" rpy="1.78388e-17 2.53007e-17 3.50616e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__5.stl"/>
-      </geometry>
-      <material name="shoulder_5_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.358661 0.193894 -0.0611064" rpy="1.78388e-17 2.53007e-17 3.50616e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/shoulder__5.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link wrist_roll -->
-  <link name="wrist_roll">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part wrist -->
-    <visual>
-      <origin xyz="0.370176 -0.154734 0.203442" rpy="1.5708 2.12442e-12 0.0687143"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist.stl"/>
-      </geometry>
-      <material name="wrist_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.370176 -0.154734 0.203442" rpy="1.5708 2.12442e-12 0.0687143"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link end_effector -->
-  <link name="end_effector">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part wrist_2 -->
-    <visual>
-      <origin xyz="0.189619 0.17695 -0.583114" rpy="0.0540298 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__2.stl"/>
-      </geometry>
-      <material name="wrist_2_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189619 0.17695 -0.583114" rpy="0.0540298 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__2.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_3 -->
-    <visual>
-      <origin xyz="0.189345 0.176964 -0.583114" rpy="0.0540298 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__3.stl"/>
-      </geometry>
-      <material name="wrist_3_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189345 0.176964 -0.583114" rpy="0.0540298 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__3.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_4 -->
-    <visual>
-      <origin xyz="0.192193 0.173682 -0.583216" rpy="0.0688698 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__4.stl"/>
-      </geometry>
-      <material name="wrist_4_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.192193 0.173682 -0.583216" rpy="0.0688698 1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__4.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_5 -->
-    <visual>
-      <origin xyz="0.189768 0.177109 -0.583122" rpy="-1.57084 1.57074 -1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__5.stl"/>
-      </geometry>
-      <material name="wrist_5_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189768 0.177109 -0.583122" rpy="-1.57084 1.57074 -1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__5.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_6 -->
-    <visual>
-      <origin xyz="0.189348 0.177132 -0.583124" rpy="-1.57084 1.57074 -1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__6.stl"/>
-      </geometry>
-      <material name="wrist_6_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189348 0.177132 -0.583124" rpy="-1.57084 1.57074 -1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/wrist__6.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part z -->
-    <visual>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/z.stl"/>
-      </geometry>
-      <material name="z_material">
-        <color rgba="0.231373 0.380392 0.705882 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/z.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part y -->
-    <visual>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/y.stl"/>
-      </geometry>
-      <material name="y_material">
-        <color rgba="0.372549 0.654902 0.239216 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/y.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part part_4 -->
-    <visual>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/part_4.stl"/>
-      </geometry>
-      <material name="part_4_material">
-        <color rgba="0.917647 0.917647 0.917647 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/part_4.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part x -->
-    <visual>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/x.stl"/>
-      </geometry>
-      <material name="x_material">
-        <color rgba="1 0 0 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 0.0274691 0.0397266" rpy="3.08756 -1.5708 0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_left/x.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Joint from wrist_roll to end_effector -->
-  <joint name="wrist_roll" type="revolute">
-    <origin xyz="-0.224547 -0.0154537 0.0169453" rpy="1.5708 0.139345 -1.50208"/>
-    <parent link="wrist_roll"/>
-    <child link="end_effector"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.716528" upper="0.854268"/>
-  </joint>
-  <!-- Joint from elbow_flex to wrist_roll -->
-  <joint name="elbow_flex" type="revolute">
-    <origin xyz="-1.7424e-05 -0.00788072 -0.240892" rpy="-1.5708 2.53007e-17 3.50616e-18"/>
-    <parent link="elbow_flex"/>
-    <child link="wrist_roll"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.11591" upper="1.32755"/>
-  </joint>
-  <!-- Joint from shoulder_roll to elbow_flex -->
-  <joint name="shoulder_roll" type="revolute">
-    <origin xyz="-0.00144298 -0.0247172 0.00296278" rpy="-1.5708 -1.56793 -0.0583134"/>
-    <parent link="shoulder_roll"/>
-    <child link="elbow_flex"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.57366" upper="1.56793"/>
-  </joint>
-  <!-- Joint from shoulder_yaw to shoulder_roll -->
-  <joint name="shoulder_yaw" type="revolute">
-    <origin xyz="-0.0247593 7.34308e-15 -0.0202799" rpy="3.14159 -1.5708 0"/>
-    <parent link="shoulder_yaw"/>
-    <child link="shoulder_roll"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.290752" upper="1.62911"/>
-  </joint>
-  <!-- Joint from shoulder_pitch to shoulder_yaw -->
-  <joint name="shoulder_pitch" type="revolute">
-    <origin xyz="0.017217 -0.0139551 0.0119912" rpy="-1.5708 0.142264 -3.14159"/>
-    <parent link="shoulder_pitch"/>
-    <child link="shoulder_yaw"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.18946" upper="1.254"/>
-  </joint>
-  <link name="ee"/>
-
-  <joint name="end_effector_to_ee" type="fixed">
-    <parent link="end_effector"/>
-    <child link="ee"/>
-    <!-- rotate tcp here -->
-    <origin xyz="0 0 0" rpy="1.5708 -1.5708 1.5708"/>
-  </joint>
-</robot>

assets/exo_right.urdf

@@ -1,347 +0,0 @@
-<?xml version="1.0" ?>
-<!-- Generated using onshape-to-robot -->
-<!-- Onshape https://cad.onshape.com/documents/30647899d0efca800f6a0b61/w/6367c32608b1610b986f4220/e/82448bde0644faba4511c9d2 -->
-<robot name="my-robot-right">
-  <link name="base_link"/>
-  <joint name="base_to_shoulder_pitch" type="fixed">
-    <origin xyz="-0.60072 0.15 0.3178" rpy="-0.0 0.0 -0.0"/>
-    <parent link="base_link"/>
-    <child link="shoulder_pitch"/>
-  </joint>
-  <!-- Link shoulder_pitch -->
-  <link name="shoulder_pitch">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder -->
-    <visual>
-      <origin xyz="-0.319917 0.188041 -0.0739695" rpy="-1.73941e-11 -2.77556e-17 3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder.stl"/>
-      </geometry>
-      <material name="shoulder_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.319917 0.188041 -0.0739695" rpy="-1.73941e-11 -2.77556e-17 3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part shoulder_2 -->
-    <visual>
-      <origin xyz="-0.319917 0.188041 -0.0739695" rpy="-1.73941e-11 1.7367e-11 -3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__2.stl"/>
-      </geometry>
-      <material name="shoulder_2_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.319917 0.188041 -0.0739695" rpy="-1.73941e-11 1.7367e-11 -3.14159"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__2.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link shoulder_yaw -->
-  <link name="shoulder_yaw">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_3 -->
-    <visual>
-      <origin xyz="0.337111 -0.085895 0.174004" rpy="-1.5708 4.85145e-12 5.68355e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__3.stl"/>
-      </geometry>
-      <material name="shoulder_3_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.337111 -0.085895 0.174004" rpy="-1.5708 4.85145e-12 5.68355e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__3.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link shoulder_roll -->
-  <link name="shoulder_roll">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_4 -->
-    <visual>
-      <origin xyz="0.188947 0.0970719 0.361641" rpy="1.62911 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__4.stl"/>
-      </geometry>
-      <material name="shoulder_4_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.188947 0.0970719 0.361641" rpy="1.62911 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__4.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link elbow_flex -->
-  <link name="elbow_flex">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part shoulder_5 -->
-    <visual>
-      <origin xyz="0.358661 -0.193894 -0.0611064" rpy="-1.78388e-17 2.53007e-17 -3.50616e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__5.stl"/>
-      </geometry>
-      <material name="shoulder_5_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.358661 -0.193894 -0.0611064" rpy="-1.78388e-17 2.53007e-17 -3.50616e-18"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/shoulder__5.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link wrist_roll -->
-  <link name="wrist_roll">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part wrist -->
-    <visual>
-      <origin xyz="0.370176 0.154734 0.203442" rpy="-1.5708 2.12442e-12 -0.0687143"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist.stl"/>
-      </geometry>
-      <material name="wrist_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.370176 0.154734 0.203442" rpy="-1.5708 2.12442e-12 -0.0687143"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Link end_effector -->
-  <link name="end_effector">
-    <inertial>
-      <origin xyz="0.0 -0.0 0.0" rpy="-0.0 0.0 -0.0"/>
-      <mass value="1e-09"/>
-      <inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
-    </inertial>
-    <!-- Part wrist_2 -->
-    <visual>
-      <origin xyz="0.189619 -0.17695 -0.583114" rpy="-0.0540298 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__2.stl"/>
-      </geometry>
-      <material name="wrist_2_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189619 -0.17695 -0.583114" rpy="-0.0540298 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__2.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_3 -->
-    <visual>
-      <origin xyz="0.189345 -0.176964 -0.583114" rpy="-0.0540298 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__3.stl"/>
-      </geometry>
-      <material name="wrist_3_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189345 -0.176964 -0.583114" rpy="-0.0540298 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__3.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_4 -->
-    <visual>
-      <origin xyz="0.192193 -0.173682 -0.583216" rpy="-0.0688698 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__4.stl"/>
-      </geometry>
-      <material name="wrist_4_material">
-        <color rgba="1 1 1 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.192193 -0.173682 -0.583216" rpy="-0.0688698 1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__4.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_5 -->
-    <visual>
-      <origin xyz="0.189768 -0.177109 -0.583122" rpy="1.57084 1.57074 1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__5.stl"/>
-      </geometry>
-      <material name="wrist_5_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189768 -0.177109 -0.583122" rpy="1.57084 1.57074 1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__5.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part wrist_6 -->
-    <visual>
-      <origin xyz="0.189348 -0.177132 -0.583124" rpy="1.57084 1.57074 1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__6.stl"/>
-      </geometry>
-      <material name="wrist_6_material">
-        <color rgba="0.964706 0.964706 0.952941 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.189348 -0.177132 -0.583124" rpy="1.57084 1.57074 1.62483"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/wrist__6.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part z -->
-    <visual>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/z.stl"/>
-      </geometry>
-      <material name="z_material">
-        <color rgba="0.231373 0.380392 0.705882 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/z.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part y -->
-    <visual>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/y.stl"/>
-      </geometry>
-      <material name="y_material">
-        <color rgba="0.372549 0.654902 0.239216 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/y.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part part_4 -->
-    <visual>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/part_4.stl"/>
-      </geometry>
-      <material name="part_4_material">
-        <color rgba="0.917647 0.917647 0.917647 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/part_4.stl"/>
-      </geometry>
-    </collision>
-    <!-- Part x -->
-    <visual>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/x.stl"/>
-      </geometry>
-      <material name="x_material">
-        <color rgba="1 0 0 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0.0115311 -0.0274691 0.0397266" rpy="-3.08756 -1.5708 -0.0"/>
-      <geometry>
-        <mesh filename="package://meshes_exo_right/x.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Joint from wrist_roll to end_effector -->
-  <joint name="wrist_roll" type="revolute">
-    <origin xyz="-0.224547 0.0154537 0.0169453" rpy="-1.5708 0.139345 1.50208"/>
-    <parent link="wrist_roll"/>
-    <child link="end_effector"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.716528" upper="0.854268"/>
-  </joint>
-  <!-- Joint from elbow_flex to wrist_roll -->
-  <joint name="elbow_flex" type="revolute">
-    <origin xyz="-1.7424e-05 0.00788072 -0.240892" rpy="1.5708 2.53007e-17 -3.50616e-18"/>
-    <parent link="elbow_flex"/>
-    <child link="wrist_roll"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.11591" upper="1.32755"/>
-  </joint>
-  <!-- Joint from shoulder_roll to elbow_flex -->
-  <joint name="shoulder_roll" type="revolute">
-    <origin xyz="-0.00144298 0.0247172 0.00296278" rpy="1.5708 -1.56793 0.0583134"/>
-    <parent link="shoulder_roll"/>
-    <child link="elbow_flex"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.57366" upper="1.56793"/>
-  </joint>
-  <!-- Joint from shoulder_yaw to shoulder_roll -->
-  <joint name="shoulder_yaw" type="revolute">
-    <origin xyz="-0.0247593 -7.34308e-15 -0.0202799" rpy="-3.14159 -1.5708 -0.0"/>
-    <parent link="shoulder_yaw"/>
-    <child link="shoulder_roll"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.290752" upper="1.62911"/>
-  </joint>
-  <!-- Joint from shoulder_pitch to shoulder_yaw -->
-  <joint name="shoulder_pitch" type="revolute">
-    <origin xyz="0.017217 0.0139551 0.0119912" rpy="1.5708 0.142264 3.14159"/>
-    <parent link="shoulder_pitch"/>
-    <child link="shoulder_yaw"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-1.18946" upper="1.254"/>
-  </joint>
-  <link name="ee"/>
-
-  <joint name="end_effector_to_ee" type="fixed">
-    <parent link="end_effector"/>
-    <child link="ee"/>
-    <!-- rotate tcp here -->
-    <origin xyz="0.0 -0.0 0.0" rpy="-1.5708 -1.5708 -1.5708"/>
-  </joint>
-</robot>

assets/g1_29dof_with_hand_original.xml

@@ -0,0 +1,748 @@
+<mujoco model="g1_29dof_with_hand">
+  <compiler angle="radian" meshdir="meshes" />
+
+  <default>
+    <default class="torso_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="leg_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="ankle_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="arm_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.2"/>
+    </default>
+    <default class="wrist_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
+    </default>
+    <default class="finger_motor">
+      <joint damping="0.05" armature="0.01" frictionloss="0.1"/>
+    </default>
+  </default>
+  <asset>
+    <mesh name="pelvis" file="pelvis.STL" />
+    <mesh name="pelvis_contour_link" file="pelvis_contour_link.STL" />
+    <mesh name="left_hip_pitch_link" file="left_hip_pitch_link.STL" />
+    <mesh name="left_hip_roll_link" file="left_hip_roll_link.STL" />
+    <mesh name="left_hip_yaw_link" file="left_hip_yaw_link.STL" />
+    <mesh name="left_knee_link" file="left_knee_link.STL" />
+    <mesh name="left_ankle_pitch_link" file="left_ankle_pitch_link.STL" />
+    <mesh name="left_ankle_roll_link" file="left_ankle_roll_link.STL" />
+    <mesh name="right_hip_pitch_link" file="right_hip_pitch_link.STL" />
+    <mesh name="right_hip_roll_link" file="right_hip_roll_link.STL" />
+    <mesh name="right_hip_yaw_link" file="right_hip_yaw_link.STL" />
+    <mesh name="right_knee_link" file="right_knee_link.STL" />
+    <mesh name="right_ankle_pitch_link" file="right_ankle_pitch_link.STL" />
+    <mesh name="right_ankle_roll_link" file="right_ankle_roll_link.STL" />
+    <mesh name="waist_yaw_link" file="waist_yaw_link.STL" />
+    <mesh name="waist_roll_link" file="waist_roll_link.STL" />
+    <mesh name="torso_link" file="torso_link.STL" />
+    <mesh name="logo_link" file="logo_link.STL" />
+    <mesh name="head_link" file="head_link.STL" />
+    <mesh name="waist_support_link" file="waist_support_link.STL" />
+    <mesh name="left_shoulder_pitch_link" file="left_shoulder_pitch_link.STL" />
+    <mesh name="left_shoulder_roll_link" file="left_shoulder_roll_link.STL" />
+    <mesh name="left_shoulder_yaw_link" file="left_shoulder_yaw_link.STL" />
+    <mesh name="left_elbow_link" file="left_elbow_link.STL" />
+    <mesh name="left_wrist_roll_link" file="left_wrist_roll_link.STL" />
+    <mesh name="left_wrist_pitch_link" file="left_wrist_pitch_link.STL" />
+    <mesh name="left_wrist_yaw_link" file="left_wrist_yaw_link.STL" />
+    <mesh name="left_hand_palm_link" file="left_hand_palm_link.STL" />
+    <mesh name="left_hand_thumb_0_link" file="left_hand_thumb_0_link.STL" />
+    <mesh name="left_hand_thumb_1_link" file="left_hand_thumb_1_link.STL" />
+    <mesh name="left_hand_thumb_2_link" file="left_hand_thumb_2_link.STL" />
+    <mesh name="left_hand_middle_0_link" file="left_hand_middle_0_link.STL" />
+    <mesh name="left_hand_middle_1_link" file="left_hand_middle_1_link.STL" />
+    <mesh name="left_hand_index_0_link" file="left_hand_index_0_link.STL" />
+    <mesh name="left_hand_index_1_link" file="left_hand_index_1_link.STL" />
+    <mesh name="right_shoulder_pitch_link" file="right_shoulder_pitch_link.STL" />
+    <mesh name="right_shoulder_roll_link" file="right_shoulder_roll_link.STL" />
+    <mesh name="right_shoulder_yaw_link" file="right_shoulder_yaw_link.STL" />
+    <mesh name="right_elbow_link" file="right_elbow_link.STL" />
+    <mesh name="right_wrist_roll_link" file="right_wrist_roll_link.STL" />
+    <mesh name="right_wrist_pitch_link" file="right_wrist_pitch_link.STL" />
+    <mesh name="right_wrist_yaw_link" file="right_wrist_yaw_link.STL" />
+    <mesh name="right_hand_palm_link" file="right_hand_palm_link.STL" />
+    <mesh name="right_hand_thumb_0_link" file="right_hand_thumb_0_link.STL" />
+    <mesh name="right_hand_thumb_1_link" file="right_hand_thumb_1_link.STL" />
+    <mesh name="right_hand_thumb_2_link" file="right_hand_thumb_2_link.STL" />
+    <mesh name="right_hand_middle_0_link" file="right_hand_middle_0_link.STL" />
+    <mesh name="right_hand_middle_1_link" file="right_hand_middle_1_link.STL" />
+    <mesh name="right_hand_index_0_link" file="right_hand_index_0_link.STL" />
+    <mesh name="right_hand_index_1_link" file="right_hand_index_1_link.STL" />
+  </asset>
+
+  <worldbody>
+    <body name="pelvis" pos="0 0 0.793">
+      <inertial pos="0 0 -0.07605" quat="1 0 -0.000399148 0" mass="3.813"
+        diaginertia="0.010549 0.0093089 0.0079184" />
+      <joint name="floating_base_joint" type="free" limited="false" actuatorfrclimited="false" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+        mesh="pelvis" />
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+        mesh="pelvis_contour_link" />
+      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="pelvis_contour_link" />
+      <body name="left_hip_pitch_link" pos="0 0.064452 -0.1027">
+        <inertial pos="0.002741 0.047791 -0.02606" quat="0.954862 0.293964 0.0302556 0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="left_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="left_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="left_hip_pitch_link" />
+        <body name="left_hip_roll_link" pos="0 0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 -0.001045 -0.087934"
+            quat="0.977808 -1.97119e-05 0.205576 -0.0403793" mass="1.52"
+            diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="left_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.5236 2.9671"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="left_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_roll_link" />
+          <body name="left_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 -0.010981 -0.15078" quat="0.600598 0.15832 0.223482 0.751181"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="left_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="left_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hip_yaw_link" />
+            <body name="left_knee_link" pos="-0.078273 0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 0.003964 -0.12074"
+                quat="0.923418 -0.0327699 0.0158246 0.382067" mass="1.932"
+                diaginertia="0.0113804 0.0112778 0.00146458" />
+              <joint name="left_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_knee_link" />
+              <body name="left_ankle_pitch_link" pos="0 -9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="left_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_ankle_pitch_link" />
+                <body name="left_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="-0.000481092 0.728482 -0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="left_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="left_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="right_hip_pitch_link" pos="0 -0.064452 -0.1027">
+        <inertial pos="0.002741 -0.047791 -0.02606" quat="0.954862 -0.293964 0.0302556 -0.030122"
+          mass="1.35" diaginertia="0.00181517 0.00153422 0.00116212" />
+        <joint name="right_hip_pitch_joint" pos="0 0 0" axis="0 1 0" range="-2.5307 2.8798"
+          actuatorfrcrange="-88 88" class="leg_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.2 0.2 0.2 1"
+          mesh="right_hip_pitch_link" />
+        <geom type="mesh" rgba="0.2 0.2 0.2 1" mesh="right_hip_pitch_link" />
+        <body name="right_hip_roll_link" pos="0 -0.052 -0.030465" quat="0.996179 0 -0.0873386 0">
+          <inertial pos="0.029812 0.001045 -0.087934" quat="0.977808 1.97119e-05 0.205576 0.0403793"
+            mass="1.52" diaginertia="0.00254986 0.00241169 0.00148755" />
+          <joint name="right_hip_roll_joint" pos="0 0 0" axis="1 0 0" range="-2.9671 0.5236"
+            actuatorfrcrange="-88 88" class="leg_motor" />
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="right_hip_roll_link" />
+          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_roll_link" />
+          <body name="right_hip_yaw_link" pos="0.025001 0 -0.12412">
+            <inertial pos="-0.057709 0.010981 -0.15078" quat="0.751181 0.223482 0.15832 0.600598"
+              mass="1.702" diaginertia="0.00776166 0.00717575 0.00160139" />
+            <joint name="right_hip_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.7576 2.7576"
+              actuatorfrcrange="-88 88" class="leg_motor" />
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="right_hip_yaw_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hip_yaw_link" />
+            <body name="right_knee_link" pos="-0.078273 -0.0021489 -0.17734"
+              quat="0.996179 0 0.0873386 0">
+              <inertial pos="0.005457 -0.003964 -0.12074"
+                quat="0.923439 0.0345276 0.0116333 -0.382012" mass="1.932"
+                diaginertia="0.011374 0.0112843 0.00146452" />
+              <joint name="right_knee_joint" pos="0 0 0" axis="0 1 0" range="-0.087267 2.8798"
+                actuatorfrcrange="-139 139" class="leg_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_knee_link" />
+              <body name="right_ankle_pitch_link" pos="0 9.4445e-05 -0.30001">
+                <inertial pos="-0.007269 0 0.011137" quat="0.603053 0.369225 0.369225 0.603053"
+                  mass="0.074" diaginertia="1.89e-05 1.40805e-05 6.9195e-06" />
+                <joint name="right_ankle_pitch_joint" pos="0 0 0" axis="0 1 0"
+                  range="-0.87267 0.5236" actuatorfrcrange="-50 50" class="ankle_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_ankle_pitch_link" />
+                <body name="right_ankle_roll_link" pos="0 0 -0.017558">
+                  <inertial pos="0.026505 0 -0.016425"
+                    quat="0.000481092 0.728482 0.000618967 0.685065" mass="0.608"
+                    diaginertia="0.00167218 0.0016161 0.000217621" />
+                  <joint name="right_ankle_roll_joint" pos="0 0 0" axis="1 0 0"
+                    range="-0.2618 0.2618" actuatorfrcrange="-50 50" class="ankle_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.2 0.2 0.2 1" mesh="right_ankle_roll_link" />
+                  <geom size="0.005" pos="-0.05 0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="-0.05 -0.025 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                  <geom size="0.005" pos="0.12 -0.03 -0.03" rgba="0.2 0.2 0.2 1" />
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link">
+        <inertial pos="0.003964 0 0.018769" quat="-0.0178291 0.628464 0.0282471 0.777121"
+          mass="0.244" diaginertia="0.000158561 0.000124229 9.67669e-05" />
+        <joint name="waist_yaw_joint" pos="0 0 0" axis="0 0 1" range="-2.618 2.618"
+          actuatorfrcrange="-88 88" class="torso_motor" />
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+          mesh="waist_yaw_link" />
+        <body name="waist_roll_link" pos="-0.0039635 0 0.035">
+          <inertial pos="0 -0.000236 0.010111" quat="0.99979 0.020492 0 0" mass="0.047"
+            diaginertia="7.515e-06 6.40206e-06 3.98394e-06" />
+          <joint name="waist_roll_joint" pos="0 0 0" axis="1 0 0" range="-0.52 0.52"
+            actuatorfrcrange="-50 50" class="torso_motor"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+            mesh="waist_roll_link" />
+          <body name="torso_link" pos="0 0 0.019">
+            <inertial pos="0.00331658 0.000261533 0.179856"
+              quat="0.999831 0.000376204 0.0179895 -0.00377704" mass="9.598"
+              diaginertia="0.12407 0.111951 0.0325382" />
+            <joint name="waist_pitch_joint" pos="0 0 0" axis="0 1 0" range="-0.52 0.52"
+              actuatorfrcrange="-50 50" class="torso_motor"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+              mesh="torso_link" />
+            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="torso_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.2 0.2 0.2 1" mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.2 0.2 0.2 1"
+              mesh="logo_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" contype="0" conaffinity="0" group="1"
+              density="0" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" type="mesh" rgba="0.2 0.2 0.2 1" mesh="head_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0"
+              group="1" density="0" rgba="0.7 0.7 0.7 1" mesh="waist_support_link" />
+            <geom pos="0.0039635 0 -0.054" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+              mesh="waist_support_link" />
+            <site name="imu" size="0.01" pos="-0.03959 -0.00224 0.13792" />
+            <body name="left_shoulder_pitch_link" pos="0.0039563 0.10022 0.23778"
+              quat="0.990264 0.139201 1.38722e-05 -9.86868e-05">
+              <inertial pos="0 0.035892 -0.011628" quat="0.654152 0.0130458 -0.326267 0.68225"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="left_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="left_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="left_shoulder_roll_link" pos="0 0.038 -0.013831"
+                quat="0.990268 -0.139172 0 0">
+                <inertial pos="-0.000227 0.00727 -0.063243"
+                  quat="0.701256 -0.0196223 -0.00710317 0.712604" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="left_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-1.5882 2.2515" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="left_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="left_shoulder_yaw_link" pos="0 0.00624 -0.1032">
+                  <inertial pos="0.010773 -0.002949 -0.072009"
+                    quat="0.716879 -0.0964829 -0.0679942 0.687134" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="left_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_shoulder_yaw_link" />
+                  <body name="left_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 0.004454 -0.010062"
+                      quat="0.541765 0.636132 0.388821 0.388129" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="left_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_elbow_link" />
+                    <body name="left_wrist_roll_link" pos="0.1 0.00188791 -0.01">
+                      <inertial pos="0.0171394 0.000537591 4.8864e-07"
+                        quat="0.575338 0.411667 -0.574906 0.411094" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="left_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_roll_link" />
+                      <body name="left_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 -0.00111685 -0.00111658"
+                          quat="0.249998 0.661363 0.293036 0.643608" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="left_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_pitch_link" />
+                        <body name="left_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 0.00212216 -0.000374562"
+                            quat="0.487149 0.493844 0.513241 0.505358" mass="0.457415"
+                            diaginertia="0.00105989 0.000895419 0.000323842" />
+                          <joint name="left_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_wrist_yaw_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <geom pos="0.0415 0.003 0" quat="1 0 0 0" type="mesh" rgba="0.7 0.7 0.7 1"
+                            mesh="left_hand_palm_link" />
+                          <body name="left_hand_thumb_0_link" pos="0.067 0.003 0">
+                            <inertial pos="-0.000884246 -0.00863407 0.000944293"
+                              quat="0.462991 0.643965 -0.460173 0.398986" mass="0.0862366"
+                              diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                            <joint name="left_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                              range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                              class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_0_link" />
+                            <body name="left_hand_thumb_1_link" pos="-0.0025 -0.0193 0">
+                              <inertial pos="-0.000827888 -0.0354744 -0.0003809"
+                                quat="0.685598 0.705471 -0.15207 0.0956069" mass="0.0588507"
+                                diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                              <joint name="left_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-0.724312 1.0472" actuatorfrcrange="-1.4 1.4"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_1_link" />
+                              <geom size="0.01 0.015 0.01" pos="-0.001 -0.032 0" type="box"
+                                rgba="0.7 0.7 0.7 1" />
+                              <body name="left_hand_thumb_2_link" pos="0 -0.0458 0">
+                                <inertial pos="-0.00171735 -0.0262819 0.000107789"
+                                  quat="0.703174 0.710977 -0.00017564 -0.00766553" mass="0.0203063"
+                                  diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                <joint name="left_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                  range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                                <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_thumb_2_link" />
+                              </body>
+                            </body>
+                          </body>
+                          <body name="left_hand_middle_0_link" pos="0.1192 0.0046 -0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_0_link" />
+                            <body name="left_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="left_hand_index_0_link" pos="0.1192 0.0046 0.0285">
+                            <inertial pos="0.0354744 0.000827888 0.0003809"
+                              quat="0.391313 0.552395 0.417187 0.606373" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="left_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="-1.5708 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_0_link" />
+                            <body name="left_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 0.00171735 -0.000107789"
+                                quat="0.502612 0.491799 0.502639 0.502861" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="left_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="-1.74533 0" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="left_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="right_shoulder_pitch_link" pos="0.0039563 -0.10021 0.23778"
+              quat="0.990264 -0.139201 1.38722e-05 9.86868e-05">
+              <inertial pos="0 -0.035892 -0.011628" quat="0.68225 -0.326267 0.0130458 0.654152"
+                mass="0.718" diaginertia="0.000465864 0.000432842 0.000406394" />
+              <joint name="right_shoulder_pitch_joint" pos="0 0 0" axis="0 1 0"
+                range="-3.0892 2.6704" actuatorfrcrange="-25 25" class="arm_motor" />
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.7 0.7 0.7 1" mesh="right_shoulder_pitch_link" />
+              <geom size="0.03 0.025" pos="0 -0.04 -0.01" quat="0.707107 0 0.707107 0"
+                type="cylinder" rgba="0.7 0.7 0.7 1" />
+              <body name="right_shoulder_roll_link" pos="0 -0.038 -0.013831"
+                quat="0.990268 0.139172 0 0">
+                <inertial pos="-0.000227 -0.00727 -0.063243"
+                  quat="0.712604 -0.00710317 -0.0196223 0.701256" mass="0.643"
+                  diaginertia="0.000691311 0.000618011 0.000388977" />
+                <joint name="right_shoulder_roll_joint" pos="0 0 0" axis="1 0 0"
+                  range="-2.2515 1.5882" actuatorfrcrange="-25 25" class="arm_motor" />
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.7 0.7 0.7 1" mesh="right_shoulder_roll_link" />
+                <geom size="0.03 0.015" pos="-0.004 -0.006 -0.053" type="cylinder"
+                  rgba="0.7 0.7 0.7 1" />
+                <body name="right_shoulder_yaw_link" pos="0 -0.00624 -0.1032">
+                  <inertial pos="0.010773 0.002949 -0.072009"
+                    quat="0.687134 -0.0679942 -0.0964829 0.716879" mass="0.734"
+                    diaginertia="0.00106187 0.00103217 0.000400661" />
+                  <joint name="right_shoulder_yaw_joint" pos="0 0 0" axis="0 0 1"
+                    range="-2.618 2.618" actuatorfrcrange="-25 25" class="arm_motor" />
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_shoulder_yaw_link" />
+                  <body name="right_elbow_link" pos="0.015783 0 -0.080518">
+                    <inertial pos="0.064956 -0.004454 -0.010062"
+                      quat="0.388129 0.388821 0.636132 0.541765" mass="0.6"
+                      diaginertia="0.000443035 0.000421612 0.000259353" />
+                    <joint name="right_elbow_joint" pos="0 0 0" axis="0 1 0" range="-1.0472 2.0944"
+                      actuatorfrcrange="-25 25" class="arm_motor" />
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_elbow_link" />
+                    <body name="right_wrist_roll_link" pos="0.1 -0.00188791 -0.01">
+                      <inertial pos="0.0171394 -0.000537591 4.8864e-07"
+                        quat="0.411667 0.575338 -0.411094 0.574906" mass="0.085445"
+                        diaginertia="5.48211e-05 4.96646e-05 3.57798e-05" />
+                      <joint name="right_wrist_roll_joint" pos="0 0 0" axis="1 0 0"
+                        range="-1.97222 1.97222" actuatorfrcrange="-25 25" class="arm_motor" />
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_roll_link" />
+                      <body name="right_wrist_pitch_link" pos="0.038 0 0">
+                        <inertial pos="0.0229999 0.00111685 -0.00111658"
+                          quat="0.643608 0.293036 0.661363 0.249998" mass="0.48405"
+                          diaginertia="0.000430353 0.000429873 0.000164648" />
+                        <joint name="right_wrist_pitch_joint" pos="0 0 0" axis="0 1 0"
+                          range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                          rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_pitch_link" />
+                        <body name="right_wrist_yaw_link" pos="0.046 0 0">
+                          <inertial pos="0.0885506 -0.00212216 0.000573742"
+                            quat="0.507224 0.511377 0.494292 0.486717" mass="0.457415"
+                            diaginertia="0.0010598 0.000895373 0.0003238" />
+                          <joint name="right_wrist_yaw_joint" pos="0 0 0" axis="0 0 1"
+                            range="-1.61443 1.61443" actuatorfrcrange="-5 5" class="wrist_motor" />
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                            rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_wrist_yaw_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh" contype="0"
+                            conaffinity="0" group="1" density="0" rgba="0.7 0.7 0.7 1"
+                            mesh="right_hand_palm_link" />
+                          <geom pos="0.0415 -0.003 0" quat="1 0 0 0" type="mesh"
+                            rgba="0.7 0.7 0.7 1" mesh="right_hand_palm_link" />
+                          <body name="right_hand_thumb_0_link" pos="0.067 -0.003 0">
+                              <inertial pos="-0.000884246 0.00863407 0.000944293"
+                                quat="0.643965 0.462991 -0.398986 0.460173" mass="0.0862366"
+                                diaginertia="1.6546e-05 1.60058e-05 1.43741e-05" />
+                              <joint name="right_hand_thumb_0_joint" pos="0 0 0" axis="0 1 0"
+                                range="-1.0472 1.0472" actuatorfrcrange="-2.45 2.45"
+                                class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_0_link" />
+                              <body name="right_hand_thumb_1_link" pos="-0.0025 0.0193 0">
+                                <inertial pos="-0.000827888 0.0354744 -0.0003809"
+                                  quat="0.705471 0.685598 -0.0956069 0.15207" mass="0.0588507"
+                                  diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                                <joint name="right_hand_thumb_1_joint" pos="0 0 0" axis="0 0 1"
+                                  range="-1.0472 0.724312" actuatorfrcrange="-1.4 1.4"
+                                  class="finger_motor" />
+                                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                  rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_1_link" />
+                                <geom size="0.01 0.015 0.01" pos="-0.001 0.032 0" type="box"
+                                  rgba="0.7 0.7 0.7 1" />
+                                <body name="right_hand_thumb_2_link" pos="0 0.0458 0">
+                                  <inertial pos="-0.00171735 0.0262819 0.000107789"
+                                    quat="0.710977 0.703174 0.00766553 0.00017564" mass="0.0203063"
+                                    diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                                  <joint name="right_hand_thumb_2_joint" pos="0 0 0" axis="0 0 1"
+                                    range="-1.74533 0" actuatorfrcrange="-1.4 1.4"
+                                    class="finger_motor" />
+                                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                    rgba="0.7 0.7 0.7 1" mesh="right_hand_thumb_2_link" />
+                                  <geom type="mesh" rgba="0.7 0.7 0.7 1"
+                                    mesh="right_hand_thumb_2_link" />
+                                </body>
+                              </body>
+                          </body>
+                          <body name="right_hand_middle_0_link" pos="0.1192 -0.0046 -0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_middle_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_0_link" />
+                            <body name="right_hand_middle_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_middle_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_middle_1_link" />
+                            </body>
+                          </body>
+                          <body name="right_hand_index_0_link" pos="0.1192 -0.0046 0.0285">
+                            <inertial pos="0.0354744 -0.000827888 0.0003809"
+                              quat="0.606373 0.417187 0.552395 0.391313" mass="0.0588507"
+                              diaginertia="1.28514e-05 1.22902e-05 5.9666e-06" />
+                            <joint name="right_hand_index_0_joint" pos="0 0 0" axis="0 0 1"
+                              range="0 1.5708" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                              rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_0_link" />
+                            <body name="right_hand_index_1_link" pos="0.0458 0 0">
+                              <inertial pos="0.0262819 -0.00171735 -0.000107789"
+                                quat="0.502861 0.502639 0.491799 0.502612" mass="0.0203063"
+                                diaginertia="4.61314e-06 3.86645e-06 1.53495e-06" />
+                              <joint name="right_hand_index_1_joint" pos="0 0 0" axis="0 0 1"
+                                range="0 1.74533" actuatorfrcrange="-1.4 1.4" class="finger_motor" />
+                              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                                rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                              <geom type="mesh" rgba="0.7 0.7 0.7 1" mesh="right_hand_index_1_link" />
+                            </body>
+                          </body>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <actuator>
+    <motor name="left_hip_pitch" joint="left_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_roll" joint="left_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="left_hip_yaw" joint="left_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="left_knee" joint="left_knee_joint" ctrlrange="-139 139" />
+    <motor name="left_ankle_pitch" joint="left_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="left_ankle_roll" joint="left_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="right_hip_pitch" joint="right_hip_pitch_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_roll" joint="right_hip_roll_joint" ctrlrange="-88 88" />
+    <motor name="right_hip_yaw" joint="right_hip_yaw_joint" ctrlrange="-88 88" />
+    <motor name="right_knee" joint="right_knee_joint" ctrlrange="-139 139" />
+    <motor name="right_ankle_pitch" joint="right_ankle_pitch_joint" ctrlrange="-50 50" />
+    <motor name="right_ankle_roll" joint="right_ankle_roll_joint" ctrlrange="-50 50" />
+
+    <motor name="waist_yaw" joint="waist_yaw_joint" ctrlrange="-88 88" />
+    <motor name="waist_roll" joint="waist_roll_joint" ctrlrange="-50 50" />
+    <motor name="waist_pitch" joint="waist_pitch_joint" ctrlrange="-50 50" />
+
+    <motor name="left_shoulder_pitch" joint="left_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_roll" joint="left_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_shoulder_yaw" joint="left_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="left_elbow" joint="left_elbow_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_roll" joint="left_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="left_wrist_pitch" joint="left_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="left_wrist_yaw" joint="left_wrist_yaw_joint" ctrlrange="-5 5" />
+
+    <motor name="left_hand_thumb_0" joint="left_hand_thumb_0_joint" ctrlrange="-2.45 2.45" />
+    <motor name="left_hand_thumb_1" joint="left_hand_thumb_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_thumb_2" joint="left_hand_thumb_2_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_middle_0" joint="left_hand_middle_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_middle_1" joint="left_hand_middle_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_index_0" joint="left_hand_index_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="left_hand_index_1" joint="left_hand_index_1_joint" ctrlrange="-1.4 1.4" />
+
+    <motor name="right_shoulder_pitch" joint="right_shoulder_pitch_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_roll" joint="right_shoulder_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_shoulder_yaw" joint="right_shoulder_yaw_joint" ctrlrange="-25 25" />
+    <motor name="right_elbow" joint="right_elbow_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_roll" joint="right_wrist_roll_joint" ctrlrange="-25 25" />
+    <motor name="right_wrist_pitch" joint="right_wrist_pitch_joint" ctrlrange="-5 5" />
+    <motor name="right_wrist_yaw" joint="right_wrist_yaw_joint" ctrlrange="-5 5" />
+
+    <motor name="right_hand_thumb_0" joint="right_hand_thumb_0_joint" ctrlrange="-2.45 2.45" />
+    <motor name="right_hand_thumb_1" joint="right_hand_thumb_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_thumb_2" joint="right_hand_thumb_2_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_middle_0" joint="right_hand_middle_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_middle_1" joint="right_hand_middle_1_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_index_0" joint="right_hand_index_0_joint" ctrlrange="-1.4 1.4" />
+    <motor name="right_hand_index_1" joint="right_hand_index_1_joint" ctrlrange="-1.4 1.4" />
+  </actuator>
+
+  <sensor>
+    <jointpos name="left_hip_pitch_pos" joint="left_hip_pitch_joint" />
+    <jointpos name="left_hip_roll_pos" joint="left_hip_roll_joint" />
+    <jointpos name="left_hip_yaw_pos" joint="left_hip_yaw_joint" />
+    <jointpos name="left_knee_pos" joint="left_knee_joint" />
+    <jointpos name="left_ankle_pitch_pos" joint="left_ankle_pitch_joint" />
+    <jointpos name="left_ankle_roll_pos" joint="left_ankle_roll_joint" />
+    <jointpos name="right_hip_pitch_pos" joint="right_hip_pitch_joint" />
+    <jointpos name="right_hip_roll_pos" joint="right_hip_roll_joint" />
+    <jointpos name="right_hip_yaw_pos" joint="right_hip_yaw_joint" />
+    <jointpos name="right_knee_pos" joint="right_knee_joint" />
+    <jointpos name="right_ankle_pitch_pos" joint="right_ankle_pitch_joint" />
+    <jointpos name="right_ankle_roll_pos" joint="right_ankle_roll_joint" />
+    <jointpos name="waist_yaw_pos" joint="waist_yaw_joint" />
+    <jointpos name="waist_roll_pos" joint="waist_roll_joint" />
+    <jointpos name="waist_pitch_pos" joint="waist_pitch_joint" />
+    <jointpos name="left_shoulder_pitch_pos" joint="left_shoulder_pitch_joint" />
+    <jointpos name="left_shoulder_roll_pos" joint="left_shoulder_roll_joint" />
+    <jointpos name="left_shoulder_yaw_pos" joint="left_shoulder_yaw_joint" />
+    <jointpos name="left_elbow_pos" joint="left_elbow_joint" />
+    <jointpos name="left_wrist_roll_pos" joint="left_wrist_roll_joint" />
+    <jointpos name="left_wrist_pitch_pos" joint="left_wrist_pitch_joint" />
+    <jointpos name="left_wrist_yaw_pos" joint="left_wrist_yaw_joint" />
+    <jointpos name="right_shoulder_pitch_pos" joint="right_shoulder_pitch_joint" />
+    <jointpos name="right_shoulder_roll_pos" joint="right_shoulder_roll_joint" />
+    <jointpos name="right_shoulder_yaw_pos" joint="right_shoulder_yaw_joint" />
+    <jointpos name="right_elbow_pos" joint="right_elbow_joint" />
+    <jointpos name="right_wrist_roll_pos" joint="right_wrist_roll_joint" />
+    <jointpos name="right_wrist_pitch_pos" joint="right_wrist_pitch_joint" />
+    <jointpos name="right_wrist_yaw_pos" joint="right_wrist_yaw_joint" />
+
+    <jointpos name="left_hand_thumb_0_pos" joint="left_hand_thumb_0_joint" />
+    <jointpos name="left_hand_thumb_1_pos" joint="left_hand_thumb_1_joint" />
+    <jointpos name="left_hand_thumb_2_pos" joint="left_hand_thumb_2_joint" />
+    <jointpos name="left_hand_middle_0_pos" joint="left_hand_middle_0_joint" />
+    <jointpos name="left_hand_middle_1_pos" joint="left_hand_middle_1_joint" />
+    <jointpos name="left_hand_index_0_pos" joint="left_hand_index_0_joint" />
+    <jointpos name="left_hand_index_1_pos" joint="left_hand_index_1_joint" />
+    <jointpos name="right_hand_thumb_0_pos" joint="right_hand_thumb_0_joint" />
+    <jointpos name="right_hand_thumb_1_pos" joint="right_hand_thumb_1_joint" />
+    <jointpos name="right_hand_thumb_2_pos" joint="right_hand_thumb_2_joint" />
+    <jointpos name="right_hand_middle_0_pos" joint="right_hand_middle_0_joint" />
+    <jointpos name="right_hand_middle_1_pos" joint="right_hand_middle_1_joint" />
+    <jointpos name="right_hand_index_0_pos" joint="right_hand_index_0_joint" />
+    <jointpos name="right_hand_index_1_pos" joint="right_hand_index_1_joint" />
+
+    <jointvel name="left_hip_pitch_vel" joint="left_hip_pitch_joint" />
+    <jointvel name="left_hip_roll_vel" joint="left_hip_roll_joint" />
+    <jointvel name="left_hip_yaw_vel" joint="left_hip_yaw_joint" />
+    <jointvel name="left_knee_vel" joint="left_knee_joint" />
+    <jointvel name="left_ankle_pitch_vel" joint="left_ankle_pitch_joint" />
+    <jointvel name="left_ankle_roll_vel" joint="left_ankle_roll_joint" />
+    <jointvel name="right_hip_pitch_vel" joint="right_hip_pitch_joint" />
+    <jointvel name="right_hip_roll_vel" joint="right_hip_roll_joint" />
+    <jointvel name="right_hip_yaw_vel" joint="right_hip_yaw_joint" />
+    <jointvel name="right_knee_vel" joint="right_knee_joint" />
+    <jointvel name="right_ankle_pitch_vel" joint="right_ankle_pitch_joint" />
+    <jointvel name="right_ankle_roll_vel" joint="right_ankle_roll_joint" />
+    <jointvel name="waist_yaw_vel" joint="waist_yaw_joint" />
+    <jointvel name="waist_roll_vel" joint="waist_roll_joint" />
+    <jointvel name="waist_pitch_vel" joint="waist_pitch_joint" />
+    <jointvel name="left_shoulder_pitch_vel" joint="left_shoulder_pitch_joint" />
+    <jointvel name="left_shoulder_roll_vel" joint="left_shoulder_roll_joint" />
+    <jointvel name="left_shoulder_yaw_vel" joint="left_shoulder_yaw_joint" />
+    <jointvel name="left_elbow_vel" joint="left_elbow_joint" />
+    <jointvel name="left_wrist_roll_vel" joint="left_wrist_roll_joint" />
+    <jointvel name="left_wrist_pitch_vel" joint="left_wrist_pitch_joint" />
+    <jointvel name="left_wrist_yaw_vel" joint="left_wrist_yaw_joint" />
+    <jointvel name="right_shoulder_pitch_vel" joint="right_shoulder_pitch_joint" />
+    <jointvel name="right_shoulder_roll_vel" joint="right_shoulder_roll_joint" />
+    <jointvel name="right_shoulder_yaw_vel" joint="right_shoulder_yaw_joint" />
+    <jointvel name="right_elbow_vel" joint="right_elbow_joint" />
+    <jointvel name="right_wrist_roll_vel" joint="right_wrist_roll_joint" />
+    <jointvel name="right_wrist_pitch_vel" joint="right_wrist_pitch_joint" />
+    <jointvel name="right_wrist_yaw_vel" joint="right_wrist_yaw_joint" />
+
+    <jointvel name="left_hand_thumb_0_vel" joint="left_hand_thumb_0_joint" />
+    <jointvel name="left_hand_thumb_1_vel" joint="left_hand_thumb_1_joint" />
+    <jointvel name="left_hand_thumb_2_vel" joint="left_hand_thumb_2_joint" />
+    <jointvel name="left_hand_middle_0_vel" joint="left_hand_middle_0_joint" />
+    <jointvel name="left_hand_middle_1_vel" joint="left_hand_middle_1_joint" />
+    <jointvel name="left_hand_index_0_vel" joint="left_hand_index_0_joint" />
+    <jointvel name="left_hand_index_1_vel" joint="left_hand_index_1_joint" />
+    <jointvel name="right_hand_thumb_0_vel" joint="right_hand_thumb_0_joint" />
+    <jointvel name="right_hand_thumb_1_vel" joint="right_hand_thumb_1_joint" />
+    <jointvel name="right_hand_thumb_2_vel" joint="right_hand_thumb_2_joint" />
+    <jointvel name="right_hand_middle_0_vel" joint="right_hand_middle_0_joint" />
+    <jointvel name="right_hand_middle_1_vel" joint="right_hand_middle_1_joint" />
+    <jointvel name="right_hand_index_0_vel" joint="right_hand_index_0_joint" />
+    <jointvel name="right_hand_index_1_vel" joint="right_hand_index_1_joint" />
+
+    <jointactuatorfrc name="left_hip_pitch_torque" joint="left_hip_pitch_joint" />
+    <jointactuatorfrc name="left_hip_roll_torque" joint="left_hip_roll_joint" />
+    <jointactuatorfrc name="left_hip_yaw_torque" joint="left_hip_yaw_joint" />
+    <jointactuatorfrc name="left_knee_torque" joint="left_knee_joint" />
+    <jointactuatorfrc name="left_ankle_pitch_torque" joint="left_ankle_pitch_joint" />
+    <jointactuatorfrc name="left_ankle_roll_torque" joint="left_ankle_roll_joint" />
+    <jointactuatorfrc name="right_hip_pitch_torque" joint="right_hip_pitch_joint" />
+    <jointactuatorfrc name="right_hip_roll_torque" joint="right_hip_roll_joint" />
+    <jointactuatorfrc name="right_hip_yaw_torque" joint="right_hip_yaw_joint" />
+    <jointactuatorfrc name="right_knee_torque" joint="right_knee_joint" />
+    <jointactuatorfrc name="right_ankle_pitch_torque" joint="right_ankle_pitch_joint" />
+    <jointactuatorfrc name="right_ankle_roll_torque" joint="right_ankle_roll_joint" />
+    <jointactuatorfrc name="waist_yaw_torque" joint="waist_yaw_joint" />
+    <jointactuatorfrc name="waist_roll_torque" joint="waist_roll_joint" />
+    <jointactuatorfrc name="waist_pitch_torque" joint="waist_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_pitch_torque" joint="left_shoulder_pitch_joint" />
+    <jointactuatorfrc name="left_shoulder_roll_torque" joint="left_shoulder_roll_joint" />
+    <jointactuatorfrc name="left_shoulder_yaw_torque" joint="left_shoulder_yaw_joint" />
+    <jointactuatorfrc name="left_elbow_torque" joint="left_elbow_joint" />
+    <jointactuatorfrc name="left_wrist_roll_torque" joint="left_wrist_roll_joint" />
+    <jointactuatorfrc name="left_wrist_pitch_torque" joint="left_wrist_pitch_joint" />
+    <jointactuatorfrc name="left_wrist_yaw_torque" joint="left_wrist_yaw_joint" />
+    <jointactuatorfrc name="right_shoulder_pitch_torque" joint="right_shoulder_pitch_joint" />
+    <jointactuatorfrc name="right_shoulder_roll_torque" joint="right_shoulder_roll_joint" />
+    <jointactuatorfrc name="right_shoulder_yaw_torque" joint="right_shoulder_yaw_joint" />
+    <jointactuatorfrc name="right_elbow_torque" joint="right_elbow_joint" />
+    <jointactuatorfrc name="right_wrist_roll_torque" joint="right_wrist_roll_joint" />
+    <jointactuatorfrc name="right_wrist_pitch_torque" joint="right_wrist_pitch_joint" />
+    <jointactuatorfrc name="right_wrist_yaw_torque" joint="right_wrist_yaw_joint" />
+
+    <jointactuatorfrc name="left_hand_thumb_0_torque" joint="left_hand_thumb_0_joint" />
+    <jointactuatorfrc name="left_hand_thumb_1_torque" joint="left_hand_thumb_1_joint" />
+    <jointactuatorfrc name="left_hand_thumb_2_torque" joint="left_hand_thumb_2_joint" />
+    <jointactuatorfrc name="left_hand_middle_0_torque" joint="left_hand_middle_0_joint" />
+    <jointactuatorfrc name="left_hand_middle_1_torque" joint="left_hand_middle_1_joint" />
+    <jointactuatorfrc name="left_hand_index_0_torque" joint="left_hand_index_0_joint" />
+    <jointactuatorfrc name="left_hand_index_1_torque" joint="left_hand_index_1_joint" />
+    <jointactuatorfrc name="right_hand_thumb_0_torque" joint="right_hand_thumb_0_joint" />
+    <jointactuatorfrc name="right_hand_thumb_1_torque" joint="right_hand_thumb_1_joint" />
+    <jointactuatorfrc name="right_hand_thumb_2_torque" joint="right_hand_thumb_2_joint" />
+    <jointactuatorfrc name="right_hand_middle_0_torque" joint="right_hand_middle_0_joint" />
+    <jointactuatorfrc name="right_hand_middle_1_torque" joint="right_hand_middle_1_joint" />
+    <jointactuatorfrc name="right_hand_index_0_torque" joint="right_hand_index_0_joint" />
+    <jointactuatorfrc name="right_hand_index_1_torque" joint="right_hand_index_1_joint" />
+
+    <framequat name="imu_quat" objtype="site" objname="imu" />
+    <gyro name="imu_gyro" site="imu" />
+    <accelerometer name="imu_acc" site="imu" />
+
+    <framepos name="frame_pos" objtype="site" objname="imu" />
+    <framelinvel name="frame_vel" objtype="site" objname="imu" />
+  </sensor>
+</mujoco>

assets/g1_body29_hand14.urdf

@@ -1,1476 +0,0 @@
-<robot name="g1">
-  <mujoco>
-    <compiler meshdir="meshes" discardvisual="false"/>
-  </mujoco>
-
-  <!-- [CAUTION] uncomment when convert to mujoco -->
-  <!-- <link name="world"></link>
-  <joint name="floating_base_joint" type="floating">
-    <parent link="world"/>
-    <child link="pelvis"/>
-  </joint> -->
-
-  <link name="pelvis">
-    <inertial>
-      <origin xyz="0 0 -0.07605" rpy="0 0 0"/>
-      <mass value="3.813"/>
-      <inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/pelvis.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-  </link>
-  <link name="pelvis_contour_link">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="0.001"/>
-      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/pelvis_contour_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/pelvis_contour_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="pelvis_contour_joint" type="fixed">
-    <parent link="pelvis"/>
-    <child link="pelvis_contour_link"/>
-  </joint>
-
-  <!-- Legs -->
-  <link name="left_hip_pitch_link">
-    <inertial>
-      <origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
-      <mass value="1.35"/>
-      <inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_pitch_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hip_pitch_joint" type="revolute">
-    <origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
-    <parent link="pelvis"/>
-    <child link="left_hip_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
-  </joint>
-  <link name="left_hip_roll_link">
-    <inertial>
-      <origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
-      <mass value="1.52"/>
-      <inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_roll_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hip_roll_joint" type="revolute">
-    <origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
-    <parent link="left_hip_pitch_link"/>
-    <child link="left_hip_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-0.5236" upper="2.9671" effort="139" velocity="20"/>
-  </joint>
-  <link name="left_hip_yaw_link">
-    <inertial>
-      <origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
-      <mass value="1.702"/>
-      <inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hip_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hip_yaw_joint" type="revolute">
-    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
-    <parent link="left_hip_roll_link"/>
-    <child link="left_hip_yaw_link"/>
-    <axis xyz="0 0 1"/>
-    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
-  </joint>
-  <link name="left_knee_link">
-    <inertial>
-      <origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
-      <mass value="1.932"/>
-      <inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_knee_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_knee_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_knee_joint" type="revolute">
-    <origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
-    <parent link="left_hip_yaw_link"/>
-    <child link="left_knee_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
-  </joint>
-  <link name="left_ankle_pitch_link">
-    <inertial>
-      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
-      <mass value="0.074"/>
-      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_ankle_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_ankle_pitch_joint" type="revolute">
-    <origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
-    <parent link="left_knee_link"/>
-    <child link="left_ankle_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
-  </joint>
-  <link name="left_ankle_roll_link">
-    <inertial>
-      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
-      <mass value="0.608"/>
-      <inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_ankle_roll_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_ankle_roll_joint" type="revolute">
-    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
-    <parent link="left_ankle_pitch_link"/>
-    <child link="left_ankle_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
-  </joint>
-  <link name="right_hip_pitch_link">
-    <inertial>
-      <origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
-      <mass value="1.35"/>
-      <inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_pitch_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hip_pitch_joint" type="revolute">
-    <origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
-    <parent link="pelvis"/>
-    <child link="right_hip_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
-  </joint>
-  <link name="right_hip_roll_link">
-    <inertial>
-      <origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
-      <mass value="1.52"/>
-      <inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_roll_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hip_roll_joint" type="revolute">
-    <origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
-    <parent link="right_hip_pitch_link"/>
-    <child link="right_hip_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-2.9671" upper="0.5236" effort="139" velocity="20"/>
-  </joint>
-  <link name="right_hip_yaw_link">
-    <inertial>
-      <origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
-      <mass value="1.702"/>
-      <inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hip_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hip_yaw_joint" type="revolute">
-    <origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
-    <parent link="right_hip_roll_link"/>
-    <child link="right_hip_yaw_link"/>
-    <axis xyz="0 0 1"/>
-    <limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
-  </joint>
-  <link name="right_knee_link">
-    <inertial>
-      <origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
-      <mass value="1.932"/>
-      <inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_knee_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_knee_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_knee_joint" type="revolute">
-    <origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
-    <parent link="right_hip_yaw_link"/>
-    <child link="right_knee_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
-  </joint>
-  <link name="right_ankle_pitch_link">
-    <inertial>
-      <origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
-      <mass value="0.074"/>
-      <inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_ankle_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_ankle_pitch_joint" type="revolute">
-    <origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
-    <parent link="right_knee_link"/>
-    <child link="right_ankle_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
-  </joint>
-  <link name="right_ankle_roll_link">
-    <inertial>
-      <origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
-      <mass value="0.608"/>
-      <inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_ankle_roll_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-    <collision>
-      <origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
-      <geometry>
-        <sphere radius="0.005"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_ankle_roll_joint" type="revolute">
-    <origin xyz="0 0 -0.017558" rpy="0 0 0"/>
-    <parent link="right_ankle_pitch_link"/>
-    <child link="right_ankle_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
-  </joint>
-
-  <!-- Torso -->
-  <link name="waist_yaw_link">
-    <inertial>
-      <origin xyz="0.003494 0.000233 0.018034" rpy="0 0 0"/>
-      <mass value="0.214"/>
-      <inertia ixx="0.00010673" ixy="2.703E-06" ixz="-7.631E-06" iyy="0.00010422" iyz="-2.01E-07" izz="0.0001625"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/waist_yaw_link_rev_1_0.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-  </link>
-  <joint name="waist_yaw_joint" type="revolute">
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <parent link="pelvis"/>
-    <child link="waist_yaw_link"/>
-    <axis xyz="0 0 1"/>
-    <limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
-  </joint>
-  <link name="waist_roll_link">
-    <inertial>
-      <origin xyz="0 2.3E-05 0" rpy="0 0 0"/>
-      <mass value="0.086"/>
-      <inertia ixx="7.079E-06" ixy="0" ixz="0" iyy="6.339E-06" iyz="0" izz="8.245E-06"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/waist_roll_link_rev_1_0.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-  </link>
-  <joint name="waist_roll_joint" type="revolute">
-    <origin xyz="-0.0039635 0 0.044" rpy="0 0 0"/>
-    <parent link="waist_yaw_link"/>
-    <child link="waist_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
-  </joint>
-  <link name="torso_link">
-    <inertial>
-      <origin xyz="0.000931 0.000346 0.15082" rpy="0 0 0"/>
-      <mass value="6.78"/>
-      <inertia ixx="0.05905" ixy="3.3302E-05" ixz="-0.0017715" iyy="0.047014" iyz="-2.2399E-05" izz="0.025652"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/torso_link_rev_1_0.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/torso_link_rev_1_0.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="waist_pitch_joint" type="revolute">
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-    <parent link="waist_roll_link"/>
-    <child link="torso_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
-  </joint>
-
-  <!-- LOGO -->
-  <joint name="logo_joint" type="fixed">
-    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
-    <parent link="torso_link"/>
-    <child link="logo_link"/>
-  </joint>
-  <link name="logo_link">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="0.001"/>
-      <inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/logo_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/logo_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-
-  <!-- Head -->
-  <link name="head_link">
-    <inertial>
-      <origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
-      <mass value="1.036"/>
-      <inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/head_link.STL"/>
-      </geometry>
-      <material name="dark">
-        <color rgba="0.2 0.2 0.2 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/head_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="head_joint" type="fixed">
-    <origin xyz="0.0039635 0 -0.044" rpy="0 0 0"/>
-    <parent link="torso_link"/>
-    <child link="head_link"/>
-  </joint>
-
-
-  <!-- IMU -->
-  <link name="imu_in_torso"></link>
-  <joint name="imu_in_torso_joint" type="fixed">
-    <origin xyz="-0.03959 -0.00224 0.14792" rpy="0 0 0"/>
-    <parent link="torso_link"/>
-    <child link="imu_in_torso"/>
-  </joint>
-
-  <link name="imu_in_pelvis"></link>
-  <joint name="imu_in_pelvis_joint" type="fixed">
-    <origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/>
-    <parent link="pelvis"/>
-    <child link="imu_in_pelvis"/>
-  </joint>
-
-  <!-- d435 -->
-  <link name="d435_link"></link>
-  <joint name="d435_joint" type="fixed">
-    <origin xyz="0.0576235 0.01753 0.42987" rpy="0 0.8307767239493009 0"/>
-    <parent link="torso_link"/>
-    <child link="d435_link"/>
-  </joint>
-
-  <!-- mid360 -->
-  <link name="mid360_link"></link>
-  <joint name="mid360_joint" type="fixed">
-    <origin xyz="0.0002835 0.00003 0.41618" rpy="0 0.04014257279586953 0"/>
-    <parent link="torso_link"/>
-    <child link="mid360_link"/>
-  </joint>
-
-  <!-- Arm -->
-  <link name="left_shoulder_pitch_link">
-    <inertial>
-      <origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
-      <mass value="0.718"/>
-      <inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_shoulder_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
-      <geometry>
-        <cylinder radius="0.03" length="0.05"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_shoulder_pitch_joint" type="revolute">
-    <origin xyz="0.0039563 0.10022 0.24778" rpy="0.27931 5.4949E-05 -0.00019159"/>
-    <parent link="torso_link"/>
-    <child link="left_shoulder_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
-  </joint>
-  <link name="left_shoulder_roll_link">
-    <inertial>
-      <origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
-      <mass value="0.643"/>
-      <inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_shoulder_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.03" length="0.03"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_shoulder_roll_joint" type="revolute">
-    <origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
-    <parent link="left_shoulder_pitch_link"/>
-    <child link="left_shoulder_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
-  </joint>
-  <link name="left_shoulder_yaw_link">
-    <inertial>
-      <origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
-      <mass value="0.734"/>
-      <inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_shoulder_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_shoulder_yaw_joint" type="revolute">
-    <origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
-    <parent link="left_shoulder_roll_link"/>
-    <child link="left_shoulder_yaw_link"/>
-    <axis xyz="0 0 1"/>
-    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
-  </joint>
-  <link name="left_elbow_link">
-    <inertial>
-      <origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
-      <mass value="0.6"/>
-      <inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_elbow_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_elbow_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_elbow_joint" type="revolute">
-    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
-    <parent link="left_shoulder_yaw_link"/>
-    <child link="left_elbow_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
-  </joint>
-  <joint name="left_wrist_roll_joint" type="revolute">
-    <origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
-    <axis xyz="1 0 0"/>
-    <parent link="left_elbow_link"/>
-    <child link="left_wrist_roll_link"/>
-    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
-  </joint>
-  <link name="left_wrist_roll_link">
-    <inertial>
-      <origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
-      <mass value="0.08544498"/>
-      <inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_roll_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_wrist_pitch_joint" type="revolute">
-    <origin xyz="0.038 0 0" rpy="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <parent link="left_wrist_roll_link"/>
-    <child link="left_wrist_pitch_link"/>
-    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
-  </joint>
-  <link name="left_wrist_pitch_link">
-    <inertial>
-      <origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
-      <mass value="0.48404956"/>
-      <inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_wrist_yaw_joint" type="revolute">
-    <origin xyz="0.046 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_wrist_pitch_link"/>
-    <child link="left_wrist_yaw_link"/>
-    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
-  </joint>
-  <link name="left_wrist_yaw_link">
-    <inertial>
-      <origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
-      <mass value="0.08457647"/>
-      <inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_wrist_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_palm_joint" type="fixed">
-    <origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
-    <parent link="left_wrist_yaw_link"/>
-    <child link="left_hand_palm_link"/>
-  </joint>
-  <link name="left_hand_palm_link">
-    <inertial>
-      <origin xyz="0.06214634836 -0.00050869656 -0.00058171093" rpy="0 0 0"/>
-      <mass value="0.37283854"/>
-      <inertia ixx="0.00027535181027" ixy="-0.00001595519465" ixz="-0.00000242161890" iyy="0.00053951827219" iyz="-0.00000042279435" izz="0.00039623390907"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_palm_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_palm_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_thumb_0_joint" type="revolute">
-    <origin xyz="0.0255 0 0" rpy="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <parent link="left_hand_palm_link"/>
-    <child link="left_hand_thumb_0_link"/>
-    <limit effort="2.45" velocity="6.857" lower="-1.04719755" upper="1.04719755"/>
-  </joint>
-  <link name="left_hand_thumb_0_link">
-    <inertial>
-      <origin xyz="-0.00088424580 -0.00863407079 0.00094429336" rpy="0 0 0"/>
-      <mass value="0.08623657"/>
-      <inertia ixx="0.00001602919238" ixy="0.00000010683177" ixz="0.00000016728875" iyy="0.00001451795012" iyz="-0.00000051094752" izz="0.00001637877663"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_thumb_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_thumb_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_thumb_1_joint" type="revolute">
-    <origin xyz="-0.0025 -0.0193 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_thumb_0_link"/>
-    <child link="left_hand_thumb_1_link"/>
-    <limit effort="1.4" velocity="12" lower="-0.72431163" upper="1.04719755"/>
-  </joint>
-  <link name="left_hand_thumb_1_link">
-    <inertial>
-      <origin xyz="-0.00082788768 -0.03547435774 -0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00001274699945" ixy="-0.00000050770784" ixz="0.00000016088850" iyy="0.00000601573947" iyz="-0.00000027839003" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_thumb_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.001 -0.032 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.02 0.03 0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_thumb_2_joint" type="revolute">
-    <origin xyz="0 -0.0458 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_thumb_1_link"/>
-    <child link="left_hand_thumb_2_link"/>
-    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
-  </joint>
-  <link name="left_hand_thumb_2_link">
-    <inertial>
-      <origin xyz="-0.00171735242 -0.02628192939 0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000461267817" ixy="-0.00000003422130" ixz="-0.00000000823881" iyy="0.00000153561368" iyz="-0.00000002549885" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_thumb_2_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_thumb_2_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_middle_0_joint" type="revolute">
-    <origin xyz="0.0777 0.0016 -0.0285" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_palm_link"/>
-    <child link="left_hand_middle_0_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.57079632" upper="0"/>
-  </joint>
-  <link name="left_hand_middle_0_link">
-    <inertial>
-      <origin xyz="0.03547435774 0.00082788768 0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00000601573947" ixy="-0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="0.00000016088850" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_middle_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_middle_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_middle_1_joint" type="revolute">
-    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_middle_0_link"/>
-    <child link="left_hand_middle_1_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
-  </joint>
-  <link name="left_hand_middle_1_link">
-    <inertial>
-      <origin xyz="0.02628192939 0.00171735242 -0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000153561368" ixy="-0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="-0.00000000823881" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_middle_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_middle_1_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_index_0_joint" type="revolute">
-    <origin xyz="0.0777 0.0016 0.0285" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_palm_link"/>
-    <child link="left_hand_index_0_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.57079632" upper="0"/>
-  </joint>
-  <link name="left_hand_index_0_link">
-    <inertial>
-      <origin xyz="0.03547435774 0.00082788768 0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00000601573947" ixy="-0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="0.00000016088850" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_index_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_index_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="left_hand_index_1_joint" type="revolute">
-    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="left_hand_index_0_link"/>
-    <child link="left_hand_index_1_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
-  </joint>
-  <link name="left_hand_index_1_link">
-    <inertial>
-      <origin xyz="0.02628192939 0.00171735242 -0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000153561368" ixy="-0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="-0.00000000823881" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_index_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/left_hand_index_1_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="right_shoulder_pitch_link">
-    <inertial>
-      <origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
-      <mass value="0.718"/>
-      <inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_shoulder_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
-      <geometry>
-        <cylinder radius="0.03" length="0.05"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_shoulder_pitch_joint" type="revolute">
-    <origin xyz="0.0039563 -0.10021 0.24778" rpy="-0.27931 5.4949E-05 0.00019159"/>
-    <parent link="torso_link"/>
-    <child link="right_shoulder_pitch_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
-  </joint>
-  <link name="right_shoulder_roll_link">
-    <inertial>
-      <origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
-      <mass value="0.643"/>
-      <inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_shoulder_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="0.03" length="0.03"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_shoulder_roll_joint" type="revolute">
-    <origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
-    <parent link="right_shoulder_pitch_link"/>
-    <child link="right_shoulder_roll_link"/>
-    <axis xyz="1 0 0"/>
-    <limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
-  </joint>
-  <link name="right_shoulder_yaw_link">
-    <inertial>
-      <origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
-      <mass value="0.734"/>
-      <inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_shoulder_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_shoulder_yaw_joint" type="revolute">
-    <origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
-    <parent link="right_shoulder_roll_link"/>
-    <child link="right_shoulder_yaw_link"/>
-    <axis xyz="0 0 1"/>
-    <limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
-  </joint>
-  <link name="right_elbow_link">
-    <inertial>
-      <origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
-      <mass value="0.6"/>
-      <inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_elbow_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_elbow_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_elbow_joint" type="revolute">
-    <origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
-    <parent link="right_shoulder_yaw_link"/>
-    <child link="right_elbow_link"/>
-    <axis xyz="0 1 0"/>
-    <limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
-  </joint>
-  <joint name="right_wrist_roll_joint" type="revolute">
-    <origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
-    <axis xyz="1 0 0"/>
-    <parent link="right_elbow_link"/>
-    <child link="right_wrist_roll_link"/>
-    <limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
-  </joint>
-  <link name="right_wrist_roll_link">
-    <inertial>
-      <origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
-      <mass value="0.08544498"/>
-      <inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_roll_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_roll_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_wrist_pitch_joint" type="revolute">
-    <origin xyz="0.038 0 0" rpy="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <parent link="right_wrist_roll_link"/>
-    <child link="right_wrist_pitch_link"/>
-    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
-  </joint>
-  <link name="right_wrist_pitch_link">
-    <inertial>
-      <origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
-      <mass value="0.48404956"/>
-      <inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_pitch_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_wrist_yaw_joint" type="revolute">
-    <origin xyz="0.046 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_wrist_pitch_link"/>
-    <child link="right_wrist_yaw_link"/>
-    <limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
-  </joint>
-  <link name="right_wrist_yaw_link">
-    <inertial>
-      <origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
-      <mass value="0.08457647"/>
-      <inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_wrist_yaw_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_palm_joint" type="fixed">
-    <origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
-    <parent link="right_wrist_yaw_link"/>
-    <child link="right_hand_palm_link"/>
-  </joint>
-  <link name="right_hand_palm_link">
-    <inertial>
-      <origin xyz="0.06214634836 0.00050869656 -0.00058171093" rpy="0 0 0"/>
-      <mass value="0.37283854"/>
-      <inertia ixx="0.00027535181027" ixy="0.00001595519465" ixz="-0.00000242161890" iyy="0.00053951827219" iyz="0.00000042279435" izz="0.00039623390907"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_palm_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_palm_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_thumb_0_joint" type="revolute">
-    <origin xyz="0.0255 0 0" rpy="0 0 0"/>
-    <axis xyz="0 1 0"/>
-    <parent link="right_hand_palm_link"/>
-    <child link="right_hand_thumb_0_link"/>
-    <limit effort="2.45" velocity="6.857" lower="-1.04719755" upper="1.04719755"/>
-  </joint>
-  <link name="right_hand_thumb_0_link">
-    <inertial>
-      <origin xyz="-0.00088424580 0.00863407079 0.00094429336" rpy="0 0 0"/>
-      <mass value="0.08623657"/>
-      <inertia ixx="0.00001602919238" ixy="-0.00000010683177" ixz="0.00000016728875" iyy="0.00001451795012" iyz="0.00000051094752" izz="0.00001637877663"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_thumb_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_thumb_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_thumb_1_joint" type="revolute">
-    <origin xyz="-0.0025 0.0193 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_thumb_0_link"/>
-    <child link="right_hand_thumb_1_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.04719755" upper="0.72431163"/>
-  </joint>
-  <link name="right_hand_thumb_1_link">
-    <inertial>
-      <origin xyz="-0.00082788768 0.03547435774 -0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00001274699945" ixy="0.00000050770784" ixz="0.00000016088850" iyy="0.00000601573947" iyz="0.00000027839003" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_thumb_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="-0.001 0.032 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.02 0.03 0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_thumb_2_joint" type="revolute">
-    <origin xyz="0 0.0458 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_thumb_1_link"/>
-    <child link="right_hand_thumb_2_link"/>
-    <limit effort="1.4" velocity="12" lower="-1.74532925" upper="0"/>
-  </joint>
-  <link name="right_hand_thumb_2_link">
-    <inertial>
-      <origin xyz="-0.00171735242 0.02628192939 0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000461267817" ixy="0.00000003422130" ixz="-0.00000000823881" iyy="0.00000153561368" iyz="0.00000002549885" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_thumb_2_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_thumb_2_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_middle_0_joint" type="revolute">
-    <origin xyz="0.0777 -0.0016 -0.0285" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_palm_link"/>
-    <child link="right_hand_middle_0_link"/>
-    <limit effort="1.4" velocity="12" lower="0" upper="1.57079632"/>
-  </joint>
-  <link name="right_hand_middle_0_link">
-    <inertial>
-      <origin xyz="0.03547435774 -0.00082788768 0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00000601573947" ixy="0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="-0.00000016088850" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_middle_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_middle_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_middle_1_joint" type="revolute">
-    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_middle_0_link"/>
-    <child link="right_hand_middle_1_link"/>
-    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
-  </joint>
-  <link name="right_hand_middle_1_link">
-    <inertial>
-      <origin xyz="0.02628192939 -0.00171735242 -0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000153561368" ixy="0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="0.00000000823881" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_middle_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_middle_1_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_index_0_joint" type="revolute">
-    <origin xyz="0.0777 -0.0016 0.0285" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_palm_link"/>
-    <child link="right_hand_index_0_link"/>
-    <limit effort="1.4" velocity="12" lower="0" upper="1.57079632"/>
-  </joint>
-  <link name="right_hand_index_0_link">
-    <inertial>
-      <origin xyz="0.03547435774 -0.00082788768 0.00038089960" rpy="0 0 0"/>
-      <mass value="0.05885070"/>
-      <inertia ixx="0.00000601573947" ixy="0.00000050770784" ixz="-0.00000027839003" iyy="0.00001274699945" iyz="-0.00000016088850" izz="0.00001234543582"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_index_0_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_index_0_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="right_hand_index_1_joint" type="revolute">
-    <origin xyz="0.0458 0 0" rpy="0 0 0"/>
-    <axis xyz="0 0 1"/>
-    <parent link="right_hand_index_0_link"/>
-    <child link="right_hand_index_1_link"/>
-    <limit effort="1.4" velocity="12" lower="0" upper="1.74532925"/>
-  </joint>
-  <link name="right_hand_index_1_link">
-    <inertial>
-      <origin xyz="0.02628192939 -0.00171735242 -0.00010778879" rpy="0 0 0"/>
-      <mass value="0.02030626"/>
-      <inertia ixx="0.00000153561368" ixy="0.00000003422130" ixz="-0.00000002549885" iyy="0.00000461267817" iyz="0.00000000823881" izz="0.00000386625776"/>
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_index_1_link.STL"/>
-      </geometry>
-      <material name="white">
-        <color rgba="0.7 0.7 0.7 1"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="meshes/right_hand_index_1_link.STL"/>
-      </geometry>
-    </collision>
-  </link>
-</robot>

assets/meshes_exo_left/shoulder.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:98759c5e7684006fd20bfba3f082022e12b4f3700fd4dfb47119929b22b2695f
-size 194384

assets/meshes_exo_left/shoulder__2.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:ff937dd8d731370c130f534916f15b63a202cded3a9a10b87c282bb72e38483d
-size 136584

assets/meshes_exo_left/shoulder__3.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:e9b7229f1980357280d437213c1d279df46ade7d37ef5eb37e6301dc7b2c2d30
-size 577884

assets/meshes_exo_left/shoulder__4.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:3b8b58ef8958d187f60b8d10cd691e01c7f3adf483ab09271824e3b9089ea410
-size 580484

assets/meshes_exo_left/shoulder__5.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:6fcb3d9a0a3c28541390fb9d7b325b041a08ae2ffab724dece4fb0178781307b
-size 164084

assets/meshes_exo_left/wrist.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:a28d777d62dcc0fb25467a682d91811e742772fa452d1141c4bffd565db037b2
-size 520784

assets/meshes_exo_left/wrist__2.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:71b29d7a3927bad99efffe572c07271531681984d46aed5984943039c17e3346
-size 102084

assets/meshes_exo_left/wrist__3.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:c03a91b4800cdbc00bd290f77fd1bd90a744a5d9b7bb2220d515d9991f696ea6
-size 137084

assets/meshes_exo_left/wrist__4.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:d898dbcddb44c176729b5144b40490437a2a09ec50a3929b255917c522fa389c
-size 724284

assets/meshes_exo_left/wrist__5.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:8eb6c0298ff682e4e199bb5f25e9e198d54db944fecf0146203cc6714993f06f
-size 138984

assets/meshes_exo_left/wrist__6.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:92590e6c4a435046f8647e3b4d3101843fadc7a9ea14475e5008ae2aa58fae1f
-size 182184

assets/meshes_exo_right/shoulder.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:42e98b8dc6f63da139874bfef108ce4a92fd68ea332ef0d2770b35679cdec572
-size 194384

assets/meshes_exo_right/shoulder__2.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:b2089082163df5498518dd2adcb005e0f32687def5707358724ca5d1f7ac70d8
-size 136584

assets/meshes_exo_right/shoulder__3.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:0f7000c1891a9fed0ccfebbb1070e8f787547970207f1c1ae51a962d85deeed7
-size 577884

assets/meshes_exo_right/shoulder__4.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:78838a47fc73f97ab3f355ba903621b5b102ce5de278a0ecf35b3cd05ab228dc
-size 580484

assets/meshes_exo_right/shoulder__5.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:d864c1b61a23a047844799d6a924a9554887d267916ba72bdc24f1ff1434ee6d
-size 164084

assets/meshes_exo_right/wrist.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:18197e26156ff8a25ab38a2f20e03870d7b8ca56f3299351e2b54377670ee3d0
-size 520784

assets/meshes_exo_right/wrist__2.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:5b9cafe9f773c7719a30390add14efbfb0f0dfefe0287228156dca052d35c6a4
-size 102084

assets/meshes_exo_right/wrist__3.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:c1f2a8cb8edc00116626e4779d9cb3f554e33d2a436b682d3e3f99ddd0ee52f9
-size 137084

assets/meshes_exo_right/wrist__4.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:e8ca263cadaa40534fd9edff649057633aaa925c6ad56e6ab25275157182b35e
-size 724284

assets/meshes_exo_right/wrist__5.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:9c9bacaf421feab7f65bb05674eca1fd972cb84690cb694eed0032dbce3c5b22
-size 138984

assets/meshes_exo_right/wrist__6.stl

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:a8568cd6286f54f186cb769b038261e0b498585443bbde573e4ebcb2dd551acb
-size 182184

assets/scene_29dof.xml

@@ -1,5 +1,5 @@
 <mujoco model="g1_29dof scene">
-  <include file="g1_29dof_no_hand.xml"/>
+  <include file="g1_29dof_old.xml"/>
 
   <statistic center="0 0 0.5" extent="2.0"/>
 

config.yaml

@@ -1,49 +1,423 @@
-# Robot Configuration
-ROBOT_TYPE: 'g1_29dof'
-ROBOT_SCENE: "assets/scene_43dof.xml"
+GEAR_WBC_CONFIG: "gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml"
 
-# DDS Communication
-DOMAIN_ID: 0
-INTERFACE: "lo"  # "lo" for simulation, network interface with "192.168.123.222" for real robot
-SIMULATOR: "mujoco"
+# copy from g1_43dof_hist.yaml
+ROBOT_TYPE: 'g1_29dof' # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" 
+ROBOT_SCENE: "assets/scene_43dof.xml" # Robot scene with 3-finger dex hands
 
-# Joystick
-USE_JOYSTICK: 1
-JOYSTICK_TYPE: "xbox"  # "xbox" or "switch"
-JOYSTICK_DEVICE: 0
+DOMAIN_ID: 0 # Domain id
+# Network Interface, "lo" for simulation and the one with "192.168.123.222" for real robot
+INTERFACE: "lo"
+SIMULATOR: "mujoco"  # "robocasa"
 
-# Simulation Settings
-FREE_BASE: False
-enable_waist: False  # False = band on torso, True = band on pelvis
-ENABLE_ELASTIC_BAND: True
+USE_JOYSTICK: 0 # Simulate Unitree WirelessController using a gamepad (0: disable, 1: enable)
+JOYSTICK_TYPE: "xbox" # support "xbox" and "switch" gamepad layout
+JOYSTICK_DEVICE: 0 # Joystick number
 
-# Timing
-SIMULATE_DT: 0.004  # 250Hz physics
-VIEWER_DT: 0.02     # 50Hz viewer update
-IMAGE_DT: 0.033333  # ~30Hz camera
+FREE_BASE: False
+enable_waist: False  # Enable waist joints control (False = band on torso, True = band on pelvis)
 
-# Rendering
-ENABLE_ONSCREEN: true
-ENABLE_OFFSCREEN: false
-MP_START_METHOD: "spawn"
+PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information of robot
+ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1
 
-# Sensors
+SIMULATE_DT: 0.002  # Simulation timestep (500Hz)
+ENABLE_ONSCREEN: true  # Show visualization window
+ENABLE_OFFSCREEN: false  # Offscreen rendering for cameras (set to true to enable camera publishing)
+IMAGE_DT: 0.033333  # Image publishing rate ~30Hz
+MP_START_METHOD: "spawn"  # Multiprocessing start method for image subprocess
+VIEWER_DT: 0.02  # Viewer update time
+REWARD_DT: 0.02
 USE_SENSOR: False
-PRINT_SCENE_INFORMATION: True
+USE_HISTORY: True
+USE_HISTORY_LOCO: True
+USE_HISTORY_MIMIC: True
+
+GAIT_PERIOD: 0.9 # 1.25
+
+MOTOR2JOINT: [0, 1, 2, 3, 4, 5,
+              6, 7, 8, 9, 10, 11,
+              12, 13, 14, 
+              15, 16, 17, 18, 19, 20, 21, 
+              22, 23, 24, 25, 26, 27, 28]
+            
+JOINT2MOTOR: [0, 1, 2, 3, 4, 5,
+              6, 7, 8, 9, 10, 11,
+              12, 13, 14, 
+              15, 16, 17, 18, 19, 20, 21, 
+              22, 23, 24, 25, 26, 27, 28]
+            
+
+UNITREE_LEGGED_CONST:
+  HIGHLEVEL: 0xEE
+  LOWLEVEL: 0xFF
+  TRIGERLEVEL: 0xF0
+  PosStopF: 2146000000.0
+  VelStopF: 16000.0
+  MODE_MACHINE: 5
+  MODE_PR:  0
+
+JOINT_KP: [
+    100, 100, 100, 200, 20, 20,
+    100, 100, 100, 200, 20, 20,
+    400, 400, 400,
+    90, 60, 20, 60, 4, 4, 4,
+    90, 60, 20, 60, 4, 4, 4
+]
+
+
+JOINT_KD: [
+   2.5, 2.5, 2.5, 5, 0.2, 0.1,
+   2.5, 2.5, 2.5, 5, 0.2, 0.1,
+   5.0, 5.0, 5.0,
+   2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2
+]
+
+# arm kp
+# soft kp, safe, test it first
+# 50, 50, 20, 20, 10, 10, 10
+# hard kp, use only if policy is safe
+# 200, 200, 80, 80, 50, 50, 50,
+
+# MOTOR_KP: [
+#     100, 100, 100, 200, 20, 20,
+#     100, 100, 100, 200, 20, 20,
+#     400, 400, 400,
+#     50, 50, 20, 20, 10, 10, 10,
+#     50, 50, 20, 20, 10, 10, 10
+# ]
+
+MOTOR_KP: [
+    150, 150, 150, 200, 40, 40,
+    150, 150, 150, 200, 40, 40, 
+    800, 250, 800,
+    100, 100, 40, 40, 100, 100, 100,
+    100, 100, 40, 40, 100, 100, 100
+]
+
+MOTOR_KD: [
+    2, 2, 2, 4, 2, 2,
+    2, 2, 2, 4, 2, 2,
+    10, 5, 10,
+    5, 5, 2, 2, 5, 5, 5,
+    5, 5, 2, 2, 5, 5, 5
+]
+
+# MOTOR_KP: [
+#     100, 100, 100, 200, 20, 20,
+#     100, 100, 100, 200, 20, 20,
+#     400, 400, 400,
+#     90, 60, 20, 60, 4, 4, 4,
+#     90, 60, 20, 60, 4, 4, 4
+# ]
+
+
+# MOTOR_KD: [
+#    2.5, 2.5, 2.5, 5, 0.2, 0.1,
+#    2.5, 2.5, 2.5, 5, 0.2, 0.1,
+#    5.0, 5.0, 5.0,
+#    2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2,
+#     2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2
+# ]
+
+
+WeakMotorJointIndex:
+  left_hip_yaw_joint: 0
+  left_hip_roll_joint: 1
+  left_hip_pitch_joint: 2
+  left_knee_joint: 3
+  left_ankle_pitch_joint: 4
+  left_ankle_roll_joint: 5
+  right_hip_yaw_joint: 6
+  right_hip_roll_joint: 7
+  right_hip_pitch_joint: 8
+  right_knee_joint: 9
+  right_ankle_pitch_joint: 10
+  right_ankle_roll_joint: 11
+  waist_yaw_joint : 12
+  waist_roll_joint : 13
+  waist_pitch_joint : 14
+  left_shoulder_pitch_joint: 15
+  left_shoulder_roll_joint: 16
+  left_shoulder_yaw_joint: 17
+  left_elbow_joint: 18
+  left_wrist_roll_joint: 19
+  left_wrist_pitch_joint: 20
+  left_wrist_yaw_joint: 21
+  right_shoulder_pitch_joint: 22
+  right_shoulder_roll_joint: 23
+  right_shoulder_yaw_joint: 24
+  right_elbow_joint: 25
+  right_wrist_roll_joint: 26
+  right_wrist_pitch_joint: 27
+  right_wrist_yaw_joint: 28
 
-# Robot Dimensions
 NUM_MOTORS: 29
 NUM_JOINTS: 29
 NUM_HAND_MOTORS: 7
 NUM_HAND_JOINTS: 7
+NUM_UPPER_BODY_JOINTS: 17
+
+DEFAULT_DOF_ANGLES: [
+    -0.1,  # left_hip_pitch_joint 
+    0.0,  # left_hip_roll_joint
+    0.0,  # left_hip_yaw_joint
+    0.3,  # left_knee_joint
+    -0.2, # left_ankle_pitch_joint
+    0.0,  # left_ankle_roll_joint
+    -0.1, # right_hip_pitch_joint
+    0.0,  # right_hip_roll_joint
+    0.0,  # right_hip_yaw_joint
+    0.3,  # right_knee_joint
+    -0.2, # right_ankle_pitch_joint
+    0.0,  # right_ankle_roll_joint
+    0.0,  # waist_yaw_joint
+    0.0,  # waist_roll_joint
+    0.0,  # waist_pitch_joint
+    0.0,  # left_shoulder_pitch_joint
+    0.0,  # left_shoulder_roll_joint
+    0.0,  # left_shoulder_yaw_joint
+    0.0,  # left_elbow_joint
+    0.0,  # left_wrist_roll_joint
+    0.0,  # left_wrist_pitch_joint
+    0.0,  # left_wrist_yaw_joint
+    0.0,  # right_shoulder_pitch_joint
+    0.0,  # right_shoulder_roll_joint
+    0.0,  # right_shoulder_yaw_joint
+    0.0,  # right_elbow_joint
+    0.0,  # right_wrist_roll_joint
+    0.0,  # right_wrist_pitch_joint
+    0.0   # right_wrist_yaw_joint
+]
+
+DEFAULT_MOTOR_ANGLES: [
+    -0.1,  # left_hip_pitch_joint 
+    0.0,  # left_hip_roll_joint
+    0.0,  # left_hip_yaw_joint
+    0.3,  # left_knee_joint
+    -0.2, # left_ankle_pitch_joint
+    0.0,  # left_ankle_roll_joint
+    -0.1, # right_hip_pitch_joint
+    0.0,  # right_hip_roll_joint
+    0.0,  # right_hip_yaw_joint
+    0.3,  # right_knee_joint
+    -0.2, # right_ankle_pitch_joint
+    0.0,  # right_ankle_roll_joint
+    0.0,  # waist_yaw_joint
+    0.0,  # waist_roll_joint
+    0.0,  # waist_pitch_joint
+    0.0,  # left_shoulder_pitch_joint
+    0.0,  # left_shoulder_roll_joint
+    0.0,  # left_shoulder_yaw_joint
+    0.0,  # left_elbow_joint
+    0.0,  # left_wrist_roll_joint
+    0.0,  # left_wrist_pitch_joint
+    0.0,  # left_wrist_yaw_joint
+    0.0,  # right_shoulder_pitch_joint
+    0.0,  # right_shoulder_roll_joint
+    0.0,  # right_shoulder_yaw_joint
+    0.0,  # right_elbow_joint
+    0.0,  # right_wrist_roll_joint
+    0.0,  # right_wrist_pitch_joint
+    0.0   # right_wrist_yaw_joint
+]
 
-# Torque Limits (Nm) - 29 body + 14 hand = 43 total
-motor_effort_limit_list: [
-    88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # left leg
-    88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # right leg
-    88.0, 50.0, 50.0,                       # waist
-    25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # left arm
-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7,    # left hand
-    25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # right arm
-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7     # right hand
+motor_pos_lower_limit_list: [-2.5307, -0.5236, -2.7576, -0.087267, -0.87267, -0.2618, 
+                             -2.5307, -2.9671, -2.7576, -0.087267, -0.87267, -0.2618, 
+                             -2.618, -0.52, -0.52,
+                             -3.0892, -1.5882, -2.618, -1.0472, 
+                             -1.972222054, -1.61443, -1.61443,
+                             -3.0892, -2.2515, -2.618, -1.0472, 
+                             -1.972222054, -1.61443, -1.61443]
+motor_pos_upper_limit_list: [2.8798, 2.9671, 2.7576, 2.8798, 0.5236, 0.2618, 
+                            2.8798, 0.5236, 2.7576, 2.8798, 0.5236, 0.2618, 
+                            2.618, 0.52, 0.52,
+                            2.6704, 2.2515, 2.618, 2.0944, 
+                            1.972222054, 1.61443, 1.61443,
+                            2.6704, 1.5882, 2.618, 2.0944, 
+                            1.972222054, 1.61443, 1.61443]
+motor_vel_limit_list: [32.0, 32.0, 32.0, 20.0, 37.0, 37.0, 
+                      32.0, 32.0, 32.0, 20.0, 37.0, 37.0, 
+                      32.0, 37.0, 37.0, 
+                      37.0, 37.0, 37.0, 37.0, 
+                      37.0, 22.0, 22.0,
+                      37.0, 37.0, 37.0, 37.0, 
+                      37.0, 22.0, 22.0]
+motor_effort_limit_list: [88.0, 88.0, 88.0, 139.0, 50.0, 50.0, 
+                        88.0, 88.0, 88.0, 139.0, 50.0, 50.0, 
+                        88.0, 50.0, 50.0,
+                        25.0, 25.0, 25.0, 25.0, 
+                        25.0, 5.0, 5.0,
+                        2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7,
+                        25.0, 25.0, 25.0, 25.0, 
+                        25.0, 5.0, 5.0,
+                        2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
+history_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  ref_motion_phase: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_loco_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  # command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_loco_height_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  command_lin_vel: 4,
+                  command_ang_vel: 4,
+                  command_base_height: 4,
+                  command_stand: 4,
+                  ref_upper_dof_pos: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  # phase_time: 4,
+                  sin_phase: 4,
+                  cos_phase: 4
+                }
+history_mimic_config: {
+                  base_ang_vel: 4,
+                  projected_gravity: 4,
+                  dof_pos: 4,
+                  dof_vel: 4,
+                  actions: 4,
+                  ref_motion_phase: 4,
+                }
+obs_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            command_lin_vel: 2,
+            command_ang_vel: 1,
+            command_stand: 1,
+            command_base_height: 1,
+            ref_upper_dof_pos: 17, # upper body actions
+            dof_pos: 29,
+            dof_vel: 29,
+            # actions: 12, # lower body actions
+            actions: 29, # full body actions
+            phase_time: 1,
+            ref_motion_phase: 1, # mimic motion phase
+            sin_phase: 1,
+            cos_phase: 1,
+          }
+obs_loco_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            command_lin_vel: 2,
+            command_ang_vel: 1,
+            command_stand: 1,
+            command_base_height: 1,
+            ref_upper_dof_pos: 17, # upper body actions
+            dof_pos: 29,
+            dof_vel: 29,
+            actions: 12, # lower body actions
+            phase_time: 1,
+            sin_phase: 1,
+            cos_phase: 1,
+          }
+obs_mimic_dims: {
+            base_lin_vel: 3,
+            base_ang_vel: 3,
+            projected_gravity: 3,
+            dof_pos: 29,
+            dof_vel: 29,
+            actions: 29, # full body actions
+            ref_motion_phase: 1, # mimic motion phase
+          }
+obs_scales: {
+    base_lin_vel: 2.0,
+    base_ang_vel: 0.25,
+    projected_gravity: 1.0,
+    command_lin_vel: 1,
+    command_ang_vel: 1,
+    command_stand: 1,
+    command_base_height: 2, # Yuanhang: it's 2, not 1!
+    ref_upper_dof_pos: 1.0,
+    dof_pos: 1.0,
+    dof_vel: 0.05,
+    history: 1.0,
+    history_loco: 1.0,
+    history_mimic: 1.0,
+    actions: 1.0,
+    phase_time: 1.0,
+    ref_motion_phase: 1.0,
+    sin_phase: 1.0,
+    cos_phase: 1.0
+  }
+
+loco_upper_body_dof_pos: [
+  0.0, 0.0, 0.0, # waist
+  0.0, 0.3, 0.0, 1.0, # left shoulder and elbow
+  0.0, 0.0, 0.0, # left wrist
+  0.0, -0.3, 0.0, 1.0, # right shoulder and elbow
+  0.0, 0.0, 0.0 # right wrist
 ]
+
+robot_dofs: {
+  "g1_29dof": [1, 1, 1, 1, 1, 1, 
+               1, 1, 1, 1, 1, 1,
+               1, 1, 1,
+               1, 1, 1, 1, 1, 1, 1,
+               1, 1, 1, 1, 1, 1, 1],
+  "g1_29dof_anneal_23dof": [1, 1, 1, 1, 1, 1, 
+                            1, 1, 1, 1, 1, 1,
+                            1, 1, 1,
+                            1, 1, 1, 1, 0, 0, 0,
+                            1, 1, 1, 1, 0, 0, 0],
+}
+
+mimic_robot_types: {
+
+  "APT_level1": "g1_29dof_anneal_23dof",
+}
+
+
+
+
+# 01281657
+mimic_models: {
+  "APT_level1": "20250116_225127-TairanTestbed_G129dofANNEAL23dof_dm_APT_video_APT_level1_MinimalFriction-0.3_RfiTrue_Far0.325_RESUME_LARGENOISE-motion_tracking-g1_29dof_anneal_23dof/exported/model_176500.onnx",
+  
+}
+
+
+
+start_upper_body_dof_pos: {
+
+  "APT_level1":
+  [0.19964170455932617, 0.07710712403059006, -0.2882401943206787, 
+  0.21672365069389343, 0.15629297494888306, -0.5167576670646667, 0.5782126784324646,
+  0.0, 0.0, 0.0, 
+  0.25740593671798706, -0.2504104673862457, 0.22500675916671753, 0.5127624273300171,
+  0.0, 0.0, 0.0],
+
+}
+
+motion_length_s: {
+  "APT_level1": 7.66,
+
+}

env.py

@@ -1,135 +1,59 @@
 from pathlib import Path
+import subprocess
 import sys
 import gymnasium as gym
 from gymnasium import spaces
 import numpy as np
 from huggingface_hub import snapshot_download
-import yaml
+import os
+import signal
 snapshot_download("lerobot/unitree-g1-mujoco")
 
-# Ensure sim module is importable
-sys.path.insert(0, str(Path(__file__).parent))
-
-from sim.simulator_factory import SimulatorFactory, init_channel
-
 
 def make_env(n_envs=1, use_async_envs=False, **kwargs):
-    """Create a UnitreeG1 simulation environment.
-    
-    Args:
-        n_envs: Number of environments (currently only 1 supported)
-        use_async_envs: Whether to use async envs (not implemented)
-        **kwargs: Additional arguments passed to simulator
-            - publish_images: bool, whether to publish camera images via ZMQ
-            - camera_port: int, ZMQ port for camera images
-            - cameras: list of camera names
-    """
+
+    # define run_sim FIRST
     repo_dir = Path(__file__).parent
-    
-    # Load config
-    config_path = repo_dir / "config.yaml"
-    with open(config_path) as f:
-        config = yaml.safe_load(f)
-    
-    # Configure cameras if requested
-    publish_images = kwargs.get("publish_images", True)
-    camera_port = kwargs.get("camera_port", 5555)
-    cameras = kwargs.get("cameras", ["head_camera"])
-    
-    enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)
-    camera_configs = {}
-    if enable_offscreen:
-        for cam_name in cameras:
-            camera_configs[cam_name] = {"height": 480, "width": 640}
-    
-    # Initialize DDS channel
-    init_channel(config=config)
-    
-    # Create simulator (runs in same process)
-    simulator = SimulatorFactory.create_simulator(
-        config=config,
-        env_name="default",
-        onscreen=config.get("ENABLE_ONSCREEN", True),
-        offscreen=enable_offscreen,
-        camera_configs=camera_configs,
-    )
-    
-    class UnitreeG1Env(gym.Env):
-        """Gymnasium environment wrapper for Unitree G1 MuJoCo simulation."""
-        
-        metadata = {"render_modes": ["human"]}
+    run_sim = repo_dir / "run_sim.py"
+
+    print("=" * 60)
+    print("launching run_sim.py as subprocess")
+    print("path:", run_sim)
+    print("=" * 60)
+
+    # now you can launch it
+    proc = subprocess.Popen([sys.executable, str(run_sim)])
+    print(f"simulator started as pid={proc.pid}")
 
-        def __init__(self, sim, cam_configs, pub_images, cam_port):
+    class DummyEnv(gym.Env):
+        metadata = {"render_modes": []}
+
+        def __init__(self, process):
             super().__init__()
-            self.simulator = sim
-            self.sim_env = sim.sim_env
-            self.step_count = 0
-            self.camera_configs = cam_configs
-            
-            # Get timing from config
-            self.sim_dt = config["SIMULATE_DT"]
-            self.viewer_dt = config.get("VIEWER_DT", 0.02)
-            self.image_dt = config.get("IMAGE_DT", 0.033333)
-            
-            # Start image publishing subprocess if requested
-            if pub_images and len(cam_configs) > 0:
-                sim.start_image_publish_subprocess(
-                    start_method=config.get("MP_START_METHOD", "spawn"),
-                    camera_port=cam_port
-                )
-                print(f"Camera images publishing on tcp://localhost:{cam_port}")
-            
-            # Define spaces
-            num_joints = config.get("NUM_MOTORS", 29)
-            self.action_space = spaces.Box(-np.pi, np.pi, shape=(num_joints,), dtype=np.float32)
-            self.observation_space = spaces.Box(-np.inf, np.inf, shape=(num_joints * 3 + 10,), dtype=np.float32)
+            self.process = process
+            self.action_space = spaces.Box(-1, 1, shape=(1,), dtype=np.float32)
+            self.observation_space = spaces.Box(-1, 1, shape=(1,), dtype=np.float32)
 
         def reset(self, seed=None, options=None):
-            """Reset the simulation."""
             super().reset(seed=seed, options=options)
-            self.simulator.reset()
-            self.step_count = 0
-            obs = self._get_obs()
-            return obs, {}
-
-        def step(self, action=None):
-            """Execute one simulation step. Caller handles timing."""
-            # Run physics step
-            self.sim_env.sim_step()
-            
-            # Update viewer at viewer rate
-            if self.step_count % int(self.viewer_dt / self.sim_dt) == 0:
-                self.sim_env.update_viewer()
-            
-            # Update render caches at image rate (for ZMQ publishing)
-            if self.step_count % int(self.image_dt / self.sim_dt) == 0:
-                self.sim_env.update_render_caches()
-            
-            self.step_count += 1
-            
-            obs = self._get_obs()
-            reward = 0.0
-            terminated = False
-            truncated = False
+            obs = np.zeros(1, dtype=np.float32)
             info = {}
-            return obs, reward, terminated, truncated, info
-        
-        def _get_obs(self):
-            """Get current observation from simulation."""
-            obs_dict = self.sim_env.prepare_obs()
-            obs = np.concatenate([
-                obs_dict.get("body_q", np.zeros(29)),
-                obs_dict.get("body_dq", np.zeros(29)),
-                obs_dict.get("body_tau_est", np.zeros(29)),
-                obs_dict.get("floating_base_pose", np.zeros(7))[:4],
-                obs_dict.get("floating_base_vel", np.zeros(6))[:3],
-                obs_dict.get("floating_base_acc", np.zeros(3)),
-            ]).astype(np.float32)
-            return obs
+            return obs, info
 
-        def close(self):
-            """Close the simulation."""
-            print("++++closing simulator ++++")
-            self.simulator.close()
+        def step(self, action):
+            return np.zeros(1, dtype=np.float32), 0.0, False, False, {}
 
-    return UnitreeG1Env(simulator, camera_configs, publish_images, camera_port)
+        def close(self):
+            pass
+
+        def kill_sim(self):
+            if self.process.poll() is None:
+                print("killing simulator subprocess...")
+                self.process.terminate()
+                try:
+                    self.process.wait(timeout=2)
+                except subprocess.TimeoutExpired:
+                    print("force killing simulator subprocess...")
+                    self.process.kill()
+
+    return DummyEnv(proc)

run_sim.py

@@ -1,7 +1,8 @@
 #!/usr/bin/env python3
+"""Standalone MuJoCo simulator for Unitree G1"""
 import sys
 from pathlib import Path
-import time
+
 # Add sim module to path
 sys.path.insert(0, str(Path(__file__).parent))
 
@@ -9,7 +10,11 @@ import yaml
 from sim.simulator_factory import SimulatorFactory, init_channel
 
 def main(n_envs=1, use_async_envs: bool = False, 
-             publish_images=True, camera_port=5555, cameras=None, **kwargs):
+             publish_images=True, camera_port=5554, cameras=None, **kwargs):
+    # Use default values
+    publish_images = True
+    camera_port = 5554
+    cameras = None
     
     # Load config
     config_path = Path(__file__).parent / "config.yaml"
@@ -18,7 +23,14 @@ def main(n_envs=1, use_async_envs: bool = False,
     
     # Override config with default values
     enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)
-
+    
+    print("="*60)
+    print("🤖 Starting Unitree G1 MuJoCo Simulator")
+    print("="*60)
+    print(f"📁 Scene: {config['ROBOT_SCENE']}")
+    print(f"⏱️  Timestep: {config['SIMULATE_DT']}s ({int(1/config['SIMULATE_DT'])} Hz)")
+    print(f"👁️  Visualization: {'ON' if config.get('ENABLE_ONSCREEN', True) else 'OFF'}")
+    
     # Configure cameras if requested
     camera_configs = {}
     if enable_offscreen:
@@ -41,24 +53,19 @@ def main(n_envs=1, use_async_envs: bool = False,
         camera_configs=camera_configs,
     )
     
-    # Start simulator
+    # Start simulator (blocking)
     print("\nSimulator running. Press Ctrl+C to exit.")
     if enable_offscreen and publish_images:
         print(f"Camera images publishing on tcp://localhost:{camera_port}")
     try:
-        if publish_images:
-            sim.start_image_publish_subprocess(
-                start_method="spawn",
-                camera_port=camera_port,
-            )
-            time.sleep(1)
-        sim.start()
+        SimulatorFactory.start_simulator(
+            sim,
+            as_thread=False,
+            enable_image_publish=publish_images,
+            camera_port=camera_port,
+        )
     except KeyboardInterrupt:
-        print("+++++Simulator interrupted by user.")
-    except Exception as e:
-        print(f"++++error in simulator: {e} ++++")
-    finally:
-        print("++++closing simulator ++++")
+        print("\nShutting down simulator...")
         sim.close()
 
 if __name__ == "__main__":

sim/base_sim.py

@@ -2,7 +2,7 @@ import argparse
 import pathlib
 from pathlib import Path
 import threading
-from threading import Thread
+from threading import Lock, Thread
 from typing import Dict
 
 import mujoco
@@ -18,8 +18,8 @@ from unitree_sdk2py.core.channel import ChannelFactoryInitialize
 import yaml
 import os
 from .image_publish_utils import ImagePublishProcess
-from .metric_utils import check_contact
-from .sim_utils import get_subtree_body_names
+from .metric_utils import check_contact, check_height
+from .sim_utilts import get_subtree_body_names
 from .unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge
 
 GR00T_WBC_ROOT = Path(__file__).resolve().parent.parent  # Points to mujoco_sim_g1/
@@ -60,6 +60,8 @@ class DefaultEnv:
         if len(camera_configs) > 0:
             print(f"✓ DefaultEnv initialized with {len(camera_configs)} camera(s): {list(camera_configs.keys())}")
 
+        # Thread safety lock
+        self.reward_lock = Lock()
 
         # Unitree bridge will be initialized by the simulator
         self.unitree_bridge = None
@@ -69,11 +71,12 @@ class DefaultEnv:
 
         # Initialize scene (defined in subclasses)
         self.init_scene()
+        self.last_reward = 0
 
-        # Setup offscreen rendering if needed (lazy init - renderers created on first use)
+        # Setup offscreen rendering if needed
         self.offscreen = offscreen
-        self.renderers = {}  # Will be lazily initialized
-        self._renderers_initialized = False
+        if self.offscreen:
+            self.init_renderers()
         self.image_dt = self.config.get("IMAGE_DT", 0.033333)
         
         # Image publishing subprocess (initialized separately)
@@ -86,8 +89,6 @@ class DefaultEnv:
             str(assets_root / self.config["ROBOT_SCENE"])
         )
         self.mj_data = mujoco.MjData(self.mj_model)
-        # Set valid floating base quaternion (MjData initializes qpos to zeros)
-        self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
         self.mj_model.opt.timestep = self.sim_dt
         self.torso_index = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, "torso_link")
         self.root_body = "pelvis"
@@ -438,6 +439,16 @@ class DefaultEnv:
             else:
                 self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING
 
+    def update_reward(self):
+        """Calculate reward. Should be implemented by subclasses."""
+        with self.reward_lock:
+            self.last_reward = 0
+
+    def get_reward(self):
+        """Thread-safe way to get the last calculated reward."""
+        with self.reward_lock:
+            return self.last_reward
+
     def set_unitree_bridge(self, unitree_bridge):
         """Set the unitree bridge from the simulator"""
         self.unitree_bridge = unitree_bridge
@@ -448,17 +459,9 @@ class DefaultEnv:
 
     def update_render_caches(self):
         """Update render cache and shared memory for subprocess."""
-        # Lazy init renderers on first call (creates OpenGL context in calling thread)
-        if not self._renderers_initialized and self.offscreen:
-            self.init_renderers()
-            self._renderers_initialized = True
-            print(f"✓ Renderers initialized lazily in thread {__import__('threading').current_thread().name}")
-        
         render_caches = {}
         for camera_name, camera_config in self.camera_configs.items():
-            renderer = self.renderers.get(camera_name)
-            if renderer is None:
-                continue
+            renderer = self.renderers[camera_name]
             if "params" in camera_config:
                 renderer.update_scene(self.mj_data, camera=camera_config["params"])
             else:
@@ -504,11 +507,114 @@ class DefaultEnv:
 
     def reset(self):
         mujoco.mj_resetData(self.mj_model, self.mj_data)
-        # Set valid floating base quaternion (identity: w=1, x=y=z=0)
-        # mj_resetData sets qpos to zeros, which gives invalid [0,0,0,0] quaternion
-        self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
-        # Propagate qpos to derived quantities (xquat, xpos, etc.)
-        mujoco.mj_forward(self.mj_model, self.mj_data)
+
+
+class CubeEnv(DefaultEnv):
+    """Environment with a cube object for pick and place tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml"
+        super().__init__(config, "cube", {}, onscreen, offscreen)
+
+    def update_reward(self):
+        """Calculate reward based on gripper contact with cube and cube height"""
+        right_hand_body = [
+            "right_hand_thumb_2_link",
+            "right_hand_middle_1_link",
+            "right_hand_index_1_link",
+        ]
+        gripper_cube_contact = check_contact(
+            self.mj_model, self.mj_data, right_hand_body, "cube_body"
+        )
+        cube_lifted = check_height(self.mj_model, self.mj_data, "cube", 0.85, 2.0)
+
+        with self.reward_lock:
+            self.last_reward = gripper_cube_contact & cube_lifted
+
+
+class BoxEnv(DefaultEnv):
+    """Environment with a box object for manipulation tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml"
+        super().__init__(config, "box", {}, onscreen, offscreen)
+
+    def reward(self):
+        """Calculate reward based on gripper contact with cube and cube height"""
+        left_hand_body = [
+            "left_hand_thumb_2_link",
+            "left_hand_middle_1_link",
+            "left_hand_index_1_link",
+        ]
+        right_hand_body = [
+            "right_hand_thumb_2_link",
+            "right_hand_middle_1_link",
+            "right_hand_index_1_link",
+        ]
+        gripper_box_contact = check_contact(self.mj_model, self.mj_data, left_hand_body, "box_body")
+        gripper_box_contact &= check_contact(
+            self.mj_model, self.mj_data, right_hand_body, "box_body"
+        )
+        box_lifted = check_height(self.mj_model, self.mj_data, "box", 0.92, 2.0)
+
+        print("gripper_box_contact: ", gripper_box_contact, "box_lifted: ", box_lifted)
+
+        with self.reward_lock:
+            self.last_reward = gripper_box_contact & box_lifted
+            return self.last_reward
+
+
+class BottleEnv(DefaultEnv):
+    """Environment with a cylinder object for manipulation tasks"""
+
+    def __init__(
+        self,
+        config: Dict[str, any],
+        onscreen: bool = False,
+        offscreen: bool = False,
+    ):
+        # Override the robot scene
+        config = config.copy()  # Create a copy to avoid modifying the original
+        config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml"
+        camera_configs = {
+            "egoview": {
+                "height": 400,
+                "width": 400,
+            },
+        }
+        super().__init__(
+            config, "cylinder", camera_configs, onscreen, offscreen
+        )
+
+        self.bottle_body = self.mj_model.body("bottle_body")
+        self.bottle_geom = self.mj_model.geom("bottle")
+
+        if self.viewer is not None:
+            self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
+            self.viewer.cam.fixedcamid = self.mj_model.camera("egoview").id
+
+    def update_reward(self):
+        """Calculate reward based on gripper contact with cylinder and cylinder height"""
+        pass
+
+    def get_privileged_obs(self):
+        obs_pos = self.mj_data.xpos[self.bottle_body.id]
+        obs_quat = self.mj_data.xquat[self.bottle_body.id]
+        return {"bottle_pos": obs_pos, "bottle_quat": obs_quat}
 
 
 class BaseSimulator:
@@ -535,11 +641,21 @@ class BaseSimulator:
 
         # Set update frequencies
         self.sim_dt = self.config["SIMULATE_DT"]
+        self.reward_dt = self.config.get("REWARD_DT", 0.02)
         self.image_dt = self.config.get("IMAGE_DT", 0.033333)
         self.viewer_dt = self.config.get("VIEWER_DT", 0.02)
 
-        # Create the environment
-        self.sim_env = DefaultEnv(config, env_name, **kwargs)
+        # Create the appropriate environment based on name
+        if env_name == "default":
+            self.sim_env = DefaultEnv(config, env_name, **kwargs)
+        elif env_name == "pnp_cube":
+            self.sim_env = CubeEnv(config, **kwargs)
+        elif env_name == "lift_box":
+            self.sim_env = BoxEnv(config, **kwargs)
+        elif env_name == "pnp_bottle":
+            self.sim_env = BottleEnv(config, **kwargs)
+        else:
+            raise ValueError(f"Invalid environment name: {env_name}")
 
         # Initialize the DDS communication layer - should be safe to call multiple times
 
@@ -606,6 +722,10 @@ class BaseSimulator:
                 if sim_cnt % int(self.viewer_dt / self.sim_dt) == 0:
                     self.sim_env.update_viewer()
 
+                # Calculate reward at reward rate
+                if sim_cnt % int(self.reward_dt / self.sim_dt) == 0:
+                    self.sim_env.update_reward()
+
                 # Update render caches at image rate
                 if sim_cnt % int(self.image_dt / self.sim_dt) == 0:
                     self.sim_env.update_render_caches()
@@ -641,11 +761,6 @@ class BaseSimulator:
     def close(self):
         """Close the simulation. Can be overridden by subclasses."""
         try:
-            # Stop image publishing subprocess
-            if hasattr(self.sim_env, "image_publish_process") and self.sim_env.image_publish_process is not None:
-                self.sim_env.image_publish_process.stop()
-                self.sim_env.image_publish_process = None
-            
             # Close viewer
             if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None:
                 self.sim_env.viewer.close()

sim/image_publish_utils.py

@@ -5,6 +5,8 @@ from typing import Any, Dict
 
 import numpy as np
 
+from .sensor_utils import ImageMessageSchema, ImageUtils, SensorServer
+
 
 def get_multiprocessing_info(verbose: bool = True):
     """Get information about multiprocessing start methods"""

sim/metric_utils.py

@@ -2,7 +2,7 @@ from typing import List, Tuple
 
 import mujoco
 
-from .sim_utils import get_body_geom_ids
+from .sim_utilts import get_body_geom_ids
 
 
 def check_contact(

sim/sensor_utils.py

@@ -1,10 +1,11 @@
 """Standalone sensor utilities for camera image publishing via ZMQ"""
 import base64
-import json
 from dataclasses import dataclass
 from typing import Any, Dict
 
 import cv2
+import msgpack
+import msgpack_numpy as m
 import numpy as np
 import zmq
 
@@ -65,8 +66,8 @@ class SensorServer:
 
     def send_message(self, data: Dict[str, Any]):
         try:
-            json_str = json.dumps(data)
-            self.socket.send_string(json_str, flags=zmq.NOBLOCK)
+            packed = msgpack.packb(data, use_bin_type=True)
+            self.socket.send(packed, flags=zmq.NOBLOCK)
         except zmq.Again:
             self.message_dropped += 1
             print(f"[Warning] message dropped: {self.message_dropped}")
@@ -94,8 +95,8 @@ class SensorClient:
         self.context.term()
 
     def receive_message(self):
-        json_str = self.socket.recv_string()
-        return json.loads(json_str)
+        packed = self.socket.recv()
+        return msgpack.unpackb(packed, object_hook=m.decode)
 
 
 class ImageUtils:

sim/{sim_utils.py → sim_utilts.py}

@@ -75,7 +75,7 @@ def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
 
 
 def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
-    """Get all geoms belonging to subtree starting at a given body
+    """Get all geoms belonging to subtree starting at a given body.
 
     Here, a subtree is defined as the kinematic tree starting at the body and including
     all its descendants.

sim/simulator_factory.py

@@ -35,6 +35,8 @@ class SimulatorFactory:
         simulator_type = config.get("SIMULATOR", "mujoco")
         if simulator_type == "mujoco":
             return SimulatorFactory._create_mujoco_simulator(config, env_name, **kwargs)
+        elif simulator_type == "robocasa":
+            return SimulatorFactory._create_robocasa_simulator(config, env_name, **kwargs)
         else:
             print(
                 f"Warning: Invalid simulator type: {simulator_type}. "
@@ -56,3 +58,91 @@ class SimulatorFactory:
         )
         return BaseSimulator(config=config, env_name=env_name, **env_kwargs)
 
+    @staticmethod
+    def _create_robocasa_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
+        """Create a RoboCasa simulator instance."""
+        from gr00t_wbc.control.envs.g1.sim.robocasa_sim import RoboCasaG1EnvServer
+        from gr00t_wbc.control.envs.robocasa.utils.controller_utils import (
+            update_robosuite_controller_configs,
+        )
+        from gr00t_wbc.control.envs.robocasa.utils.sim_utils import change_simulation_timestep
+
+        change_simulation_timestep(config["SIMULATE_DT"])
+
+        # Use default environment if not specified
+        if env_name == "default":
+            env_name = "GroundOnly"
+
+        # Get or create controller configurations
+        controller_configs = kwargs.get("controller_configs")
+        if controller_configs is None:
+            wbc_version = kwargs.get("wbc_version", "gear_wbc")
+            controller_configs = update_robosuite_controller_configs("G1", wbc_version)
+
+        # Build environment kwargs
+        env_kwargs = dict(
+            onscreen=kwargs.pop("onscreen", True),
+            offscreen=kwargs.pop("offscreen", False),
+            camera_names=kwargs.pop("camera_names", None),
+            camera_heights=kwargs.pop("camera_heights", None),
+            camera_widths=kwargs.pop("camera_widths", None),
+            control_freq=kwargs.pop("control_freq", 50),
+            controller_configs=controller_configs,
+            ik_indicator=kwargs.pop("ik_indicator", False),
+            randomize_cameras=kwargs.pop("randomize_cameras", True),
+        )
+
+        kwargs.update(
+            {
+                "verbose": config.pop("verbose", False),
+                "sim_freq": 1 / config.pop("SIMULATE_DT"),
+            }
+        )
+
+        return RoboCasaG1EnvServer(
+            env_name=env_name,
+            wbc_config=config,
+            env_kwargs=env_kwargs,
+            **kwargs,
+        )
+
+    @staticmethod
+    def start_simulator(
+        simulator,
+        as_thread: bool = True,
+        enable_image_publish: bool = False,
+        mp_start_method: str = "spawn",
+        camera_port: int = 5555,
+    ):
+        """
+        Start the simulator either as a thread or as a separate process.
+
+        Args:
+            simulator: The simulator instance to start
+            config: Configuration dictionary
+            as_thread: If True, start as thread; if False, start as subprocess
+            enable_offscreen: If True and not as_thread, start image publishing
+        """
+
+        if as_thread:
+            simulator.start_as_thread()
+        else:
+            # Wrap in try-except to make sure simulator is properly closed upon exit.
+            try:
+                if enable_image_publish:
+                    simulator.start_image_publish_subprocess(
+                        start_method=mp_start_method,
+                        camera_port=camera_port,
+                    )
+                    time.sleep(1)
+                simulator.start()
+            except KeyboardInterrupt:
+                print("+++++Simulator interrupted by user.")
+            except Exception as e:
+                print(f"++++error in simulator: {e} ++++")
+            finally:
+                print("++++closing simulator ++++")
+                simulator.close()
+
+        # Allow simulator to initialize
+        time.sleep(1)

sim/unitree_sdk2py_bridge.py

@@ -16,18 +16,27 @@ from unitree_sdk2py.idl.default import (
     unitree_hg_msg_dds__HandState_ as HandState_default,
 )
 from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
-from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_
+try:
+    from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_, OdoState_
+    HAS_ODOSTATE = True
+except ImportError:
+    from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_
+    OdoState_ = None
+    HAS_ODOSTATE = False
+    print("Warning: OdoState_ not available in unitree_sdk2py")
 
 
 class UnitreeSdk2Bridge:
     """
-    Bridge between Unitree SDK2 and the MuJoCo simulation.
-    Handles sending and receiving DDS messages for body and hand control.
+    This class is responsible for bridging the Unitree SDK2 with the Gr00t environment.
+    It is responsible for sending and receiving messages to and from the Unitree SDK2.
+    Both the body and hand are supported.
     """
 
     def __init__(self, config):
         # Note that we do not give the mjdata and mjmodel to the UnitreeSdk2Bridge.
         # It is unsafe and would be unflexible if we use a hand-plugged robot model
+
         robot_type = config["ROBOT_TYPE"]
         if "g1" in robot_type or "h1-2" in robot_type:
             from unitree_sdk2py.idl.default import (
@@ -35,6 +44,10 @@ class UnitreeSdk2Bridge:
                 unitree_hg_msg_dds__LowCmd_,
                 unitree_hg_msg_dds__LowState_ as LowState_default,
             )
+            try:
+                from unitree_sdk2py.idl.default import unitree_hg_msg_dds__OdoState_ as OdoState_default
+            except ImportError:
+                OdoState_default = None
             from unitree_sdk2py.idl.unitree_hg.msg.dds_ import IMUState_, LowCmd_, LowState_
 
             self.low_cmd = unitree_hg_msg_dds__LowCmd_()
@@ -53,14 +66,33 @@ class UnitreeSdk2Bridge:
         self.num_body_motor = config["NUM_MOTORS"]
         self.num_hand_motor = config.get("NUM_HAND_MOTORS", 0)
         self.use_sensor = config["USE_SENSOR"]
+
         self.have_imu_ = False
         self.have_frame_sensor_ = False
+        # if self.use_sensor:
+        #     MOTOR_SENSOR_NUM = 3
+        #     self.dim_motor_sensor = MOTOR_SENSOR_NUM * self.num_motor
+        #     # Check sensor
+        #     for i in range(self.dim_motor_sensor, self.mj_model.nsensor):
+        #         name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i)
+        #         if name == "imu_quat":
+        #             self.have_imu_ = True
+        #         if name == "frame_pos":
+        #             self.have_frame_sensor_ = True
 
         # Unitree sdk2 message
         self.low_state = LowState_default()
         self.low_state_puber = ChannelPublisher("rt/lowstate", LowState_)
         self.low_state_puber.Init()
 
+        # Only create odo_state for supported robot types (if available)
+        if ("g1" in robot_type or "h1-2" in robot_type) and HAS_ODOSTATE and OdoState_default:
+            self.odo_state = OdoState_default()
+            self.odo_state_puber = ChannelPublisher("rt/odostate", OdoState_)
+            self.odo_state_puber.Init()
+        else:
+            self.odo_state = None
+            self.odo_state_puber = None
         self.torso_imu_state = IMUState_default()
         self.torso_imu_puber = ChannelPublisher("rt/secondary_imu", IMUState_)
         self.torso_imu_puber.Init()
@@ -68,7 +100,6 @@ class UnitreeSdk2Bridge:
         self.left_hand_state = HandState_default()
         self.left_hand_state_puber = ChannelPublisher("rt/dex3/left/state", HandState_)
         self.left_hand_state_puber.Init()
-
         self.right_hand_state = HandState_default()
         self.right_hand_state_puber = ChannelPublisher("rt/dex3/right/state", HandState_)
         self.right_hand_state_puber.Init()
@@ -79,7 +110,6 @@ class UnitreeSdk2Bridge:
         self.left_hand_cmd = HandCmd_default()
         self.left_hand_cmd_suber = ChannelSubscriber("rt/dex3/left/cmd", HandCmd_)
         self.left_hand_cmd_suber.Init(self.LeftHandCmdHandler, 1)
-
         self.right_hand_cmd = HandCmd_default()
         self.right_hand_cmd_suber = ChannelSubscriber("rt/dex3/right/cmd", HandCmd_)
         self.right_hand_cmd_suber.Init(self.RightHandCmdHandler, 1)
@@ -117,8 +147,29 @@ class UnitreeSdk2Bridge:
 
         # Store config for initialization
         self.config = config
-
+        
         self.reset()
+        
+        # Initialize motors with default KP/KD from config to make robot stiff at startup
+        self._initialize_motor_defaults()
+
+    def _initialize_motor_defaults(self):
+        """Initialize motor commands with default KP/KD and joint positions"""
+        if "MOTOR_KP" in self.config and "MOTOR_KD" in self.config:
+            motor_kp = self.config["MOTOR_KP"]
+            motor_kd = self.config["MOTOR_KD"]
+            default_dof_angles = self.config.get("DEFAULT_DOF_ANGLES", [0.0] * self.num_body_motor)
+            
+            for i in range(min(self.num_body_motor, len(motor_kp))):
+                self.low_cmd.motor_cmd[i].kp = motor_kp[i]
+                self.low_cmd.motor_cmd[i].kd = motor_kd[i]
+                self.low_cmd.motor_cmd[i].q = default_dof_angles[i] if i < len(default_dof_angles) else 0.0
+                self.low_cmd.motor_cmd[i].dq = 0.0
+                self.low_cmd.motor_cmd[i].tau = 0.0
+            
+            print(f"✓ Initialized {self.num_body_motor} motors with default KP/KD gains")
+        else:
+            print("⚠ Warning: MOTOR_KP/MOTOR_KD not found in config, robot will be limp at startup")
 
     def reset(self):
         with self.low_cmd_lock:
@@ -173,6 +224,12 @@ class UnitreeSdk2Bridge:
             raise NotImplementedError("Frame sensor data is not implemented yet.")
         else:
             # Get data from ground truth
+            # Publish odo state only if available
+            if self.odo_state is not None:
+                self.odo_state.position[:] = obs["floating_base_pose"][:3]
+                self.odo_state.linear_velocity[:] = obs["floating_base_vel"][:3]
+                self.odo_state.orientation[:] = obs["floating_base_pose"][3:7]
+                self.odo_state.angular_velocity[:] = obs["floating_base_vel"][3:6]
             # quaternion: w, x, y, z
             self.low_state.imu_state.quaternion[:] = obs["floating_base_pose"][3:7]
             # angular velocity
@@ -187,26 +244,13 @@ class UnitreeSdk2Bridge:
         if self.have_frame_sensor_:
             raise NotImplementedError("Frame sensor data is not implemented yet.")
         self.low_state.tick = int(obs["time"] * 1e3)
-
-        # Copy wireless controller data into lowstate for controllers that read from rt/lowstate
-        if self.joystick is not None:
-            import struct
-            # Pack wireless controller data in the format expected by unitree_g1.py RemoteController.set()
-            # bytes 0-1: padding, bytes 2-3: keys, bytes 4-7: lx, bytes 8-11: rx, 
-            # bytes 12-15: ry, bytes 16-19: padding, bytes 20-23: ly
-            keys = int(self.wireless_controller.keys)
-            lx = float(self.wireless_controller.lx)
-            ly = float(self.wireless_controller.ly)
-            rx = float(self.wireless_controller.rx)
-            ry = float(self.wireless_controller.ry)
-            wireless_data = struct.pack('<HHfffff', 0, keys, lx, rx, ry, 0.0, ly)
-            # Copy bytes into the existing wireless_remote array
-            for i, b in enumerate(wireless_data):
-                if i < len(self.low_state.wireless_remote):
-                    self.low_state.wireless_remote[i] = b
-
         self.low_state_puber.Write(self.low_state)
 
+        # Publish odo state only if available
+        if self.odo_state is not None and self.odo_state_puber is not None:
+            self.odo_state.tick = int(obs["time"] * 1e3)
+            self.odo_state_puber.Write(self.odo_state)
+
         self.torso_imu_puber.Write(self.torso_imu_state)
 
         # publish hand state
@@ -271,9 +315,6 @@ class UnitreeSdk2Bridge:
             self.wireless_controller.rx = self.joystick.get_axis(self.axis_id["RX"])
             self.wireless_controller.ry = -self.joystick.get_axis(self.axis_id["RY"])
 
-            # Debug: print joystick values
-            lx, ly, rx, ry = self.wireless_controller.lx, self.wireless_controller.ly, self.wireless_controller.rx, self.wireless_controller.ry
-
             self.wireless_controller_puber.Write(self.wireless_controller)
 
     def SetupJoystick(self, device_id=0, js_type="xbox"):
@@ -340,6 +381,7 @@ class UnitreeSdk2Bridge:
                 }
             else:
                 print("Unsupported OS. ")
+
         elif js_type == "switch":
             # Yuanhang: may differ for different OS, need to be checked
             self.axis_id = {
@@ -352,6 +394,7 @@ class UnitreeSdk2Bridge:
                 "DX": 6,  # Directional pad x
                 "DY": 7,  # Directional pad y
             }
+
             self.button_id = {
                 "X": 3,
                 "Y": 4,
@@ -373,18 +416,21 @@ class UnitreeSdk2Bridge:
             if name:
                 logger.info(f"link_index: {i}, name: {name}")
         print(" ")
+
         logger.info(colored("<<------------- Joint ------------->>", "green"))
         for i in range(self.mj_model.njnt):
             name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_JOINT, i)
             if name:
                 logger.info(f"joint_index: {i}, name: {name}")
         print(" ")
+
         logger.info(colored("<<------------- Actuator ------------->>", "green"))
         for i in range(self.mj_model.nu):
             name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_ACTUATOR, i)
             if name:
                 logger.info(f"actuator_index: {i}, name: {name}")
         print(" ")
+
         logger.info(colored("<<------------- Sensor ------------->>", "green"))
         index = 0
         for i in range(self.mj_model.nsensor):

view_cameras_live.py

@@ -202,7 +202,7 @@ def main():
     parser = argparse.ArgumentParser(description="Live camera viewer for MuJoCo simulator")
     parser.add_argument("--host", type=str, default="localhost",
                        help="Simulator host address (default: localhost)")
-    parser.add_argument("--port", type=int, default=5554,
+    parser.add_argument("--port", type=int, default=5555,
                        help="ZMQ port (default: 5555)")
     parser.add_argument("--interval", type=int, default=33,
                        help="Update interval in ms (default: 33 = ~30fps)")