admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-06-26 04:37:01 +00:00
Code Issues Packages Projects Releases Wiki Activity

ready to review

Browse Source
This commit is contained in:
Martino Russi
2025-11-26 22:00:17 +01:00
parent 3ec332fabc
commit 288cfc7f8e
5 changed files with 70 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/robots/unitree_g1/config_unitree_g1.py
+2 -1
View File
@@ -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
+2 -3
View File
@@ -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
+23 -31
View File
@@ -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__":
src/lerobot/robots/unitree_g1/unitree_g1.py
+20 -6
View File
@@ -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
+23 -40
View File
@@ -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