文章轉自 https://blog.csdn.net/sunbibei/article/details/52297524 特此鳴謝原創作者的辛勤付出
1 URDF 文件
1.1 link和joint
圖中機器人的描述方式
<robot name="test_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>
以上描述只能夠描述機器人的大致樣子，不能夠確定link的位置關系和具體形狀。
1.2 位置
在定義好了機器人的骨架后, 進一步我們可以使用origin子標簽進行定義link所應該在的位置. 但是有一點應該注意到, link和link之間是使用joint進行連接, 那么link的位置, 就由連接他的joint確定. 所以, 該子標簽是定義在joint內. 在三維空間中, 要精確描述一個剛性體的姿態, 僅僅使用他的xyz坐標是不夠的, 還需要使用rpy. rpy角是描述船舶在海中航行時姿態的一種方法. 將船的行駛方向取為z軸, 繞z軸旋轉稱為滾動(Roll), 繞y軸旋轉稱為俯仰(Pitch), 繞x軸旋轉稱為偏轉(Yaw). 這種描述方式大量運用於各個領域. 依稀記得, kinect2關於臉部模型匹配的DEMO程序里面, 對臉部的描述就用到了這種描述方式來描述姿態. 在機器人中, 當然運用就更多了. 現在對之前的內容進行擴充. 其中rpy代表的是角度, 用弧度表示.
<robot name="test_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.7" rpy="0 0 0" /> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> <origin xyz="-.2 .5 -0.3" rpy="0 0 1.57" /> </joint> <joint name="joint3" type="continuous"> <parent link="link3"/> <child link="link4"/> <origin xyz=".5 0 0.2" rpy="0 0 -1.57" /> </joint> </robot>
上述位置關系定義如下圖所示. xyz就如同一個平移向量, 將下一個link的原點坐標移動到下一個位置(起點是父link的原點). 同時, 你也可以多嘗試幾次rpy, 體會下一個坐標系是如何進行變換的. 雖然現在還沒有任何東西出來, 但每個link的空間位置以及姿態已經被我們所指定了.
1.3 形狀
<?xml version="1.0"?> <robot name="test_robot"> <link name="link1"> <visual> <geometry> <cylinder length="0.6" radius="0.2" /> </geometry> </visual> </link> <link name="link2"> <visual> <geometry> <box size="0.6 0.2 .1" /> </geometry> </visual> </link> <link name="link3" > <visual> <geometry> <sphere radius="0.2"/> </geometry> </visual> </link> <link name="link4" > <visual> <origin rpy="0 -1.57 0" xyz="0 0 0"/> <geometry> <mesh filename="package://urdf_csdn/urdf/mesh/knife.stl"/> </geometry> </visual> </link> <joint name="joint1" type="fixed"> <parent link="link1"/> <child link="link2"/> <origin xyz="0.22 0 0.6" rpy="0 1.57 0" /> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> <origin xyz="-0.1 0.5 0" /> </joint> <joint name="joint3" type="fixed"> <parent link="link3"/> <child link="link4"/> <origin xyz=".5 0 0" rpy="0 0 -1.57" /> </joint> </robot>
1.4 Collision和joint limit
當然, 每個link一般是不會產生重合的, 在運動規划的時候, 也會去避免碰撞到自己, 所以針對於每一個link, 還有一個collision標簽, 和visual標簽內容完全一樣.
前面內容可以看到, 每個link可以看作是一個剛體, 剛體和剛體之間是通過joint進行連接, 那么, 問題就來了. 這個joint是固定的? 還是可以任意的動? 如果可以動, 那么, 問題又來了, 極限位置是多少? 等等等等…
比如, 我們限定joint2只能沿着y軸旋轉, 則需要添加 <axis xyz="0 1 0"/> , 類似的, 可以指定其他關節的轉動軸, 例如 <axis xyz="-0.2 0.1 1"/> .
比如, 我們要限定joint2的移動范圍, 則需要添加<limit lower="-3.14" upper="3.14" effort="150.0" velocity="3.15"/>, 從標簽中可以看到, 上下限以及速度, 力矩等都是可以指定的.
<?xml version="1.0"?> <robot name="test_robot"> <link name="link1"> <visual> <geometry> <cylinder length="0.6" radius="0.2" /> </geometry> </visual> <collision> <origin xyz="0.0 0 0.0" rpy="0 0 0" /> <geometry> <cylinder length="0.6" radius="0.2" /> </geometry> </collision> </link> <link name="link2"> <visual> <geometry> <box size="0.6 0.2 .1" /> </geometry> </visual> </link> <link name="link3" > <visual> <geometry> <sphere radius="0.2"/> </geometry> </visual> </link> <link name="link4" > <visual> <origin rpy="0 -1.57 0" xyz="0 0 0"/> <geometry> <mesh filename="package://urdf_csdn/urdf/mesh/knife.stl"/> </geometry> </visual> </link> <joint name="joint1" type="fixed"> <parent link="link1"/> <child link="link2"/> <origin xyz="0.22 0 0.6" rpy="0 1.57 0" /> </joint> <joint name="joint2" type="continuous"> <parent link="link1"/> <child link="link3"/> <origin xyz="-0.1 0.5 0" /> <axis xyz="0 1 0"/> <limit lower="-3.14" upper="3.14" effort="150.0" velocity="3.15"/> </joint> <joint name="joint3" type="fixed"> <parent link="link3"/> <child link="link4"/> <origin xyz=".5 0 0" rpy="0 0 -1.57" /> </joint> </robot>
1.5 可視化
查看urdf文件, 可以使用urdf_tutorial包, 命令格式roslaunch urdf_tutorial display.launch model:=path/to/your/xxx.urdf, 會使用Rviz進行顯示, 如上述內容, 進行顯示之后, 可以得到下圖所示內容. 坐標系圖示, 需要添加TF條目進行顯示. 值得注意的地方, 將Global Options中的fixed frame設定為link1. 如果你設定了可移動的關節, 想查看以下關節移動的效果以及設定等, 使用roslaunch urdf_tutorial display.launch model:=path/to/your/xxx.urdf gui:=true, 你會看到彈出一個控制面板(如果沒找到, 再好好找一找, 或許真的很小).
通過上述方式, 在rviz中正確顯示模型之后, 新打開一個命令行, 輸入rostopic list命令, 可以查看到類似如下的輸出. 可以看到, 開啟了/joint_states話題(Topic), 使用rostopic echo /joint_states可以看到話題數據
2 XACRO文件
前面也提到了, XACRO文件和URDF實質上是等價的. XACRO格式提供了一些更高級的方式來組織編輯機器人描述. 主要提供了三種方式來使得整個描述文件變得簡單. 借用在教程中一句話來形容xacro的優勢: “Fortunately, you can use the xacro package to make your life simpler”.
2.1 Constants
Usage: <xacro:property name="WIDTH" value="2.0"/>
類似於C語言中的宏定義, 在頭部定義, 如 <xacro:property name="WIDTH" value="2.0"/> , 以 ${WIDTH} 的方式進行使用. 經常會看到的一個常量定義, <property name="PI" value="3.14159265" /> . 還有定義一個前綴, 這樣后面關節名都可以方便的進行修改. 比如 <property name="prefix" value="my_"/> , 后面關節名字就可以類似的進行更新. <joint name="${prefix}joint1" type="revolute"/>.
在有了上面的常量定義之后, 類似於宏定義, 完成字符串替換, 同時還可以進行一些簡單的數學運算.
Usage: ${1/2} , ${PI*(WIDTH*0.5)}
2.2 Macros
這個才是xacro文件中最重要的部分. 就像宏函數一樣, 完成一些最小模塊的定義, 方便重用, 以及可以使用參數來標識不同的部分.
2.2.1 Simple Macro
Usage
<xacro:macro name="default_origin"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:macro> <xacro:default_origin />
前面三行對宏進行定義, 第四行是使用.
2.2.2 Paramiterized Macro
Usage
<xacro:macro name="default_link" params="prefix"> <link name="${prefix}_link1" /> </xacro:macro> <xacro:default_link prefix="my" />
類似, 前三行定義, 第四行是進行使用. 當然, 不單由這樣簡單的參數, 還可以使用塊參數.
Usage
<xacro:macro name="default_link" params="prefix *origin"> <link name="${prefix}_link1" > <xacro:insert_block name="prigin" /> </link> </xacro:macro> <xacro:default_link prefix="my"> <origin xyz="0 0 0" rpy="0 0 0" /> </xacro:default_link>
一般情況下, 很多已有的機器人模型, 都是以xacro格式提供描述, 而在xacro文件中, 整個機器人定義為一個很大的宏. 例如, barrett hand, 想進一步了解的朋友可以點擊前面的鏈接, 查看以下barrett hand是如何進行描述的
2.2.3 Include
很多模型都是已宏的形式進行定義, 並以最小集團分成很多個文件. 而最終的機器人描述就變得非常簡單了. 下面摘錄一個ur5的描述文件. 從中可以看出來xacro的強大優勢. 在最后的示例中我們還能夠看到, urdf文件也是能夠直接導入進來的.
<?xml version="1.0"?> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur5" > <!-- common stuff --> <xacro:include filename="$(find ur_description)/urdf/ur5/common.gazebo.xacro" /> <!-- ur5 --> <xacro:include filename="$(find ur_description)/urdf/ur5/ur5.urdf.xacro" /> <!-- arm --> <xacro:ur5_robot prefix="" joint_limited="false"/> <link name="world" /> <joint name="world_joint" type="fixed"> <parent link="world" /> <child link = "base_link" /> <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> </joint> </robot>
當然, 此時的簡單是建立在之前的復雜的基礎上的. 從上述內容中可以看到, 首先是在ur_description包中找到另外幾個xacro文件, 將其包含進來. 當然應該注意到, include類似於C語言中的include, 先將該文件擴展到包含的位置. 但包含進來的文件很有可能只是一個參數宏的定義. 並沒有被調用. 所以, 示例中調用了一個宏(<xacro:ur5_robot prefix="" joint_limited="false"/>), 產生一個ur5機器人
2.3 可視化
urdf_tutorial包也是可以查看xacro文件的. 使用
roslaunch urdf_tutorial xacrodisplay.launch model:=path/to/your/xxx.urdf.xacro.
3 關於urdf_tutorial
前面提到的可視化都是使用urdf_tutorial包進行的. 分別調用了兩個launch文件. 在上面的示例中我們還看到了不但會使用rviz進行可視化, 還會發起一些話題等. 其實這些我們能夠從他的launch文件中一窺究竟.
打開命令行, 輸入: rosed urdf_tutorial dispaly.launch. 會使用vim打開該文件. 可以看到下述內容
<launch> <arg name="model" /> <arg name="gui" default="False" /> <param name="robot_description" textfile="$(arg model)" /> <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>
由上可以看到, 參數model是沒有默認值的, 所以調用該launch文件必須指定model參數. 其他都比較易懂, 主要解釋以下robot_description, 可以看到, 其前面是param. 這個是指定ros 參數服務器中的參數值. 而打開rviz之后, rviz就是直接從參數服務器中讀取機器人描述文件, 也就是這個參數. 然后進行顯示. use_gui也是如此對顯示產生的影響. 另外, 還發起了兩個發布者節點, 分別發布joint_states和robot_state. 這也就是會由joint_states話題的原因. xacrodispaly.launch文件和上面類似, 但在處理文件時, 使用的是: <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" /> . 在launch中將xacro文件解析為urdf.
至於其中啟動的兩個節點, joint_state_publisher 和 robot_state_publisher, 可以查看ROS Answer上相關的解釋. 另外, 在robot_state_publisher概述中提到, robot_state_publisher從 /joint_states 話題中獲取機器人joint角度作為輸入, 使用機器人的運動學樹模型計算出機器人link的3D姿態, 然后將其發布到話題 /tf 和 /tf_static . joint_state_publisher從ROS參數服務器中讀取robot_description參數, 找到所有non-fixed joint, 發布他們的 JointState 消息到 /joint_states 話題