MoveIt!机械臂控制（五）

一、Gazebo机械臂仿真

在未拥有机械臂实体的情况下，可以通过Gazebo来仿真一个机械臂

1.1创建配置文件

首先需要配置controller插件，Gazebo中需要用到的控制器就是ros_control提供的joint_position_controller，配置文件marm_gazebo/config/arm_gazebo_control.yaml

arm:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
  
  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: position_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: position_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint3_position_controller:
    type: position_controllers/JointPositionController
    joint: joint3
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint4_position_controller:
    type: position_controllers/JointPositionController
    joint: joint4
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint5_position_controller:
    type: position_controllers/JointPositionController
    joint: joint5
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint6_position_controller:
    type: position_controllers/JointPositionController
    joint: joint6
    pid: {p: 100.0, i: 0.01, d: 10.0}

这个配置文件定义了每个关节的位置控制器JointPositionController，并需要将控制器绑定到具体的joint上，还设置了每个关节控制的PID参数。另外还配置了一个joint_state_controller，用来发布机器人每个关节的状态，类似于joint_state_publisher节点

1.2创建Launch文件

launch文件首先将配置文件中所有的参数加载到ROS参数服务器上，然后使用controller_spawner一次性加载所有控制器，最后还要通过robot_state_publisher节点发布机器人的状态

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find marm_gazebo)/config/arm_gazebo_control.yaml" command="load"/>
    <!-- 加载controllers -->
    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/arm" args="joint_state_controller
                                          joint1_position_controller
                                          joint2_position_controller
                                          joint3_position_controller
                                          joint4_position_controller
                                          joint5_position_controller
                                          joint6_position_controller"/>

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
          respawn="false" output="screen">
        <remap from="/joint_states" to="/arm/joint_states" />
    </node>
</launch>

再创建一个顶层的launch文件marm_gazebo/launch/arm_gazebo_control.launch包含上面的arm_gazebo_controller.launch，并启动Gazebo仿真环境

<launch>
    <!-- 启动Gazebo  -->
    <include file="$(find marm_gazebo)/launch/arm_world.launch" />   
    <!-- 启动Gazebo controllers -->
    <include file="$(find marm_gazebo)/launch/arm_gazebo_controller.launch" />   
</launch>

1.3开始仿真

使用命令运行机器人仿真环境

$ roslaunch marm_gazebo arm_gazebo_control.launch

通过rostopic list命令查看当前系统中的话题列表

其中可以看到position_controller的一系列话题列表

这一类的话题都比较简单，只包含一个64位浮点数的位置指令，所以需要让哪个轴转动，就发哪个轴的消息即可，如：

$ rostopic pub /arm/joint2_position_controller/command std_msgs/Float64 1.0

同时可以使用如下命令查看每个轴的实时情况

$ rostopic echo /arm/joint_states

二、使用MoveIt!控制Gazebo中的机械臂

2.1关节轨迹控制器

MoveIt!完成运动规划之后的输出接口是一个命名为FollowJointTrajectory的action，其中包含一系列规划好的路径点轨迹

ros_control为我们提供了一个名为Joint Trajectory Controller的控制器插件可以完成将路径点轨迹转化为Gazebo机器人需要的joint位置输入

Joint Trajectory Controller用来控制一组joint在空间关节的运动，通过接收到的路径点信息，使用样条插补函数计算的机器人各关节的周期位置

该控制器同样需要一个yaml文件进行配置，marm_gazebo/config/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}

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

创建一个launch文件加载该控制器插件marm_gazebo/launch/arm_trajectory_controller.launch

<launch>

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

    <node name="arm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/arm" args="arm_joint_controller gripper_controller"/>

</launch>

2.2MoveIt!控制器

修改MoveIt!控制器配置文件marm_moveit_config/config/controllers_gazebo.yaml，其添加了控制器的命名空间，保证Gazebo中ros_controller发布的action对接

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

  - name: arm/gripper_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - finger_joint1
      - finger_joint2

后修改marm_moveit_config功能包中的arm_moveit_controller_manager.launch，加载修改之后的控制其配置文件

<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 -->
  <!-- Arbotix -->
  <rosparam file="$(find marm_moveit_config)/config/controllers.yaml"/>
  <!-- Gazebo -->
  <rosparam file="$(find marm_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>

2.3关机状态控制器

关节状态控制器是一个可选插件主要作用就是发布机器人的关节状态和TF变换，否则在RViz的Fixed Frame设置中不能看到下拉列表中的坐标系选项

关节状态控制器在marm_gazebo/config/arm_gazebo_joint_states.yaml

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

后创建marm_gazebo/launch/arm_gazebo_states.launch实现参数加载

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find marm_gazebo)/config/arm_gazebo_joint_states.yaml" command="load"/>
    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/arm" args="joint_state_controller" />
    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
        <remap from="/joint_states" to="/arm/joint_states" />
    </node>
</launch>

2.4运行效果

创建一个marm_gazebo/launch/arm_bringup_moveit.launch，启动Gazebo并加载所有控制器，最后启动MoveIt!

<launch>
  
    <!-- Launch Gazebo  -->
    <include file="$(find marm_gazebo)/launch/arm_world.launch" />

    <!-- ros_control arm launch file -->
    <include file="$(find marm_gazebo)/launch/arm_gazebo_states.launch" />   

    <!-- ros_control trajectory control dof arm launch file -->
    <include file="$(find marm_gazebo)/launch/arm_trajectory_controller.launch" />

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

</launch>

运行：

$ roslaunch marm_gazebo arm_bringup_moveit.launch

三、问题解决

• 没有安装相应的ros控制软件包

解决办法：

$ sudo apt-get update
$ sudo apt-get install ros-melodic-joint-state-controller
$ sudo apt-get install ros-melodic-position-controllers