meiqi/TG3

Files

meiqi d02b99d2aa feat: capture head and waist RGB-D data

2026-08-10 16:40:00 +08:00

16 KiB

Raw Blame History

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

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

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，也会自动停止。

./home.sh start
./home.sh status
./home.sh cancel

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

部署与检查

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

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

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

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

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

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

停止与查看状态：

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

当前 config.toml 由桥接程序直接建立 OmniSocket Session，不再经过机器人本机 ZMQ 接收代理。不走公网时推荐在 EAI 运行本地 kcpserver Hub：EAI sender 指向 127.0.0.1:14049，机器人保持 network.transport="omnisocket" 并把 omnisocket_server 改为 EAI 局域网地址。这样保留 5003+5001 合并、session 和 Peer 校验，完整步骤见迁移指南第 6 节。

若一定要完全无 Hub，可把 network.transport 改成 zmq；保留的 iarm_endpoint 为 tcp://192.168.5.14:5003。纯 ZMQ 模式只读取 5003 原始帧，不合并 5001 的其他六维手势；此前删除的 0.25 秒输入陈旧门控和下面的 2 秒 OmniSocket 重建看门狗都不作用于 ZMQ，因此断流会保留最后输入。当前切换到纯直连时仅允许 monitor-only 链路验证：只停用行走仍不能防止陈旧 Z+C 在断网后计时或双臂/手继续使用旧帧。完成 ZMQ 专用断流门控前不得启用运动；本地 OmniSocket Hub 推荐方案见 docs/天工3.0本地同构臂遥操迁移部署指南.md 第 6 节，纯 ZMQ 备选见第 7 节。

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

独立数采

数采使用左摇杆按下键（L3），不是左侧 X/Y/Z 面键：

遥操已成功开启后，先保持 L3 松开至少 0.5 s；
连续按住 L3 1 s 开始一条 episode；
松开至少 0.5 s，再次连续按住 1 s 正常结束；
长按 Z+C 结束遥操、安全解除或桥退出时，也会异步请求结束当前 episode；
数采失败只记录错误，不解除遥操、不阻塞 50 Hz 控制，也不延迟 STOP/Home。

button_joystick.left 是 xTELE 5003 中独立的摇杆按压字段。已安装的 xTELE 0.1.2 没有给它注册处理函数；X/Y/Z/A/B/C 均已有厂家功能或本项目绑定，因此不复用面键。新会话和服务重启后都先锁定为“必须松开”，畸形或陈旧按键帧不能被当作有效松开。

项目自有 tg3-data-recorder.service 在 Nvidia 上运行独立 ros2 bag record，不停止、重配或接管 Ubuntu 厂家 /record_bag_node。仅录 config.toml 中的明确白名单，包含 /robot_state、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE 应用帧，以及头/腰 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 -a 或录点云。

为避免 1280x720 原始 RGB+深度每台相机约 8.3 GB/min 的持续写盘负载，默认录制 color/image_raw/compressed（JPEG）和 depth/image_raw/compressedDepth（16UC1 无损 PNG）。现场单台腰部相机实测合计约 22–25 MB/s（约 1.3–1.5 GB/min，随场景变化）。腰部图像、内参、Metadata 和 /tf_static 是 required topics；两路图像还必须在整条 bag 内分别达到至少 20 Hz 平均频率。相机服务未发布或图像严重掉流时， L3 启动或最终校验会失败，不会生成缺图像却标记 READY 的 episode。头部话题在白名单中但不是 required：当前 orbbec_head.service 未启动不影响腰部数采；将来在 episode 开始前启动后，rosbag 会自动发现并采集，无需再改配置。静态 TF 会被保存用于相机外参，但当前收尾只验证该 Topic 非空，不解析每条 TF 的 frame 语义。

机器人暂存目录：

/home/nvidia/tg3_data_collection/
  active/   # 尚未完成，不能取走
  ready/    # 已正常收尾、校验并生成 READY
  failed/   # 启动、磁盘、进程或校验失败，保留供诊断

MCAP 使用 zstd_fast、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟，加入双 RGB-D 后启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完 metadata.yaml，随后执行 ros2 bag info，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写 manifest.json 和 READY，最后才原子进入 ready/。Nvidia 需已安装 python3-yaml （现场已验证 PyYAML 6.0.1）。

相机由厂家系统服务管理，本项目只订阅、不自动启停。录制前检查或按需启动：

systemctl is-active orbbec_waist.service orbbec_head.service
sudo systemctl enable --now orbbec_waist.service
# 需要头部图像时再执行；启动后下一条 episode 会自动包含头部 RGB-D：
sudo systemctl enable --now orbbec_head.service

录制服务只加载厂家完整 /opt/robot_tele_server/install 消息 overlay，不加载本项目 ros2_py/install；项目 overlay 仅供桥的 Python ArmStatus 绑定使用。两者混在同一个 rosbag 环境会让部分 ros2_bridge_msgs C++ 类型支持出现动态库符号冲突。

部署两个机器人侧服务：

mkdir -p ~/.config/systemd/user
cp tg3-data-recorder.service tg3-local-teleop.service ~/.config/systemd/user/
systemctl --user daemon-reload
systemctl --user enable --now tg3-data-recorder.service
systemctl --user restart tg3-local-teleop.service

只读检查：

systemctl --user --no-pager status tg3-data-recorder.service
ros2 topic echo --once /tg3/data_collection/status
python3 -m json.tool /home/nvidia/tg3_local_teleop/status.json
find /home/nvidia/tg3_data_collection/ready -mindepth 1 -maxdepth 1 -type d

完成 episode 由 PS 本机服务校验后保存到项目 Data_Get，详见 ../tg3_data_collection/README.md。机器人 ready/ 中的副本不会自动删除。

完整重启顺序

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

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

检查：

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，新进程才会读取修改后的配置：

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。若需要无人登录也随系统启动，只执行一次：

sudo loginctl enable-linger eai

当前厂商配置备份位于：

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

16 KiB Raw Blame History Unescape Escape