diff --git a/G1_INSPIRE_TELEOP.md b/G1_INSPIRE_TELEOP.md new file mode 100644 index 00000000..299ca0ae --- /dev/null +++ b/G1_INSPIRE_TELEOP.md @@ -0,0 +1,247 @@ +# G1 + Inspire Hand MuJoCo Teleop Bridge + +This document summarizes the work done to enable **Quest 3 → xr_teleoperate → MuJoCo** teleoperation for a **G1 29-DoF upper body with Inspire DFX hands**, as a Sim2Sim alternative to Isaac Lab. + +--- + +## Goal + +Replace the Isaac Sim backend with MuJoCo while keeping **xr_teleoperate unchanged**. Both sides communicate over the same Unitree DDS topics used on the real robot and in Isaac simulation. + +``` +Quest 3 → xr_teleoperate → DDS → MuJoCo (g1_inspire_sim) +``` + +Scope: **upper body only** (arms + waist + Inspire hands). Pelvis is fixed to the world so the robot does not fall without a walking policy. + +--- + +## What Was Built + +### Phase 0 — MuJoCo model (G1 + Inspire, pelvis fixed) + +- Converted Inspire hand URDFs from `xr_teleoperate/assets/inspire_hand/` into MuJoCo body snippets. +- Merged them onto `g1_29dof.xml` at both wrist yaw links using the official mount transform from `h1_2.urdf`: + - Left: `pos="0.054 0 0"`, `quat="0.707 0 0 0.707"` + - Right: `pos="0.054 0 0"`, `quat="0 0.707 -0.707 0"` +- Removed the pelvis free joint so the robot stands still. +- Added 12 hand position actuators in **Inspire DDS motor order** (ids 0–5 right, 6–11 left). +- Added equality constraints for URDF mimic joints (intermediate/distal fingers follow proximal joints). +- Replaced default rubber-hand geoms with Inspire hand meshes. + +**Output files:** + +| File | Description | +|------|-------------| +| `unitree_robots/g1/g1_29dof_inspire_fixed.xml` | Full robot model (41 actuators: 29 body + 12 hand) | +| `unitree_robots/g1/scene_29dof_inspire_fixed.xml` | Scene wrapper (floor, lighting, skybox) | +| `unitree_robots/g1/inspire_build/convert_hands.py` | URDF → MJCF conversion script | +| `unitree_robots/g1/inspire_build/merge_g1_inspire.py` | Model merge script (re-runnable) | + +### Phase 1 — Body DDS bridge (arms + waist + legs hold) + +- Subscribes to `rt/lowcmd` (`unitree_hg` LowCmd_, 29 motors). +- Applies PD control: `tau = tau_ff + kp*(q_target - q) + kd*(dq_target - dq)`. +- Publishes `rt/lowstate` with joint positions, velocities, torques, and IMU data. +- Joints not yet commanded are held at their current pose with default hold gains (`kp=60`, `kd=1.5`) so legs do not swing freely. + +### Phase 2 — Inspire hand DDS bridge + +- Subscribes to `rt/inspire/cmd` (`unitree_go` MotorCmds_, 12 motors, normalized 0–1). +- Publishes `rt/inspire/state` continuously (**required** — `Inspire_Controller_DFX` in xr_teleoperate blocks until it receives hand state). +- Maps normalized commands to joint angles using the same ranges as Isaac `inspire_dds.py` and xr_teleoperate. + +**Inspire DDS motor order (12 channels):** + +| ID | Joint | Range (rad) | +|----|-------|-------------| +| 0 | R_pinky_proximal | 0.0 – 1.7 | +| 1 | R_ring_proximal | 0.0 – 1.7 | +| 2 | R_middle_proximal | 0.0 – 1.7 | +| 3 | R_index_proximal | 0.0 – 1.7 | +| 4 | R_thumb_proximal_pitch | 0.0 – 0.5 | +| 5 | R_thumb_proximal_yaw | -0.1 – 1.3 | +| 6 | L_pinky_proximal | 0.0 – 1.7 | +| 7 | L_ring_proximal | 0.0 – 1.7 | +| 8 | L_middle_proximal | 0.0 – 1.7 | +| 9 | L_index_proximal | 0.0 – 1.7 | +| 10 | L_thumb_proximal_pitch | 0.0 – 0.5 | +| 11 | L_thumb_proximal_yaw | -0.1 – 1.3 | + +Normalization: `q_norm = (max - q) / (max - min)` where **1.0 = fully open**, **0.0 = fully closed**. + +### Phase 3 — Integration entry point + +- `g1_inspire_sim.py` runs the MuJoCo viewer + physics loop + DDS bridge on **domain 1** (same as `xr_teleoperate --sim`). +- No changes to xr_teleoperate are required; only the simulator backend is swapped. + +--- + +## New Files (simulate_python/) + +| File | Role | +|------|------| +| `g1_inspire_bridge.py` | DDS ↔ MuJoCo bridge (body + Inspire hands) | +| `g1_inspire_sim.py` | Main sim launcher (viewer + physics + bridge) | +| `test/test_g1_inspire_teleop.py` | Standalone test script (fake teleop without Quest) | + +--- + +## DDS Topics + +| Topic | Direction | Message type | Purpose | +|-------|-----------|--------------|---------| +| `rt/lowcmd` | teleop → sim | `unitree_hg` LowCmd_ | Body joint targets (29 motors) | +| `rt/lowstate` | sim → teleop | `unitree_hg` LowState_ | Body joint feedback + IMU | +| `rt/inspire/cmd` | teleop → sim | `unitree_go` MotorCmds_ | Hand targets (12 motors, 0–1) | +| `rt/inspire/state` | sim → teleop | `unitree_go` MotorStates_ | Hand feedback (12 motors, 0–1) | + +DDS domain: **1** for simulation (matches `ChannelFactoryInitialize(1)` in xr_teleoperate when `--sim` is set). + +--- + +## How to Run + +### Prerequisites + +- Conda env `unitree` with `mujoco` and `unitree_sdk2py` installed. +- Conda env `tv` with xr_teleoperate dependencies installed. +- Quest 3 and host PC on the same Wi-Fi (for Phase 3). + +### Terminal 1 — MuJoCo sim + +```bash +conda activate unitree +cd ~/Documents/fibo/project_humanoid/unitree_mujoco/simulate_python +python g1_inspire_sim.py +``` + +Options: + +- `--headless` — run without MuJoCo viewer (for automated testing). +- `--interface lo` — DDS network interface (default: `lo`; use your LAN interface if teleop runs on another machine). + +### Terminal 2 — xr_teleoperate + +```bash +conda activate tv +cd ~/Documents/fibo/project_humanoid/xr_teleoperate/teleop +python teleop_hand_and_arm.py --arm=G1_29 --ee=inspire_dfx --sim --body-tracking upper +``` + +### Quest 3 + +1. Open `https://:8012/?ws=wss://:8012` in the headset browser. +2. Click **Virtual Reality** and accept certificate prompts. +3. Align your arms to the robot's initial pose (arms at sides). +4. Press **`r`** in the teleop terminal to start tracking. + +### Standalone test (no Quest) + +With `g1_inspire_sim.py --headless` running: + +```bash +conda activate unitree +cd ~/Documents/fibo/project_humanoid/unitree_mujoco/simulate_python +python test/test_g1_inspire_teleop.py +``` + +--- + +## Test Results (automated) + +| Test | Result | +|------|--------| +| Model loads (`scene_29dof_inspire_fixed.xml`) | Pass — 41 actuators, 53 joints, 12 equality constraints | +| `rt/lowstate` published | Pass | +| `rt/inspire/state` published | Pass — initial values ~1.0 (hands open) | +| Arm tracking (shoulder/elbow/wrist/waist) | Pass — targets reached within ~5% at teleop-like gains | +| Hand open/close (cmd 1.0 / 0.0 / 0.5) | Pass — all 12 channels track normalized commands | +| Mimic joints (finger intermediate/distal) | Pass — coupled via equality constraints | + +--- + +## Architecture Diagram + +``` +┌─────────────┐ WebXR/Vuer ┌──────────────────┐ +│ Quest 3 │ ◄──────────────────►│ xr_teleoperate │ +└─────────────┘ │ (teleop_hand_ │ + │ and_arm.py) │ + └────────┬─────────┘ + │ CycloneDDS (domain 1) + ┌──────────────┼──────────────┐ + │ │ │ + rt/lowcmd rt/inspire/cmd (no image + │ │ server in + ▼ ▼ MuJoCo MVP) + ┌─────────────────────────────┐ + │ g1_inspire_sim.py │ + │ ┌───────────────────────┐ │ + │ │ g1_inspire_bridge.py │ │ + │ │ - LowCmdHandler │ │ + │ │ - InspireCmdHandler │ │ + │ │ - PublishLowState │ │ + │ │ - PublishInspireState│ │ + │ └───────────┬───────────┘ │ + │ │ │ + │ ┌───────────▼───────────┐ │ + │ │ MuJoCo physics step │ │ + │ │ g1_29dof_inspire_ │ │ + │ │ fixed.xml (41 ctrl) │ │ + │ └───────────────────────┘ │ + └─────────────────────────────┘ + │ │ + rt/lowstate rt/inspire/state + │ │ + └──────┬───────┘ + ▼ + xr_teleoperate + (feedback / state) +``` + +--- + +## Known Limitations + +1. **No camera feed** — Unlike Isaac Lab, this MuJoCo setup does not stream a robot POV image to Quest. Teleop still works for arm/hand control; immersive view requires a separate image pipeline (e.g. teleimager) if needed later. +2. **No `rt/sim_state`** — Recording with `--record` will have empty `sim_state` fields. Teleop and hand/arm control work without it. +3. **Fixed pelvis** — Robot cannot walk or fall. Leg joints are held in place. Suitable for upper-body teleop dev only. +4. **Hand mount transform** — Uses H1_2 URDF mount offsets. Fine-tune in `inspire_build/merge_g1_inspire.py` if finger orientation looks wrong in VR. +5. **Shoulder tracking under gravity** — At teleop gains (`kp=80`), shoulder pitch may not reach large forward targets fully due to arm weight in simulation. This matches expected sim2real behavior; increase gains or add feedforward if needed. + +--- + +## Rebuilding the Model + +If you change hand meshes, mount pose, or joint limits: + +```bash +conda activate unitree +cd ~/Documents/fibo/project_humanoid/unitree_mujoco/unitree_robots/g1/inspire_build +python convert_hands.py # URDF → MJCF snippets +python merge_g1_inspire.py # merge into g1_29dof_inspire_fixed.xml + scene +``` + +Inspire hand STL meshes are copied into `unitree_robots/g1/meshes/` from `xr_teleoperate/assets/inspire_hand/meshes/`. + +--- + +## Relation to Other Repos + +| Repo | Role in this stack | +|------|-------------------| +| `xr_teleoperate` | Quest 3 input, IK, hand retargeting, DDS command publisher — **unchanged** | +| `unitree_mujoco` | MuJoCo sim + DDS bridge — **this work** | +| `unitree_sim_isaaclab` | Original Isaac Sim backend (replaced for this dev path) | +| `deploy/` | Locomotion RL policy deploy — **separate concern**, not used for teleop | + +--- + +## Next Steps (optional) + +- [ ] Add MuJoCo offscreen camera → teleimager for Quest POV in sim +- [ ] Publish stub `rt/sim_state` for `--record` compatibility +- [ ] Sim2Real: drop `--sim`, set DDS domain 0 + robot network interface +- [ ] Tune hand mount transform after first Quest session +- [ ] Add `--no_g1_state_pub`-style DDS isolation if multicast issues appear (see `unitree_sim_isaaclab/CODEX_CONTEXT.md`) diff --git a/PROGRESS.md b/PROGRESS.md new file mode 100644 index 00000000..eb4f7ff8 --- /dev/null +++ b/PROGRESS.md @@ -0,0 +1,94 @@ +# Progress — G1 + Inspire Teleop in MuJoCo (Quest 3) + +Last updated: 2026-07-20 + +## Done + +### 1. Robot model (Phase 0) +- Converted Inspire hand URDFs to MJCF (`unitree_robots/g1/inspire_build/convert_hands.py`). +- Merged both hands into the G1 29-DoF model (`inspire_build/merge_g1_inspire.py`): + - Hands attached to `left/right_wrist_yaw_link`, original rubber hands removed. + - Pelvis fixed (freejoint removed) — robot stands still, upper-body teleop only. + - URDF `mimic` joints converted to MuJoCo `` constraints. + - 12 position actuators added for the hands (6 per hand). +- Output: `unitree_robots/g1/g1_29dof_inspire_fixed.xml` + `scene_29dof_inspire_fixed.xml`. + +### 2. DDS bridge (Phases 1–2) +- `simulate_python/g1_inspire_bridge.py`: + - Body: subscribes `rt/lowcmd`, publishes `rt/lowstate` (joints + IMU), PD control in MuJoCo. + - Hands: subscribes `rt/inspire/cmd`, publishes `rt/inspire/state`, with the same + normalization ranges as `unitree_sim_isaaclab` so `xr_teleoperate` works unmodified. + - Default PD gains hold the legs/waist so the robot stands stably with no commands. +- `simulate_python/g1_inspire_sim.py`: sim launcher, DDS domain 1 (= `xr_teleoperate --sim`), + physics + viewer threads, `--headless` option. + +### 3. Automated control test (Phase 3) +- `simulate_python/test/test_g1_inspire_teleop.py`: fake teleoperator that sends sinusoidal + arm commands + hand open/close over DDS and verifies `rt/lowstate` / `rt/inspire/state` + feedback. **Passing**. + +### 4. Camera feed to Quest 3 (Phase 4) — done, verified end-to-end +Same pipeline as Isaac Sim: sim renders → shared memory → teleimager image server → +`xr_teleoperate` / Quest (ZMQ :55555 / WebRTC :60001, config served on :60000). + +- Stereo head cameras added to the model (`head_left_eye` / `head_right_eye`): + IPD 64 mm, mounted on the head front, pitched 25° down, fovy 70°, + 1280x720 offscreen framebuffer (`inspire_build/merge_g1_inspire.py`). +- `g1_inspire_sim.py` got a `CameraThread`: renders both eyes at 480x640 @ 30 FPS via EGL + (`MUJOCO_GL=egl` set automatically) and writes them to shared memory + (`isaac_left/right_image_shm`) in the exact Isaac format. `--no-camera` disables it. +- `simulate_python/tools/shared_memory_utils.py`: writer/reader copied from + `unitree_sim_isaaclab` (self-contained, no cross-repo import). +- `simulate_python/run_image_server.py` + `cam_config_mujoco.yaml`: launches teleimager's + IsaacSim-mode image server (runs in the `tv` env). Head camera binocular 480x1280, + wrist cameras disabled. +- Environment fixes: + - `xr_teleoperate/teleop/teleimager/.../image_server.py`: `logging_mp.basicConfig` → + `basic_config` (import crashed in the `tv` env). + - Installed `aiortc` 1.15.0 into the `tv` env (WebRTC support; was missing). + - Generated a self-signed TLS cert at `~/.config/xr_teleoperate/{cert,key}.pem` + (used by both televuer and teleimager WebRTC). + +**End-to-end test passed:** headless sim + image server running together — +`ImageClient` received the camera config on :60000 and a live 480x1280 binocular frame +over ZMQ (first-person view, both Inspire hands visible). The DDS control test still +passes while the camera streams (no interference between the two paths). + +## How to run the full Quest 3 session + +```bash +# Terminal 1 — MuJoCo sim (unitree env) +cd unitree_mujoco/simulate_python +python g1_inspire_sim.py + +# Terminal 2 — image server (tv env) +cd unitree_mujoco/simulate_python +conda activate tv +python run_image_server.py + +# Terminal 3 — teleop (tv env) +cd xr_teleoperate/teleop +conda activate tv +python teleop_hand_and_arm.py --arm=G1_29 --ee=inspire_dfx --sim \ + --img-server-ip 127.0.0.1 --image-transport zmq +``` + +Then on the Quest 3 browser open `https://:8012?ws=wss://:8012`, +accept the self-signed cert, enter VR, and press **r** in Terminal 3 to start teleop. +(`--image-transport zmq` is the simplest; for WebRTC, first visit +`https://:60001` once on the Quest to accept the cert, then use +`--image-transport webrtc`.) + +### 5. XR tracking-loss safety fallback (2026-07-21) +- `xr_teleoperate/teleop/teleop_hand_and_arm.py`: detects tracking loss (raw Quest poses + frozen longer than `--tracking-timeout`, default 0.5 s, or invalid/singular) and stops + feeding XR data to the IK — no more arm spin when the headset is taken off. +- Fallback selectable with `--tracking-fallback`: `hold` (default, freeze at last pose) + or `home` (ramp arms back to the default pose at ≤0.5 rad/s). +- Hand/gripper targets also freeze during loss; on recovery the arm velocity limit + ramps up gradually to avoid a jump. + +## Known limitations +- Robot base is fixed (no locomotion) — intended for upper-body + hands teleop. +- Scene is an empty floor; no table/objects yet. +- Head cameras are fixed to the torso (no head-yaw follow). diff --git a/SIM_USAGE_GUIDE.md b/SIM_USAGE_GUIDE.md new file mode 100644 index 00000000..8e52e845 --- /dev/null +++ b/SIM_USAGE_GUIDE.md @@ -0,0 +1,315 @@ +# Simulator Usage Guide + +How to run MuJoCo simulation in this repo, configure parameters, and switch between **sim → sim** and **sim → real**. + +Controllers talk the same Unitree DDS topics as the physical robot. Switching backends is mostly **domain ID + network interface**. + +``` +Controller (sdk2 / sdk2py / ros2 / xr_teleoperate) + │ rt/lowcmd + ▼ + DDS bridge (this repo) + │ PD: tau = tau_ff + kp*(q* - q) + kd*(dq* - dq) + ▼ + MuJoCo physics + │ + ▼ + rt/lowstate (+ sportmodestate, wireless, inspire/state) +``` + +--- + +## 1. Quick start — run the sim + +### Option A: C++ simulator (recommended) + +```bash +cd simulate +mkdir -p build && cd build +cmake .. && make -j4 + +# Use simulate/config.yaml +./unitree_mujoco + +# Or override robot / scene / DDS +./unitree_mujoco -r go2 -s scene_terrain.xml +./unitree_mujoco -r g1 -s scene_29dof.xml +./unitree_mujoco -r g1 -s scene_29dof.xml -i 1 -n lo +``` + +CLI flags: `-r` robot, `-s` scene, `-i` domain id, `-n` interface. + +### Option B: Python simulator + +Edit `simulate_python/config.py`, then: + +```bash +cd simulate_python +python3 unitree_mujoco.py +``` + +In another terminal, smoke-test DDS: + +```bash +python3 test/test_unitree_sdk2.py # Go2 / unitree_go +python3 test/test_unitree_sdk2_g1.py # G1 / unitree_hg +``` + +### Option C: G1 + Inspire teleop sim (sim2sim with Quest) + +```bash +cd simulate_python +python g1_inspire_sim.py # viewer + DDS domain 1 +# python g1_inspire_sim.py --headless # no viewer +# python g1_inspire_sim.py --no-camera # skip head cameras +``` + +See [G1_INSPIRE_TELEOP.md](./G1_INSPIRE_TELEOP.md) for the full Quest / xr_teleoperate stack. + +### Humanoid tip (elastic band) + +For H1/G1 standing init, enable the virtual hoist (`enable_elastic_band`), then in the viewer: + +| Key | Action | +|-----|--------| +| **9** | Toggle elastic band on/off | +| **7** | Lower robot | +| **8** | Lift robot | + +Lower onto the feet before releasing the band. + +--- + +## 2. Parameters + +### Sim vs real (the important pair) + +| Mode | Domain ID | Interface | +|------|-----------|-----------| +| **Simulation** | `1` | `"lo"` (loopback) | +| **Real robot** | `0` | robot Ethernet NIC (e.g. `enp3s0`) | + +Keep controller logic identical; only change how you initialize the DDS channel factory. + +### C++ — `simulate/config.yaml` + +| Parameter | Example | Meaning | +|-----------|---------|---------| +| `robot` | `"g1"` | Folder under `unitree_robots/` | +| `robot_scene` | `"scene_29dof.xml"` | Scene XML in that folder | +| `domain_id` | `1` | DDS domain (`1` sim, `0` real) | +| `interface` | `"lo"` | DDS network interface | +| `use_joystick` | `0` / `1` | Publish gamepad as `rt/wirelesscontroller` | +| `joystick_type` | `"xbox"` / `"switch"` | Button/axis layout | +| `joystick_device` | `"/dev/input/js0"` | Joystick device path | +| `joystick_bits` | `16` | Axis resolution | +| `print_scene_information` | `1` | Dump links / joints / sensors at start | +| `enable_elastic_band` | `1` | Virtual hoist for humanoids | + +Supported robots include: `go2`, `go2w`, `b2`, `b2w`, `a2`, `h1`, `h1_2`, `h2`, `g1`, `r1`. + +### Python — `simulate_python/config.py` + +| Parameter | Example | Meaning | +|-----------|---------|---------| +| `ROBOT` | `"go2"` | Robot name | +| `ROBOT_SCENE` | `../unitree_robots/{ROBOT}/scene.xml` | Full path to scene | +| `DOMAIN_ID` | `1` | DDS domain | +| `INTERFACE` | `"lo"` | DDS interface | +| `USE_JOYSTICK` | `1` | Gamepad → wireless controller | +| `JOYSTICK_TYPE` | `"xbox"` | Layout | +| `JOYSTICK_DEVICE` | `0` | pygame joystick index | +| `PRINT_SCENE_INFORMATION` | `True` | Scene dump | +| `ENABLE_ELASTIC_BAND` | `False` | Virtual hoist | +| `SIMULATE_DT` | `0.005` | Physics timestep (s); must exceed `viewer.sync()` cost | +| `VIEWER_DT` | `0.02` | Viewer refresh (~50 Hz) | + +**Note:** C++ `config.yaml` may be set to G1 while Python `config.py` defaults to Go2 — set the robot explicitly before you run. + +### G1 + Inspire teleop (`g1_inspire_sim.py` / `g1_inspire_bridge.py`) + +| Parameter | Typical | Meaning | +|-----------|---------|---------| +| Domain | `1` | Matches `xr_teleoperate --sim` | +| `SIMULATE_DT` | `0.002` | Physics dt | +| Hold gains | `kp=60`, `kd=1.5` | Hold uncommanded joints until first `lowcmd` | +| Inspire cmds | normalized `0–1` | `1.0` open, `0.0` closed | + +Camera stream (optional): `simulate_python/cam_config_mujoco.yaml`. + +### Control message types + +| Robots | IDL | +|--------|-----| +| Go2, B2, H1, Go2w, B2w, … | `unitree_go` | +| G1, H1-2 | `unitree_hg` | + +Low-level topics used here: + +- `rt/lowcmd` / `rt/lowstate` +- `rt/sportmodestate` (pose/vel kept in sim even when real robot hides it) +- `rt/wirelesscontroller` +- `rt/secondary_imu` (G1, C++ bridge) +- G1+Inspire: `rt/inspire/cmd`, `rt/inspire/state` + +Motor index order matches hardware. For G1, see `unitree_robots/g1/g1_joint_index_dds.md`. + +### Terrain + +Parametric stairs / rough ground / height maps: see `terrain_tool/readme.md`. Output scenes (e.g. `scene_terrain.xml`) are loaded via `robot_scene` / `-s`. + +This repo does **not** implement runtime domain randomization or RL policy export. + +--- + +## 3. Sim → Real + +Same LowCmd / LowState API; only DDS transport changes. + +### Checklist + +1. Develop and verify the controller against MuJoCo with `domain_id=1`, `interface=lo`. +2. Confirm motor indices, PD gains, and command rate match hardware docs. +3. On the real robot: turn off conflicting onboard motion services if you take over low-level control. +4. Re-run the **same** controller with `domain_id=0` and the robot NIC name. + +### Python example (`example/python/stand_go2.py`) + +```bash +# Terminal 1 — start Go2 sim (config: robot=go2, domain 1, lo) +cd simulate/build && ./unitree_mujoco -r go2 -s scene.xml + +# Terminal 2 — controller → sim +cd example/python +python3 stand_go2.py + +# Same controller → real robot +python3 stand_go2.py enp3s0 # replace with your NIC +``` + +```python +if len(sys.argv) < 2: + ChannelFactoryInitialize(1, "lo") # sim +else: + ChannelFactoryInitialize(0, sys.argv[1]) # real +``` + +### C++ example (`example/cpp/stand_go2`) + +```bash +cd example/cpp && mkdir -p build && cd build && cmake .. && make -j4 +./stand_go2 # sim +./stand_go2 enp3s0 # real +``` + +### ROS2 example (`example/ros2`) + +```bash +# Sim +source ~/unitree_ros2/setup_local.sh +export ROS_DOMAIN_ID=1 +./install/stand_go2/bin/stand_go2 + +# Real +source ~/unitree_ros2/setup.sh +export ROS_DOMAIN_ID=0 +./install/stand_go2/bin/stand_go2 +``` + +### G1 + Inspire teleop → real + +Planned path (controller stays the same): + +1. Drop `--sim` on `xr_teleoperate`. +2. Use DDS domain `0` + robot network interface. +3. Ensure Inspire hand DDS and body `unitree_hg` topics match the physical stack. + +Locomotion RL deploy lives in a separate `deploy/` repo, not here. + +--- + +## 4. Sim → Sim + +Use this MuJoCo backend as a drop-in for another simulator (e.g. Isaac Lab) when the controller already speaks Unitree DDS. + +### Pattern + +| Target | Domain | Interface | Notes | +|--------|--------|-----------|-------| +| This MuJoCo sim | `1` | `lo` (or LAN NIC if teleop is remote) | Start `unitree_mujoco` or `g1_inspire_sim.py` | +| Isaac / other Unitree sim | usually `1` + `--sim` | as that stack documents | Same topics | +| Real robot | `0` | robot NIC | See section 3 | + +Any controller that publishes `rt/lowcmd` (and optionally inspire / wireless topics) can target MuJoCo without code changes beyond channel init. + +### G1 + Inspire: Isaac Lab → MuJoCo + +Documented end-to-end in [G1_INSPIRE_TELEOP.md](./G1_INSPIRE_TELEOP.md): + +``` +Quest 3 → xr_teleoperate (--sim) → DDS domain 1 → MuJoCo (g1_inspire_sim) +``` + +Compatibility choices: + +- Same DDS topics/types as Isaac and the real robot +- Same Inspire 0–1 normalization as Isaac `inspire_dds.py` +- Fixed pelvis (upper body + hands only; no walking policy required) + +```bash +# Terminal 1 — MuJoCo +conda activate unitree +cd simulate_python +python g1_inspire_sim.py + +# Terminal 2 — teleop (external xr_teleoperate repo) +conda activate tv +cd ../xr_teleoperate/teleop +python teleop_hand_and_arm.py --arm=G1_29 --ee=inspire_dfx --sim +``` + +Standalone fake teleop (no Quest): + +```bash +python g1_inspire_sim.py --headless # terminal 1 +python test/test_g1_inspire_teleop.py # terminal 2 +``` + +### Generic controller sim2sim + +1. Start MuJoCo with the matching robot/scene and domain `1`. +2. Point your existing Isaac/sdk2 controller at domain `1` + `lo`. +3. Verify joint order (`unitree_go` vs `unitree_hg`) and PD form. +4. Iterate gains/timing in MuJoCo before sim2real. + +There is no ONNX / policy-export package in this repo — transfer is at the **DDS + PD command** level. + +--- + +## 5. Key files + +| Path | Role | +|------|------| +| `simulate/config.yaml` | C++ sim parameters | +| `simulate_python/config.py` | Python sim parameters | +| `simulate/build/unitree_mujoco` | C++ entry binary | +| `simulate_python/unitree_mujoco.py` | Python entry | +| `simulate_python/g1_inspire_sim.py` | G1+Inspire teleop sim | +| `simulate_python/g1_inspire_bridge.py` | Body + hand DDS bridge | +| `example/python/stand_go2.py` | Sim2real pattern (Python) | +| `example/cpp/stand_go2.cpp` | Sim2real pattern (C++) | +| `example/ros2/` | Sim2real pattern (ROS2) | +| `unitree_robots/` | MJCF scenes per robot | +| `terrain_tool/` | Procedural terrain scenes | + +--- + +## 6. Related docs + +| Doc | Contents | +|-----|----------| +| [readme.md](./readme.md) | Install, overview, joystick, terrain, sim2real examples | +| [G1_INSPIRE_TELEOP.md](./G1_INSPIRE_TELEOP.md) | Quest → MuJoCo sim2sim teleop | +| [PROGRESS.md](./PROGRESS.md) | Session notes / camera + image server | +| [terrain_tool/readme.md](./terrain_tool/readme.md) | Terrain generator | +| [unitree_robots/g1/g1_joint_index_dds.md](./unitree_robots/g1/g1_joint_index_dds.md) | G1 joint DDS order | diff --git a/simulate/config.yaml b/simulate/config.yaml index ba106385..5962d69c 100644 --- a/simulate/config.yaml +++ b/simulate/config.yaml @@ -1,7 +1,7 @@ -robot: "go2" # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1", "h2", "as2" -robot_scene: "scene.xml" # Robot scene, /unitree_robots/[robot]/scene.xml +robot: "g1" # Robot name, "go2", "b2", "b2w", "h1", "go2w", "g1", "h2" +robot_scene: "scene_29dof.xml" # Robot scene, /unitree_robots/[robot]/scene.xml -domain_id: 1 # Domain id +domain_id: 0 # Domain id interface: "lo" # Interface # DDS IDL type: -1 auto, 0 unitree_go, 1 unitree_hg. Auto selects unitree_hg for as2. idl_type: -1 @@ -13,4 +13,4 @@ joystick_bits: 16 # Some game controllers may only have 8-bit accuracy print_scene_information: 1 # Print link, joint and sensors information of robot -enable_elastic_band: 0 # Virtual spring band, used for lifting h1 +enable_elastic_band: 1 # Virtual spring band, used for lifting h1/g1 diff --git a/simulate_python/cam_config_mujoco.yaml b/simulate_python/cam_config_mujoco.yaml new file mode 100644 index 00000000..e64bec38 --- /dev/null +++ b/simulate_python/cam_config_mujoco.yaml @@ -0,0 +1,53 @@ +# Camera config for the MuJoCo G1+Inspire sim (served to xr_teleoperate on port 60000). +# The head camera is a virtual stereo pair rendered by g1_inspire_sim.py into +# shared memory; teleimager streams it exactly like the Isaac Sim pipeline. + +webrtc: + bitrate: + min: 2000000 + default: 5000000 + max: 12000000 + gop_length: 60 + +head_camera: + enable_zmq: true + zmq_port: 55555 + enable_webrtc: true + webrtc_port: 60001 + webrtc_codec: h264 + type: isaacsim # read from shared memory (written by the MuJoCo sim) + image_shape: [480, 1280] # binocular: left eye 480x640 + right eye 480x640 + binocular: true + fps: 30 + video_id: null + serial_number: null + physical_path: null + +# No wrist cameras in the MuJoCo scene (keys must exist for xr_teleoperate). +left_wrist_camera: + enable_zmq: false + zmq_port: 55556 + enable_webrtc: false + webrtc_port: 60002 + webrtc_codec: h264 + type: isaacsim + image_shape: [480, 640] + binocular: false + fps: 30 + video_id: null + serial_number: null + physical_path: null + +right_wrist_camera: + enable_zmq: false + zmq_port: 55557 + enable_webrtc: false + webrtc_port: 60003 + webrtc_codec: h264 + type: isaacsim + image_shape: [480, 640] + binocular: false + fps: 30 + video_id: null + serial_number: null + physical_path: null diff --git a/simulate_python/g1_inspire_bridge.py b/simulate_python/g1_inspire_bridge.py new file mode 100644 index 00000000..a1d2cc95 --- /dev/null +++ b/simulate_python/g1_inspire_bridge.py @@ -0,0 +1,202 @@ +"""DDS <-> MuJoCo bridge for G1 29-DoF body + Inspire hands. + +Topics (matching xr_teleoperate and the real robot): +- subscribe rt/lowcmd (unitree_hg LowCmd_) : body joint targets (PD) +- publish rt/lowstate (unitree_hg LowState_) : body joint states + IMU +- subscribe rt/inspire/cmd (unitree_go MotorCmds_): 12 normalized hand targets +- publish rt/inspire/state (unitree_go MotorStates_): 12 normalized states + +Assumptions about the MuJoCo model (scene_29dof_inspire_fixed.xml): +- actuators 0..28 : body torque motors in Unitree G1 DDS joint order +- actuators 29..40 : hand position actuators in Inspire DDS id order + (0-5 right pinky/ring/middle/index/thumb-bend/thumb-yaw, 6-11 left) +- jointpos/jointvel/jointactuatorfrc + IMU sensors exist for the body joints +""" + +import threading + +import mujoco +import numpy as np + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from unitree_sdk2py.utils.thread import RecurrentThread +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as HGLowState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ +from unitree_sdk2py.idl.default import ( + unitree_go_msg_dds__MotorCmd_, + unitree_go_msg_dds__MotorState_, +) +from unitree_sdk2py.utils.crc import CRC + +TOPIC_LOWCMD = "rt/lowcmd" +TOPIC_LOWSTATE = "rt/lowstate" +TOPIC_INSPIRE_CMD = "rt/inspire/cmd" +TOPIC_INSPIRE_STATE = "rt/inspire/state" + +NUM_BODY_MOTOR = 29 +NUM_HAND_MOTOR = 12 + +# normalization ranges per Inspire DDS id (q_norm = (max - q) / (max - min)) +INSPIRE_RANGES = np.array( + [[0.0, 1.7]] * 4 + [[0.0, 0.5], [-0.1, 1.3]] + + [[0.0, 1.7]] * 4 + [[0.0, 0.5], [-0.1, 1.3]] +) + +# hold gains used for joints until a lowcmd arrives (keeps legs from dangling) +HOLD_KP = 60.0 +HOLD_KD = 1.5 + + +class G1InspireBridge: + def __init__(self, mj_model, mj_data, data_lock): + self.mj_model = mj_model + self.mj_data = mj_data + self.data_lock = data_lock + self.lock = threading.Lock() + self.crc = CRC() + + assert mj_model.nu == NUM_BODY_MOTOR + NUM_HAND_MOTOR, ( + f"expected {NUM_BODY_MOTOR + NUM_HAND_MOTOR} actuators, got {mj_model.nu}" + ) + + # --- resolve body joint addresses from actuator order --- + self.body_qadr = np.zeros(NUM_BODY_MOTOR, dtype=int) + self.body_dqadr = np.zeros(NUM_BODY_MOTOR, dtype=int) + for i in range(NUM_BODY_MOTOR): + jid = mj_model.actuator_trnid[i, 0] + self.body_qadr[i] = mj_model.jnt_qposadr[jid] + self.body_dqadr[i] = mj_model.jnt_dofadr[jid] + + # --- hand joint addresses (actuators 29..40, inspire DDS order) --- + self.hand_qadr = np.zeros(NUM_HAND_MOTOR, dtype=int) + for i in range(NUM_HAND_MOTOR): + jid = mj_model.actuator_trnid[NUM_BODY_MOTOR + i, 0] + self.hand_qadr[i] = mj_model.jnt_qposadr[jid] + + # --- IMU sensor addresses (by name, robust to layout changes) --- + def sadr(name): + sid = mujoco.mj_name2id(mj_model, mujoco.mjtObj.mjOBJ_SENSOR, name) + return mj_model.sensor_adr[sid] if sid >= 0 else -1 + + self.imu_quat_adr = sadr("imu_quat") + self.imu_gyro_adr = sadr("imu_gyro") + self.imu_acc_adr = sadr("imu_acc") + + # --- body command state (hold current pose until lowcmd arrives) --- + self.cmd_q = self.mj_data.qpos[self.body_qadr].copy() + self.cmd_dq = np.zeros(NUM_BODY_MOTOR) + self.cmd_tau = np.zeros(NUM_BODY_MOTOR) + self.cmd_kp = np.full(NUM_BODY_MOTOR, HOLD_KP) + self.cmd_kd = np.full(NUM_BODY_MOTOR, HOLD_KD) + self.lowcmd_received = False + + # Hold the model's initial pose until the first complete DDS command. + self.hand_target = self.mj_data.qpos[self.hand_qadr].copy() + self.inspire_command_received = False + + # --- DDS pub/sub --- + self.low_state = unitree_hg_msg_dds__LowState_() + self.low_state_puber = ChannelPublisher(TOPIC_LOWSTATE, HGLowState_) + self.low_state_puber.Init() + + self.inspire_state = MotorStates_() + self.inspire_state.states = [ + unitree_go_msg_dds__MotorState_() for _ in range(NUM_HAND_MOTOR) + ] + self.inspire_state_puber = ChannelPublisher(TOPIC_INSPIRE_STATE, MotorStates_) + self.inspire_state_puber.Init() + + self.low_cmd_suber = ChannelSubscriber(TOPIC_LOWCMD, HGLowCmd_) + self.low_cmd_suber.Init(self.LowCmdHandler, 10) + + self.inspire_cmd_suber = ChannelSubscriber(TOPIC_INSPIRE_CMD, MotorCmds_) + self.inspire_cmd_suber.Init(self.InspireCmdHandler, 10) + + dt = mj_model.opt.timestep + self.low_state_thread = RecurrentThread( + interval=max(dt, 0.002), target=self.PublishLowState, name="sim_lowstate" + ) + self.low_state_thread.Start() + self.inspire_state_thread = RecurrentThread( + interval=0.01, target=self.PublishInspireState, name="sim_inspire_state" + ) + self.inspire_state_thread.Start() + + # ------------------------------------------------------------- subscribe + def LowCmdHandler(self, msg: HGLowCmd_): + with self.lock: + for i in range(NUM_BODY_MOTOR): + mc = msg.motor_cmd[i] + self.cmd_q[i] = mc.q + self.cmd_dq[i] = mc.dq + self.cmd_tau[i] = mc.tau + self.cmd_kp[i] = mc.kp + self.cmd_kd[i] = mc.kd + self.lowcmd_received = True + + def InspireCmdHandler(self, msg: MotorCmds_): + if len(msg.cmds) != NUM_HAND_MOTOR: + return + target = np.empty(NUM_HAND_MOTOR) + for i, (lo, hi) in enumerate(INSPIRE_RANGES): + q_norm = np.clip(msg.cmds[i].q, 0.0, 1.0) + # q_norm: 1.0 = fully open (q=lo), 0.0 = fully closed (q=hi) + target[i] = hi - q_norm * (hi - lo) + with self.lock: + self.hand_target[:] = target + self.inspire_command_received = True + + # ------------------------------------------------------------------ step + def update_ctrl(self): + """Call once per physics step (with the sim lock held by the caller).""" + d = self.mj_data + q = d.qpos[self.body_qadr] + dq = d.qvel[self.body_dqadr] + with self.lock: + tau = ( + self.cmd_tau + + self.cmd_kp * (self.cmd_q - q) + + self.cmd_kd * (self.cmd_dq - dq) + ) + d.ctrl[:NUM_BODY_MOTOR] = tau + d.ctrl[NUM_BODY_MOTOR:NUM_BODY_MOTOR + NUM_HAND_MOTOR] = self.hand_target + + # ------------------------------------------------------------- publish + def PublishLowState(self): + d = self.mj_data + with self.data_lock: + q = d.qpos[self.body_qadr].copy() + dq = d.qvel[self.body_dqadr].copy() + force = d.actuator_force[:NUM_BODY_MOTOR].copy() + sensors = d.sensordata.copy() + for i in range(NUM_BODY_MOTOR): + ms = self.low_state.motor_state[i] + ms.q = q[i] + ms.dq = dq[i] + ms.tau_est = force[i] + if self.imu_quat_adr >= 0: + for k in range(4): + self.low_state.imu_state.quaternion[k] = sensors[self.imu_quat_adr + k] + if self.imu_gyro_adr >= 0: + for k in range(3): + self.low_state.imu_state.gyroscope[k] = sensors[self.imu_gyro_adr + k] + if self.imu_acc_adr >= 0: + for k in range(3): + self.low_state.imu_state.accelerometer[k] = sensors[self.imu_acc_adr + k] + self.low_state.tick += 1 + self.low_state.crc = self.crc.Crc(self.low_state) + self.low_state_puber.Write(self.low_state) + + def PublishInspireState(self): + with self.data_lock: + q = self.mj_data.qpos[self.hand_qadr].copy() + for i in range(NUM_HAND_MOTOR): + lo, hi = INSPIRE_RANGES[i] + self.inspire_state.states[i].q = float(np.clip((hi - q[i]) / (hi - lo), 0.0, 1.0)) + self.inspire_state_puber.Write(self.inspire_state) + + def close(self): + self.low_state_thread.Wait(1.0) + self.inspire_state_thread.Wait(1.0) diff --git a/simulate_python/g1_inspire_sim.py b/simulate_python/g1_inspire_sim.py new file mode 100644 index 00000000..0298c823 --- /dev/null +++ b/simulate_python/g1_inspire_sim.py @@ -0,0 +1,151 @@ +"""G1 29-DoF + Inspire hands MuJoCo sim with a Unitree DDS bridge. + +Counterpart of unitree_sim_isaaclab for the xr_teleoperate stack: + + Terminal 1: python g1_inspire_sim.py + Terminal 2: python run_image_server.py (tv env, for Quest video) + Terminal 3: cd xr_teleoperate/teleop && \ + python teleop_hand_and_arm.py --arm=G1_29 --ee=inspire_dfx --sim + +Uses DDS domain 1 (simulation), same as xr_teleoperate --sim. +Head-camera frames (left/right eye) are written to shared memory in the same +format as unitree_sim_isaaclab, so teleimager's IsaacSim image server can +stream them to the Quest. +""" + +import argparse +import os +import threading +import time +from pathlib import Path +from threading import Thread + +# Offscreen head-camera rendering uses EGL so it does not fight over GLFW with +# the interactive viewer (which always uses GLFW, independent of MUJOCO_GL). +# Must be set before mujoco is imported. +os.environ.setdefault("MUJOCO_GL", "egl") + +import mujoco +import mujoco.viewer + +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from g1_inspire_bridge import G1InspireBridge + +SCENE = str(Path(__file__).resolve().parents[1] / "unitree_robots/g1/scene_29dof_inspire_fixed.xml") +DOMAIN_ID = 1 # 1 = simulation (matches xr_teleoperate --sim), 0 = real robot +SIMULATE_DT = 0.002 +VIEWER_DT = 0.02 +CAMERA_FPS = 30 +EYE_HEIGHT, EYE_WIDTH = 480, 640 # per eye; binocular head stream = 480x1280 + +locker = threading.Lock() + + +def CameraThread(mj_model, mj_data, is_running): + """Render the stereo head cameras and write frames to shared memory.""" + from tools.shared_memory_utils import MultiImageWriter + + try: + renderer = mujoco.Renderer(mj_model, height=EYE_HEIGHT, width=EYE_WIDTH) + except Exception as e: + print(f"[g1_inspire_sim] camera renderer failed ({e}); " + "continuing without camera (use --no-camera to silence)") + return + # MuJoCo renders RGB; writer converts RGB->BGR by default (skip_cvtcolor=False) + writer = MultiImageWriter() + period = 1.0 / CAMERA_FPS + print(f"[g1_inspire_sim] head cameras streaming to shared memory @ {CAMERA_FPS} FPS") + try: + while is_running(): + t0 = time.perf_counter() + images = {} + with locker: + for key, cam in (("left", "head_left_eye"), ("right", "head_right_eye")): + renderer.update_scene(mj_data, camera=cam) + images[key] = renderer.render() + writer.write_images(images) + remain = period - (time.perf_counter() - t0) + if remain > 0: + time.sleep(remain) + finally: + writer.close() + renderer.close() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--interface", type=str, default="lo", + help="network interface for DDS (default: lo)") + parser.add_argument("--scene", type=str, default=SCENE) + parser.add_argument("--headless", action="store_true", + help="run without the MuJoCo viewer (for testing)") + parser.add_argument("--no-camera", action="store_true", + help="disable head-camera rendering to shared memory") + args = parser.parse_args() + + mj_model = mujoco.MjModel.from_xml_path(args.scene) + mj_data = mujoco.MjData(mj_model) + mj_model.opt.timestep = SIMULATE_DT + stop_event = threading.Event() + + if args.headless: + class _FakeViewer: + def is_running(self): + return not stop_event.is_set() + + def sync(self): + pass + + viewer = _FakeViewer() + else: + viewer = mujoco.viewer.launch_passive(mj_model, mj_data) + + ChannelFactoryInitialize(DOMAIN_ID, args.interface) + bridge = G1InspireBridge(mj_model, mj_data, locker) + print(f"[g1_inspire_sim] DDS bridge up (domain {DOMAIN_ID}, interface {args.interface})") + print("[g1_inspire_sim] topics: rt/lowcmd rt/lowstate rt/inspire/cmd rt/inspire/state") + + def SimulationThread(): + while not stop_event.is_set() and viewer.is_running(): + step_start = time.perf_counter() + with locker: + bridge.update_ctrl() + mujoco.mj_step(mj_model, mj_data) + remain = mj_model.opt.timestep - (time.perf_counter() - step_start) + if remain > 0: + time.sleep(remain) + + def ViewerThread(): + while not stop_event.is_set() and viewer.is_running(): + with locker: + viewer.sync() + time.sleep(VIEWER_DT) + + sim_thread = Thread(target=SimulationThread) + viewer_thread = Thread(target=ViewerThread) + threads = [sim_thread, viewer_thread] + if not args.no_camera: + threads.append(Thread( + target=CameraThread, + args=(mj_model, mj_data, + lambda: not stop_event.is_set() and viewer.is_running()), + daemon=True, + )) + for t in threads: + t.start() + try: + sim_thread.join() + viewer_thread.join() + except KeyboardInterrupt: + pass + finally: + stop_event.set() + sim_thread.join(1.0) + viewer_thread.join(1.0) + bridge.close() + if not args.headless: + viewer.close() + + +if __name__ == "__main__": + main() diff --git a/simulate_python/run_image_server.py b/simulate_python/run_image_server.py new file mode 100644 index 00000000..fa419f55 --- /dev/null +++ b/simulate_python/run_image_server.py @@ -0,0 +1,44 @@ +"""Image server for the MuJoCo G1+Inspire sim -> Quest 3. + +Streams the head-camera frames that g1_inspire_sim.py writes to shared memory, +reusing teleimager's IsaacSim mode (ZMQ on 55555, WebRTC on 60001, camera +config served on 60000). + +Run in the *tv* conda env (where teleimager is installed): + + conda activate tv + python run_image_server.py + +Start g1_inspire_sim.py first so shared-memory frames exist. +""" + +import os +import signal +import sys + +import yaml + +# Make `tools.shared_memory_utils` importable for teleimager's IsaacSimCamera. +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from teleimager.image_server import ImageServer, signal_handler + +CONFIG = os.path.join(os.path.dirname(os.path.abspath(__file__)), "cam_config_mujoco.yaml") + + +def main(): + with open(CONFIG, "r") as f: + cam_config = yaml.safe_load(f) + + server = ImageServer(cam_config, realsense_enable=False, + camera_finder_verbose=False, isaacsim_enable=True) + signal.signal(signal.SIGINT, lambda s, f: signal_handler(server, s, f)) + signal.signal(signal.SIGTERM, lambda s, f: signal_handler(server, s, f)) + server.start() + print("[run_image_server] streaming MuJoCo head camera " + "(config :60000, zmq :55555, webrtc :60001). Ctrl+C to stop.") + server.wait() + + +if __name__ == "__main__": + main() diff --git a/simulate_python/test/g1_stand_hold.py b/simulate_python/test/g1_stand_hold.py new file mode 100644 index 00000000..e565748d --- /dev/null +++ b/simulate_python/test/g1_stand_hold.py @@ -0,0 +1,90 @@ +""" +Hold G1 in a stable standing pose (for unitree_mujoco simulation). + +Usage: + 1. Start the simulator (G1 loaded, elastic band ON). + 2. Run: python3 g1_stand_hold.py + 3. In the sim window: press 8 to lift, 7 to lower until feet touch ground, + then press 9 to release the band. The robot holds its pose. +""" +import time +import numpy as np + +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ +from unitree_sdk2py.utils.crc import CRC + +G1_NUM_MOTOR = 29 +CONTROL_DT = 0.002 +RAMP_TIME = 3.0 # seconds to move from current pose to stand pose + +Kp = [ + 100, 100, 100, 150, 40, 40, # left leg + 100, 100, 100, 150, 40, 40, # right leg + 100, 40, 40, # waist + 40, 40, 40, 40, 40, 40, 40, # left arm + 40, 40, 40, 40, 40, 40, 40, # right arm +] +Kd = [ + 2, 2, 2, 4, 2, 2, + 2, 2, 2, 4, 2, 2, + 2, 1, 1, + 1, 1, 1, 1, 1, 1, 1, + 1, 1, 1, 1, 1, 1, 1, +] + +# Slight crouch: hips/knees/ankles bent so the robot is statically stable +stand_pose = np.zeros(G1_NUM_MOTOR) +stand_pose[0] = -0.2 # LeftHipPitch +stand_pose[3] = 0.42 # LeftKnee +stand_pose[4] = -0.23 # LeftAnklePitch +stand_pose[6] = -0.2 # RightHipPitch +stand_pose[9] = 0.42 # RightKnee +stand_pose[10] = -0.23 # RightAnklePitch + +low_state = None + + +def LowStateHandler(msg: LowState_): + global low_state + low_state = msg + + +if __name__ == "__main__": + ChannelFactoryInitialize(1, "lo") + + suber = ChannelSubscriber("rt/lowstate", LowState_) + suber.Init(LowStateHandler, 10) + puber = ChannelPublisher("rt/lowcmd", LowCmd_) + puber.Init() + crc = CRC() + + print("Waiting for robot state...") + while low_state is None: + time.sleep(0.1) + print("Connected. Holding stand pose. Ctrl+C to stop.") + + start_pose = np.array([low_state.motor_state[i].q for i in range(G1_NUM_MOTOR)]) + cmd = unitree_hg_msg_dds__LowCmd_() + cmd.mode_pr = 0 # PR mode + cmd.mode_machine = low_state.mode_machine + + t = 0.0 + while True: + ratio = np.clip(t / RAMP_TIME, 0.0, 1.0) + target = (1.0 - ratio) * start_pose + ratio * stand_pose + + for i in range(G1_NUM_MOTOR): + cmd.motor_cmd[i].mode = 1 + cmd.motor_cmd[i].q = float(target[i]) + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].tau = 0.0 + cmd.motor_cmd[i].kp = Kp[i] + cmd.motor_cmd[i].kd = Kd[i] + + cmd.crc = crc.Crc(cmd) + puber.Write(cmd) + t += CONTROL_DT + time.sleep(CONTROL_DT) diff --git a/simulate_python/test/test_g1_inspire_model.py b/simulate_python/test/test_g1_inspire_model.py new file mode 100644 index 00000000..88c186ba --- /dev/null +++ b/simulate_python/test/test_g1_inspire_model.py @@ -0,0 +1,110 @@ +import unittest +from pathlib import Path +from xml.etree import ElementTree + +import mujoco +import numpy as np + + +ROOT = Path(__file__).resolve().parents[2] +MODEL = ROOT / "unitree_robots/g1/g1_29dof_inspire_fixed.xml" +SCENE = ROOT / "unitree_robots/g1/scene_29dof_inspire_fixed.xml" +ACTUATORS = [ + "R_pinky_proximal", "R_ring_proximal", "R_middle_proximal", + "R_index_proximal", "R_thumb_proximal_pitch", "R_thumb_proximal_yaw", + "L_pinky_proximal", "L_ring_proximal", "L_middle_proximal", + "L_index_proximal", "L_thumb_proximal_pitch", "L_thumb_proximal_yaw", +] +RANGES = np.array( + [[0.0, 1.7]] * 4 + [[0.0, 0.5], [-0.1, 1.3]] + + [[0.0, 1.7]] * 4 + [[0.0, 0.5], [-0.1, 1.3]] +) + + +def names(model, obj, count): + return [mujoco.mj_id2name(model, obj, i) for i in range(count)] + + +class G1InspireModelTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.model = mujoco.MjModel.from_xml_path(str(SCENE)) + cls.hand_actuators = np.arange(29, 41) + cls.hand_qadr = np.array([ + cls.model.jnt_qposadr[cls.model.actuator_trnid[i, 0]] + for i in cls.hand_actuators + ]) + + def test_structure_and_mapping(self): + mujoco.MjModel.from_xml_path(str(MODEL)) + model = self.model + joint_names = names(model, mujoco.mjtObj.mjOBJ_JOINT, model.njnt) + actuator_names = names(model, mujoco.mjtObj.mjOBJ_ACTUATOR, model.nu) + hand_joints = [n for n in joint_names if n.startswith(("L_", "R_"))] + + self.assertEqual((model.nq, model.nv, model.nu), (53, 53, 41)) + self.assertEqual(len(hand_joints), 24) + self.assertEqual(model.neq, 12) + self.assertEqual(actuator_names[29:41], ACTUATORS) + np.testing.assert_array_equal(model.actuator_ctrlrange[29:41], RANGES) + self.assertEqual(len(joint_names), len(set(joint_names))) + self.assertEqual(len(actuator_names), len(set(actuator_names))) + + for hand, wrist in (("L_hand_base_link", "left_wrist_yaw_link"), + ("R_hand_base_link", "right_wrist_yaw_link")): + bid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, hand) + self.assertEqual( + mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, + model.body_parentid[bid]), + wrist, + ) + + root = ElementTree.parse(MODEL).getroot() + meshdir = MODEL.parent / root.find("compiler").get("meshdir") + self.assertTrue(all((meshdir / mesh.get("file")).is_file() + for mesh in root.findall("./asset/mesh"))) + + def test_full_range_and_equalities(self): + model = self.model + data = mujoco.MjData(model) + + for target in (RANGES[:, 1], RANGES[:, 0]): + data.ctrl[self.hand_actuators] = target + for _ in range(3000): + mujoco.mj_step(model, data) + np.testing.assert_allclose(data.qpos[self.hand_qadr[[5, 11]]], + target[[5, 11]], atol=0.015) + + data.ctrl[self.hand_actuators] = RANGES[:, 1] - 0.5 * np.diff(RANGES, axis=1)[:, 0] + for _ in range(2000): + mujoco.mj_step(model, data) + for i in range(model.neq): + joint1, joint2 = model.eq_obj1id[i], model.eq_obj2id[i] + q1 = data.qpos[model.jnt_qposadr[joint1]] + q2 = data.qpos[model.jnt_qposadr[joint2]] + self.assertAlmostEqual(q1, model.eq_data[i, 1] * q2, delta=0.002) + + def test_finger_object_collision_remains_enabled(self): + model = self.model + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, + "L_index_intermediate") + marker = next(i for i in np.flatnonzero(model.geom_bodyid == body) + if model.geom_type[i] == mujoco.mjtGeom.mjGEOM_SPHERE) + + spec = mujoco.MjSpec.from_file(str(SCENE)) + spec.worldbody.add_geom(name="collision_probe", + type=mujoco.mjtGeom.mjGEOM_SPHERE, + size=[0.01, 0, 0], pos=data.geom_xpos[marker]) + probe_model = spec.compile() + probe_data = mujoco.MjData(probe_model) + mujoco.mj_forward(probe_model, probe_data) + probe = mujoco.mj_name2id(probe_model, mujoco.mjtObj.mjOBJ_GEOM, + "collision_probe") + self.assertTrue(any(probe in (c.geom1, c.geom2) + for c in probe_data.contact)) + + +if __name__ == "__main__": + unittest.main() diff --git a/simulate_python/test/test_g1_inspire_teleop.py b/simulate_python/test/test_g1_inspire_teleop.py new file mode 100644 index 00000000..26dfaab8 --- /dev/null +++ b/simulate_python/test/test_g1_inspire_teleop.py @@ -0,0 +1,129 @@ +"""DDS integration test for the G1 body and Inspire hands. + +Run g1_inspire_sim.py first, then run this in another terminal: + + python test/test_g1_inspire_teleop.py +""" + +import sys +import time + +import numpy as np + +from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, +) +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as HGLowState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ +from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ +from unitree_sdk2py.utils.crc import CRC + +DOMAIN_ID = 1 +KP, KD = 80.0, 3.0 + +# G1 29-DoF DDS indices +LEFT_SHOULDER_PITCH = 15 +state = {"lowstate": None, "inspire": None} + + +def wait_for(topic, timeout=5.0): + deadline = time.monotonic() + timeout + while state[topic] is None and time.monotonic() < deadline: + time.sleep(0.05) + if state[topic] is None: + raise AssertionError(f"no rt/{topic} received within {timeout}s") + + +def hand_state(): + msg = state["inspire"] + assert len(msg.states) == 12, f"expected 12 hand states, got {len(msg.states)}" + return np.array([motor.q for motor in msg.states]) + + +def main(): + ChannelFactoryInitialize(DOMAIN_ID, sys.argv[1] if len(sys.argv) > 1 else "lo") + + lowcmd_pub = ChannelPublisher("rt/lowcmd", HGLowCmd_) + lowcmd_pub.Init() + inspire_pub = ChannelPublisher("rt/inspire/cmd", MotorCmds_) + inspire_pub.Init() + + lowstate_sub = ChannelSubscriber("rt/lowstate", HGLowState_) + lowstate_sub.Init(lambda msg: state.update(lowstate=msg), 10) + inspire_sub = ChannelSubscriber("rt/inspire/state", MotorStates_) + inspire_sub.Init(lambda msg: state.update(inspire=msg), 10) + + crc = CRC() + cmd = unitree_hg_msg_dds__LowCmd_() + hand_cmd = MotorCmds_() + hand_cmd.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(12)] + + wait_for("lowstate") + wait_for("inspire") + ls = state["lowstate"] + assert len(ls.motor_state) >= 29, "lowstate is missing G1 body motors" + assert len(hand_cmd.cmds) == 12 + for i in range(29): + cmd.motor_cmd[i].q = ls.motor_state[i].q + cmd.motor_cmd[i].kp = KP + cmd.motor_cmd[i].kd = KD + + def drive(seconds, hand_q, shoulder_q=None): + values = np.broadcast_to(hand_q, (12,)) + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + if shoulder_q is not None: + cmd.motor_cmd[LEFT_SHOULDER_PITCH].q = shoulder_q + cmd.crc = crc.Crc(cmd) + lowcmd_pub.Write(cmd) + for i, value in enumerate(values): + hand_cmd.cmds[i].q = float(value) + inspire_pub.Write(hand_cmd) + time.sleep(0.01) + + # No hand command should change the model's startup pose. + startup = hand_state() + time.sleep(0.5) + np.testing.assert_allclose(hand_state(), startup, atol=0.02) + + drive(4.0, 0.0) + closed = hand_state() + assert np.max(closed) < 0.23, f"hands did not close: {closed}" + assert np.max(np.abs(closed - startup)) > 0.5, "hand command caused no motion" + + drive(5.0, 1.0) + opened = hand_state() + np.testing.assert_allclose(opened, 1.0, atol=0.13) + + drive(3.0, 0.5) + midpoint = hand_state() + np.testing.assert_allclose(midpoint, 0.5, atol=0.12) + + # DDS index 0 is right pinky; changing it must not move the other fingers. + individual = np.ones(12) + individual[0] = 0.0 + drive(4.0, individual) + fingers = hand_state() + assert fingers[0] < 0.1, f"right pinky did not close: {fingers[0]}" + np.testing.assert_array_less(0.85, fingers[[1, 2, 3, 6, 7, 8, 9]]) + + initial_shoulder = state["lowstate"].motor_state[LEFT_SHOULDER_PITCH].q + shoulder_target = initial_shoulder - 0.25 + drive(3.0, 1.0, shoulder_target) + final_shoulder = state["lowstate"].motor_state[LEFT_SHOULDER_PITCH].q + assert abs(final_shoulder - initial_shoulder) > 0.1, "arm command caused no motion" + assert abs(final_shoulder - shoulder_target) < 0.1, ( + f"shoulder target {shoulder_target:.3f}, got {final_shoulder:.3f}" + ) + + print("PASS: body DDS, 12-hand DDS, startup hold, range, midpoint, and index mapping") + print(f"PASS: left shoulder {initial_shoulder:.3f} -> {final_shoulder:.3f}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/simulate_python/test/test_unitree_sdk2_g1.py b/simulate_python/test/test_unitree_sdk2_g1.py new file mode 100644 index 00000000..76f31b74 --- /dev/null +++ b/simulate_python/test/test_unitree_sdk2_g1.py @@ -0,0 +1,48 @@ +import time +from unitree_sdk2py.core.channel import ChannelFactoryInitialize +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.idl.unitree_go.msg.dds_ import SportModeState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ +from unitree_sdk2py.utils.crc import CRC + +G1_NUM_MOTOR = 29 + + +def HighStateHandler(msg: SportModeState_): + print("Position: ", msg.position) + + +def LowStateHandler(msg: LowState_): + print("IMU state: ", msg.imu_state) + + +if __name__ == "__main__": + ChannelFactoryInitialize(1, "lo") + high_state_suber = ChannelSubscriber("rt/sportmodestate", SportModeState_) + low_state_suber = ChannelSubscriber("rt/lowstate", LowState_) + + high_state_suber.Init(HighStateHandler, 10) + low_state_suber.Init(LowStateHandler, 10) + + low_cmd_puber = ChannelPublisher("rt/lowcmd", LowCmd_) + low_cmd_puber.Init() + crc = CRC() + + cmd = unitree_hg_msg_dds__LowCmd_() + cmd.mode_pr = 0 # PR mode + cmd.mode_machine = 0 + + while True: + for i in range(G1_NUM_MOTOR): + cmd.motor_cmd[i].mode = 0x01 + cmd.motor_cmd[i].q = 0.0 + cmd.motor_cmd[i].kp = 0.0 + cmd.motor_cmd[i].dq = 0.0 + cmd.motor_cmd[i].kd = 0.0 + cmd.motor_cmd[i].tau = 1.0 + + cmd.crc = crc.Crc(cmd) + low_cmd_puber.Write(cmd) + time.sleep(0.002) diff --git a/simulate_python/tools/__init__.py b/simulate_python/tools/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/simulate_python/tools/shared_memory_utils.py b/simulate_python/tools/shared_memory_utils.py new file mode 100644 index 00000000..7b2feef0 --- /dev/null +++ b/simulate_python/tools/shared_memory_utils.py @@ -0,0 +1,428 @@ +# Copyright (c) 2025, Unitree Robotics Co., Ltd. All Rights Reserved. +# License: Apache License, Version 2.0 +""" +A simplified multi-image shared memory tool module +When writing, concatenate three images (head, left, right) horizontally and write them +When reading, split the concatenated image into three independent images +""" + +import ctypes +import time +import numpy as np +import cv2 +from multiprocessing import shared_memory +from typing import Optional, Dict, List +import struct +import os + +# shared memory configuration +# Use separate shared memory for each image +def get_shm_name(image_name: str) -> str: + """Get shared memory name for a specific image""" + return f"isaac_{image_name}_image_shm" + +SHM_SIZE_PER_IMAGE = 640 * 480 * 3 + 128 # ~1MB per image + header + buffer + +# Backward compatibility +SHM_NAME = "isaac_multi_image_shm" # Kept for backward compatibility +SHM_SIZE = SHM_SIZE_PER_IMAGE * 3 # Kept for backward compatibility + +# define the simplified header structure +class SimpleImageHeader(ctypes.LittleEndianStructure): # Use little-endian for cross-platform compatibility + """Simplified image header structure for individual images""" + _fields_ = [ + ('timestamp', ctypes.c_uint64), # timestamp + ('height', ctypes.c_uint32), # image height + ('width', ctypes.c_uint32), # image width + ('channels', ctypes.c_uint32), # number of channels + ('image_name', ctypes.c_char * 16), # image name (e.g., 'head', 'left', 'right') + ('data_size', ctypes.c_uint32), # data size + ('encoding', ctypes.c_uint32), # 0=raw BGR, 1=JPEG + ('quality', ctypes.c_uint32), # JPEG quality (valid if encoding=1) + ] + + +class MultiImageWriter: + """A simplified multi-image shared memory writer using separate SHM for each image""" + + def __init__(self, enable_jpeg: bool = False, jpeg_quality: int = 85, skip_cvtcolor: bool = False): + """Initialize the multi-image shared memory writer + + Args: + enable_jpeg: whether to enable JPEG compression + jpeg_quality: JPEG quality (0-100) + skip_cvtcolor: whether to skip color conversion + """ + # 50 FPS 限速（避免高频阻塞主循环） + self._min_interval_sec = 1.0 / 50.0 + self._last_write_ts_ms = 0 + + # 压缩与颜色空间配置（由主进程注入） + self._enable_jpeg = bool(enable_jpeg) + self._jpeg_quality = int(jpeg_quality) + self._skip_cvtcolor = bool(skip_cvtcolor) + + # 为每个图像维护独立的共享内存 + self.shms = {} # image_name -> SharedMemory + print(f"[MultiImageWriter] Initialized with separate SHM per image") + + def set_options(self, *, enable_jpeg: Optional[bool] = None, jpeg_quality: Optional[int] = None, skip_cvtcolor: Optional[bool] = None): + if enable_jpeg is not None: + self._enable_jpeg = bool(enable_jpeg) + if jpeg_quality is not None: + self._jpeg_quality = int(jpeg_quality) + if skip_cvtcolor is not None: + self._skip_cvtcolor = bool(skip_cvtcolor) + + def write_images(self, images: Dict[str, np.ndarray]) -> bool: + """Write multiple images to separate shared memories + + Args: + images: the image dictionary, the key is the image name ('head', 'left', 'right'), the value is the image array + + Returns: + bool: whether the writing is successful + """ + if not images: + return False + + # 轻量限速：最多 50 FPS，直接跳过多余写入，避免阻塞主循环 + now_ms = int(time.time() * 1000) + if self._last_write_ts_ms and (now_ms - self._last_write_ts_ms) < int(self._min_interval_sec * 1000): + return True + + success_count = 0 + + for image_name, image in images.items(): + try: + # 为每个图像获取独立的共享内存 + shm_name = get_shm_name(image_name) + if shm_name not in self.shms: + try: + # 尝试打开现有的共享内存 + self.shms[shm_name] = shared_memory.SharedMemory(name=shm_name) + except FileNotFoundError: + # 如果不存在，创建新的共享内存 + self.shms[shm_name] = shared_memory.SharedMemory(create=True, size=SHM_SIZE_PER_IMAGE, name=shm_name) + + shm = self.shms[shm_name] + + # 确保连续内存布局，尽量减少拷贝 + if not image.flags['C_CONTIGUOUS']: + image = np.ascontiguousarray(image) + # OpenCV 期望 BGR 格式；可通过配置跳过转换 + if image.ndim == 3 and image.shape[2] == 3: + if not self._skip_cvtcolor: + image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) + + # get the image information + height, width, channels = image.shape + + # 准备头部 + header = SimpleImageHeader() + header.timestamp = now_ms # millisecond timestamp + header.height = height + header.width = width + header.channels = channels + header.image_name = image_name.encode('utf-8')[:15].ljust(16, b'\x00') # truncate and pad to 16 bytes + + # 计算数据 + if self._enable_jpeg: + encode_params = [int(cv2.IMWRITE_JPEG_QUALITY), int(self._jpeg_quality)] + ok, buffer = cv2.imencode('.jpg', image, encode_params) + if not ok: + print(f"[MultiImageWriter] Failed to encode {image_name} as JPEG") + continue + data_bytes = buffer.tobytes() + header.encoding = 1 + header.quality = int(self._jpeg_quality) + else: + data_bytes = image.tobytes() + header.encoding = 0 + header.quality = 0 + + header.data_size = len(data_bytes) + + # 检查空间是否足够 + header_size = ctypes.sizeof(SimpleImageHeader) + total_size = header_size + header.data_size + if total_size > shm.size: + print(f"[MultiImageWriter] Not enough space for {image_name}: need {total_size}, available {shm.size}") + continue + + # 写入头部 + header_bytes = ctypes.string_at(ctypes.byref(header), header_size) + shm.buf[0:header_size] = header_bytes + + # 写入数据 + data_start = header_size + data_end = data_start + header.data_size + shm.buf[data_start:data_end] = data_bytes + + success_count += 1 + + except Exception as e: + print(f"[MultiImageWriter] Error writing {image_name}: {e}") + continue + + self._last_write_ts_ms = now_ms + return success_count > 0 + + def close(self): + """Close all shared memories""" + for shm_name, shm in self.shms.items(): + try: + shm.close() + print(f"[MultiImageWriter] Shared memory closed: {shm_name}") + except Exception as e: + print(f"[MultiImageWriter] Error closing {shm_name}: {e}") + self.shms.clear() + + +class MultiImageReader: + """A simplified multi-image shared memory reader using separate SHM per image""" + + def __init__(self): + """Initialize the multi-image shared memory reader""" + self.last_timestamps = {} # image_name -> last_timestamp + self.buffer = {} # image_name -> cached_image + self.shms = {} # image_name -> SharedMemory + + def read_images(self) -> Optional[Dict[str, np.ndarray]]: + """Read images from all available separate shared memories + + Returns: + Dict[str, np.ndarray]: the image dictionary, the key is the image name, the value is the image array + """ + images = {} + image_names = ['head', 'left', 'right'] # Standard image names + + for image_name in image_names: + try: + shm_name = get_shm_name(image_name) + + # Open shared memory if not already open + if shm_name not in self.shms: + try: + self.shms[shm_name] = shared_memory.SharedMemory(name=shm_name) + except FileNotFoundError: + continue # Skip if shared memory doesn't exist + + shm = self.shms[shm_name] + header_size = ctypes.sizeof(SimpleImageHeader) + + # Read header + header_data = bytes(shm.buf[:header_size]) + header = SimpleImageHeader.from_buffer_copy(header_data) + + # Check timestamp + last_ts = self.last_timestamps.get(image_name, 0) + if header.timestamp <= last_ts: + # Return cached image if available + if image_name in self.buffer: + images[image_name] = self.buffer[image_name] + continue + + # Read payload + data_start = header_size + data_end = data_start + header.data_size + payload = bytes(shm.buf[data_start:data_end]) + + # Decode image + if header.encoding == 1: # JPEG + encoded = np.frombuffer(payload, dtype=np.uint8) + image = cv2.imdecode(encoded, cv2.IMREAD_COLOR) + if image is None: + continue + else: # RAW + image = np.frombuffer(payload, dtype=np.uint8) + expected_size = header.height * header.width * header.channels + if image.size != expected_size: + print(f"[MultiImageReader] Data size mismatch for {image_name}: expected {expected_size}, got {image.size}") + continue + image = image.reshape(header.height, header.width, header.channels) + + # Cache and return + self.buffer[image_name] = image + self.last_timestamps[image_name] = header.timestamp + images[image_name] = image + + except Exception as e: + print(f"[MultiImageReader] Error reading {image_name}: {e}") + continue + + return images if images else None + + def read_concatenated_image(self) -> Optional[np.ndarray]: + """Read all images and concatenate them horizontally (for backward compatibility) + + Returns: + np.ndarray: the concatenated image array; if the reading fails, return None + """ + images = self.read_images() + if images is None or not images: + return None + + try: + # Concatenate images in order: head, left, right + image_order = ['head', 'left', 'right'] + frames_to_concat = [] + + for image_name in image_order: + if image_name in images: + frames_to_concat.append(images[image_name]) + + if not frames_to_concat: + return None + + if len(frames_to_concat) > 1: + concatenated_image = cv2.hconcat(frames_to_concat) + else: + concatenated_image = frames_to_concat[0] + + return concatenated_image + + except Exception as e: + print(f"[MultiImageReader] Error concatenating images: {e}") + return None + + def read_single_image(self, image_name: str) -> Optional[np.ndarray]: + """Read a single specific image from its dedicated shared memory. + + Args: + image_name: Name of the image to read ("head", "left", or "right") + + Returns: + np.ndarray: The requested image array, or None if not found or error + """ + try: + shm_name = get_shm_name(image_name) + + # Open shared memory if not already open + if shm_name not in self.shms: + try: + self.shms[shm_name] = shared_memory.SharedMemory(name=shm_name) + except FileNotFoundError: + return None + + shm = self.shms[shm_name] + header_size = ctypes.sizeof(SimpleImageHeader) + + # Read header + header_data = bytes(shm.buf[:header_size]) + header = SimpleImageHeader.from_buffer_copy(header_data) + + # Check if there is new data + last_ts = self.last_timestamps.get(image_name, 0) + if header.timestamp <= last_ts: + # Return cached image if available + return self.buffer.get(image_name) + + # Read payload + data_start = header_size + data_end = data_start + header.data_size + payload = bytes(shm.buf[data_start:data_end]) + + # Decode image + if header.encoding == 1: # JPEG + encoded = np.frombuffer(payload, dtype=np.uint8) + image = cv2.imdecode(encoded, cv2.IMREAD_COLOR) + if image is None: + return None + else: # RAW + image = np.frombuffer(payload, dtype=np.uint8) + expected_size = header.height * header.width * header.channels + if image.size != expected_size: + print(f"[MultiImageReader] Data size mismatch for {image_name}: expected {expected_size}, got {image.size}") + return None + image = image.reshape(header.height, header.width, header.channels) + + # Update buffer and timestamp + self.buffer[image_name] = image + self.last_timestamps[image_name] = header.timestamp + return image + + except Exception as e: + print(f"[MultiImageReader] Error reading single image {image_name}: {e}") + return None + + def read_encoded_frame(self, image_name: str = "head") -> Optional[bytes]: + """Read encoded payload for a specific image if available (e.g., JPEG). Returns bytes or None.""" + if self.shm is None: + return None + + try: + # Scan through all images in shared memory + header_size = ctypes.sizeof(SimpleImageHeader) + current_offset = 0 + + while current_offset < self.shm.size - header_size: + # Read header + header_data = bytes(self.shm.buf[current_offset:current_offset + header_size]) + header = SimpleImageHeader.from_buffer_copy(header_data) + + # Check if this is the image we want and it's encoded + current_image_name = header.image_name.decode('utf-8').rstrip('\x00') + if current_image_name == image_name and header.encoding == 1: + # Check if there is new data + if header.timestamp <= self.last_timestamp: + return None + + # Read the payload + data_start = current_offset + header_size + data_end = data_start + header.data_size + payload = bytes(self.shm.buf[data_start:data_end]) + + self.last_timestamp = header.timestamp + return payload + + # Move to next image + current_offset += header_size + header.data_size + + return None + + except Exception as e: + print(f"[MultiImageReader] Error reading encoded frame for {image_name}: {e}") + return None + + def close(self): + """Close all shared memories""" + for shm_name, shm in self.shms.items(): + try: + shm.close() + print(f"[MultiImageReader] Shared memory closed: {shm_name}") + except Exception as e: + print(f"[MultiImageReader] Error closing {shm_name}: {e}") + self.shms.clear() + self.buffer.clear() + self.last_timestamps.clear() + + +# backward compatible class (single image) +class SharedMemoryWriter: + """Backward compatible single image writer""" + + def __init__(self, shm_name: str = SHM_NAME, shm_size: int = SHM_SIZE): + self.multi_writer = MultiImageWriter(shm_name, shm_size) + + def write_image(self, image: np.ndarray) -> bool: + """Write a single image (as the head image)""" + return self.multi_writer.write_images({'head': image}) + + def close(self): + self.multi_writer.close() + + +class SharedMemoryReader: + """Backward compatible single image reader""" + + def __init__(self, shm_name: str = SHM_NAME): + self.multi_reader = MultiImageReader(shm_name) + + def read_image(self) -> Optional[np.ndarray]: + """Read a single image (the head image)""" + images = self.multi_reader.read_images() + return images.get('head') if images else None + + def close(self): + self.multi_reader.close() diff --git a/unitree_robots/g1/g1_29dof_inspire_fixed.xml b/unitree_robots/g1/g1_29dof_inspire_fixed.xml new file mode 100644 index 00000000..d2518c41 --- /dev/null +++ b/unitree_robots/g1/g1_29dof_inspire_fixed.xml @@ -0,0 +1,605 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_robots/g1/inspire_build/convert_hands.py b/unitree_robots/g1/inspire_build/convert_hands.py new file mode 100644 index 00000000..2b088d00 --- /dev/null +++ b/unitree_robots/g1/inspire_build/convert_hands.py @@ -0,0 +1,44 @@ +"""Convert inspire hand URDFs to MJCF body snippets for merging into g1_29dof. + +Steps: +1. Load each URDF with MuJoCo (mimic tags are ignored by MuJoCo's URDF loader, + so mimic joints become regular joints; we couple them later with ). +2. Save the MJCF, strip debug axis geoms (cylinders/spheres without a mesh). +3. Print the subtree of the hand base link for manual/scripted merging. +""" +import os +import re +import sys +import xml.etree.ElementTree as ET + +import mujoco + +SRC = "/home/panu/Documents/fibo/project_humanoid/xr_teleoperate/assets/inspire_hand" +OUT = os.path.dirname(os.path.abspath(__file__)) + +for side, base in (("left", "L_hand_base_link"), ("right", "R_hand_base_link")): + urdf = os.path.join(SRC, f"inspire_hand_{side}.urdf") + spec = mujoco.MjSpec.from_file(urdf) + spec.meshdir = SRC # filenames already contain the meshes/ prefix + xml_path = os.path.join(OUT, f"inspire_{side}_raw.xml") + with open(xml_path, "w") as f: + f.write(spec.to_xml()) + print(f"saved {xml_path}") + + tree = ET.parse(xml_path) + root = tree.getroot() + wb = root.find("worldbody") + base_body = wb.find(f".//body[@name='{base}']") or wb.find("body") + # strip debug axis geoms (non-mesh geoms in base body only) + for geom in list(base_body.findall("geom")): + if geom.get("type") in ("cylinder", "sphere"): + base_body.remove(geom) + snippet_path = os.path.join(OUT, f"inspire_{side}_body.xml") + ET.ElementTree(base_body).write(snippet_path) + print(f"saved {snippet_path}") + + # list joints for verification + joints = [j.get("name") for j in base_body.iter("joint")] + print(f"{side} joints ({len(joints)}):") + for j in joints: + print(" ", j) diff --git a/unitree_robots/g1/inspire_build/inspire_left_body.xml b/unitree_robots/g1/inspire_build/inspire_left_body.xml new file mode 100644 index 00000000..74695c83 --- /dev/null +++ b/unitree_robots/g1/inspire_build/inspire_left_body.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_robots/g1/inspire_build/inspire_left_raw.xml b/unitree_robots/g1/inspire_build/inspire_left_raw.xml new file mode 100644 index 00000000..46c7fc2d --- /dev/null +++ b/unitree_robots/g1/inspire_build/inspire_left_raw.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unitree_robots/g1/inspire_build/inspire_right_body.xml b/unitree_robots/g1/inspire_build/inspire_right_body.xml new file mode 100644 index 00000000..cc67e506 --- /dev/null +++ b/unitree_robots/g1/inspire_build/inspire_right_body.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_robots/g1/inspire_build/inspire_right_raw.xml b/unitree_robots/g1/inspire_build/inspire_right_raw.xml new file mode 100644 index 00000000..bd36497c --- /dev/null +++ b/unitree_robots/g1/inspire_build/inspire_right_raw.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/unitree_robots/g1/inspire_build/merge_g1_inspire.py b/unitree_robots/g1/inspire_build/merge_g1_inspire.py new file mode 100644 index 00000000..f97e5840 --- /dev/null +++ b/unitree_robots/g1/inspire_build/merge_g1_inspire.py @@ -0,0 +1,215 @@ +"""Build g1_29dof_inspire_fixed.xml: + +G1 29-DoF body (pelvis welded to the world, no freejoint) with Inspire hands +attached to both wrist yaw links. Adds: +- 12 position actuators for the driven finger joints, ordered to match the + Unitree inspire DDS convention (ids 0-5 right hand, 6-11 left hand). +- equality couplings replacing the URDF mimic joints. + +Mount transform comes from the official h1_2.urdf inspire mount: + left : pos 0.054 0 0, rpy(0,0,+pi/2) -> quat (0.7071068, 0, 0, 0.7071068) + right: pos 0.054 0 0, rpy(pi,0,-pi/2) -> quat (0, 0.7071068, -0.7071068, 0) +""" +import os +import re +import xml.etree.ElementTree as ET + +HERE = os.path.dirname(os.path.abspath(__file__)) +G1_DIR = os.path.dirname(HERE) + +SQ2 = 0.70710678118654757 + +HAND_JOINT_ATTRS = {"damping": "0.05", "armature": "0.002", "frictionloss": "0.01"} + +# inspire DDS id -> (joint name, min, max) ; q_norm = (max - q)/(max-min) +DDS_ORDER = [ + ("R_pinky_proximal_joint", 0.0, 1.7), + ("R_ring_proximal_joint", 0.0, 1.7), + ("R_middle_proximal_joint", 0.0, 1.7), + ("R_index_proximal_joint", 0.0, 1.7), + ("R_thumb_proximal_pitch_joint", 0.0, 0.5), + ("R_thumb_proximal_yaw_joint", -0.1, 1.3), + ("L_pinky_proximal_joint", 0.0, 1.7), + ("L_ring_proximal_joint", 0.0, 1.7), + ("L_middle_proximal_joint", 0.0, 1.7), + ("L_index_proximal_joint", 0.0, 1.7), + ("L_thumb_proximal_pitch_joint", 0.0, 0.5), + ("L_thumb_proximal_yaw_joint", -0.1, 1.3), +] + +# mimic couplings: dependent joint = multiplier * driver joint +MIMICS = [] +for s in ("L", "R"): + for f in ("index", "middle", "ring", "pinky"): + MIMICS.append((f"{s}_{f}_intermediate_joint", f"{s}_{f}_proximal_joint", 1.0)) + MIMICS.append((f"{s}_thumb_intermediate_joint", f"{s}_thumb_proximal_pitch_joint", 1.6)) + MIMICS.append((f"{s}_thumb_distal_joint", f"{s}_thumb_proximal_pitch_joint", 2.4)) + + +# base-link inertials from the URDFs (dropped by the URDF importer since the +# root link becomes the worldbody). fullinertia order: ixx iyy izz ixy ixz iyz +BASE_INERTIAL = { + "left": { + "pos": "-0.002551 -0.066047 -0.0019357", + "mass": "0.14143", + "fullinertia": "0.0001234 8.3835e-05 7.7231e-05 2.1995e-06 -1.7694e-06 1.5968e-06", + }, + "right": { + "pos": "-0.0025264 -0.066047 0.0019598", + "mass": "0.14143", + "fullinertia": "0.00012281 8.3832e-05 7.6663e-05 2.1711e-06 1.7709e-06 -1.6551e-06", + }, +} + + +def load_hand(side: str) -> tuple[list[ET.Element], ET.Element]: + """Return (mesh asset elements, hand base body element). + + The URDF importer turns the root link (X_hand_base_link) into the + worldbody, so we rebuild it as a proper body and move the base geoms and + finger subtrees into it. + """ + raw = ET.parse(os.path.join(HERE, f"inspire_{side}_raw.xml")).getroot() + meshes = list(raw.find("asset").findall("mesh")) + for m in meshes: + # file paths become relative to g1 meshdir="meshes" (STLs copied there) + m.set("file", os.path.basename(m.get("file"))) + m.attrib.pop("content_type", None) + base_name = "L_hand_base_link" if side == "left" else "R_hand_base_link" + wb = raw.find("worldbody") + + base = ET.Element("body", {"name": base_name}) + ET.SubElement(base, "inertial", BASE_INERTIAL[side]) + for geom in wb.findall("geom"): + # keep only base mesh geoms, drop URDF debug axis cylinders/spheres + if geom.get("mesh") == base_name: + base.append(geom) + for body in wb.findall("body"): + base.append(body) + + # stabilizing joint params + for j in base.iter("joint"): + for k, v in HAND_JOINT_ATTRS.items(): + j.set(k, v) + return meshes, base + + +def main() -> None: + tree = ET.parse(os.path.join(G1_DIR, "g1_29dof.xml")) + root = tree.getroot() + root.set("model", "g1_29dof_inspire_fixed") + + # --- fix pelvis: remove the freejoint --- + pelvis = root.find(".//body[@name='pelvis']") + fj = pelvis.find("joint[@name='floating_base_joint']") + pelvis.remove(fj) + + # --- stereo head cameras (for Quest binocular view) --- + # torso_link carries the head mesh (center ~(0.007, 0, 0.375), front face + # ~x=0.075). Cameras look along body +X (forward), image-up = body +Z, + # pitched 25 deg downward toward the workspace. + # xyaxes: cam x = -Y_body (image right), cam y = up vector after pitch. + torso = root.find(".//body[@name='torso_link']") + ipd = 0.064 # interpupillary distance + pitch_up = "0.4226 0 0.9063" # sin(25deg), 0, cos(25deg) + for name, y in (("head_left_eye", ipd / 2), ("head_right_eye", -ipd / 2)): + ET.SubElement(torso, "camera", { + "name": name, + "pos": f"0.075 {y} 0.41", + "xyaxes": f"0 -1 0 {pitch_up}", + "fovy": "70", + }) + + # offscreen framebuffer large enough for 640x480 eye renders + visual = root.find("visual") + if visual is None: + visual = ET.SubElement(root, "visual") + ET.SubElement(visual, "global", {"offwidth": "1280", "offheight": "720"}) + + asset = root.find("asset") + + for side, parent_name, mount_quat in ( + ("left", "left_wrist_yaw_link", f"{SQ2} 0 0 {SQ2}"), + ("right", "right_wrist_yaw_link", f"0 {SQ2} -{SQ2} 0"), + ): + meshes, hand_body = load_hand(side) + for m in meshes: + asset.append(m) + + wrist = root.find(f".//body[@name='{parent_name}']") + # remove rubber hand visual geom + for geom in list(wrist.findall("geom")): + if geom.get("mesh", "").endswith("_rubber_hand"): + wrist.remove(geom) + + hand_body.set("pos", "0.054 0 0") + hand_body.set("quat", mount_quat) + wrist.append(hand_body) + + # The thumb's proximal link overlaps its palm at the joint origin. Keep + # all other self/object contacts, but exclude these two impossible pairs. + contact = ET.SubElement(root, "contact") + for side in ("L", "R"): + ET.SubElement(contact, "exclude", { + "body1": f"{side}_hand_base_link", + "body2": f"{side}_thumb_proximal", + }) + + # --- actuators: 12 position actuators in inspire DDS order --- + actuator = root.find("actuator") + for name, lo, hi in DDS_ORDER: + ET.SubElement(actuator, "position", { + "name": name.replace("_joint", ""), + "joint": name, + "kp": "1.0", + "kv": "0.05", + "ctrlrange": f"{lo} {hi}", + "forcerange": "-1 1", + }) + + # --- equality couplings for mimic joints --- + equality = ET.SubElement(root, "equality") + for dep, drv, mult in MIMICS: + ET.SubElement(equality, "joint", { + "joint1": dep, + "joint2": drv, + "polycoef": f"0 {mult} 0 0 0", + }) + + ET.indent(tree, space=" ") + out = os.path.join(G1_DIR, "g1_29dof_inspire_fixed.xml") + tree.write(out) + print(f"wrote {out}") + + scene = f""" + + + + + + + + + + + + + + + + + + + + + +""" + scene_out = os.path.join(G1_DIR, "scene_29dof_inspire_fixed.xml") + with open(scene_out, "w") as f: + f.write(scene) + print(f"wrote {scene_out}") + + +if __name__ == "__main__": + main() diff --git a/unitree_robots/g1/inspire_build/snaps/g1_full.png b/unitree_robots/g1/inspire_build/snaps/g1_full.png new file mode 100644 index 00000000..a4bc24e4 Binary files /dev/null and b/unitree_robots/g1/inspire_build/snaps/g1_full.png differ diff --git a/unitree_robots/g1/inspire_build/snaps/g1_hand.png b/unitree_robots/g1/inspire_build/snaps/g1_hand.png new file mode 100644 index 00000000..3805ded0 Binary files /dev/null and b/unitree_robots/g1/inspire_build/snaps/g1_hand.png differ diff --git a/unitree_robots/g1/meshes/L_hand_base_link.STL b/unitree_robots/g1/meshes/L_hand_base_link.STL new file mode 100644 index 00000000..a2f67eea Binary files /dev/null and b/unitree_robots/g1/meshes/L_hand_base_link.STL differ diff --git a/unitree_robots/g1/meshes/Link11_L.STL b/unitree_robots/g1/meshes/Link11_L.STL new file mode 100644 index 00000000..9933a6ce Binary files /dev/null and b/unitree_robots/g1/meshes/Link11_L.STL differ diff --git a/unitree_robots/g1/meshes/Link11_R.STL b/unitree_robots/g1/meshes/Link11_R.STL new file mode 100644 index 00000000..30817a81 Binary files /dev/null and b/unitree_robots/g1/meshes/Link11_R.STL differ diff --git a/unitree_robots/g1/meshes/Link12_L.STL b/unitree_robots/g1/meshes/Link12_L.STL new file mode 100644 index 00000000..08071d3e Binary files /dev/null and b/unitree_robots/g1/meshes/Link12_L.STL differ diff --git a/unitree_robots/g1/meshes/Link12_R.STL b/unitree_robots/g1/meshes/Link12_R.STL new file mode 100644 index 00000000..137dbdef Binary files /dev/null and b/unitree_robots/g1/meshes/Link12_R.STL differ diff --git a/unitree_robots/g1/meshes/Link13_L.STL b/unitree_robots/g1/meshes/Link13_L.STL new file mode 100644 index 00000000..03a34258 Binary files /dev/null and b/unitree_robots/g1/meshes/Link13_L.STL differ diff --git a/unitree_robots/g1/meshes/Link13_R.STL b/unitree_robots/g1/meshes/Link13_R.STL new file mode 100644 index 00000000..a42829f7 Binary files /dev/null and b/unitree_robots/g1/meshes/Link13_R.STL differ diff --git a/unitree_robots/g1/meshes/Link14_L.STL b/unitree_robots/g1/meshes/Link14_L.STL new file mode 100644 index 00000000..291a14f3 Binary files /dev/null and b/unitree_robots/g1/meshes/Link14_L.STL differ diff --git a/unitree_robots/g1/meshes/Link14_R.STL b/unitree_robots/g1/meshes/Link14_R.STL new file mode 100644 index 00000000..13f98f89 Binary files /dev/null and b/unitree_robots/g1/meshes/Link14_R.STL differ diff --git a/unitree_robots/g1/meshes/Link15_L.STL b/unitree_robots/g1/meshes/Link15_L.STL new file mode 100644 index 00000000..f8994311 Binary files /dev/null and b/unitree_robots/g1/meshes/Link15_L.STL differ diff --git a/unitree_robots/g1/meshes/Link15_R.STL b/unitree_robots/g1/meshes/Link15_R.STL new file mode 100644 index 00000000..cbfc7110 Binary files /dev/null and b/unitree_robots/g1/meshes/Link15_R.STL differ diff --git a/unitree_robots/g1/meshes/Link16_L.STL b/unitree_robots/g1/meshes/Link16_L.STL new file mode 100644 index 00000000..24a5a83d Binary files /dev/null and b/unitree_robots/g1/meshes/Link16_L.STL differ diff --git a/unitree_robots/g1/meshes/Link16_R.STL b/unitree_robots/g1/meshes/Link16_R.STL new file mode 100644 index 00000000..0cf0f8d1 Binary files /dev/null and b/unitree_robots/g1/meshes/Link16_R.STL differ diff --git a/unitree_robots/g1/meshes/Link17_L.STL b/unitree_robots/g1/meshes/Link17_L.STL new file mode 100644 index 00000000..8f0aca60 Binary files /dev/null and b/unitree_robots/g1/meshes/Link17_L.STL differ diff --git a/unitree_robots/g1/meshes/Link17_R.STL b/unitree_robots/g1/meshes/Link17_R.STL new file mode 100644 index 00000000..3f630a30 Binary files /dev/null and b/unitree_robots/g1/meshes/Link17_R.STL differ diff --git a/unitree_robots/g1/meshes/Link18_L.STL b/unitree_robots/g1/meshes/Link18_L.STL new file mode 100644 index 00000000..c498cd75 Binary files /dev/null and b/unitree_robots/g1/meshes/Link18_L.STL differ diff --git a/unitree_robots/g1/meshes/Link18_R.STL b/unitree_robots/g1/meshes/Link18_R.STL new file mode 100644 index 00000000..ebc2616f Binary files /dev/null and b/unitree_robots/g1/meshes/Link18_R.STL differ diff --git a/unitree_robots/g1/meshes/Link19_L.STL b/unitree_robots/g1/meshes/Link19_L.STL new file mode 100644 index 00000000..c3cabb92 Binary files /dev/null and b/unitree_robots/g1/meshes/Link19_L.STL differ diff --git a/unitree_robots/g1/meshes/Link19_R.STL b/unitree_robots/g1/meshes/Link19_R.STL new file mode 100644 index 00000000..5c3703ec Binary files /dev/null and b/unitree_robots/g1/meshes/Link19_R.STL differ diff --git a/unitree_robots/g1/meshes/Link20_L.STL b/unitree_robots/g1/meshes/Link20_L.STL new file mode 100644 index 00000000..23c4efa5 Binary files /dev/null and b/unitree_robots/g1/meshes/Link20_L.STL differ diff --git a/unitree_robots/g1/meshes/Link20_R.STL b/unitree_robots/g1/meshes/Link20_R.STL new file mode 100644 index 00000000..5fed852f Binary files /dev/null and b/unitree_robots/g1/meshes/Link20_R.STL differ diff --git a/unitree_robots/g1/meshes/Link21_L.STL b/unitree_robots/g1/meshes/Link21_L.STL new file mode 100644 index 00000000..ccb63bfc Binary files /dev/null and b/unitree_robots/g1/meshes/Link21_L.STL differ diff --git a/unitree_robots/g1/meshes/Link21_R.STL b/unitree_robots/g1/meshes/Link21_R.STL new file mode 100644 index 00000000..d4710f12 Binary files /dev/null and b/unitree_robots/g1/meshes/Link21_R.STL differ diff --git a/unitree_robots/g1/meshes/Link22_L.STL b/unitree_robots/g1/meshes/Link22_L.STL new file mode 100644 index 00000000..6b9cdadd Binary files /dev/null and b/unitree_robots/g1/meshes/Link22_L.STL differ diff --git a/unitree_robots/g1/meshes/Link22_R.STL b/unitree_robots/g1/meshes/Link22_R.STL new file mode 100644 index 00000000..c2cf58b3 Binary files /dev/null and b/unitree_robots/g1/meshes/Link22_R.STL differ diff --git a/unitree_robots/g1/meshes/R_hand_base_link.STL b/unitree_robots/g1/meshes/R_hand_base_link.STL new file mode 100644 index 00000000..3478a981 Binary files /dev/null and b/unitree_robots/g1/meshes/R_hand_base_link.STL differ diff --git a/unitree_robots/g1/scene_29dof_inspire_fixed.xml b/unitree_robots/g1/scene_29dof_inspire_fixed.xml new file mode 100644 index 00000000..ce8f4992 --- /dev/null +++ b/unitree_robots/g1/scene_29dof_inspire_fixed.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + +