From 288cfc7f8e7dede20c658c81ccc345430958ee83 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Wed, 26 Nov 2025 22:00:17 +0100 Subject: [PATCH] ready to review --- .../robots/unitree_g1/config_unitree_g1.py | 3 +- src/lerobot/robots/unitree_g1/g1_utils.py | 5 +- src/lerobot/robots/unitree_g1/robot_server.py | 54 +++++++--------- src/lerobot/robots/unitree_g1/unitree_g1.py | 26 ++++++-- .../robots/unitree_g1/unitree_sdk2_socket.py | 63 +++++++------------ 5 files changed, 70 insertions(+), 81 deletions(-) diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index ac099d160..f43d4ce79 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/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" diff --git a/src/lerobot/robots/unitree_g1/g1_utils.py b/src/lerobot/robots/unitree_g1/g1_utils.py index cfaba98bf..f2e39d042 100644 --- a/src/lerobot/robots/unitree_g1/g1_utils.py +++ b/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 diff --git a/src/lerobot/robots/unitree_g1/robot_server.py b/src/lerobot/robots/unitree_g1/robot_server.py index 0a3e70feb..2e8680eda 100644 --- a/src/lerobot/robots/unitree_g1/robot_server.py +++ b/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" -kTopicLowState = "rt/lowstate" +kTopicLowCommand_Debug = "rt/lowcmd" #action to robot +kTopicLowState = "rt/lowstate" #observation from robot -LOWCMD_PORT = 6000 # laptop -> robot -LOWSTATE_PORT = 6001 # robot -> laptop +LOWCMD_PORT = 6000 +LOWSTATE_PORT = 6001 -def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float): - """ - read lowstate from dds and push to laptop at ~state_period. - runs in its own thread. - """ +def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float):#read observation from DDS and send to server 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): - """ - read lowcmd from laptop (zmq) and push to dds. - runs in its own thread. - """ +def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC):#send action to robot + 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__": diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index 5df8b0ed4..bac603309 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/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 diff --git a/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py b/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py index d7741cd2a..e69abd520 100644 --- a/src/lerobot/robots/unitree_g1/unitree_sdk2_socket.py +++ b/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 -ROBOT_IP = "172.18.129.215" -LOWCMD_PORT = 6000 # laptop -> robot -LOWSTATE_PORT = 6001 # robot -> laptop +from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config _ctx = None _lowcmd_sock = None _lowstate_sock = None +LOWCMD_PORT = 6000 +LOWSTATE_PORT = 6001 -def ChannelFactoryInitialize(*args, **kwargs): - global _ctx, _lowcmd_sock, _lowstate_sock - if _ctx is not None: - return + +def ChannelFactoryInitialize(*args, **kwargs):#DDS to socket bridge + global _ctx, _lowcmd_sock, _lowstate_sock\ + + # 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.connect(f"tcp://{ROBOT_IP}:{LOWCMD_PORT}") + _lowcmd_sock.setsockopt(zmq.CONFLATE, 1)#keep only last message + _lowcmd_sock.connect(f"tcp://{robot_ip}:{LOWCMD_PORT}") - # lowstate: SUB from robot - _lowstate_sock = _ctx.socket(zmq.SUB) # no topic filtering + # lowstate: robot observation + _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.setsockopt_string(zmq.SUBSCRIBE, "") # subscribe to all + _lowstate_sock.connect(f"tcp://{robot_ip}:{LOWSTATE_PORT}") + _lowstate_sock.setsockopt_string(zmq.SUBSCRIBE, "") -class ChannelPublisher: - # just enough api for your code: __init__, Init, Write +class ChannelPublisher: #send action to robot 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 - payload = pickle.dumps((self.topic, msg)) - _lowcmd_sock.send(payload) + _lowcmd_sock.send(pickle.dumps((self.topic, msg))) -class ChannelSubscriber: - # api: __init__, Init, Read +class ChannelSubscriber: #read observation from robot 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): - """Block until we get a lowstate, optionally with timeout (ms).""" - 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 + def Read(self): + topic, msg = pickle.loads(_lowstate_sock.recv()) return msg