BrainCo Revo2 简易视觉遥操

本项目在 Ubuntu 22.04 和 ROS 2 Humble 环境中，使用 Intel RealSense D435i 或笔记本普通摄像头捕捉操作者右手，通过 MediaPipe 识别手部关键点，再将动作映射到 MuJoCo 中的 BrainCo Revo2 二代灵巧手。

当前版本的目标是建立一条可观察、可测试并具备基本安全状态机的仿真控制链路：

摄像头图像
  -> MediaPipe 右手 21 个关键点
  -> 深度/画面有效性检查
  -> 操作者标定与六通道动作映射
  -> 滤波、关节限位、速度和加速度限制
  -> 离合、显式使能、HOLD、急停状态机
  -> MuJoCo Revo2 灵巧手

安全说明：默认启动文件只控制 MuJoCo 仿真，不会向实际 Revo2 发送命令。 真机通信、电流限制、温度保护、堵转保护、硬件看门狗和物理急停尚未完成验收， 因此不能把当前版本直接用于真机闭环控制。

一、当前已经实现的功能

• D435i 彩色图像和对齐深度图采集，默认分辨率为 640×480@30 Hz。
• 普通 USB/笔记本摄像头采集，默认使用 /dev/video0 和 MJPG。
• MediaPipe Hand Landmarker 单右手 21 点识别。
• D435i 手掌区域鲁棒深度估计和 0.35～0.70 m 工作距离检查。
• 张手、握拳、拇指外展三姿态的操作者标定。
• 六个主动电机通道的语义映射：拇指弯曲、拇指外展、食指、中指、无名指、小指。
• One Euro 滤波、目标关节限位、速度限制和加速度限制。
• 带锁存行为的安全状态机：离合、显式使能、HOLD、故障、急停和复位。
• MuJoCo 中六个主动关节和五个被动耦合关节的控制与反馈。
• D435i 和普通摄像头两条仿真启动路径。
• 26 项离线算法及 MuJoCo 回归测试。

历史实测中，D435i 以 USB 3.2、5 Gbit/s 连接，统一手部观测接近 30 Hz， MuJoCo 状态反馈约为 50 Hz。D435i 链路已经验证过动作变化、跟踪丢失进入 HOLD、 急停锁存和进程正常退出。

二、目录结构

revo2-d435i-mujoco-teleop/
├── DeepCamera/
│   ├── config/                 # D435i、跟踪和 RealSense ROS 参数
│   ├── models/                 # 下载后生成，不提交模型二进制
│   ├── ros2_ws/src/
│   │   ├── hand_landmarker/    # D435i/普通相机手部识别节点
│   │   └── depth_gate/         # ROS 图像路径的深度检查节点
│   └── vendor/                 # 上游依赖，脚本下载，不进入 Git
├── RobotHand/
│   ├── calibration/            # 本机操作者标定
│   ├── config/                 # 关节限制和安全参数
│   ├── docs/                   # 执行计划与环境记录
│   ├── ros2_ws/src/
│   │   ├── revo2_interfaces/   # 自定义 ROS 消息
│   │   ├── revo2_retarget/     # 手部特征到 Revo2 的映射
│   │   ├── revo2_safety/       # 安全状态机和安全目标门控
│   │   ├── revo2_mujoco/       # MuJoCo 模型生成和 ROS 桥接
│   │   └── revo2_bringup/      # 标定及仿真启动文件
│   ├── tests/                  # 离线测试
│   └── vendor/                 # BrainCo 上游依赖，不进入 Git
├── conda/                      # 两套 Conda 环境及锁定文件
├── scripts/                    # 安装、构建、测试、标定和启动脚本
├── repo-lock.yaml              # 上游仓库提交和模型哈希
└── THIRD_PARTY.md              # 第三方依赖及许可证说明

三、软件和硬件要求

推荐环境：

• Ubuntu 22.04。
• Git、curl、unzip 和 Conda/Miniconda。
• ROS 2 Humble 由项目的 RoboStack Conda 环境提供。
• MuJoCo、MediaPipe 和 RealSense Python 库由 Conda 环境管理。
• D435i 路径需要 Intel RealSense D435i 和 USB 3 数据线/接口。
• 普通相机路径需要可通过 V4L2/OpenCV 读取的摄像头。
• MuJoCo 图形窗口需要可用的桌面显示和 OpenGL/GLX。

本项目不要与已经 source /opt/ros/humble/setup.bash 的终端混用。APT ROS 和 Conda ROS 的 Python/C++ ABI 及依赖来源不同，混用可能导致难以定位的导入或链接错误。

四、从零安装

1. 克隆项目

git clone https://github.com/Hakurei-Ma-risa/revo2-d435i-mujoco-teleop.git
cd revo2-d435i-mujoco-teleop

2. 下载固定版本的第三方依赖

./scripts/fetch_dependencies.sh

该脚本会完成以下操作：

• 从上游仓库下载运行和构建所需的 BrainCo 与 RealSense 项目。
• 将各仓库切换到 repo-lock.yaml 中固定的提交。
• 安装构建 BrainCo 驱动所需的 Stark SDK。
• 下载 MediaPipe 手部模型并校验 SHA-256。

可选研究项目默认不下载。如需 AnyDexRetarget、dex-retargeting 和其他参考仓库：

./scripts/fetch_dependencies.sh --with-references

部分上游参考仓库在锁定版本中没有声明许可证，只能按照其上游条款用于研究参考， 不要直接复制或重新发布其中的源码和模型。

3. 创建 Conda 环境

轻量环境用于离线算法、MuJoCo 和测试：

./scripts/setup_conda_env.sh

ROS 2 Humble 环境用于构建和运行完整链路：

./scripts/setup_ros_conda_env.sh

环境名称分别是：

• revo2-teleop
• revo2-ros-humble

4. 构建 ROS 2 工作区

在没有激活 /opt/ros/humble 的新终端中运行：

./scripts/build_ros2.sh

构建脚本会先根据固定的 BrainCo MJCF 生成本机 MuJoCo 控制模型，然后依次构建 RobotHand 和 DeepCamera 两个 ROS 2 overlay。

5. 运行离线测试

./scripts/run_offline_tests.sh

正常结果应为：

26 passed

测试通过只代表算法和仿真模型满足当前回归条件，不代表摄像头、USB 或真机硬件已经验收。

五、使用 D435i 遥操作 MuJoCo

1. 安装 RealSense udev 规则

首次使用时，在项目根目录执行：

sudo install -m 644 \
  DeepCamera/vendor/librealsense/config/99-realsense-libusb.rules \
  /etc/udev/rules.d/99-realsense-libusb.rules
sudo udevadm control --reload-rules
sudo udevadm trigger --subsystem-match=usb

执行后重新插拔 D435i。不要在主目录直接使用相对路径运行这条命令，否则会出现 “没有那个文件或目录”。

2. 检查相机和 USB 连接

source scripts/activate_revo2_ros.sh
rs-enumerate-devices -s
lsusb -t

应当能够看到 D435i，并确认 USB 速率为 5000M 或更高。若显示 480M，说明相机 工作在 USB 2 模式，通常需要更换接口、数据线、转接器或避免经过低速 Hub。

默认配置不保存相机序列号，会自动选择第一台 RealSense。如果同时连接了多台相机， 可在本机配置中指定序列号，但不要把硬件序列号提交到公开仓库。

3. 进行操作者标定

./scripts/calibrate_operator.sh operator

在 D435i 预览窗口中保持完整右手可见，根据提示使用：

• 1：采集张开手掌。
• 2：采集握拳。
• 3：采集张手且拇指充分外展。
• 4：三组姿态完成后保存。

每种姿态会取 60 个有效帧的中位数。标定结果写入 RobotHand/calibration/operator.yaml，该文件只保存在本机，不会上传 GitHub。 操作者、相机位置或拍摄角度明显改变后应重新标定。

4. 启动 D435i 仿真链路

./scripts/start_teleop_sim.sh

保持完整右手位于画面内，手掌距离相机约 0.35～0.70 m。看到预览窗口显示 DEPTH OK 后再执行使能操作。

六、使用笔记本普通相机

如果只需要测试仿真部分，可以运行：

./scripts/start_webcam_sim.sh

默认打开 /dev/video0。如果图像设备编号不同：

./scripts/start_webcam_sim.sh camera_index:=2

普通相机没有真实深度，因此使用手掌大小和画面边界构成“视觉工作区检查”，预览中会显示 VISUAL GATE OK。这只能排除手太小、太大或接近画面边缘的情况，不能判断真实距离， 安全能力低于 D435i。

当前普通相机路径还没有独立标定流程，启动脚本仍要求存在 RobotHand/calibration/operator.yaml。可以暂时复用 D435i 标定做链路调试，但它不是 普通相机的最终可靠标定方案。

七、仿真操作方法

D435i/普通相机预览窗口和 MuJoCo 窗口目前都能接收以下按键：

• C：切换仿真离合状态。
• E：在跟踪连续有效至少 10 帧后显式进入 ENGAGED。
• X：锁存急停。
• R：清除急停并释放离合。

推荐操作顺序：

1. 等待预览窗口显示跟踪和工作区检查有效。
2. 按一次 C 接合离合。
3. 按一次 E 显式使能。
4. 缓慢张合右手，确认 MuJoCo 灵巧手跟随。
5. 需要停止时按 X，或释放离合。

不要在预览窗口和 MuJoCo 窗口各按一次 C。两个窗口会向同一个离合话题发送命令， 连续两次按键相当于打开后又关闭。

跟踪丢失或目标超时后，状态机会进入 HOLD。即使图像恢复，也不会自动重新使能； 必须释放并重新接合离合，再按 E。该设计用于避免恢复画面瞬间产生意外动作。

八、运行状态和诊断

在另一个干净终端中：

source scripts/activate_revo2_ros.sh
ros2 node list
ros2 topic echo /teleop/status --once
ros2 topic echo /teleop/revo2_target_raw --once
ros2 topic echo /teleop/revo2_target_safe --once
ros2 topic echo /sim/revo2/joint_states --once
ros2 topic hz /teleop/hand_observation_depth
ros2 topic hz /revo2/motor_state

仿真运行时应包含以下主要节点：

/d435i_hand_tracker 或 /webcam_hand_tracker
/revo2_retarget
/revo2_safety
/revo2_mujoco_bridge

主要数据链路：

话题	作用
/teleop/hand_observation_depth	手部关键点、跟踪状态和工作区检查结果
/teleop/revo2_target_raw	标定和映射后的原始 Revo2 目标
/teleop/revo2_target_safe	通过状态机门控后的安全目标
/teleop/status	离合、状态、数据新鲜度和故障原因
/sim/revo2/joint_states	MuJoCo 关节状态
/revo2/motor_state	仿真中的 Revo2 电机反馈

如果画面正常但灵巧手不动，应沿上述顺序逐层检查：

1. 手部观测中的 tracking_valid 和 depth_valid 是否为真。
2. 原始目标的 valid 是否为真，position[] 是否随张合手变化。
3. 安全目标是否只在 ENGAGED 时有效并发生变化。
4. 状态是否保持 ENGAGED，目标和反馈是否新鲜。
5. MuJoCo 关节位置是否跟随安全目标。

第一处不再变化的数据就是优先排查的层级。

九、正常关闭与残留进程检查

在启动仿真的终端按一次 Ctrl+C。正常情况下，相机、重定向、安全节点和 MuJoCo 桥接节点都会退出并释放视频设备。

如怀疑存在残留进程，先检查：

ps -eo pid,ppid,stat,cmd | grep -E \
  'd435i_hand_tracker|webcam_hand_tracker|revo2_retarget|revo2_safety|revo2_mujoco|teleop_sim'

确认具体 PID 后优先发送 SIGTERM，只有进程确认无响应时才考虑 SIGKILL。检查摄像头 是否仍被占用：

for dev in /dev/video*; do fuser "$dev" 2>/dev/null; done

十、开发中遇到的问题及解决方法

1. RealSense udev 规则找不到

现象：从主目录执行安装命令时，提示规则文件不存在。

原因：命令使用了相对于项目根目录的路径，但当前终端位于其他目录。

处理：进入仓库根目录后执行安装命令，或者使用文件的绝对路径。

2. 无法确认 D435i 是否工作在 USB 3

只看到设备名称并不能证明带宽正确。项目最终结合 lsusb -t、sysfs 和 librealsense 确认链路为 USB 3.2、5000 Mbps。低于 USB 3 时，双路 640×480 彩色/深度数据容易降帧， 默认 D435i 节点会拒绝非 USB 3 设备。

3. ROS 原始图像传输帧率不足

直接 librealsense 可以接近 30 Hz，但最初通过 ROS 同时传输彩色和对齐深度图时， Fast DDS 只测得约 7.6 Hz 彩色和 2.2 Hz 深度。切换到 Cyclone DDS 和传感器数据 QoS 后有所改善，但两个原始图像流仍不能稳定达到 30 Hz。

最终处理：默认节点在同一进程中完成 RealSense 采集、深度对齐、MediaPipe 推理和 手掌深度检查，只发布轻量的 HandObservation。官方 RealSense ROS 图像链路保留用于 诊断和 rosbag 采集，不作为默认控制传输路径。

4. RealSense ROS wrapper 偶发警告

官方 wrapper 曾出现 libusb control-transfer Resource temporarily unavailable，以及一次 运动模块相关警告。直接 SDK 的持续彩色、深度和 IMU 测试能够通过，因此当前控制链路 默认使用直接 SDK；wrapper 只作为诊断备用。若直接 SDK 也失败，再检查 USB、udev、 设备占用、固件和权限。

5. MediaPipe 关闭时可能卡死

当前 Linux/MediaPipe 组合中，VIDEO 模式的 HandLandmarker.close() 偶尔会在原生代码 中死锁。放入 Python 线程也无法可靠解决，因为原生调用可能一直持有 GIL。

当前绕过方式：信号处理器只设置停止事件，主线程先释放相机和 ROS 资源，跟踪进程最后 使用 os._exit() 退出，避免调用有问题的 MediaPipe 析构路径。在替换该逻辑前必须做 反复启动/停止压力测试，否则可能重新出现关不掉进程或摄像头一直被占用的问题。

6. MuJoCo 最初没有可视化窗口

最初的桥接节点只在后台运行。后来加入 mujoco.viewer.launch_passive()、约 60 Hz 的 窗口同步和按键控制，才形成可见的遥操作验证界面。

7. MuJoCo 退出时出现 GLXBadContext

关闭窗口后立刻结束进程，会使 GLX 后台线程来不及清理。现在使用事件驱动主循环， 显式调用 viewer.close()，并预留短暂清理时间。图形驱动或桌面环境不同的机器仍需 重新进行多次启停测试。

8. Revo2 MuJoCo 模型数值不稳定

同时启用重力、复杂网格碰撞、较硬的位置控制和关节耦合时，模型会出现高频振荡甚至 数值发散。当前采取：

• implicitfast 积分器。
• 暂时关闭重力。
• 暂时关闭网格碰撞。
• 位置增益 kp=2.0。
• 速度增益 kv=0.1。
• 增加关节阻尼。
• 使用约 0.02 s 的耦合约束时间常数。

这些设置适合验证动作映射和控制稳定性，但不适合宣称抓取真实性。

9. 两个窗口重复发送快捷键

相机预览和 MuJoCo 窗口都会发布离合、使能和急停请求。若在两个窗口重复按 C， 离合会被切换两次，使后续 E 被拒绝。当前应只选择一个窗口输入控制快捷键；后续需要 统一的操作界面和唯一的快捷键输入源。

十一、当前已知 Bug 和限制

尚未完成验收的普通相机动态控制

普通相机链路能够打开 /dev/video0、运行 MediaPipe、进入 ENGAGED，并以约 50 Hz 收到 MuJoCo 状态。但一次 8 秒采样中，11 个关节的位置变化范围全部为 0.0 rad。 可能原因包括操作者在采样期间没有明显动作、跟踪短暂进入 HOLD、复用的 D435i 标定不适合 普通相机，或某一层目标没有发生变化。

因此目前只能确认普通相机链路“可以启动”，不能确认它已经可靠完成动态遥操作。应按照 第八节依次记录原始目标、安全目标和 MuJoCo 关节状态，找到第一处零变化的层级，并增加 普通相机专用标定后重新验收。

其他限制

1. MuJoCo 中重力和碰撞关闭，尚不能进行可信的接触和物体抓取实验。
2. 五个远端指关节采用固定等式耦合，没有独立的指尖优化重定向。
3. 单相机 MediaPipe 在手指相互遮挡或快速转动时可能丢失关键点。
4. D435i 深度当前只用于手掌距离检查，没有为 21 个关键点逐点融合真实深度。
5. 普通相机没有绝对距离信息，却复用了消息中的 depth_valid 字段表示视觉区域有效； 后续应该把深度有效与普通相机视觉有效拆成不同字段。
6. 普通相机专用操作者标定尚未实现。
7. MuJoCo 控制模型依赖下载到固定目录的上游网格文件，不能随意改变 vendor 目录结构。
8. 当前没有验证实际 Revo2 的通信、编码器零位、方向、减速比、电流、温度、堵转和故障状态。
9. 当前没有真机看门狗、电气隔离和物理急停验收，真机命令后端必须保持禁用。

十二、后续建议

推荐按以下优先级继续开发：

1. 完成普通相机的独立标定和逐层动态目标验收。
2. 对 D435i 的每个关键点采样邻域深度，并反投影到相机坐标系，形成真正的度量 3D 手部输入。
3. 记录图像时间戳、推理完成时间、目标发布时间和 MuJoCo 应用时间，量化端到端延迟。
4. 使用基本体或凸包替代高密度碰撞网格，逐步恢复重力、接触、桌面和抓取物体。
5. 真机首先只读设备身份、固件、位置、电流、温度和故障，不发送控制指令。
6. 逐电机确认零位、方向、限位和反馈，再进入只计算不下发的 shadow mode。
7. 加入硬件看门狗、电流/温度/堵转限制和物理急停后，才进行低速低电流台架测试。