From 57d33c295e4ee013300055c2ae7e94fa2f347d06 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Thu, 30 Jul 2026 14:48:09 +0200 Subject: [PATCH] delete lowstate_to_obs --- .../controllers/sonic_whole_body.py | 6 +-- src/lerobot/robots/unitree_g1/unitree_g1.py | 49 +++---------------- 2 files changed, 8 insertions(+), 47 deletions(-) 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 800dc1d4c..c4fc19212 100644 --- a/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py +++ b/src/lerobot/robots/unitree_g1/controllers/sonic_whole_body.py @@ -44,7 +44,6 @@ from ..g1_utils import ( G1_29_JointIndex, get_gravity_orientation, ) -from ..unitree_g1 import lowstate_to_obs logger = logging.getLogger(__name__) @@ -337,10 +336,9 @@ class SonicWholeBodyController: logger.info("SONIC startup blend complete -> full policy control") return blended - def run_step(self, action: dict, lowstate) -> dict: - if lowstate is None: + def run_step(self, action: dict, obs: dict) -> dict: + if not obs: return {} - obs = lowstate_to_obs(lowstate) # Token-only interface (token-output VLA): a dense 64-D ``motion_token.{i}`` command # is decoded directly, encoder bypassed. diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index b844776ab..86df4cd7c 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -105,47 +105,6 @@ class G1_29_LowState: # noqa: N801 mode_machine: int = 0 # Robot mode -def lowstate_to_obs(lowstate) -> dict: - """Build a robot observation dict from a Unitree lowstate. - - Shared by ``UnitreeG1.get_observation`` and the SONIC pipeline so the - lowstate -> obs mapping lives in exactly one place. Keys match the - ``.q``/``imu.*`` schema consumed across the controllers. - """ - obs: dict = {} - - for motor in G1_29_JointIndex: - idx = motor.value - obs[f"{motor.name}.q"] = lowstate.motor_state[idx].q - obs[f"{motor.name}.dq"] = lowstate.motor_state[idx].dq - obs[f"{motor.name}.tau"] = lowstate.motor_state[idx].tau_est - - imu = lowstate.imu_state - if imu.gyroscope: - obs["imu.gyro.x"] = imu.gyroscope[0] - obs["imu.gyro.y"] = imu.gyroscope[1] - obs["imu.gyro.z"] = imu.gyroscope[2] - if imu.accelerometer: - obs["imu.accel.x"] = imu.accelerometer[0] - obs["imu.accel.y"] = imu.accelerometer[1] - obs["imu.accel.z"] = imu.accelerometer[2] - if imu.quaternion: - obs["imu.quat.w"] = imu.quaternion[0] - obs["imu.quat.x"] = imu.quaternion[1] - obs["imu.quat.y"] = imu.quaternion[2] - obs["imu.quat.z"] = imu.quaternion[3] - if imu.rpy: - obs["imu.rpy.roll"] = imu.rpy[0] - obs["imu.rpy.pitch"] = imu.rpy[1] - obs["imu.rpy.yaw"] = imu.rpy[2] - - wr = getattr(lowstate, "wireless_remote", None) - if wr: - obs["wireless_remote"] = bytes(wr) if not isinstance(wr, (bytes, bytearray)) else wr - - return obs - - class UnitreeG1(Robot): config_class = UnitreeG1Config name = "unitree_g1" @@ -377,8 +336,12 @@ class UnitreeG1(Robot): with self._controller_action_lock: controller_input = dict(self.controller_input) - # Run controller step - controller_action = self.controller.run_step(controller_input, lowstate) + # Full-body controllers (SONIC) consume the full observation dict; others + # take the raw lowstate. get_observation() is the single lowstate -> obs builder. + controller_state = ( + self.get_observation() if getattr(self.controller, "full_body", False) else lowstate + ) + controller_action = self.controller.run_step(controller_input, controller_state) # Write controller output snapshot with self._controller_action_lock: