feat: package TG3 TS1P OmniSocket teleoperation
This commit is contained in:
185
tg3_local_teleop/README.md
Normal file
185
tg3_local_teleop/README.md
Normal file
@@ -0,0 +1,185 @@
|
||||
# 天工 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 + 左摇杆；50 Hz TwistStamped）
|
||||
-> 天工 3.0 HBWALK 行走
|
||||
```
|
||||
|
||||
## 自动运行与操作
|
||||
|
||||
- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
|
||||
- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
|
||||
本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
|
||||
- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
|
||||
机器人立即停控并以 `0.25 rad/s` 自动回 Home。
|
||||
- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
|
||||
- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
|
||||
前后/转向；不控制腰和头。
|
||||
- 行走：遥操已启动后，右 `C` 是即时 deadman；按住 C 并把左摇杆推出死区后，在下一个
|
||||
`50 Hz` 周期立即响应，不再等待 3 秒。松开 C 或摇杆回中立即发零速，再按也立即恢复。
|
||||
左摇杆上下控制前后，左右控制原地转向。
|
||||
按官方半身行走 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 后续直接输出六维归一化位置，会自动使用六维位置。
|
||||
|
||||
## 安全门控
|
||||
|
||||
只有以下条件全部满足才允许开始：机器人 `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 + 左摇杆
|
||||
即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
|
||||
回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
|
||||
|
||||
灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
|
||||
位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
|
||||
和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
|
||||
或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
|
||||
最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
|
||||
|
||||
## 限速双臂回 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
|
||||
```
|
||||
Reference in New Issue
Block a user