【从零开始的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>
<rosparam file="$(find simple_arm)/config/controllers.yaml" command="load"/>
<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：
（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）定义摄像头
<robot>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/simple_armrobotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
plugin>
gazebo>
<gazebo reference="base_link">
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="link_1">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="link_2">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="camera_link">
<material>Gazebo/Greymaterial>
gazebo>
<gazebo reference="camera_link">
<sensor type="camera" name="rgb_camera">
<update_rate>30.0update_rate>
<camera name="rgb_camera">
<horizontal_fov>1.4horizontal_fov>
<image>
<width>640width>
<height>480height>
<format>R8G8B8format>
image>
<clip>
<near>0.02near>
<far>300far>
clip>
camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>truealwaysOn>
<updateRate>0.0updateRate>
<cameraName>rgb_cameracameraName>
<imageTopicName>image_rawimageTopicName>
<cameraInfoTopicName>camera_infocameraInfoTopicName>
<frameName>camera_linkframeName>
<hackBaseline>0.0hackBaseline>
<distortionK1>0.0distortionK1>
<distortionK2>0.0distortionK2>
<distortionK3>0.0distortionK3>
<distortionT1>0.0distortionT1>
<distortionT2>0.0distortionT2>
plugin>
sensor>
gazebo>
robot>
simple_arm.urdf.xacro:其中...为省略部分：
这个部分定义link、joint和transmission
<robot name="simple_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="link_type" value="cuboidal" />
<xacro:property name="PI" value="3.14159"/>
...
<xacro:include filename="$(find simple_arm)/urdf/simple_arm.gazebo.xacro" />
<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>
...
<link name="camera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${camera_size}"/>
geometry>
collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<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>
<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/SimpleTransmissiontype>
<joint name="joint_1">
<hardwareInterface>EffortJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<hardwareInterface>EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
<transmission name="tran2">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="joint_2">
<hardwareInterface>EffortJointInterfacehardwareInterface>
joint>
<actuator name="motor2">
<hardwareInterface>EffortJointInterfacehardwareInterface>
<mechanicalReduction>1mechanicalReduction>
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连接起来。
<robot>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/arm1robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
plugin>
gazebo>
<gazebo reference="base_link">
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="link1">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="link2">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Whitematerial>
gazebo>
<gazebo reference="link3">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="link4">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Whitematerial>
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文件。
<robot
name="arm1"
xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find arm1)/urdf/arm1.gazebo.xacro" />
<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>
<joint name="fixed_base_joint" type="fixed">
<parent link="world"/>
<child link="base_link"/>
joint>
...(省略部分)
<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>
<rosparam file="$(find arm1)/config/controllers.yaml" command="load"/>
<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>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find arm1)/urdf/arm1.urdf.xacro'"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="false"/>
node>
<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"/>
<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
运行如下：