镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
snapshot: sync mint main from current workspace 2026-03-03 16:32:56 +0800
这个提交包含在:
@ -0,0 +1,168 @@
|
||||
#!/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 base64
|
||||
import json
|
||||
from typing import Any
|
||||
|
||||
import zmq
|
||||
|
||||
from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
|
||||
|
||||
_ctx: zmq.Context | None = None
|
||||
_lowcmd_sock: zmq.Socket | None = None
|
||||
_lowstate_sock: zmq.Socket | None = None
|
||||
|
||||
LOWCMD_PORT = 6000
|
||||
LOWSTATE_PORT = 6001
|
||||
|
||||
# DDS topic names follow Unitree SDK naming conventions
|
||||
# ruff: noqa: N816
|
||||
kTopicLowCommand_Debug = "rt/lowcmd"
|
||||
|
||||
|
||||
class LowStateMsg:
|
||||
"""
|
||||
Wrapper class that mimics the Unitree SDK LowState_ message structure.
|
||||
|
||||
Reconstructs the message from deserialized JSON data to maintain
|
||||
compatibility with existing code that expects SDK message objects.
|
||||
"""
|
||||
|
||||
class MotorState:
|
||||
"""Motor state data for a single joint."""
|
||||
|
||||
def __init__(self, data: dict[str, Any]) -> None:
|
||||
self.q: float = data.get("q", 0.0)
|
||||
self.dq: float = data.get("dq", 0.0)
|
||||
self.tau_est: float = data.get("tau_est", 0.0)
|
||||
self.temperature: float = data.get("temperature", 0.0)
|
||||
|
||||
class IMUState:
|
||||
"""IMU sensor data."""
|
||||
|
||||
def __init__(self, data: dict[str, Any]) -> None:
|
||||
self.quaternion: list[float] = data.get("quaternion", [1.0, 0.0, 0.0, 0.0])
|
||||
self.gyroscope: list[float] = data.get("gyroscope", [0.0, 0.0, 0.0])
|
||||
self.accelerometer: list[float] = data.get("accelerometer", [0.0, 0.0, 0.0])
|
||||
self.rpy: list[float] = data.get("rpy", [0.0, 0.0, 0.0])
|
||||
self.temperature: float = data.get("temperature", 0.0)
|
||||
|
||||
def __init__(self, data: dict[str, Any]) -> None:
|
||||
"""Initialize from deserialized JSON data."""
|
||||
self.motor_state = [self.MotorState(m) for m in data.get("motor_state", [])]
|
||||
self.imu_state = self.IMUState(data.get("imu_state", {}))
|
||||
# Decode base64-encoded wireless_remote bytes
|
||||
wireless_b64 = data.get("wireless_remote", "")
|
||||
self.wireless_remote: bytes = base64.b64decode(wireless_b64) if wireless_b64 else b""
|
||||
self.mode_machine: int = data.get("mode_machine", 0)
|
||||
|
||||
|
||||
def lowcmd_to_dict(topic: str, msg: Any) -> dict[str, Any]:
|
||||
"""Convert LowCmd message to a JSON-serializable dictionary."""
|
||||
motor_cmds = []
|
||||
# Iterate over all motor commands in the message
|
||||
for i in range(len(msg.motor_cmd)):
|
||||
motor_cmds.append(
|
||||
{
|
||||
"mode": int(msg.motor_cmd[i].mode),
|
||||
"q": float(msg.motor_cmd[i].q),
|
||||
"dq": float(msg.motor_cmd[i].dq),
|
||||
"kp": float(msg.motor_cmd[i].kp),
|
||||
"kd": float(msg.motor_cmd[i].kd),
|
||||
"tau": float(msg.motor_cmd[i].tau),
|
||||
}
|
||||
)
|
||||
|
||||
return {
|
||||
"topic": topic,
|
||||
"data": {
|
||||
"mode_pr": int(msg.mode_pr),
|
||||
"mode_machine": int(msg.mode_machine),
|
||||
"motor_cmd": motor_cmds,
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
def ChannelFactoryInitialize(*args: Any, **kwargs: Any) -> None: # noqa: N802
|
||||
"""
|
||||
Initialize ZMQ sockets for robot communication.
|
||||
|
||||
This function mimics the Unitree SDK's ChannelFactoryInitialize but uses
|
||||
ZMQ sockets to connect to the robot server bridge instead of DDS.
|
||||
"""
|
||||
global _ctx, _lowcmd_sock, _lowstate_sock
|
||||
|
||||
# read socket config
|
||||
config = UnitreeG1Config()
|
||||
robot_ip = config.robot_ip
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
_ctx = ctx
|
||||
|
||||
# lowcmd: send robot commands
|
||||
lowcmd_sock = ctx.socket(zmq.PUSH)
|
||||
lowcmd_sock.setsockopt(zmq.CONFLATE, 1) # keep only last message
|
||||
lowcmd_sock.connect(f"tcp://{robot_ip}:{LOWCMD_PORT}")
|
||||
_lowcmd_sock = lowcmd_sock
|
||||
|
||||
# lowstate: receive robot observations
|
||||
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, "")
|
||||
_lowstate_sock = lowstate_sock
|
||||
|
||||
|
||||
class ChannelPublisher:
|
||||
"""ZMQ-based publisher that sends commands to the robot server."""
|
||||
|
||||
def __init__(self, topic: str, msg_type: type) -> None:
|
||||
self.topic = topic
|
||||
self.msg_type = msg_type
|
||||
|
||||
def Init(self) -> None: # noqa: N802
|
||||
"""Initialize the publisher (no-op for ZMQ)."""
|
||||
pass
|
||||
|
||||
def Write(self, msg: Any) -> None: # noqa: N802
|
||||
"""Serialize and send a command message to the robot."""
|
||||
if _lowcmd_sock is None:
|
||||
raise RuntimeError("ChannelFactoryInitialize must be called first")
|
||||
|
||||
payload = json.dumps(lowcmd_to_dict(self.topic, msg)).encode("utf-8")
|
||||
_lowcmd_sock.send(payload)
|
||||
|
||||
|
||||
class ChannelSubscriber:
|
||||
"""ZMQ-based subscriber that receives state from the robot server."""
|
||||
|
||||
def __init__(self, topic: str, msg_type: type) -> None:
|
||||
self.topic = topic
|
||||
self.msg_type = msg_type
|
||||
|
||||
def Init(self) -> None: # noqa: N802
|
||||
"""Initialize the subscriber (no-op for ZMQ)."""
|
||||
pass
|
||||
|
||||
def Read(self) -> LowStateMsg: # noqa: N802
|
||||
"""Receive and deserialize a state message from the robot."""
|
||||
if _lowstate_sock is None:
|
||||
raise RuntimeError("ChannelFactoryInitialize must be called first")
|
||||
|
||||
payload = _lowstate_sock.recv()
|
||||
msg_dict = json.loads(payload.decode("utf-8"))
|
||||
return LowStateMsg(msg_dict.get("data", {}))
|
||||
在新工单中引用
屏蔽一个用户