delete lowstate_to_obs

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.

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
-    ``<joint>.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: