A expressão URDF é um acrónimo de Unified Robot Description Format, ou seja, em português um Formato Unificado para Descrição de Robot.
Os ficheiros URDF são expressos em formato XLM, e são compostos fundamentalmente por etiquetas Link, que descrevem os seus componentes mecanicos e etiquetas Joint, que descrevem como os diversos Links se relacionam.
Informações básicas sobre o URDF no wiki do ROS
- http://wiki.ros.org/urdf
- http://wiki.ros.org/urdf/XML
- http://wiki.ros.org/urdf/Tutorials
Preparar pacote do ROS
Antes de avançar na exposição da estrutura e formato dos ficheiros URDF devemos preparar uma pacote do ROS para conter os ficheiros que vamos usar.
cd ~/catkin_ws/src catkin_create_pkg description_tutorial std_msgs rospy roscpp rviz controller_manager gazebo_ros joint_state_publisher robot_state_publisher rosdep install -y --from-paths . --ignore-src --rosdistro kinetic cd description_tutorial/ mkdir launch mkdir urdf cd ~/catkin_ws/ catkin_make
nano launch/display1.launch
<launch> <arg name="model" default="robot1.urdf" /> <arg name="gui" default="True" /> <param name="robot_description" textfile="$(find description_tutorial)/urdf/$(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="robot_state_publisher" /> <node name="rviz" pkg="rviz" type="rviz" args="-d $(find description_tutorial)/robot1.rviz" required="true" /> </launch>
Para executar a visualização de um robot descrito num ficheiro urdf, usar o seguinte comando:
roslaunch description_tutorial display1.launch model:=urdf/robot1.urdf
Anatomia de um ficheiro URDF
Um ficheiro URFD é composto por etiquetas no formato tipico do XML, seguindo uma hierarquia especifica.
- robot
- link name=”link_1″
- inertial
- origin xyz=”0 0 0″ rpy=”0 0 0″
- mass value=”1″
- inertia ixx=”100″ ixy=”0″ ixz=”0″ iyy=”100″ iyz=”0″ izz=”100″
- visual
- origin xyz=”0 0 0″ rpy=”0 0 0″
- geometry
- [box, cylinder, sphere, mesh]
- material
- color rgba=”1 0 0 1.0″
- texture
- collision
- origin xyz=”0 0 0″ rpy=”0 0 0″
- geometry
- [box, cylinder, sphere]
- inertial
- joint name=”joint_1″ type=”floating”
- parent link=”link_1″
- child link=”link_2″
- origin xyz=”0 0 0″ rpy=”0 0 0″
- axis xyz=”0 0 1″
- calibration rising =”0.0″ falling=”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″
- link name=”link_1″
Passemos a exemplos de ficheiros URDF onde se faz uso das etiquetas indicadas acima.
Tomemos como objecto de exemplo o braço róbotico eezyBotArm MK1, que por ter uma cadeia cinamática em malha fechada, iremos usar a cadeia cinemática equivalente em malha aberta.
Tamanhos em milimeteros
AC = 32
AB = 70
CQ = 35
DE = 34
DF = 34
FG = 17
GH = 80
IJ = 1
JK = 95
NO = 1
OP = 127
LH = 79
HM = 35
Comecemos pela base, que será representada por um cilindro que na figura acima está definido pelos A, B, C e está fixo ao mundo de forma imutavel.
nano ~/catkin_ws/src/description_tutorial/urdf/robot1.urdf
<?xml version="1.0"?> <robot name="robot1"> <link name="base_link"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <cylinder length="0.032" radius="0.035"/> </geometry> <material name="blue"/> </visual> </link> <material name="blue"> <color rgba="0 0 1 1"/> </material> </robot>
Como se pode ver na imagem o cilindro é representado afundado até meia altura no plano de representação. Isto acontece porque nos objectos do tipo: box, cylinder, e sphere, a origem é no centro do objecto.
Para dispormos adequadamente o objecto teremos de indicar o deslocamento da origem. Para o efeito colocamos o valor z da origem do visual do link em 0.016.
nano ~/catkin_ws/src/description_tutorial/urdf/robot1.urdf
<?xml version="1.0"?> <robot name="robot1"> <link name="base_link"> <visual> <origin xyz="0 0 0.016" rpy="0 0 0" /> <geometry> <cylinder length="0.032" radius="0.035"/> </geometry> <material name="blue"/> </visual> </link> <material name="blue"> <color rgba="0 0 1 1"/> </material> </robot>
A seguir passemos ao segundo objecto, definido pelos pontos D, E e F, é do tipo box e roda sobre o eixo z no ponto Q.
Para definirmos outro objecto (link) temos também de definir a ligação entre eles (joint).
nano ~/catkin_ws/src/description_tutorial/urdf/robot1.urdf
<?xml version="1.0"?> <robot name="robot1"> <link name="base_link"> <visual> <origin xyz="0 0 0.016" rpy="0 0 0" /> <geometry> <cylinder length="0.032" radius="0.035"/> </geometry> <material name="blue"/> </visual> </link> <joint name="joint_1" type="revolute"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="base_link" /> <child link="link_1" /> <axis xyz="0 0 1" /> <limit effort="0" velocity="0" lower="-0.7854" upper="0.7854"/> <!-- [-45,45]--> </joint> <link name="link_1"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <box size="0.01 0.034 0.034" /> </geometry> <material name="blue" /> </visual> </link> <material name="blue"> <color rgba="0 0 1 1"/> </material> </robot>
Pelas imagens podemos verificar que a posicão do objecto não é adequada. O seu centro fica sobre a origem do link anterior. Para resolver a situação deslocamos a origem do joint para a soma da altura da base, mas metade da altura do novo objecto.
<joint name="joint_1" type="revolute"> <origin xyz="0 0 0.049" rpy="0 0 0" /> <parent link="base_link" /> <child link="link_1" /> <axis xyz="0 0 1" /> <limit effort="0" velocity="0" lower="-0.7854" upper="0.7854"/> <!-- [-45,45]--> </joint>
Após esta correção a apresentação fica conforme desejado.
A seguir passemos ao terceiro objecto, a haste vertical definida pelos pontos I, J e K, do tipo box e roda sobre o eixo x no ponto G.
Mais uma vez, para definirmos outro objecto (link) temos também de definir a ligação entre eles (joint). Aproveitamos também para redefinir a cor da box do link_1, para vermelho.
nano ~/catkin_ws/src/description_tutorial/urdf/robot1.urdf
<?xml version="1.0"?> <robot name="robot1"> <link name="base_link"> <visual> <origin xyz="0 0 0.016" rpy="0 0 0" /> <geometry> <cylinder length="0.032" radius="0.035"/> </geometry> <material name="blue"/> </visual> </link> <joint name="joint_1" type="revolute"> <origin xyz="0 0 0.049" rpy="0 0 0" /> <parent link="base_link" /> <child link="link_1" /> <axis xyz="0 0 1" /> <limit effort="0" velocity="0" lower="-0.7854" upper="0.7854"/> <!-- [-45,45]--> </joint> <link name="link_1"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <box size="0.01 0.034 0.034" /> </geometry> <material name="red" /> </visual> </link> <joint name="joint_2" type="revolute"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="link_1" /> <child link="link_2" /> <axis xyz="1 0 0" /> <limit effort="0" velocity="0" lower="-0.6109" upper="0.9599"/> <!-- [-35,55]--> </joint> <link name="link_2"> <visual> <origin xyz="0 0 0" rpy="0 0 0" /> <geometry> <box size="0.01 0.01 0.08" /> </geometry> <material name="blue" /> </visual> </link> <material name="red"> <color rgba="1 0 1 1"/> </material> <material name="blue"> <color rgba="0 0 1 1"/> </material> </robot>
Mais uma vez verificamos que a posição do objecto não é adequada.
As duas razões são, a posição do joint G no link_1 e no link_1 estão no centro dos respectivos objctos, e portanto teremos que redefinir, a origem do joint pelo deslocamento na vertical de metade da altura (0.017) do suporte (link_1), e a origem do visual pelo deslocamento na vertical de metade da altura (0.04) da haste (link_2).
<joint name="joint_2" type="revolute"> <origin xyz="0 0 0.017" rpy="0 0 0" /> <parent link="link_1" /> <child link="link_2" /> <axis xyz="1 0 0" /> <limit effort="0" velocity="0" lower="-0.6109" upper="0.9599"/> <!-- [-35,55]--> </joint> <link name="link_2"> <visual> <origin xyz="0 0 0.04" rpy="0 0 0" /> <geometry> <box size="0.01 0.01 0.08" /> </geometry> <material name="blue" /> </visual> </link>
Por último passemos ao quarto objecto, a haste horizontal definida pelos pontos P, O e N, do tipo box e roda sobre o eixo x no ponto H.
Mais uma vez, para definirmos outro objecto (link) temos também de definir a ligação entre eles (joint).
Chamo a atenção que desta vez foram efectuados acertos logo de inicio. No joint_3 a origem z foi deslocada 0.08 unidades, e a origem ydo visual do link_3 foi deslocada 0.0225 unidades.
nano ~/catkin_ws/src/description_tutorial/urdf/robot1.urdf
<?xml version=”1.0″?>
<robot name=”robot1″>
<link name=”base_link”>
<visual>
<origin xyz=”0 0 0.016″ rpy=”0 0 0″ />
<geometry>
<cylinder length=”0.032″ radius=”0.035″/>
</geometry>
<material name=”blue”/>
</visual>
</link>
<joint name=”joint_1″ type=”revolute”>
<origin xyz=”0 0 0.049″ rpy=”0 0 0″ />
<parent link=”base_link” />
<child link=”link_1″ />
<axis xyz=”0 0 1″ />
<limit
effort=”0″
velocity=”0″
lower=”-0.7854″ upper=”0.7854″/>
<!– [-45,45]–>
</joint>
<link name=”link_1″>
<visual>
<origin xyz=”0 0 0″ rpy=”0 0 0″ />
<geometry>
<box size=”0.01 0.034 0.034″ />
</geometry>
<material name=”red” />
</visual>
</link>
<joint name=”joint_2″ type=”revolute”>
<origin xyz=”0 0 0.017″ rpy=”0 0 0″ />
<parent link=”link_1″ />
<child link=”link_2″ />
<axis xyz=”1 0 0″ />
<limit
effort=”0″
velocity=”0″
lower=”-0.6109″ upper=”0.9599″/>
<!– [-35,55]–>
</joint>
<link name=”link_2″>
<visual>
<origin xyz=”0 0 0.04″ rpy=”0 0 0″ />
<geometry>
<box size=”0.01 0.01 0.08″ />
</geometry>
<material name=”blue” />
</visual>
</link>
<joint name=”joint_3″ type=”revolute”>
<origin xyz=”0 0 0.08″ rpy=”0 0 0″ />
<parent link=”link_2″ />
<child link=”link_3″ />
<axis xyz=”1 0 0″ />
<limit
effort=”0″
velocity=”0″
lower=”-1.2217″ upper=”0.3491″/>
<!– [-70,20]–>
</joint>
<link name=”link_3″>
<visual>
<origin
xyz=”0 0.0225 0″
rpy=”0 0 0″ />
<geometry>
<box size=”0.0075 0.127 0.01″ />
</geometry>
<material name=”red” />
</visual>
</link>
<material name=”red”>
<color rgba=”1 0 1 1″/>
</material>
<material name=”blue”>
<color rgba=”0 0 1 1″/>
</material>
</robot>
No final, conforme video abaixo, podemos manipular a posição dos 3 joints com o joint_state_publisher_gui.