feat(unitree_g1): make SONIC token interface implicit

Drop the ``sonic_token_action`` config flag; the 64-D latent-token
action/observation interface now switches on automatically whenever the
SONIC whole-body controller is selected (``controller == "SonicWholeBodyController"``).
Keyed via a ``_sonic_token`` property so client, onboard and sim roles agree.
This commit is contained in:
Martino Russi
2026-07-30 11:28:59 +02:00
parent 0c57cd03f2
commit bbfc4ff443
5 changed files with 39 additions and 51 deletions
+2 -3
View File
@@ -320,8 +320,8 @@ cd ~/lerobot
python src/lerobot/robots/unitree_g1/run_g1_server.py --handshake --camera python src/lerobot/robots/unitree_g1/run_g1_server.py --handshake --camera
``` ```
**From your laptop** — run the token policy; `--robot.sonic_token_action=true` switches the **From your laptop** — run the token policy; selecting `--robot.controller=SonicWholeBodyController`
robot to the 64-D latent-token action/observation interface: implicitly switches the robot to the 64-D latent-token action/observation interface:
```bash ```bash
lerobot-rollout \ lerobot-rollout \
@@ -331,7 +331,6 @@ lerobot-rollout \
--robot.is_simulation=false \ --robot.is_simulation=false \
--robot.robot_ip=<ROBOT_IP> \ --robot.robot_ip=<ROBOT_IP> \
--robot.controller=SonicWholeBodyController \ --robot.controller=SonicWholeBodyController \
--robot.sonic_token_action=true \
--robot.cameras='{"ego_view": {"type": "zmq", "server_address": "<ROBOT_IP>", "port": 5555, "camera_name": "head_camera", "width": 640, "height": 480, "fps": 30}}' \ --robot.cameras='{"ego_view": {"type": "zmq", "server_address": "<ROBOT_IP>", "port": 5555, "camera_name": "head_camera", "width": 640, "height": 480, "fps": 30}}' \
--task="walk back and forth" \ --task="walk back and forth" \
--duration=1000 \ --duration=1000 \
@@ -86,15 +86,10 @@ class UnitreeG1Config(RobotConfig):
# Compensates for gravity on the unitree's arms using the arm ik solver # Compensates for gravity on the unitree's arms using the arm ik solver
gravity_compensation: bool = False gravity_compensation: bool = False
# Token-output VLA interface for the SONIC decoder. When True (and the controller
# is ``SonicWholeBodyController``), the robot advertises a 64-D latent-token action
# space (``motion_token.{i}.pos``) and exposes the last commanded token as a 64-D
# ``observation.state`` (``motion_token_state.{i}.pos``). This lets
# ``lerobot-rollout`` drive a policy trained with 64-D SONIC motion tokens as both
# state and action (e.g. nepyope/sonic_walk): the decoder consumes the token
# directly, encoder bypassed. Ignored unless a SONIC whole-body controller is active.
sonic_token_action: bool = False
# Locomotion controller class name, e.g. "GrootLocomotionController", # Locomotion controller class name, e.g. "GrootLocomotionController",
# "HolosomaLocomotionController", or "SonicWholeBodyController". None disables it. # "HolosomaLocomotionController", or "SonicWholeBodyController". None disables it.
# Selecting "SonicWholeBodyController" implicitly switches the robot to the 64-D
# latent-token action/observation interface (``motion_token.{i}.pos`` action and a
# ``motion_token_state.{i}.pos`` state echo) so ``lerobot-rollout`` can drive a
# policy trained on SONIC motion tokens (e.g. nepyope/sonic_walk).
controller: str | None = None controller: str | None = None
@@ -332,8 +332,8 @@ class SonicWholeBodyController:
self._init_step = 0 self._init_step = 0
self._start_pose: dict[str, float] = {} self._start_pose: dict[str, float] = {}
# Token-interface state. ``token_mode`` is set True by the robot when the deploy is # Token-interface state. ``token_mode`` is set True by the robot whenever a SONIC
# token-driven (``UnitreeG1Config.sonic_token_action``): the controller then holds a # whole-body controller is selected (token-driven deploy): the controller then holds a
# stable *neutral* token until the first real token arrives, and afterwards holds the # stable *neutral* token until the first real token arrives, and afterwards holds the
# *last* token received between ticks (the async controller runs ~50 Hz while a token # *last* token received between ticks (the async controller runs ~50 Hz while a token
# VLA streams ~30 Hz). This lives here (not in the entry-point script) so it applies # VLA streams ~30 Hz). This lives here (not in the entry-point script) so it applies
@@ -232,7 +232,6 @@ def serve_onboard_controller(
dds_interface=dds_interface, dds_interface=dds_interface,
release_motion_control=not sim, release_motion_control=not sim,
physical_remote=not sim, physical_remote=not sim,
sonic_token_action=sonic_token_action,
cameras={}, cameras={},
) )
+31 -36
View File
@@ -179,9 +179,10 @@ class UnitreeG1(Robot):
else: else:
self.controller = make_locomotion_controller(config.controller) self.controller = make_locomotion_controller(config.controller)
# Token-driven deploy: let a SONIC controller hold a neutral token until the # Token-driven deploy: a SONIC whole-body controller always runs in token
# first real one arrives, then hold the last token between control ticks. # mode -- it holds a neutral token until the first real one arrives, then
if config.sonic_token_action and hasattr(self.controller, "token_mode"): # holds the last token between control ticks.
if hasattr(self.controller, "token_mode"):
self.controller.token_mode = True self.controller.token_mode = True
# Controller thread state # Controller thread state
@@ -199,14 +200,24 @@ class UnitreeG1(Robot):
# Token-mode state: last 64-D SONIC latent token commanded by the policy, # Token-mode state: last 64-D SONIC latent token commanded by the policy,
# echoed back as ``observation.state`` so a token-output VLA closes the loop # echoed back as ``observation.state`` so a token-output VLA closes the loop
# on its own previous token (see ``sonic_token_action``). Seeded to zeros; # on its own previous token. Implicit whenever the SONIC whole-body controller
# the controller's startup blend eases joints in regardless. # is active. Seeded to zeros; the controller's startup blend eases joints in.
self._last_token: np.ndarray | None = None self._last_token: np.ndarray | None = None
if config.sonic_token_action: if self._sonic_token:
from .controllers.sonic_whole_body import TOKEN_DIM from .controllers.sonic_whole_body import TOKEN_DIM
self._last_token = np.zeros(TOKEN_DIM, dtype=np.float32) self._last_token = np.zeros(TOKEN_DIM, dtype=np.float32)
@property
def _sonic_token(self) -> bool:
"""Whether the SONIC whole-body decoder is active.
A SONIC controller consumes a 64-D latent motion token as its action and echoes
the last commanded token as ``observation.state``. Keyed purely off the selected
controller so the token interface is implicit -- no separate config flag.
"""
return self.config.controller == "SonicWholeBodyController"
def _subscribe_lowstate(self): # polls robot state @ 250Hz def _subscribe_lowstate(self): # polls robot state @ 250Hz
while not self._shutdown_event.is_set(): while not self._shutdown_event.is_set():
start_time = time.time() start_time = time.time()
@@ -295,10 +306,10 @@ class UnitreeG1(Robot):
def _token_state_ft(self) -> dict[str, type]: def _token_state_ft(self) -> dict[str, type]:
"""64-D SONIC latent-token proprio state (``motion_token_state.{i}.pos``). """64-D SONIC latent-token proprio state (``motion_token_state.{i}.pos``).
Exposed only in ``sonic_token_action`` mode; aggregated by the rollout into a Exposed only when a SONIC whole-body controller is active; aggregated by the
64-D ``observation.state`` (the last token the policy commanded). rollout into a 64-D ``observation.state`` (the last token the policy commanded).
""" """
if not self.config.sonic_token_action: if not self._sonic_token:
return {} return {}
from .controllers.sonic_whole_body import TOKEN_DIM, token_state_key from .controllers.sonic_whole_body import TOKEN_DIM, token_state_key
@@ -314,18 +325,18 @@ class UnitreeG1(Robot):
@cached_property @cached_property
def action_features(self) -> dict[str, type]: def action_features(self) -> dict[str, type]:
# Role-agnostic: the schema is a pure function of (controller name, # Role-agnostic: the schema is a pure function of the controller name. The thin
# sonic_token_action). The thin client advertises the same schema as the # client advertises the same schema as the onboard robot so the exact same
# onboard robot so the exact same policy output routes straight through. # policy output routes straight through.
# No controller configured at all: raw 29-DoF joint teleop. # No controller configured at all: raw 29-DoF joint teleop.
if self.config.controller is None and not self.config.sonic_token_action: if self.config.controller is None:
return {f"{G1_29_JointIndex(motor).name}.q": float for motor in G1_29_JointIndex} return {f"{G1_29_JointIndex(motor).name}.q": float for motor in G1_29_JointIndex}
# Token-output VLA (SONIC decoder): advertise a 64-D latent-token action space # Token-output VLA (SONIC decoder): advertise a 64-D latent-token action space
# (``motion_token.{i}.pos``) so ``lerobot-rollout`` maps a 64-D policy output # (``motion_token.{i}.pos``) so ``lerobot-rollout`` maps a 64-D policy output
# straight onto the decoder, bypassing the encoder. # straight onto the decoder, bypassing the encoder.
if self.config.sonic_token_action: if self._sonic_token:
from .controllers.sonic_whole_body import TOKEN_DIM, token_action_key from .controllers.sonic_whole_body import TOKEN_DIM, token_action_key
return {token_action_key(i): float for i in range(TOKEN_DIM)} return {token_action_key(i): float for i in range(TOKEN_DIM)}
@@ -465,12 +476,12 @@ class UnitreeG1(Robot):
# 1) Handshake: agree with the server on which controller it will run onboard. # 1) Handshake: agree with the server on which controller it will run onboard.
logger.info( logger.info(
"[client] handshaking with %s:%d (controller=%s, token=%s)...", "[client] handshaking with %s:%d (controller=%s, token=%s)...",
server_ip, HANDSHAKE_PORT, self.config.controller, self.config.sonic_token_action, server_ip, HANDSHAKE_PORT, self.config.controller, self._sonic_token,
) )
self._client_caps = request_controller( self._client_caps = request_controller(
server_ip, server_ip,
self.config.controller, self.config.controller,
sonic_token_action=self.config.sonic_token_action, sonic_token_action=self._sonic_token,
port=HANDSHAKE_PORT, port=HANDSHAKE_PORT,
) )
logger.info("[client] server agreed: %s", self._client_caps) logger.info("[client] server agreed: %s", self._client_caps)
@@ -555,26 +566,10 @@ class UnitreeG1(Robot):
# Initialize DDS channel and simulation environment # Initialize DDS channel and simulation environment
if self.config.is_simulation: if self.config.is_simulation:
from lerobot.envs.utils import ( from lerobot.envs import make_env
_download_hub_file,
_import_hub_module,
_normalize_hub_result,
)
self._ChannelFactoryInitialize(0, "lo") self._ChannelFactoryInitialize(0, "lo")
# Call the hub env's make_env directly so we can disable the offscreen self._env_wrapper = make_env("lerobot/unitree-g1-mujoco", trust_remote_code=True)
# head_camera renderer. We drive image-conditioned policies from external
# camera frames, never the sim's own camera, so building a MuJoCo offscreen
# GL context is pure liability: it
# crashes with "Failed to make the EGL context current" when GLFW/SDL
# already own a context, killing the sim thread and hanging on
# "Waiting for robot state...". publish_images=False -> no renderer.
repo_id, _, local_file, _ = _download_hub_file(
"lerobot/unitree-g1-mujoco", True, None
)
hub_mod = _import_hub_module(local_file, repo_id)
raw = hub_mod.make_env(n_envs=1, use_async_envs=False, publish_images=False, cameras=[])
self._env_wrapper = _normalize_hub_result(raw)
# Extract the actual gym env from the dict structure # Extract the actual gym env from the dict structure
self.sim_env = self._env_wrapper["hub_env"][0].envs[0] self.sim_env = self._env_wrapper["hub_env"][0].envs[0]
elif self.config.onboard: elif self.config.onboard:
@@ -747,7 +742,7 @@ class UnitreeG1(Robot):
# Token mode: echo the last commanded latent token as observation.state so a # Token mode: echo the last commanded latent token as observation.state so a
# token-output VLA closes the loop on its own previous token. # token-output VLA closes the loop on its own previous token.
if self.config.sonic_token_action: if self._sonic_token:
from .controllers.sonic_whole_body import token_state_key from .controllers.sonic_whole_body import token_state_key
token = self._last_token if self._last_token is not None else [] token = self._last_token if self._last_token is not None else []
@@ -769,7 +764,7 @@ class UnitreeG1(Robot):
action_to_publish = action action_to_publish = action
if self.controller is not None: if self.controller is not None:
if self.config.sonic_token_action: if self._sonic_token:
from .controllers.sonic_whole_body import _extract_token_from_action from .controllers.sonic_whole_body import _extract_token_from_action
token = _extract_token_from_action(action) token = _extract_token_from_action(action)