宇树G1-D机器人 VLA推断控制系统开发实录
本文记录了在宇树G1-D人形机器人上从零部署VLA（Vision-Language-Action）推断控制系统的完整过程，涵盖Unitree SDK话题体系、双进程架构、实时控制、EMA滤波等核心技术点。
目录
- 项目概述
- [Unitree SDK 话题体系](#Unitree SDK 话题体系)
- 双进程架构：解决CycloneDDS冲突
- EMA低通滤波：解决控制频率不匹配
- 保持锁机制：解决控制空窗期
- 系统架构总览
- 完整代码
- 总结
一、项目概述
1.1 硬件平台
| 组件 | 规格 |
|---|---|
| 机器人本体 | 宇树 G1-D 人形机器人 |
| 末端执行器 | Dex3 灵巧手（左右各7自由度） |
| 传感器 | 头部广角相机 + 左右腕部相机，共3路 |
| 推理服务器 | RTX5090 |
1.2 软件栈
| 层级 | 技术 |
|---|---|
| VLA推理框架 | NVIDIA Isaac GR00T |
| 机器人SDK | Unitree SDK2 Python |
| 通信中间件 | CycloneDDS |
| 机器人操作系统 | Ubuntu 20.04 + ROS2 Foxy |
| 底层控制通信 | DDS（rt/lowcmd、rt/dex3） |
| 推理通信 | TCP + msgpack |
二、Unitree SDK 话题体系
这是整个系统最基础的知识点，理解了话题体系才能理解后续所有控制逻辑。
2.1 两条控制总线
G1-D 的控制指令走两条完全独立的 DDS 话题总线：
┌──────────────────────────────────────────────────────────┐
│ 话题：rt/lowcmd │
│ 消息类型：unitree_hg_msg_dds__LowCmd_ │
│ 方向：发送（控制） │
│ │
│ 控制对象：腿部 + 腰部 + 手臂 = 29个电机 │
│ DDS电机槽位编号： │
│ [0-5] 左腿 6个电机 │
│ [6-11] 右腿 6个电机 │
│ [12-14] 腰部 3个电机 ← waist_yaw / waist_roll / waist_pitch │
│ [15-21] 左臂 7个电机 ← shoulder(3) + elbow(1) + wrist(3) │
│ [22-28] 右臂 7个电机 ← shoulder(3) + elbow(1) + wrist(3) │
│ │
│ 注意：不包含手指，手指走独立话题 │
│ 注意：每帧必须附带CRC校验，否则电机不执行 │
│ 注意：有超时机制，停止发送后电机会失控松软 │
└──────────────────────────────────────────────────────────┘
┌──────────────────────────────────────────────────────────┐
│ 话题：rt/lowstate │
│ 消息类型：LowState_ │
│ 方向：接收（状态读取） │
│ │
│ 内容：29个电机的实际关节角度、速度、力矩 │
│ 用途：读取当前关节角度，作为插值起点和EMA跟踪的参考 │
└──────────────────────────────────────────────────────────┘
┌──────────────────────────────────────────────────────────┐
│ 话题：rt/dex3/left/cmd / rt/dex3/right/cmd │
│ 消息类型：unitree_hg_msg_dds__HandCmd_ │
│ 方向：发送（控制） │
│ │
│ 控制对象：左/右 Dex3 灵巧手，各7个电机 │
│ 左手硬件槽位：[T0, T1, T2, M0, M1, I0, I1] │
│ 右手硬件槽位：[T0, T1, T2, I0, I1, M0, M1] ← M/I相反！ │
│ │
│ 注意：mode字节构造方式与LowCmd不同（见下方） │
│ 注意：同样有超时机制，需持续发送 │
└──────────────────────────────────────────────────────────┘2.2 LowCmd 消息结构
python
# rt/lowcmd 每个电机一组参数，共29组
cmd.motor_cmd[i].mode = 1 # 1=使能，0=失能
cmd.motor_cmd[i].q = 0.0 # 目标关节角度（rad）
cmd.motor_cmd[i].dq = 0.0 # 目标关节速度（rad/s），纯位置控制时填0
cmd.motor_cmd[i].tau = 0.0 # 前馈力矩（N·m），纯位置控制时填0
cmd.motor_cmd[i].kp = 40.0 # 位置增益
cmd.motor_cmd[i].kd = 1.0 # 速度增益
# 实际输出力矩 = kp×(q_des-q_cur) + kd×(dq_des-dq_cur) + tau
# 本项目只用位置模式：设q和kp/kd，其余填0
# 每帧必须计算CRC，否则电机拒绝执行
cmd.crc = crc.Crc(cmd)
pub.Write(cmd)2.3 HandCmd 消息结构
python
# rt/dex3/*/cmd 每个电机一组参数，共7组
# mode字节与LowCmd不同，需要用RIS_Mode_t位域构造
def ris_mode_byte(motor_id, status=0x01, timeout=0):
v = (motor_id & 0x0F) # bits 0-3：电机ID
v |= (status & 0x07) << 4 # bits 4-6：状态（0x01=使能）
v |= (timeout & 0x01) << 7 # bit 7：超时标志
return v
msg.motor_cmd[i].mode = ris_mode_byte(i, status=0x01, timeout=0)
msg.motor_cmd[i].q = 0.0 # 目标角度（rad）
msg.motor_cmd[i].dq = 0.0
msg.motor_cmd[i].tau = 0.0
msg.motor_cmd[i].kp = 1.5
msg.motor_cmd[i].kd = 0.1
# HandCmd 不需要CRC，直接发送
pub.Write(msg)2.4 右手槽位重映射
Dex3 灵巧手左右手的 middle 和 index 关节硬件槽位顺序相反，这是一个容易踩的坑：
训练数据关节定义（左右手相同）：
[T0, T1, T2, M0, M1, I0, I1]
拇指 中指 食指
Dex3 硬件实际槽位：
左手：[T0, T1, T2, M0, M1, I0, I1] ← 与训练数据一致 ✅
右手：[T0, T1, T2, I0, I1, M0, M1] ← 食指和中指顺序相反！❌
发送给右手时必须重映射：
q_hardware = [q[0], q[1], q[2], q[5], q[6], q[3], q[4]]
# 把 [T0,T1,T2,M0,M1,I0,I1] → [T0,T1,T2,I0,I1,M0,M1]2.5 关节索引体系：两套并行编号
G1-D 开发中存在两套关节编号，容易混淆：
ROS2 /joint_states 话题顺序（完整43个关节）：
[0-11] 腿部12个（left/right hip_pitch/roll/yaw + knee + ankle_pitch/roll）
[12-14] 腰部3个 （waist_yaw / waist_roll / waist_pitch）
[15-21] 左臂7个 （shoulder_pitch/roll/yaw + elbow + wrist_roll/pitch/yaw）
[22-28] 右臂7个
[29-35] 左手7个 （thumb_0/1/2 + middle_0/1 + index_0/1）
[36-42] 右手7个rt/lowcmd DDS槽位（物理电机编号，29个，不含手指）：
[0-5] 左腿6个
[6-11] 右腿6个
[12-14] 腰部3个 ← waist_yaw / waist_roll / waist_pitch
[15-21] 左臂7个
[22-28] 右臂7个手指不在29个电机里，走独立话题 rt/dex3/*/cmd。
VLA训练数据只用上半身31维（去掉腿部，VLA不控制腿）：
waist(3) + left_arm(7) + left_hand(7) + right_arm(7) + right_hand(7) = 31维开发时需要维护一张映射表，把31维VLA输出拆分发给正确的话题：
python
# 31维中各部分在训练数据顺序中的索引
UPPER_BODY_STATE_IDX = [0,1,2, 3,4,5,6,7,8,9, 17,18,19,20,21,22,23] # 17维 → rt/lowcmd
LEFT_HAND_STATE_IDX = [10,11,12,13,14,15,16] # 7维 → rt/dex3/left/cmd
RIGHT_HAND_STATE_IDX = [24,25,26,27,28,29,30] # 7维 → rt/dex3/right/cmd
# 17维上半身 → 对应的DDS电机槽位（一一对应）
UPPER_BODY_DDS_IDX = [12,13,14, # waist → 槽位12-14
15,16,17,18,19,20,21, # left arm → 槽位15-21
22,23,24,25,26,27,28] # right arm → 槽位22-282.6 电机控制频率要求
rt/lowcmd：有超时机制，停止发送后电机会在约200ms内失控松软
→ 必须持续发送，本项目用 500Hz（每2ms一帧）
rt/dex3/*/cmd：同样有超时机制
→ 必须持续发送，本项目用 200Hz（每5ms一帧）
这是 _hold_loop 和 HandController._loop 存在的根本原因：
持续发送不是为了精度，而是为了让电机不超时松软三、双进程架构：解决CycloneDDS冲突
3.1 问题现象
[ChannelFactory] create domain error.
Occurred upon initialisation of a cyclonedds.domain.Domain
Exception: channel factory init error.3.2 根本原因
ROS2 的 RMW（Robot Middleware）层通过 rmw_cyclonedds 使用 CycloneDDS，Unitree SDK 也直接调用 CycloneDDS API。两者都要初始化 Domain 0，但 CycloneDDS 内部是全局单例，同一进程只能初始化一次：
ROS2通信栈：
rclpy → rcl → rmw_cyclonedds → CycloneDDS API
Unitree SDK通信栈：
ChannelFactory → CycloneDDS API（直接调用，无RMW层）
同一进程内：
rclpy.init() → CycloneDDS.init(domain=0) → 成功 ✅
ChannelFactoryInitialize(0) → CycloneDDS.init(domain=0) → 失败 ❌多线程也无法解决：线程共享进程内存，全局单例依然只有一份。
3.3 解决方案：独立进程
进程有独立的虚拟地址空间，每个进程的全局变量互不影响：
ROS2子进程：独立内存 → 自己的CycloneDDS实例 → rclpy.init() 成功
主进程： 独立内存 → 自己的CycloneDDS实例 → Unitree SDK 成功
两个实例各自通信，互不干扰两个进程之间通过 multiprocessing.Queue 传递传感器数据，MPQueue 底层用操作系统管道实现，自动处理同步，对 Python 最友好。
关键细节：import rclpy 必须放在子进程函数内部
python
# ❌ 顶层import：主进程启动时就加载CycloneDDS
import rclpy
# ✅ 函数内import：只有子进程执行时才加载，主进程不受影响
def ros2_sensor_process(data_queue, stop_event):
import rclpy # 只在子进程里执行
import threading
rclpy.init()
...3.4 为什么 ChannelFactoryInitialize 要指定网卡
python
ChannelFactoryInitialize(0, "eth0")
# ↑ ↑
# domain 网卡名（CycloneDDS要求）这是 CycloneDDS 的要求，Unitree SDK 只是透传参数。一台机器可能有多块网卡（eth0连机器人、wlan0是WiFi、lo是回环、docker0是虚拟网卡），不指定时 CycloneDDS 可能从错误的网卡广播 DDS 发现包，电机控制器收不到指令。
四、EMA低通滤波：解决控制频率不匹配
4.1 问题根因
VLA模型推断频率：15Hz → 每66ms才输出一个新目标
手臂控制频率： 500Hz → 每2ms发一帧
夹爪控制频率： 200Hz → 每5ms发一帧
不加滤波的效果：
t=0ms 新目标到来，目标值跳变
t=0.1ms 电机立刻到达新位置 ← 看起来"一卡"
t=66ms 下一个目标到来，再卡一次
→ 手臂卡顿，夹爪在限位边界颤抖4.2 EMA 公式
EMA（指数移动平均）本质是一阶IIR低通滤波器：
filtered(t) = α × target(t) + (1-α) × filtered(t-1)
截止频率：fc ≈ α × fs / (2π)
α越小 → 截止频率越低 → 滤波越强 → 动作越平滑但响应越慢4.3 参数选择
手臂（500Hz，α=0.0075）：
每步移动目标距离的0.75%
66ms内（一个VLA帧间隔）：500Hz × 66ms = 33步，完成约22%过渡
约267步（534ms）完全到位
夹爪（200Hz，α=0.05）：
每步移动5%
66ms内：200Hz × 66ms = 13步，完成约48%过渡
约45步（225ms）完全到位4.4 EMA 如何解决夹爪颤抖
夹爪颤抖的直接原因：bag录制数据中某些关节值在硬件限位边界来回跳动（传感器噪声±0.001rad），被 clip 到限位后每帧在限位值和噪声值之间交替，电机来回微动。
加 EMA 后，即使目标每帧在0和0.001之间跳变，filtered 每帧变化不超过 0.05 × 0.001 = 0.00005 rad，远低于电机能感知的阈值，颤抖消除。
五、保持锁机制：解决控制空窗期
5.1 问题背景
rt/lowcmd 有超时机制，停止发送后电机约200ms内失控松软（手臂直接掉落）。启动流程中存在多个危险空窗期：
Stage1归零（3s）结束
↓ ← 空窗！不发指令 → 手臂掉落
等待VLA第一帧推断成功（数秒）
↓ ← VLA第一帧输出不可信，不能直接执行
VLA正常控制5.2 解决方案
用 _hold_pose 标志位 + _hold_loop 后台线程消除所有空窗期：
Stage1结束瞬间
→ 立刻启动_hold_loop后台线程（500Hz持续发送，电机不超时）
→ _hold_pose=True，set_upper_body_target()静默丢弃外部输入
move_to_first_frame执行
→ 直接写_target_upper17（绕过_hold_pose）
→ _hold_loop跟随新目标，持续发送，过渡平滑
到达起始姿态，等待回车
→ _hold_loop继续500Hz发送，机器人保持不动
VLA第一帧推断成功 → release_hold()
→ _hold_pose = False
→ _hold_loop检测到后退出，直接调用_vla_loop()
→ 无缝衔接，零间隙，电机全程不超时5.3 状态机
init_blocking() → 当前→零位（3s）
↓ 立刻
_hold_loop（500Hz持续发，_hold_pose=True）
↓
move_to_first_frame（3s，更新_target_upper17）
↓
等待回车（保持起始姿态）
↓ release_hold()
_vla_loop（500Hz EMA跟踪VLA目标）六、系统架构总览
6.1 完整数据流
┌──────────────────────────────────────────────────────────────────┐
│ ROS2 子进程（独立进程，CycloneDDS隔离） │
│ /head_camera_wide/compressed ┐ │
│ /left_arm_camera/compressed ├─ 30Hz 打包 → MPQueue │
│ /right_arm_camera/compressed │ │
│ /joint_states（31维） ┘ │
└──────────────────────────────────────────────────────────────────┘
↓ multiprocessing.Queue
┌──────────────────────────────────────────────────────────────────┐
│ 主进程（Unitree DDS独占） │
│ DataBuffer（只保留最新帧） │
│ ↓ │
│ G1DVLAController │
│ joint_state[:30] + 图像 → msgpack → TCP → 推理服务器 │
│ 接收 action_chunk(16×30) → cmd_queue │
│ ↓ 15Hz执行 │
│ _apply(action31) │
│ ├─ arm.set_upper_body_target(17维) │
│ │ 500Hz EMA(α=0.0075) → rt/lowcmd（29个电机） │
│ ├─ left_hand.set_target_from_state(7维) │
│ │ 200Hz EMA(α=0.05) → rt/dex3/left/cmd（7个电机） │
│ └─ right_hand.set_target_from_state(7维，含M/I重映射) │
│ 200Hz EMA(α=0.05) → rt/dex3/right/cmd（7个电机） │
└──────────────────────────────────────────────────────────────────┘
↓ TCP（4字节大端长度头 + msgpack body）
┌──────────────────────────────────────────────────────────────────┐
│ 推理服务器 │
│ Isaac GR00T VLA模型 │
│ 输入：3路图像 + 30维关节状态 │
│ 输出：action_chunk(16×30) │
└──────────────────────────────────────────────────────────────────┘6.2 控制时序
t=0s Stage1（29个电机，当前→零位，3s线性插值）
t=3s _hold_loop启动（500Hz发腰+臂17个槽位，腿部保0）
t=3s move_to_first_frame（零位→起始姿态，3s插值）
t=6s 到达起始姿态，等待回车
[回车] flush积压数据，等待最新传感器帧
_infer()：推断 → 第一帧成功 → release_hold()
_vla_loop接管，15Hz执行action_chunk，500Hz/200Hz EMA跟踪6.3 问题与解决方案汇总
| 问题现象 | 根本原因 | 解决方案 |
|---|---|---|
| CycloneDDS初始化失败 | ROS2和Unitree SDK共用CycloneDDS单例 | 双进程，进程内存隔离 |
| Stage1后手臂掉落 | lowcmd超时，空窗期电机失控 | _hold_loop Stage1结束瞬间无缝接管 |
| 手臂动作卡顿 | 15Hz推断 vs 500Hz控制频率不匹配 | EMA α=0.0075 |
| 夹爪关节颤抖 | 传感器噪声在限位边界反复clip | EMA α=0.05 |
| 右手动作方向错误 | Dex3右手M/I槽位顺序与左手相反 | 硬件槽位重映射 |
| 模型Shape不匹配 | 训练30维，代码发31维 | 发送截取前30维，接收后补0 |
七、完整代码
python
#!/usr/bin/env python3
"""
G1-D VLA 推断控制器（双进程版）
Unitree SDK 话题说明：
rt/lowcmd → unitree_hg_msg_dds__LowCmd_ 发送，控制29个电机（腿+腰+臂），需CRC
rt/lowstate → LowState_ 接收，读取29个电机实际状态
rt/dex3/left/cmd → unitree_hg_msg_dds__HandCmd_ 发送，控制左手7个电机
rt/dex3/right/cmd → unitree_hg_msg_dds__HandCmd_ 发送，控制右手7个电机（M/I重映射）
DDS电机槽位（rt/lowcmd）：
[0-5]左腿 [6-11]右腿 [12-14]腰部 [15-21]左臂 [22-28]右臂
右手Dex3槽位与左手相反（M/I互换），发送前需重映射：
state顺序 [T0,T1,T2,M0,M1,I0,I1] → 硬件槽位 [T0,T1,T2,I0,I1,M0,M1]
执行顺序：
1. 启动ROS2子进程（相机 + /joint_states → MPQueue）
2. Unitree DDS初始化
3. Stage1（3s）：当前姿态→零位（29个电机全部归零）
4. _hold_loop：500Hz持续发，等待release_hold()
5. move_to_first_frame（3s）：零位→bag起始姿态
6. 保持起始姿态，等待人为回车确认
7. 回车后清空积压数据，进入VLA推断循环
8. 第一帧推断成功 → release_hold → _vla_loop无缝接管
"""
import time
import threading
import queue
import socket
import struct
import argparse
from datetime import datetime
from typing import Optional
from multiprocessing import Process, Queue as MPQueue, Event as MPEvent
import numpy as np
import msgpack
from unitree_sdk2py.core.channel import (
ChannelFactoryInitialize, ChannelPublisher, ChannelSubscriber,
)
from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
from unitree_sdk2py.idl.default import (
unitree_hg_msg_dds__LowCmd_, unitree_hg_msg_dds__HandCmd_,
)
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_, HandCmd_
from unitree_sdk2py.utils.crc import CRC
# ═══════════════════════════════════════════════════════════════════════════════
# bag 第一帧起始姿态（从录制数据第一帧提取，换新任务时更新）
# ═══════════════════════════════════════════════════════════════════════════════
# 上半身17维：[waist(3), left_arm(7), right_arm(7)]
FIRST_FRAME_UPPER17 = np.array([
-0.0718, 0.0614, 0.0000, # waist_yaw, waist_roll, waist_pitch
-0.1022, 0.0902, 0.0571, # left shoulder pitch/roll/yaw
0.9257, 0.1105, -1.1413, # left elbow, wrist_roll, wrist_pitch
0.0426, # left wrist_yaw
-0.0388, -0.2589, -0.0906, # right shoulder pitch/roll/yaw
0.8096, 0.0210, -0.9294, # right elbow, wrist_roll, wrist_pitch
0.2306, # right wrist_yaw
], dtype=np.float64)
# 左手7维 [T0,T1,T2,M0,M1,I0,I1]（STATE_JOINT_NAMES顺序）
FIRST_FRAME_LEFT_HAND = np.array([
-0.8623, 0.9210, 0.5668,
-0.8767, -0.0273,
-0.2021, -0.8948,
], dtype=np.float64)
# 右手7维 [T0,T1,T2,M0,M1,I0,I1]（STATE_JOINT_NAMES顺序，内部自动重映射）
FIRST_FRAME_RIGHT_HAND = np.array([
-0.6698, -0.9210, -0.6473,
0.6638, 0.0041,
-0.0010, 0.7469,
], dtype=np.float64)
# ═══════════════════════════════════════════════════════════════════════════════
# 常量
# ═══════════════════════════════════════════════════════════════════════════════
# VLA训练数据的31维关节顺序（上半身，不含腿部）
STATE_JOINT_NAMES = [
"waist_yaw", "waist_roll", "waist_pitch",
"left_shoulder_pitch", "left_shoulder_roll", "left_shoulder_yaw",
"left_elbow", "left_wrist_roll", "left_wrist_pitch", "left_wrist_yaw",
"left_hand_thumb_0", "left_hand_thumb_1", "left_hand_thumb_2",
"left_hand_middle_0", "left_hand_middle_1",
"left_hand_index_0", "left_hand_index_1",
"right_shoulder_pitch", "right_shoulder_roll", "right_shoulder_yaw",
"right_elbow", "right_wrist_roll", "right_wrist_pitch", "right_wrist_yaw",
"right_hand_thumb_0", "right_hand_thumb_1", "right_hand_thumb_2",
"right_hand_middle_0", "right_hand_middle_1",
"right_hand_index_0", "right_hand_index_1",
]
STATE_DIM = len(STATE_JOINT_NAMES) # 31
# 31维中各部分的索引，对应不同话题
UPPER_BODY_STATE_IDX = [0,1,2, 3,4,5,6,7,8,9, 17,18,19,20,21,22,23] # 17维 → rt/lowcmd
LEFT_HAND_STATE_IDX = [10,11,12,13,14,15,16] # 7维 → rt/dex3/left/cmd
RIGHT_HAND_STATE_IDX = [24,25,26,27,28,29,30] # 7维 → rt/dex3/right/cmd
# rt/lowcmd DDS电机槽位：29个电机的物理编号
# [0-5]左腿 [6-11]右腿 [12-14]腰部 [15-21]左臂 [22-28]右臂
# 17维上半身state → 对应的DDS槽位（一一对应）
UPPER_BODY_DDS_IDX = [12,13,14, 15,16,17,18,19,20,21, 22,23,24,25,26,27,28]
G1_NUM_MOTOR = 29 # rt/lowcmd 控制的电机总数
# 腰部单独管理，VLA控制时始终强制锁零位防止前倾
WAIST_DDS_IDX = [12, 13, 14]
WAIST_KP = [100, 80, 80]
WAIST_KD = [2, 2, 2]
# 全身PD增益（按DDS槽位顺序，29个）
Kp = [60,60,60,100,40,40, # 左腿 0-5
60,60,60,100,40,40, # 右腿 6-11
100,80,80, # 腰部 12-14
40,40,40,40,40,40,40, # 左臂 15-21
40,40,40,40,40,40,40] # 右臂 22-28
Kd = [1,1,1,2,1,1, 1,1,1,2,1,1, 2,2,2,
1,1,1,1,1,1,1, 1,1,1,1,1,1,1]
INIT_DURATION = 3.0
TRANSIT_DURATION = 3.0
ARM_CTRL_DT = 0.002 # 手臂控制周期 500Hz
ARM_ALPHA = 0.0075 # 手臂EMA平滑系数
# DDS话题名
TOPIC_LOWCMD = "rt/lowcmd"
TOPIC_LOWSTATE = "rt/lowstate"
TOPIC_LEFT_CMD = "rt/dex3/left/cmd"
TOPIC_RIGHT_CMD = "rt/dex3/right/cmd"
# Dex3 左手槽位限位：[T0, T1, T2, M0, M1, I0, I1 ]
MAX_LEFT = np.array([ 1.05, 1.05, 1.75, 0.00, 0.00, 0.00, 0.00], np.float32)
MIN_LEFT = np.array([-1.05, -0.724, 0.00, -1.57, -1.75, -1.57, -1.75], np.float32)
# Dex3 右手槽位限位：[T0, T1, T2, I0, I1, M0, M1 ]（M/I与左手相反）
MAX_RIGHT = np.array([ 1.05, 0.742, 0.00, 1.57, 1.75, 1.57, 1.75], np.float32)
MIN_RIGHT = np.array([-1.05, -1.05, -1.75, 0.00, 0.00, 0.00, 0.00], np.float32)
HAND_KP = 1.5
HAND_KD = 0.1
HAND_DT = 1.0 / 200.0 # 夹爪控制周期 200Hz
HAND_ALPHA = 0.05 # 夹爪EMA平滑系数
# ═══════════════════════════════════════════════════════════════════════════════
# ROS2 子进程（独立进程，CycloneDDS与主进程完全隔离）
# ═══════════════════════════════════════════════════════════════════════════════
def ros2_sensor_process(data_queue: MPQueue, stop_event: MPEvent):
"""
订阅三路相机 + /joint_states，30Hz打包放入data_queue。
必须在独立进程中运行，避免与主进程Unitree CycloneDDS冲突。
所有import必须在函数内部，防止主进程加载rclpy导致CycloneDDS被提前初始化。
"""
import threading
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from sensor_msgs.msg import CompressedImage, JointState
class SensorNode(Node):
def __init__(self):
super().__init__("g1d_sensor_node")
self._cam_head = None
self._cam_left = None
self._cam_right = None
self._joints = None
self._lock = threading.Lock()
sensor_qos = QoSProfile(
reliability=ReliabilityPolicy.BEST_EFFORT,
history=HistoryPolicy.KEEP_LAST, depth=10,
durability=DurabilityPolicy.VOLATILE,
)
self.create_subscription(CompressedImage, "/head_camera_wide/compressed",
lambda m: self._set_cam("cam_head", bytes(m.data)), sensor_qos)
self.create_subscription(CompressedImage, "/left_arm_camera/compressed",
lambda m: self._set_cam("cam_left", bytes(m.data)), sensor_qos)
self.create_subscription(CompressedImage, "/right_arm_camera/compressed",
lambda m: self._set_cam("cam_right", bytes(m.data)), sensor_qos)
self.create_subscription(JointState, "/joint_states", self._cb_joints, 10)
self.create_timer(1.0 / 30.0, self._timer_pub)
self.get_logger().info("SensorNode 启动")
def _set_cam(self, key, data):
with self._lock:
setattr(self, f"_{key}", data)
def _cb_joints(self, msg: JointState):
n2p = dict(zip(msg.name, msg.position))
state = [n2p.get(n, 0.0) for n in STATE_JOINT_NAMES]
with self._lock:
self._joints = state
def _timer_pub(self):
with self._lock:
if any(v is None for v in [self._cam_head, self._cam_left,
self._cam_right, self._joints]):
return
pkg = {
"cam_head": self._cam_head,
"cam_left": self._cam_left,
"cam_right": self._cam_right,
"joint_state": self._joints,
}
try:
data_queue.put_nowait(pkg)
except Exception:
pass
rclpy.init()
node = SensorNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
try:
while not stop_event.is_set():
executor.spin_once(timeout_sec=0.05)
finally:
node.destroy_node()
rclpy.shutdown()
# ═══════════════════════════════════════════════════════════════════════════════
# 工具函数
# ═══════════════════════════════════════════════════════════════════════════════
def ris_mode_byte(motor_id: int, status: int = 0x01, timeout: int = 0) -> int:
"""
构造Dex3电机mode字节（与C++ RIS_Mode_t位域完全一致）
bits 0-3：motor_id
bits 4-6：status（0x01=使能）
bit 7： timeout标志
注意：rt/lowcmd的mode字段直接填整数1，HandCmd才用这个函数
"""
v = (motor_id & 0x0F)
v |= (status & 0x07) << 4
v |= (timeout & 0x01) << 7
return v
# ═══════════════════════════════════════════════════════════════════════════════
# HandController：Dex3手部控制器
# 话题：rt/dex3/left/cmd 或 rt/dex3/right/cmd
# 消息类型：unitree_hg_msg_dds__HandCmd_（7个电机）
# ═══════════════════════════════════════════════════════════════════════════════
class HandController:
"""
200Hz持续发布手指目标角度到rt/dex3/*/cmd。
EMA低通滤波（α=0.05）消除15Hz跳变引起的颤抖。
保持锁：初始化阶段忽略外部更新，等待release_hold()解锁后才跟随VLA。
"""
def __init__(self, topic: str, max_lim: np.ndarray, min_lim: np.ndarray,
is_right: bool = False):
self.max_lim = max_lim
self.min_lim = min_lim
self.is_right = is_right
self.pub = ChannelPublisher(topic, HandCmd_)
self.pub.Init()
self._target = np.zeros(7, np.float32)
self._filtered = np.zeros(7, np.float32)
self._lock = threading.Lock()
self._stop = threading.Event()
self._hold_pose = True
threading.Thread(target=self._loop, daemon=True).start()
def set_target_from_state(self, state_q7: np.ndarray):
"""
输入STATE_JOINT_NAMES顺序的7维：[T0,T1,T2,M0,M1,I0,I1]
右手自动重映射到Dex3硬件槽位：[T0,T1,T2,I0,I1,M0,M1]
_hold_pose=True时静默丢弃
"""
with self._lock:
if self._hold_pose:
return
q = np.asarray(state_q7, np.float32)
if self.is_right:
q = np.array([q[0],q[1],q[2],q[5],q[6],q[3],q[4]], np.float32)
self._target = np.clip(q, self.min_lim, self.max_lim)
def set_target_direct(self, state_q7: np.ndarray):
"""绕过_hold_pose直接设置目标，用于初始化阶段"""
q = np.asarray(state_q7, np.float32)
if self.is_right:
q = np.array([q[0],q[1],q[2],q[5],q[6],q[3],q[4]], np.float32)
with self._lock:
self._target = np.clip(q, self.min_lim, self.max_lim)
def release_hold(self):
with self._lock:
self._hold_pose = False
side = "右手" if self.is_right else "左手"
print(f"[Hand {side}] 保持锁解除 → VLA 控制")
def _loop(self):
"""200Hz EMA平滑发送到rt/dex3/*/cmd"""
while not self._stop.is_set():
t0 = time.time()
with self._lock:
self._filtered = (HAND_ALPHA * self._target
+ (1.0 - HAND_ALPHA) * self._filtered)
q = self._filtered.copy()
msg = unitree_hg_msg_dds__HandCmd_()
for i in range(7):
msg.motor_cmd[i].mode = ris_mode_byte(i, 0x01, 0)
msg.motor_cmd[i].q = float(q[i])
msg.motor_cmd[i].dq = 0.0
msg.motor_cmd[i].tau = 0.0
msg.motor_cmd[i].kp = HAND_KP
msg.motor_cmd[i].kd = HAND_KD
self.pub.Write(msg)
time.sleep(max(0.0, HAND_DT - (time.time() - t0)))
def close(self):
self._stop.set()
# ═══════════════════════════════════════════════════════════════════════════════
# ArmLowCmdController：手臂 + 腰部控制器
# 话题：rt/lowcmd（发送）/ rt/lowstate（接收）
# 消息类型：unitree_hg_msg_dds__LowCmd_ / LowState_（29个电机）
# ═══════════════════════════════════════════════════════════════════════════════
class ArmLowCmdController:
"""
通过rt/lowcmd控制29个电机（腿+腰+臂）。
VLA阶段只更新上半身17个槽位，腿部(0-11)填0保持直立，腰部强制锁零位。
状态机：
init_blocking() 当前→零位（3s）→ 立刻启动_hold_loop（无空窗）
_hold_loop 500Hz EMA持续发，等待release_hold()
move_to_first_frame 零位→起始姿态（3s），_hold_loop跟随
release_hold() 解锁，_hold_loop退出→直接调用_vla_loop()
_vla_loop 500Hz EMA跟踪VLA目标（α=0.0075）
"""
def __init__(self):
self._lock = threading.Lock()
self._ready = threading.Event()
self._stop = threading.Event()
self._latest_state = None
self._mode_machine = 0
self._target_upper17 : Optional[np.ndarray] = None
self._filtered_upper17: Optional[np.ndarray] = None
self._hold_pose = True
self._release_motion_mode()
self.pub = ChannelPublisher(TOPIC_LOWCMD, LowCmd_)
self.sub = ChannelSubscriber(TOPIC_LOWSTATE, LowState_)
self.pub.Init()
self.sub.Init()
self.crc = CRC()
threading.Thread(target=self._sub_loop, daemon=True).start()
print("[ArmCtrl] 等待 lowstate...")
if not self._ready.wait(timeout=10.0):
raise RuntimeError("lowstate 超时，请检查 DDS 网络")
print("[ArmCtrl] lowstate 就绪")
def _release_motion_mode(self):
"""
释放motion switcher（运动模式切换器）。
G1-D默认运行在某种运动模式下，该模式会抢占lowcmd控制权，
必须先释放才能让我们的lowcmd指令生效。
"""
msc = MotionSwitcherClient()
msc.SetTimeout(5.0)
msc.Init()
status, result = msc.CheckMode()
print(f"[ArmCtrl] 当前 motion mode: {result}")
while result and result.get("name"):
msc.ReleaseMode()
time.sleep(1.0)
status, result = msc.CheckMode()
print("[ArmCtrl] motion mode 已释放")
def _sub_loop(self):
"""后台500Hz订阅rt/lowstate，持续更新29个电机的实际关节角度"""
while not self._stop.is_set():
msg = self.sub.Read()
if msg is not None:
with self._lock:
self._latest_state = msg
self._mode_machine = msg.mode_machine
if not self._ready.is_set():
self._ready.set()
time.sleep(0.002)
def _snapshot(self):
"""线程安全读取当前状态，返回(q29, upper_q17, mode_machine)"""
with self._lock:
if self._latest_state is None:
return None, None, 0
q29 = np.array([self._latest_state.motor_state[i].q
for i in range(G1_NUM_MOTOR)], dtype=np.float64)
upper17 = q29[UPPER_BODY_DDS_IDX]
mm = self._mode_machine
return q29, upper17, mm
def _send(self, q29: np.ndarray, mm: int):
"""
构造unitree_hg_msg_dds__LowCmd_并发送到rt/lowcmd。
腰部（DDS槽位12-14）强制覆盖为0，防止机器人前倾。
rt/lowcmd必须计算CRC校验，否则电机拒绝执行。
"""
cmd = unitree_hg_msg_dds__LowCmd_()
cmd.mode_pr = 0
cmd.mode_machine = mm
for i in range(G1_NUM_MOTOR):
cmd.motor_cmd[i].mode = 1
cmd.motor_cmd[i].tau = 0.0
cmd.motor_cmd[i].q = float(q29[i])
cmd.motor_cmd[i].dq = 0.0
cmd.motor_cmd[i].kp = Kp[i]
cmd.motor_cmd[i].kd = Kd[i]
for idx, dds in enumerate(WAIST_DDS_IDX):
cmd.motor_cmd[dds].q = 0.0
cmd.motor_cmd[dds].kp = WAIST_KP[idx]
cmd.motor_cmd[dds].kd = WAIST_KD[idx]
cmd.crc = self.crc.Crc(cmd)
self.pub.Write(cmd)
def init_blocking(self):
"""
Stage1（3s）：所有29个电机从当前位置线性插值到零位。
插值公式：q = (1-ratio) × q_init，即从q_init插值到0。
q_init是启动时读取的实际关节角度，全程固定（防止基准漂移）。
完成后立刻启动_hold_loop，无空窗，电机不超时。
"""
q29_0, _, _ = self._snapshot()
if q29_0 is None:
raise RuntimeError("无法获取初始状态")
q_init = q29_0.copy()
print(f"[ArmCtrl] Stage1: 当前 → 零位 ({INIT_DURATION:.0f}s)")
t0 = time.time()
while True:
elapsed = time.time() - t0
ratio = np.clip(elapsed / INIT_DURATION, 0.0, 1.0)
_, _, mm = self._snapshot()
cmd = unitree_hg_msg_dds__LowCmd_()
cmd.mode_pr = 0
cmd.mode_machine = mm
for i in range(G1_NUM_MOTOR):
cmd.motor_cmd[i].mode = 1
cmd.motor_cmd[i].tau = 0.0
cmd.motor_cmd[i].q = (1.0 - ratio) * q_init[i]
cmd.motor_cmd[i].dq = 0.0
cmd.motor_cmd[i].kp = Kp[i]
cmd.motor_cmd[i].kd = Kd[i]
for idx, dds in enumerate(WAIST_DDS_IDX):
cmd.motor_cmd[dds].q = 0.0
cmd.motor_cmd[dds].kp = WAIST_KP[idx]
cmd.motor_cmd[dds].kd = WAIST_KD[idx]
cmd.crc = self.crc.Crc(cmd)
self.pub.Write(cmd)
if ratio >= 1.0:
break
time.sleep(ARM_CTRL_DT)
with self._lock:
self._target_upper17 = np.zeros(17, dtype=np.float64)
self._filtered_upper17 = np.zeros(17, dtype=np.float64)
print("[ArmCtrl] Stage1 完成，_hold_loop 启动...")
threading.Thread(target=self._hold_loop, daemon=True).start()
def move_to_first_frame(self, left_hand: HandController,
right_hand: HandController,
duration: float = TRANSIT_DURATION):
"""
零位→bag起始姿态（3s线性插值）。
直接写_target_upper17，_hold_loop在后台持续跟随该目标发送。
"""
_, current_upper, _ = self._snapshot()
if current_upper is None:
current_upper = np.zeros(17, dtype=np.float64)
print(f"[ArmCtrl] 过渡到起始姿态 ({duration:.0f}s)...")
steps = int(duration / ARM_CTRL_DT)
for step in range(steps + 1):
alpha = step / steps
target = (1.0 - alpha) * current_upper + alpha * FIRST_FRAME_UPPER17
with self._lock:
self._target_upper17 = target.copy()
left_hand.set_target_direct(
(1.0 - alpha) * np.zeros(7) + alpha * FIRST_FRAME_LEFT_HAND)
right_hand.set_target_direct(
(1.0 - alpha) * np.zeros(7) + alpha * FIRST_FRAME_RIGHT_HAND)
time.sleep(ARM_CTRL_DT)
print("[ArmCtrl] 已到达起始姿态，保持中")
def set_upper_body_target(self, upper_q17: np.ndarray):
"""VLA推断调用；_hold_pose=True时静默丢弃"""
with self._lock:
if self._hold_pose:
return
self._target_upper17 = np.asarray(upper_q17, dtype=np.float64)
def release_hold(self):
with self._lock:
self._hold_pose = False
print("[ArmCtrl] 保持锁解除 → _vla_loop 即将接管")
def _hold_loop(self):
"""
500Hz EMA持续发送rt/lowcmd。
只更新上半身17个槽位，腿部(0-11)保持0，腰部在_send()内强制覆盖0。
_hold_pose=False后退出，直接调用_vla_loop()，无空窗衔接。
"""
print("[ArmCtrl] _hold_loop 运行，等待 release_hold()...")
while self._hold_pose:
with self._lock:
if self._target_upper17 is None or self._latest_state is None:
time.sleep(ARM_CTRL_DT)
continue
self._filtered_upper17 = (ARM_ALPHA * self._target_upper17
+ (1.0 - ARM_ALPHA) * self._filtered_upper17)
desired17 = self._filtered_upper17.copy()
mm = self._mode_machine
q29 = np.zeros(G1_NUM_MOTOR, dtype=np.float64)
for loc, dds in enumerate(UPPER_BODY_DDS_IDX):
q29[dds] = desired17[loc]
self._send(q29, mm)
time.sleep(ARM_CTRL_DT)
print("[ArmCtrl] _hold_loop 结束，_vla_loop 接管")
self._vla_loop()
def _vla_loop(self):
"""500Hz EMA跟踪VLA目标（α=0.0075，约267步完全到位）"""
while not self._stop.is_set():
t0 = time.time()
with self._lock:
if self._target_upper17 is None or self._latest_state is None:
time.sleep(ARM_CTRL_DT)
continue
self._filtered_upper17 = (ARM_ALPHA * self._target_upper17
+ (1.0 - ARM_ALPHA) * self._filtered_upper17)
desired17 = self._filtered_upper17.copy()
mm = self._mode_machine
q29 = np.zeros(G1_NUM_MOTOR, dtype=np.float64)
for loc, dds in enumerate(UPPER_BODY_DDS_IDX):
q29[dds] = desired17[loc]
self._send(q29, mm)
time.sleep(max(0.0, ARM_CTRL_DT - (time.time() - t0)))
def close(self):
self._stop.set()
# ═══════════════════════════════════════════════════════════════════════════════
# DataBuffer：传感器数据缓存（主进程侧）
# ═══════════════════════════════════════════════════════════════════════════════
class DataBuffer:
def __init__(self, data_queue: MPQueue):
self._queue = data_queue
self._lock = threading.Lock()
self._latest = None
threading.Thread(target=self._drain, daemon=True).start()
def _drain(self):
while True:
try:
pkg = self._queue.get(timeout=1.0)
with self._lock:
self._latest = pkg
except Exception:
pass
def snapshot(self):
with self._lock:
if self._latest is None:
return None, "等待 ROS2 子进程数据"
return dict(self._latest), "OK"
def flush(self):
count = 0
while not self._queue.empty():
try:
self._queue.get_nowait()
count += 1
except Exception:
break
if count:
print(f"[DataBuffer] 清空 {count} 帧积压数据")
# ═══════════════════════════════════════════════════════════════════════════════
# G1DVLAController：VLA推断主控制器
# ═══════════════════════════════════════════════════════════════════════════════
class G1DVLAController:
def __init__(self, arm, left_hand, right_hand, buffer,
server_ip, server_port, control_freq, control_horizon):
self.arm = arm
self.left_hand = left_hand
self.right_hand = right_hand
self.buffer = buffer
self.server_ip = server_ip
self.server_port = server_port
self.control_freq = control_freq
self.control_horizon = control_horizon
self.cmd_queue = queue.Queue()
self._tcp_lock = threading.Lock()
self.tcp_socket = None
self.tcp_connected = False
self._stop = threading.Event()
def _connect(self):
with self._tcp_lock:
try:
if self.tcp_socket:
self.tcp_socket.close()
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((self.server_ip, self.server_port))
s.settimeout(30.0)
self.tcp_socket = s
self.tcp_connected = True
print(f"[VLA] 已连接推理服务器 {self.server_ip}:{self.server_port}")
except Exception as e:
self.tcp_connected = False
print(f"[VLA] 连接失败: {e}")
def run(self):
self._connect()
print("[VLA] 等待传感器数据就绪...")
while not self._stop.is_set():
obs, status = self.buffer.snapshot()
if obs is not None:
break
print(f"[VLA] 传感器等待中: {status}")
time.sleep(0.5)
print("[VLA] 传感器就绪")
print("\n[VLA] 机器人已到达起始姿态，保持中...")
print("[VLA] 确认周围无障碍物后，按 Enter 开始 VLA 推断...")
input()
self.buffer.flush()
print("[VLA] 等待最新传感器帧...")
for _ in range(20):
obs, _ = self.buffer.snapshot()
if obs is not None:
break
time.sleep(0.05)
print("[VLA] 数据就绪，开始推断\n")
period = 1.0 / self.control_freq
step = 0
first_done = False
while not self._stop.is_set():
t0 = time.time()
if not self.tcp_connected:
print("[VLA] 无连接，尝试重连...")
self._connect()
time.sleep(1.0)
continue
if not self.cmd_queue.empty():
step += 1
action = self.cmd_queue.get()
a = np.asarray(action)
print(f"[VLA] 执行第 {step} 步 | "
f"左臂{np.round(a[UPPER_BODY_STATE_IDX][3:10], 2)} "
f"右臂{np.round(a[UPPER_BODY_STATE_IDX][10:], 2)}")
self._apply(action)
else:
step = 0
success = self._infer()
if success and not first_done:
first_done = True
self.arm.release_hold()
self.left_hand.release_hold()
self.right_hand.release_hold()
time.sleep(max(0.0, period - (time.time() - t0)))
def _apply(self, action31):
"""31维action拆分：17维→rt/lowcmd，7维→rt/dex3/left，7维→rt/dex3/right"""
a = np.asarray(action31, np.float64)
self.arm.set_upper_body_target(a[UPPER_BODY_STATE_IDX])
self.left_hand.set_target_from_state(a[LEFT_HAND_STATE_IDX])
self.right_hand.set_target_from_state(a[RIGHT_HAND_STATE_IDX])
def _infer(self) -> bool:
"""
TCP发送观测，接收action_chunk入队。
通信协议：4字节大端长度头 + msgpack body。
发送30维（模型训练用30维），接收30维后末尾补0还原31维。
"""
obs, status = self.buffer.snapshot()
if obs is None:
print(f"[VLA] 等待传感器: {status}")
return False
try:
payload = {
"timestamp": datetime.now().isoformat(),
"image_info": {"width": 640, "height": 480, "format": "jpeg"},
"video.head_camera": obs["cam_head"],
"video.left_hand_camera": obs["cam_left"],
"video.right_hand_camera": obs["cam_right"],
"state.FollowerArm": obs["joint_state"][:30],
}
data = msgpack.packb(payload, use_bin_type=True)
with self._tcp_lock:
self.tcp_socket.sendall(struct.pack(">I", len(data)))
self.tcp_socket.sendall(data)
hdr = self.tcp_socket.recv(4)
if not hdr:
raise ConnectionError("服务器返回空 header")
resp = msgpack.unpackb(
self._recv_all(struct.unpack(">I", hdr)[0]), raw=False)
ac = resp.get("action_chunk", {})
if "action.LeaderArm" not in ac:
print("[VLA] 返回数据中无 action.LeaderArm")
return False
wps = np.asarray(ac["action.LeaderArm"], np.float64) # (16, 30)
if wps.ndim != 2 or wps.shape[1] != 30:
print(f"[VLA] waypoints shape 异常: {wps.shape}")
return False
horizon = min(self.control_horizon, wps.shape[0])
while not self.cmd_queue.empty():
self.cmd_queue.get()
for i in range(1, horizon):
self.cmd_queue.put(np.append(wps[i], 0.0)) # 补0还原31维
print(f"[VLA] 收到 {wps.shape}，入队 {horizon-1} 步")
return True
except Exception as e:
self.tcp_connected = False
print(f"[VLA] 推理异常: {e}")
return False
def _recv_all(self, n: int) -> bytes:
buf = b""
while len(buf) < n:
chunk = self.tcp_socket.recv(n - len(buf))
if not chunk:
raise ConnectionError("socket 断开")
buf += chunk
return buf
def stop(self):
self._stop.set()
with self._tcp_lock:
if self.tcp_socket:
try:
self.tcp_socket.close()
except OSError:
pass
# ═══════════════════════════════════════════════════════════════════════════════
# 入口
# ═══════════════════════════════════════════════════════════════════════════════
def main():
parser = argparse.ArgumentParser(description="G1-D VLA 推断控制器")
parser.add_argument("--network-interface", default=None,
help="Unitree DDS 网卡名（CycloneDDS要求指定，如eth0）")
parser.add_argument("--server-ip", default="192.168.123.53")
parser.add_argument("--server-port", type=int, default=8888)
parser.add_argument("--control-freq", type=int, default=15)
parser.add_argument("--control-horizon", type=int, default=16)
parser.add_argument("--skip-init", action="store_true")
args = parser.parse_args()
print("WARNING: 确保机器人周围无障碍物！")
input("按 Enter 继续...")
# ① 启动ROS2子进程（独立进程，CycloneDDS与主进程完全隔离）
data_queue = MPQueue(maxsize=5)
stop_event = MPEvent()
ros2_proc = Process(
target=ros2_sensor_process,
args=(data_queue, stop_event),
daemon=True,
)
ros2_proc.start()
print(f"[Main] ROS2 子进程已启动 PID={ros2_proc.pid}")
# ② Unitree DDS初始化（主进程独占，指定eth0确保指令走正确网卡）
if args.network_interface:
ChannelFactoryInitialize(0, args.network_interface)
else:
ChannelFactoryInitialize(0)
# ③ 创建底层控制器
arm = ArmLowCmdController()
left_hand = HandController(TOPIC_LEFT_CMD, MAX_LEFT, MIN_LEFT, is_right=False)
right_hand = HandController(TOPIC_RIGHT_CMD, MAX_RIGHT, MIN_RIGHT, is_right=True)
buffer = DataBuffer(data_queue)
# ④ Stage1：29个电机归零（3s），结束后_hold_loop自动启动
if not args.skip_init:
print(f"\n开始初始化（Stage1 约 {INIT_DURATION:.0f}s）...")
arm.init_blocking()
# ⑤ 零位→起始姿态（3s），_hold_loop后台跟随
arm.move_to_first_frame(left_hand, right_hand, duration=TRANSIT_DURATION)
# ⑥ VLA推断循环（等待回车→flush→推断→release_hold）
vla = G1DVLAController(
arm=arm, left_hand=left_hand, right_hand=right_hand,
buffer=buffer,
server_ip=args.server_ip, server_port=args.server_port,
control_freq=args.control_freq, control_horizon=args.control_horizon,
)
print("VLA 推断控制运行中，Ctrl+C 退出\n")
try:
vla.run()
except KeyboardInterrupt:
pass
finally:
print("\n正在退出...")
vla.stop()
arm.close()
left_hand.close()
right_hand.close()
stop_event.set()
ros2_proc.join(timeout=3.0)
print("退出完成")
if __name__ == "__main__":
main()八、总结
8.1 Unitree SDK 话题速查
| 话题 | 消息类型 | 方向 | 控制对象 | 推荐频率 |
|---|---|---|---|---|
| rt/lowcmd | unitree_hg_msg_dds__LowCmd_ | 发送 | 29个电机（腿+腰+臂） | 500Hz |
| rt/lowstate | LowState_ | 接收 | 读取29个电机实际状态 | 订阅 |
| rt/dex3/left/cmd | unitree_hg_msg_dds__HandCmd_ | 发送 | 左手7个电机 | 200Hz |
| rt/dex3/right/cmd | unitree_hg_msg_dds__HandCmd_ | 发送 | 右手7个电机（M/I重映射） | 200Hz |
8.2 三大核心技术点
双进程：解决 CycloneDDS 全局单例冲突，ROS2和Unitree SDK各用独立进程
EMA滤波：解决 15Hz推断 vs 500Hz/200Hz控制频率不匹配，消除卡顿和颤抖
保持锁：解决 lowcmd超时失控，init→hold→vla 全程无空窗衔接
8.3 启动命令
bash
export PYTHONPATH=$PYTHONPATH:/home/unitree/unitree_sdk2_python
source /opt/ros/foxy/setup.bash
source ~/unitree_ros2/cyclonedds_ws/install/setup.bash
source ~/unitree_ros2_ws/install/setup.bash
python3 G1-D_vla_dds_controller_node.py \
--network-interface eth0 \
--server-ip 192.168.123.53 \
--server-port 8888``