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-toolsEn 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_fileUna 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_fileUsando 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.rvizAbrimos 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: 161El 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_jointsUna 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.urdfSe 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.
