diff --git a/.gitignore b/.gitignore
index b47e22cbf..c4d1f769f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -173,7 +173,3 @@ outputs/
# Dev folders
.cache/*
-*.stl
-*.urdf
-*.xml
-*.part
diff --git a/mujoco_sim_g1/CAMERA_README.md b/mujoco_sim_g1/CAMERA_README.md
deleted file mode 100644
index dd407a876..000000000
--- a/mujoco_sim_g1/CAMERA_README.md
+++ /dev/null
@@ -1,256 +0,0 @@
-# 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
-
-
-
-
-
-
-
-
-
-```
-
-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
-
-
-
-```
-
-**`assets/scene_43dof.xml`** - World-frame cameras:
-```xml
-
-
-
-```
-
-### 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
-```
-
diff --git a/mujoco_sim_g1/MUJOCO_LOG.TXT b/mujoco_sim_g1/MUJOCO_LOG.TXT
deleted file mode 100644
index daa3c30e6..000000000
--- a/mujoco_sim_g1/MUJOCO_LOG.TXT
+++ /dev/null
@@ -1,3 +0,0 @@
-Thu Nov 20 17:42:09 2025
-WARNING: Nan, Inf or huge value in QACC at DOF 0. The simulation is unstable. Time = 124.1560.
-
diff --git a/mujoco_sim_g1/README.md b/mujoco_sim_g1/README.md
deleted file mode 100644
index 3fa6b01e1..000000000
--- a/mujoco_sim_g1/README.md
+++ /dev/null
@@ -1,167 +0,0 @@
-# Standalone MuJoCo Simulator for Unitree G1
-
-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
-
-```
-
-### 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.
-
diff --git a/mujoco_sim_g1/assets/meshes/head_link.STL b/mujoco_sim_g1/assets/meshes/head_link.STL
deleted file mode 100644
index 2ee5fba15..000000000
Binary files a/mujoco_sim_g1/assets/meshes/head_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_ankle_pitch_link.STL b/mujoco_sim_g1/assets/meshes/left_ankle_pitch_link.STL
deleted file mode 100644
index 69de84901..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_ankle_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_ankle_roll_link.STL b/mujoco_sim_g1/assets/meshes/left_ankle_roll_link.STL
deleted file mode 100644
index 8864e9f98..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_ankle_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_elbow_link.STL b/mujoco_sim_g1/assets/meshes/left_elbow_link.STL
deleted file mode 100644
index 1a96d99ba..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_elbow_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_elbow_link_merge.STL b/mujoco_sim_g1/assets/meshes/left_elbow_link_merge.STL
deleted file mode 100644
index c3b9dad9b..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_elbow_link_merge.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_index_0_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_index_0_link.STL
deleted file mode 100644
index 8069369af..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_index_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_index_1_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_index_1_link.STL
deleted file mode 100644
index 89d231d7e..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_index_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_middle_0_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_middle_0_link.STL
deleted file mode 100644
index 8069369af..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_middle_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_middle_1_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_middle_1_link.STL
deleted file mode 100644
index 89d231d7e..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_middle_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_palm_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_palm_link.STL
deleted file mode 100644
index 7d595ed8f..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_palm_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_thumb_0_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_thumb_0_link.STL
deleted file mode 100644
index 3028bb4d6..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_thumb_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_thumb_1_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_thumb_1_link.STL
deleted file mode 100644
index d1c080c86..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_thumb_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hand_thumb_2_link.STL b/mujoco_sim_g1/assets/meshes/left_hand_thumb_2_link.STL
deleted file mode 100644
index 8b32e9663..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hand_thumb_2_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hip_pitch_link.STL b/mujoco_sim_g1/assets/meshes/left_hip_pitch_link.STL
deleted file mode 100644
index 5b751c767..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hip_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hip_roll_link.STL b/mujoco_sim_g1/assets/meshes/left_hip_roll_link.STL
deleted file mode 100644
index 778437ffe..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hip_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_hip_yaw_link.STL b/mujoco_sim_g1/assets/meshes/left_hip_yaw_link.STL
deleted file mode 100644
index 383093ab9..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_hip_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_knee_link.STL b/mujoco_sim_g1/assets/meshes/left_knee_link.STL
deleted file mode 100644
index f2e98e54e..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_knee_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_rubber_hand.STL b/mujoco_sim_g1/assets/meshes/left_rubber_hand.STL
deleted file mode 100644
index c44830f1f..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_rubber_hand.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_shoulder_pitch_link.STL b/mujoco_sim_g1/assets/meshes/left_shoulder_pitch_link.STL
deleted file mode 100644
index e698311fb..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_shoulder_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_shoulder_roll_link.STL b/mujoco_sim_g1/assets/meshes/left_shoulder_roll_link.STL
deleted file mode 100644
index 80bca84ac..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_shoulder_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_shoulder_yaw_link.STL b/mujoco_sim_g1/assets/meshes/left_shoulder_yaw_link.STL
deleted file mode 100644
index 281e69905..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_shoulder_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_wrist_pitch_link.STL b/mujoco_sim_g1/assets/meshes/left_wrist_pitch_link.STL
deleted file mode 100644
index 82cc224a8..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_wrist_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_wrist_roll_link.STL b/mujoco_sim_g1/assets/meshes/left_wrist_roll_link.STL
deleted file mode 100644
index f3c263a7a..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_wrist_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_wrist_roll_rubber_hand.STL b/mujoco_sim_g1/assets/meshes/left_wrist_roll_rubber_hand.STL
deleted file mode 100644
index 8fa435b66..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_wrist_roll_rubber_hand.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/left_wrist_yaw_link.STL b/mujoco_sim_g1/assets/meshes/left_wrist_yaw_link.STL
deleted file mode 100644
index 31be4fd45..000000000
Binary files a/mujoco_sim_g1/assets/meshes/left_wrist_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/logo_link.STL b/mujoco_sim_g1/assets/meshes/logo_link.STL
deleted file mode 100644
index e97920985..000000000
Binary files a/mujoco_sim_g1/assets/meshes/logo_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/pelvis.STL b/mujoco_sim_g1/assets/meshes/pelvis.STL
deleted file mode 100644
index 691a779b9..000000000
Binary files a/mujoco_sim_g1/assets/meshes/pelvis.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/pelvis_contour_link.STL b/mujoco_sim_g1/assets/meshes/pelvis_contour_link.STL
deleted file mode 100644
index 42434339a..000000000
Binary files a/mujoco_sim_g1/assets/meshes/pelvis_contour_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_ankle_pitch_link.STL b/mujoco_sim_g1/assets/meshes/right_ankle_pitch_link.STL
deleted file mode 100644
index e77d8a2fe..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_ankle_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_ankle_roll_link.STL b/mujoco_sim_g1/assets/meshes/right_ankle_roll_link.STL
deleted file mode 100644
index d4261dd7c..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_ankle_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_elbow_link.STL b/mujoco_sim_g1/assets/meshes/right_elbow_link.STL
deleted file mode 100644
index f259e3812..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_elbow_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_elbow_link_merge.STL b/mujoco_sim_g1/assets/meshes/right_elbow_link_merge.STL
deleted file mode 100644
index 83ce0ba0c..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_elbow_link_merge.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_index_0_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_index_0_link.STL
deleted file mode 100644
index f87ad3212..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_index_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_index_1_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_index_1_link.STL
deleted file mode 100644
index 6dea51ad9..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_index_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_middle_0_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_middle_0_link.STL
deleted file mode 100644
index f87ad3212..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_middle_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_middle_1_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_middle_1_link.STL
deleted file mode 100644
index 6dea51ad9..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_middle_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_palm_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_palm_link.STL
deleted file mode 100644
index 5ae00a783..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_palm_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_thumb_0_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_thumb_0_link.STL
deleted file mode 100644
index 1cae7f18e..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_thumb_0_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_thumb_1_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_thumb_1_link.STL
deleted file mode 100644
index c141fbf5a..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_thumb_1_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hand_thumb_2_link.STL b/mujoco_sim_g1/assets/meshes/right_hand_thumb_2_link.STL
deleted file mode 100644
index e942923c5..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hand_thumb_2_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hip_pitch_link.STL b/mujoco_sim_g1/assets/meshes/right_hip_pitch_link.STL
deleted file mode 100644
index 998a0a0f5..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hip_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hip_roll_link.STL b/mujoco_sim_g1/assets/meshes/right_hip_roll_link.STL
deleted file mode 100644
index 47b2eebdd..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hip_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_hip_yaw_link.STL b/mujoco_sim_g1/assets/meshes/right_hip_yaw_link.STL
deleted file mode 100644
index 371856427..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_hip_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_knee_link.STL b/mujoco_sim_g1/assets/meshes/right_knee_link.STL
deleted file mode 100644
index 76d21a3d8..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_knee_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_rubber_hand.STL b/mujoco_sim_g1/assets/meshes/right_rubber_hand.STL
deleted file mode 100644
index 0aacffbb8..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_rubber_hand.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_shoulder_pitch_link.STL b/mujoco_sim_g1/assets/meshes/right_shoulder_pitch_link.STL
deleted file mode 100644
index 3f5b4ed47..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_shoulder_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_shoulder_roll_link.STL b/mujoco_sim_g1/assets/meshes/right_shoulder_roll_link.STL
deleted file mode 100644
index 179d61753..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_shoulder_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_shoulder_yaw_link.STL b/mujoco_sim_g1/assets/meshes/right_shoulder_yaw_link.STL
deleted file mode 100644
index 2ba6076a8..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_shoulder_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_wrist_pitch_link.STL b/mujoco_sim_g1/assets/meshes/right_wrist_pitch_link.STL
deleted file mode 100644
index da194543c..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_wrist_pitch_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_wrist_roll_link.STL b/mujoco_sim_g1/assets/meshes/right_wrist_roll_link.STL
deleted file mode 100644
index 26868d228..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_wrist_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_wrist_roll_rubber_hand.STL b/mujoco_sim_g1/assets/meshes/right_wrist_roll_rubber_hand.STL
deleted file mode 100644
index c365aa9d8..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_wrist_roll_rubber_hand.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/right_wrist_yaw_link.STL b/mujoco_sim_g1/assets/meshes/right_wrist_yaw_link.STL
deleted file mode 100644
index d78890286..000000000
Binary files a/mujoco_sim_g1/assets/meshes/right_wrist_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_constraint_L_link.STL b/mujoco_sim_g1/assets/meshes/torso_constraint_L_link.STL
deleted file mode 100644
index 75d82f578..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_constraint_L_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_constraint_L_rod_link.STL b/mujoco_sim_g1/assets/meshes/torso_constraint_L_rod_link.STL
deleted file mode 100644
index 6747f3f93..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_constraint_L_rod_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_constraint_R_link.STL b/mujoco_sim_g1/assets/meshes/torso_constraint_R_link.STL
deleted file mode 100644
index 5cb5958ba..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_constraint_R_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_constraint_R_rod_link.STL b/mujoco_sim_g1/assets/meshes/torso_constraint_R_rod_link.STL
deleted file mode 100644
index 95cf415f7..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_constraint_R_rod_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_link.STL b/mujoco_sim_g1/assets/meshes/torso_link.STL
deleted file mode 100644
index 17745af9c..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/torso_link_rev_1_0.STL b/mujoco_sim_g1/assets/meshes/torso_link_rev_1_0.STL
deleted file mode 100644
index 8a759a709..000000000
Binary files a/mujoco_sim_g1/assets/meshes/torso_link_rev_1_0.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_constraint_L.STL b/mujoco_sim_g1/assets/meshes/waist_constraint_L.STL
deleted file mode 100644
index 911410fa5..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_constraint_L.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_constraint_R.STL b/mujoco_sim_g1/assets/meshes/waist_constraint_R.STL
deleted file mode 100644
index babe79492..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_constraint_R.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_roll_link.STL b/mujoco_sim_g1/assets/meshes/waist_roll_link.STL
deleted file mode 100644
index 65831abd2..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_roll_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_roll_link_rev_1_0.STL b/mujoco_sim_g1/assets/meshes/waist_roll_link_rev_1_0.STL
deleted file mode 100644
index a64f330c5..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_roll_link_rev_1_0.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_support_link.STL b/mujoco_sim_g1/assets/meshes/waist_support_link.STL
deleted file mode 100644
index 63660fb6e..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_support_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_yaw_link.STL b/mujoco_sim_g1/assets/meshes/waist_yaw_link.STL
deleted file mode 100644
index 7d36b02cc..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_yaw_link.STL and /dev/null differ
diff --git a/mujoco_sim_g1/assets/meshes/waist_yaw_link_rev_1_0.STL b/mujoco_sim_g1/assets/meshes/waist_yaw_link_rev_1_0.STL
deleted file mode 100644
index 0fabb63d3..000000000
Binary files a/mujoco_sim_g1/assets/meshes/waist_yaw_link_rev_1_0.STL and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000000.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000000.jpg
deleted file mode 100644
index 63350956f..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000000.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000001.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000001.jpg
deleted file mode 100644
index fdfcda027..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000001.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000002.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000002.jpg
deleted file mode 100644
index b6e2d77b8..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000002.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000003.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000003.jpg
deleted file mode 100644
index 5bea804b0..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000003.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000004.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000004.jpg
deleted file mode 100644
index d13ea5c70..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000004.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000005.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000005.jpg
deleted file mode 100644
index 63c5d4a74..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000005.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000006.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000006.jpg
deleted file mode 100644
index 7b5db10fb..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000006.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000007.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000007.jpg
deleted file mode 100644
index 4aa1802fe..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000007.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000008.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000008.jpg
deleted file mode 100644
index e5f96f78e..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000008.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000009.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000009.jpg
deleted file mode 100644
index e9a3605d4..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000009.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000010.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000010.jpg
deleted file mode 100644
index 6b1083d68..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000010.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000011.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000011.jpg
deleted file mode 100644
index 1943e01c2..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000011.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000012.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000012.jpg
deleted file mode 100644
index 211ead630..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000012.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000013.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000013.jpg
deleted file mode 100644
index 5a12a4d51..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000013.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000014.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000014.jpg
deleted file mode 100644
index 2994ab3d4..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000014.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000015.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000015.jpg
deleted file mode 100644
index 1c149e146..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000015.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000016.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000016.jpg
deleted file mode 100644
index 475c0267b..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000016.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000017.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000017.jpg
deleted file mode 100644
index 10e99a45a..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000017.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000018.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000018.jpg
deleted file mode 100644
index 59c91e6d3..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000018.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000019.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000019.jpg
deleted file mode 100644
index c399d18fc..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000019.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000020.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000020.jpg
deleted file mode 100644
index 2e42c1fc9..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000020.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000021.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000021.jpg
deleted file mode 100644
index 1d451cc13..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000021.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000022.jpg b/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000022.jpg
deleted file mode 100644
index 866ce223d..000000000
Binary files a/mujoco_sim_g1/camera_recordings/head_camera/head_camera_000022.jpg and /dev/null differ
diff --git a/mujoco_sim_g1/config.yaml b/mujoco_sim_g1/config.yaml
deleted file mode 100644
index b85a34355..000000000
--- a/mujoco_sim_g1/config.yaml
+++ /dev/null
@@ -1,423 +0,0 @@
-GEAR_WBC_CONFIG: "gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml"
-
-# 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
-
-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"
-
-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
-
-FREE_BASE: False
-enable_waist: False # Enable waist joints control (False = band on torso, True = band on pelvis)
-
-PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information of robot
-ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1
-
-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
-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
-
-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
-]
-
-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,
-
-}
diff --git a/mujoco_sim_g1/requirements.txt b/mujoco_sim_g1/requirements.txt
deleted file mode 100644
index 32e4c434a..000000000
--- a/mujoco_sim_g1/requirements.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-mujoco>=3.0.0
-numpy>=1.24.0
-pyyaml>=6.0
-unitree-sdk2py>=1.0.0
-loguru>=0.7.0
-
-# Camera publishing dependencies
-opencv-python>=4.8.0
-pyzmq>=25.0.0
-msgpack>=1.0.0
-msgpack-numpy>=0.4.8
-matplotlib>=3.5.0 # For live camera viewer
-
diff --git a/mujoco_sim_g1/run_sim.py b/mujoco_sim_g1/run_sim.py
deleted file mode 100755
index 7b86b86e9..000000000
--- a/mujoco_sim_g1/run_sim.py
+++ /dev/null
@@ -1,77 +0,0 @@
-#!/usr/bin/env python3
-"""Standalone MuJoCo simulator for Unitree G1"""
-import argparse
-import sys
-from pathlib import Path
-
-# Add sim module to path
-sys.path.insert(0, str(Path(__file__).parent))
-
-import yaml
-from sim.simulator_factory import SimulatorFactory, init_channel
-
-def main():
- parser = argparse.ArgumentParser(description="Unitree G1 MuJoCo Simulator")
- parser.add_argument("--publish-images", action="store_true",
- help="Enable camera image publishing via ZMQ (requires offscreen rendering)")
- parser.add_argument("--camera-port", type=int, default=5555,
- help="ZMQ port for camera publishing (default: 5555)")
- parser.add_argument("--cameras", type=str, nargs="+", default=None,
- help="Camera names to publish (default: head_camera)")
- args = parser.parse_args()
-
- # Load config
- config_path = Path(__file__).parent / "config.yaml"
- with open(config_path) as f:
- config = yaml.safe_load(f)
-
- # Override config with command line args
- enable_offscreen = args.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:
- camera_list = args.cameras or ["head_camera"]
- for cam_name in camera_list:
- camera_configs[cam_name] = {"height": 480, "width": 640}
- print(f"📷 Cameras: {', '.join(camera_list)} → ZMQ port {args.camera_port}")
-
- print("="*60)
-
- # Initialize DDS channel
- init_channel(config=config)
-
- # Create simulator
- sim = SimulatorFactory.create_simulator(
- config=config,
- env_name="default",
- onscreen=config.get("ENABLE_ONSCREEN", True),
- offscreen=enable_offscreen,
- camera_configs=camera_configs,
- )
-
- # Start simulator (blocking)
- print("\nSimulator running. Press Ctrl+C to exit.")
- if enable_offscreen and args.publish_images:
- print(f"Camera images publishing on tcp://localhost:{args.camera_port}")
- try:
- SimulatorFactory.start_simulator(
- sim,
- as_thread=False,
- enable_image_publish=args.publish_images,
- camera_port=args.camera_port,
- )
- except KeyboardInterrupt:
- print("\nShutting down simulator...")
- sim.close()
-
-if __name__ == "__main__":
- main()
-
diff --git a/mujoco_sim_g1/sim/__init__.py b/mujoco_sim_g1/sim/__init__.py
deleted file mode 100644
index e69de29bb..000000000
diff --git a/mujoco_sim_g1/sim/base_sim.py b/mujoco_sim_g1/sim/base_sim.py
deleted file mode 100644
index 707718bfc..000000000
--- a/mujoco_sim_g1/sim/base_sim.py
+++ /dev/null
@@ -1,803 +0,0 @@
-import argparse
-import pathlib
-from pathlib import Path
-import threading
-from threading import Lock, Thread
-from typing import Dict
-
-import mujoco
-import mujoco.viewer
-import numpy as np
-try:
- import rclpy
- HAS_RCLPY = True
-except ImportError:
- HAS_RCLPY = False
- print("Warning: rclpy not found. Camera image publishing will be disabled.")
-from unitree_sdk2py.core.channel import ChannelFactoryInitialize
-import yaml
-
-from .image_publish_utils import ImagePublishProcess
-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/
-
-
-class DefaultEnv:
- """Base environment class that handles simulation environment setup and step"""
-
- def __init__(
- self,
- config: Dict[str, any],
- env_name: str = "default",
- camera_configs: Dict[str, any] = None,
- onscreen: bool = False,
- offscreen: bool = False,
- ):
- # Avoid mutable default argument gotcha
- if camera_configs is None:
- camera_configs = {}
-
- # global_view is only set up for this specifc scene for now.
- if config["ROBOT_SCENE"] == "gr00t_wbc/control/robot_model/model_data/g1/scene_29dof.xml":
- camera_configs["global_view"] = {
- "height": 400,
- "width": 400,
- }
- self.config = config
- self.env_name = env_name
- self.num_body_dof = self.config["NUM_JOINTS"]
- self.num_hand_dof = self.config["NUM_HAND_JOINTS"]
- self.sim_dt = self.config["SIMULATE_DT"]
- self.obs = None
- self.torques = np.zeros(self.num_body_dof + self.num_hand_dof * 2)
- self.torque_limit = np.array(self.config["motor_effort_limit_list"])
- self.camera_configs = camera_configs
-
- # Debug: print camera config
- 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
-
- # Store display mode
- self.onscreen = onscreen
-
- # Initialize scene (defined in subclasses)
- self.init_scene()
- self.last_reward = 0
-
- # Setup offscreen rendering if needed
- self.offscreen = offscreen
- if self.offscreen:
- self.init_renderers()
- self.image_dt = self.config.get("IMAGE_DT", 0.033333)
-
- # Image publishing subprocess (initialized separately)
- self.image_publish_process = None
-
- def init_scene(self):
- """Initialize the default robot scene"""
- self.mj_model = mujoco.MjModel.from_xml_path(
- str(pathlib.Path(GR00T_WBC_ROOT) / self.config["ROBOT_SCENE"])
- )
- self.mj_data = mujoco.MjData(self.mj_model)
- 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"
- # Enable the elastic band
- if self.config["ENABLE_ELASTIC_BAND"]:
- self.elastic_band = ElasticBand()
- if "g1" in self.config["ROBOT_TYPE"]:
- if self.config["enable_waist"]:
- self.band_attached_link = self.mj_model.body("pelvis").id
- else:
- self.band_attached_link = self.mj_model.body("torso_link").id
- elif "h1" in self.config["ROBOT_TYPE"]:
- self.band_attached_link = self.mj_model.body("torso_link").id
- else:
- self.band_attached_link = self.mj_model.body("base_link").id
-
- if self.onscreen:
- self.viewer = mujoco.viewer.launch_passive(
- self.mj_model,
- self.mj_data,
- key_callback=self.elastic_band.MujuocoKeyCallback,
- show_left_ui=False,
- show_right_ui=False,
- )
- else:
- mujoco.mj_forward(self.mj_model, self.mj_data)
- self.viewer = None
- else:
- if self.onscreen:
- self.viewer = mujoco.viewer.launch_passive(
- self.mj_model, self.mj_data, show_left_ui=False, show_right_ui=False
- )
- else:
- mujoco.mj_forward(self.mj_model, self.mj_data)
- self.viewer = None
-
- if self.viewer:
- # viewer camera
- self.viewer.cam.azimuth = 120 # Horizontal rotation in degrees
- self.viewer.cam.elevation = -30 # Vertical tilt in degrees
- self.viewer.cam.distance = 2.0 # Distance from camera to target
- self.viewer.cam.lookat = np.array([0, 0, 0.5]) # Point the camera is looking at
-
- # Note that the actuator order is the same as the joint order in the mujoco model.
- self.body_joint_index = []
- self.left_hand_index = []
- self.right_hand_index = []
- for i in range(self.mj_model.njnt):
- name = self.mj_model.joint(i).name
- if any(
- [
- part_name in name
- for part_name in ["hip", "knee", "ankle", "waist", "shoulder", "elbow", "wrist"]
- ]
- ):
- self.body_joint_index.append(i)
- elif "left_hand" in name:
- self.left_hand_index.append(i)
- elif "right_hand" in name:
- self.right_hand_index.append(i)
-
- assert len(self.body_joint_index) == self.config["NUM_JOINTS"], \
- f"Expected {self.config['NUM_JOINTS']} body joints, got {len(self.body_joint_index)}"
- # Hand joints are optional (some models don't have hands)
- if self.config.get("NUM_HAND_JOINTS", 0) > 0:
- expected_hands = self.config["NUM_HAND_JOINTS"]
- if len(self.left_hand_index) != expected_hands or len(self.right_hand_index) != expected_hands:
- print(f"Warning: Expected {expected_hands} hand joints, got left={len(self.left_hand_index)}, right={len(self.right_hand_index)}")
- print("Continuing without hands...")
-
- self.body_joint_index = np.array(self.body_joint_index)
- self.left_hand_index = np.array(self.left_hand_index)
- self.right_hand_index = np.array(self.right_hand_index)
-
- def init_renderers(self):
- # Initialize camera renderers
- self.renderers = {}
- for camera_name, camera_config in self.camera_configs.items():
- renderer = mujoco.Renderer(
- self.mj_model, height=camera_config["height"], width=camera_config["width"]
- )
- self.renderers[camera_name] = renderer
-
- def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555):
- """Start image publishing subprocess using ZMQ"""
- # Use spawn method for better GIL isolation, or configured method
- if len(self.camera_configs) == 0:
- print(
- "Warning: No camera configs provided, image publishing subprocess will not be started"
- )
- return
- start_method = self.config.get("MP_START_METHOD", "spawn")
- self.image_publish_process = ImagePublishProcess(
- camera_configs=self.camera_configs,
- image_dt=self.image_dt,
- zmq_port=camera_port,
- start_method=start_method,
- verbose=self.config.get("verbose", False),
- )
- self.image_publish_process.start_process()
- print(f"✓ Started image publishing subprocess on ZMQ port {camera_port}")
-
- def compute_body_torques(self) -> np.ndarray:
- """Compute body torques based on the current robot state"""
- body_torques = np.zeros(self.num_body_dof)
- if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
- for i in range(self.unitree_bridge.num_body_motor):
- if self.unitree_bridge.use_sensor:
- body_torques[i] = (
- self.unitree_bridge.low_cmd.motor_cmd[i].tau
- + self.unitree_bridge.low_cmd.motor_cmd[i].kp
- * (self.unitree_bridge.low_cmd.motor_cmd[i].q - self.mj_data.sensordata[i])
- + self.unitree_bridge.low_cmd.motor_cmd[i].kd
- * (
- self.unitree_bridge.low_cmd.motor_cmd[i].dq
- - self.mj_data.sensordata[i + self.unitree_bridge.num_body_motor]
- )
- )
- else:
- body_torques[i] = (
- self.unitree_bridge.low_cmd.motor_cmd[i].tau
- + self.unitree_bridge.low_cmd.motor_cmd[i].kp
- * (
- self.unitree_bridge.low_cmd.motor_cmd[i].q
- - self.mj_data.qpos[self.body_joint_index[i] + 7 - 1]
- )
- + self.unitree_bridge.low_cmd.motor_cmd[i].kd
- * (
- self.unitree_bridge.low_cmd.motor_cmd[i].dq
- - self.mj_data.qvel[self.body_joint_index[i] + 6 - 1]
- )
- )
- return body_torques
-
- def compute_hand_torques(self) -> np.ndarray:
- """Compute hand torques based on the current robot state"""
- left_hand_torques = np.zeros(self.num_hand_dof)
- right_hand_torques = np.zeros(self.num_hand_dof)
- if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
- for i in range(self.unitree_bridge.num_hand_motor):
- left_hand_torques[i] = (
- self.unitree_bridge.left_hand_cmd.motor_cmd[i].tau
- + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kp
- * (
- self.unitree_bridge.left_hand_cmd.motor_cmd[i].q
- - self.mj_data.qpos[self.left_hand_index[i] + 7 - 1]
- )
- + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kd
- * (
- self.unitree_bridge.left_hand_cmd.motor_cmd[i].dq
- - self.mj_data.qvel[self.left_hand_index[i] + 6 - 1]
- )
- )
- right_hand_torques[i] = (
- self.unitree_bridge.right_hand_cmd.motor_cmd[i].tau
- + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kp
- * (
- self.unitree_bridge.right_hand_cmd.motor_cmd[i].q
- - self.mj_data.qpos[self.right_hand_index[i] + 7 - 1]
- )
- + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kd
- * (
- self.unitree_bridge.right_hand_cmd.motor_cmd[i].dq
- - self.mj_data.qvel[self.right_hand_index[i] + 6 - 1]
- )
- )
- return np.concatenate((left_hand_torques, right_hand_torques))
-
- def compute_body_qpos(self) -> np.ndarray:
- """Compute body joint positions based on the current command"""
- body_qpos = np.zeros(self.num_body_dof)
- if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
- for i in range(self.unitree_bridge.num_body_motor):
- body_qpos[i] = self.unitree_bridge.low_cmd.motor_cmd[i].q
- return body_qpos
-
- def compute_hand_qpos(self) -> np.ndarray:
- """Compute hand joint positions based on the current command"""
- hand_qpos = np.zeros(self.num_hand_dof * 2)
- if self.unitree_bridge is not None and self.unitree_bridge.low_cmd:
- for i in range(self.unitree_bridge.num_hand_motor):
- hand_qpos[i] = self.unitree_bridge.left_hand_cmd.motor_cmd[i].q
- hand_qpos[i + self.num_hand_dof] = self.unitree_bridge.right_hand_cmd.motor_cmd[i].q
- return hand_qpos
-
- def prepare_obs(self) -> Dict[str, any]:
- """Prepare observation dictionary from the current robot state"""
- obs = {}
- obs["floating_base_pose"] = self.mj_data.qpos[:7]
- obs["floating_base_vel"] = self.mj_data.qvel[:6]
- obs["floating_base_acc"] = self.mj_data.qacc[:6]
- obs["secondary_imu_quat"] = self.mj_data.xquat[self.torso_index]
- obs["secondary_imu_vel"] = self.mj_data.cvel[self.torso_index]
- obs["body_q"] = self.mj_data.qpos[self.body_joint_index + 7 - 1]
- obs["body_dq"] = self.mj_data.qvel[self.body_joint_index + 6 - 1]
- obs["body_ddq"] = self.mj_data.qacc[self.body_joint_index + 6 - 1]
- obs["body_tau_est"] = self.mj_data.actuator_force[self.body_joint_index - 1]
- if self.num_hand_dof > 0:
- obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_index + 7 - 1]
- obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_index + 6 - 1]
- obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_index + 6 - 1]
- obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_index - 1]
- obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_index + 7 - 1]
- obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_index + 6 - 1]
- obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_index + 6 - 1]
- obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_index - 1]
- obs["time"] = self.mj_data.time
- return obs
-
- def sim_step(self):
- self.obs = self.prepare_obs()
- self.unitree_bridge.PublishLowState(self.obs)
- if self.unitree_bridge.joystick:
- self.unitree_bridge.PublishWirelessController()
- if self.config["ENABLE_ELASTIC_BAND"]:
- if self.elastic_band.enable:
- # Get Cartesian pose and velocity of the band_attached_link
- pose = np.concatenate(
- [
- self.mj_data.xpos[self.band_attached_link], # link position in world
- self.mj_data.xquat[
- self.band_attached_link
- ], # link quaternion in world [w,x,y,z]
- np.zeros(6), # placeholder for velocity
- ]
- )
-
- # Get velocity in world frame
- mujoco.mj_objectVelocity(
- self.mj_model,
- self.mj_data,
- mujoco.mjtObj.mjOBJ_BODY,
- self.band_attached_link,
- pose[7:13],
- 0, # 0 for world frame
- )
-
- # Reorder velocity from [ang, lin] to [lin, ang]
- pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy()
- self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose)
- else:
- # explicitly resetting the force when the band is not enabled
- self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)
- body_torques = self.compute_body_torques()
- hand_torques = self.compute_hand_torques()
- self.torques[self.body_joint_index - 1] = body_torques
- if self.num_hand_dof > 0:
- self.torques[self.left_hand_index - 1] = hand_torques[: self.num_hand_dof]
- self.torques[self.right_hand_index - 1] = hand_torques[self.num_hand_dof :]
-
- self.torques = np.clip(self.torques, -self.torque_limit, self.torque_limit)
-
- if self.config["FREE_BASE"]:
- self.mj_data.ctrl = np.concatenate((np.zeros(6), self.torques))
- else:
- self.mj_data.ctrl = self.torques
- mujoco.mj_step(self.mj_model, self.mj_data)
- # self.check_self_collision()
-
- def kinematics_step(self):
- """
- Run kinematics only: compute the qpos of the robot and directly set the qpos.
- For debugging purposes.
- """
- if self.unitree_bridge is not None:
- self.unitree_bridge.PublishLowState(self.prepare_obs())
- if self.unitree_bridge.joystick:
- self.unitree_bridge.PublishWirelessController()
-
- if self.config["ENABLE_ELASTIC_BAND"]:
- if self.elastic_band.enable:
- # Get Cartesian pose and velocity of the band_attached_link
- pose = np.concatenate(
- [
- self.mj_data.xpos[self.band_attached_link], # link position in world
- self.mj_data.xquat[
- self.band_attached_link
- ], # link quaternion in world [w,x,y,z]
- np.zeros(6), # placeholder for velocity
- ]
- )
-
- # Get velocity in world frame
- mujoco.mj_objectVelocity(
- self.mj_model,
- self.mj_data,
- mujoco.mjtObj.mjOBJ_BODY,
- self.band_attached_link,
- pose[7:13],
- 0, # 0 for world frame
- )
-
- # Reorder velocity from [ang, lin] to [lin, ang]
- pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy()
-
- self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose)
- else:
- # explicitly resetting the force when the band is not enabled
- self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)
-
- body_qpos = self.compute_body_qpos() # (num_body_dof,)
- hand_qpos = self.compute_hand_qpos() # (num_hand_dof * 2,)
-
- self.mj_data.qpos[self.body_joint_index + 7 - 1] = body_qpos
- self.mj_data.qpos[self.left_hand_index + 7 - 1] = hand_qpos[: self.num_hand_dof]
- self.mj_data.qpos[self.right_hand_index + 7 - 1] = hand_qpos[self.num_hand_dof :]
-
- mujoco.mj_kinematics(self.mj_model, self.mj_data)
- mujoco.mj_comPos(self.mj_model, self.mj_data)
-
- def apply_perturbation(self, key):
- """Apply perturbation to the robot"""
- # Add velocity perturbations in body frame
- perturbation_x_body = 0.0 # forward/backward in body frame
- perturbation_y_body = 0.0 # left/right in body frame
- if key == "up":
- perturbation_x_body = 1.0 # forward
- elif key == "down":
- perturbation_x_body = -1.0 # backward
- elif key == "left":
- perturbation_y_body = 1.0 # left
- elif key == "right":
- perturbation_y_body = -1.0 # right
-
- # Transform body frame velocity to world frame using MuJoCo's rotation
- vel_body = np.array([perturbation_x_body, perturbation_y_body, 0.0])
- vel_world = np.zeros(3)
- base_quat = self.mj_data.qpos[3:7] # [w, x, y, z] quaternion
-
- # Use MuJoCo's robust quaternion rotation (handles invalid quaternions automatically)
- mujoco.mju_rotVecQuat(vel_world, vel_body, base_quat)
-
- # Apply to base linear velocity in world frame
- self.mj_data.qvel[0] += vel_world[0] # world X velocity
- self.mj_data.qvel[1] += vel_world[1] # world Y velocity
-
- # Update dynamics after velocity change
- mujoco.mj_forward(self.mj_model, self.mj_data)
-
- def update_viewer(self):
- if self.viewer is not None:
- self.viewer.sync()
-
- def update_viewer_camera(self):
- if self.viewer is not None:
- if self.viewer.cam.type == mujoco.mjtCamera.mjCAMERA_TRACKING:
- self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE
- 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
-
- def get_privileged_obs(self):
- """Get privileged observation. Should be implemented by subclasses."""
- return {}
-
- def update_render_caches(self):
- """Update render cache and shared memory for subprocess."""
- render_caches = {}
- for camera_name, camera_config in self.camera_configs.items():
- renderer = self.renderers[camera_name]
- if "params" in camera_config:
- renderer.update_scene(self.mj_data, camera=camera_config["params"])
- else:
- renderer.update_scene(self.mj_data, camera=camera_name)
- render_caches[camera_name + "_image"] = renderer.render()
-
- # Update shared memory if image publishing process is available
- if self.image_publish_process is not None:
- self.image_publish_process.update_shared_memory(render_caches)
-
- return render_caches
-
- def handle_keyboard_button(self, key):
- if self.elastic_band is not None:
- self.elastic_band.handle_keyboard_button(key)
-
- if key == "backspace":
- self.reset()
- if key == "v":
- self.update_viewer_camera()
- if key in ["up", "down", "left", "right"]:
- self.apply_perturbation(key)
-
- def check_fall(self):
- """Check if the robot has fallen"""
- self.fall = False
- if self.mj_data.qpos[2] < 0.2:
- self.fall = True
- print(f"Warning: Robot has fallen, height: {self.mj_data.qpos[2]:.3f} m")
-
- if self.fall:
- self.reset()
-
- def check_self_collision(self):
- """Check for self-collision of the robot"""
- robot_bodies = get_subtree_body_names(self.mj_model, self.mj_model.body(self.root_body).id)
- self_collision, contact_bodies = check_contact(
- self.mj_model, self.mj_data, robot_bodies, robot_bodies, return_all_contact_bodies=True
- )
- if self_collision:
- print(f"Warning: Self-collision detected: {contact_bodies}")
- return self_collision
-
- def reset(self):
- mujoco.mj_resetData(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:
- """Base simulator class that handles initialization and running of simulations"""
-
- def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs):
- self.config = config
- self.env_name = env_name
-
- # Initialize ROS 2 node (optional, only if rclpy is available)
- if HAS_RCLPY:
- if not rclpy.ok():
- rclpy.init()
- self.node = rclpy.create_node("sim_mujoco")
- self.thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True)
- self.thread.start()
- else:
- self.thread = None
- executor = rclpy.get_global_executor()
- self.node = executor.get_nodes()[0] # will only take the first node
- else:
- self.node = None
- self.thread = None
-
- # 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 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
-
- try:
- if self.config.get("INTERFACE", None):
- ChannelFactoryInitialize(self.config["DOMAIN_ID"], self.config["INTERFACE"])
- else:
- ChannelFactoryInitialize(self.config["DOMAIN_ID"])
- except Exception as e:
- # If it fails because it's already initialized, that's okay
- print(f"Note: Channel factory initialization attempt: {e}")
-
- # Initialize the unitree bridge and pass it to the environment
- self.init_unitree_bridge()
- self.sim_env.set_unitree_bridge(self.unitree_bridge)
-
- # Initialize additional components
- self.init_subscriber()
- self.init_publisher()
-
- self.sim_thread = None
-
- def start_as_thread(self):
- # Create simulation thread
- self.sim_thread = Thread(target=self.start)
- self.sim_thread.start()
-
- def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555):
- """Start the image publish subprocess"""
- self.sim_env.start_image_publish_subprocess(start_method, camera_port)
-
- def init_subscriber(self):
- """Initialize subscribers. Can be overridden by subclasses."""
- pass
-
- def init_publisher(self):
- """Initialize publishers. Can be overridden by subclasses."""
- pass
-
- def init_unitree_bridge(self):
- """Initialize the unitree SDK bridge"""
- self.unitree_bridge = UnitreeSdk2Bridge(self.config)
- if self.config["USE_JOYSTICK"]:
- self.unitree_bridge.SetupJoystick(
- device_id=self.config["JOYSTICK_DEVICE"], js_type=self.config["JOYSTICK_TYPE"]
- )
-
- def start(self):
- """Main simulation loop"""
- import time
- sim_cnt = 0
- last_time = time.time()
-
- print(f"Starting simulation loop. Viewer: {self.sim_env.viewer is not None}")
-
- try:
- while (
- self.sim_env.viewer and self.sim_env.viewer.is_running()
- ) or self.sim_env.viewer is None:
- # Run simulation step
- self.sim_env.sim_step()
-
- # Update viewer at viewer rate
- 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()
-
- # Sleep to maintain correct rate (simple timing without ROS)
- elapsed = time.time() - last_time
- sleep_time = max(0, self.sim_dt - elapsed)
- if sleep_time > 0:
- time.sleep(sleep_time)
- last_time = time.time()
-
- sim_cnt += 1
-
- print(f"Loop exited. Viewer running: {self.sim_env.viewer.is_running() if self.sim_env.viewer else 'No viewer'}")
- except KeyboardInterrupt:
- # User pressed Ctrl+C - exit cleanly
- print("Keyboard interrupt received")
- pass
- except Exception as e:
- print(f"Exception in simulation loop: {e}")
- import traceback
- traceback.print_exc()
- self.close()
-
- def __del__(self):
- """Clean up resources when simulator is deleted"""
- self.close()
-
- def reset(self):
- """Reset the simulation. Can be overridden by subclasses."""
- self.sim_env.reset()
-
- def close(self):
- """Close the simulation. Can be overridden by subclasses."""
- try:
- # Close viewer
- if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None:
- self.sim_env.viewer.close()
-
- # Shutdown ROS (if available)
- if HAS_RCLPY and rclpy.ok():
- rclpy.shutdown()
- except Exception as e:
- print(f"Warning during close: {e}")
-
- def get_privileged_obs(self):
- obs = self.sim_env.get_privileged_obs()
- # TODO: add ros2 topic to get privileged obs
- return obs
-
- def handle_keyboard_button(self, key):
- # Only handles keyboard buttons for default env.
- if self.env_name == "default":
- self.sim_env.handle_keyboard_button(key)
-
-
-if __name__ == "__main__":
- parser = argparse.ArgumentParser(description="Robot")
- parser.add_argument(
- "--config",
- type=str,
- default="./gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml",
- help="config file",
- )
- args = parser.parse_args()
-
- with open(args.config, "r") as file:
- config = yaml.load(file, Loader=yaml.FullLoader)
-
- if config.get("INTERFACE", None):
- ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"])
- else:
- ChannelFactoryInitialize(config["DOMAIN_ID"])
-
- simulation = BaseSimulator(config)
- simulation.start_as_thread()
diff --git a/mujoco_sim_g1/sim/image_publish_utils.py b/mujoco_sim_g1/sim/image_publish_utils.py
deleted file mode 100644
index c9dd78953..000000000
--- a/mujoco_sim_g1/sim/image_publish_utils.py
+++ /dev/null
@@ -1,257 +0,0 @@
-import multiprocessing as mp
-from multiprocessing import shared_memory
-import time
-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"""
-
- if verbose:
- print(f"Available start methods: {mp.get_all_start_methods()}")
- return mp.get_start_method()
-
-
-class ImagePublishProcess:
- """Subprocess for publishing images using shared memory and ZMQ"""
-
- def __init__(
- self,
- camera_configs: Dict[str, Any],
- image_dt: float,
- zmq_port: int = 5555,
- start_method: str = "spawn",
- verbose: bool = False,
- ):
- self.camera_configs = camera_configs
- self.image_dt = image_dt
- self.zmq_port = zmq_port
- self.verbose = verbose
- self.shared_memory_blocks = {}
- self.shared_memory_info = {}
- self.process = None
-
- # Use specific context to avoid global state pollution
- self.mp_context = mp.get_context(start_method)
- if self.verbose:
- print(f"Using multiprocessing context: {start_method}")
-
- self.stop_event = self.mp_context.Event()
- self.data_ready_event = self.mp_context.Event()
-
- # Ensure events start in correct state
- self.stop_event.clear()
- self.data_ready_event.clear()
-
- if self.verbose:
- print(f"Initial stop_event state: {self.stop_event.is_set()}")
- print(f"Initial data_ready_event state: {self.data_ready_event.is_set()}")
-
- # Calculate shared memory requirements for each camera
- for camera_name, camera_config in camera_configs.items():
- height = camera_config["height"]
- width = camera_config["width"]
- # RGB image: height * width * 3 (uint8)
- size = height * width * 3
-
- # Create shared memory block
- shm = shared_memory.SharedMemory(create=True, size=size)
- self.shared_memory_blocks[camera_name] = shm
- self.shared_memory_info[camera_name] = {
- "name": shm.name,
- "size": size,
- "shape": (height, width, 3),
- "dtype": np.uint8,
- }
-
- def start_process(self):
- """Start the image publishing subprocess"""
- if self.verbose:
- print(f"Starting subprocess with stop_event state: {self.stop_event.is_set()}")
- self.process = self.mp_context.Process(
- target=self._image_publish_worker,
- args=(
- self.shared_memory_info,
- self.image_dt,
- self.zmq_port,
- self.stop_event,
- self.data_ready_event,
- self.verbose,
- ),
- )
- self.process.start()
- if self.verbose:
- print(f"Subprocess started, PID: {self.process.pid}")
-
- def update_shared_memory(self, render_caches: Dict[str, np.ndarray]):
- """Update shared memory with new rendered images"""
- images_updated = 0
- for camera_name in self.camera_configs.keys():
- image_key = f"{camera_name}_image"
- if image_key in render_caches:
- image = render_caches[image_key]
-
- # Ensure image is uint8 and has correct shape
- if image.dtype != np.uint8:
- image = (image * 255).astype(np.uint8)
-
- # Get shared memory array
- shm = self.shared_memory_blocks[camera_name]
- shared_array = np.ndarray(
- self.shared_memory_info[camera_name]["shape"],
- dtype=self.shared_memory_info[camera_name]["dtype"],
- buffer=shm.buf,
- )
-
- # Copy image data to shared memory atomically
- np.copyto(shared_array, image)
- images_updated += 1
-
- # Signal that new data is ready only after all images are written
- if images_updated > 0:
- if self.verbose:
- print(f"Main process: Updated {images_updated} images, setting data_ready_event")
- self.data_ready_event.set()
- elif self.verbose:
- print(
- "Main process: No images to update. "
- "please check if camera configs are provided and the renderer is properly initialized"
- )
-
- def stop(self):
- """Stop the image publishing subprocess"""
- if self.verbose:
- print("Stopping image publishing subprocess...")
- self.stop_event.set()
-
- if self.process and self.process.is_alive():
- # Give the process time to clean up gracefully
- self.process.join(timeout=5)
- if self.process.is_alive():
- if self.verbose:
- print("Subprocess didn't stop gracefully, terminating...")
- self.process.terminate()
- self.process.join(timeout=2)
- if self.process.is_alive():
- if self.verbose:
- print("Force killing subprocess...")
- self.process.kill()
- self.process.join()
-
- # Clean up shared memory
- for camera_name, shm in self.shared_memory_blocks.items():
- try:
- shm.close()
- shm.unlink()
- if self.verbose:
- print(f"Cleaned up shared memory for {camera_name}")
- except Exception as e:
- if self.verbose:
- print(f"Warning: Failed to cleanup shared memory for {camera_name}: {e}")
-
- self.shared_memory_blocks.clear()
- if self.verbose:
- print("Image publishing subprocess stopped and cleaned up")
-
- @staticmethod
- def _image_publish_worker(
- shared_memory_info, image_dt, zmq_port, stop_event, data_ready_event, verbose
- ):
- """Worker function that runs in the subprocess"""
- # Import dependencies within worker (needed for multiprocessing spawn mode)
- from .sensor_utils import ImageMessageSchema, ImageUtils, SensorServer
-
- if verbose:
- print(f"Worker started! PID: {__import__('os').getpid()}")
- print(f"Worker stop_event state at start: {stop_event.is_set()}")
- print(f"Worker data_ready_event state at start: {data_ready_event.is_set()}")
-
- try:
- # Initialize ZMQ sensor server
- sensor_server = SensorServer()
- sensor_server.start_server(port=zmq_port)
-
- # Connect to shared memory blocks
- shared_arrays = {}
- shm_blocks = {}
- for camera_name, info in shared_memory_info.items():
- shm = shared_memory.SharedMemory(name=info["name"])
- shm_blocks[camera_name] = shm
- shared_arrays[camera_name] = np.ndarray(
- info["shape"], dtype=info["dtype"], buffer=shm.buf
- )
-
- print(
- f"Image publishing subprocess started with {len(shared_arrays)} cameras on ZMQ port {zmq_port}"
- )
-
- loop_count = 0
- last_data_time = time.time()
-
- while not stop_event.is_set():
- loop_count += 1
-
- # Wait for new data with shorter timeout for better responsiveness
- timeout = min(image_dt, 0.1) # Max 100ms timeout
- data_available = data_ready_event.wait(timeout=timeout)
-
- current_time = time.time()
-
- if data_available:
- data_ready_event.clear()
- if loop_count % 50 == 0:
- print("Image publish frequency: ", 1 / (current_time - last_data_time))
- last_data_time = current_time
-
- # Collect all camera images and serialize them
- try:
- # Copy all images atomically at once
- image_copies = {name: arr.copy() for name, arr in shared_arrays.items()}
-
- # Create message with all camera images
- message_dict = {
- "images": image_copies,
- "timestamps": {name: current_time for name in image_copies.keys()},
- }
-
- # Create ImageMessageSchema and serialize
- image_msg = ImageMessageSchema(
- timestamps=message_dict.get("timestamps"),
- images=message_dict.get("images", None),
- )
-
- # Serialize and send via ZMQ
- serialized_data = image_msg.serialize()
-
- # Add individual camera images to the message
- for camera_name, image_copy in image_copies.items():
- serialized_data[f"{camera_name}"] = ImageUtils.encode_image(image_copy)
-
- sensor_server.send_message(serialized_data)
-
- except Exception as e:
- print(f"Error publishing images: {e}")
-
- elif verbose and loop_count % 10 == 0:
- print(f"Subprocess: Still waiting for data... (iteration {loop_count})")
-
- # Small sleep to prevent busy waiting when no data
- if not data_available:
- time.sleep(0.001)
-
- except KeyboardInterrupt:
- print("Image publisher interrupted by user")
- finally:
- # Clean up
- try:
- for shm in shm_blocks.values():
- shm.close()
- sensor_server.stop_server()
- except Exception as e:
- print(f"Error during subprocess cleanup: {e}")
- if verbose:
- print("Image publish subprocess stopped")
diff --git a/mujoco_sim_g1/sim/metric_utils.py b/mujoco_sim_g1/sim/metric_utils.py
deleted file mode 100644
index 369b8797c..000000000
--- a/mujoco_sim_g1/sim/metric_utils.py
+++ /dev/null
@@ -1,71 +0,0 @@
-from typing import List, Tuple
-
-import mujoco
-
-from .sim_utilts import get_body_geom_ids
-
-
-def check_contact(
- mj_model: mujoco.MjModel,
- mj_data: mujoco.MjData,
- bodies_1: List[str] | str,
- bodies_2: List[str] | str,
- return_all_contact_bodies: bool = False,
-) -> Tuple[bool, List[Tuple[str, str]]] | bool:
- """
- Finds contact between two body groups. Any geom in the body is considered to be in contact.
- Args:
- mj_model (MujocoModel): Current simulation object
- mj_data (MjData): Current simulation data
- bodies_1 (str or list of int): an individual body name or list of body names.
- bodies_2 (str or list of int): another individual body name or list of body names.
- Returns:
- bool: True if any body in @bodies_1 is in contact with any body in @bodies_2.
- """
- if isinstance(bodies_1, str):
- bodies_1 = [bodies_1]
- if isinstance(bodies_2, str):
- bodies_2 = [bodies_2]
-
- geoms_1 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_1]
- geoms_1 = [g for geom_list in geoms_1 for g in geom_list]
- geoms_2 = [get_body_geom_ids(mj_model, mj_model.body(g).id) for g in bodies_2]
- geoms_2 = [g for geom_list in geoms_2 for g in geom_list]
- contact_bodies = []
- for i in range(mj_data.ncon):
- contact = mj_data.contact[i]
- # check contact geom in geoms
- c1_in_g1 = contact.geom1 in geoms_1
- c2_in_g2 = contact.geom2 in geoms_2 if geoms_2 is not None else True
- # check contact geom in geoms (flipped)
- c2_in_g1 = contact.geom2 in geoms_1
- c1_in_g2 = contact.geom1 in geoms_2 if geoms_2 is not None else True
- if (c1_in_g1 and c2_in_g2) or (c1_in_g2 and c2_in_g1):
- contact_bodies.append(
- (
- mj_model.body(mj_model.geom(contact.geom1).bodyid).name,
- mj_model.body(mj_model.geom(contact.geom2).bodyid).name,
- )
- )
- if not return_all_contact_bodies:
- break
- if return_all_contact_bodies:
- return len(contact_bodies) > 0, set(contact_bodies)
- else:
- return len(contact_bodies) > 0
-
-
-def check_height(
- mj_model: mujoco.MjModel,
- mj_data: mujoco.MjData,
- geom_name: str,
- lower_bound: float = -float("inf"),
- upper_bound: float = float("inf"),
-):
- """
- Checks if the height of a geom is greater than a given height.
- """
- geom_id = mj_model.geom(geom_name).id
- return (
- mj_data.geom_xpos[geom_id][2] < upper_bound and mj_data.geom_xpos[geom_id][2] > lower_bound
- )
diff --git a/mujoco_sim_g1/sim/sensor_utils.py b/mujoco_sim_g1/sim/sensor_utils.py
deleted file mode 100644
index 7b205c2e6..000000000
--- a/mujoco_sim_g1/sim/sensor_utils.py
+++ /dev/null
@@ -1,130 +0,0 @@
-"""Standalone sensor utilities for camera image publishing via ZMQ"""
-import base64
-from dataclasses import dataclass
-from typing import Any, Dict
-
-import cv2
-import msgpack
-import msgpack_numpy as m
-import numpy as np
-import zmq
-
-
-@dataclass
-class ImageMessageSchema:
- """
- Standardized message schema for image data.
- Used to serialize/deserialize image data for network transmission.
- """
-
- timestamps: Dict[str, float]
- """Dictionary of timestamps, keyed by image identifier (e.g., {"ego_view": 123.45})"""
- images: Dict[str, np.ndarray]
- """Dictionary of images, keyed by image identifier (e.g., {"ego_view": array})"""
-
- def serialize(self) -> Dict[str, Any]:
- """Serialize the message for transmission."""
- serialized_msg = {"timestamps": self.timestamps, "images": {}}
- for key, image in self.images.items():
- serialized_msg["images"][key] = ImageUtils.encode_image(image)
- return serialized_msg
-
- @staticmethod
- def deserialize(data: Dict[str, Any]) -> "ImageMessageSchema":
- """Deserialize received message data."""
- timestamps = data.get("timestamps", {})
- images = {}
- for key, value in data.get("images", {}).items():
- if isinstance(value, str):
- images[key] = ImageUtils.decode_image(value)
- else:
- images[key] = value
- return ImageMessageSchema(timestamps=timestamps, images=images)
-
- def asdict(self) -> Dict[str, Any]:
- """Convert to dictionary format."""
- return {"timestamps": self.timestamps, "images": self.images}
-
-
-class SensorServer:
- """ZMQ-based sensor server for publishing camera images"""
-
- def start_server(self, port: int):
- self.context = zmq.Context()
- self.socket = self.context.socket(zmq.PUB)
- self.socket.setsockopt(zmq.SNDHWM, 20) # high water mark
- self.socket.setsockopt(zmq.LINGER, 0)
- self.socket.bind(f"tcp://*:{port}")
- print(f"Sensor server running at tcp://*:{port}")
-
- self.message_sent = 0
- self.message_dropped = 0
-
- def stop_server(self):
- self.socket.close()
- self.context.term()
-
- def send_message(self, data: Dict[str, Any]):
- try:
- 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}")
- self.message_sent += 1
-
- if self.message_sent % 100 == 0:
- print(
- f"[Sensor server] Message sent: {self.message_sent}, message dropped: {self.message_dropped}"
- )
-
-
-class SensorClient:
- """ZMQ-based sensor client for subscribing to camera images"""
-
- def start_client(self, server_ip: str, port: int):
- self.context = zmq.Context()
- self.socket = self.context.socket(zmq.SUB)
- self.socket.setsockopt_string(zmq.SUBSCRIBE, "")
- self.socket.setsockopt(zmq.CONFLATE, True) # last msg only.
- self.socket.setsockopt(zmq.RCVHWM, 3) # queue size 3 for receive buffer
- self.socket.connect(f"tcp://{server_ip}:{port}")
-
- def stop_client(self):
- self.socket.close()
- self.context.term()
-
- def receive_message(self):
- packed = self.socket.recv()
- return msgpack.unpackb(packed, object_hook=m.decode)
-
-
-class ImageUtils:
- """Utilities for encoding/decoding images for network transmission"""
-
- @staticmethod
- def encode_image(image: np.ndarray) -> str:
- """Encode numpy image to base64-encoded JPEG string"""
- _, color_buffer = cv2.imencode(".jpg", image, [int(cv2.IMWRITE_JPEG_QUALITY), 80])
- return base64.b64encode(color_buffer).decode("utf-8")
-
- @staticmethod
- def encode_depth_image(image: np.ndarray) -> str:
- """Encode depth image to base64-encoded PNG string"""
- depth_compressed = cv2.imencode(".png", image)[1].tobytes()
- return base64.b64encode(depth_compressed).decode("utf-8")
-
- @staticmethod
- def decode_image(image: str) -> np.ndarray:
- """Decode base64-encoded JPEG string to numpy image"""
- color_data = base64.b64decode(image)
- color_array = np.frombuffer(color_data, dtype=np.uint8)
- return cv2.imdecode(color_array, cv2.IMREAD_COLOR)
-
- @staticmethod
- def decode_depth_image(image: str) -> np.ndarray:
- """Decode base64-encoded PNG string to depth image"""
- depth_data = base64.b64decode(image)
- depth_array = np.frombuffer(depth_data, dtype=np.uint8)
- return cv2.imdecode(depth_array, cv2.IMREAD_UNCHANGED)
-
diff --git a/mujoco_sim_g1/sim/sim_utilts.py b/mujoco_sim_g1/sim/sim_utilts.py
deleted file mode 100644
index e032eb6d8..000000000
--- a/mujoco_sim_g1/sim/sim_utilts.py
+++ /dev/null
@@ -1,96 +0,0 @@
-"""
-Utility functions for working with Mujoco models.
-copied from https://github.com/kevinzakka/mink/blob/main/mink/utils.py
-"""
-
-from typing import List
-
-import mujoco
-
-
-def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
- """Get immediate children bodies belonging to a given body.
-
- Args:
- model: Mujoco model.
- body_id: ID of body.
-
- Returns:
- A List containing all child body ids.
- """
- return [
- i
- for i in range(model.nbody)
- if model.body_parentid[i] == body_id and body_id != i # Exclude the body itself.
- ]
-
-
-def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
- """Get all bodies belonging to subtree starting at a given body.
-
- Args:
- model: Mujoco model.
- body_id: ID of body where subtree starts.
-
- Returns:
- A List containing all subtree body ids.
- """
- body_ids: List[int] = []
- stack = [body_id]
- while stack:
- body_id = stack.pop()
- body_ids.append(body_id)
- stack += get_body_body_ids(model, body_id)
- return body_ids
-
-
-def get_subtree_body_names(model: mujoco.MjModel, body_id: int) -> List[str]:
- """Get all bodies belonging to subtree starting at a given body.
- Args:
- model: Mujoco model.
- body_id: ID of body where subtree starts.
-
- Returns:
- A List containing all subtree body names.
- """
- return [model.body(i).name for i in get_subtree_body_ids(model, body_id)]
-
-
-def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
- """Get immediate geoms belonging to a given body.
-
- Here, immediate geoms are those directly attached to the body and not its
- descendants.
-
- Args:
- model: Mujoco model.
- body_id: ID of body.
-
- Returns:
- A list containing all body geom ids.
- """
- geom_start = model.body_geomadr[body_id]
- geom_end = geom_start + model.body_geomnum[body_id]
- return list(range(geom_start, geom_end))
-
-
-def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
- """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.
-
- Args:
- model: Mujoco model.
- body_id: ID of body where subtree starts.
-
- Returns:
- A list containing all subtree geom ids.
- """
- geom_ids: List[int] = []
- stack = [body_id]
- while stack:
- body_id = stack.pop()
- geom_ids.extend(get_body_geom_ids(model, body_id))
- stack += get_body_body_ids(model, body_id)
- return geom_ids
diff --git a/mujoco_sim_g1/sim/simulator_factory.py b/mujoco_sim_g1/sim/simulator_factory.py
deleted file mode 100644
index 3dca58dc2..000000000
--- a/mujoco_sim_g1/sim/simulator_factory.py
+++ /dev/null
@@ -1,148 +0,0 @@
-import time
-from typing import Any, Dict
-
-from unitree_sdk2py.core.channel import ChannelFactoryInitialize
-
-from .base_sim import BaseSimulator
-
-
-def init_channel(config: Dict[str, Any]) -> None:
- """
- Initialize the communication channel for simulator/robot communication.
-
- Args:
- config: Configuration dictionary containing DOMAIN_ID and optionally INTERFACE
- """
- if config.get("INTERFACE", None):
- ChannelFactoryInitialize(config["DOMAIN_ID"], config["INTERFACE"])
- else:
- ChannelFactoryInitialize(config["DOMAIN_ID"])
-
-
-class SimulatorFactory:
- """Factory class for creating different types of simulators."""
-
- @staticmethod
- def create_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
- """
- Create a simulator based on the configuration.
-
- Args:
- config: Configuration dictionary containing SIMULATOR type
- env_name: Environment name
- **kwargs: Additional keyword arguments for specific simulators
- """
- 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}. "
- "If you are using run_sim_loop, please ignore this warning."
- )
- return None
-
- @staticmethod
- def _create_mujoco_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs):
- """Create a MuJoCo simulator instance."""
- camera_configs = kwargs.pop("camera_configs", {})
- if len(camera_configs) > 0:
- print(f"Debug: SimulatorFactory received {len(camera_configs)} camera config(s)")
-
- env_kwargs = dict(
- onscreen=kwargs.pop("onscreen", True),
- offscreen=kwargs.pop("offscreen", False),
- camera_configs=camera_configs,
- )
- 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)
diff --git a/mujoco_sim_g1/sim/unitree_sdk2py_bridge.py b/mujoco_sim_g1/sim/unitree_sdk2py_bridge.py
deleted file mode 100644
index 6cc596611..000000000
--- a/mujoco_sim_g1/sim/unitree_sdk2py_bridge.py
+++ /dev/null
@@ -1,497 +0,0 @@
-import sys
-import threading
-from typing import Dict, Tuple
-
-import glfw
-from loguru import logger
-import mujoco
-import numpy as np
-import pygame
-import scipy.spatial.transform
-from termcolor import colored
-from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber
-from unitree_sdk2py.idl.default import (
- unitree_go_msg_dds__WirelessController_,
- unitree_hg_msg_dds__HandCmd_ as HandCmd_default,
- unitree_hg_msg_dds__HandState_ as HandState_default,
-)
-from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
-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:
- """
- 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 (
- unitree_hg_msg_dds__IMUState_ as IMUState_default,
- 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_()
- elif "h1" == robot_type or "go2" == robot_type:
- from unitree_sdk2py.idl.default import (
- unitree_go_msg_dds__LowCmd_,
- unitree_go_msg_dds__LowState_ as LowState_default,
- unitree_hg_msg_dds__IMUState_ as IMUState_default,
- )
- from unitree_sdk2py.idl.unitree_go.msg.dds_ import IMUState_, LowCmd_, LowState_
-
- self.low_cmd = unitree_go_msg_dds__LowCmd_()
- else:
- raise ValueError(f"Invalid robot type '{robot_type}'. Expected 'g1', 'h1', or 'go2'.")
-
- 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()
-
- 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()
-
- self.low_cmd_suber = ChannelSubscriber("rt/lowcmd", LowCmd_)
- self.low_cmd_suber.Init(self.LowCmdHandler, 1)
-
- 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)
-
- self.low_cmd_lock = threading.Lock()
- self.left_hand_cmd_lock = threading.Lock()
- self.right_hand_cmd_lock = threading.Lock()
-
- self.wireless_controller = unitree_go_msg_dds__WirelessController_()
- self.wireless_controller_puber = ChannelPublisher(
- "rt/wirelesscontroller", WirelessController_
- )
- self.wireless_controller_puber.Init()
-
- # joystick
- self.key_map = {
- "R1": 0,
- "L1": 1,
- "start": 2,
- "select": 3,
- "R2": 4,
- "L2": 5,
- "F1": 6,
- "F2": 7,
- "A": 8,
- "B": 9,
- "X": 10,
- "Y": 11,
- "up": 12,
- "right": 13,
- "down": 14,
- "left": 15,
- }
- self.joystick = None
-
- # 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:
- self.low_cmd_received = False
- self.new_low_cmd = False
- with self.left_hand_cmd_lock:
- self.left_hand_cmd_received = False
- self.new_left_hand_cmd = False
- with self.right_hand_cmd_lock:
- self.right_hand_cmd_received = False
- self.new_right_hand_cmd = False
-
- def LowCmdHandler(self, msg):
- with self.low_cmd_lock:
- self.low_cmd = msg
- self.low_cmd_received = True
- self.new_low_cmd = True
-
- def LeftHandCmdHandler(self, msg):
- with self.left_hand_cmd_lock:
- self.left_hand_cmd = msg
- self.left_hand_cmd_received = True
- self.new_left_hand_cmd = True
-
- def RightHandCmdHandler(self, msg):
- with self.right_hand_cmd_lock:
- self.right_hand_cmd = msg
- self.right_hand_cmd_received = True
- self.new_right_hand_cmd = True
-
- def cmd_received(self):
- with self.low_cmd_lock:
- low_cmd_received = self.low_cmd_received
- with self.left_hand_cmd_lock:
- left_hand_cmd_received = self.left_hand_cmd_received
- with self.right_hand_cmd_lock:
- right_hand_cmd_received = self.right_hand_cmd_received
- return low_cmd_received or left_hand_cmd_received or right_hand_cmd_received
-
- def PublishLowState(self, obs: Dict[str, any]):
- # publish body state
- if self.use_sensor:
- raise NotImplementedError("Sensor data is not implemented yet.")
- else:
- for i in range(self.num_body_motor):
- self.low_state.motor_state[i].q = obs["body_q"][i]
- self.low_state.motor_state[i].dq = obs["body_dq"][i]
- self.low_state.motor_state[i].ddq = obs["body_ddq"][i]
- self.low_state.motor_state[i].tau_est = obs["body_tau_est"][i]
-
- if self.use_sensor and self.have_frame_sensor_:
- 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
- self.low_state.imu_state.gyroscope[:] = obs["floating_base_vel"][3:6]
- # linear acceleration
- self.low_state.imu_state.accelerometer[:] = obs["floating_base_acc"][:3]
-
- self.torso_imu_state.quaternion[:] = obs["secondary_imu_quat"]
- self.torso_imu_state.gyroscope[:] = obs["secondary_imu_vel"][3:6]
-
- # acceleration: x, y, z (only available when frame sensor is enabled)
- if self.have_frame_sensor_:
- raise NotImplementedError("Frame sensor data is not implemented yet.")
- self.low_state.tick = int(obs["time"] * 1e3)
- 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
- for i in range(self.num_hand_motor):
- self.left_hand_state.motor_state[i].q = obs["left_hand_q"][i]
- self.left_hand_state.motor_state[i].dq = obs["left_hand_dq"][i]
- self.left_hand_state_puber.Write(self.left_hand_state)
-
- for i in range(self.num_hand_motor):
- self.right_hand_state.motor_state[i].q = obs["right_hand_q"][i]
- self.right_hand_state.motor_state[i].dq = obs["right_hand_dq"][i]
- self.right_hand_state_puber.Write(self.right_hand_state)
-
- def GetAction(self) -> Tuple[np.ndarray, bool, bool]:
- with self.low_cmd_lock:
- body_q = [self.low_cmd.motor_cmd[i].q for i in range(self.num_body_motor)]
- with self.left_hand_cmd_lock:
- left_hand_q = [self.left_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)]
- with self.right_hand_cmd_lock:
- right_hand_q = [self.right_hand_cmd.motor_cmd[i].q for i in range(self.num_hand_motor)]
- with self.low_cmd_lock and self.left_hand_cmd_lock and self.right_hand_cmd_lock:
- is_new_action = self.new_low_cmd and self.new_left_hand_cmd and self.new_right_hand_cmd
- if is_new_action:
- self.new_low_cmd = False
- self.new_left_hand_cmd = False
- self.new_right_hand_cmd = False
-
- return (
- np.concatenate([body_q[:-7], left_hand_q, body_q[-7:], right_hand_q]),
- self.cmd_received(),
- is_new_action,
- )
-
- def PublishWirelessController(self):
- if self.joystick is not None:
- pygame.event.get()
- key_state = [0] * 16
- key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"])
- key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"])
- key_state[self.key_map["start"]] = self.joystick.get_button(self.button_id["START"])
- key_state[self.key_map["select"]] = self.joystick.get_button(self.button_id["SELECT"])
- key_state[self.key_map["R2"]] = self.joystick.get_axis(self.axis_id["RT"]) > 0
- key_state[self.key_map["L2"]] = self.joystick.get_axis(self.axis_id["LT"]) > 0
- key_state[self.key_map["F1"]] = 0
- key_state[self.key_map["F2"]] = 0
- key_state[self.key_map["A"]] = self.joystick.get_button(self.button_id["A"])
- key_state[self.key_map["B"]] = self.joystick.get_button(self.button_id["B"])
- key_state[self.key_map["X"]] = self.joystick.get_button(self.button_id["X"])
- key_state[self.key_map["Y"]] = self.joystick.get_button(self.button_id["Y"])
- key_state[self.key_map["up"]] = self.joystick.get_hat(0)[1] > 0
- key_state[self.key_map["right"]] = self.joystick.get_hat(0)[0] > 0
- key_state[self.key_map["down"]] = self.joystick.get_hat(0)[1] < 0
- key_state[self.key_map["left"]] = self.joystick.get_hat(0)[0] < 0
-
- key_value = 0
- for i in range(16):
- key_value += key_state[i] << i
-
- self.wireless_controller.keys = key_value
- self.wireless_controller.lx = self.joystick.get_axis(self.axis_id["LX"])
- self.wireless_controller.ly = -self.joystick.get_axis(self.axis_id["LY"])
- self.wireless_controller.rx = self.joystick.get_axis(self.axis_id["RX"])
- self.wireless_controller.ry = -self.joystick.get_axis(self.axis_id["RY"])
-
- self.wireless_controller_puber.Write(self.wireless_controller)
-
- def SetupJoystick(self, device_id=0, js_type="xbox"):
- pygame.init()
- pygame.joystick.init()
- joystick_count = pygame.joystick.get_count()
- if joystick_count > 0:
- self.joystick = pygame.joystick.Joystick(device_id)
- self.joystick.init()
- else:
- print("No gamepad detected.")
- sys.exit()
-
- if js_type == "xbox":
- if sys.platform.startswith("linux"):
- self.axis_id = {
- "LX": 0, # Left stick axis x
- "LY": 1, # Left stick axis y
- "RX": 3, # Right stick axis x
- "RY": 4, # Right stick axis y
- "LT": 2, # Left trigger
- "RT": 5, # Right trigger
- "DX": 6, # Directional pad x
- "DY": 7, # Directional pad y
- }
- self.button_id = {
- "X": 2,
- "Y": 3,
- "B": 1,
- "A": 0,
- "LB": 4,
- "RB": 5,
- "SELECT": 6,
- "START": 7,
- "XBOX": 8,
- "LSB": 9,
- "RSB": 10,
- }
- elif sys.platform == "darwin":
- self.axis_id = {
- "LX": 0, # Left stick axis x
- "LY": 1, # Left stick axis y
- "RX": 2, # Right stick axis x
- "RY": 3, # Right stick axis y
- "LT": 4, # Left trigger
- "RT": 5, # Right trigger
- }
- self.button_id = {
- "X": 2,
- "Y": 3,
- "B": 1,
- "A": 0,
- "LB": 9,
- "RB": 10,
- "SELECT": 4,
- "START": 6,
- "XBOX": 5,
- "LSB": 7,
- "RSB": 8,
- "DYU": 11,
- "DYD": 12,
- "DXL": 13,
- "DXR": 14,
- }
- else:
- print("Unsupported OS. ")
-
- elif js_type == "switch":
- # Yuanhang: may differ for different OS, need to be checked
- self.axis_id = {
- "LX": 0, # Left stick axis x
- "LY": 1, # Left stick axis y
- "RX": 2, # Right stick axis x
- "RY": 3, # Right stick axis y
- "LT": 5, # Left trigger
- "RT": 4, # Right trigger
- "DX": 6, # Directional pad x
- "DY": 7, # Directional pad y
- }
-
- self.button_id = {
- "X": 3,
- "Y": 4,
- "B": 1,
- "A": 0,
- "LB": 6,
- "RB": 7,
- "SELECT": 10,
- "START": 11,
- }
- else:
- print("Unsupported gamepad. ")
-
- def PrintSceneInformation(self):
- print(" ")
- logger.info(colored("<<------------- Link ------------->>", "green"))
- for i in range(self.mj_model.nbody):
- name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_BODY, i)
- 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):
- name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i)
- if name:
- logger.info(
- f"sensor_index: {index}, name: {name}, dim: {self.mj_model.sensor_dim[i]}"
- )
- index = index + self.mj_model.sensor_dim[i]
- print(" ")
-
-
-class ElasticBand:
- """
- ref: https://github.com/unitreerobotics/unitree_mujoco
- """
-
- def __init__(self):
- self.kp_pos = 10000
- self.kd_pos = 1000
- self.kp_ang = 1000
- self.kd_ang = 10
- self.point = np.array([0, 0, 1])
- self.length = 0
- self.enable = True
-
- def Advance(self, pose):
- """
- Args:
- pose: 13D array containing:
- - pose[0:3]: position in world frame
- - pose[3:7]: quaternion [w,x,y,z] in world frame
- - pose[7:10]: linear velocity in world frame
- - pose[10:13]: angular velocity in world frame
- Returns:
- np.ndarray: 6D vector [fx, fy, fz, tx, ty, tz]
- """
- pos = pose[0:3]
- quat = pose[3:7]
- lin_vel = pose[7:10]
- ang_vel = pose[10:13]
-
- δx = self.point - pos
- f = self.kp_pos * (δx + np.array([0, 0, self.length])) + self.kd_pos * (0 - lin_vel)
-
- # --- Orientation PD control for torque ---
- quat = np.array([quat[1], quat[2], quat[3], quat[0]]) # reorder to [x,y,z,w] for scipy
- rot = scipy.spatial.transform.Rotation.from_quat(quat)
- rotvec = rot.as_rotvec() # axis-angle error
- torque = -self.kp_ang * rotvec - self.kd_ang * ang_vel
-
- return np.concatenate([f, torque])
-
- def MujuocoKeyCallback(self, key):
- if key == glfw.KEY_7:
- self.length -= 0.1
- if key == glfw.KEY_8:
- self.length += 0.1
- if key == glfw.KEY_9:
- self.enable = not self.enable
-
- def handle_keyboard_button(self, key):
- if key == "9":
- self.enable = not self.enable
diff --git a/mujoco_sim_g1/view_cameras.py b/mujoco_sim_g1/view_cameras.py
deleted file mode 100644
index 6467c0aeb..000000000
--- a/mujoco_sim_g1/view_cameras.py
+++ /dev/null
@@ -1,133 +0,0 @@
-#!/usr/bin/env python3
-"""
-Camera recorder for MuJoCo simulator
-Subscribes to ZMQ camera feed and saves images to disk
-"""
-import argparse
-import sys
-import time
-from pathlib import Path
-
-# Add sim module to path
-sys.path.insert(0, str(Path(__file__).parent))
-
-import cv2
-import numpy as np
-from sim.sensor_utils import SensorClient
-
-
-def main():
- parser = argparse.ArgumentParser(description="Save camera streams from MuJoCo simulator")
- parser.add_argument("--host", type=str, default="localhost",
- help="Simulator host address (default: localhost)")
- parser.add_argument("--port", type=int, default=5555,
- help="ZMQ port (default: 5555)")
- parser.add_argument("--save-dir", type=str, default="./camera_recordings",
- help="Directory to save images (default: ./camera_recordings)")
- parser.add_argument("--save-rate", type=int, default=5,
- help="Save every Nth frame (default: 5 = ~6Hz at 30fps stream)")
- parser.add_argument("--max-frames", type=int, default=None,
- help="Maximum frames to save before exiting (default: unlimited)")
- args = parser.parse_args()
-
- # Create save directory
- save_dir = Path(args.save_dir)
- save_dir.mkdir(parents=True, exist_ok=True)
-
- print("="*60)
- print("📷 MuJoCo Camera Recorder")
- print("="*60)
- print(f"🌐 Connecting to: tcp://{args.host}:{args.port}")
- print(f"💾 Saving to: {save_dir.absolute()}")
- print(f"⏬ Save rate: Every {args.save_rate} frames")
- if args.max_frames:
- print(f"🎬 Max frames: {args.max_frames}")
- print("="*60)
- print("\nWaiting for camera data...")
- print("Press Ctrl+C to stop recording\n")
-
- # Connect to sensor server
- client = SensorClient()
- client.start_client(server_ip=args.host, port=args.port)
-
- frame_count = 0
- saved_count = 0
- last_time = time.time()
- fps_display = 0
-
- # Track per-camera frame counts
- camera_frame_counts = {}
-
- try:
- while True:
- try:
- # Receive image data
- data = client.receive_message()
-
- # Calculate FPS
- frame_count += 1
- current_time = time.time()
- if current_time - last_time >= 1.0:
- fps_display = frame_count / (current_time - last_time)
- frame_count = 0
- last_time = current_time
-
- # Process each camera
- for key, value in data.items():
- if key == "timestamps":
- continue
-
- # Initialize counter for this camera
- if key not in camera_frame_counts:
- camera_frame_counts[key] = 0
- # Create subdirectory for each camera
- (save_dir / key).mkdir(exist_ok=True)
-
- camera_frame_counts[key] += 1
-
- # Only save every Nth frame
- if camera_frame_counts[key] % args.save_rate != 0:
- continue
-
- # Decode image if it's a string (base64 encoded)
- if isinstance(value, str):
- from sim.sensor_utils import ImageUtils
- img = ImageUtils.decode_image(value)
- elif isinstance(value, np.ndarray):
- img = value
- else:
- continue
-
- # Save image
- filename = f"{key}_{saved_count:06d}.jpg"
- save_path = save_dir / key / filename
- cv2.imwrite(str(save_path), img)
-
- # Print progress
- if saved_count % 10 == 0:
- print(f"[{fps_display:5.1f} FPS] Saved {saved_count:4d} frames to {key}/")
-
- saved_count += 1
-
- # Check if we've reached max frames
- if args.max_frames and saved_count >= args.max_frames:
- print(f"\n✓ Reached max frames ({args.max_frames}). Stopping...")
- return
-
- except KeyboardInterrupt:
- raise
- except Exception as e:
- print(f"Error receiving/saving frame: {e}")
- time.sleep(0.1)
-
- except KeyboardInterrupt:
- print(f"\n\n✓ Recording stopped")
- print(f"📊 Total frames saved: {saved_count}")
- print(f"📁 Location: {save_dir.absolute()}")
- finally:
- client.stop_client()
-
-
-if __name__ == "__main__":
- main()
-
diff --git a/mujoco_sim_g1/view_cameras_live.py b/mujoco_sim_g1/view_cameras_live.py
deleted file mode 100644
index bacb637ab..000000000
--- a/mujoco_sim_g1/view_cameras_live.py
+++ /dev/null
@@ -1,224 +0,0 @@
-#!/usr/bin/env python3
-"""
-Live camera viewer for MuJoCo simulator using matplotlib
-Works without X11/GTK - suitable for SSH sessions with X forwarding
-"""
-import argparse
-import sys
-import time
-from pathlib import Path
-
-# Add sim module to path
-sys.path.insert(0, str(Path(__file__).parent))
-
-import cv2
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib.animation import FuncAnimation
-from sim.sensor_utils import SensorClient, ImageUtils
-
-
-class CameraViewer:
- def __init__(self, host, port):
- self.client = SensorClient()
- self.client.start_client(server_ip=host, port=port)
-
- self.fig = None
- self.axes = {}
- self.images = {}
- self.text_objs = {}
-
- self.frame_count = 0
- self.last_time = time.time()
- self.fps = 0
-
- def init_plot(self):
- """Initialize matplotlib figure and axes"""
- # Wait for first frame to know how many cameras we have
- print("Waiting for first frame to detect cameras...")
- data = self.client.receive_message()
-
- # Parse camera names - handle nested 'images' dict
- camera_names = []
- if "images" in data and isinstance(data["images"], dict):
- # Nested structure: data["images"]["camera_name"]
- camera_names = list(data["images"].keys())
- else:
- # Flat structure: data["camera_name"] directly
- camera_names = [k for k in data.keys() if k not in ["timestamps", "images"]]
-
- num_cameras = len(camera_names)
-
- if num_cameras == 0:
- print("No cameras found in stream!")
- return False
-
- print(f"Found {num_cameras} camera(s): {', '.join(camera_names)}")
-
- # Create subplots
- if num_cameras == 1:
- self.fig, ax = plt.subplots(1, 1, figsize=(10, 8))
- axes_list = [ax]
- elif num_cameras == 2:
- self.fig, axes_list = plt.subplots(1, 2, figsize=(16, 6))
- else:
- rows = (num_cameras + 1) // 2
- self.fig, axes_list = plt.subplots(rows, 2, figsize=(16, 6 * rows))
- axes_list = axes_list.flatten()
-
- # Initialize each camera subplot
- for i, cam_name in enumerate(camera_names):
- ax = axes_list[i]
- ax.set_title(f"{cam_name}", fontsize=12, fontweight='bold')
- ax.axis('off')
-
- # Get image data from nested or flat structure
- if "images" in data and cam_name in data["images"]:
- img_data = data["images"][cam_name]
- elif cam_name in data:
- img_data = data[cam_name]
- else:
- img_data = cam_name # Use the actual data if it's the value
-
- # Decode first image
- if isinstance(img_data, str):
- img = ImageUtils.decode_image(img_data)
- elif isinstance(img_data, np.ndarray):
- img = img_data
- else:
- print(f"Warning: Unknown image format for {cam_name}: {type(img_data)}")
- continue
-
- # Check if image is valid
- if img is None or not isinstance(img, np.ndarray):
- print(f"Warning: Invalid image data for {cam_name}")
- continue
-
- # Convert BGR to RGB for matplotlib
- img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
-
- # Display image
- im = ax.imshow(img_rgb)
- self.images[cam_name] = im
- self.axes[cam_name] = ax
-
- # Add FPS text
- text = ax.text(0.02, 0.98, 'FPS: 0.0',
- transform=ax.transAxes,
- fontsize=10,
- verticalalignment='top',
- bbox=dict(boxstyle='round', facecolor='black', alpha=0.7),
- color='lime',
- fontweight='bold')
- self.text_objs[cam_name] = text
-
- # Hide unused subplots
- if num_cameras < len(axes_list):
- for i in range(num_cameras, len(axes_list)):
- axes_list[i].axis('off')
-
- self.fig.tight_layout()
- return True
-
- def update_frame(self, frame_num):
- """Update function for animation"""
- try:
- # Receive new frame
- data = self.client.receive_message()
-
- # Calculate FPS
- self.frame_count += 1
- current_time = time.time()
- if current_time - self.last_time >= 1.0:
- self.fps = self.frame_count / (current_time - self.last_time)
- self.frame_count = 0
- self.last_time = current_time
-
- # Update each camera
- for cam_name in self.images.keys():
- # Get image data from nested or flat structure
- if "images" in data and cam_name in data["images"]:
- img_data = data["images"][cam_name]
- elif cam_name in data:
- img_data = data[cam_name]
- else:
- continue
-
- # Decode image
- if isinstance(img_data, str):
- img = ImageUtils.decode_image(img_data)
- elif isinstance(img_data, np.ndarray):
- img = img_data
- else:
- continue
-
- # Check if image is valid
- if img is None or not isinstance(img, np.ndarray):
- continue
-
- # Convert BGR to RGB for matplotlib
- img_rgb = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
-
- # Update image
- self.images[cam_name].set_data(img_rgb)
-
- # Update FPS text
- self.text_objs[cam_name].set_text(f'FPS: {self.fps:.1f}')
-
- except Exception as e:
- print(f"Error updating frame: {e}")
-
- return list(self.images.values()) + list(self.text_objs.values())
-
- def start(self, interval=33):
- """Start the live viewer"""
- if not self.init_plot():
- return
-
- print(f"\n{'='*60}")
- print("📹 Live camera viewer started!")
- print("Close the window or press Ctrl+C to exit")
- print(f"{'='*60}\n")
-
- # Create animation
- anim = FuncAnimation(
- self.fig,
- self.update_frame,
- interval=interval, # ms between frames
- blit=True,
- cache_frame_data=False
- )
-
- try:
- plt.show()
- except KeyboardInterrupt:
- print("\nStopping viewer...")
- finally:
- self.client.stop_client()
- plt.close('all')
-
-
-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=5555,
- help="ZMQ port (default: 5555)")
- parser.add_argument("--interval", type=int, default=33,
- help="Update interval in ms (default: 33 = ~30fps)")
- args = parser.parse_args()
-
- print("="*60)
- print("📷 MuJoCo Live Camera Viewer (matplotlib)")
- print("="*60)
- print(f"🌐 Connecting to: tcp://{args.host}:{args.port}")
- print(f"⏱️ Update interval: {args.interval}ms (~{1000/args.interval:.0f} fps)")
- print("="*60)
-
- viewer = CameraViewer(host=args.host, port=args.port)
- viewer.start(interval=args.interval)
-
-
-if __name__ == "__main__":
- main()
-
diff --git a/src/lerobot/envs/utils.py b/src/lerobot/envs/utils.py
index 8d0f24922..b89d053a2 100644
--- a/src/lerobot/envs/utils.py
+++ b/src/lerobot/envs/utils.py
@@ -221,7 +221,22 @@ def _load_module_from_path(path: str, module_name: str | None = None):
if spec is None:
raise ImportError(f"Could not load module spec for {module_name} from {path}")
module = importlib.util.module_from_spec(spec)
- spec.loader.exec_module(module) # type: ignore
+
+ # Add the module's directory to sys.path so it can import local modules
+ import sys
+ module_dir = os.path.dirname(os.path.abspath(path))
+ sys_path_modified = False
+ if module_dir not in sys.path:
+ sys.path.insert(0, module_dir)
+ sys_path_modified = True
+
+ try:
+ spec.loader.exec_module(module) # type: ignore
+ finally:
+ # Clean up sys.path after import
+ if sys_path_modified:
+ sys.path.remove(module_dir)
+
return module
diff --git a/src/lerobot/robots/unitree_g1/assets/g1/g1_body23.urdf b/src/lerobot/robots/unitree_g1/assets/g1/g1_body23.urdf
new file mode 100644
index 000000000..6ce66eca4
--- /dev/null
+++ b/src/lerobot/robots/unitree_g1/assets/g1/g1_body23.urdf
@@ -0,0 +1,903 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.urdf b/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.urdf
new file mode 100644
index 000000000..156a5fd52
--- /dev/null
+++ b/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.urdf
@@ -0,0 +1,1476 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.xml b/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.xml
new file mode 100644
index 000000000..01fd5e2cb
--- /dev/null
+++ b/src/lerobot/robots/unitree_g1/assets/g1/g1_body29_hand14.xml
@@ -0,0 +1,408 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/test_dance_minimal.py b/test_dance_minimal.py
new file mode 100644
index 000000000..dbbbf4eb2
--- /dev/null
+++ b/test_dance_minimal.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+"""Minimal test script for dance_102 motion imitation."""
+
+import sys
+from pathlib import Path
+import time
+sys.path.insert(0, str(Path(__file__).parent / "src"))
+
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
+
+config = UnitreeG1Config(
+ motion_imitation_control=True, # Enable motion imitation (dance)
+ simulation_mode=False
+)
+
+robot = UnitreeG1(config)
+
+# Keep alive
+try:
+ print("\n" + "="*60)
+ print("Dance_102 Motion Imitation Running!")
+ print(f"Motion duration: {robot.motion_loader.duration:.2f}s")
+ print("The robot will loop the dance motion")
+ print("Press Ctrl+C to stop")
+ print("="*60 + "\n")
+
+ while True:
+ time.sleep(1.0)
+ # Print progress every second
+ if robot.motion_loader:
+ progress = (robot.motion_loader.current_time / robot.motion_loader.duration) * 100
+ print(f"Progress: {progress:.1f}% - Time: {robot.motion_loader.current_time:.2f}s / {robot.motion_loader.duration:.2f}s")
+except KeyboardInterrupt:
+ print("\nStopping dance...")
+ robot.stop_motion_imitation_thread()
+ print("Dance stopped!")
+
diff --git a/test_locomotion_minimal.py b/test_locomotion_minimal.py
new file mode 100644
index 000000000..367fd879a
--- /dev/null
+++ b/test_locomotion_minimal.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+"""Minimal test script - just initialize robot with locomotion."""
+
+import sys
+from pathlib import Path
+import time
+sys.path.insert(0, str(Path(__file__).parent / "src"))
+
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
+
+config = UnitreeG1Config(locomotion_control=True, simulation_mode=True
+, policy_path="src/lerobot/robots/unitree_g1/assets/g1/locomotion/GR00T-WholeBodyControl-Walk-converted.onnx")
+#, policy_path="src/lerobot/robots/unitree_g1/assets/g1/locomotion/motion.pt")
+# dance 102
+robot = UnitreeG1(config)
+# Keep alive
+try:
+ while True:
+ time.sleep(1.0)
+except KeyboardInterrupt:
+ robot.stop_locomotion_thread()
+
+
diff --git a/test_zmq_camera_demo.py b/test_zmq_camera_demo.py
new file mode 100644
index 000000000..903aa6a23
--- /dev/null
+++ b/test_zmq_camera_demo.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+"""
+Single-script ZMQ camera demo - runs server and client in one process
+Demonstrates ZMQ camera functionality for PR reviewers
+"""
+
+import time
+from threading import Thread
+import cv2
+import numpy as np
+import zmq
+import matplotlib.pyplot as plt
+from matplotlib.animation import FuncAnimation
+
+from threading import Event
+
+from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig
+from lerobot.cameras.configs import ColorMode
+
+
+def create_test_pattern(width=640, height=480, frame_num=0):
+ """Generate simple vertical color bars"""
+ img = np.zeros((height, width, 3), dtype=np.uint8)
+
+ # Classic SMPTE color bars
+ colors = [
+ (255, 255, 255), # White
+ (0, 255, 255), # Yellow
+ (0, 255, 255), # Cyan
+ (0, 255, 0), # Green
+ (255, 0, 255), # Magenta
+ (0, 0, 255), # Red
+ (255, 0, 0), # Blue
+ ]
+
+ bar_width = width // len(colors)
+ for i, color in enumerate(colors):
+ x_start = i * bar_width
+ x_end = (i + 1) * bar_width if i < len(colors) - 1 else width
+ img[:, x_start:x_end] = color
+
+ return img
+
+
+def run_server(port=5550, stop_event=None):
+ """Background thread: publishes test pattern frames via ZMQ"""
+ context = zmq.Context()
+ socket = context.socket(zmq.PUB)
+ socket.bind(f"tcp://*:{port}")
+ time.sleep(0.5)
+
+ frame_num = 0
+ while stop_event is None or not stop_event.is_set():
+ frame = create_test_pattern(frame_num=frame_num)
+ _, buffer = cv2.imencode('.jpg', frame, [cv2.IMWRITE_JPEG_QUALITY, 85])
+ socket.send(buffer.tobytes())
+ frame_num += 1
+ time.sleep(1.0 / 30) # 30 FPS
+
+ socket.close()
+ context.term()
+
+port = 5550
+
+#SERVER
+
+stop_event = Event()
+server_thread = Thread(target=run_server, args=(port, stop_event), daemon=True)
+server_thread.start()
+time.sleep(1) # Let server initialize
+
+#CLIENT
+
+config = ZMQCameraConfig(
+ server_address="localhost",
+ port=port,
+ color_mode=ColorMode.RGB,
+ timeout_ms=5000
+)
+camera = ZMQCamera(config)
+camera.connect()
+
+# Setup matplotlib viewer
+fig, ax = plt.subplots(figsize=(10, 7))
+ax.set_title('ZMQ Camera Demo - Server & Client in one script', fontweight='bold')
+ax.axis('off')
+
+frame = camera.read()
+im = ax.imshow(frame)
+text = ax.text(0.02, 0.98, '', transform=ax.transAxes, fontsize=10, va='top',
+ bbox=dict(boxstyle='round', facecolor='black', alpha=0.7),
+ color='lime', fontweight='bold', family='monospace')
+fig.tight_layout()
+
+frame_count = [0]
+fps_times = []
+
+def update(frame_num):
+ loop_start = time.time()
+ frame = camera.read()
+ frame_count[0] += 1
+
+ im.set_data(frame)
+
+ fps_times.append(time.time() - loop_start)
+ if len(fps_times) > 30:
+ fps_times.pop(0)
+ fps = len(fps_times) / sum(fps_times) if fps_times else 0
+
+ text.set_text(f'Frame: {frame_count[0]}\nFPS: {fps:.1f}')
+ return [im, text]
+
+try:
+ anim = FuncAnimation(fig, update, interval=33, blit=True, cache_frame_data=False)
+ plt.show()
+except KeyboardInterrupt:
+ pass
+finally:
+ camera.disconnect()
+ stop_event.set()
+ plt.close('all')
diff --git a/test_zmq_camera_server.py b/test_zmq_camera_server.py
new file mode 100644
index 000000000..1d67dd0ac
--- /dev/null
+++ b/test_zmq_camera_server.py
@@ -0,0 +1,250 @@
+#!/usr/bin/env python3
+"""
+ZMQ Camera Server - Publishes camera frames via ZeroMQ
+
+This script captures frames from a camera (or generates test patterns) and
+publishes them as JPEG-encoded images over a ZMQ PUB socket.
+
+Usage:
+ # Using a webcam (default camera index 0)
+ python test_zmq_camera_server.py --port 5554
+
+ # Using a specific camera
+ python test_zmq_camera_server.py --camera 1 --port 5555
+
+ # Using test pattern (no camera required)
+ python test_zmq_camera_server.py --test-pattern --port 5554
+
+ # Specify IP address and resolution
+ python test_zmq_camera_server.py --address 192.168.1.100 --width 1280 --height 720
+"""
+
+import argparse
+import time
+from datetime import datetime
+
+import cv2
+import numpy as np
+import zmq
+
+
+def create_test_pattern(width: int = 640, height: int = 480, frame_count: int = 0) -> np.ndarray:
+ """
+ Creates an animated test pattern image.
+
+ Args:
+ width: Image width
+ height: Image height
+ frame_count: Current frame number for animation
+
+ Returns:
+ BGR image array
+ """
+ # Create a colorful test pattern
+ img = np.zeros((height, width, 3), dtype=np.uint8)
+
+ # Animated gradient background
+ for i in range(height):
+ color_shift = int(((i + frame_count) % height) / height * 255)
+ img[i, :] = [color_shift, 128, 255 - color_shift]
+
+ # Moving circle
+ center_x = int(width // 2 + 200 * np.sin(frame_count * 0.05))
+ center_y = int(height // 2 + 100 * np.cos(frame_count * 0.05))
+ cv2.circle(img, (center_x, center_y), 50, (0, 255, 0), -1)
+
+ # Add text with timestamp
+ text = f"ZMQ Test Pattern - Frame {frame_count}"
+ cv2.putText(img, text, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
+
+ timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+ cv2.putText(img, timestamp, (20, height - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1)
+
+ return img
+
+
+def main():
+ parser = argparse.ArgumentParser(description="ZMQ Camera Server - Publishes camera frames via ZeroMQ")
+ parser.add_argument("--address", type=str, default="*",
+ help="IP address to bind to (* for all interfaces)")
+ parser.add_argument("--port", type=int, default=5550,
+ help="Port number to publish on (default: 5554)")
+ parser.add_argument("--camera", type=int, default=0,
+ help="Camera device index (default: 0)")
+ parser.add_argument("--test-pattern", action="store_true",
+ help="Use animated test pattern instead of camera")
+ parser.add_argument("--width", type=int, default=640,
+ help="Frame width (default: 640)")
+ parser.add_argument("--height", type=int, default=480,
+ help="Frame height (default: 480)")
+ parser.add_argument("--fps", type=int, default=30,
+ help="Target frames per second (default: 30)")
+ parser.add_argument("--quality", type=int, default=80,
+ help="JPEG quality 1-100 (default: 80)")
+ parser.add_argument("--display", action="store_true",
+ help="Display the video feed locally")
+
+ args = parser.parse_args()
+
+ # Initialize ZMQ
+ print(f"Initializing ZMQ publisher on tcp://{args.address}:{args.port}")
+ context = zmq.Context()
+ socket = context.socket(zmq.PUB)
+ socket.bind(f"tcp://{args.address}:{args.port}")
+
+ # Give ZMQ time to establish connections
+ print("Waiting for subscribers to connect...")
+ time.sleep(1)
+
+ # Initialize camera or test pattern
+ cap = None
+ if not args.test_pattern:
+ print(f"Opening camera {args.camera}...")
+ cap = cv2.VideoCapture(args.camera)
+
+ if not cap.isOpened():
+ print(f"ERROR: Could not open camera {args.camera}")
+ print("Falling back to test pattern mode")
+ args.test_pattern = True
+ else:
+ # Set camera properties
+ cap.set(cv2.CAP_PROP_FRAME_WIDTH, args.width)
+ cap.set(cv2.CAP_PROP_FRAME_HEIGHT, args.height)
+ cap.set(cv2.CAP_PROP_FPS, args.fps)
+
+ # Read actual properties (camera may not support requested values)
+ actual_width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+ actual_height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+ actual_fps = cap.get(cv2.CAP_PROP_FPS)
+
+ print(f"Camera opened successfully")
+ print(f" Resolution: {actual_width}x{actual_height}")
+ print(f" FPS: {actual_fps}")
+
+ if args.test_pattern:
+ print(f"Using test pattern mode")
+ print(f" Resolution: {args.width}x{args.height}")
+ print(f" FPS: {args.fps}")
+
+ print("\n" + "="*60)
+ print("SERVER RUNNING - Press Ctrl+C to stop")
+ print("="*60)
+ print(f"Publishing JPEG frames on tcp://{args.address}:{args.port}")
+ if args.address == "*":
+ print("\nClients can connect using your machine's IP address")
+ print("Run 'hostname -I' or 'ipconfig' to find your IP")
+ print("\nPress Ctrl+C to stop the server")
+ print("="*60 + "\n")
+
+ frame_count = 0
+ start_time = time.time()
+ last_print_time = start_time
+ frame_times = []
+
+ # JPEG encoding parameters
+ encode_params = [int(cv2.IMWRITE_JPEG_QUALITY), args.quality]
+
+ try:
+ while True:
+ loop_start = time.time()
+
+ # Capture or generate frame
+ if args.test_pattern:
+ frame = create_test_pattern(args.width, args.height, frame_count)
+ ret = True
+ else:
+ ret, frame = cap.read()
+
+ if not ret:
+ print("ERROR: Failed to capture frame")
+ break
+
+ # Resize if needed
+ if frame.shape[1] != args.width or frame.shape[0] != args.height:
+ frame = cv2.resize(frame, (args.width, args.height))
+
+ # Encode frame as JPEG
+ encode_start = time.time()
+ _, buffer = cv2.imencode('.jpg', frame, encode_params)
+ encode_time = (time.time() - encode_start) * 1000
+
+ # Publish frame
+ publish_start = time.time()
+ socket.send(buffer.tobytes())
+ publish_time = (time.time() - publish_start) * 1000
+
+ frame_count += 1
+
+ # Display locally if requested
+ if args.display:
+ # Add stats overlay
+ display_frame = frame.copy()
+ stats_text = f"Frame: {frame_count} | FPS: {len(frame_times):.1f}"
+ cv2.putText(display_frame, stats_text, (10, 30),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
+ cv2.imshow('ZMQ Camera Server', display_frame)
+
+ # Exit on 'q' key
+ if cv2.waitKey(1) & 0xFF == ord('q'):
+ print("\nStopping server (user pressed 'q')...")
+ break
+
+ # Calculate timing statistics
+ loop_time = time.time() - loop_start
+ frame_times.append(loop_time)
+
+ # Keep only last second of frame times for FPS calculation
+ if len(frame_times) > args.fps * 2:
+ frame_times = frame_times[-args.fps:]
+
+ # Print statistics every 2 seconds
+ current_time = time.time()
+ if current_time - last_print_time >= 2.0:
+ elapsed = current_time - start_time
+ actual_fps = len(frame_times) / sum(frame_times) if frame_times else 0
+ avg_loop_time = np.mean(frame_times) * 1000 if frame_times else 0
+
+ print(f"[{elapsed:6.1f}s] Frames: {frame_count:5d} | "
+ f"FPS: {actual_fps:5.1f} | "
+ f"Loop: {avg_loop_time:5.1f}ms | "
+ f"Encode: {encode_time:4.1f}ms | "
+ f"Publish: {publish_time:4.1f}ms")
+
+ last_print_time = current_time
+
+ # Control frame rate
+ target_loop_time = 1.0 / args.fps
+ sleep_time = target_loop_time - loop_time
+ if sleep_time > 0:
+ time.sleep(sleep_time)
+
+ except KeyboardInterrupt:
+ print("\n\nStopping server (Ctrl+C pressed)...")
+
+ finally:
+ # Cleanup
+ if cap is not None:
+ cap.release()
+
+ if args.display:
+ cv2.destroyAllWindows()
+
+ socket.close()
+ context.term()
+
+ # Print final statistics
+ elapsed = time.time() - start_time
+ avg_fps = frame_count / elapsed if elapsed > 0 else 0
+
+ print("\n" + "="*60)
+ print("SERVER STOPPED")
+ print("="*60)
+ print(f"Total frames published: {frame_count}")
+ print(f"Total time: {elapsed:.2f}s")
+ print(f"Average FPS: {avg_fps:.2f}")
+ print("="*60)
+
+
+if __name__ == "__main__":
+ main()
+
diff --git a/unitree_sim_isaaclab b/unitree_sim_isaaclab
deleted file mode 160000
index 5b89b377b..000000000
--- a/unitree_sim_isaaclab
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 5b89b377bcf576eb256aaa994ada2b01c97cdfa1