使用URDF创建机械臂仿真模型

使用ROS强大的机器人功能，建立ROS可以识别的机器人模型是第一步，也是无法绕过去的环节.ROS通过URDF（UnifiedRobot Description Format) 就是描述机器人硬件尺寸布局.它是同ROS沟通的渠道，有了机器人的物理尺寸，ROS就可以有效的进行Navagation和碰撞检测(collision detect)了。
Unified Robot Description Format，统一机器人描述格式，简称为URDF。ROS中的urdf功能包包含一个URDF的C++解析器，URDF文件使用XML格式描述机器人模型。必须承认，URDF 是个很难懂的语言，但有RViz的强大仿真功能，我们很容易调试它。

一，软件环境

OS:Ubuntu 15.10
ROS:Kinetic Full package

二.基础模型

创建工作空间,在src下创建beginner_tutorials功能包(见相关ROS基础入门)
1.创建ros package命名为beginner_tutorials

catkin_create_pkg my_robot std_msgs rospy roscpp urdf

2.建立文件夹launch和urdf分别用来存放.launch文件和.urdf文件
3.建立urdf文件
在beginner_tutorials 文件夹下建立urdf文件夹，创建机械臂的描述文my_robot.urdf，描述代码如下：

     <robot name="my_robot">
      <link name="link1" />
      <link name="link2" />
      <link name="link3" />
      <link name="link4" />
      <joint name="joint1" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
      joint>
      <joint name="joint2" type="continuous">
         <parent link="link1"/>
         <child link="link3"/>
      joint>
      <joint name="joint3" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
      joint>
     robot>

上边的URDF模型定义了机器人的4个环节（link），然后定义了三个关节（joint）来描述环节之间的关联。

ROS为用户提供了一个检查URDF语法的工具：

sudo apt-get install liburdfdom-tools

安装完毕后，执行检查：

check_urdf my_robot.urdf

出现错误:

Error: Error document empty.
         at line 72 in /build/buildd/urdfdom-0.2.10+dfsg/urdf_parser/src/model.cpp
ERROR: Model Parsing the xml failed

解决:

check_urdf /home/daniel/catkin_ws/src/beginner_tutorials/urdf/my_robot.urdf

然后正常显示:

robot name is: my_robot
---------- Successfully Parsed XML ---------------
root Link: link1 has 2 child(ren)
    child(1):  link2
    child(2):  link3
        child(1):  link4

三 . 添加机器人尺寸

在基础模型之上，我们为机器人添加尺寸大小。由于每个环节的参考系都位于该环节的底部，关节也是如此，所以在表示尺寸大小时，只需要描述其相对于连接的关节的相对位置关系即可。urdf中的域就是用来表示这种相对关系。(定义每个link所在位置)
为所有关节应用尺寸：

    <robot name="my_robot">
      <link name="link1" />
      <link name="link2" />
      <link name="link3" />
      <link name="link4" />
      <joint name="joint1" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="5 3 0" rpy="0 0 0" />
      joint>
        <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link3"/>
        <origin xyz="-2 5 0" rpy="0 0 1.57" />
      joint>
        <joint name="joint3" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
        <origin xyz="5 0 0" rpy="0 0 -1.57" />
     joint>
    robot>

例如，joint2相对于连接的link1在x轴和y轴都有相对位移，而且在z轴上还有90度的旋转变换，所以表示成域的参数就如下所示：

    <origin xyz="-2 5 0" rpy="0 0 1.57" />

四.添加运动学参数

如果我们为机器人的关节添加旋转轴参数，那么该机器人模型就可以具备基本的运动学参数。
例如，joint2围绕正y轴旋转，可以表示成：

   <axis xyz="0 1 0" />

同理，joint1的旋转轴是：

  <axis xyz="-0.707 0.707 0" />

应用到我们的URDF中：

    <robot name="my_robot">
      <link name="link1" />
      <link name="link2" />
      <link name="link3" />
      <link name="link4" />
      <joint name="joint1" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="5 3 0" rpy="0 0 0" />
        <axis xyz="-0.9 0.15 0" />
      joint>
        <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link3"/>
        <origin xyz="-2 5 0" rpy="0 0 1.57" />
        <axis xyz="-0.707 0.707 0" />
      joint>
        <joint name="joint3" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
        <origin xyz="5 0 0" rpy="0 0 -1.57" />
        <axis xyz="0.707 -0.707 0" />
     joint>
    robot>

然后使用check_urdf检查语法错误。

五 . 图形化显示URDF模型

我们现在已经完成了一个简单的URDF模型创建，ROS提供了相应的工具可以让URDF图像化显示出来。

urdf_to_graphiz  /home/daniel/catkin_ws/src/my_robot/urdf/my_robot.urdf

然后打开生成的pdf文件，即可看到图形化的urdf：

六. 创建display.launch文件

该文件可以帮助启动RViz来观察机器人。其中有三个node，一个是rviz,另外两个joint_state_publisher robot_state_publisher是必须的node。

第一个输入参数 model 就是要启动的urdf文件路径。
第二个输入参数 gui 指定是否启用关节转动控制面板窗口。

<launch>  
   <arg name="model" />  
   <arg name="gui" default="False" />  
   <param name="robot_description" textfile="($find my_robot)/urdf/my_robot.urdf)" />  
   <param name="use_gui" value="$(arg gui)"/>  
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />  
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />  
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />  
launch>  

link可以指定许多属性：
定义惯性
定义一个link的外观，大小，颜色和材质纹理贴图
定义碰撞检测属性
我们详细看一下的定义：

定义了几何形状为一个立方体，三个参数分别为长宽高，单位是米(m). 这里我们指定下层小车板子的板的尺寸。可以有好几个选项： 立方体 圆柱体 球体
指定颜色rgb和透明度a. 它们取值范围都是[０,1] 区间。
切换到launch文件夹下,
使用下面命令启动rviz来查看我们刚刚完成的机器人模型：

 roslaunch urdf_tutorial display.launch model:=urdf/my_robot.urdf