ready to review

src/lerobot/robots/unitree_g1/config_unitree_g1.py

@@ -51,4 +51,5 @@ class UnitreeG1Config(RobotConfig):
 
     control_dt = 1.0 / 250.0 # 250Hz
 
-
+    # socket config for ZMQ bridge
+    robot_ip: str = "172.18.129.215"

src/lerobot/robots/unitree_g1/g1_utils.py

@@ -1,6 +1,5 @@
 from enum import IntEnum
 
-
 class G1_29_JointArmIndex(IntEnum):
     # Left arm
     kLeftShoulderPitch = 15
@@ -20,8 +19,8 @@ class G1_29_JointArmIndex(IntEnum):
     kRightWristPitch = 27
     kRightWristYaw = 28
 
-
 class G1_29_JointIndex(IntEnum):
+
     # Left leg
     kLeftHipPitch = 0
     kLeftHipRoll = 1
@@ -38,7 +37,7 @@ class G1_29_JointIndex(IntEnum):
     kRightAnklePitch = 10
     kRightAnkleRoll = 11
 
-    kWaistYaw = 12  # we're c
+    kWaistYaw = 12
     kWaistRoll = 13
     kWaistPitch = 14
 

src/lerobot/robots/unitree_g1/robot_server.py

@@ -9,29 +9,25 @@ from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelPublish
 from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as hg_LowCmd, LowState_ as hg_LowState
 from unitree_sdk2py.utils.crc import CRC
 
-kTopicLowCommand_Debug = "rt/lowcmd"
+kTopicLowCommand_Debug = "rt/lowcmd" #action to robot
-kTopicLowState = "rt/lowstate"
+kTopicLowState = "rt/lowstate" #observation from robot
 
-LOWCMD_PORT = 6000  # laptop -> robot
+LOWCMD_PORT = 6000
-LOWSTATE_PORT = 6001  # robot -> laptop
+LOWSTATE_PORT = 6001
 
 
-def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float):
+def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float):#read observation from DDS and send to server
-    """
-    read lowstate from dds and push to laptop at ~state_period.
-    runs in its own thread.
-    """
     last_state_time = 0.0
 
     while True:
-        # read from dds (blocking)
+        # read from DDS
         msg = lowstate_sub.Read()
         if msg is None:
             continue
 
         now = time.time()
         # optional downsampling (if robot dds rate > state_period)
-        if now - last_state_time >= state_period:
+        if now - last_state_time >= state_period: 
             payload = pickle.dumps((kTopicLowState, msg), protocol=pickle.HIGHEST_PROTOCOL)
             try:
                 lowstate_sock.send(payload, zmq.NOBLOCK)
@@ -41,15 +37,11 @@ def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float):
             last_state_time = now
 
 
-def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC):
+def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC):#send action to robot
-    """
+
-    read lowcmd from laptop (zmq) and push to dds.
-    runs in its own thread.
-    """
     while True:
-        # blocking wait for commands from laptop
         payload = lowcmd_sock.recv()
-        topic, cmd = pickle.loads(payload)  # cmd is hg_LowCmd
+        topic, cmd = pickle.loads(payload)
 
         # recompute crc just in case
         cmd.crc = crc.Crc(cmd)
@@ -57,15 +49,15 @@ def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC):
         if topic == kTopicLowCommand_Debug:
             lowcmd_pub_debug.Write(cmd)
         else:
-            # ignore unknown topics
             pass
+        
 
 
 def main():
-    # dds init
+    # initialize DDS
     ChannelFactoryInitialize(0)
 
-    # acquire motion mode on the robot
+    # stop all active publishers on the robot
     msc = MotionSwitcherClient()
     msc.SetTimeout(5.0)
     msc.Init()
@@ -78,50 +70,50 @@ def main():
 
     crc = CRC()
 
-    # dds publishers / subscriber
+    # initialize DDS publisher
     lowcmd_pub_debug = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd)
     lowcmd_pub_debug.Init()
-
+    
+    # initialize DDS subscriber
     lowstate_sub = ChannelSubscriber(kTopicLowState, hg_LowState)
     lowstate_sub.Init()
 
-    # zmq setup
+    # initialize ZMQ
     ctx = zmq.Context.instance()
 
-    # commands from laptop
+    # send action to robot
     lowcmd_sock = ctx.socket(zmq.PULL)
     lowcmd_sock.bind(f"tcp://0.0.0.0:{LOWCMD_PORT}")
 
-    # state to laptop
+    # send observation to server
     lowstate_sock = ctx.socket(zmq.PUB)
     lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}")
 
     state_period = 0.002  # ~500 hz
 
-    # start threads
+    # start observation forwarding thread
     t_state = threading.Thread(
         target=state_forward_loop,
         args=(lowstate_sub, lowstate_sock, state_period),
         daemon=True,
     )
+    t_state.start()
+
+    # start action forwarding thread
     t_cmd = threading.Thread(
         target=cmd_forward_loop,
         args=(lowcmd_sock, lowcmd_pub_debug, crc),
         daemon=True,
     )
-
-    t_state.start()
     t_cmd.start()
 
     print("bridge running (lowstate -> zmq, lowcmd -> dds)")
-
     # keep main thread alive so daemon threads don’t exit
     try:
         while True:
             time.sleep(1.0)
     except KeyboardInterrupt:
         print("shutting down bridge...")
-        # sockets/context will be cleaned up on process exit
 
 
 if __name__ == "__main__":

src/lerobot/robots/unitree_g1/unitree_g1.py

@@ -1,3 +1,19 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
 import json
 import logging
 import struct
@@ -123,7 +139,10 @@ class UnitreeG1(Robot):
         self.subscribe_thread.daemon = True
         self.subscribe_thread.start()
 
-
+        while not self.is_connected:
+            time.sleep(0.1)
+            logger.warning("[UnitreeG1] Waiting to connect to robot...")
+        logger.warning("[UnitreeG1] Connected to robot.")
 
         # initialize hg's lowcmd msg
         self.crc = CRC()
@@ -190,11 +209,6 @@ class UnitreeG1(Robot):
     def connect(self, calibrate: bool = True) -> None: #connect to DDS
         ChannelFactoryInitialize(0)
 
-        while not self.lowstate_buffer.GetData():
-            time.sleep(0.1)
-            logger.warning("[UnitreeG1] Waiting to subscribe dds...")
-        logger.warning("[UnitreeG1] Subscribe dds ok.")
-
     def disconnect(self):
         pass
 

src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py

@@ -1,55 +1,50 @@
-# unitree_sdk2_socket.py
 import pickle
-
 import zmq
 
-# you can tune these or read from env
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
-ROBOT_IP = "172.18.129.215"
-LOWCMD_PORT = 6000  # laptop -> robot
-LOWSTATE_PORT = 6001  # robot -> laptop
 
 _ctx = None
 _lowcmd_sock = None
 _lowstate_sock = None
 
+LOWCMD_PORT = 6000
+LOWSTATE_PORT = 6001
 
-def ChannelFactoryInitialize(*args, **kwargs):
+
-    global _ctx, _lowcmd_sock, _lowstate_sock
+def ChannelFactoryInitialize(*args, **kwargs):#DDS to socket bridge
-    if _ctx is not None:
+    global _ctx, _lowcmd_sock, _lowstate_sock\
-        return
+    
+    # read socket config
+    config = UnitreeG1Config()
+    robot_ip = config.robot_ip
+    
     _ctx = zmq.Context.instance()
 
-    # lowcmd: PUSH from laptop to robot
+    # lowcmd: robot action
     _lowcmd_sock = _ctx.socket(zmq.PUSH)
-    _lowcmd_sock.setsockopt(zmq.CONFLATE, 1)
+    _lowcmd_sock.setsockopt(zmq.CONFLATE, 1)#keep only last message
-    _lowcmd_sock.connect(f"tcp://{ROBOT_IP}:{LOWCMD_PORT}")
+    _lowcmd_sock.connect(f"tcp://{robot_ip}:{LOWCMD_PORT}")
 
-    # lowstate: SUB from robot
+    # lowstate: robot observation
-    _lowstate_sock = _ctx.socket(zmq.SUB)  # no topic filtering
+    _lowstate_sock = _ctx.socket(zmq.SUB)
     _lowstate_sock.setsockopt(zmq.CONFLATE, 1)  # keep only last message
-    _lowstate_sock.connect(f"tcp://{ROBOT_IP}:{LOWSTATE_PORT}")
+    _lowstate_sock.connect(f"tcp://{robot_ip}:{LOWSTATE_PORT}")
-    _lowstate_sock.setsockopt_string(zmq.SUBSCRIBE, "")  # subscribe to all
+    _lowstate_sock.setsockopt_string(zmq.SUBSCRIBE, "")
 
 
-class ChannelPublisher:
+class ChannelPublisher: #send action to robot
-    # just enough api for your code: __init__, Init, Write
     def __init__(self, topic, msg_type):
-        # we ignore topic/msg_type, the bridge only supports the topics you use
         self.topic = topic
         self.msg_type = msg_type
 
     def Init(self):
-        # nothing to do, sockets are global
         pass
 
     def Write(self, msg):
-        # msg is hg_LowCmd_ instance – we just pickle it
+        _lowcmd_sock.send(pickle.dumps((self.topic, msg)))
-        payload = pickle.dumps((self.topic, msg))
-        _lowcmd_sock.send(payload)
 
 
-class ChannelSubscriber:
+class ChannelSubscriber: #read observation from robot
-    # api: __init__, Init, Read
     def __init__(self, topic, msg_type):
         self.topic = topic
         self.msg_type = msg_type
@@ -57,18 +52,6 @@ class ChannelSubscriber:
     def Init(self):
         pass
 
-    def Read(self, timeout_ms=None):
+    def Read(self):
-        """Block until we get a lowstate, optionally with timeout (ms)."""
+        topic, msg = pickle.loads(_lowstate_sock.recv())
-        if timeout_ms is None:
-            payload = _lowstate_sock.recv()
-        else:
-            poller = zmq.Poller()
-            poller.register(_lowstate_sock, zmq.POLLIN)
-            events = dict(poller.poll(timeout_ms))
-            if _lowstate_sock not in events:
-                return None
-            payload = _lowstate_sock.recv()
-
-        topic, msg = pickle.loads(payload)
-        # you can assert topic == self.topic, but not necessary if you only use one
         return msg