【从零开始的ROS四轴机械臂控制】（二） – ROS与Gazebo连接，Gazebo仿真及urdf文件修改

【从零开始的ROS四轴机械臂控制（二）】

• • 四、urdf文件及gazebo仿真
• • • 1.simple_arm示例
  • • • （1）config文件夹
      • （2）launch文件夹
      • （3）meshes文件夹
      • （4）urdf文件夹
      • （5）worlds文件夹
      • （6）文件间联系
  • • 2.根据simple_arm示例改进arm0文件夹
  • • • (1)urdf文件夹
      • • ①arm1.gazebo.xacro
        • ②arm1.urdf.xacro
          • (2)config文件夹
          • (3)launch文件夹
          • • ①robot_control.xml
            • ②robot_description.xml
            • ③gazebo.launch
          • (4)scripts文件夹

四、urdf文件及gazebo仿真

  首先先忽略硬件部分，来看一下gazebo仿真部分。这一个部分的目标是，实现使用arm_mover节点和look_away服务节点来控制gazebo中的机器人。  
  本部分完成后如上图所示，使用simple_mover控制机械臂移动。   我找了很多关于urdf资料后发现，很多人讲urdf说的比较模糊，而且没有实际例子来验证。或者介绍的比较模糊，并不是很能理解从Solidworks导出urdf文件后如何进行实际的应用。很多博客都是以导入到rviz中能使用作为结束，但是这样的话对我们这个项目来说是远远不够的。   所以为了能顺利地进行项目，所以重点来研究一下urdf文件和包。 我在改进我的arm0的包的时候主要参考了simple_arm这个示例，如果不想看simple_arm示例可以直接跳到四、2根据simple_arm 示例改进arm0文件夹，其主要详细说明了四轴机械臂项目的arm0包。  

1.simple_arm示例

有关于工作区可以参考博客【Udacity机器人软件工程师课程笔记（九）-ROS-Catkin包、工作空间和目录结构】   有关simple_arm和gazebo可以参考【Udacity机器人软件工程师课程笔记（十）-ROS-Catkin-包（package）和gazebo】  

以下是从github仓库克隆的 simple_arm 的包。  
  接下来要针对每一个文件来分析其作用。当时我在学习使用simple_arm这个包时，没有很好的研究其文件内容。但是现在要搭建针对于自己项目的gazebo文件，还是很有必要好好的看一下其中文件的结构及内容的。  

（1）config文件夹

  config文件夹：配置文件  
controllers.yaml  

simple_arm:
    #list of controllers
    joint_state_controller:
      type: joint_state_controller/JointStateController
      publish_rate: 50
    joint_1_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint_1
      pid: {p: 100.0, i: 0.01, d: 10.0}
    joint_2_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint_2
      pid: {p: 100.0, i: 0.01, d: 10.0}

（2）launch文件夹

  launch：提供更自动化的启动节点的方式
  robot_control.xml：  

<launch>
  <!--将关节控制器加载到param服务器-->
  <rosparam file="$(find simple_arm)/config/controllers.yaml" command="load"/>
  <!--Load controllers-->
  <node name="spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/simple_arm" args="joint_state_controller
    joint_1_position_controller
    joint_2_position_controller"/>
</launch>
```xml
robot_description.xml：

  “` robot_spawn.launch “`xml“`  

（3）meshes文件夹

 
  meshes中包含了相机的3d模型文件。   meshes 中文件可以包括meshes（.dae（Collada）或.stl（STereoLithography）格式的CAD文件）。  

（4）urdf文件夹

  urdf：通用机器人描述文件。  
  simple_arm.gazebo.xacro: 定义gazebo中的的模型，其中包括  

• （1）定义gazebo
• （2）定义link
  • 1）base link
  • 2）link_1
  • 3）link_2
  • 4）camera_link
• （3）定义摄像头

<?xml version="1.0"?>
<robot>
  <!-- ros_control插件 -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/simple_arm</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>

  <!--base_link-->
  <gazebo reference="base_link">
    <material>Gazebo/White</material>
  </gazebo>

  <!--link_1-->
  <gazebo reference="link_1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Orange</material>
  </gazebo>

  <!--link_2-->
  <gazebo reference="link_2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/White</material>
  </gazebo>

  <!--camera_link-->
  <gazebo reference="camera_link">
    <material>Gazebo/Grey</material>
  </gazebo>

  <!-- 摄像头-->
  <gazebo reference="camera_link">
    <sensor type="camera" name="rgb_camera">
      <update_rate>30.0</update_rate>
      <camera name="rgb_camera">
        <horizontal_fov>1.4</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>rgb_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_link</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

</robot>

  simple_arm.urdf.xacro:其中...为省略部分：   这个部分定义link、joint和transmission  

<?xml version="1.0"?>
<robot name="simple_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!--Define constants-->
  <xacro:property name="link_type" value="cuboidal" />
  <xacro:property name="PI" value="3.14159"/>
  ...

  <!--Import gazebo elements-->
  <xacro:include filename="$(find simple_arm)/urdf/simple_arm.gazebo.xacro" />

  <!--Links-->
  <link name="world"/>

  <link name="base_link">
    <visual>
      <origin xyz="0 0 ${length1/2}" rpy="0 0 0"/>
      <geometry>
        <cylinder length="${length1}" radius="${radius1}"/>
      </geometry>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 ${length1/2}" rpy="0 0 0"/>
      <geometry>
        <cylinder length="${length1}" radius="${radius1}"/>
      </geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 ${length1/2}" rpy="0 0 0"/>
      <mass value="${mass1}"/>
      <inertia
        ixx="${mass1 / 12.0 * (2*radius1*2*radius1 + length1*length1)}" ixy="0.0" ixz="0.0"
        iyy="${mass1 / 12.0 * (length1*length1 + 2*radius1*2*radius1)}" iyz="0.0"
        izz="${mass1 / 12.0 * (2*radius1*2*radius1 + 2*radius1*2*radius1)}"/>
    </inertial>
  </link>
  
  ...
  
  <!--camera_link-->
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <!--mesh filename="package://simple_arm/meshes/camera.dae"/-->
        <sphere radius="${camera_size}"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <!--mesh filename="package://simple_arm/meshes/camera.dae"/-->
        <sphere radius="${camera_size}"/>
      </geometry>
      <material name="red">
        <color rgba="1.0 0 0 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

  <!--Joints-->
  <joint name="fixed_base_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>

  ...
  
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 0 1" />
    <origin xyz="0 0 ${length3}" rpy="0 -${PI/2} 0"/>
    <parent link="link_2"/>
    <child link="camera_link"/>
  </joint>

  <!--转换和执行器-->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</robot>

（5）worlds文件夹

 
willow_garage.world是用于Gazebo仿真环境的XML文件，如果不定义可以使用空白世界。所以这个文件为可选文件。  

（6）文件间联系

 
  从框图中我们可以比较清楚的的除了文件的框架结构。所有的文件都指向到robot_spawn.launch文件。要实现搭建自己的包，就要按照框图一步步的修改从solidworks导出的urdf文件。  

各文件夹功能如下所示   • script（python可执行文件） • src（C ++源文件） • msg（用于自定义消息定义） • srv（用于服务消息定义） • include – >作为依赖项所需的头文件/库 • config – >配置文件 • launch – >提供更自动化的启动节点的方式   其他文件夹可能包括   • urdf（通用机器人描述文件） • meshes（.dae（Collada）或.stl（STereoLithography）格式的CAD文件） • worlds（用于Gazebo仿真环境的XML文件）  

2.根据simple_arm示例改进arm0文件夹

  由于某些原因（导出了多次urdf文件），我的文件夹实际名称为arm1，在此特别说明一下,在接下来也会以arm1作为例子。  
  以上是arm1包中的所有内容，我已经将它上传至github，其版本号定为Robot arm 0.1.1，功能是通过simple_mover.py实现了最基本机械臂的运动 。   arm1包（arm version 0.1.1）结构如下  

arm1
  |-config
  |  |-controllers.yaml
  |-launch
  |  |-gazebo.launch
  |  |-robot_control.xml
  |  |-robot_description.xml
  |-meshes(未更改)
  |  |-base_link.STL
  |  |-link1.STL
  |  |-link2.STL
  |  |-link3.STL
  |  |-link4.STL
  |-scripts
  |  |-simple_mover
  |-urdf
  |  |-arm1.urdf.xacro
  |  |-arm1.gazebo.xacro
  |-worlds
  |  |-willow_garage.world
  |-CMakeLists.txt(未更改)
  |-export.log(未更改)
  |-package.xml(未更改)

    其中 CMakeLists.txt、export.log、package.xml和meshes文件夹中内容为solidworks转出的自带内容，其它文件都进行了更改。  

(1)urdf文件夹

  这部分使用Xacro（XML宏）。   Xacro是一种XML宏语言。使用xacro，可以使用扩展较大的XML表达式的宏来构造更短，更易读的XML文件。  

有关xacro的更多信息可以参考ROS Wiki

①arm1.gazebo.xacro

  arm1.gazebo.xacro的主要功能是，添加gazebo_ros_control插件，将ros和gazebo连接起来。  

<?xml version="1.0"?>
<robot>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/arm1</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
  <!--base_link-->
  <gazebo reference="base_link">
    <material>Gazebo/White</material>
  </gazebo>
  <!--link_1-->
  <gazebo reference="link1">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Orange</material>
  </gazebo>
  <!--link_2-->

  <gazebo reference="link2">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/White</material>
  </gazebo>
  <!--link_3-->
  <gazebo reference="link3">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/Orange</material>
  </gazebo> 
 <!--link_4-->
  <gazebo reference="link4">
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
    <material>Gazebo/White</material>
  </gazebo>
</robot>

②arm1.urdf.xacro

  为了方便阅读，我在其中删除了一部分的内容,完整部分可以查看github。   这个文件主要定义了arm的urdf文件。其中包括link，joint和Transmission。link和joint 在solidworks转换出来的文件中已经给出了，在这里不在赘述。   主要注意的有三个地方。   一、我们在urdf文件中，需要对joint打上gazebo使用的transmission标签，transmission把TF（ros）的连接关系与仿真平台上的驱动设备（gazebo）联系在了一起，有了执行器，gazebo就可以在物理层面上对模型进行驱动了。

更多关于transmission标签可以参考此处。

  二、我们需要定义world link，然后再将base link 和world link连接起来，这样模型才能在gazebo世界中固定。   三、需要导入arm1.gazebo.xacro文件。  

<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
     Commit Version: 1.5.1-0-g916b5db  Build Version: 1.5.7152.31018
     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
  name="arm1" 
  xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!--Import gazebo elements-->
  <xacro:include filename="$(find arm1)/urdf/arm1.gazebo.xacro" />
  <!--Links and joints-->
  <link name="world"/>
  <link name="base_link">
    <inertial>
      <origin
        xyz="0.000877732518704125 -6.1202787414609E-05 0.0136416983965608"
        rpy="0 0 0" />
      <mass
        value="0.0149906128541691" />
      <inertia
        ixx="9.0485354260676E-07"
        ixy="9.67790693248534E-12"
        ixz="-1.26661641226237E-08"
        iyy="1.3035966603116E-06"
        iyz="-4.46725888473771E-12"
        izz="1.62723345073819E-06" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://arm1/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://arm1/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <!--this joint used to fix model-->
  <joint name="fixed_base_joint" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
  ...(省略部分)
  <!--Transmission and actuators-->
  <transmission name="tran1">
  ...(省略部分)
  </transmission>
</robot>

(2)config文件夹

  为每个joint添加控制器controller，保存为controllers.yaml文件。  

arm1:
    #list of controllers
    joint_state_controller:
      type: joint_state_controller/JointStateController
      publish_rate: 50
    joint1_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint1
      pid: {p: 1, i: 0.001, d: 0.002}
    joint2_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint2
      pid: {p: 1, i: 0.001, d: 0.002}
    joint3_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint3
      pid: {p: 1, i: 0.001, d: 0.002}
    joint4_position_controller:
      type: effort_controllers/JointPositionController
      joint: joint4
      pid: {p: 1, i: 0.001, d: 0.002}

(3)launch文件夹

①robot_control.xml

  该xml文件负责加载控制器。  

<launch>
  <!--Load the joint controllers to param server-->
  <rosparam file="$(find arm1)/config/controllers.yaml" command="load"/>
  <!--Load controllers-->
  <node name="spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/arm1" args="joint_state_controller
    joint1_position_controller
    joint2_position_controller
    joint3_position_controller
    joint4_position_controller"/>
</launch>

②robot_description.xml

  机器人描述文件，其每个部分的功能见注释。  

<launch>
  <!--Load urdf to param server-->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find arm1)/urdf/arm1.urdf.xacro'"/>

  <!--GUI used to send fake joint values-->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
  </node>
  <!--Publish robot state to TF-->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
    <remap from="/joint_states" to="/arm1/joint_states" />
  </node>
</launch>

③gazebo.launch

  最终的运行文件，加载gazebo和相关节点。  

<launch>
  <include file="$(find arm1)/launch/robot_description.xml"/>
  <include file="$(find arm1)/launch/robot_control.xml"/>

  <!--Launch a gazebo world-->
  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
    <arg name="world_name" value="$(find arm1)/worlds/willow_garage.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>

  <node 
     name="urdf_spawner" 
     pkg="gazebo_ros" 
     type="spawn_model" 
     respawn="false" output="screen"
     args="-urdf -param robot_description -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -model arm1"/>

</launch>

  打开一个新的终端  

source ~/catkin_ws/devel/setup.bash
roslaunch arm1 gazebo.launch

  然后我们就可以看到gazebo成功打开了，模型成功加载如下。  
  若gazebo中模型出现抖动，则考虑controllers.yaml中的pid参数的设置。我当时按照pid默认值来运行gazebo模型，结果造成了非常严重的模型抖动。猜测可能是因为模型尺寸和simple_arm的尺寸相差较大而导致的。   确定了只是因为pid参数的问题之后，那就需要调参了。调参可是门艺术啊。我调整完的参数已经在controllers.yaml中给出了，其效果比默认值好上很多了，但是还是有抖动。   每次调参数，需要重新开启新的终端加载gazebo。如果有必要，可以输入以下命令，来关闭所有的gazevo服务。  

$ killall gzserver

(4)scripts文件夹

  scripts文件夹中存放python的可执行文件，我们需要把我们的simple_mover放到这里边。 若不存在scripts文件夹，则首先创建它

$ cd ~/catkin_ws/src/arm1/
$ mkdir scripts
$ cd ~/catkin_ws/src/arm1/scripts 

  创建simple_mover 文件

$ sudo touch simple_mover 

  使文件可执行

$ sudo chmod 777 simple_mover 

  编写simple_mover文件，也可以直接打开文件进行编辑。

nano simple_mover

  以下内容可以参考博客【Udacity机器人软件工程师课程笔记（十一）-ROS-编写ROS节点】  

#!/usr/bin/env python

import math
import rospy
from std_msgs.msg import Float64

def mover():
    pub_j1 = rospy.Publisher('/arm1/joint1_position_controller/command',
                             Float64, queue_size=10)
    pub_j2 = rospy.Publisher('/arm1/joint2_position_controller/command',
                             Float64, queue_size=10)
    pub_j3 = rospy.Publisher('/arm1/joint3_position_controller/command',
                             Float64, queue_size=10)
    pub_j4 = rospy.Publisher('/arm1/joint4_position_controller/command',
                             Float64, queue_size=10)
    rospy.init_node('arm_mover')
    rate = rospy.Rate(10)
    start_time = 0

    while not start_time:
        start_time = rospy.Time.now().to_sec()

    while not rospy.is_shutdown():
        elapsed = rospy.Time.now().to_sec() - start_time
        pub_j1.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        pub_j2.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
		pub_j3.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
		pub_j4.publish(math.sin(2*math.pi*0.1*elapsed)*(math.pi/2))
        rate.sleep()

if __name__ == '__main__':
    try:
        mover()
    except rospy.ROSInterruptException:
	pass

  在gazebo运行的条件下，新建一个终端。  

source ~/catkin_ws/devel/setup.bash
rosrun arm1 simple_mover

  运行如下：