ROS中urdf文件与xacro文件的编写

urdf文件与xacro文件的编写

一、程序包及文件系统

创建工作空间 创建包等不再赘述
/工作空间/src/包 内文件结构如图

二、urdf的编写

1、.urdf文件的写法

1.1、机器人urdf总体框架：

<robot name="pr2">
<!-- 机器人 links  joints 等内容 -->
</robot>

机器人内容包含以下几个元素

<link>
定义杆件
<joint>
定义关节
<transmission>
定义变速模块（不常用到）
<gazebo>
用于扩展Gazebo仿真

其实我们最常用的就是link和joint两个标签，仿真会用到gazebo标签，transmission标签目前我也没接触到，以后补。

1.2、link标签

其实link标签只定义杆件的几何参数、惯量、坐标系零点等，不涉及父坐标系和子坐标系等问题，这些坐标关系的定义都在joint标签内进行。
link必须声明自己的名字 这个是必选属性。
link一图搞清结构

参考：http://wiki.ros.org/urdf/XML/link.

例子1：

 <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>

例子2：

  <link name="torso"><!--link名字必须声明-->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://robot_description/meshes/base_link.DAE"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.065 0 0.0"/>
      <geometry>
        <mesh filename="package://robot_description/meshes/base_link_simple.DAE"/>
      </geometry>
    </collision>
    <collision_checking>
      <origin rpy="0 0 0" xyz="-0.065 0 0.0"/>
      <geometry>
        <cylinder length="0.7" radius="0.27"/>
      </geometry>
    </collision_checking>
    <inertial>
      ...
    </inertial>
  </link>  

1.3、joint标签

joint一图搞清结构

type详细解释：
指定关节类型，其中type可以是下列之一：
revolute-沿轴旋转并具有由上限和下限指定的有限范围的铰链接头。
continuous-绕轴旋转的连续铰链接头，没有上限和下限。（正儿八经的旋转关节）
prismatic-沿轴滑动的滑动关节，并具有由上限和下限指定的有限范围。
fixed-这实际上不是关节，因为它无法移动。所有自由度均被锁定。这种类型的关节不需要轴，校准，动力学，极限或安全控制器。
floating-该关节允许在所有6个自由度上运动。
planar-此关节允许在垂直于轴的平面内运动。

limit标签各个参数的计算方法参考：http://wiki.ros.org/pr2_controller_manager/safety_limits.

参考： http://wiki.ros.org/urdf/XML/joint.
例子：

 <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>

1.4、transmission标签

待补充
参考：http://wiki.ros.org/urdf/XML/Transmission.

1.5gazebo标签

待补充
参考：http://gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros.

2、个人案例：

<robot  name="robot_urdf">
  <link    name="base_link">
    <visual>
        <origin xyz = "0 0 0" rpy = "0 0 0 "/>
        <geometry>
            <box size ="0.5 0.3 0.2"/>
        </geometry>
        <material name = "yellow">
            <color rgba = "1 0.4 0 1"/>
        </material>
    </visual>
</link>

    <joint name = "left_wheel_joint" type = "continuous">
        <origin xyz = "-0.2 -0.15 -0.05" rpy = "0 0 0"/>
        <parent link = "base_link"/>
        <child link = "left_wheel_link"/>
        <axis xyz = "0 1 0"/>
    </joint>

    <link name = "left_wheel_link">
        <visual>
            <origin xyz = "0 0 0" rpy = "1.5707 0 0"/>
            <geometry>
                <cylinder radius = "0.075" length = "0.08"/>
            </geometry>
            <material name = "gray">
                <color rgba="0.75 0.75 0.75 1"/>
            </material>
        </visual>
    </link>

    <joint name = "right_wheel_joint" type = "continuous">
        <origin xyz = "-0.2 0.15 -0.05" rpy = "0 0 0"/>
        <parent link = "base_link"/>
        <child link = "right_wheel_link"/>
        <axis xyz = "0 1 0"/>
    </joint>

    <link name = "right_wheel_link">
        <visual>
            <origin xyz = "0 0 0" rpy = "1.5707 0 0"/>
            <geometry>
                <cylinder radius = "0.075" length = "0.08"/>
            </geometry>
            <material name = "gray">
                <color rgba="0.75 0.75 0.75 1"/>
            </material>
        </visual>
    </link>


    <joint name = "front_castor_joint" type = "fixed">
        <origin xyz = "0.2 0 -0.05 " rpy = "0 0 0 "/>
        <parent link = "base_link"/>
        <child link = "front_castor_link"/>
        <axis xyz = "0 1 0"/>
    </joint>

    <link name = "front_castor_link">
        <visual>
            <origin xyz = "0 0 0" rpy ="1.5707 0 0"/>
            <geometry>
                <sphere radius = "0.075"/>
            </geometry>
            <material name = "black">
                <color rgba = "0 0 0 0.95"/>
            </material>
        </visual>
    </link>

</robot>
    

上边的URDF模型定义了一个小车，和两个驱动轮一个万向轮相连

3、urdf文件检查方法

ROS为用户提供了一个检查URDF语法的工具：
未安装则使用如下命令安装：

sudo apt-get install liburdfdom-tools

安装完毕后，执行检查，二选一：

check_urdf mbot.urdf
urdf_to_graphiz mbot.urdf 

4、urdf文件的.launch文件编写

<launch>
     <!--加载机器人模型-->
    <param name = "robot_description" textfile = "$(find robot_description)/urdf/robot_urdf.urdf"/>
    <!-- 设置GUI参数，显示关节控制插件 -->
    <param name = "use_gui" value = "true"/>
	<!-- 运行joint_state_publisher_gui节点，发布机器人的关节状态  -->
    <node name ="joint_state_publisher" pkg = "joint_state_publisher" type = "joint_state_publisher"/>
    <!-- 运行robot_state_publisher节点，发布tf,将机器人的连杆关节之间的关系，通过TF的形式，整理成三维姿态发布出去  -->
    <node name = "robot_state_publisher" pkg = "robot_state_publisher" type = "state_publisher"/>
 	<!-- 运行rviz  -->
    <node name = "rviz" pkg = "rviz" type = "rviz" args = "-d $(find robot_description)/config/robot_urdf.rviz" required = "true"/>
</launch>

三、xacro的编写

1、.xacro文件写法

xacro按照我的学习理解就是解决代码可读性和代码复用问题的
urdf写多了你会感觉到好难改，因为不像c++那样有变量和循环，很多变量你需要一个个找一个个改，而xacro中不仅有变量，还可以将一个零件写的模块化。修改一个模型如果不调整整体结构，只改变大小，可以仅通过修改变量实现。
这种标签类语言我本人之前也是从未接触过的，如果有说的不对的地方，各位可以指出来。
xacro的核心在于宏定义
核心代码段就是下面的

<xacro:macro name="pr2_arm" params="suffix parent reflect">
  <pr2_upperarm suffix="${suffix}" reflect="${reflect}" parent="${parent}" />
  <pr2_forearm suffix="${suffix}" reflect="${reflect}" parent="elbow_flex_${suffix}" />
</xacro:macro>

<xacro:pr2_arm suffix="left" reflect="1" parent="torso" />
<xacro:pr2_arm suffix="right" reflect="-1" parent="torso" />

其实整个宏定义就是以

<xacro:macro name="pr2_arm" params="suffix parent reflect">

开头，
以

</xacro:macro>

结束
这表示整个其中的内容都是可以复用的
name参数就是起个函数名，params就是此段复用代码的输入参数
比如params =“suffix parent reflect”,就是使用了三个参数（输入的变量）
后面的<xacro:pr2_arm suffix=“left” reflect=“1” parent=“torso” />
在我理解就是调用本段宏（我个人理解的函数）,输入参数（变量）为 suffix=“left” reflect=“1” parent=“torso”

<xacro:pr2_arm suffix="left" reflect="1" parent="torso" />
<xacro:pr2_arm suffix="right" reflect="-1" parent="torso" />

即为调用了两次这个宏（函数），也就是将两个机械臂（结构）实现了。
除此之外，这个宏（函数）也可以无参数，如下

<xacro:macro name="robot_base">
        <link name="base_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="${base_length} ${base_width} ${base_height}" />
                </geometry>
                <material name="yellow" />
            </visual>
        </link>

        <wheel prefix="left" reflect="-1"/>
        <wheel prefix="right" reflect="1"/>
    </xacro:macro>

这就表示了一个小车底盘加上了左右两个轮子。
但是这些不复用的宏都要写到一起，在这一段中将整个base_link和左右轮都放到了一起，如果将车的base_link和其他零件分割到两个宏中定义，就会查找不到base_link或者其他link，因为他们分成了两个宏，没有连接到一起。这点一定要注意！！！将表示base_link与各个零件相互连接关系的代码段放到一个宏内。（都一起放到</xacro:macro>这对标签外面，我测试了也ok，目前不知道这样做会带来什么后果，不推荐）

<xacro:macro name="robot_base">
<!--中间是对base的定义和对各个link、joint组成的宏进行调用-->
</xacro:macro>

个人案例：

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

    <!-- PROPERTY LIST -->
    <xacro:property name="M_PI" value="3.1415926"/>
    <xacro:property name="base_length" value="0.5"/>
    <xacro:property name="base_width" value="0.3"/>
    <xacro:property name="base_height" value="0.2"/>

    <xacro:property name="wheel_radius" value="0.075"/>
    <xacro:property name="wheel_length" value="0.08"/>
    <xacro:property name="wheel_joint_y" value="-0.15"/>
    <xacro:property name="wheel_joint_z" value="-0.05"/>
    <xacro:property name="wheel_joint_x" value="-0.15"/>

    <xacro:property name="caster_radius" value="0.075"/> 
    <xacro:property name="caster_joint_x" value="0.18"/>
    <xacro:property name="caster_joint_z" value="-0.05"/>

    <!-- Defining the colors used in this robot -->
    <material name="yellow">
        <color rgba="1 0.4 0 1"/>
    </material>
    <material name="black">
        <color rgba="0 0 0 0.95"/>
    </material>
    <material name="gray">
        <color rgba="0.75 0.75 0.75 1"/>
    </material>
    
    <!-- Macro for robot wheel -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="${prefix}_wheel_joint" type="continuous">
            <origin xyz="${wheel_joint_x} ${reflect*wheel_joint_y} ${wheel_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="${prefix}_wheel_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="${prefix}_wheel_link">
            <visual>
                <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
                <geometry>
                    <cylinder radius="${wheel_radius}" length = "${wheel_length}"/>
                </geometry>
                <material name="gray" />
            </visual>
        </link>
    </xacro:macro>

    <!-- Macro for robot base -->
    <xacro:macro name="mbot_base">
        <link name="base_footprint">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>

        <joint name="base_footprint_joint" type="fixed">
            <origin xyz="0 0 ${base_height/2 + caster_joint_z +caster_radius}" rpy="0 0 0" />        
            <parent link="base_footprint"/>
            <child link="base_link" />
        </joint>

        <link name="base_link">
            <visual>
                <origin xyz=" 0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="${base_length} ${base_width} ${base_height}" />
                </geometry>
                <material name="yellow" />
            </visual>
        </link>

        <joint name="caster_joint" type="continuous">
            <origin xyz="${caster_joint_x} 0 ${caster_joint_z}" rpy="0 0 0"/>
            <parent link="base_link"/>
            <child link="caster_link"/>
            <axis xyz="0 1 0"/>
        </joint>

        <link name="caster_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0"/>
                <geometry>
                    <sphere radius="${caster_radius}" />
                </geometry>
                <material name="black" />
            </visual>
        </link>

        <wheel prefix="left" reflect="-1"/>
        <wheel prefix="right" reflect="1"/>


    </xacro:macro>
</robot>

这个文件和之前的urdf一样，也是一个base_link 左右连了两个车轮，前边一个万向轮。

2、xacro文件的调用

你的小车底盘已经都写好了，现在如果有一个雷达、一个摄像头要家装上去怎么办呢，又重写刚才的.xacro吗？不慌，他又提供了类似c的include调用。
如果小车和雷达你都有了只想装配起来使用，那就使用这样的标签进行调用，以下方法三选一：

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

文件“ other_file.xacro”将被xacro包含并扩展

个人案例：

<?xml version="1.0"?>
<robot name="robot_my" xmlns:xacro="http://www.ros.org/wiki/xacro"><!-- 声明是xacro文件 -->
    <xacro:include filename="$(find robot_description)/urdf/xacro/robot_xacro.xacro"/> <!-- 包含文件 -->
    <robot_xacro/>   <!-- 调用宏定义 -->
</robot>

注：ros noetic以上版本使用 <xacro：robot_xacro/>调用宏定义，其他版本使用 <robot_xacro/> 调用宏定义
参考：http://wiki.ros.org/xacro/.

3、xacro文件的检查方法

xacro没有一个很好的可视化检查方法，目前想检查职能转换为.urdf文件再检查，命令如下：

rosrun xacro xacro --inorder mbot.xacro> new_mbot_from_xacro.urdf

即为：

rosrun xacro xacro --inorder 原有xacro文件.xacro> 转格式后的urdf文件.urdf

之后执行urdf检查，和上面的urdf的检查方法一致，二选一：

check_urdf new_mbot_from_xacro.urdf
urdf_to_graphiz new_mbot_from_xacro.urdf 

注：在做检查步骤之前一定要搞好环境变量
source devel/setup.bash 之后cd到你的xacro文件所在的目录下

4、xacro文件的.launch文件写法

xacro模型的launch文件的写法其实和urdf的launch文件差不多。只有加载机器人代码段不同，如下：

<launch>
	<!-- 加载机器人（小车）xacro文件 -->
	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot.xacro'" />
	<arg name="gui" default="true" />

	<!-- 设置GUI参数，显示关节控制插件 -->
	<param name="robot_description" command="$(arg model)" />
	<param name="use_gui" value="$(arg gui)"/>

    <!-- 运行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="robot_state_publisher" />

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

</launch>

5、执行.launch文件后查看效果

roslaunch mbot_description display_mbot_base_xacro.launch

urdf的显示方法同理 roslaunch 你的urdf模型对应的.launch文件
选择正确的坐标系，我这里选择base_footprint
添加机器人模型显示
点击add 选择robotmodel

如果想查看tf坐标关系就在add打开的窗口中选择TF添加，此处不再赘述。