作者：林宇恒 单位：北京邮电大学具身智能机器人操作实验室 更新日期：2026年8月4日
| 组件 | 版本 |
|---|---|
| Ubuntu | 22.04.5 LTS (Jammy) |
| Kernel | 6.8.0-124-generic |
| ROS | ROS 2 Humble (Desktop) |
| Python | 3.10 |
| Conda | 25.11.1 |
| PyTorch | 2.13.0 |
| MuJoCo | 3.10.0 |
| stable-baselines3 | 2.9.0 |
| Gymnasium | 1.3.0 |
| NumPy | 2.2.6 |
| OpenCV-Python | 5.0.0.93 |
| Matplotlib | 3.10.9 |
├── config // 配置
├── imgs // 演示图片
├── robot_model // 机器人模型文件
├── src // 源码
│ ├── bodyctrl_msgs/ // 机器控制相关话题
│ ├── rh5dg2_interfaces/ // G2相关话题
│ ├── msgs/ // 该功能包需要的话题
│ ├── hand/ // F2/G2 灵巧手手势控制
│ ├── moveit/ // moveit规划、手臂位姿初始化
│ ├── sim/ // 仿真环境，包括仿真环境打开、相机读取、关节控制
│ ├── convert/ // 模型转换工具
│ ├── tiansuo/ // 遥操臂
│ ├── tianyi/ // 机器人控制
│ ├── camera/ // 传统视觉测试
│ └── testall/ // 测试脚本
├── .gitignore
├── CLAUDE.md
└── readme.md
将urdf模型转换为mujoco所需的mjcf模型(已经转换好了，要转换自己的模型需修改文件路径)
python3 src/convert/urdf_to_mjcf.py添加仿真摄像头 直接ai改吧[doge]。提示词示例：
为tianyi2.xml的机器人头部添加深度摄像头，生成新的xml文件，不要覆盖原文件，摄像头可以输出RGB图像与深度图像。摄像头具体参数如下：奥比中光 Gemini 335 是一款双目结构光 RGB‑D 相机。彩色传感器分辨率 1920×1080 @30fps，视场角水平 69°、垂直 42°、对角线 77°；深度传感器分辨率 1280×800 @30fps，视场角水平 79°、垂直 63°、对角线 93°，工作距离 0.2 m–2.5 m，深度精度优于 2%（2 m 内），彩色与深度已对齐，基线约 40 mm，整机尺寸约 95×35×25 mm，重量约 100 g，USB 3.0 接口。在 MuJoCo 仿真中将其建模为头部固定相机：采用彩色传感器的垂直 FOV 42°（fovy="42"），分辨率设为 1920×1080（可降低至 640×480 用于训练），深度裁剪近平面 near="0.2"、远平面 far="2.5"，内参由视场角和分辨率自动计算。位置安放在机器人头部连杆前上方（示例局部坐标 pos="0.12 0 0.08"），光轴指向前方，四元数默认为 1 0 0 0（需根据头部坐标系调整为看向场景方向）。渲染时通过同一相机同时获取 RGB 图像（renderer.render()）和深度图像（renderer.render(renderer.DEPTH)），真实深度由 depth * (far - near) + near 换算。创建场景 比较复杂的场景可以自行寻找模型，对于简单的场景可以直接AI生成XML文件(一次不能生成完美的模型，多次调整)。提示词示例：
创建场景xml文件scene_connectline.xml， 包含一个桌子（1.2m*0.8m，高0.75m，视为刚体）、有USB-A接口的充电头（0.03m*0.055m*0.055m，视为刚体，在其中一个0.03m*0.055m的面上有一个标准USB-A接口12mm*4.5mm，深度为14mm，内部舌片厚2.2mm，距离四周均有一定距离保证线头可以插入），要跟长为1m的USB-A转TYPEC线材，USB-A端线头尺寸与上面接口对应，线头经过个15mm*5mm*19mm的刚体与线体连接，线头视为刚体，线体可视为多段链条，桌子是固定的物体，充电头和线都是可以自由物体合并场景与机器人
python3 src/convert/merge_scene.py场景测试
source install/setup.bash
ros2 launch sim sim.launch.py
关节控制和关节数据读取都是JointState.msg，topic分别是"/joint_command"和"/joint_states"
关节控制测试
source install/setup.bash
ros2 launch sim sim.launch.py再开一个终端
source install/setup.bash
ros2 run testall joint_control_test从一个简单的逆解改为使用moveit2,moveit效果可以在rviz中独立演示，也可以桥接入mujoco，桥接节点将/sim/joint_states转化为moveit需要的/joint_states，/moveit/arm/planned_trajectory转化为/joint_command控制仿真
moveit独立测试（仅 RViz，不连仿真）
source install/setup.bash
ros2 launch moveit demo.launch.py手臂位姿初始化
source install/setup.bash
ros2 run moveit arm_init_node.pyik_node 订阅双臂 delta_pose_joint 组合增量话题（位姿+关节增量），与 plan_node 分工：plan_node 处理 pose/delta_pose/joint_target/delta_joint 四种模式，ik_node 专管遥操微操的组合增量——位姿增量叠加当前末端位姿，统一调 /compute_ik 以"目标位姿+关节约束"求解 7 关节解（无关节增量时退化为纯位姿求解），结果直接发布到 /joint_command 关节指令话题，不经 MoveIt 规划/轨迹执行（实测混合 goal 采样无法命中故统一经 IK 转为关节指令；直接发布与遥操直控机制一致）。
source install/setup.bash
ros2 run moveit ik_node.py遥操按钮增量模式（mode 1/2 摇杆微操）的 delta_pose_joint 由 teleop_sim.launch.py 自动启动 ik_node 处理，无需手动运行。
IK 解发布前先插值：计算目标值与反馈当前值（/joint_states）各关节角度差，取最大角度差 max_delta，插值步数 n = max(1, ceil(max_delta / interp_step_rad))；n>1 时按 interp_interval 间隔逐拍发布第 k 步（当前 + (目标-当前)·k/n），避免单次大增量直接跳变导致 PD 超调。插值基准取反馈当前值（非上次命令值）：增量目标的语义本就是"当前位置 + 增量"，从反馈值起步保证命令单调、无回退无漂移；新指令到达时未发完的插值序列取消，重新以最新反馈值插值。参数在 config/ik_node.yaml 配置（节点自行读取，launch 可覆盖）。
位姿叠加锚点（命令基准）：增量叠加在"上次命令目标位姿"而非 TF 实际位姿上——关节空间插值的中间点/PD 滞后会使实际位置偏离命令，若以实际位置为叠加基准，偏离被锁定进新目标并累积漂移（速度式增量无绝对锚点，FK 校验只能保证解到目标，管不到目标本身被污染）。锚点仅在首次进入增量模式、或与实际位置偏离超过 anchor_resync_dist（期间被其他控制移动过）时用 TF 重新初始化。
实测 KDL 带关节约束（如 wrist_roll）求解时约束优先，末端位置精度被牺牲，速度式增量下位置误差同向累积（wrist_roll 微操时 TCP 的 x/z 系统性漂移）。ik_node 每解出 7 关节后调 /compute_fk 验证实际末端位置：与目标位姿偏差超 pose_tolerance（默认 5mm）时以"目标位置 += 偏差"补偿后重试一次 IK（同一关节约束），仍超差仅告警——从机制上阻断漂移累积。配合 kinematics.yaml 中 kinematics_solver_timeout 调大到 0.1s（原 0.005s 对 7 DOF 数值 IK 过短易未收敛）。
source install/setup.bash
ros2 run moveit ik_node.py # 插值参数见 config/ik_node.yaml（interp_step_rad / interp_interval）moveit_bridge_node 将仿真状态同步至 /joint_states；trac_node 接收 MoveIt 规划结果执行轨迹，两节点共同替代 ros2_control mock 硬件。
moveit+mujoco联合启动（一键启动仿真 + MoveIt + 桥接 + 手臂初始化 + 手势控制）
source install/setup.bash
ros2 launch sim moveit_sim.launch.py # 完整启动（含 RViz）
ros2 launch sim moveit_sim.launch.py rviz:=false # 无 RViz也可以在 RViz 里直接点击 Plan & Execute 规划运动到目标位姿。
scene_objects_node 启动时向 MoveIt 发布静态障碍物（任务桌子），并发布允许碰撞矩阵（PlanningScene diff）：插线任务要求指尖在桌面表面操作（压线、贴桌面插线），若桌面为完全禁碰障碍物，起点/目标指尖贴桌会被判碰撞导致规划失败（RRTConnect 目标树采样失败）。故在 moveit/config/scene_objects.yaml 的 allowed_contacts 中允许 8 个指尖 link 与 table 接触，前臂/手腕等其余部位仍与桌面避碰。
ros2 run moveit scene_objects_node.pybodyctrl_msgs 包与机器人上话题相同。连接机器后，robot_relay_node 将真实机器人运动映射到rviz上，演示用。
source install/setup.bash
ros2 launch tianyi real_robot_rviz.launch.pymotion_record_node 订阅真实机器人的控制指令话题，保存为 CSV 文件。motion_replay_node 读取多个 CSV 路径，通过 /motion_replay/play（Int8）接收回放索引，按原始时间间隔将关节指令重建并发布回真实机器人话题。
ros2 run tianyi motion_record_node
ros2 service call /motion_record/start std_srvs/srv/Trigger # 开始录制
ros2 service call /motion_record/stop std_srvs/srv/Trigger # 停止并保存
ros2 run tianyi motion_replay_node
ros2 service call /motion_replay/play msgs/srv/Replay "{index: 0}" # 回放第 0 个文件sim_bridge_node 将机器人控制话题转换成仿真控制，用于在仿真中查看录制动作
ros2 launch tianyi sim_replay.launch.py
ros2 service call /motion_replay/play msgs/srv/Replay "{index: 0}" # 回放第 0 个 CSV真机位置控制指令（CmdMotorCtrl）按 motor_id 配置不同 PD 增益，写入 tianyi/config/motor_groups.yaml 的 pd_gains 块：肩/肘大关节刚度大、腕小关节刚度小，左臂 11-17 与右臂 21-27 对称。ctrl_node（遥操桥接）与 motion_replay_node（回放）构造指令时读取，未配置的电机 kp=kd=0 不施加跟踪力。同文件顶层 publish_rate 控制 ctrl_node 真机指令发布频率（遥操/回放指令更新节奏，决定从端平滑度；rclpy 定时器 500Hz 为边缘值，200~300Hz 更稳），yaml 优先、launch 参数兜底。
# 无独立启动，随 ctrl_node / motion_replay_node 生效，改 yaml 后重建
colcon build --packages-select tianyiF2手先需要启动bodyctrl，G2手先需要额外启动脚本（双手start_rh5g2_both.sh 右手start_rh5g2_right.sh 如果启动报错则重启机器）
F2手势在hand的f2_gestures.yaml里，G2手势在hand的g2_gestures.yaml；手势数自动从f2_gestures.yaml读取
F2发布 /joint_command/{left,right}_hand（用于接入仿真），需要再启动ctrl_node连接仿真控制话题与真实控制话题才能控制手，G2可以直接控制手。
ros2 run hand f2_gesture_node
ros2 run hand g2_gesture_node每次遥操数据流开始（含中断后重新开始），双臂先由 MoveIt 规划一条缓慢轨迹接近当前遥操值，到达后切换为直接遥操直控，避免瞬移。接近期间操作者移动主端，目标会跟随重规划；超时则强制切换直控兜底。可通过按钮切换为微操模式，由遥操改为通过摇杆控制末端位姿进行微调。
ros2 run tiansuo teleop_simple_node # 简易模式，只有手势控制与遥操
ros2 run tiansuo teleop_node # 包含启动时规划复位、正常遥操与空间站对接模式
ros2 run tiansuo teleop_retarget_node # 包含启动时复位、正常遥操与斯坦尼康模式仿真测试
ros2 launch tiansuo teleop_sim.launch.py
ros2 launch tiansuo teleop_retarget.launch.py 真实机器控制，可通过修改tianyi下的motor_groups.yaml来修改要把哪些控制映射到真实控制话题。
source install/setup.bash
ros2 launch tiansuo teleop_gesture.launch.py # F2
ros2 launch tiansuo teleop_g2.launch.py # G2
ros2 launch tiansuo teleop_all.launch.py # 如果两边手不一样启动这个绕过 xtele 包直接经串口 (Dynamixel Protocol 2.0) 读写同构臂 15 个舵机（左臂 06、右臂 1016、腰部 25）：后台读线程逐 ID 持续轮询各关节角度（每关节记录真实采样时刻，一轮约 30ms=2ms/关节×15，发布不被串口阻塞），减零位偏移 (joint_offsets)、应用方向 (joint_signs) 后以 TiansuoTeleop 发布 /master_device/state（手/按钮/滚轮/摇杆等置 0，格式与 zmq_node 一致可替换）；发布定时器 (publish_rate) 从最新采样向未来线性平滑（把已确认的单周期位移在一个采样周期内播完，下一采样时刻恰好衔接新值，消除采样台阶；采样中断时饱和在最后一周期位移，不超前发散），输出跳变上界 = 单周期速度变化 + 发布栅格量化（真实手速下 <0.01 rad；interpolate: false 可关闭直通最新采样）。FTDI latency timer 自动设置：DxlBus 打开串口后把 /sys/bus/usb-serial/devices/<tty>/latency_timer 从默认 16ms 改为 1ms（默认 16ms 让每个孤立响应包延迟 16ms 才上报主机，逐 ID 读 15 个舵机时每轮固定 240ms），写失败仅告警不影响使用。订阅 /teleop/ctrl，按各 id 的使能标志与目标角度，加零位、限幅 (joint_limits) 后下发 goal_position，失能则关闭扭矩可外力自由转动。总线层加事务锁保证读线程与控制线程并发安全（半双工串口任何时刻只有一个包流），控制写做去重去抖——torque_enable 仅在状态变化时写、目标变化 ≥1 LSB 才重发，整批写后 drain 消费残留状态响应，杜绝残留包污染读事务导致的卡顿突变。配置在 xlinker_bimanual.yaml，字段与 xlinker 一致。
ros2 run tiansuo teleop_read_node
ros2 run testall teleop_read_test # 交互式测试: list/show/set/off
ros2 run testall cmd_freq_probe # 检测 /arm/cmd_ctrl 指令帧率与 pos 实际更新率真实机器直读遥操（teleop_sim_direct 的真实机器版）：主端舵机经 teleop_read_node 直读 → teleop_node 遥操（含 MoveIt APPROACH 状态机）→ ctrl_node 桥接真实机器 6 路控制话题；真实状态经 robot_relay_node 聚合 /joint_states 供 MoveIt 使用，含 F2 手势。安全注意：数据流开始后双臂会先由 MoveIt 缓慢接近主端姿态再直控，若机器人当前位姿与主端差异过大，先手动将机器人摆到相近位姿再启动。
ros2 launch tiansuo teleop_real_direct.launch.py rviz:=true基于 Orbbec Gemini335 深度相机的无接触遥操：用 pyorbbecsdk2 打开相机并做软件 D2C 对齐（深度与 RGB 逐像素对齐），MediaPipe 检测肩/腕/食指关键点，深度反投影得手臂末端相对根部的 3D 坐标，按人体臂长与机器人臂长（MJCF 零位 FK 计算）比例换算成机器人末端目标，再以"目标−当前位姿"差值发给 ik_node 逆解执行。相机与机器人摆放的轴映射、末端基准朝向等均可配。arm_track_node 自带可视化窗口，三图并排：RGB 原图、深度伪彩图、深度叠加+关键点图（左臂绿/右臂红圆点+连线，overlay_alpha 调透明度），无 GUI 环境自动降级关闭。
ros2 launch cam_teleop cam_teleop.launch.py # 仿真 + 双臂初始化 + 相机窗口 + 三图可视化窗口
ros2 run cam_teleop orbbec_node # 仅相机采集节点修复"冷启动后 APPROACH 轨迹执行成功却不进 DIRECT"的四个根因：
- 仿真漂移：rclpy 2ms 定时器实际约 200Hz，模拟时间按步数推进导致身体占位轴被高频推离零位、臂关节漂移。修复：sim_node 按真实时间追赶步进（每 tick 上限 100 步）+ 身体指令变化检测（变化超 0.005 rad 才写入）。
- L 臂规划失败：MoveIt/FCL（URDF 几何）将手指初始位姿误判与 base（机身）边缘接触，MuJoCo（MJCF 几何）同状态零接触——两引擎碰撞几何不一致，start state 碰撞导致规划无效不执行。修复：SRDF 默认允许碰撞矩阵禁用 base↔24 个手指 link 的检查 + plan_node 执行失败自动重试（2s 间隔最多 3 次）。
- ACM 被整体替换：scene_objects_node 发布的
/planning_scenediff 在 Humble 下会整体替换 move_group 默认 ACM（SRDF 900+ 条默认禁用条目全丢），导致手内部 Adjacent 对失效、start state 碰撞误报 -26。修复：发布前先调/get_planning_scene查询完整 ACM（51 entry），合并 allowed_contacts 后发布完整 ACM（52 entry），不再只发少量 entry 的 diff。 - 重试定时器被误取消：ACM 查询遇"服务未就绪"时
timer.reset()后被_publish_objects末尾的无条件timer.cancel()立刻取消，重试永不执行。修复：cancel 收进查询分支——发出查询后 cancel、由失败回调 reset 重试；服务未就绪分支只 reset 不 cancel。
冷启动实测：完整链路 2.5s 进 DIRECT、双臂规划执行成功；move_group 慢启动场景下 ACM 仍稳定发布（51→52 entry，含重试路径）。
ros2 launch tiansuo teleop_sim.launch.py环境异常退出后 ros2 launch 的子树可能残留（move_group、sim_node、ik_node 等仍占着话题/服务），多套环境并存会在同一 ROS 域争抢导致行为异常。实测教训：残留 teleop_node/trac_node 会继续直控仿真，把 sim 拉离新环境的目标，导致遥操 APPROACH 到达判定永不满足、120s 超时后才进直控。每次启动新环境前务必先清理，kill_all.sh 一键清空所有残留进程，清理前后各打印进程清单供核对：
bash kill_all.sh