Cómo revisar los archivos URDF

Por / 9 de septiembre de 2022 / ROS2, SIMULACIÓN / , ,

Cuando nuestro modelo es complicado puede ser bastante tedioso revisar el código URDF en caso de que nos aparezca algún tipo de error o cuando el comportamiento de las articulaciones no es el esperado.

En este artículo vamos a ver algunas herramientas que tenemos a nuestra disposición y que nos van a ayudar a identificar y resolver algunos de los problemas que nos pueden surgir con los archivos URDF.

Comprobación de la sintaxis

Podemos asegurar que el código no tiene errores de sintaxis empleando las herramientas check_urdf y urdf_to_graphiz que se encuentran en el paquete debian liburdfdom-tools.

Si no tenemos ya dicho paquete ejecutaremos desde el terminal el siguiente comando para instalarlo.

sudo apt install liburdfdom-tools

En caso de que hayamos empleado el lenguaje de macros XACRO es necesario como paso previo generar el archivo con el código URDF. Para ello ejecutamos el siguiente comando, donde hay que sustituir xacro_file por el archivo con el código XACRO y urdf_file por el nombre que queramos dar al archivo de salida.

xacro xacro_file > urdf_file

Una vez que ya tengamos el archivo URDF realizaremos la comprobación de la sintaxis mediante la ejecución de la herramienta check_urdf . Si no detecta errores, nos dará una salida como la del siguiente ejemplo.

user@ubuntu:~/ros2_ws/src/mars_robot/urdf$ check_urdf model.urdf

robot name is: mars_robot
---------- Successfully Parsed XML ---------------
root Link: base_link has 5 child(ren)
    child(1):  link_camera_frame
        child(1):  link_camera_optical
    child(2):  link_caster_wheel
    child(3):  link_left_wheel
    child(4):  link_lidar
    child(5):  link_right_wheel

También podemos usar la herramienta urdf_to_graphiz que nos creará un archivo pdf con una representación de la estructura de nuestro modelo. 

urdf_to_graphiz urdf_file

Usando el mismo ejemplo el contenido del pdf generado sería el siguiente.

Verificación del comportamiento de las articulaciones

Otra cosa que podemos hacer para verificar que nuestro código URDF es correcto es ver si los movimientos de las articulaciones son correctos. Para ello utilizaremos RVIZ2 para visualizar el robot y los paquetes joint_state_publisher y robot_state_publisher, los cuales publican el estado de las articulaciones y las transformaciones entre los distintos sistemas de referencia asociados a cada uno de los enlaces respectivamente. Con esta información RVIZ2 es capaz de representar el robot.

Por comodidad, en vez de ejecutar cada paquete en una consola diferente lo que vamos a hacer es crear uno nuevo, al que llamaremos por ejemplo check_joints, que simplemente va a contener un archivo launch que nos va a permitir ejecutarlos todos a la vez y al que además pasaremos como un parámetro el nombre del archivo URDF que queremos probar.

El primer paso será pues ir a la carpeta src de nuestro espacio de trabajo y crear este paquete. Para ello ejecutaremos los siguientes comandos desde el terminal, sustituyendo por la ruta al espacio de trabajo:

cd ~/<ws_path>/src 
ros2 pkg create check_joints --build-type ament_python --dependencies rclpy

A continuación añadiremos las carpetas launch y config a nuestro paquete y en su interior crearemos los archivos check_joints.launch.py  y config.rviz:

mkdir -p check_joints/launch
mkdir -p check_joints/config
cd check_joints/launch
touch check_joints.launch.py
cd ..
cd config
touch config.rviz

Abrimos el archivo config.rviz y copiamos el siguiente código. La parte importante son las líneas donde se indica a RVIZ2 que cargue un modelo de robot cuya descripción es proporcionada por el topic robot_description:

 

Class: rviz_default_plugins/RobotModel
…
…
Visual Enabled: true

Panels:
  - Class: rviz_common/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /RobotModel1
      Splitter Ratio: 0.5
    Tree Height: 336
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /2D Goal Pose1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz_common/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: ""
  - Class: rviz_visual_tools/RvizVisualToolsGui
    Name: RvizVisualToolsGui
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz_default_plugins/RobotModel
      Collision Enabled: true
      Description File: ""
      Description Source: Topic
      Description Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: robot_description
      Enabled: true
      Name: RobotModel
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/Interact
      Hide Inactive Objects: true
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Covariance x: 0.25
      Covariance y: 0.25
      Covariance yaw: 0.06853891909122467
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/SetGoal
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /goal_pose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 3.969904661178589
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0.6851969957351685
        Y: -0.14429132640361786
        Z: 0.42723408341407776
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.660398006439209
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 5.8485822677612305
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 704
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd00000004000000000000016400000222fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000021e000000410000004100ffffff000000010000010f00000222fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000222000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002310000022200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  RvizVisualToolsGui:
    collapsed: false
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1200
  X: 529
  Y: 161

El siguiente paso es editar el archivo check_joints.launch.py y pegar el siguiente código.  Básicamente lo que hace es leer el archivo URDF que se le pase mediante el argumento urdf_file, analizarlo para determinar cual es el enlace raíz y crear los nodos rviz2, robot_state_publisher y joint_state_publisher_gui.

Lo más destacable del mismo es que los nodos no se crean directamente dentro de la función  generate_launch_description( ) que es lo habitual en un archivo launch, sino que se hace en una nueva función setup_launch( ) que hemos creado y a la que se llama empleando OpaqueFunction. 

El motivo es que no se puede acceder al valor que se pasa a un argumento para usarlo como si fuera una variable python en la función generate_launch_description( ), pero necesitamos conocer el nombre del archivo URDF para poder abrirlo, encontrar el enlace raíz y pasarlo como argumento del nodo RVIZ2. OpaqueFunction permite llamar a esta nueva función donde podemos obtener el valor del argumento mediante LaunchConfiguration(‘urdf_file’).perform(context).

import xacro, os   
import xml.etree.ElementTree as ET

from launch.actions import OpaqueFunction
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command, LaunchConfiguration


def generate_launch_description():

    # Declara el argumento 'urdf_file' que proporciona el nombre del archivo que
    # contiene el codigo URDF de nuestro modelo. 
    urdf_file_arg = DeclareLaunchArgument(
        "urdf_file", default_value='', description='Name of URDF file.'
    )
 
    return LaunchDescription([
        urdf_file_arg,
        OpaqueFunction(function=setup_launch)
    ])

def setup_launch(context, *args, **kwargs):

    # Obtiene el valor del argumento 'urdf_file'.
    urdf_file = LaunchConfiguration('urdf_file').perform(context)

    # Genera una cadena de texto que contene el código URDF.
    doc = xacro.parse(open(urdf_file))
    xacro.process_doc(doc)
    urdf_code = doc.toxml()

    # Genera una estructura de arbol con la información XML del codigo URDF.
    tree = ET.ElementTree(ET.fromstring(urdf_code))
    root = tree.getroot()

    # Recorre el arbol para crear una lista con todos los enlaces presentes en el modelo URDF.
    links = []
    for item in root.iter('link'):
        links.append(item.attrib['name'])
        print(item.tag, ": ", item.attrib['name'])

    # Obtiene el enlace raiz. Para ello elimina de la lista los enlaces que aparecen como 
    # secundarios en las articulaciones.
    for item in root.iter('joint'):
        print(item.tag, ": ", item.attrib['name'])
        for sub_item in item.iter('parent'):
            print("  |--- ", sub_item.tag, ": ", sub_item.attrib['link'])
        for sub_item in item.iter('child'):
            print("  |--- ", sub_item.tag, ": ", sub_item.attrib['link'])
            links.remove(sub_item.attrib['link'])

    if len(links)>1:
        print("Error: el archivo URDF contiene más de un enlace raiz.")
        return

    root_link = links[0]
    print("root_link: ", root_link)

    # Obtiene la dirección del archivo rviz2.config que contiene la configuración para RVIZ2.
    rviz2_config = os.path.join(get_package_share_directory('check_joints'), 'config/config.rviz')

    # Crea y configura el nodo RVIZ2, el cual permite visualizar el modelo URDF a partir de
    # la información que proporciona el nodo ROBOT_STATE_PUBLISHER. 
    node_rviz2 = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz2_config, '-f', root_link],
        output={'both': 'log'},
    )

    # Crea y configura el nodo JOINT_STATE_PUBLISHER_GUI, el cual permite simular el estado de
    # las articulaciones.
    node_joint_state_publisher_gui = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        arguments=['--ros-args', '--log-level', 'error'],
        output={'both': 'log'},
    )

    # Crea y configura el nodo ROBOT_STATE_PUBLISHER, el cual resuelve la cinematica 
    # directa del robot a partir de la información que proporciona el nodo
    # JOINT_STATE_PUBLISHER_GUI.
    node_robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', urdf_file])}],
        arguments=['--ros-args', '--log-level', 'error'],
        output={'both': 'log'},
    )


    # Devuelve los nodos creados.
    return [node_rviz2, node_robot_state_publisher, node_joint_state_publisher_gui]

Por ultimo editamos el archivo setup.py del paquete e insertamos las dos líneas que aparecen al final de la lista data_files para que se incluya en la instalación del mismo el contenido de las carpetas launch y config.

import os
from glob import glob
from setuptools import setup

package_name = 'check_joints'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*.rviz')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ros',
    maintainer_email='ros@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

Y ya solo nos queda compilar el paquete con el comando colcon build.

colcon build --packages-select check_joints

Una vez que ya tenemos nuestro paquete listo podemos probar nuestro modelo ejecutando el archivo launch, al que pasaremos como parámetro el nombre del archivo URDF.

ros2 launch check_joints check_joints.launch.py urdf_file:=model.urdf

Se nos abrirán dos ventanas: una con RVIZ2 donde podremos visualizar nuestro modelo y otra generada por el nodo joint_state_publisher_gui donde aparecen todas las articulaciones que no sean fijas, en la que podremos ir variando los valores asociados al estado de las mismas (ángulos de giro o desplazamientos).

De este modo podemos ir moviendo las articulaciones y viendo simultáneamente en RVIZ2 si el comportamiento es el que deseamos.

Puedes descargar el paquete check_joints desde Github.

Post Views: 78
Comparte este artículo:

Entradas relacionadas

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *