Gazebo与ros_control（3）：Moveit输出规划轨迹到Gazebo

在上一篇中介绍了ros_control的两个应用，但是还没结束，因为规划问题是一个很重要，而且也很有挑战性的问题。好在ROS里提供了一些规划的接口，比如主要用于机械臂规划的MoveIt，其实它也可以用于规划底座，这个只是在Moveit的一个PPT里看见过，是用于PR2的，其他的好像没看见过。这里主要是将上篇的7自由度手臂结合ros_control与Moveit。

1. 关于Moveit的配置

我就不多讲，给出几个关键地方的截图

这里的Planning Group比较特别，它的手臂是用的Chain，从base_link到两个夹子中间的grasping_frame

而夹子的那个Group是没有选择KDL求解器的。

这里我感觉对我自己的youBot很有启发，因为我也有一个grasp_frame设在了夹子之间。

2. 配置controller.yaml文件和controller_manager.launch.xml文件

前面那个文件在config文件夹下建立，ros by example上面有详细说明。后面那一个在launch文件夹直接修改。目的是为MoveIt的controller添加配置，因为Moveit的规划完的结果一般是不能直接给到机器人上的，通过action接口作为一个桥梁。

controller.yaml文件如下

controller_manager_ns: controller_manager
controller_list:
  - name: seven_dof_arm/seven_dof_arm_joint_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - shoulder_pan_joint
      - shoulder_pitch_joint
      - elbow_roll_joint
      - elbow_pitch_joint
      - wrist_roll_joint
      - wrist_pitch_joint
      - gripper_roll_joint
  - name: seven_dof_arm/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1
      - finger_joint2

controller_manager.launch.xml文件如下。这个文件很特别，见过几个都是这样写的，暂且说写法唯一。

<launch>
	<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
	<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
	<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

	<!-- load controller_list -->
	<arg name="use_controller_manager" default="true" />
	<param name="use_controller_manager" value="$(arg use_controller_manager)" />

	<!-- Load joint controller configurations from YAML file to parameter server -->
	<rosparam file="$(find seven_dof_arm_config)/config/controllers.yaml"/>
</launch>

3. 为Gazebo配置controller

在前面两篇文章中，有配置过gazebo的controller的参数，rrbot中是叫config/rrbot_control.yaml。7自由度手臂也是一个control.yaml其中都是设置了joint_state_controller/JointStateController和effort_controllers/JointPositionController（rrbot）以及position_controllers/JointPositionController（seven_dof_arm）。然后调用一个launch文件读取配置。

这里的controller借助了action，如果你还有印象，一般action都是trajectory相关的，其实在这里为了与Moveit那边连起来，所以还要设置一个trajectory controller。

trajectory_control.yaml 内容如下

seven_dof_arm:
  seven_dof_arm_joint_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - shoulder_pan_joint
      - shoulder_pitch_joint
      - elbow_roll_joint
      - elbow_pitch_joint
      - wrist_roll_joint
      - wrist_pitch_joint
      - gripper_roll_joint

    gains:
      shoulder_pan_joint:   {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      shoulder_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      elbow_roll_joint:  {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      elbow_pitch_joint:       {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      wrist_roll_joint:    {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      wrist_pitch_joint:      {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
      gripper_roll_joint:    {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}


  gripper_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - finger_joint1
      - finger_joint2
    gains:
      finger_joint1:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
      finger_joint2:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}

同样的，通过一个launch文件加载上面的参数

seven_dof_arm_trajectory_controller.launch内容如下

<launch>
	<rosparam file="$(find seven_dof_arm_gazebo)/config/trajectory_control.yaml" command="load"/>

  <node name="seven_dof_arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/seven_dof_arm" args="seven_dof_arm_joint_controller gripper_controller"/>

</launch>

4. 启动Gazebo和MoveIt

seven_dof_arm_bringup_gazebo_moveit.launch内容如下

<launch>


  <!-- load urdf to Gazebo  -->
  <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_world.launch" />   

  <!-- JointStateController and robot state publisher -->
  <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_gazebo_states.launch" />   

  <!-- ros_control TrajectoryController -->
  <include file="$(find seven_dof_arm_gazebo)/launch/seven_dof_arm_trajectory_controller.launch" />    

  <!-- moveit move group -->
  <include file="$(find seven_dof_arm_config)/launch/moveit_planning_execution.launch" />    


</launch>

注意，最后一个launch是在seven_dof_arm_config文件夹，因为是启动moveit中的move group。

当在rviz中用moveit设置一个目标姿态，然后plan and execute的时候，Gazebo中的模型也动起来了。通过节点关系点图，我们也可以看到，它们之间是通过follow_joint_trajectory/action_topic来进行通讯的。

最后的仿真视频：点击打开视频