TG3/tg3_local_teleop/README.md

# 天工 3.0 本地同构臂双臂、BrainCo 灵巧手与 HBWALK 行走控制

这套桥接不修改工控机 xTELE、`robot_tele_server` 或机器人控制源码。数据链路为：

```text
TS1P 同构臂
  -> 工控机 xTELE tcp://127.0.0.1:5003
  -> OmniSocket KCP Hub 175.178.116.187:14049
  -> tg3_local_teleop（直接 OmniSocket Session；HBWALK / 错误 / 限位门控）
  -> /encoder_identical_joint（14 维 JointState）
  -> 厂家 freq_change_tg3_node（同步、滤波、200 Hz）
  -> /arm/cmd
  -> 天工 3.0 双臂
  -> /left_hand/set_motor_multi + /right_hand/set_motor_multi
  -> 天工 3.0 BrainCo Revo2 双灵巧手
  -> /hric/robot/cmd_vel（右 C + 左摇杆前后 / 左 Z + 右摇杆转向；50 Hz TwistStamped）
  -> 天工 3.0 HBWALK 行走
```

## 自动运行与操作

- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
- 用户服务启动前会用短生命周期 ROS 探针等待 `/hric/robot/rl_state` 可发现，再创建
  长生命周期桥接节点，避免开机早期网络接口尚未就绪时 Fast DDS 固化为空接口。
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
  本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
  机器人立即停控并以 `0.25 rad/s` 自动回 Home。
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
  前后/转向；不控制腰和头。
- 行走：遥操已启动后，右 `C` + 左摇杆上下控制前后，左 `Z` + 右摇杆左右控制原地
  转向；两个组合都在下一个 `50 Hz` 周期立即响应，不再等待 3 秒。松开对应按键或
  摇杆回中立即把该轴清零，再按也立即恢复。
  前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度，
  但最大值仍受官方 `0.8 rad/s` 上限约束。
  按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
  `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
- xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴
  映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
- 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
  在不清楚厂商语义的情况下把它擅自绑定为状态切换。
- 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
  BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
  若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
- 右手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 1 秒开启；松开
  至少 `0.5 s` 后，再连续长按右 `B` 1 秒关闭。首次启动和每个新遥操会话都必须先
  稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈
  中断均不触发；计时期间冻结右手最后一条命令，左手和双臂仍照常跟随。
- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号
  “单食指”手势 `state 0`：归一化目标
  `[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`，对应当前 `1~1000` 位置范围约为
  `[201, 688, 1, 980, 980, 980]`；顺序为大拇指弯曲、大拇指旋转、食指、中指、
  无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。

## 安全门控

只有以下条件全部满足才允许开始：机器人 `current_state` 和 `child_state` 均为
`HBWALK`、状态为 `running`；同构臂数据新鲜；左右 CAN/伺服无错误；采样频率正常；
启动瞬间的 14 个目标均在天工 3.0 文档限位内；没有检测到云端第二路同构臂命令。
自定义关节目标限位仅对每个新 `session_id` 的 START 检查一次，采集/跟随过程中不再检查；电机错误、
HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 原有的姿态同步、
关节限位和碰撞保护保持不变。为启用二次开放文档规定的 HBWALK topic 行走，本机
`xmigcs/config/dex_config.yaml` 的 HBWALK 速度许可已由全零改为当前 NAVIGATE 的上限；
实际发出的速度由本桥按官方 Topic 范围限制为 `linear.x=[-0.8, 1.0] m/s`、
`angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。

行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。

灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在
STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。

## 限速双臂回 Home

回 Home 命令读取机器人真实 `/freq_change/arm_status` 作为轨迹起点，以 `0.25 rad/s`
把关节 11..17、21..27 平滑移动到现场保存的双臂 Home 姿态；Home 不是电机全零位。
只有未武装、HBWALK、机器人双臂反馈新鲜且无错误时才接受。Home 轨迹不依赖已经主动
停止的 EAI 数据，只依赖机器人反馈和机器人侧安全门；执行取消命令会立即停止发布。
若机器人实际位置落后指令超过 `0.08 rad`，也会自动停止。

```bash
./home.sh start
./home.sh status
./home.sh cancel
```

`home.sh start` 会引起机器人双臂运动，执行前必须确认防摔、净空和急停。当前 Home
角度保存在 `config.toml` 的 `home.joint_goal_rad` 中。

## 部署与检查

程序目录放在机器人算力主机 `/home/nvidia/tg3_local_teleop`。只读监测测试：

```bash
./run.sh --duration 10
./status.sh
```

安装并启用用户服务自动启动：

```bash
mkdir -p ~/.config/systemd/user
cp tg3-local-teleop.service ~/.config/systemd/user/
systemctl --user daemon-reload
systemctl --user enable tg3-local-teleop.service
```

现场确认防摔、双臂活动范围无人、急停可用后，才可启动：

```bash
systemctl --user start tg3-local-teleop.service
systemctl --user status tg3-local-teleop.service
```

停止与查看状态：

```bash
systemctl --user stop tg3-local-teleop.service
./status.sh
```

当前 `config.toml` 由桥接程序直接建立 OmniSocket Session，不再经过机器人本机 ZMQ
接收代理。若需回退到机器人与工控机的局域网直连，可把 `network.transport` 改成
`zmq`；保留的 `iarm_endpoint` 为 `tcp://192.168.5.14:5003`。

机器人侧不再以 `0.25 s` 公网输入间隔解除武装。活动会话超过 `2 s` 仍无帧时重建机器人
OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或收到 STOP 后没有业务帧是
正常待机，不触发反复重启。只有匹配的操作员 STOP 自动回 Home；意外断网不自动产生
回位运动。

## 完整重启顺序

只重启本项目的网络与机器人桥时，先启动 Nvidia 接收端，再启动 EAI 本地门控服务；
EAI 在收到物理 START 前不会建立发送 Session：

```bash
ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
```

检查：

```bash
ssh eai 'systemctl --user --no-pager status tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user --no-pager status tg3-local-teleop.service'
```

修改 Ubuntu 的 `xmigcs/config/dex_config.yaml` 后，必须重启 xmigcs 才会加载新值。现场
固件没有公开的单独 `rl/xmigcs` 启停 service、action 或 CLI；
`/proc_manager/config/notify` 只通知进程管理器重新读取它自己的固定
`proc_manager.json`，消息内容只写入日志，不能执行 `CMD_STOP_PROC` 或
`CMD_START_PROC`。不要向该话题发送进程启停 JSON。

首选做法是先退出遥操并停止本项目两个服务，再按厂商原有物理流程切到 `48V Off`。
现场 `proc_manager` 会在真实的 `48vOff` 状态下停止 `rl` 和 `robot_control`；随后按原流程
恢复 48V 并启动机器人，`robotcontrol_state` 恢复后会重新创建 xmigcs，新进程才会读取
修改后的配置：

```bash
ssh eai 'systemctl --user stop tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user stop tg3-local-teleop.service'

# 此处按厂商原有物理流程切到 48V Off。
# 本机断电后 Ubuntu/网络可能同时离线，SSH 失败是正常现象，不在断电期间执行检查。

# 按厂商原有流程恢复 48V 并启动机器人，等待状态初始化完成；下列命令应显示新 PID。
ssh ubuntu@192.168.41.1 \
  "pgrep -af '^/usr/bin/python3 /home/ubuntu/.local/bin/xmigcs '"
ssh ubuntu@192.168.41.1 \
  "ps -eo pid,lstart,comm,args | grep '[x]migcs'"

ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
```

不要直接 `kill` xmigcs，也不要仅为加载此配置执行
`sudo systemctl restart proc_manager.service`。后者会同时停止 joystick、robot_control、
xmigcs 等全部子进程；而 `robot_control` 和 `rl` 均为 `boot_start=false`，可能不能自动
恢复，不能把它当作 xmigcs 单进程重启命令。

EAI 当前用户服务虽已启用，但该用户的 systemd linger 为关闭状态；重启命令可通过 SSH
正常执行，整机冷启动后则需先登录 EAI。若需要无人登录也随系统启动，只执行一次：

```bash
sudo loginctl enable-linger eai
```

当前厂商配置备份位于：

```text
/home/ubuntu/.local/lib/python3.12/site-packages/xmigcs/config/
  dex_config.yaml.before-local-teleop-20260807
```