机械臂智能装配（二）Gazebo中的机械臂仿真 打通moveit和gazebo之间的通信 实现工作空间规划 python

一，前言

前面介绍了,机械臂使用MoveIt!实现运动控制,控制的过程和结
果显示在rviz中。如果有真实的机械臂,就可以直接连接机器人实体,发布运动规划的结果,通过机械臂的控制器实现真实运动。但是大部分读者不一定有机械臂实体,那么我们也可以通过Gazebo来仿真一个机械臂。机械臂模型的创建过程已经在10.3节完成,下面将讲解如何使用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文件marm_gazebo/launch/arm_gazebo_controller.launch,加载设置好的所有控制器,代码如下:

<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文件首先将配置文件中的所有参数加载到ROS参数服务器上,然后使用controller_spawner一次性加载所有控制器,最后还要通过robot_state_publisher节点发布机器人的状态。

再创建一个顶层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

现在就可以使用如下命令启动机器人仿真环境了:

在启动的终端中,可以看到类似下图所示的控制器加载信息,如果没有显示这些信息,机器人是无法在Gazebo中运动的。

那怎么让机器人运动起来呢?当然是发送控制运动的话题消息。
用rostopic list命令查看当前系统中的话题列表,应该可以看到如下所示的控制器订阅的控制命令话题。

/arm/joint1_position_controller/command
/arm/joint2_position_controller/command
/arm/joint3_position_controller/command
/arm/joint4_position_controller/command
/arm/joint5_position_controller/command
/arm/joint6_position_controller/command
/arm/joint_states
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/rosout
/rosout_agg
/tf
/tf_static

这些话题消息的类型都比较简单,只包含一个64位浮点数的位置指令,所以需要让哪个轴转动,就发布相应哪个轴的消息。例如要让机器人的joint2运动到弧度为1的位置,只需要发布以下话题消息:

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

消息发布后,gazebo中的机械臂应该立刻就会开始运动,joint2会移动到如下图所示弧度为1的位置。
可以使用以下命令监控每个轴的实时状态

rostopic echo /arm/joint_states

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

上面在Gazebo中的运动控制还是有点简单,虽然可以控制机器人运动,但是无法完成复杂运动的规划。此时我们又想到了专门做运动规划的MoveIt!,那么能不能通过MoveIt!实现Gazebo中仿真机器人的运动规划呢?答案当然是肯定的,接下来学习如何打通MoveIt!和Gazebo之间的通信。

2.1 关节轨迹控制器

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

Joint Trajectory Controller用来控制一组joint在关节空间的运动,通过接收到的路径点信息,可以使用样条插补函数计算得到机器人各关节的周期位置。这里的样条函数有以下几种。

1)线性样条:只能保证位置的连续,速度、加速度不连续。

2)三次样条:可以保证位置和速度的连续,但是加速度不连续。

3)五次样条:保证位置、速度、加速度都连续。

这个控制器的使用方法和其他控制器的类似,同样需要创建一个配置文件。这里创建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文件加载Joint Trajectory Controller,创建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.2 MoveIt!控制器

在MoveIt!端也要修改之前控制器的配置文件controllers.yaml。重新创建一个MoveIt!控制器的配置文件marm_moveit_config/config/controllers_gazebo.yaml,代码如下

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

controllers_gazebo.yaml的内容与controllers.yaml的几乎一致,区别在于添加了控制器的命名空间,否则无法与Gazebo中ros_controller发布的action对接,会提示action客户端连接失败的错误

然后修改marm_moveit_config功能包中的arm_moveit_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 -->
  <!-- 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>

现在通过launch文件运行MoveIt!和Gazebo:

roslaunch marm_gazebo arm_bringup_moveit.launch

稍等一会,rviz和Gazebo就会启动

rviz效果：

gazebo的效果：

2.5 工作空间规划

在上一个博客中，我们实现了机械臂在rviz中的工作空间规划，那么现在就在gazebo中实现。

运行了roslaunch marm_gazebo arm_bringup_moveit.launch命令后
我们在终端中输入：

rosrun marm_planning moveit_ik_demo.py

稍等片刻，可以看到
rviz效果：

gazebo效果：

rviz和gazebo两个实现了同步运动。

2.6 去掉rviz

我的个人习惯，我只想使用gazebo这个仿真，不想启动两个，那么我们就把rviz给去掉。

启动rviz的的文件位于：marm_moveit_config/launch/moveit_planning_execution.launch

打开后，我们把有moveit_rviz.launch这一行给注释掉，结果如下

<launch>
 # 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
 <include file="$(find marm_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>
 # The visualization component of MoveIt!
 <!--<include file="$(find marm_moveit_config)/launch/moveit_rviz.launch"/>-->

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/arm/joint_states]</rosparam>
  </node>

</launch>

我们重新启动，就会发现只有gazebo仿真启动了。

四，执行过程中的错误解决

报错1

然后会看到出现这样的错误

[ERROR] [1604554506.351339231, 0.013000000]: GazeboRosControlPlugin missing <legacyModeNS> while using DefaultRobotHWSim, defaults to true.
This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, where the robotNamespace is disregarded and absolute paths are used instead.
If you do not want to fix this issue in an old package just set <legacyModeNS> to true.

解决方法：
打开marm_description/urdf中的arm.xacro
找到这一段代码

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

在机械臂的 urdf 描述文件中，对 gazebo 插件描述添加 true 可以解决

    <!-- ros_control plugin -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/arm</robotNamespace>
	    <legacyModeNS>true</legacyModeNS>
        </plugin>
    </gazebo>

重新运行，这个错误就不会出现了

错误2

[ERROR] [1604554506.483296774, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint1
[ERROR] [1604554506.487116418, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint2
[ERROR] [1604554506.492438670, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint3
[ERROR] [1604554506.495822717, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint4
[ERROR] [1604554506.498060833, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint5
[ERROR] [1604554506.500233078, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/joint6
[ERROR] [1604554506.502268967, 0.013000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/finger_joint1

这个错误可以忽略，之前也出现过，项目也能运行。

错误3

在我的启动后，rviz回报错

解决方法：

这个错误的源头应该是一个marm_moveit_config/launch/moveit.rviz文件的问题，但是我没找出来，所以只能手动修改

1,把Global Options下面的Fixed Frame改为base_link（原来是map）

2，点击add

3,添加RobotModel