diff --git a/.gitattributes b/.gitattributes index 8dc0b67080237f3f47dea5a4edb81f4df0916e10..5c45dbce09feef30173b68f5e2c87514ce272faf 100644 --- a/.gitattributes +++ b/.gitattributes @@ -86,25 +86,3 @@ assets/meshes/waist_constraint_R.STL filter=lfs diff=lfs merge=lfs -text assets/meshes/waist_support_link.STL filter=lfs diff=lfs merge=lfs -text assets/meshes/waist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text assets/meshes/waist_yaw_link_rev_1_0.STL filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/shoulder__2.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/shoulder__3.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/shoulder__4.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/shoulder__5.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/shoulder.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist__2.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist__3.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist__4.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist__5.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist__6.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_left/wrist.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/shoulder__2.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/shoulder__3.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/shoulder__4.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/shoulder__5.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/shoulder.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist__2.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist__3.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist__4.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist__5.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist__6.stl filter=lfs diff=lfs merge=lfs -text -assets/meshes_exo_right/wrist.stl filter=lfs diff=lfs merge=lfs -text diff --git a/CAMERA_README.md b/CAMERA_README.md new file mode 100644 index 0000000000000000000000000000000000000000..dd407a876d3cdad6fdc41731616332e741a3843e --- /dev/null +++ b/CAMERA_README.md @@ -0,0 +1,256 @@ +# Camera System for MuJoCo G1 Simulator + +## Overview + +The simulator has two cameras defined: + +### 1. **`head_camera`** - Robot Ego-View +- **Location**: Attached to `torso_link` body +- **Position**: `[0.06, 0.0, 0.45]` relative to torso (6cm forward, 45cm up) +- **Orientation**: `euler="0 -0.8 -1.57"` (facing forward, slightly tilted down) +- **FOV**: 90 degrees +- **Purpose**: First-person view from the robot's perspective (like a head-mounted camera) + +### 2. **`global_view`** - Third-Person View +- **Location**: Fixed in world coordinates +- **Position**: `[2.910, -5.040, 3.860]` (behind and above the robot) +- **Purpose**: External observer view for visualization + +## How Camera Publishing Works + +The camera system uses a **zero-copy architecture** with three components: + +``` +┌─────────────────────┐ +│ MuJoCo Simulator │ +│ (Main Process) │ +│ │ +│ 1. Render cameras │ +│ 2. Copy to shmem │──┐ +└─────────────────────┘ │ + │ Shared Memory + │ (fast IPC) + ┌────▼────────────────┐ + │ Image Publisher │ + │ (Subprocess) │ + │ │ + │ 3. Encode images │ + │ 4. ZMQ publish │ + └─────────┬───────────┘ + │ + │ TCP (ZMQ) + │ port 5555 + ▼ + ┌─────────────────────┐ + │ Your Policy/Client │ + │ (Subscribe) │ + └─────────────────────┘ +``` + +### Key Technologies: +- **MuJoCo Renderer**: Captures RGB images from virtual cameras +- **Shared Memory (`multiprocessing.shared_memory`)**: Zero-copy transfer between processes +- **ZMQ (ZeroMQ)**: Network socket for publishing images (TCP) +- **No ROS2 required!** Pure Python multiprocessing + +## Usage + +### Basic Simulation (No Camera Publishing) +```bash +python run_sim.py +``` + +### With Camera Publishing +```bash +# Publish head camera on default port 5555 +python run_sim.py --publish-images + +# Publish multiple cameras +python run_sim.py --publish-images --cameras head_camera global_view + +# Custom port +python run_sim.py --publish-images --camera-port 6000 +``` + +### Viewing Camera Streams + +In a **separate terminal**, run the camera viewer: + +```bash +# Basic usage (default: localhost:5555) +python view_cameras.py + +# Custom host/port +python view_cameras.py --host 192.168.1.100 --port 6000 + +# Save images to directory +python view_cameras.py --save ./camera_recordings + +# Adjust display rate +python view_cameras.py --fps 60 +``` + +**Keyboard Controls:** +- `q`: Quit viewer +- `s`: Save snapshot of current frame + +**Example Workflow:** +```bash +# Terminal 1: Start simulator with camera publishing +python run_sim.py --publish-images + +# Terminal 2: View the camera feed +python view_cameras.py +``` + +### Receiving Images in Your Code + +```python +import zmq +import numpy as np +from gr00t_wbc.control.sensor.sensor_server import ImageMessageSchema + +# Connect to camera publisher +context = zmq.Context() +socket = context.socket(zmq.SUB) +socket.connect("tcp://localhost:5555") +socket.setsockopt(zmq.SUBSCRIBE, b"") # Subscribe to all messages + +while True: + # Receive serialized image data + data = socket.recv_pyobj() + + # Decode images + if "head_camera" in data: + # Decode image (returns numpy array HxWx3 uint8) + img = decode_image(data["head_camera"]) + + # Use image for your policy + process_observation(img) +``` + +## Camera Configuration + +Edit `config.yaml` to change camera settings: + +```yaml +IMAGE_DT: 0.033333 # Publishing rate (30 Hz) +ENABLE_OFFSCREEN: false # Enable for camera rendering +MP_START_METHOD: "spawn" # Multiprocessing method +``` + +Or programmatically in `run_sim.py`: + +```python +camera_configs = { + "head_camera": { + "height": 480, + "width": 640 + }, + "custom_camera": { + "height": 224, + "width": 224 + } +} +``` + +## Adding Custom Cameras + +Edit `assets/g1_29dof_with_hand.xml` or `assets/scene_43dof.xml`: + +```xml + + + + + + + + + +``` + +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/README.md b/README.md index ddf0b41aaa94d629b7092a354856d98ba75fe322..3fa6b01e16d8a417ca347077c084da7cf97eae89 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,167 @@ -# MuJoCo Sim for Unitree G1 +# Standalone MuJoCo Simulator for Unitree G1 -Standalone MuJoCo physics simulator for the Unitree G1 robot, adapted from gr00t_wbc. Currently supports G1_29dof. +This is a standalone MuJoCo physics simulator for the Unitree G1 robot, extracted from the GR00T-WholeBodyControl repository. + +## Features + +- **Physics Simulation**: Runs G1 robot in MuJoCo at 500Hz (2ms timestep) +- **DDS Communication**: Uses Unitree SDK2 DDS for robot communication +- **Compatible**: Works with existing `unitree_g1.py` control code via DDS +- **Visualization**: Real-time 3D visualization of robot motion + +## Directory Structure + +``` +mujoco_sim_g1/ +├── requirements.txt # Python dependencies +├── run_sim.py # Main launcher script +├── config.yaml # Simulation configuration +├── sim/ # Simulation modules +│ ├── base_sim.py +│ ├── simulator_factory.py +│ ├── unitree_sdk2py_bridge.py +│ └── ... +└── assets/ # Robot models + ├── g1_29dof_with_hand.xml + └── meshes/*.STL +``` + +## Installation + +### 1. Create Virtual Environment (Recommended) + +```bash +cd mujoco_sim_g1 +python3 -m venv venv +source venv/bin/activate # On Windows: venv\Scripts\activate +``` + +### 2. Install Dependencies + +```bash +pip install -r requirements.txt +``` + +**Note**: If you encounter issues with `rclpy`, you can comment out the ROS2 imports in `sim/base_sim.py` (lines 11, 609-617) if you don't need camera publishing. + +## Usage + +### Basic Usage + +```bash +# Activate environment +source venv/bin/activate + +# Run the simulator +python run_sim.py +``` + +The simulator will: +1. Load the G1 robot model +2. Initialize DDS communication on domain 0 +3. Open a MuJoCo visualization window +4. Start listening for motor commands via DDS + +### Running with Your Robot Control Code + +Once the simulator is running, you can control it from another terminal: + +```bash +# In another terminal +cd /home/yope/Documents/lerobot +conda activate unitree_lerobot + +# Run your existing control code +python test_locomotion_minimal.py +``` + +Your `unitree_g1.py` code will automatically connect to the simulator via DDS! + +## Configuration + +Edit `config.yaml` to customize: + +- **SIMULATE_DT**: Simulation timestep (default: 0.002s = 500Hz) +- **DOMAIN_ID**: DDS domain ID (default: 0) +- **ENABLE_ONSCREEN**: Show visualization (default: true) +- **USE_JOYSTICK**: Enable gamepad control (default: false) +- **ROBOT_SCENE**: Path to MuJoCo XML scene + +### PD Gains + +The simulator uses the following PD gains (matching NVIDIA GR00T): + +**Legs (indices 0-11):** +- Hip joints: KP=150, KD=2 +- Knee joints: KP=300, KD=4 +- Ankle joints: KP=40, KD=2 + +**Waist (indices 12-14):** +- All waist joints: KP=250, KD=5 + +**Arms (indices 15-28):** +- Shoulders: KP=100, KD=2-5 +- Elbows/Wrists: KP=20-40, KD=1-2 + +## Troubleshooting + +### ImportError: cannot import name 'ChannelFactoryInitialize' + +```bash +pip install --upgrade unitree-sdk2py +``` + +### ROS2/rclpy errors + +If you don't need camera publishing, edit `sim/base_sim.py`: +- Comment out line 11: `import rclpy` +- Comment out lines 609-617 (ROS2 initialization) + +### Meshes not found + +Make sure mesh paths in `assets/g1_29dof_with_hand.xml` are relative: +```xml + +``` + +### Robot falls immediately + +Check that: +1. PD gains match NVIDIA's values (see config.yaml) +2. Velocity command scaling is correct (ang_vel_scale=0.25, cmd_scale=[2.0, 2.0, 0.25]) +3. Observations for missing joints are zeroed out (indices 12, 14, 20, 21, 27, 28) + +## Technical Details + +### Communication + +The simulator publishes: +- **`rt/lowstate`**: Robot state (joint positions, velocities, IMU, etc.) +- **`rt/wirelesscontroller`**: Wireless remote controller state (if joystick enabled) + +The simulator subscribes to: +- **`rt/lowcmd`**: Motor commands (position, velocity, torque, KP, KD) + +### Coordinate Frames + +- **World frame**: Z-up +- **Joint ordering**: 29 DOF (12 legs + 3 waist + 14 arms) +- **IMU**: Quaternion in [w, x, y, z] format + +### Performance + +- Simulation runs at ~500Hz (2ms timestep) +- Viewer updates at ~50Hz (20ms) +- Typical CPU usage: 20-40% on single core + +## Files from GR00T-WholeBodyControl + +This standalone simulator was extracted from: +- `gr00t_wbc/control/envs/g1/sim/` (simulation modules) +- `gr00t_wbc/control/robot_model/model_data/g1/` (robot model files) +- `gr00t_wbc/control/main/teleop/configs/g1_29dof_gear_wbc.yaml` (configuration) + +## License + +Follows the license of the original GR00T-WholeBodyControl repository. -set use joystick to 1 to control the robot \ No newline at end of file diff --git a/assets/exo_left.urdf b/assets/exo_left.urdf deleted file mode 100644 index b37ab533b2845a990bc2d8788d4077459f68484b..0000000000000000000000000000000000000000 --- a/assets/exo_left.urdf +++ /dev/null @@ -1,347 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/assets/exo_right.urdf b/assets/exo_right.urdf deleted file mode 100644 index f176316e33b6928674f0a9cf36568c474781a87e..0000000000000000000000000000000000000000 --- a/assets/exo_right.urdf +++ /dev/null @@ -1,347 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/assets/g1_29dof_no_hand.xml b/assets/g1_29dof_old.xml similarity index 100% rename from assets/g1_29dof_no_hand.xml rename to assets/g1_29dof_old.xml diff --git a/assets/g1_29dof_with_hand_original.xml b/assets/g1_29dof_with_hand_original.xml new file mode 100644 index 0000000000000000000000000000000000000000..a53cd991faf3a296bf67acbf78cd702ce15c31d0 --- /dev/null +++ b/assets/g1_29dof_with_hand_original.xml @@ -0,0 +1,748 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/assets/g1_body29_hand14.urdf b/assets/g1_body29_hand14.urdf deleted file mode 100644 index 156a5fd524b08ae54b0123f589f9f0948dc5f4c7..0000000000000000000000000000000000000000 --- a/assets/g1_body29_hand14.urdf +++ /dev/null @@ -1,1476 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/assets/meshes_exo_left/part_4.stl b/assets/meshes_exo_left/part_4.stl deleted file mode 100644 index e0004e13372738fe87a2e168021e6aaffa3955bd..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_left/part_4.stl and /dev/null differ diff --git a/assets/meshes_exo_left/shoulder.stl b/assets/meshes_exo_left/shoulder.stl deleted file mode 100644 index 1817d2a315362fcc87e74115f9bef2a1f7890bea..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/shoulder.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:98759c5e7684006fd20bfba3f082022e12b4f3700fd4dfb47119929b22b2695f -size 194384 diff --git a/assets/meshes_exo_left/shoulder__2.stl b/assets/meshes_exo_left/shoulder__2.stl deleted file mode 100644 index 4509ab9030c6b5f38c4999c74492693a475052b9..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/shoulder__2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:ff937dd8d731370c130f534916f15b63a202cded3a9a10b87c282bb72e38483d -size 136584 diff --git a/assets/meshes_exo_left/shoulder__3.stl b/assets/meshes_exo_left/shoulder__3.stl deleted file mode 100644 index 83b7cd1ac907cb5bd5f8d94e1df092f71d018202..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/shoulder__3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e9b7229f1980357280d437213c1d279df46ade7d37ef5eb37e6301dc7b2c2d30 -size 577884 diff --git a/assets/meshes_exo_left/shoulder__4.stl b/assets/meshes_exo_left/shoulder__4.stl deleted file mode 100644 index 2f4401cf0b9bae6aada2f4ed50065253eec8deca..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/shoulder__4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3b8b58ef8958d187f60b8d10cd691e01c7f3adf483ab09271824e3b9089ea410 -size 580484 diff --git a/assets/meshes_exo_left/shoulder__5.stl b/assets/meshes_exo_left/shoulder__5.stl deleted file mode 100644 index 457db0d56770e5b0a335d85d8998d34136df7e9b..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/shoulder__5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6fcb3d9a0a3c28541390fb9d7b325b041a08ae2ffab724dece4fb0178781307b -size 164084 diff --git a/assets/meshes_exo_left/wrist.stl b/assets/meshes_exo_left/wrist.stl deleted file mode 100644 index b30cbb04b15bafd69a7d2316236fa04e629bc709..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a28d777d62dcc0fb25467a682d91811e742772fa452d1141c4bffd565db037b2 -size 520784 diff --git a/assets/meshes_exo_left/wrist__2.stl b/assets/meshes_exo_left/wrist__2.stl deleted file mode 100644 index bd25bef28a1a6de32709735978f579e59329c2c4..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist__2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:71b29d7a3927bad99efffe572c07271531681984d46aed5984943039c17e3346 -size 102084 diff --git a/assets/meshes_exo_left/wrist__3.stl b/assets/meshes_exo_left/wrist__3.stl deleted file mode 100644 index 579659390cd53ba7b40b6a39e33774e456ccd5b8..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist__3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c03a91b4800cdbc00bd290f77fd1bd90a744a5d9b7bb2220d515d9991f696ea6 -size 137084 diff --git a/assets/meshes_exo_left/wrist__4.stl b/assets/meshes_exo_left/wrist__4.stl deleted file mode 100644 index 67fb64c0e3ef98381b6c7b88c18f157659ac9fde..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist__4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d898dbcddb44c176729b5144b40490437a2a09ec50a3929b255917c522fa389c -size 724284 diff --git a/assets/meshes_exo_left/wrist__5.stl b/assets/meshes_exo_left/wrist__5.stl deleted file mode 100644 index 9d4db5b3d914a2d2b52c52c01a9eafc12bc53280..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist__5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:8eb6c0298ff682e4e199bb5f25e9e198d54db944fecf0146203cc6714993f06f -size 138984 diff --git a/assets/meshes_exo_left/wrist__6.stl b/assets/meshes_exo_left/wrist__6.stl deleted file mode 100644 index be87c70c82f2c9dadc1dcb0eff12b00771199523..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_left/wrist__6.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:92590e6c4a435046f8647e3b4d3101843fadc7a9ea14475e5008ae2aa58fae1f -size 182184 diff --git a/assets/meshes_exo_left/x.stl b/assets/meshes_exo_left/x.stl deleted file mode 100644 index c3901d41ed980c0f1998f85628dc54dc7a6fddec..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_left/x.stl and /dev/null differ diff --git a/assets/meshes_exo_left/y.stl b/assets/meshes_exo_left/y.stl deleted file mode 100644 index d8265d69b551ae65295d691023981a8bdd4910a5..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_left/y.stl and /dev/null differ diff --git a/assets/meshes_exo_left/z.stl b/assets/meshes_exo_left/z.stl deleted file mode 100644 index abfbc02174a388f535eb61775312fe8708f2125c..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_left/z.stl and /dev/null differ diff --git a/assets/meshes_exo_right/part_4.stl b/assets/meshes_exo_right/part_4.stl deleted file mode 100644 index 210476af0979c199c6e5ef60fdc65a53e4b59af7..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_right/part_4.stl and /dev/null differ diff --git a/assets/meshes_exo_right/shoulder.stl b/assets/meshes_exo_right/shoulder.stl deleted file mode 100644 index 0f6a7e918086e4b5d1ce60606f0189aa6f7738c6..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/shoulder.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:42e98b8dc6f63da139874bfef108ce4a92fd68ea332ef0d2770b35679cdec572 -size 194384 diff --git a/assets/meshes_exo_right/shoulder__2.stl b/assets/meshes_exo_right/shoulder__2.stl deleted file mode 100644 index afaaa5db7854d452b416ad4420149fd50a6473e2..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/shoulder__2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:b2089082163df5498518dd2adcb005e0f32687def5707358724ca5d1f7ac70d8 -size 136584 diff --git a/assets/meshes_exo_right/shoulder__3.stl b/assets/meshes_exo_right/shoulder__3.stl deleted file mode 100644 index 37d81303dbeec257c9d70782bade25de3a5bdb0b..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/shoulder__3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:0f7000c1891a9fed0ccfebbb1070e8f787547970207f1c1ae51a962d85deeed7 -size 577884 diff --git a/assets/meshes_exo_right/shoulder__4.stl b/assets/meshes_exo_right/shoulder__4.stl deleted file mode 100644 index c9b5378029c7a9341d8bfe5c9b5f528034788004..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/shoulder__4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:78838a47fc73f97ab3f355ba903621b5b102ce5de278a0ecf35b3cd05ab228dc -size 580484 diff --git a/assets/meshes_exo_right/shoulder__5.stl b/assets/meshes_exo_right/shoulder__5.stl deleted file mode 100644 index 806cc16612dd72f0c3390a6fb13dfb827ed2106c..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/shoulder__5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:d864c1b61a23a047844799d6a924a9554887d267916ba72bdc24f1ff1434ee6d -size 164084 diff --git a/assets/meshes_exo_right/wrist.stl b/assets/meshes_exo_right/wrist.stl deleted file mode 100644 index 4ba78bee3fbc4840d2ba489c955e1e6d95c9b60a..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:18197e26156ff8a25ab38a2f20e03870d7b8ca56f3299351e2b54377670ee3d0 -size 520784 diff --git a/assets/meshes_exo_right/wrist__2.stl b/assets/meshes_exo_right/wrist__2.stl deleted file mode 100644 index e23af8c9f0f88d654efa38bae0d092da9862d3e3..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist__2.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5b9cafe9f773c7719a30390add14efbfb0f0dfefe0287228156dca052d35c6a4 -size 102084 diff --git a/assets/meshes_exo_right/wrist__3.stl b/assets/meshes_exo_right/wrist__3.stl deleted file mode 100644 index 151ca5b2e7bdc023657c194c3e386e98b515f399..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist__3.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c1f2a8cb8edc00116626e4779d9cb3f554e33d2a436b682d3e3f99ddd0ee52f9 -size 137084 diff --git a/assets/meshes_exo_right/wrist__4.stl b/assets/meshes_exo_right/wrist__4.stl deleted file mode 100644 index 62b9bd0ebf9e0e8dffc2731a3cda88b7f1b88a30..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist__4.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e8ca263cadaa40534fd9edff649057633aaa925c6ad56e6ab25275157182b35e -size 724284 diff --git a/assets/meshes_exo_right/wrist__5.stl b/assets/meshes_exo_right/wrist__5.stl deleted file mode 100644 index 7b2e3765ee377a95b7f71bc300691e6ce00d347e..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist__5.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c9bacaf421feab7f65bb05674eca1fd972cb84690cb694eed0032dbce3c5b22 -size 138984 diff --git a/assets/meshes_exo_right/wrist__6.stl b/assets/meshes_exo_right/wrist__6.stl deleted file mode 100644 index 0dcc69dc5834075b1ea40c831f34ce48c6b296c1..0000000000000000000000000000000000000000 --- a/assets/meshes_exo_right/wrist__6.stl +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:a8568cd6286f54f186cb769b038261e0b498585443bbde573e4ebcb2dd551acb -size 182184 diff --git a/assets/meshes_exo_right/x.stl b/assets/meshes_exo_right/x.stl deleted file mode 100644 index 825da479868b8635fe390343a157773aa5b630e5..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_right/x.stl and /dev/null differ diff --git a/assets/meshes_exo_right/y.stl b/assets/meshes_exo_right/y.stl deleted file mode 100644 index 117d2301c874278101b64f178e7fde5acda78b40..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_right/y.stl and /dev/null differ diff --git a/assets/meshes_exo_right/z.stl b/assets/meshes_exo_right/z.stl deleted file mode 100644 index 2e09a510139f00a386654c31a483d6f973311f2f..0000000000000000000000000000000000000000 Binary files a/assets/meshes_exo_right/z.stl and /dev/null differ diff --git a/assets/scene_29dof.xml b/assets/scene_29dof.xml index 48264e2559b9322649eddb7b6493944ffbd34317..0b60f2f83228277ea83ebea17a0ede1eb7a5383b 100644 --- a/assets/scene_29dof.xml +++ b/assets/scene_29dof.xml @@ -1,5 +1,5 @@ - + diff --git a/config.yaml b/config.yaml index cff69ffffe9372a62315f0edb84d472ac86c99bc..b85a343559bc1d350b4daccce51ee93432d67184 100644 --- a/config.yaml +++ b/config.yaml @@ -1,49 +1,423 @@ -# Robot Configuration -ROBOT_TYPE: 'g1_29dof' -ROBOT_SCENE: "assets/scene_43dof.xml" +GEAR_WBC_CONFIG: "gr00t_wbc/sim2mujoco/resources/robots/g1/g1_gear_wbc.yaml" -# DDS Communication -DOMAIN_ID: 0 -INTERFACE: "lo" # "lo" for simulation, network interface with "192.168.123.222" for real robot -SIMULATOR: "mujoco" +# copy from g1_43dof_hist.yaml +ROBOT_TYPE: 'g1_29dof' # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1" +ROBOT_SCENE: "assets/scene_43dof.xml" # Robot scene with 3-finger dex hands -# Joystick -USE_JOYSTICK: 1 -JOYSTICK_TYPE: "xbox" # "xbox" or "switch" -JOYSTICK_DEVICE: 0 +DOMAIN_ID: 0 # Domain id +# Network Interface, "lo" for simulation and the one with "192.168.123.222" for real robot +INTERFACE: "lo" +SIMULATOR: "mujoco" # "robocasa" -# Simulation Settings -FREE_BASE: False -enable_waist: False # False = band on torso, True = band on pelvis -ENABLE_ELASTIC_BAND: True +USE_JOYSTICK: 0 # Simulate Unitree WirelessController using a gamepad (0: disable, 1: enable) +JOYSTICK_TYPE: "xbox" # support "xbox" and "switch" gamepad layout +JOYSTICK_DEVICE: 0 # Joystick number -# Timing -SIMULATE_DT: 0.004 # 250Hz physics -VIEWER_DT: 0.02 # 50Hz viewer update -IMAGE_DT: 0.033333 # ~30Hz camera +FREE_BASE: False +enable_waist: False # Enable waist joints control (False = band on torso, True = band on pelvis) -# Rendering -ENABLE_ONSCREEN: true -ENABLE_OFFSCREEN: false -MP_START_METHOD: "spawn" +PRINT_SCENE_INFORMATION: True # Print link, joint and sensors information of robot +ENABLE_ELASTIC_BAND: True # Virtual spring band, used for lifting h1 -# Sensors +SIMULATE_DT: 0.002 # Simulation timestep (500Hz) +ENABLE_ONSCREEN: true # Show visualization window +ENABLE_OFFSCREEN: false # Offscreen rendering for cameras (set to true to enable camera publishing) +IMAGE_DT: 0.033333 # Image publishing rate ~30Hz +MP_START_METHOD: "spawn" # Multiprocessing start method for image subprocess +VIEWER_DT: 0.02 # Viewer update time +REWARD_DT: 0.02 USE_SENSOR: False -PRINT_SCENE_INFORMATION: True +USE_HISTORY: True +USE_HISTORY_LOCO: True +USE_HISTORY_MIMIC: True + +GAIT_PERIOD: 0.9 # 1.25 + +MOTOR2JOINT: [0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 10, 11, + 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28] + +JOINT2MOTOR: [0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 10, 11, + 12, 13, 14, + 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28] + + +UNITREE_LEGGED_CONST: + HIGHLEVEL: 0xEE + LOWLEVEL: 0xFF + TRIGERLEVEL: 0xF0 + PosStopF: 2146000000.0 + VelStopF: 16000.0 + MODE_MACHINE: 5 + MODE_PR: 0 + +JOINT_KP: [ + 100, 100, 100, 200, 20, 20, + 100, 100, 100, 200, 20, 20, + 400, 400, 400, + 90, 60, 20, 60, 4, 4, 4, + 90, 60, 20, 60, 4, 4, 4 +] + + +JOINT_KD: [ + 2.5, 2.5, 2.5, 5, 0.2, 0.1, + 2.5, 2.5, 2.5, 5, 0.2, 0.1, + 5.0, 5.0, 5.0, + 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2, + 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2 +] + +# arm kp +# soft kp, safe, test it first +# 50, 50, 20, 20, 10, 10, 10 +# hard kp, use only if policy is safe +# 200, 200, 80, 80, 50, 50, 50, + +# MOTOR_KP: [ +# 100, 100, 100, 200, 20, 20, +# 100, 100, 100, 200, 20, 20, +# 400, 400, 400, +# 50, 50, 20, 20, 10, 10, 10, +# 50, 50, 20, 20, 10, 10, 10 +# ] + +MOTOR_KP: [ + 150, 150, 150, 200, 40, 40, + 150, 150, 150, 200, 40, 40, + 800, 250, 800, + 100, 100, 40, 40, 100, 100, 100, + 100, 100, 40, 40, 100, 100, 100 +] + +MOTOR_KD: [ + 2, 2, 2, 4, 2, 2, + 2, 2, 2, 4, 2, 2, + 10, 5, 10, + 5, 5, 2, 2, 5, 5, 5, + 5, 5, 2, 2, 5, 5, 5 +] + +# MOTOR_KP: [ +# 100, 100, 100, 200, 20, 20, +# 100, 100, 100, 200, 20, 20, +# 400, 400, 400, +# 90, 60, 20, 60, 4, 4, 4, +# 90, 60, 20, 60, 4, 4, 4 +# ] + + +# MOTOR_KD: [ +# 2.5, 2.5, 2.5, 5, 0.2, 0.1, +# 2.5, 2.5, 2.5, 5, 0.2, 0.1, +# 5.0, 5.0, 5.0, +# 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2, +# 2.0, 1.0, 0.4, 1.0, 0.2, 0.2, 0.2 +# ] + + +WeakMotorJointIndex: + left_hip_yaw_joint: 0 + left_hip_roll_joint: 1 + left_hip_pitch_joint: 2 + left_knee_joint: 3 + left_ankle_pitch_joint: 4 + left_ankle_roll_joint: 5 + right_hip_yaw_joint: 6 + right_hip_roll_joint: 7 + right_hip_pitch_joint: 8 + right_knee_joint: 9 + right_ankle_pitch_joint: 10 + right_ankle_roll_joint: 11 + waist_yaw_joint : 12 + waist_roll_joint : 13 + waist_pitch_joint : 14 + left_shoulder_pitch_joint: 15 + left_shoulder_roll_joint: 16 + left_shoulder_yaw_joint: 17 + left_elbow_joint: 18 + left_wrist_roll_joint: 19 + left_wrist_pitch_joint: 20 + left_wrist_yaw_joint: 21 + right_shoulder_pitch_joint: 22 + right_shoulder_roll_joint: 23 + right_shoulder_yaw_joint: 24 + right_elbow_joint: 25 + right_wrist_roll_joint: 26 + right_wrist_pitch_joint: 27 + right_wrist_yaw_joint: 28 -# Robot Dimensions NUM_MOTORS: 29 NUM_JOINTS: 29 NUM_HAND_MOTORS: 7 NUM_HAND_JOINTS: 7 +NUM_UPPER_BODY_JOINTS: 17 + +DEFAULT_DOF_ANGLES: [ + -0.1, # left_hip_pitch_joint + 0.0, # left_hip_roll_joint + 0.0, # left_hip_yaw_joint + 0.3, # left_knee_joint + -0.2, # left_ankle_pitch_joint + 0.0, # left_ankle_roll_joint + -0.1, # right_hip_pitch_joint + 0.0, # right_hip_roll_joint + 0.0, # right_hip_yaw_joint + 0.3, # right_knee_joint + -0.2, # right_ankle_pitch_joint + 0.0, # right_ankle_roll_joint + 0.0, # waist_yaw_joint + 0.0, # waist_roll_joint + 0.0, # waist_pitch_joint + 0.0, # left_shoulder_pitch_joint + 0.0, # left_shoulder_roll_joint + 0.0, # left_shoulder_yaw_joint + 0.0, # left_elbow_joint + 0.0, # left_wrist_roll_joint + 0.0, # left_wrist_pitch_joint + 0.0, # left_wrist_yaw_joint + 0.0, # right_shoulder_pitch_joint + 0.0, # right_shoulder_roll_joint + 0.0, # right_shoulder_yaw_joint + 0.0, # right_elbow_joint + 0.0, # right_wrist_roll_joint + 0.0, # right_wrist_pitch_joint + 0.0 # right_wrist_yaw_joint +] + +DEFAULT_MOTOR_ANGLES: [ + -0.1, # left_hip_pitch_joint + 0.0, # left_hip_roll_joint + 0.0, # left_hip_yaw_joint + 0.3, # left_knee_joint + -0.2, # left_ankle_pitch_joint + 0.0, # left_ankle_roll_joint + -0.1, # right_hip_pitch_joint + 0.0, # right_hip_roll_joint + 0.0, # right_hip_yaw_joint + 0.3, # right_knee_joint + -0.2, # right_ankle_pitch_joint + 0.0, # right_ankle_roll_joint + 0.0, # waist_yaw_joint + 0.0, # waist_roll_joint + 0.0, # waist_pitch_joint + 0.0, # left_shoulder_pitch_joint + 0.0, # left_shoulder_roll_joint + 0.0, # left_shoulder_yaw_joint + 0.0, # left_elbow_joint + 0.0, # left_wrist_roll_joint + 0.0, # left_wrist_pitch_joint + 0.0, # left_wrist_yaw_joint + 0.0, # right_shoulder_pitch_joint + 0.0, # right_shoulder_roll_joint + 0.0, # right_shoulder_yaw_joint + 0.0, # right_elbow_joint + 0.0, # right_wrist_roll_joint + 0.0, # right_wrist_pitch_joint + 0.0 # right_wrist_yaw_joint +] -# Torque Limits (Nm) - 29 body + 14 hand = 43 total -motor_effort_limit_list: [ - 88.0, 88.0, 88.0, 139.0, 50.0, 50.0, # left leg - 88.0, 88.0, 88.0, 139.0, 50.0, 50.0, # right leg - 88.0, 50.0, 50.0, # waist - 25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # left arm - 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, # left hand - 25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # right arm - 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7 # right hand +motor_pos_lower_limit_list: [-2.5307, -0.5236, -2.7576, -0.087267, -0.87267, -0.2618, + -2.5307, -2.9671, -2.7576, -0.087267, -0.87267, -0.2618, + -2.618, -0.52, -0.52, + -3.0892, -1.5882, -2.618, -1.0472, + -1.972222054, -1.61443, -1.61443, + -3.0892, -2.2515, -2.618, -1.0472, + -1.972222054, -1.61443, -1.61443] +motor_pos_upper_limit_list: [2.8798, 2.9671, 2.7576, 2.8798, 0.5236, 0.2618, + 2.8798, 0.5236, 2.7576, 2.8798, 0.5236, 0.2618, + 2.618, 0.52, 0.52, + 2.6704, 2.2515, 2.618, 2.0944, + 1.972222054, 1.61443, 1.61443, + 2.6704, 1.5882, 2.618, 2.0944, + 1.972222054, 1.61443, 1.61443] +motor_vel_limit_list: [32.0, 32.0, 32.0, 20.0, 37.0, 37.0, + 32.0, 32.0, 32.0, 20.0, 37.0, 37.0, + 32.0, 37.0, 37.0, + 37.0, 37.0, 37.0, 37.0, + 37.0, 22.0, 22.0, + 37.0, 37.0, 37.0, 37.0, + 37.0, 22.0, 22.0] +motor_effort_limit_list: [88.0, 88.0, 88.0, 139.0, 50.0, 50.0, + 88.0, 88.0, 88.0, 139.0, 50.0, 50.0, + 88.0, 50.0, 50.0, + 25.0, 25.0, 25.0, 25.0, + 25.0, 5.0, 5.0, + 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, + 25.0, 25.0, 25.0, 25.0, + 25.0, 5.0, 5.0, + 2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7] +history_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + ref_motion_phase: 4, + sin_phase: 4, + cos_phase: 4 + } +history_loco_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + # command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + sin_phase: 4, + cos_phase: 4 + } +history_loco_height_config: { + base_ang_vel: 4, + projected_gravity: 4, + command_lin_vel: 4, + command_ang_vel: 4, + command_base_height: 4, + command_stand: 4, + ref_upper_dof_pos: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + # phase_time: 4, + sin_phase: 4, + cos_phase: 4 + } +history_mimic_config: { + base_ang_vel: 4, + projected_gravity: 4, + dof_pos: 4, + dof_vel: 4, + actions: 4, + ref_motion_phase: 4, + } +obs_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + command_lin_vel: 2, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 1, + ref_upper_dof_pos: 17, # upper body actions + dof_pos: 29, + dof_vel: 29, + # actions: 12, # lower body actions + actions: 29, # full body actions + phase_time: 1, + ref_motion_phase: 1, # mimic motion phase + sin_phase: 1, + cos_phase: 1, + } +obs_loco_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + command_lin_vel: 2, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 1, + ref_upper_dof_pos: 17, # upper body actions + dof_pos: 29, + dof_vel: 29, + actions: 12, # lower body actions + phase_time: 1, + sin_phase: 1, + cos_phase: 1, + } +obs_mimic_dims: { + base_lin_vel: 3, + base_ang_vel: 3, + projected_gravity: 3, + dof_pos: 29, + dof_vel: 29, + actions: 29, # full body actions + ref_motion_phase: 1, # mimic motion phase + } +obs_scales: { + base_lin_vel: 2.0, + base_ang_vel: 0.25, + projected_gravity: 1.0, + command_lin_vel: 1, + command_ang_vel: 1, + command_stand: 1, + command_base_height: 2, # Yuanhang: it's 2, not 1! + ref_upper_dof_pos: 1.0, + dof_pos: 1.0, + dof_vel: 0.05, + history: 1.0, + history_loco: 1.0, + history_mimic: 1.0, + actions: 1.0, + phase_time: 1.0, + ref_motion_phase: 1.0, + sin_phase: 1.0, + cos_phase: 1.0 + } + +loco_upper_body_dof_pos: [ + 0.0, 0.0, 0.0, # waist + 0.0, 0.3, 0.0, 1.0, # left shoulder and elbow + 0.0, 0.0, 0.0, # left wrist + 0.0, -0.3, 0.0, 1.0, # right shoulder and elbow + 0.0, 0.0, 0.0 # right wrist ] + +robot_dofs: { + "g1_29dof": [1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1], + "g1_29dof_anneal_23dof": [1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, + 1, 1, 1, + 1, 1, 1, 1, 0, 0, 0, + 1, 1, 1, 1, 0, 0, 0], +} + +mimic_robot_types: { + + "APT_level1": "g1_29dof_anneal_23dof", +} + + + + +# 01281657 +mimic_models: { + "APT_level1": "20250116_225127-TairanTestbed_G129dofANNEAL23dof_dm_APT_video_APT_level1_MinimalFriction-0.3_RfiTrue_Far0.325_RESUME_LARGENOISE-motion_tracking-g1_29dof_anneal_23dof/exported/model_176500.onnx", + +} + + + +start_upper_body_dof_pos: { + + "APT_level1": + [0.19964170455932617, 0.07710712403059006, -0.2882401943206787, + 0.21672365069389343, 0.15629297494888306, -0.5167576670646667, 0.5782126784324646, + 0.0, 0.0, 0.0, + 0.25740593671798706, -0.2504104673862457, 0.22500675916671753, 0.5127624273300171, + 0.0, 0.0, 0.0], + +} + +motion_length_s: { + "APT_level1": 7.66, + +} diff --git a/env.py b/env.py index 6c64b9e4094c2defdd74c12398363065dbfe4d7f..7218b739c2dc41fb64fe7a431399814e0868d21c 100644 --- a/env.py +++ b/env.py @@ -1,135 +1,59 @@ from pathlib import Path +import subprocess import sys import gymnasium as gym from gymnasium import spaces import numpy as np from huggingface_hub import snapshot_download -import yaml +import os +import signal snapshot_download("lerobot/unitree-g1-mujoco") -# Ensure sim module is importable -sys.path.insert(0, str(Path(__file__).parent)) - -from sim.simulator_factory import SimulatorFactory, init_channel - def make_env(n_envs=1, use_async_envs=False, **kwargs): - """Create a UnitreeG1 simulation environment. - - Args: - n_envs: Number of environments (currently only 1 supported) - use_async_envs: Whether to use async envs (not implemented) - **kwargs: Additional arguments passed to simulator - - publish_images: bool, whether to publish camera images via ZMQ - - camera_port: int, ZMQ port for camera images - - cameras: list of camera names - """ + + # define run_sim FIRST repo_dir = Path(__file__).parent - - # Load config - config_path = repo_dir / "config.yaml" - with open(config_path) as f: - config = yaml.safe_load(f) - - # Configure cameras if requested - publish_images = kwargs.get("publish_images", True) - camera_port = kwargs.get("camera_port", 5555) - cameras = kwargs.get("cameras", ["head_camera"]) - - enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False) - camera_configs = {} - if enable_offscreen: - for cam_name in cameras: - camera_configs[cam_name] = {"height": 480, "width": 640} - - # Initialize DDS channel - init_channel(config=config) - - # Create simulator (runs in same process) - simulator = SimulatorFactory.create_simulator( - config=config, - env_name="default", - onscreen=config.get("ENABLE_ONSCREEN", True), - offscreen=enable_offscreen, - camera_configs=camera_configs, - ) - - class UnitreeG1Env(gym.Env): - """Gymnasium environment wrapper for Unitree G1 MuJoCo simulation.""" - - metadata = {"render_modes": ["human"]} + run_sim = repo_dir / "run_sim.py" + + print("=" * 60) + print("launching run_sim.py as subprocess") + print("path:", run_sim) + print("=" * 60) + + # now you can launch it + proc = subprocess.Popen([sys.executable, str(run_sim)]) + print(f"simulator started as pid={proc.pid}") - def __init__(self, sim, cam_configs, pub_images, cam_port): + class DummyEnv(gym.Env): + metadata = {"render_modes": []} + + def __init__(self, process): super().__init__() - self.simulator = sim - self.sim_env = sim.sim_env - self.step_count = 0 - self.camera_configs = cam_configs - - # Get timing from config - self.sim_dt = config["SIMULATE_DT"] - self.viewer_dt = config.get("VIEWER_DT", 0.02) - self.image_dt = config.get("IMAGE_DT", 0.033333) - - # Start image publishing subprocess if requested - if pub_images and len(cam_configs) > 0: - sim.start_image_publish_subprocess( - start_method=config.get("MP_START_METHOD", "spawn"), - camera_port=cam_port - ) - print(f"Camera images publishing on tcp://localhost:{cam_port}") - - # Define spaces - num_joints = config.get("NUM_MOTORS", 29) - self.action_space = spaces.Box(-np.pi, np.pi, shape=(num_joints,), dtype=np.float32) - self.observation_space = spaces.Box(-np.inf, np.inf, shape=(num_joints * 3 + 10,), dtype=np.float32) + self.process = process + self.action_space = spaces.Box(-1, 1, shape=(1,), dtype=np.float32) + self.observation_space = spaces.Box(-1, 1, shape=(1,), dtype=np.float32) def reset(self, seed=None, options=None): - """Reset the simulation.""" super().reset(seed=seed, options=options) - self.simulator.reset() - self.step_count = 0 - obs = self._get_obs() - return obs, {} - - def step(self, action=None): - """Execute one simulation step. Caller handles timing.""" - # Run physics step - self.sim_env.sim_step() - - # Update viewer at viewer rate - if self.step_count % int(self.viewer_dt / self.sim_dt) == 0: - self.sim_env.update_viewer() - - # Update render caches at image rate (for ZMQ publishing) - if self.step_count % int(self.image_dt / self.sim_dt) == 0: - self.sim_env.update_render_caches() - - self.step_count += 1 - - obs = self._get_obs() - reward = 0.0 - terminated = False - truncated = False + obs = np.zeros(1, dtype=np.float32) info = {} - return obs, reward, terminated, truncated, info - - def _get_obs(self): - """Get current observation from simulation.""" - obs_dict = self.sim_env.prepare_obs() - obs = np.concatenate([ - obs_dict.get("body_q", np.zeros(29)), - obs_dict.get("body_dq", np.zeros(29)), - obs_dict.get("body_tau_est", np.zeros(29)), - obs_dict.get("floating_base_pose", np.zeros(7))[:4], - obs_dict.get("floating_base_vel", np.zeros(6))[:3], - obs_dict.get("floating_base_acc", np.zeros(3)), - ]).astype(np.float32) - return obs + return obs, info - def close(self): - """Close the simulation.""" - print("++++closing simulator ++++") - self.simulator.close() + def step(self, action): + return np.zeros(1, dtype=np.float32), 0.0, False, False, {} - return UnitreeG1Env(simulator, camera_configs, publish_images, camera_port) + def close(self): + pass + + def kill_sim(self): + if self.process.poll() is None: + print("killing simulator subprocess...") + self.process.terminate() + try: + self.process.wait(timeout=2) + except subprocess.TimeoutExpired: + print("force killing simulator subprocess...") + self.process.kill() + + return DummyEnv(proc) diff --git a/run_sim.py b/run_sim.py index 3ac58b7acd98f3aaf8ccc4004ca607bc1d00c0a6..c8af3088ba7fe0d05dad1d091610d16d0f0ae7d3 100644 --- a/run_sim.py +++ b/run_sim.py @@ -1,7 +1,8 @@ #!/usr/bin/env python3 +"""Standalone MuJoCo simulator for Unitree G1""" import sys from pathlib import Path -import time + # Add sim module to path sys.path.insert(0, str(Path(__file__).parent)) @@ -9,7 +10,11 @@ import yaml from sim.simulator_factory import SimulatorFactory, init_channel def main(n_envs=1, use_async_envs: bool = False, - publish_images=True, camera_port=5555, cameras=None, **kwargs): + publish_images=True, camera_port=5554, cameras=None, **kwargs): + # Use default values + publish_images = True + camera_port = 5554 + cameras = None # Load config config_path = Path(__file__).parent / "config.yaml" @@ -18,7 +23,14 @@ def main(n_envs=1, use_async_envs: bool = False, # Override config with default values enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False) - + + print("="*60) + print("🤖 Starting Unitree G1 MuJoCo Simulator") + print("="*60) + print(f"📁 Scene: {config['ROBOT_SCENE']}") + print(f"⏱️ Timestep: {config['SIMULATE_DT']}s ({int(1/config['SIMULATE_DT'])} Hz)") + print(f"👁️ Visualization: {'ON' if config.get('ENABLE_ONSCREEN', True) else 'OFF'}") + # Configure cameras if requested camera_configs = {} if enable_offscreen: @@ -41,24 +53,19 @@ def main(n_envs=1, use_async_envs: bool = False, camera_configs=camera_configs, ) - # Start simulator + # Start simulator (blocking) print("\nSimulator running. Press Ctrl+C to exit.") if enable_offscreen and publish_images: print(f"Camera images publishing on tcp://localhost:{camera_port}") try: - if publish_images: - sim.start_image_publish_subprocess( - start_method="spawn", - camera_port=camera_port, - ) - time.sleep(1) - sim.start() + SimulatorFactory.start_simulator( + sim, + as_thread=False, + enable_image_publish=publish_images, + camera_port=camera_port, + ) except KeyboardInterrupt: - print("+++++Simulator interrupted by user.") - except Exception as e: - print(f"++++error in simulator: {e} ++++") - finally: - print("++++closing simulator ++++") + print("\nShutting down simulator...") sim.close() if __name__ == "__main__": diff --git a/sim/base_sim.py b/sim/base_sim.py index 0353111ccc2c7e620d89a1b9f779b41bd099d815..8c4c348bca35de6843cfaa758d69ead07f1c7332 100644 --- a/sim/base_sim.py +++ b/sim/base_sim.py @@ -2,7 +2,7 @@ import argparse import pathlib from pathlib import Path import threading -from threading import Thread +from threading import Lock, Thread from typing import Dict import mujoco @@ -18,8 +18,8 @@ from unitree_sdk2py.core.channel import ChannelFactoryInitialize import yaml import os from .image_publish_utils import ImagePublishProcess -from .metric_utils import check_contact -from .sim_utils import get_subtree_body_names +from .metric_utils import check_contact, check_height +from .sim_utilts import get_subtree_body_names from .unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge GR00T_WBC_ROOT = Path(__file__).resolve().parent.parent # Points to mujoco_sim_g1/ @@ -60,6 +60,8 @@ class DefaultEnv: if len(camera_configs) > 0: print(f"✓ DefaultEnv initialized with {len(camera_configs)} camera(s): {list(camera_configs.keys())}") + # Thread safety lock + self.reward_lock = Lock() # Unitree bridge will be initialized by the simulator self.unitree_bridge = None @@ -69,11 +71,12 @@ class DefaultEnv: # Initialize scene (defined in subclasses) self.init_scene() + self.last_reward = 0 - # Setup offscreen rendering if needed (lazy init - renderers created on first use) + # Setup offscreen rendering if needed self.offscreen = offscreen - self.renderers = {} # Will be lazily initialized - self._renderers_initialized = False + if self.offscreen: + self.init_renderers() self.image_dt = self.config.get("IMAGE_DT", 0.033333) # Image publishing subprocess (initialized separately) @@ -86,8 +89,6 @@ class DefaultEnv: str(assets_root / self.config["ROBOT_SCENE"]) ) self.mj_data = mujoco.MjData(self.mj_model) - # Set valid floating base quaternion (MjData initializes qpos to zeros) - self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.mj_model.opt.timestep = self.sim_dt self.torso_index = mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_BODY, "torso_link") self.root_body = "pelvis" @@ -438,6 +439,16 @@ class DefaultEnv: else: self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING + def update_reward(self): + """Calculate reward. Should be implemented by subclasses.""" + with self.reward_lock: + self.last_reward = 0 + + def get_reward(self): + """Thread-safe way to get the last calculated reward.""" + with self.reward_lock: + return self.last_reward + def set_unitree_bridge(self, unitree_bridge): """Set the unitree bridge from the simulator""" self.unitree_bridge = unitree_bridge @@ -448,17 +459,9 @@ class DefaultEnv: def update_render_caches(self): """Update render cache and shared memory for subprocess.""" - # Lazy init renderers on first call (creates OpenGL context in calling thread) - if not self._renderers_initialized and self.offscreen: - self.init_renderers() - self._renderers_initialized = True - print(f"✓ Renderers initialized lazily in thread {__import__('threading').current_thread().name}") - render_caches = {} for camera_name, camera_config in self.camera_configs.items(): - renderer = self.renderers.get(camera_name) - if renderer is None: - continue + renderer = self.renderers[camera_name] if "params" in camera_config: renderer.update_scene(self.mj_data, camera=camera_config["params"]) else: @@ -504,11 +507,114 @@ class DefaultEnv: def reset(self): mujoco.mj_resetData(self.mj_model, self.mj_data) - # Set valid floating base quaternion (identity: w=1, x=y=z=0) - # mj_resetData sets qpos to zeros, which gives invalid [0,0,0,0] quaternion - self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] - # Propagate qpos to derived quantities (xquat, xpos, etc.) - mujoco.mj_forward(self.mj_model, self.mj_data) + + +class CubeEnv(DefaultEnv): + """Environment with a cube object for pick and place tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_cube_43dof.xml" + super().__init__(config, "cube", {}, onscreen, offscreen) + + def update_reward(self): + """Calculate reward based on gripper contact with cube and cube height""" + right_hand_body = [ + "right_hand_thumb_2_link", + "right_hand_middle_1_link", + "right_hand_index_1_link", + ] + gripper_cube_contact = check_contact( + self.mj_model, self.mj_data, right_hand_body, "cube_body" + ) + cube_lifted = check_height(self.mj_model, self.mj_data, "cube", 0.85, 2.0) + + with self.reward_lock: + self.last_reward = gripper_cube_contact & cube_lifted + + +class BoxEnv(DefaultEnv): + """Environment with a box object for manipulation tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/lift_box_43dof.xml" + super().__init__(config, "box", {}, onscreen, offscreen) + + def reward(self): + """Calculate reward based on gripper contact with cube and cube height""" + left_hand_body = [ + "left_hand_thumb_2_link", + "left_hand_middle_1_link", + "left_hand_index_1_link", + ] + right_hand_body = [ + "right_hand_thumb_2_link", + "right_hand_middle_1_link", + "right_hand_index_1_link", + ] + gripper_box_contact = check_contact(self.mj_model, self.mj_data, left_hand_body, "box_body") + gripper_box_contact &= check_contact( + self.mj_model, self.mj_data, right_hand_body, "box_body" + ) + box_lifted = check_height(self.mj_model, self.mj_data, "box", 0.92, 2.0) + + print("gripper_box_contact: ", gripper_box_contact, "box_lifted: ", box_lifted) + + with self.reward_lock: + self.last_reward = gripper_box_contact & box_lifted + return self.last_reward + + +class BottleEnv(DefaultEnv): + """Environment with a cylinder object for manipulation tasks""" + + def __init__( + self, + config: Dict[str, any], + onscreen: bool = False, + offscreen: bool = False, + ): + # Override the robot scene + config = config.copy() # Create a copy to avoid modifying the original + config["ROBOT_SCENE"] = "gr00t_wbc/control/robot_model/model_data/g1/pnp_bottle_43dof.xml" + camera_configs = { + "egoview": { + "height": 400, + "width": 400, + }, + } + super().__init__( + config, "cylinder", camera_configs, onscreen, offscreen + ) + + self.bottle_body = self.mj_model.body("bottle_body") + self.bottle_geom = self.mj_model.geom("bottle") + + if self.viewer is not None: + self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED + self.viewer.cam.fixedcamid = self.mj_model.camera("egoview").id + + def update_reward(self): + """Calculate reward based on gripper contact with cylinder and cylinder height""" + pass + + def get_privileged_obs(self): + obs_pos = self.mj_data.xpos[self.bottle_body.id] + obs_quat = self.mj_data.xquat[self.bottle_body.id] + return {"bottle_pos": obs_pos, "bottle_quat": obs_quat} class BaseSimulator: @@ -535,11 +641,21 @@ class BaseSimulator: # Set update frequencies self.sim_dt = self.config["SIMULATE_DT"] + self.reward_dt = self.config.get("REWARD_DT", 0.02) self.image_dt = self.config.get("IMAGE_DT", 0.033333) self.viewer_dt = self.config.get("VIEWER_DT", 0.02) - # Create the environment - self.sim_env = DefaultEnv(config, env_name, **kwargs) + # Create the appropriate environment based on name + if env_name == "default": + self.sim_env = DefaultEnv(config, env_name, **kwargs) + elif env_name == "pnp_cube": + self.sim_env = CubeEnv(config, **kwargs) + elif env_name == "lift_box": + self.sim_env = BoxEnv(config, **kwargs) + elif env_name == "pnp_bottle": + self.sim_env = BottleEnv(config, **kwargs) + else: + raise ValueError(f"Invalid environment name: {env_name}") # Initialize the DDS communication layer - should be safe to call multiple times @@ -606,6 +722,10 @@ class BaseSimulator: if sim_cnt % int(self.viewer_dt / self.sim_dt) == 0: self.sim_env.update_viewer() + # Calculate reward at reward rate + if sim_cnt % int(self.reward_dt / self.sim_dt) == 0: + self.sim_env.update_reward() + # Update render caches at image rate if sim_cnt % int(self.image_dt / self.sim_dt) == 0: self.sim_env.update_render_caches() @@ -641,11 +761,6 @@ class BaseSimulator: def close(self): """Close the simulation. Can be overridden by subclasses.""" try: - # Stop image publishing subprocess - if hasattr(self.sim_env, "image_publish_process") and self.sim_env.image_publish_process is not None: - self.sim_env.image_publish_process.stop() - self.sim_env.image_publish_process = None - # Close viewer if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None: self.sim_env.viewer.close() diff --git a/sim/image_publish_utils.py b/sim/image_publish_utils.py index ab8ab19f748d7f7c74e138ef30f27cda6180cfa1..c9dd78953e6f5c0b5b357bd8bd3d2fb71643c29c 100644 --- a/sim/image_publish_utils.py +++ b/sim/image_publish_utils.py @@ -5,6 +5,8 @@ from typing import Any, Dict import numpy as np +from .sensor_utils import ImageMessageSchema, ImageUtils, SensorServer + def get_multiprocessing_info(verbose: bool = True): """Get information about multiprocessing start methods""" diff --git a/sim/metric_utils.py b/sim/metric_utils.py index 32c05365a445c1e7aaacba15779cf8f69567956d..369b8797ca185d8ba4cf87fc770bd034a56a5df3 100644 --- a/sim/metric_utils.py +++ b/sim/metric_utils.py @@ -2,7 +2,7 @@ from typing import List, Tuple import mujoco -from .sim_utils import get_body_geom_ids +from .sim_utilts import get_body_geom_ids def check_contact( diff --git a/sim/sensor_utils.py b/sim/sensor_utils.py index 160874da60c27a2185d739f4cb8726be4741b034..7b205c2e6c68e22d0468c13aba107a0b6e4cff10 100644 --- a/sim/sensor_utils.py +++ b/sim/sensor_utils.py @@ -1,10 +1,11 @@ """Standalone sensor utilities for camera image publishing via ZMQ""" import base64 -import json from dataclasses import dataclass from typing import Any, Dict import cv2 +import msgpack +import msgpack_numpy as m import numpy as np import zmq @@ -65,8 +66,8 @@ class SensorServer: def send_message(self, data: Dict[str, Any]): try: - json_str = json.dumps(data) - self.socket.send_string(json_str, flags=zmq.NOBLOCK) + packed = msgpack.packb(data, use_bin_type=True) + self.socket.send(packed, flags=zmq.NOBLOCK) except zmq.Again: self.message_dropped += 1 print(f"[Warning] message dropped: {self.message_dropped}") @@ -94,8 +95,8 @@ class SensorClient: self.context.term() def receive_message(self): - json_str = self.socket.recv_string() - return json.loads(json_str) + packed = self.socket.recv() + return msgpack.unpackb(packed, object_hook=m.decode) class ImageUtils: diff --git a/sim/sim_utils.py b/sim/sim_utilts.py similarity index 99% rename from sim/sim_utils.py rename to sim/sim_utilts.py index c50a6d73a04e20f5f1bd5c49bb35449926673735..e032eb6d801469cb543dd10fa6cef11a90f643a8 100644 --- a/sim/sim_utils.py +++ b/sim/sim_utilts.py @@ -75,7 +75,7 @@ def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]: def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]: - """Get all geoms belonging to subtree starting at a given body + """Get all geoms belonging to subtree starting at a given body. Here, a subtree is defined as the kinematic tree starting at the body and including all its descendants. diff --git a/sim/simulator_factory.py b/sim/simulator_factory.py index 7457f39e2ce5aceabb5bf4b32d794b790cfb8edb..3dca58dc250a1520c5b2bad0f0bbb375ae3aa01e 100644 --- a/sim/simulator_factory.py +++ b/sim/simulator_factory.py @@ -35,6 +35,8 @@ class SimulatorFactory: simulator_type = config.get("SIMULATOR", "mujoco") if simulator_type == "mujoco": return SimulatorFactory._create_mujoco_simulator(config, env_name, **kwargs) + elif simulator_type == "robocasa": + return SimulatorFactory._create_robocasa_simulator(config, env_name, **kwargs) else: print( f"Warning: Invalid simulator type: {simulator_type}. " @@ -56,3 +58,91 @@ class SimulatorFactory: ) return BaseSimulator(config=config, env_name=env_name, **env_kwargs) + @staticmethod + def _create_robocasa_simulator(config: Dict[str, Any], env_name: str = "default", **kwargs): + """Create a RoboCasa simulator instance.""" + from gr00t_wbc.control.envs.g1.sim.robocasa_sim import RoboCasaG1EnvServer + from gr00t_wbc.control.envs.robocasa.utils.controller_utils import ( + update_robosuite_controller_configs, + ) + from gr00t_wbc.control.envs.robocasa.utils.sim_utils import change_simulation_timestep + + change_simulation_timestep(config["SIMULATE_DT"]) + + # Use default environment if not specified + if env_name == "default": + env_name = "GroundOnly" + + # Get or create controller configurations + controller_configs = kwargs.get("controller_configs") + if controller_configs is None: + wbc_version = kwargs.get("wbc_version", "gear_wbc") + controller_configs = update_robosuite_controller_configs("G1", wbc_version) + + # Build environment kwargs + env_kwargs = dict( + onscreen=kwargs.pop("onscreen", True), + offscreen=kwargs.pop("offscreen", False), + camera_names=kwargs.pop("camera_names", None), + camera_heights=kwargs.pop("camera_heights", None), + camera_widths=kwargs.pop("camera_widths", None), + control_freq=kwargs.pop("control_freq", 50), + controller_configs=controller_configs, + ik_indicator=kwargs.pop("ik_indicator", False), + randomize_cameras=kwargs.pop("randomize_cameras", True), + ) + + kwargs.update( + { + "verbose": config.pop("verbose", False), + "sim_freq": 1 / config.pop("SIMULATE_DT"), + } + ) + + return RoboCasaG1EnvServer( + env_name=env_name, + wbc_config=config, + env_kwargs=env_kwargs, + **kwargs, + ) + + @staticmethod + def start_simulator( + simulator, + as_thread: bool = True, + enable_image_publish: bool = False, + mp_start_method: str = "spawn", + camera_port: int = 5555, + ): + """ + Start the simulator either as a thread or as a separate process. + + Args: + simulator: The simulator instance to start + config: Configuration dictionary + as_thread: If True, start as thread; if False, start as subprocess + enable_offscreen: If True and not as_thread, start image publishing + """ + + if as_thread: + simulator.start_as_thread() + else: + # Wrap in try-except to make sure simulator is properly closed upon exit. + try: + if enable_image_publish: + simulator.start_image_publish_subprocess( + start_method=mp_start_method, + camera_port=camera_port, + ) + time.sleep(1) + simulator.start() + except KeyboardInterrupt: + print("+++++Simulator interrupted by user.") + except Exception as e: + print(f"++++error in simulator: {e} ++++") + finally: + print("++++closing simulator ++++") + simulator.close() + + # Allow simulator to initialize + time.sleep(1) diff --git a/sim/unitree_sdk2py_bridge.py b/sim/unitree_sdk2py_bridge.py index 62e301e101e9bb9202481a6dde2020f9f64f0bc1..6cc596611a7e00ce526179da48584c8ee618f368 100644 --- a/sim/unitree_sdk2py_bridge.py +++ b/sim/unitree_sdk2py_bridge.py @@ -16,18 +16,27 @@ from unitree_sdk2py.idl.default import ( unitree_hg_msg_dds__HandState_ as HandState_default, ) from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_ -from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ +try: + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_, OdoState_ + HAS_ODOSTATE = True +except ImportError: + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ + OdoState_ = None + HAS_ODOSTATE = False + print("Warning: OdoState_ not available in unitree_sdk2py") class UnitreeSdk2Bridge: """ - Bridge between Unitree SDK2 and the MuJoCo simulation. - Handles sending and receiving DDS messages for body and hand control. + This class is responsible for bridging the Unitree SDK2 with the Gr00t environment. + It is responsible for sending and receiving messages to and from the Unitree SDK2. + Both the body and hand are supported. """ def __init__(self, config): # Note that we do not give the mjdata and mjmodel to the UnitreeSdk2Bridge. # It is unsafe and would be unflexible if we use a hand-plugged robot model + robot_type = config["ROBOT_TYPE"] if "g1" in robot_type or "h1-2" in robot_type: from unitree_sdk2py.idl.default import ( @@ -35,6 +44,10 @@ class UnitreeSdk2Bridge: unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__LowState_ as LowState_default, ) + try: + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__OdoState_ as OdoState_default + except ImportError: + OdoState_default = None from unitree_sdk2py.idl.unitree_hg.msg.dds_ import IMUState_, LowCmd_, LowState_ self.low_cmd = unitree_hg_msg_dds__LowCmd_() @@ -53,14 +66,33 @@ class UnitreeSdk2Bridge: self.num_body_motor = config["NUM_MOTORS"] self.num_hand_motor = config.get("NUM_HAND_MOTORS", 0) self.use_sensor = config["USE_SENSOR"] + self.have_imu_ = False self.have_frame_sensor_ = False + # if self.use_sensor: + # MOTOR_SENSOR_NUM = 3 + # self.dim_motor_sensor = MOTOR_SENSOR_NUM * self.num_motor + # # Check sensor + # for i in range(self.dim_motor_sensor, self.mj_model.nsensor): + # name = mujoco.mj_id2name(self.mj_model, mujoco._enums.mjtObj.mjOBJ_SENSOR, i) + # if name == "imu_quat": + # self.have_imu_ = True + # if name == "frame_pos": + # self.have_frame_sensor_ = True # Unitree sdk2 message self.low_state = LowState_default() self.low_state_puber = ChannelPublisher("rt/lowstate", LowState_) self.low_state_puber.Init() + # Only create odo_state for supported robot types (if available) + if ("g1" in robot_type or "h1-2" in robot_type) and HAS_ODOSTATE and OdoState_default: + self.odo_state = OdoState_default() + self.odo_state_puber = ChannelPublisher("rt/odostate", OdoState_) + self.odo_state_puber.Init() + else: + self.odo_state = None + self.odo_state_puber = None self.torso_imu_state = IMUState_default() self.torso_imu_puber = ChannelPublisher("rt/secondary_imu", IMUState_) self.torso_imu_puber.Init() @@ -68,7 +100,6 @@ class UnitreeSdk2Bridge: self.left_hand_state = HandState_default() self.left_hand_state_puber = ChannelPublisher("rt/dex3/left/state", HandState_) self.left_hand_state_puber.Init() - self.right_hand_state = HandState_default() self.right_hand_state_puber = ChannelPublisher("rt/dex3/right/state", HandState_) self.right_hand_state_puber.Init() @@ -79,7 +110,6 @@ class UnitreeSdk2Bridge: self.left_hand_cmd = HandCmd_default() self.left_hand_cmd_suber = ChannelSubscriber("rt/dex3/left/cmd", HandCmd_) self.left_hand_cmd_suber.Init(self.LeftHandCmdHandler, 1) - self.right_hand_cmd = HandCmd_default() self.right_hand_cmd_suber = ChannelSubscriber("rt/dex3/right/cmd", HandCmd_) self.right_hand_cmd_suber.Init(self.RightHandCmdHandler, 1) @@ -117,8 +147,29 @@ class UnitreeSdk2Bridge: # Store config for initialization self.config = config - + self.reset() + + # Initialize motors with default KP/KD from config to make robot stiff at startup + self._initialize_motor_defaults() + + def _initialize_motor_defaults(self): + """Initialize motor commands with default KP/KD and joint positions""" + if "MOTOR_KP" in self.config and "MOTOR_KD" in self.config: + motor_kp = self.config["MOTOR_KP"] + motor_kd = self.config["MOTOR_KD"] + default_dof_angles = self.config.get("DEFAULT_DOF_ANGLES", [0.0] * self.num_body_motor) + + for i in range(min(self.num_body_motor, len(motor_kp))): + self.low_cmd.motor_cmd[i].kp = motor_kp[i] + self.low_cmd.motor_cmd[i].kd = motor_kd[i] + self.low_cmd.motor_cmd[i].q = default_dof_angles[i] if i < len(default_dof_angles) else 0.0 + self.low_cmd.motor_cmd[i].dq = 0.0 + self.low_cmd.motor_cmd[i].tau = 0.0 + + print(f"✓ Initialized {self.num_body_motor} motors with default KP/KD gains") + else: + print("⚠ Warning: MOTOR_KP/MOTOR_KD not found in config, robot will be limp at startup") def reset(self): with self.low_cmd_lock: @@ -173,6 +224,12 @@ class UnitreeSdk2Bridge: raise NotImplementedError("Frame sensor data is not implemented yet.") else: # Get data from ground truth + # Publish odo state only if available + if self.odo_state is not None: + self.odo_state.position[:] = obs["floating_base_pose"][:3] + self.odo_state.linear_velocity[:] = obs["floating_base_vel"][:3] + self.odo_state.orientation[:] = obs["floating_base_pose"][3:7] + self.odo_state.angular_velocity[:] = obs["floating_base_vel"][3:6] # quaternion: w, x, y, z self.low_state.imu_state.quaternion[:] = obs["floating_base_pose"][3:7] # angular velocity @@ -187,26 +244,13 @@ class UnitreeSdk2Bridge: if self.have_frame_sensor_: raise NotImplementedError("Frame sensor data is not implemented yet.") self.low_state.tick = int(obs["time"] * 1e3) - - # Copy wireless controller data into lowstate for controllers that read from rt/lowstate - if self.joystick is not None: - import struct - # Pack wireless controller data in the format expected by unitree_g1.py RemoteController.set() - # bytes 0-1: padding, bytes 2-3: keys, bytes 4-7: lx, bytes 8-11: rx, - # bytes 12-15: ry, bytes 16-19: padding, bytes 20-23: ly - keys = int(self.wireless_controller.keys) - lx = float(self.wireless_controller.lx) - ly = float(self.wireless_controller.ly) - rx = float(self.wireless_controller.rx) - ry = float(self.wireless_controller.ry) - wireless_data = struct.pack('>", "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): diff --git a/view_cameras_live.py b/view_cameras_live.py index 7045dcb1d9264facc245ff24d3cef1a02d9f466d..bacb637ab2543281052266081c5d6c55db08b6db 100644 --- a/view_cameras_live.py +++ b/view_cameras_live.py @@ -202,7 +202,7 @@ def main(): parser = argparse.ArgumentParser(description="Live camera viewer for MuJoCo simulator") parser.add_argument("--host", type=str, default="localhost", help="Simulator host address (default: localhost)") - parser.add_argument("--port", type=int, default=5554, + parser.add_argument("--port", type=int, default=5555, help="ZMQ port (default: 5555)") parser.add_argument("--interval", type=int, default=33, help="Update interval in ms (default: 33 = ~30fps)")