TG3/docs/天工3.0本地同构臂遥操部署汇报.md

# 天工 3.0 本地同构臂遥操部署汇报

日期：2026-08-06
设备：天工 3.0（HBWALK）+ TS1P 同构臂 + BrainCo Revo2 双灵巧手

## 1. 最终结果

已实现不依赖厂商“智能驾驶舱平台”配对的本地双臂、双手遥操。运行时链路为：

```text
TS1P 同构臂
  -> EAI 原厂 xTELE
  -> tcp://127.0.0.1:5003（原始关节、按键和诊断，仅 EAI 机内）
  -> tcp://127.0.0.1:5001（xTELE 处理后的双手目标，仅 EAI 机内）
  -> 发送器仅把 5001 的 hand.position 合并到 5003 原始帧
  -> tg3-omnisocket-sender
  -> OmniSocket / KCP（UDP）
  -> 175.178.116.187:14049 公网 Hub
  -> 机器人内 tg3_local_teleop 直接接收 OmniSocket
  -> /encoder_identical_joint（双臂 14 维 JointState）
  -> 原厂 freq_change_tg3_node
  -> /arm/cmd
  -> 天工 3.0 双臂

同一个 tg3_local_teleop
  -> /left_hand/set_motor_multi
  -> /right_hand/set_motor_multi
  -> 原厂 BrainCo Revo2 双手驱动
```

机器人端不再设置单独的 `tg3-omnisocket-receiver.service`，OmniSocket 接收、校验和
ROS 发布都在 `tg3_local_teleop` 内完成。开发电脑不参与运行链路，只保存源码和本报告。

## 2. 为什么采用这条链路

### 2.1 保留原厂 xTELE 与机器人控制栈

TS1P 的串口/CAN、关节标定、按键、扳机和错误状态已经由原厂 xTELE 正确处理。xTELE
当前安装版本的核心模块是编译后的扩展，直接修改或替换会增加标定、按键兼容和设备安全
风险，因此保留原程序及配置，不修改其源码。

机器人端也继续使用原厂 `freq_change_tg3_node`、`/arm/cmd`、BrainCo 驱动及其默认
限位、电流、堵转和碰撞保护。本项目只在原厂已有输入接口之前增加网络桥和安全门控。

### 2.2 为什么链路中仍能看到 TCP

`tcp://127.0.0.1:5003` 和 `tcp://127.0.0.1:5001` 都是 xTELE 已有的本机 ZMQ
发布接口，地址为回环地址：

- 数据不会离开 EAI 工控机；
- 它不是 EAI 到机器人之间的网络传输；
- 保留它可以避免修改编译后的 xTELE；
- 真正跨机器、跨公网的部分使用 OmniSocket KCP/UDP。

如果连这段本机 TCP 也删除，就必须修改或替换原厂 xTELE，违背“不改原有代码”的约束。

### 2.3 为什么不直接使用原无线 UDP

普通点对点 UDP 适合同一局域网，但公网环境会遇到 NAT、地址变化、丢包、乱序和重传问题。
OmniSocket 使用固定 Peer ID 经过 KCP Hub 转发，提供有序传输、丢包重传、会话注册和统计，
因此同一套代码可通过 Wi-Fi、有线互联网运行，不要求机器人与 EAI 位于同一网段。

当前 Peer 配置：

| 项目 | 值 |
|---|---|
| Hub | `175.178.116.187:14049` |
| EAI Peer ID | `tg3-009027fa8190-iarm` |
| 机器人 Peer ID | `tg3-009027fa8190-robot` |
| EAI 本机 xTELE 原始数据 | `tcp://127.0.0.1:5003` |
| EAI 本机 xTELE 处理后命令 | `tcp://127.0.0.1:5001`（只合并 `hand.position`） |

## 3. 端到端频率

不同位置的“频率”含义不同，不能只用一个数表示整条链路。

| 环节 | 频率/策略 | 说明 |
|---|---:|---|
| TS1P 关节采样 `freq` | 现场约 `80~92 Hz` | xTELE JSON 自带的左右臂/CAN采样频率；本次抓帧为 `91.405 Hz`，启动最低门槛为 `30 Hz` |
| xTELE ZMQ 发布 | 实测 `98.02 Hz` | 连续接收 500 帧耗时 5.101 秒 |
| xTELE 处理后命令 `5001` | 现场约 `50 Hz` | 只取最新有效双手目标，最大允许年龄 `0.25 s` |
| EAI OmniSocket 发送 | 待机 `0 Hz`，遥操约 `98~100 Hz` | Z+C 3 秒后才连接并逐帧发送；STOP 刷新后关闭 Session，待机无注册/心跳 |
| 机器人 OmniSocket 接收 | 待机无业务帧，遥操约 `100 Hz` | 网络状况变化时会波动；待机无帧不再被判为故障 |
| 本地桥双臂 ROS 发布 | `50 Hz` | 仅合法新会话通过启动门控后向 `/encoder_identical_joint` 发布 |
| 本地桥双手 ROS 发布 | `50 Hz` | 与双臂共用同一次武装；BrainCo 指令同频发布 |
| 原厂双臂频率转换 | `200 Hz` | 由原厂 `freq_change_tg3_node` 完成同步、滤波并输出 `/arm/cmd` |
| BrainCo 状态反馈 | `30 Hz` | 原厂 `brainco_hand.yaml` 的 `publish_hz` |

这里的 `98~100 Hz` 是现场测量值而非硬实时保证；公网延迟、Hub 负载和系统调度都会造成
小幅波动。控制桥以“最新帧”为准，不排队回放旧动作。

## 4. 数据格式

### 4.1 xTELE 原始 JSON

xTELE 每帧发布 UTF-8 JSON。现场帧的结构如下（数值仅作格式示例）：

```json
{
  "timestamp": 1786018731538,
  "isomorphic_arm_id": "IArm009027FA8190",
  "isomorphic_arm_type": "TS1P",
  "arm": {
    "position": {
      "left":  [0.021, 0.021, 0.499, -0.153, -0.442, 0.183, 0.060],
      "right": [-0.021, -0.003, -0.683, -0.206, 0.874, 0.069, -0.160]
    }
  },
  "hand": {"position": {"left": 0.0, "right": 0.0}},
  "waist": {"position": 0.236},
  "servo_error": {
    "left": [0, 0, 0, 0, 0, 0, 0],
    "right": [0, 0, 0, 0, 0, 0, 0],
    "waist": [0]
  },
  "freq": {"left": 91.405, "right": 91.405, "waist": 91.405},
  "button": {
    "left":  [0, 0, 0, 0, 0],
    "right": [0, 0, 0, 0, 0]
  },
  "trigger": {"left": 0.0, "right": 0.0},
  "joycan_error": [0, 0]
}
```

原帧还包含 `scroll`、`joystick`、`button_joystick`、`button_wheel`、`vibration_mode`、
`total_current` 和 `total_power`。发送端保留 5003 的双臂、按键、诊断等字段，只用 5001
同一时刻的处理结果替换 `hand.position`，并增加 `tg3_transport` 来源标记。机器人桥仅消费
本项目所需字段：

- `arm.position.left/right`：左右各 7 个关节，单位为弧度；
- `hand.position.left/right`：未选择组合手势时为 `0~1` 扳机开合量；选择手势后可为厂家
  xTELE 生成的 6 维 BrainCoRevo2 归一化目标；
- `button.left/right`：左侧顺序以 X/Y/Z 开头，右侧以 A/B/C 开头；索引 2 即 Z/C；
- `servo_error`、`joycan_error` 和 `freq`：启动与运行安全门控；
- `isomorphic_arm_id/type`：设备身份校验。

腰、头、腿和行走数据不由本桥控制。

### 4.2 OmniSocket 二进制封包

EAI 在原 JSON 前增加 24 字节网络序头部，格式为 `!4sQQI`：

| 偏移 | 长度 | 类型 | 含义 |
|---:|---:|---|---|
| 0 | 4 | bytes | 固定魔数 `TG3A` |
| 4 | 8 | uint64 | 单调递增序列号 |
| 12 | 8 | uint64 | EAI 发送时间 `time.time_ns()` |
| 20 | 4 | uint32 | 后续 JSON 字节长度 |
| 24 | 可变 | bytes | 原始 UTF-8 JSON |

JSON 内的 `tg3_transport` 由 EAI sender 强制覆盖，包含协议版本 `2`、128-bit 随机
`session_id`、会话内递增 `session_seq` 和 `session_state=start|active|stop`。START
连续发送 50 个源帧，机器人对每个 ID 只执行一次启动安全检查；STOP 是最后一个业务帧。
旧 ID、缺少 START 的 ACTIVE、错 ID 的 STOP 或伪造的源 metadata 均不能武装机器人。

单帧 JSON 上限为 1 MiB。机器人端依次检查发送 Peer、消息类型、魔数、长度、JSON
结构、设备 ID、序列号和包龄。由于 EAI 与机器人系统时钟可能存在固定偏差，包龄使用
“当前包原始年龄减去本会话最小年龄”的相对值；额外排队超过 `300 ms` 的包会被丢弃。

### 4.3 ROS 双臂格式

机器人桥发布 `sensor_msgs/msg/JointState`：

- Topic：`/encoder_identical_joint`；
- `header.frame_id`：`tg3_local_teleop`；
- `name`：`left_joints_0..6`、`right_joints_0..6`；
- `position`：14 个弧度值，先左臂后右臂；
- `velocity`：14 个零值，保持原厂位置控制路径。

原厂节点继续负责转换为电机 11..17、21..27 的 `/arm/cmd`。

### 4.4 BrainCo 双手格式

双手分别发布：

- `/left_hand/set_motor_multi`；
- `/right_hand/set_motor_multi`；
- 类型：`brainco_hand_msgs/msg/SetMotorMulti`；
- `mode=5`（位置 + 期望时间）；
- `positions`：6 个 `1~1000` 位置值；
- `durations`：6 个 `800 ms`；
- `speeds/currents/pwms`：均为 0，由原厂位置+时间模式处理。

一维扳机值映射如下：

| 扳机 | 含义 | 六电机目标 |
|---:|---|---|
| `0` | 现场实测默认打开位 | `[401, 401, 51, 51, 51, 51]` |
| `1` | 保守常规闭合抓握 | `[900, 500, 451, 520, 520, 451]` |

中间值线性插值。手部从机器人实测位置开始，每秒最多变化 400 个位置单位。BrainCo
状态 `0=空闲`、`1=运动`、`2=接触/堵转或到限位`、`3=持续力` 均为原厂正常状态；
仅状态失联或未知/非法值会触发本地停止，原厂电流、堵转和限位保护保持不变。

xTELE 已注册的手势组合键由工控机原程序负责短按/长按、互斥和手势编号判定。选中手势后，
5001 的 `hand.position.left/right` 会变为 6 维数组，机器人桥直接换算为 BrainCo 的
`1~1000` 位置值；本项目不再在机器人端复刻组合键状态机。5002 的日志、帮助、RGB/D、
关节浮层等 `webui` 事件不发送到机器人；腰和头不接入本地桥。HBWALK 行走使用 5003
传输的原始右 C 与左摇杆；遥操开启后按键和摇杆在下一个 50 Hz 周期即时解析并发布
`geometry_msgs/msg/TwistStamped` 到 `/hric/robot/cmd_vel`，不再叠加第二个 3 秒计时。

## 5. 启停、回 Home 与安全逻辑

### 5.1 操作方式

1. 机器人进入 `HBWALK`，状态为 `running`；
2. 两个扳机先松开；
3. 同时长按左 Z + 右 C 3 秒，EAI 生成 START 会话并开始发送，机器人通过启动门控后
   启动双臂和双手跟随；
4. 左扳机控制左手，右扳机控制右手，松开为张开、扣下为闭合；
5. 行走时按住右 C 并推出左摇杆即刻响应；上下为前后、左右为转向；
6. 松开 C 或摇杆回中立即停走，再次操作无需重新等待；
7. 再次长按左 Z + 右 C 3 秒发送 STOP；STOP 后 EAI 不再发送 xTELE 业务数据；
8. 停止后双臂以 `0.25 rad/s` 限速返回现场保存的 Home 姿态，手保持最后目标。

行走以 50 Hz 发布，死区为 0.2，二次曲线，并按官方半身行走 Topic 范围限制为前进
`1.0 m/s`、后退 `0.8 m/s`、转向 `0.8 rad/s`；
停止时立即发零速并补发 10 帧。已安装的 xTELE 0.1.2 没有注册右侧同手 `C+A`，所以
本地桥也不猜测其含义、不对它执行状态切换。

服务随系统自动启动只代表“等待数据并监测”，不会自动武装，也不会绕过物理长按。

### 5.2 本地门控

启动前要求：

- 当前状态和子状态均为 `HBWALK`，运行状态为 `running`；
- xTELE 数据新鲜，设备 ID/type 正确，采样频率不低于 30 Hz；
- 同构臂 CAN、双臂电机反馈无错误；
- BrainCo 双手状态反馈新鲜且完整；
- 启动瞬间 14 个同构臂目标位于天工 3.0 文档关节范围内；
- 没有检测到第二路云端/外部手臂或灵巧手命令源。

自定义关节目标限位只在启动时检查；采集/跟随期间不重复增加该限位。运行时仍持续检查
HBWALK、数据失联、CAN/电机错误和命令源冲突，原厂默认保护没有修改。

首次启动时，双臂限速器从机器人实时关节位置开始，双手限速器从实时手指位置开始，
避免第一帧直接跳到同构臂目标。

### 5.3 断线处理

- 活动会话的同构臂输入超过 `0.25 s` 未更新：立即解除武装并停止发布；
- 活动会话超过 `2 s` 仍没有新帧才重建机器人接收进程；待机或 STOP 后无业务帧是正常状态；
- 只有匹配的操作员 STOP 触发自动 Home，意外断网只停控；Home 继续依赖机器人反馈，
  不再依赖已经停止的 EAI 数据；
- 公网 Hub 重启后，建议重启 EAI 发送服务以清空 KCP 旧队列：

```bash
systemctl --user restart tg3-omnisocket-sender.service
```

本次服务器恢复后重启发送服务，KCP 状态恢复为反馈约 6 ms、发送队列 0；机器人随后恢复
约 100 Hz 收包并通过安全门控。

## 6. EAI 工控机上完成的操作

运行账号：`eai`

1. 保留原厂目录 `/home/eai/dev/sysEAI`、`xtele_main.py`、TS1P 标定和
   `/home/eai/.config/xhumanoid/xtele/default.toml`，未修改原厂控制源码；
2. 确认系统级 `xtele-monitor.service` 已启用并运行，它调用原有
   `/home/eai/dev/sysEAI/start_xtele.sh`；
3. 部署 OmniSocketGo Python 运行库到 `/home/eai/OmniSocketGo`；
4. 新增 `/home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py`：
   - 订阅 `tcp://127.0.0.1:5003` 原始帧；
   - 订阅 `tcp://127.0.0.1:5001` 处理后命令，只提取双侧 `hand.position`；
   - 在 EAI 本机用 Z+C 连续 3 秒生成唯一 START/STOP 会话；待机仍读本地数据但不上公网；
   - 保留 5003 的双臂、按键、摇杆和 CAN/舵机诊断，用 5001 的厂家手势目标覆盖双手字段；
   - 只保留最新帧；
   - 添加 `TG3A` 头部；
   - 仅在 START 到 STOP 之间发送到机器人 Peer，并写入不可伪造的 protocol/session 元数据；
   - 输出 `status.json` 统计；
5. 新增并启用用户服务
   `/home/eai/.config/systemd/user/tg3-omnisocket-sender.service`，设置
   `Restart=always`；
6. 服务器恢复后重启上述发送服务，清除了断线期间 KCP 积压；
7. 删除的仅是此前由本项目写入、现已废弃的接收代理/旧服务；未删除任何原厂代码。

常用检查命令：

```bash
systemctl status xtele-monitor.service
systemctl --user status tg3-omnisocket-sender.service
cat /home/eai/tg3_omnisocket_transport/status.json
```

## 7. 机器人上完成的操作

运行账号：`nvidia`，地址：`192.168.41.2`

1. 部署 OmniSocketGo Python 运行库到 `/home/nvidia/OmniSocketGo`；
2. 新增 `/home/nvidia/tg3_local_teleop`，主要文件为：
   - `tg3_local_teleop.py`：OmniSocket 接收、校验、ROS 双臂/双手发布、安全门控、
     断线看门狗和回 Home；
   - `config.toml`：Peer、频率、话题、关节范围、手部映射和 Home 姿态；
   - `run.sh`、`status.sh`、`home.sh`：运行、状态和回 Home 命令；
   - `status.json`：实时状态；
3. 新增并启用用户服务
   `/home/nvidia/.config/systemd/user/tg3-local-teleop.service`；
4. 将 OmniSocket 接收直接集成进 `tg3_local_teleop`，不再经过机器人本机 ZMQ/TCP
   接收代理；
5. 删除本项目此前创建的旧 `tg3-omnisocket-receiver.service` 和无用代理代码；
6. 接入原厂 `/encoder_identical_joint` 和 `freq_change_tg3_node` 完成双臂控制；
7. 根据机器人实际配置 `left_hand_type/right_hand_type=brainco`，接入 BrainCo
   `/set_motor_multi` 与 `/motor_status` 完成双手控制；未修改原厂手型配置；
8. 保存当前真实双臂姿态为 Home，而不是把 14 个关节归到电机零位；
9. 加入服务器断线后的 2 秒进程级自动重连看门狗；
10. 历次部署前均在未武装状态检查，旧版本备份只包含本项目文件；未删除或覆盖原厂程序。

常用检查命令：

```bash
systemctl --user status tg3-local-teleop.service
cat /home/nvidia/tg3_local_teleop/status.json

cd /home/nvidia/tg3_local_teleop
./home.sh status
./home.sh start
./home.sh cancel
```

`home.sh start` 会驱动机器人双臂，执行前必须确认防护、活动空间和急停。

## 8. 当前保留与未修改内容

- 原厂 xTELE、TS1P 串口/CAN、按键和标定逻辑未修改；
- 原厂 `freq_change_tg3_node`、`/arm/cmd` 及其默认限位未修改；
- 原厂 BrainCo 驱动、手型配置、电流/堵转/限位保护未修改；
- 原有代码没有删除；清理范围仅限本项目早期写入的旧代理和旧服务；
- 本地开发电脑没有部署运行服务，不是遥操链路中的单点依赖。

## 9. 验收结论与参考资料

现场已完成以下验证：

- 待机时 EAI 本地 `frames_received` 增长而 `frames_sent` 保持不变；START 后机器人稳定
  收到约 100 Hz 数据，STOP 后再次归零；
- 左右 Z/C 长按启停正常，服务不会自行武装；
- 双臂在 HBWALK 下正常跟随，停止后按限速逻辑返回保存的 Home；
- 左右扳机分别输出 `0~1`，双灵巧手开合、限速和 BrainCo 运动状态处理正常；
- 工控机 5001 双手语义帧已通过 OmniSocket 合并发送，实测无畸形帧、无字段丢失；
- 服务器断开时机器人停止发布，服务器恢复并重建会话后可继续工作。

参考资料：

- 用户提供的《具身天工 3.0 + SDK + 文档（26.04.x）》；
- 机器人已安装的 `brainco_hand_msgs`、`brainco_hand.yaml` 和 BrainCo 示例；
- [BrainCo Hand 官方 SDK 文档](https://www.brainco-hz.com/docs/revolimb-hand/revo2/get_sdk.html)；
- `/home/eai/OmniSocketGo/README.md` 及本项目实际运行统计；
- 本项目 `omnisocket_xtele_sender.py`、`tg3_local_teleop.py` 和 `config.toml`。

后续迁移步骤、IP/Peer 修改表和新机器人验收流程见
《天工3.0本地同构臂遥操迁移部署指南.md》。