gr00t-WholeBodyControl/docs/source/tutorials/vr_wholebody_teleop.md

PICO VR Whole-body Teleop

Full whole-body teleoperation using PICO VR headset and controllers. To teleop, use the option --input-type zmq_manager during deployment. The zmq_manager input type switches between a planner mode (locomotion commands via ZMQ) and a streamed motion mode (full-body SMPL poses from PICO).

:class: danger
Whole-body teleoperation involves fast, agile motions. **Always** maintain a clear safety zone and keep a safety operator at the keyboard ready to trigger an emergency stop (**`O`** in the C++ terminal, or **A+B+X+Y** on the PICO controllers).

You **must wear tight-fitting pants or leggings** to guarantee line-of-sight for the foot trackers — loose or baggy clothing can make tracking fail unpredictably and may result in dangerous motion.

:width: 100%

Video: End-to-end teleoperation walkthrough — PICO calibration, policy engagement, and the robot balancing independently. See the Teleoperation Guide for detailed best practices.

Prerequisites

1. Completed the Quick Start — you can run the sim2sim loop.
2. Completed the VR Teleop Setup — PICO hardware is installed, calibrated, connected, and .venv_teleop is ready.

---

Step-by-Step: Teleop in SIM

Run three terminals to teleoperate the simulated robot.

Terminal 1 — Launch virtual robot in MuJoCo Simulator

From the repo root:

# bash install_scripts/install_pico.sh

source .venv_teleop/bin/activate
python gear_sonic/scripts/run_sim_loop.py

Terminal 2 — C++ Deployment

From gear_sonic_deploy/:

cd gear_sonic_deploy
source scripts/setup_env.sh
./deploy.sh sim --input-type zmq_manager
# Wait until you see "Init done"

The `--zmq-host` flag defaults to `localhost`, which is correct when both C++ deployment scripts and teleop scripts (Terminal 3) run on the same machine. If the teleop script runs on a different machine, pass `--zmq-host <IP-of-teleop-machine>`.

Terminal 3 — PICO Teleop Streamer

From the repo root:

source .venv_teleop/bin/activate

# With full visualization (recommended for first run):
python gear_sonic/scripts/pico_manager_thread_server.py --manager \
    --vis_vr3pt --vis_smpl

# Without visualization (for headless / onboard practice):
# python gear_sonic/scripts/pico_manager_thread_server.py --manager

When you turn on the visualization, wait for a window to pop up showing a Unitree G1 mesh with all joints at the default angles. If no window shows up, double-check the PICO's XRoboToolKit IP configuration in the VR Teleop Setup.

Your First Teleop Session

1. Assume the calibration pose — stand upright, feet together, upper arms at your sides, forearms bent 90° forward (L-shape at each elbow), palms inward. See Calibration Pose for details.
2. Press A + B + X + Y simultaneously to engage the control policy and run the initial full calibration (CALIB_FULL).
3. Align your arms with the robot's current pose, then press A + X to enter full-body SMPL teleop (POSE mode). Move your arms and legs — the robot follows.
4. Press A + X again to fall back to PLANNER (idle) mode.
5. Press A + B + X + Y again to stop the robot.

Basic whole-body teleop workflow: calibration pose → engage → POSE mode → PLANNER idle → stop.

---

(pico-controls)=

Complete PICO Controls

Modes & Calibration

The system has 4 operating modes and 2 calibration types.

Modes:

Mode	Encoder	Description
OFF	--	Policy not running. Stand in calibration pose, then press A+B+X+Y to start policy.
POSE	SMPL	Whole-body teleop — streaming the SMPL pose from PICO to the C++ deployment side. Your motion will directly map to the robot .
PLANNER	G1	Locomotion planner active; upper body controller by planner. Joysticks control direction and heading in walking and running modes.
PLANNER_FROZEN_UPPER	G1	Planner locomotion; upper body frozen at last POSE snapshot.
VR_3PT	TELEOP	Planner locomotion; upper body follows VR 3-point tracking (head + 2 hands). Depends on non-IK-based VR 3-point calibration.

Calibration types (non-IK workflow for minimal latency):

Type	What Is Calibrated	When Triggered
CALIB_FULL	Head + both wrists against all-zero reference pose.	Once on first A+B+X+Y (startup).
CALIB	Both wrists only against the current robot pose.	Each switch into VR_3PT via Left Stick Click.

State Machine

There are 4 modes and 2 control chains. Each chain forms a triangle: A+X (or B+Y) returns to POSE from both the planner node and its VR_3PT sub-mode.

  ┌──────────────────────────────────────┐
  │  A+B+X+Y (any mode) ──► OFF          │
  └──────────────────────────────────────┘

  Startup:
    OFF ──(A+B+X+Y)──► PLANNER ──(A+X)──► POSE
          CALIB_FULL

  Chain 1 — G1 encoder listens to planner-generated full-body motion: PLANNER

    PLANNER ─── L-Stick (+CALIB) ──► VR_3PT
     ▲    │ ◄─────── L-Stick ─────────  │
     │    │                             │
     │A+X │A+X                    A+X   │
     │    ▼                             ▼
     └─ POSE ◄──────────────────────────┘

  Chain 2 — G1 encoder listens to planner-generated lower-body motion: PLANNER_FROZEN_UPPER

    PLANNER_FROZEN_UPPER ── L-Stick (+CALIB) ──► VR_3PT
         ▲     │ ◄──────── L-Stick ──────────     │
         │     │                                  │
         │B+Y  │B+Y                          B+Y  │
         │     ▼                                  ▼
         └── POSE ◄───────────────────────────────┘

:class: danger
**Before switching into POSE or VR_3PT**, always align your body with the robot's current pose first!!

- **POSE:** The robot instantly snaps to your physical pose. A large mismatch causes sudden, aggressive motion.
- **VR_3PT:** A misaligned calibration produces erratic, dangerous motion as soon as you move your arms. Check more info at section [per-switch-calib](#per-switch-calib)

(calibration-pose)=

VR_3PT Calibration Hint

The VR_3PT mode depends on accurate calibration. Two calibration events occur:

One-time CALIB_FULL (head + wrists)

Before pressing A + B + X + Y for the first time, you must stand in the robot's all-zero reference pose. The system captures your PICO body-tracking frame as the zero-reference for all subsequent motion mapping.

The reference pose:

1. Stand upright, feet together, looking straight forward.
2. Upper arms hang straight down, close to your torso.
3. Forearms bent 90° forward (L-shape at each elbow), palms facing inward.

Launch the teleop script with `--vis_vr3pt` to see the robot's reference pose in a visualization window. Match your body to it before pressing the start combo.

(per-switch-calib)=

Per-switch CALIB (wrists only)

Each time you enter VR_3PT via Left Stick Click, the system re-calibrates both wrists against the robot's current pose. Always align your arms with the robot before clicking.

Below is an example of bad calibration practice — transitioning into VR_3PT without aligning your arms to the robot's current pose. The robot may not jump immediately, but will exhibit erratic and dangerous motion as soon as you move.

Bad practice: entering VR_3PT without arm alignment causes erratic, unsafe motion.

:class: danger
**Before switching into POSE or VR_3PT**, always align your body with the robot's current pose first.

**Recovery from bad VR_3PT calibration:**
1. Freeze the upper body — switch back via **Left Stick Click**.
2. Re-align your arms with the robot's current (possibly distorted) pose.
3. Switch to **POSE** mode (**A+X**) to reset.

Below is the recovery procedure — if you accidentally enter a badly calibrated VR_3PT state, freeze the upper body (Left Stick Click back), then switch to POSE mode (A+X) to reset safely.

Recovery from bad VR_3PT calibration: freeze upper body → re-align → switch to POSE mode.

Quick-Start Cheatsheet

Action	Button	Notes
Start / Stop policy	A+B+X+Y	First press: engage + CALIB_FULL. Again: emergency stop → OFF.
Toggle POSE	A+X	Switches between PLANNER ↔ POSE. OR from VR_3PT (entered via PLANNER) → POSE.
Toggle PLANNER_FROZEN_UPPER	B+Y	Switches between POSE ↔ PLANNER_FROZEN_UPPER. OR from VR_3PT (entered via PLANNER_FROZEN_UPPER) → POSE.
Toggle VR_3PT	Left Stick Click	From any Planner mode → VR_3PT (triggers CALIB). Click again to return.
Hand grasp	Trigger (per hand)	Controls the corresponding hand's grasp.

Joystick Controls (Planner Modes)

Active in PLANNER, PLANNER_FROZEN_UPPER, and VR_3PT:

Input	Function
Left Stick	Move direction (forward / backward / strafe)
Right Stick (horizontal)	Yaw / heading (continuous accumulation)
A + B	Next locomotion mode
X + Y	Previous locomotion mode

Locomotion modes (cycled via A+B / X+Y):

ID	Mode
0	Idle [DEFAULT]
1	Slow Walk
2	Walk
3	Run
4	Squat
5	Kneel (two legs)
6	Kneel
7	Lying face-down
8	Crawling
9–16	Boxing variants (idle, walk, punches, hooks)
17	Forward Jump
18	Stealth Walk
19	Injured Walk

---

Emergency Stop

Method	Action
PICO controllers	Press A+B+X+Y simultaneously → OFF
Keyboard (C++ terminal)	Press O for immediate stop

---

Step-by-Step: Teleop on Real Robot

:class: danger
Only proceed once you can smoothly control the robot in simulation and are comfortable with emergency stops. **Terminate any running `run_sim_loop.py` process first** — simultaneous sim and real instances will conflict.

The real-robot workflow uses two terminals (no MuJoCo simulator).

Terminal 1 — C++ Deployment (Real Robot)

From gear_sonic_deploy/:

cd gear_sonic_deploy
source scripts/setup_env.sh

# 'real' auto-detects the robot network interface (192.168.123.x).
# If auto-detection fails, pass the G1's IP directly:
#   ./deploy.sh --input-type zmq_manager <G1-IP>
./deploy.sh real --input-type zmq_manager

# Wait until you see "Init done"

If the teleop script (Terminal 2) runs on a different machine, add `--zmq-host <IP-of-teleop-machine>` so the C++ side knows where the ZMQ publisher is.

Terminal 2 — PICO Teleop Streamer

From the repo root:

source .venv_teleop/bin/activate
python gear_sonic/scripts/pico_manager_thread_server.py --manager

# If running offboard with a display, add visualization:
#   --vis_vr3pt --vis_smpl

Update the IP in the PICO's XRoboToolKit app to match this machine before starting.

Follow the same start sequence: calibration pose → A+B+X+Y → A+X for POSE mode. See Complete PICO Controls for all available commands.