简介

        MoveIt!与Gazebo的联合仿真，其主要思路为搭建ros_control和MoveIt!的桥梁。先在MoveIt!端配置关节和传感器接口yaml文件，将其加载到rviz端；再在机器人端配置ros_control和接口yaml文件，将机器人加载到Gazebo。最后同时启动加载有ros_control的Gazebo和加载有MoveIt的rviz，达到联合仿真的目的。

配置说明

1. 仿真与真实机械臂配置

1.1 MoveIt!端的配置

① 控制器配置文件controllers_gazebo.yaml controllers_gazebo.yaml在rm_65_moveit_config/config目录下，代码如下：

controller_manager_ns: controller_manager
controller_list:
  - name: arm/arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

② 修改启动文件rm_65_moveit_controller_manager.launch.xml

        修改rm_65_moveit_config功能包中的rm_65_moveit_controller_manager.launch.xml，配置加载刚才创建的controllers_gazebo.yaml文件到参数服务器，代码如下：

③ 启动文件moveit_planning_execution.launch

                moveit_planning_execution.launch是在rm_65_moveit_config/launch目录下新创建的启动文件，用以加载MoveIt和rviz，最后运行关节状态发布器，用以向Gazebo中发布指令，代码如下：

 # The planning and execution components of MoveIt! configured to 
 # publish the current configuration of the robot (simulated or real)
 # and the current state of the world as seen by the planner
 
  
 
 
 # The visualization component of MoveIt!
 
 
  
  
    
    [/arm/joint_states]
  

1.2机器人端的配置

关节轨迹控制器

        MoveIt!完成运动规划后的输出接口是一个命名为“FollowJointTrajectory”的action，其中包含了一系列规划好的路径点轨迹，这些信息可以通过ros_control为我们提供的一个名为“Joint Trajectory Controller”的控制器插件转换成Gazebo中机器人需要的joint位置。

① 配置文件rm_65_trajectory_control.yaml

        “FollowJointTrajectory”控制器的使用首先需要一个配置文件对机械臂六个轴的轨迹控制配置控制参数，即rm_gazebo/config目录下的rm_65_trajectory_control.yaml配置文件，代码如下：

arm:
  arm_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
 
    gains:
      joint1:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint2:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint3:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint4:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint5:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      joint6:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}

② 控制器启动文件 

        通过一个launch文件加载Joint Trajectory Controller，即rm_gazebo/launch目录下的arm_65_trajectory_controller.launch文件，代码如下：

    
              output="screen" ns="/arm" args="arm_joint_controller"/>

关节状态控制器

        关节状态控制器是一个可选插件，主要作用是发布机器人的关节状态和TF变换，否则在rviz的Fixed Frame设置中看不到下拉列表中的坐标系选项，只能手动输入，但是依然可以正常使用。

① 关节状态控制器配置文件arm_gazebo_joint_states.yaml，arm_gazebo_joint_states.yaml配置文件在rm_gazebo/config目录下，代码如下：

arm:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

② 启动文件arm_gazebo_states.launch加载参数

arm_gazebo_states.launch文件在rm_gazebo/launch目录下，代码如下：

    
    
 
              output="screen" ns="/arm" args="joint_state_controller" />
 
    
            respawn="false" output="screen">
        
    

1.3启动文件arm_65_bringup_moveit.launch

        顶层启动文件arm_65_bringup_moveit.launch在rm_gazebo/launch目录下，用以启动Gazebo，并且加载所有的控制器，最后启动MoveIt!，代码如下：

2. 运行效果

启动节点前，需要确保rm_65_moveit_config/launch/rm_65_moveit_controller_manager.launch.xml文件中配置加载到参数服务器的是controllers_gazebo.yaml文件，见图6-1：

图1-1 rm_65_moveit_controller_manager.launch.xml加载controllers_gazebo.yaml

执行以下命令运行MoveIt!和Gazebo： 

cd ~/ws_rmrobot
source devel/setup.bash
roslaunch rm_gazebo arm_65_bringup_moveit.launch

图1-2 RM65-B机器人在Gazebo中显示效果

启动后打开的rviz如图6-3所示：

图1-3 rviz启动界面

        在打开的rviz中，暂时还看不到任何机器人模型，甚至还会提示一些错误，接下来进行简单配置解决这些问题。

        首先将“Fixed Frame”修改成“base_link”；然后点击左侧插件列表栏中的“Add”按钮，将MotionPlanning加入控制窗口。此时应该可以看到机器人出现在右侧主界面中，Gazebo中机器人的姿态应该和rvize中的显示一致。操作如图6-4~6-6所示：

 图1-4 在rviz中将“Fixed Frame”修改成“base_link”

图1-5 rviz中将MotionPlanning加入控制窗口

图1-6 rviz中显示RM65-B机器人 

        接下来使用MoveIt!规划运动的几种方式就可以控制Gazebo中的机器人了，例如图6-7拖动机器人末端到一个位置，然后点击“Plan & Execute”按钮，可以看到rviz中机器人开始规划执行并且可以看到Gazebo中的机器人开始运动且与rviz中的机器人模型保持一致，如图6-8所示。        

图1-7 使用MoveIt!拖动规划执行 

 图1-8 Gazebo中机器人按rviz规划同步执行效果