Soma Zero Tutorials
🔍 搜索功能尚未开启,敬请期待。

附录1 Gazebo 物理仿真:用 gz_ros2_control 把 MoveIt 接到物理引擎(数字孪生)

第三章里我们一路从 ROS2 基础走到 URDF、MoveIt,但那些要么在 RViz 里、要么对着真机。而 RViz 只是个显示器——你给它一组关节角,它就摆成那个样子,没有重力、没有惯性、没有接触力。机械臂在 RViz 里永远不会"掉下来",夹爪在 RViz 里也永远夹不住任何东西。

这篇附录,我们把同一台机械臂放进 Gazebo 物理引擎,并用 gz_ros2_controlMoveIt 的运动规划真正"执行"到带物理的虚拟机械臂上——相当于用 Gazebo "承接"了整台机器人:第三章针对真机的那套 MoveIt 流程,原封不动就能先跑在这台虚拟 Episode1 上。这样得到的就是一套数字孪生:还没碰真机,就能在仿真里验证"这条轨迹能不能走、夹爪能不能夹住、臂会不会撞桌子",把弄坏真机的风险降到最低。

本附录所有代码、命令、实测数据均出自 episode-ros-ws/src/episode1_gz_sim/ 这个包,可直接对照仓库文件;文末有一键自测脚本与验收清单。

一、为什么要用 Gazebo(它和 RViz 到底差在哪)

很多人第一次会困惑:"我 RViz 里 MoveIt 都能 Plan&Execute 了,为什么还要 Gazebo?"关键区别在有没有物理

维度RViz(可视化)Gazebo(物理仿真)
本质一个 3D 显示器一个物理引擎(默认 DART)
关节怎么动你发什么关节角,它显示什么电机施力 → 物理求解 → 关节才动
重力有(臂会被重力拉、需要力矩顶住)
碰撞 / 接触只做"几何碰撞检查"(MoveIt 算的)真接触力(夹爪能压住物体、物体会被推动)
能验证什么轨迹几何上可不可行轨迹物理上能不能执行、抓取稳不稳
角色给人看给算法当"虚拟真机"(数字孪生)

一句话:RViz 回答"这样摆姿势对不对",Gazebo 回答"这台机器人真这么动会发生什么"。 做抓放、做下棋这类要真实接触的任务,必须有物理仿真兜底,才能在不损坏真机的前提下反复试错。

二、整体架构:一条 Plan&Execute 在仿真里怎么流动

理解这套系统,关键是看清一次"执行"信号的完整链路

┌─────────────┐ 规划 ┌───────────┐ FollowJointTrajectory ┌───────────────────────┐ │ RViz / 你的 │ ───────► │ move_group │ ───────(action)──────► │ episode_arm_controller │ │ 代码(目标) │ │ (MoveIt) │ │ (JTC, ros2_control) │ └─────────────┘ └───────────┘ └───────────┬───────────┘ ▲ │ 写 position 命令 │ /joint_states ▼ ┌──────┴─────────────────┐ ◄── 关节状态回流 ──┐ ┌───────────────────────────┐ │ joint_state_broadcaster │◄─────────────────────│ Gazebo 物理关节 ◄── gz_ros2_control │ └────────────────────────┘ │ (重力/惯性/接触) (GazeboSimSystem) │ └───────────────────────────┘

要点:

1. MoveIt 这一层和真机完全一样——move_group、规划组 episode_arm、控制器 episode_arm_controller 这些名字,仿真和真机共用一套。 2. 唯一被替换的是最底层的"硬件接口":真机那一层是厂商的电机驱动;仿真这一层换成 gz_ros2_controlGazeboSimSystem——它把 ros2_control 的"读状态 / 写命令"实现成"读 Gazebo 物理关节 / 给 Gazebo 关节施力"。 3. 所以同一套 MoveIt 配置,换个底层接口就能在 sim 和 real 之间切。这正是"数字孪生"的价值:你在仿真里调好的规划、写好的上层逻辑,搬到真机几乎不用改。

这条链路上的关键话题 / 动作 / 服务(调试时常用):

名称类型作用
/move_actionaction(moveit_msgs/MoveGroupRViz 或你的代码把"规划+执行"请求发给 MoveIt
/episode_arm_controller/follow_joint_trajectoryaction(FollowJointTrajectoryMoveIt 把规划好的轨迹下发给臂控制器
/joint_statestopicjoint_state_broadcaster 发布的关节状态(来自 Gazebo 物理),MoveIt 用它当当前状态
/gripper_controller/joint_trajectorytopic夹爪开合命令(两指同值)
/check_state_validityservice查某个关节状态是否自碰撞(自测脚本用它验夹爪免碰撞矩阵)
/clocktopicGazebo → ROS 的仿真时钟(全程 use_sim_time:=true,大家用同一个时间基准)

三、需要装什么

本机(Ubuntu 24.04 + ROS2 Jazzy)已通过 apt 安装:

组件作用
ros-jazzy-ros-gz(Gazebo Sim 8.11.0 = gz Harmonic)Gazebo 物理引擎本体 + ros_gz_bridge(ROS↔gz 桥)+ ros_gz_sim(spawn 机器人)
ros-jazzy-gz-ros2-control把 ros2_control 接到 Gazebo 的插件(本附录主角)
ros-jazzy-ros2-control / ros2-controllers控制器框架 + JointTrajectoryController
ros-jazzy-gz-ros2-control-demos官方示例(学 initial_value 等写法的参考,不参与运行)
sudo apt install ros-jazzy-ros-gz ros-jazzy-gz-ros2-control \ ros-jazzy-ros2-control ros-jazzy-ros2-controllers

注:真机路线不需要 Gazebo,这些只为仿真。运行前先把工作区编译好(cd episode-ros-ws && colcon build)。运行时记得 conda 关闭、source /opt/ros/jazzy/setup.bash——conda 的 libstdc++/numpy 会和系统 ROS 的库打 ABI 架,导致 rclpy/rviz 崩。

四、关键文件逐个讲(都在 episode1_gz_sim 包)

整个仿真包不改任何官方文件,只用 xacro include 官方 URDF 再叠加。结构:

episode1_gz_sim/ ├── urdf/ │ ├── episode1_with_gripper.urdf.xacro # 官方臂 URDF + 简化二指夹爪 │ └── episode1_gz.urdf.xacro # 上者 + gz_ros2_control 硬件接口 + gz 插件 ├── config/gz_ros2_controllers.yaml # controller_manager + 两个 JTC + 位置增益 ├── worlds/episode_world.sdf # 地面=桌面 + 光照 + 物理 ├── launch/sim.launch.py # 编排一切(含三个仿真侧补丁) └── scripts/ # headless 自测脚本

4.1 夹爪为什么用"平行二指"近似

真实夹爪是回环连杆(闭式运动链),而 URDF 只能描述(每个 link 只有一个父),无法精确表达闭环。业界标准做法:用单自由度平行二指近似——指尖位置、开合行程、碰撞体都对,足够仿真"夹棋子"。几何用方块近似(暂无官方 CAD),挂载位姿可在 RViz 里目测后微调 episode1_with_gripper.urdf.xacro 顶部参数。

4.2 gz_ros2_control 硬件接口(episode1_gz.urdf.xacro

这是把臂"接进物理"的核心。给每个关节声明 command / state 接口,并——这是防摔的关键——给位置状态接口设 initial_value(初始关节角 = home):

<ros2_control name="GazeboSimSystem" type="system"> <hardware> <plugin>gz_ros2_control/GazeboSimSystem</plugin> </hardware> <joint name="joint1"> <command_interface name="position"/> <state_interface name="position"> <param name="initial_value">0.0</param> <!-- ★ spawn 即置 home,防止启动空窗期重力塌臂 --> </state_interface> <state_interface name="velocity"/> </joint> <!-- joint2..joint6、left_finger_joint、right_finger_joint 同理,都带 initial_value --> </ros2_control> <!-- 把上面这套硬件接口交给 Gazebo 的 gz_ros2_control 系统插件 --> <gazebo> <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin"> <parameters>$(find episode1_gz_sim)/config/gz_ros2_controllers.yaml</parameters> </plugin> </gazebo>

4.3 控制器配置(gz_ros2_controllers.yaml

定义 controller_manager 下的三个控制器(关节状态广播 + 臂 JTC + 夹爪 JTC),以及 gz_ros2_control 的位置增益

# 位置增益改为“逐关节”设(在 URDF 各关节 <command_interface name="position"> 里,见 §7.6 抖动): # 臂=5(低,防振荡)、夹爪=60(高,够驱动)。全局只留 hold_joints。 /**: ros__parameters: hold_joints: true # 没有控制器接管命令时,保持当前关节位 controller_manager: ros__parameters: update_rate: 250 # Hz(逼近 gz 物理 1000Hz,减小离散过冲抖动,见 §7.6) use_sim_time: true joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster episode_arm_controller: # ★ 特意与官方同名,MoveIt 才能无缝连上 type: joint_trajectory_controller/JointTrajectoryController gripper_controller: type: joint_trajectory_controller/JointTrajectoryController episode_arm_controller: # 6 个臂关节,位置命令 ros__parameters: joints: [joint1, joint2, joint3, joint4, joint5, joint6] command_interfaces: [position] state_interfaces: [position, velocity] allow_nonzero_velocity_at_trajectory_end: true gripper_controller: # 两指都驱动(同发一个开合值即对称);gz 不支持 mimic ros__parameters: joints: [left_finger_joint, right_finger_joint] command_interfaces: [position] state_interfaces: [position, velocity] allow_nonzero_velocity_at_trajectory_end: true

4.4 物理世界(episode_world.sdf

一个最小但完整的物理世界:Physics(物理求解器,1 ms 步长)+ UserCommands + SceneBroadcaster + Sensors(ogre2 渲染)四个插件 + 一盏太阳光 + 一块地面。地面 z=0 就是桌面——以后摆棋盘、放棋子都以这个平面为基准。需要更真实的场景(桌子、棋盘、棋子)时,往这个 sdf 里加 <model> 即可,不用动机器人那边。

4.5 启动编排 + 三个"仿真侧叠加补丁"(sim.launch.py

sim.launch.py 负责把一切串起来:设资源路径(让 gz 找到 package:// 网格)→ 起 robot_state_publisher → 起 Gazebo → spawn 机器人 → 按顺序上三个控制器(状态广播 → 臂 → 夹爪)→ 起 move_group → 起 RViz。

这里有三处"只在喂给仿真的内存数据上打补丁、绝不改官方文件"的关键处理(每一处对应第七节的一个坑):

# 补丁①:官方臂关节 effort=0/velocity=0,Gazebo 物理下施不出力矩 → 只给喂 gz 的 URDF 字符串补有限值 _robot_xml = xacro.process_file(gz_xacro).toxml() _robot_xml = _robot_xml.replace('effort="0"', 'effort="100"').replace('velocity="0"', 'velocity="3.14"') # 补丁②:官方 SRDF 免碰撞矩阵只覆盖裸臂 → 在内存里给夹爪连杆补免碰撞对(官方 SRDF 文件不动) _srdf = moveit_config.robot_description_semantic["robot_description_semantic"] _srdf = _srdf.replace("</robot>", ' <disable_collisions link1="link6" link2="gripper_base_link" reason="Adjacent"/> ' ' <disable_collisions link1="gripper_base_link" link2="left_finger_link" reason="Adjacent"/> ' ' <disable_collisions link1="gripper_base_link" link2="right_finger_link" reason="Adjacent"/> ' ' <disable_collisions link1="left_finger_link" link2="right_finger_link" reason="Never"/> ' ' <disable_collisions link1="link6" link2="left_finger_link" reason="Never"/> ' ' <disable_collisions link1="link6" link2="right_finger_link" reason="Never"/> ' "</robot>") moveit_config.robot_description_semantic = {"robot_description_semantic": _srdf} # 补丁③:放宽 MoveIt 执行起点容差,吸收 P 控制顶重力的稳态残差(否则间歇 CONTROL_FAILED) sim_exec_params = {"trajectory_execution.allowed_start_tolerance": 0.1} # move_group 节点参数里带上 sim_exec_params;move_group 和 RViz 都用打过补丁的 robot_description / SRDF

五、启动并在仿真里做 Plan&Execute

cd ~/2026-summer-career-projects/episode-ros-ws source /opt/ros/jazzy/setup.bash && source install/setup.bash ros2 launch episode1_gz_sim sim.launch.py # 弹出 Gazebo + RViz
  • RViz 的 MotionPlanning 面板里拖动交互球设一个目标 → PlanExecute;这时 Gazebo 里那台有物理的机械臂会真的跟着动
  • 不想要 RViz:... rviz:=false;完全无界面(冷烟测试 / 自测):... headless:=true rviz:=false

不走 MoveIt、直接发命令调试(绕过规划,直接驱动控制器):

# 臂:直接发一条关节轨迹(让 joint2 转到 0.5 rad) ros2 topic pub --once /episode_arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory \ "{joint_names: [joint1,joint2,joint3,joint4,joint5,joint6], points: [{positions: [0,0.5,0,0,0,0], time_from_start: {sec: 3}}]}" # 夹爪:两指同值,0=闭合,0.022=张开 ros2 topic pub --once /gripper_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory \ "{joint_names: [left_finger_joint,right_finger_joint], points: [{positions: [0.02,0.02], time_from_start: {sec: 1}}]}"

⚠️ 停止仿真:在 launch 终端按 Ctrl-C 正常关闭。从另一个终端 pkill -f "gz sim" 杀不掉 gz(它是 ruby 包装的子进程),会残留多实例污染 ROS 图;若要从外部杀,用 kill -9 <PID>pgrep -af "gz sim"pgrep ruby 找 PID)或按进程组杀。这是新手最容易踩的"明明重启了还是不对"的坑。

六、headless 自测(不开界面也能验,适合反复回归)

episode1_gz_sim/scripts/ 下有两个纯 rclpy 端到端自测脚本,连到正在跑的仿真自动跑完所有检查:

  • test_pipeline.py —— 一次验 4 件事:

1. 防摔:启动后臂稳在 home(6 关节 |q| < 0.1 rad) 2. 自碰撞:home 全 0 状态经 /check_state_validity 合法、contacts = 0 3. 全链路/move_action(MoveIt)Plan&Execute 返回 error_code == 1 且执行后关节到位(< 0.06 rad) 4. 夹爪:开 → 合

  • test_execute_stress.py —— 连续 Plan&Execute 8 次(交替两个位姿),压测执行稳定性。
# 终端1:headless 起仿真 ros2 launch episode1_gz_sim sim.launch.py headless:=true rviz:=false # 终端2:跑测试(脚本内部会自动等仿真就绪) python3 src/episode1_gz_sim/scripts/test_pipeline.py python3 src/episode1_gz_sim/scripts/test_execute_stress.py

2026-06-23 验收:全新重启 ×3 全部 6/6 + 执行压力 8/8 = 17/17 通过。建立这套自测的意义在于:以后改了 URDF、改了参数,跑一遍脚本就知道有没有把仿真搞坏,不用每次人肉点 RViz。

七、踩过的坑与调参(真实排错记录,最值得看)

这一节是这套仿真最容易卡住的地方,逐个讲清"症状 → 根因 → 解法"。前三个坑当初是真的踩到、靠自测脚本一个个揪出来的。

7.1 机械臂一启动就"摔倒"塌成一团

  • 症状:带 GUI 一打开,臂在 Gazebo 里直接软下去趴在桌上。
  • 根因:用 gz sim -r(立即跑物理)时,从 spawn 到 gz_ros2_control 配置完成有约 3.5 秒空窗,这期间关节无人施力,被重力拉塌;等控制器激活,hold_joints 锁住的已经是塌下去的姿态。再加上官方臂关节 effort=0,物理下根本施不出力矩。
  • 解法(三管齐下):① 给喂 gz 的 URDF 字符串把臂关节 effort=0 → 100(补丁①);② 给关节足够的位置增益顶住重力(最初用全局 position_proportional_gain=50,后因抖动改为逐关节,详见 §7.6);③ 给每个关节 <state_interface name="position">initial_value=0(spawn 即置 home)。第③点是官方 gz_ros2_control demo 的标准做法,缺它必塌。
  • ⚠️ 注意:这条只治"关节下垂"。若整个机器人掉地上,是另一个更根本的问题,见 §7.5。

7.2 Plan&Execute 报 START_STATE_IN_COLLISION(夹爪自碰撞)

  • 症状CheckStartStateCollision failed, because '1 contact(s) detected : gripper_base_link - right_finger_link',规划在第一步就被拒。
  • 根因:官方 SRDF 的免碰撞矩阵(disable_collisions)只覆盖裸臂;我们新加的夹爪连杆没进表,MoveIt 把"夹爪底座紧挨着手指"这种设计上必然的接触误判成自碰撞。
  • 解法:在 launch 里内存中给 SRDF 补夹爪免碰撞对(补丁②,link6↔gripper_basegripper_base↔左右指左右指之间link6↔左右指),move_group 和 RViz 都吃到打过补丁的版本。官方 SRDF 文件一字不改。

7.3 间歇性 CONTROL_FAILED(同样的操作,时好时坏)

  • 症状:第一次 Plan&Execute 成功,重启后再点又报 Invalid Trajectory: start point deviates from current robot state more than 0.01 at joint 'jointX'CONTROL_FAILED,臂几乎没动。
  • 根因position_proportional_gain 是个 P 控制,顶重力时必然有 ~1–2° 稳态残差(臂稳不到精确的命令位)。而 MoveIt 默认 trajectory_execution.allowed_start_tolerance = 0.01 rad(0.57°) 太紧——"轨迹起点 vs 当前状态偏差 > 0.01"就拒绝执行。残差时大时小于 0.01,所以时好时坏。这个坑在 GUI 里点 Plan&Execute 也会随机撞上,特别隐蔽。
  • 解法:给仿真的 move_groupallowed_start_tolerance 放宽到 0.1 rad(~5.7°)(补丁③),覆盖残差。真机走官方配置、用真实电机闭环,不需要也不受此影响。

7.4 夹爪 mimic 在 Gazebo(DART)下不生效

  • 症状:用 URDF mimic 让右指跟随左指,gz 启动时报约束警告,右指不动。
  • 根因:Gazebo 默认物理引擎 DART 不支持 URDF mimic 约束
  • 解法:左右指都设成被驱动关节,由 gripper_controller(JTC)同发一个开合值实现对称开合,不用 mimic。

7.5 整个机器人"掉在地上"(⚠️ 最根本,务必先排查)

  • 症状:仿真一打开,整台机器人(不只是某个关节软)直接掉到地面趴着。
  • 根因:URDF 的根链接(base_link)没有经 fixed joint 连到一个名为 world 的链接 → Gazebo 把整台机器人当成自由漂浮的刚体,重力下整体坠落。⚠️ 这时 /joint_states 的关节角仍然是 0,所以只看关节角根本查不出来,必须看 gz 世界位姿:gz topic -e -t /world/episode_world/pose/info(看 link6 的 z 是 ~0.4 站着、还是 ~0 趴着)。
  • 解法:加 <link name="world"/> + 一块 40×80cm 安装底板 mount_plate,用 world → 底板 → base_link 两个 fixed joint 焊死(等价真机上电后被底座物理固定);SRDF 里放开 mount_plate↔base_link/link1 免碰撞。依据:官方 gz Harmonic spawn_urdf 教程 rrbot.urdf 即此法。加 world 后 MoveIt 规划帧变 world(官方 SRDF 无 virtual_joint,无冲突)。

7.6 焊死后"疯狂抖动"(位置控制 vs 速度控制 + 阻尼)

  • 症状:base 焊住不掉了,但臂在原地高频抖动
  • 根因:base 固定后臂第一次真正顶着重力。此时若用全局高增益(如 50)的 position_proportional_gain("位置误差→速度"P 环),任何小误差就让速度指令饱和(实测 vmax=3.14 rad/s),加上官方关节无阻尼、速度型位置控制无法静态保持 → 暴力极限环(实测位置 p2p≈10°)。
  • 解法(三招):① launch 内存里给臂关节注入 <dynamics damping="8.0" friction="0.5"/>(官方 URDF 零阻尼是病根之一);② 不设全局 position_proportional_gain,改在 URDF 每个关节 <command_interface name="position">逐关节设增益(臂 5、夹爪 60)→ gz_ros2_control 走稳定位置控制;③ update_rate 100→250Hz(逼近物理 1000Hz)。结果:抖动降到 p2p=0.000、跟随精确、夹爪精确开合。代价:关节为刚性位置控制,不模拟物理柔顺/抓取力(V1.0 够用,物理抓取留 V2)。
  • ⚠️ 全局增益是把双刃剑:臂要低(防振)、夹爪要高(够驱动),矛盾 → 必须逐关节分设。scripts/measure_jitter.py 可量化抖动。

7.7 Plan&Execute 执行"非常慢"(其实是真机限速,不是 bug)

  • 症状:点 Execute 后 Gazebo 里臂动得很慢。
  • 根因:官方 MoveIt joint_limits.yaml 用的是真机电机的实际转速上限(按恩培经验 RPM 换算)——joint2 仅 0.314 rad/s ≈ 18°/s(最慢瓶颈)。MoveIt 按这些限速做时间参数化 → 执行就这么慢。这是"忠实数字孪生":仿真速度=真机速度,免得"仿真飞快、真机慢吞吞"。
  • 想仿真里跑快点调试:把 joint_limits.yamlmax_velocity 调大(仅仿真用),或给 launch 加一个速度缩放开关;真机务必保留官方限速。RViz 的 Velocity Scaling 滑块默认已是 1.0、到顶了,所以只能从限速层面放开。

汇总表

症状根因解法
整台机器人掉地上base 没经 fixed joint 焊到 worldworld+40×80 底板,world→底板→base 焊死(§7.5)
单关节启动塌下去effort=0 + 无 initial_value,空窗期被重力拉塌effort→100 + initial_value=0 + 足够增益(§7.1/7.6)
START_STATE_IN_COLLISION夹爪/底板没进 SRDF 免碰撞矩阵launch 内存 patch SRDF 补免碰撞对
间歇 CONTROL_FAILEDP 控制稳态残差 > 默认起点容差 0.01allowed_start_tolerance 放宽到 0.1
右指不跟随DART 不支持 mimic双指都驱动、同步发同一个值
焊死后疯狂抖动全局高增益速度饱和 + 关节无阻尼注阻尼 + 逐关节增益 + update_rate 250(走位置控制,§7.6)
执行非常慢joint_limits.yaml 用真机限速忠实孪生;要快就调大 max_velocity(仅仿真)

八、仿真 vs 真机:同一套 MoveIt,两套接法

最后强调这套设计的核心——上层不变,只换底层

维度仿真(本附录 episode1_gz_sim真机(官方 episode1_urdf_1113_moveit
底层硬件接口gz_ros2_controlGazeboSimSystem厂商电机驱动(episode_controllerinterface 节点)
关节状态来源Gazebo 物理引擎真实编码器
MoveIt 配置共用(规划组 episode_arm、ikfast 逆解)共用
控制器名episode_arm_controller(特意同名)episode_arm_controller
三个仿真补丁需要(effort / SRDF / 容差)不需要、绝不施加(避免污染真机链路)
安全随便撞,没有真机风险回零只用 gui_server、绝不裸回零、首次小幅、手放急停

铁律:仿真侧的补丁(effort、SRDF 免碰撞、放宽容差)只为绕过仿真特性,绝不能带到真机配置里——真机要的是真实碰撞检查和真实闭环精度。所以仿真和真机各用各的 launch,互不污染。

九、验收清单

  • ros2 launch episode1_gz_sim sim.launch.py 起来后,Gazebo 里臂稳稳停在 home,不摔
  • RViz Plan&Execute → Gazebo 物理臂跟着动,多次执行稳定不报 CONTROL_FAILED
  • 夹爪能开(0.022)能合(0)。
  • /joint_states 来自 Gazebo(物理),MoveIt 用它当当前状态。
  • scripts/test_pipeline.py 跑出 6/6test_execute_stress.py 跑出 8/8

做到这些,你就有了一台可以反复折腾、不怕弄坏的"虚拟 Episode1"——后面写抓放逻辑、调下棋流程,都先在这台虚拟机器人上验证,再搬真机。这,就是"数字孪生"对一个机器人项目最实在的价值。