youbot-xsens-controller es un stack de ROS (Robot Operating System) que incluye las herramientas necesarias para la toma de datos de IMUs xsens y el control del brazo robótico del robot Youbot de KUKA. También es posible controlar el simulador del robot para el programa Gazebo gracias a que tanto el robot como el simulador escuchan los mismos topics de ROS.
Este stack está dividido en tres paquetes:
-
xsens_driver: Paquete para la toma de datos de un IMU xsens o una red de IMUs xsens conectados al master xbus y su publicación en ROS. El programa
xsens_node
detecta el número de sensores conectados al PC y los configura para que devuelva los siguientes datos:- Vector aceleración lineal.
- Vector velocidad angular.
- Vector campo magnético.
- Cuaternión de orientación.
Se puede modificar este programa para obtener otro tipo de datos. Más información en al apartado dedicado a este paquete.
-
youbot_controller: Paquete para el control de cada una de las articulaciones del brazo robótico y la posición del gripper a través de topics de ROS. El programa
youbot_controller
está preparado para subscribirse a los topics que contienen los cuaterniones de orientación de tres sensores xsens publicados por el programaxsens_node
. Más información en el apartado dedicado a este paquete. -
dfv: Librería para trabajar con cuaterniones de orientación, vectores y matrices. El programa
example
contiene ejemplos de cómo usar la librería. Más información en el apartado dedicado a este paquete.
Este stack se ha realizado en Ubuntu 12.04 LTS y ROS Fuerte. Es posible que no funcione en otras versiones de Ubuntu o ROS.
Para instalar el paquete simplemente nos situaremos en nuestro espacio de trabajo de ROS y clonaremos el repositorio:
$ git clone https://github.com/DaniSagan/youbot-xsens-controller
Para la toma de datos del sensor xsens con el paquete xsens_driver
no nos hará falta instalar ninguna cosa más. Sin embargo el paquete youbot_controller
hace uso del paquete brics_actuator
para el intercambio de mensajes con el robot o el simulador. El robot Youbot ya incorpora este paquete en su instalación de fábrica, pero si queremos usar el simulador en un PC normal lo más seguro es que no lo tengamos instalado. Si no tuviéramos dicho paquete, realizaremos su instalación de la siguiente forma:
$ sudo apt-get install ros-fuerte-pr2-controllers
$ sudo apt-get install ros-fuerte-pr2-simulator
Nos situamos en nuestro espacio de trabajo:
$ git clone https://github.com/youbot/youbot-ros-pkg.git
$ git clone https://github.com/ipa320/cob_common.git
$ rospack profile
$ rosmake brics_actuator youbot_description
Compilamos el stack:
$ rosmake youbot-xsens-controller
Una vez instalado y compilado el stack youbot-xsens-controller
con sus dependencias, podemos lanzar los programas de ejemplo de cada paquete para ver su funcionamiento.
Conectamos el xbus master con tres sensores xsens al puerto USB del ordenador y lanzamos ROS:
$ roscore
Ejecutamos el programa xsens_node
:
$ rosrun xsens_driver xsens_node
Lanzamos el simulador del brazo del robot:
$ roslaunch youbot_description youbot_arm_publisher.launch
Si lo estimáramos conveniente, colocaremos el brazo en una posición más cómoda. Ahora ejecutamos el controlador:
$ rosrun youbot_controller youbot_controller
Si todo ha ido bien, podremos controlar el simulador del brazo moviendo los sensores.
Conectamos el xbus master con tres sensores xsens al puerto USB del Youbot y en el PC embebido lanzamos ROS:
$ roscore
Ejecutamos el programa xsens_node
:
$ rosrun xsens_driver xsens_node
Lanzamos el driver del robot una vez hayamos puesto en marcha el brazo robótico (Si necesitáramos permisos de usuario nos logeamos como superuser con el comando $ su -
):
$ roslaunch youbot_oodl youbot_oodl_driver.launch
Ahora ejecutamos el controlador:
$ rosrun youbot_controller youbot_controller
Si todo ha ido bien, podremos controlar el brazo real moviendo los sensores.
-
xsens_node: Este programa realiza un escaneo de los puertos del PC y detecta cuántos sensores hay conectados. A continuación los configura para que devuelvan los datos calibrados y el cuaternión de orientación (configuración por defecto), y asigna una matriz de prerrotación unitaria. Para cada sensor publica los siguientes topics:
/xsens_node/sensorX/acc : vector aceleración /xsens_node/sensorX/gyr : vector velocidad angular /xsens_node/sensorX/mag : vector campo magnético /xsens_node/sensorX/ori_quat : cuaternión de orientación
Donde X es el número de identificación de cada sensor (0, 1, 2...).
Si se hubiera establecido otra configuración, los posibles topics son los siguientes:/xsens_node/sensorX/raw_acc : vector aceleración (raw) /xsens_node/sensorX/raw_gyr : vector velocidad angular (raw) /xsens_node/sensorX/raw_mag : vector campo magnñetico (raw) /xsens_node/sensorX/pos_lla : posición (latitud, longitud, altitud) /xsens_node/sensorX/ori_matrix : matriz de orientación /xsens_node/sensorX/ori_euler : ángulos de Euler
También se publican los siguientes valores en el servidor de parámetros de ROS:
/xsens_node/sensor_count : número de IMUs conectados /xsens_node/sensorX/output_mode : modo de salida /xsens_node/sensorX/output_settings : configuración de salida
-
youbot_controller: Este programa comprueba que el número de sensores coectados es 3 (parámetro
/xsens_node/sensor_count
) y obtiene sus cuaterniones de orientación (topic/xsens_node/sensorX/ori_quat
). Realiza el cálculo de los ángulos que hay entre cada sensor (roll, pitch y yaw) y publica las posiciones de cada articulación en el topic/arm_1/arm_controller/position_command
y la posición del gripper en el topicarm_1/gripper_controller/position_command
-
example: En el código de este programa se pueden ver ejemplos de operaciones y funciones que hacen uso de las clases Vector3, Quaternion y Matrix de la librería dfv. Estas clases son usadas por el programa
youbot_controller
para la obtención de los ángulos entre sensores.
Al conectar los sensores es posible que haya que esperar 5 - 10 segundos antes de que el programa xsens_node
sea capaz de detectarlos. Si tras dos o tres intentos aún no los detecta, puede que nuestro usuario de Ubuntu no pertenezca al grupo dialout
y por lo tanto no tenga privilegios para leer el puerto serie. Esto podemos comprobarlo con el siguiente comando:
$ groups
Si no vemos el grupo dialout
en la salida de este comando, ejecutamos lo siguiente:
$ sudo adduser <nombre_de_usuario> dialout
Sustituyendo <nombre_de_usuario>
por nuestro nombre (p. ej. $ sudo adduser daniel dialout
)Y reiniciamos el ordenador para que el cambio tenga efecto. Ahora el programa xsens_node
tendría que detectar los sensores.
Este error puede deberse a que necesitemos permisos de superusuario. Para logearnos como superusuario ejecutamos el siguiente comando:
$ su -
Es necesario añadir la siguiente línea al archivo youbot-ros-pkg/youbot_common/youbot_description/launch/youbot_arm_publisher.launch
:
<node name="gazebo_gui" pkg="gazebo" type="gui" respawn="false" output="screen"/>
Después de lo siguiente:
<node name="gazebo" pkg="gazebo" type="gazebo" args="$(find gazebo_worlds)/worlds/empty.world" respawn="false" output="screen">
<env name="GAZEBO_RESOURCE_PATH" value="$(find youbot_description):$(find gazebo_worlds):$(find gazebo)/gazebo/share/gazebo" />
</node>