27、机器人仿真与运动规划：Gazebo与MoveIt!实战指南

机器人仿真与运动规划：Gazebo与MoveIt!实战指南

在机器人开发过程中，仿真和运动规划是非常重要的环节。Gazebo作为一款强大的3D机器人模拟器，能支持ROS进行机器人设计、算法测试等工作；而MoveIt!则是一个集成库，为机械臂提供了运动规划等多种功能。下面将详细介绍如何使用Gazebo和MoveIt!对OpenManipulator Chain进行仿真和控制。

1. Gazebo设置

Gazebo是一个独立的支持ROS的3D机器人模拟器，它能够实时监控机器人及周围环境的物理变化（如惯性、扭矩、碰撞等），这是它相较于RViz的优势所在，能有效避免实验过程中机器人故障和人员伤亡。

1.1 配置Gazebo仿真标签

之前创建的URDF是用于RViz可视化的，若要在Gazebo仿真环境中使用，需添加一些标签。这些标签存储在 open_manipulator_chain.gazebo.xacro 文件中。以下是部分代码示例：

<!-- Link1 -->
<gazebo reference="link1">
  <mu1>0.2</mu1>
  <mu2>0.2</mu2>
  <material>Gazebo/Grey</material>
</gazebo>

在这个代码中， <mu1> 和 <mu2> 用于设置摩擦系数， <material> 用于设置链接的颜色。颜色和惯性信息对于在Gazebo中使用链接的设置至关重要，由于之前创建的URDF文件中已包含惯性信息，因此只需配置颜色。此外，还可以为Gazebo支持的物理引擎Open Dynamics Engine (ODE)设置重力、阻尼和摩擦力。

还有一个关于 ros_control 插件的设置：

<!-- ros_control plugin -->
<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/open_manipulator_chain</robotNamespace>
    <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
</gazebo>

这里的 <plugin> 是用于传感器和机器人状态控制的工具， <robotNamespace> 设置在Gazebo中使用的机器人名称， <robotSimType> 设置机器人仿真接口的插件名称。

完整的 open_manipulator_chain.gazebo.xacro 文件如下：

<?xml version="1.0"?>
<robot>
  <!-- World -->
  <gazebo reference="world">
  </gazebo>
  <!-- Link1 -->
  <gazebo reference="link1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- Link2 -->
  <gazebo reference="link2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- Link3 -->
  <gazebo reference="link3">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- Link4 -->
  <gazebo reference="link4">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- Link5 -->
  <gazebo reference="link5">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- grip_link -->
  <gazebo reference="grip_link">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- grip_link_sub -->
  <gazebo reference="grip_link_sub">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/open_manipulator_chain</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
</robot>

这个文件用于设置Gazebo仿真的参数，并包含在 open_manipulator_chain.xacro 文件中。

1.2 在Gazebo中显示OpenManipulator Chain

使用完成的URDF在Gazebo中显示OpenManipulator Chain，具体步骤如下：
1. 进入 open_manipulator_gazebo 的 launch 文件夹：

$ roscd open_manipulator_gazebo/launch

2. 查看该文件夹下的文件：

$ ls
open_manipulator_gazebo.launch  # 启动Gazebo的文件
position_controller.launch      # 启动ROS - CONTROL的文件

3. 打开 open_manipulator_gazebo.launch 文件查看节点信息：

$ roscd open_manipulator_gazebo/launch
$ gedit open_manipulator_gazebo.launch

open_manipulator_gazebo.launch 文件内容如下：

<?xml version="1.0" ?>
<launch>
  <!-- These are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="world_name" value="$(find open_manipulator_gazebo)/world/empty.world"/>
  <arg name="debug" value="$(arg debug)" />
  <arg name="gui" value="$(arg gui)" />
  <arg name="paused" value="$(arg paused)"/>
  <arg name="use_sim_time" value="$(arg use_sim_time)"/>
  <arg name="headless" value="$(arg headless)"/>
  </include>
  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
  command="$(find xacro)/xacro.py '$(find open_manipulator_description)/urdf/open_manipulator_chain.xacro'"/>
  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
  args="-urdf -model open_manipulator_chain -z 0.0 -param robot_description"/>
  <!-- ros_control robotis manipulator launch file -->
  <include file="$(find open_manipulator_gazebo)/launch/position_controller.launch"/>
</launch>

这个启动文件包含了 empty_world.launch 文件、 spawn_model 节点和 position_controller.launch 文件。 empty_world.launch 文件包含执行Gazebo的节点，可设置仿真环境、GUI和时间； spawn_model 节点负责根据URDF调用机器人； position_controller.launch 负责设置和执行ROS - CONTROL。

4. 运行Gazebo：

$ roslaunch open_manipulator_gazebo open_manipulator_gazebo.launch

运行后，就能在Gazebo仿真空间中看到OpenManipulator Chain。

1.3 查看主题列表并控制机器人

打开一个新的终端窗口，查看主题列表：

$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/joint_states
/open_manipulator_chain/grip_joint_position/command
/open_manipulator_chain/grip_joint_sub_position/command
/open_manipulator_chain/joint1_position/command
/open_manipulator_chain/joint2_position/command
/open_manipulator_chain/joint3_position/command
/open_manipulator_chain/joint4_position/command
/open_manipulator_chain/joint_states
/rosout
/rosout_agg

从主题列表中可以看到有 /gazebo 命名空间和 /open_manipulator_chain 命名空间的主题。使用ROS - Control通过 /open_manipulator_chain 命名空间的主题来检查和控制Gazebo中机器人的状态。例如，移动机器人的第二个关节：

$ rostopic pub /open_manipulator_chain/joint2_position/command std_msgs/Float64 "data: 1.0" --once

通过简单的消息通信，就能看到OpenManipulator Chain的第二个关节移动。

2. MoveIt!介绍与使用

MoveIt!是一个用于机械臂的集成库，提供了快速逆运动学分析、高级操作算法、机器人手控制、动力学、控制器和运动规划等多种功能。它还提供了GUI，方便用户进行各种设置，并且支持使用RViz进行可视化反馈。

2.1 move_group节点

move_group 节点可以通过ROS动作和服务与用户交换命令。MoveIt!提供了各种用户界面，用户可以使用C++语言的 move_group 接口、Python语言的 move_commander 和 Motion Planning plugin to RViz 与 move_group 节点进行通信。

move_group 节点从URDF、Semantic Robot Description Format (SRDF)和MoveIt! Configuration获取机器人的信息。其中，URDF是之前创建的，而SRDF和MoveIt! Configuration将通过MoveIt!提供的Setup Assistant创建。

move_group 节点通过ROS主题和动作提供机器人及其环境的状态和控制信息。关节状态使用 sensor_msg/JointStates 消息，变换信息使用tf库，控制器使用 FollowTrajectoryAction 接口向用户通知机器人状态。此外，用户还可以通过 planning scene 获取机器人运行环境和状态的信息。

move_group 还提供了插件功能，通过开源库为用户的机器人应用各种功能（如控制、路径生成、动力学等）。内置的插件包括Open Motion Planning Library (OMPL)、Kinematic and Dynamic Library (KDL)和A Flexible Collision Library (FCL)等。

2.2 使用MoveIt! Setup Assistant创建MoveIt!包

创建用于机械臂的MoveIt!包需要URDF、SRDF和MoveIt! Configuration文件，MoveIt! Setup Assistant可以根据URDF创建SRDF和MoveIt! Configuration文件。以下是使用MoveIt! Setup Assistant为OpenManipulator Chain创建MoveIt!包的步骤：
1. 运行MoveIt! Setup Assistant：

$ roslaunch moveit_setup_assistant setup_assistant.launch

2. 点击 [Create New MoveIt Configuration Package] 按钮创建新包。
3. 点击 [Browse] 按钮，打开之前创建的 open_manipulator_chain.xacro 文件，然后点击 [Load Files] 按钮。
4. 自碰撞设置 ：进入 Self - Collision 页面，定义构建 Self - Collision Matrix 所需的采样密度，必要时用户可以确定机器人各链接之间的碰撞范围。设置所需的采样密度（默认值为 10,000 ），然后点击 [Generate Collision Matrix] 按钮。
5. 虚拟关节设置 ： Virtual Joints 页面提供了机械臂基座和参考坐标系之间的虚拟关节。由于OpenManipulator Chain的基座是固定的，所以无需设置虚拟关节。
6. 规划组设置 ：MoveIt!为划分成组的机械臂提供运动规划。OpenManipulator Chain由四个关节和一个抓手组成，需要设置 arm 组和 gripper 组。
   • 创建 arm 组：点击 [Add Group] 按钮，设置组名为 arm ，选择所需的运动学分析插件，然后选择关节1 - 4，点击 [Save] 按钮创建组。
   • 创建 gripper 组：由于线性抓手由一个执行器控制，不支持运动学分析插件，因此将运动学分析插件设置为 None 。选择 grip_joint 和 grip_joint_sub 关节，完成 gripper 组的创建。
7. 机器人姿态设置 ：在 Robot Poses 页面，点击 [Add Pose] 按钮，将所有关节值设置为0.0并写入姿态名称，以注册机器人的特定姿态。
8. 末端执行器设置 ：在 End Effectors 页面，点击 [Add End Effector] 按钮，注册OpenManipulator Chain的线性抓手。填写 End Effector Name ，并在 Planning Groups 页面创建的组中选择 gripper 。
9. 被动关节设置 ： Passive Joints 页面允许指定排除在运动规划之外的关节。OpenManipulator Chain中没有被动关节，无需进行更改。
10. 作者信息设置 ：在 Author Information 页面，填写包创建者的姓名和电子邮件。
11. 生成配置文件 ：完成所有设置后，在 Configuration Files 页面，点击 [Browse] 按钮在 open_manipulator 文件夹中创建 open_manipulator_moveit_example 文件夹，然后点击 [Generate Package] 按钮，生成包含MoveIt! Configuration可执行文件的 config 文件夹和 launch 文件夹。

生成包后，进行以下操作：
1. 查看生成的包：

$ cd ~/catkin_ws/src/open_manipulator/open_manipulator_moveit_example
$ ls
Config        # MoveIt!配置的YAML和SRDF文件
launch        # 启动文件
.setup_assistant  # 设置助手生成的包信息
CMakeLists.txt    # CMake构建系统输入文件
package.xml       # 定义包属性

2. 编译工作空间：

$ cd ~/catkin_ws && catkin_make

3. 运行演示：

$ roslaunch open_manipulator_moveit_example demo.launch

运行演示后，可以通过RViz窗口看到OpenManipulator Chain。在左下角的 MotionPlanning 命令窗口的 Context 页面，选择OMPL支持的运动规划库（如 RRTConnectkConfigDefault ），然后进入 Planning 页面。

由于OpenManipulator Chain只有四个关节，其端点只有X、Z和Pitch轴的自由度（剩余一个自由度是第一个关节的Yaw轴旋转）。记住这一点，将端点移动到所需的坐标，然后点击 [Plan & Excute] 按钮检查运动情况。

此外，还可以通过在 config 文件夹的 kinematics.yaml 文件底部写入 position_only_ik: true ，使用移动绿色球体标记（Interactive marker）仅指定位置的方法来指定机械臂的目标姿态。

$ roscd open_manipulator_moveit/config/
$ gedit kinematics.yaml
arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3
  position_only_ik: true

2.3 应用industrial_trajectory_filters

由于RRT路径生成算法返回的关节值是从当前位置到目标位置的路径，但从第n个姿态移动到第(n + 1)个姿态需要在更小采样时间获得的关节值，因此使用ROS - INDUSTRIAL支持的 industrial_trajectory_filters 。具体步骤如下：
1. 下载ROS - INDUSTRIAL CORE包：

$ git clone https://github.com/ros-industrial/industrial_core.git

2. 检查下载的ros - industrial包中的 industrial_trajectory_filters ：

$ cd ~/catkin_ws/src/industrial_core/industrial_trajectory_filters/

3. 将 industrial_trajectory_filters 文件夹中的 planning_request_adapters_plugin_description.xml 、 src 和 include 文件夹复制到之前创建的MoveIt!包中：

$ cp -r planning_request_adapters_plugin_description.xml src include ~/catkin_ws/src/open_manipulator/open_manipulator_moveit_example/

4. 在 config 文件夹中创建 smoothing_filter_params.yaml 文件并指定系数：

$ cd ~/catkin_ws/src/open_manipulator/open_manipulator_moveit_example/config
$ gedit smoothing_filter_params.yaml
smoothing_filter_name: /move_group/smoothing_5_coef
smoothing_5_coef:
           - 0.25
           - 0.50
           - 1.00
           - 0.50
           - 0.25

5. 打开 launch 文件夹中的 ompl_planning_pipeline.launch.xml 文件，在 planning_adapters 中添加以下过滤器：

$ cd ~/catkin_ws/src/open_manipulator/open_manipulator_moveit_example/launch
$ gedit ompl_planning_pipeline.launch.xml
industrial_trajectory_filters/UniformSampleFilter
industrial_trajectory_filters/AddSmoothingFilter

6. 在同一文件中添加以下参数：

<param name="sample_duration" value="0.04" />
<rosparam command="load" file="$(find open_manipulator_moveit)/config/smoothing_filter_params.yaml"/>

7. 插入上述内容后的启动文件示例如下：

<launch>
  <!-- OMPL Plugin for MoveIt! -->
  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
  <!-- The request adapters (plugins) used when planning with OMPL.
       ORDER MATTERS -->
  <arg name="planning_adapters" value="
               industrial_trajectory_filters/UniformSampleFilter
               industrial_trajectory_filters/AddSmoothingFilter
</launch>

通过以上步骤，就可以使用Gazebo对OpenManipulator Chain进行仿真，并使用MoveIt!进行运动规划和控制。整个过程涵盖了从环境搭建、模型配置到实际操作的各个方面，为机器人开发提供了一套完整的解决方案。

下面是整个操作流程的mermaid流程图：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始]):::startend --> B(设置Gazebo仿真标签):::process
    B --> C(在Gazebo中显示OpenManipulator Chain):::process
    C --> D(查看主题列表并控制机器人):::process
    D --> E(运行MoveIt! Setup Assistant):::process
    E --> F{选择操作}:::decision
    F -->|创建新包| G(设置自碰撞、虚拟关节等信息):::process
    F -->|修改现有包| H(根据需求修改配置):::process
    G --> I(生成MoveIt!包并编译运行):::process
    H --> I
    I --> J(应用industrial_trajectory_filters):::process
    J --> K([结束]):::startend

通过这个流程图，可以清晰地看到整个操作的步骤和流程，方便开发者进行参考和实践。

总之，Gazebo和MoveIt!为机器人的开发和测试提供了强大的工具和功能。通过合理使用这些工具，可以提高机器人开发的效率和质量，为机器人的实际应用打下坚实的基础。希望本文的介绍和操作步骤能对机器人开发者有所帮助。

机器人仿真与运动规划：Gazebo与MoveIt!实战指南

3. 关键技术点分析

3.1 Gazebo与RViz的对比

Gazebo和RViz在机器人开发中都有重要作用，但它们的功能有所不同，具体对比如下：
| 对比项 | Gazebo | RViz |
| — | — | — |
| 功能定位 | 3D机器人模拟器，支持机器人设计、算法测试、回归分析、人工智能训练等 | 可视化工具，主要用于显示机器人模型和传感器数据 |
| 物理模拟 | 可以实时监控机器人及周围环境的物理变化（惯性、扭矩、碰撞等） | 无法实时获取物理变化信息 |
| 应用场景 | 用于模拟机器人在真实环境中的行为，预防实验中的机器人故障和人员伤亡 | 用于快速验证机器人模型和算法的可视化效果 |

3.2 MoveIt!的插件功能

MoveIt!的 move_group 节点提供了插件功能，通过开源库为用户的机器人应用各种功能。以下是一些常见的插件及其作用：
| 插件名称 | 作用 |
| — | — |
| Open Motion Planning Library (OMPL) | 提供多种运动规划算法，如RRT等，用于寻找机器人从当前位置到目标位置的路径 |
| Kinematic and Dynamic Library (KDL) | 用于机器人的运动学和动力学分析，计算关节角度和末端执行器的位置 |
| A Flexible Collision Library (FCL) | 用于检测机器人与周围环境的碰撞，确保机器人运动的安全性 |

4. 常见问题及解决方案

4.1 Gazebo仿真中机器人不显示

• 可能原因 ：
  • URDF文件配置错误，如缺少必要的标签或参数设置不正确。
  • 启动文件中机器人模型加载失败。
• 解决方案 ：
  • 检查 open_manipulator_chain.gazebo.xacro 文件，确保所有标签和参数设置正确。
  • 检查 open_manipulator_gazebo.launch 文件，确保URDF文件正确加载到ROS参数服务器。

4.2 MoveIt!规划失败

• 可能原因 ：
  • 运动规划库选择不合适，无法找到可行的路径。
  • 机器人模型的运动学参数设置错误。
• 解决方案 ：
  • 尝试更换不同的运动规划库，如在RViz的 MotionPlanning 命令窗口中选择其他规划库。
  • 检查 kinematics.yaml 文件，确保运动学参数设置正确。

5. 总结与展望

通过本文的介绍，我们详细了解了如何使用Gazebo对OpenManipulator Chain进行仿真，以及如何使用MoveIt!进行运动规划和控制。整个过程包括Gazebo的配置、MoveIt!包的创建、运动规划的设置以及 industrial_trajectory_filters 的应用。

在未来的机器人开发中，Gazebo和MoveIt!将继续发挥重要作用。随着技术的不断发展，我们可以期待更加高效、智能的机器人仿真和运动规划工具的出现。同时，结合人工智能和机器学习技术，机器人将能够更好地适应复杂的环境，实现更加灵活和智能的运动。

以下是整个机器人开发流程的总结mermaid流程图：

graph LR
    classDef startend fill:#F5EBFF,stroke:#BE8FED,stroke-width:2px;
    classDef process fill:#E5F6FF,stroke:#73A6FF,stroke-width:2px;
    classDef decision fill:#FFF6CC,stroke:#FFBC52,stroke-width:2px;

    A([开始开发]):::startend --> B(创建URDF模型):::process
    B --> C(配置Gazebo仿真):::process
    C --> D(使用MoveIt!进行运动规划):::process
    D --> E{规划是否成功}:::decision
    E -->|是| F(应用industrial_trajectory_filters):::process
    E -->|否| G(调整参数和规划库):::process
    G --> D
    F --> H(进行机器人实验和测试):::process
    H --> I{实验结果是否满意}:::decision
    I -->|是| J([完成开发]):::startend
    I -->|否| K(分析问题并改进):::process
    K --> C

这个流程图总结了从机器人模型创建到最终开发完成的整个过程，包括中间可能遇到的问题和解决方法。开发者可以根据这个流程图，有条不紊地进行机器人的开发和测试工作。

希望本文能够帮助读者更好地理解和掌握Gazebo和MoveIt!的使用，为机器人开发提供有益的参考。在实际应用中，读者可以根据具体需求对上述步骤和方法进行调整和优化，以实现更加高效和智能的机器人开发。