From 2eae31ea2b87b6092040d30153ddd24c7cf0b82e Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Wed, 15 Jul 2026 14:59:22 +0200 Subject: [PATCH] fix(unitree_g1): disable SMPL root-motion anchor to prevent sim instability Feeding the per-frame SMPL root quaternion into the mode-2 anchor produced root-acceleration spikes (NaN QACC at DOF 0) mid-episode during replay. Keep the anchor self-driven until the reference root trajectory is smoothed/rate-matched (30 Hz dataset -> 50 Hz control). Co-authored-by: Cursor --- .../robots/unitree_g1/controllers/sonic_whole_body.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py b/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py index 7fca5f1d1..1e637d1b6 100644 --- a/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py +++ b/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py @@ -349,7 +349,12 @@ class SonicWholeBodyController: self._enter_wholebody() self.controller.smpl_joints_10frame_step1 = smpl # Root orientation (if provided) steers the mode-2 anchor/heading. - self.controller.smpl_root_quat = root_quat + # Temporarily disabled: feeding the per-frame SMPL root quaternion produced + # root-acceleration spikes (NaN QACC at DOF 0, sim unstable) mid-episode. + # Keep the anchor self-driven until the reference root trajectory is + # smoothed/rate-matched (30 Hz dataset -> 50 Hz control). + self.controller.smpl_root_quat = None + _ = root_quat return self._runtime.tick(obs, debug=False, use_joystick=False) # No (or stale) SMPL: fall back to locomotion so the robot stays balanced.