ROS机械臂开发：创建机器人URDF模型

1. URDF建模原理

URDF是一种使用XML格式描述的机器人模型文件

• Links：坐标系与几何关系
• Joints：Links之间的关系

<link>标签

• 描述机器人某个刚体部分的外观和物理属性；
• 描述连杆尺寸、颜色、形状、惯性矩阵、碰撞属性等；
• 每个link会成为一个坐标系

一个典型的link标签如下：

<link name="my_link">
   <inertial>
     <origin xyz="0 0 0.5" rpy="0 0 0"/>
     <mass value="1"/>
     <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
   </inertial>

   <visual>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <geometry>
       <box size="1 1 1" />
     </geometry>
     <material name="Cyan">
       <color rgba="0 1.0 1.0 1.0"/>
     </material>
   </visual>

   <collision>
     <origin xyz="0 0 0" rpy="0 0 0"/>
     <geometry>
       <cylinder radius="1" length="0.5"/>
     </geometry>
   </collision>
 </link>

（注意长度单位是米，角度单位是弧度）
<collision>标签描述了碰撞属性，如下图红色框（圆柱体）所示，当进入框内时就会认为发生碰撞，这是为了加速碰撞检测做的一种简化

link.png

<joint>标签

• 描述两个link之间的关系，分为六种类型；
• 包括关节运动的位置和速度限制；
• 描述机器人关节的运动学和动力学属性

 <joint name="my_joint" type="floating">
    <origin xyz="0 0 1" rpy="0 0 3.1416"/>
    <parent link="link1"/>
    <child link="link2"/>

    <calibration rising="0.0"/>
    <dynamics damping="0.0" friction="0.0"/>
    <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
    <safety_controller k_velocity="10" k_position="15" soft_lower_limit="-2.0" soft_upper_limit="0.5" />
 </joint>

每个joint连接两个link，其中的origin是相对于上一个joint的origin描述的，而上面的<link>标签中的origin是相对于joint坐标系表达的，也就是说在机器人运动过程中，每个连杆的joint和link坐标系是相对静止的，理解这一点很重要。

joint.png

<robot>标签

整个模型由<robot>根标签包含，一个完整的机器人模型，由一系列<link>和<joint>组成

<robot name="pr2">
  <link> ... </link>
  <link> ... </link>
  <link> ... </link>

  <joint>  ....  </joint>
  <joint>  ....  </joint>
  <joint>  ....  </joint>
</robot>

robot.png

URDF建模存在的问题

• 模型冗长，重复内容过多
• 参数修改麻烦，不便于二次开发
• 没有参数自动计算功能

由此产生了URDF的改进版本：xacro模型文件，它具有如下优点：

• 精简模型代码：
  创建宏定义
  文件包含
• 提供可编程接口：
  常量}
  变量
  数学计算
  条件语句

常量定义

<xacro:property name="the_radius" value="2.1" />
<xacro:property name="the_length" value="4.5" />

常量使用

<geometry type="cylinder" radius="${the_radius}" length="${the_length}" />

数学计算

<!-- + - * /运算-->
<xacro:property name="radius" value="4.3" />
<circle diameter="${2 * radius}" />

宏定义

<xacro:macro name="pr2_caster" params="suffix *origin **content **anothercontent">
  <joint name="caster_${suffix}_joint">
    <axis xyz="0 0 1" />
  </joint>
  <link name="caster_${suffix}">
    <xacro:insert_block name="origin" />
    <xacro:insert_block name="content" />
    <xacro:insert_block name="anothercontent" />
  </link>
</xacro:macro>

宏调用

<xacro:pr2_caster suffix="front_left">
  <pose xyz="0 1 0" rpy="0 0 0" />
  <container>
    <color name="yellow"/>
    <mass>0.1</mass>
  </container>
  <another>
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
    </inertial>
  </another>
</xacro:pr2_caster>

文件包含
比如传感器一个文件，机器人本体一个文件，移动底盘一个文件，用一个文件include三个文件拼在一起

<xacro:include filename="$(find package)/other_file.xacro" />
<xacro:include filename="other_file.xacro" />
<xacro:include filename="$(cwd)/other_file.xacro" />

2. 机械臂URDF建模

以一个6轴机器人为例，下图是在rviz中加载xacro文件后的显示

6 DOF robot

颜色定义

<material name="Black">
    <color rgba="0 0 0 1"/>
</material>
<material name="White">
    <color rgba="1 1 1 1"/>
</material>
<material name="Blue">
    <color rgba="0 0 1 1"/>
</material>
<material name="Red">
    <color rgba="1 0 0 1"/>
</material>

link定义

包含可视化，惯性矩阵，碰撞检测三个部分

<link name="link0">
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="${link0_radius}" length="${link0_length}"/>
        </geometry>
        <material name="White" />
    </visual>
    <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <cylinder radius="${link0_radius}" length="${link0_length}"/>
        </geometry>
    </collision>
    <cylinder_inertial_matrix m="${link0_mass}" r="${link0_radius}" h="${link0_length}"/>
</link>

joint定义

<joint name="joint1" type="revolute">
    <parent link="link0"/>
    <child link="link1"/>
    <origin xyz="0 0 ${link0_length/2}" rpy="0 ${M_PI/2} 0" />
    <axis xyz="-1 0 0" />
    <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}"/>
</joint>

参数定义

<!-- Constants -->
<xacro:property name="M_PI" value="3.14159"/>

<!-- link1 properties -->
<xacro:property name="link0_radius" value="0.05" />
<xacro:property name="link0_length" value="0.04" />
<xacro:property name="link0_mass" value="1" />

下面是在rviz中可视化该模型的launch文件，

<launch>
    <arg name="model" />
    <!-- 加载机器人模型参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find marm_description)/urdf/marm.xacro" />

    <!-- 设置GUI参数，显示关节控制插件 -->
    <param name="use_gui" value="true"/>

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    
    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

    <!-- 运行rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>

3. 从SolidWorks三维模型导出URDF

真实的机械臂不会是这样的圆柱、方块组成的简单模型，真实场景是购买到厂商机械臂本体后，对方给你一个3D模型文件，如.stp文件，需要solidworks转成urdf导入ROS中使用。

步骤如下：

1. 在solidworks里加载模型（以UR3的STP模型文件为例）

   
   导入3D模型

2. 安装插件sw2urdfSetup做配置，link的部分，joint关系
   插件下载地址
   a) 插入基准轴，如图共6个axis
   

   插入旋转轴
   b) 工具--->File--->Export as URDF
   依次添加7个link（多出来的一个是基座base_link）和6个joint，如图：
   添加joint和link
   添加完成后，点击preview and export，在弹出来的窗口中，依次配置joint限位参数，当然留到URDF文件中手动添加tag也可以。
   配置joint限位
   最后next，finish，选择功能包保存路径，这个功能包将会在ROS中使用。

导出的功能包
c) 修复一些小问题
直接编译功能包会出现一些错误，还需要修改一些小的问题：
（1）email地址，如下图的第10行，需改为xxx@xxx.xxx的形式 修改email地址

（2）修改display.launch文件中的urdf文件路径

修改urdf文件路径

3. 测试模型显示
   得到正确的模型功能包后，在rviz中测试一下模型是否正确显示以及各个关节运动是否正常。

i5@i5-ThinkPad-T460p:~/ws_moveit$ catkin_make
i5@i5-ThinkPad-T460p:~/ws_moveit$ roslaunch ur3 display.launch

打开rviz后，将Fixed frame更改为base_link，点击Add，添加RobotModel

rviz