MODELADO CINEM ´ ATICO Y SIMULACI ´ ON REALISTA DEL MANIPULADOR M ´ OVIL DE BAJO COSTE TURTLEBOT2 + WIDOW-X EN ROS Marina Aguilar, Ana Cruz-Mart´ ın y Juan-Antonio Fern´ andez-Madrigal Departamento de Ingenier´ ıa de Sistemas y Autom´ atica Universidad de M´ alaga [email protected], [email protected], [email protected]Resumen El manipulador m´ ovil compuesto de la plataforma Turtlebot2 y el brazo Widow-X supone una solu- ci´on de bajo coste muy interesante para laborato- rios docentes y de investigaci´on. Lamentablemen- te, el conjunto no dispone de un simulador comple- to en el popular framework de programaci´on ROS; tampoco el fabricante ofrece el modelado cinem´ati- co del brazo. En este art´ ıculo se describe el trabajo realizado sobre ambas cuestiones para nuestro ro- bot CRUMB, y c´omo se han implementado en Ga- zebo, un simulador realista para ROS donde tam- bi´ en se ha a˜ nadido la posibilidad de incluir ruido sensorial simple, algo que la mayor´ ıa de los simu- ladores no ofrecen. Se incluyen tambi´ en algunos experimentos de manipulaci´on y navegaci´on que demuestran las posibilidades de nuestro trabajo. Palabras clave: manipulador m´ ovil, simulaci´ on, modelado cinem´ atico, Turtlebot2, Widow-X, ROS, Gazebo. 1. INTRODUCCI ´ ON Como parte del proyecto puente de investigaci´ on denominado CRUMB [1] (Cognitive-Robotics- sUpporting Mobile Base ), financiado a trav´ es del Plan Propio de Investigaci´ on de la Universidad de M´ alaga, se adquiri´ o un manipulador m´ ovil de ba- jo coste sobre el que implementar desarrollos de Rob´ otica Cognitiva. Este robot est´ a formado por una base m´ ovil Turtlebot2 [2] y un brazo manipu- lador Widow-X [3], mostrados en la figura 1. Para facilitar que varios miembros del equipo investigador pudieran trabajar de manera si- mult´ anea con esta plataforma rob´ otica, se deci- di´ o crear un simulador realista compatible con el robot real, de manera que los algoritmos desarro- llados en el primero pudieran utilizarse en el ro- bot CRUMB con m´ ınimos cambios, y que se in- corporaran efectos suficientemente realistas en ci- nem´ atica, din´ amica y ruido de los sensores. Como Figura 1: Manipulador m´ ovil CRUMB. el robot deb´ ıa ser capaz de realizar tareas de mani- pulaci´ on, en primer lugar era necesario obtener el modelo cinem´ atico del brazo manipulador Widow- X, no disponible por parte del fabricante, y resol- ver los problemas directo e inverso. Adem´ as, el si- mulador del robot completo deb´ ıa estar integrado en ROS [4], dada su popularidad como framework de programaci´ on en la comunidad Rob´ otica y por tanto, su idoneidad para compartir desarrollos de diversos investigadores. En este art´ ıculo se presenta el trabajo realizado bajo los mencionados objetivos, concretamente el modelado cinem´ atico del brazo y la simulaci´ on realista del manipulador m´ ovil en Gazebo [5] (que se encarga de la din´ amica realista), as´ ı como la implementaci´ on de un interfaz de programaci´ on pr´ acticamente id´ entico al del robot real (es decir, los topics de ROS necesarios) y la inclusi´ on de ruido simple en los sensores. Actas de las XXXIX Jornadas de Automática, Badajoz, 5-7 de Septiembre de 2018 262
Transcript
MODELADO CINEMATICO Y SIMULACION REALISTADEL MANIPULADOR MOVIL DE BAJO COSTE
TURTLEBOT2 + WIDOW-X EN ROS
Marina Aguilar, Ana Cruz-Martın y Juan-Antonio Fernandez-MadrigalDepartamento de Ingenierıa de Sistemas y Automatica
El manipulador movil compuesto de la plataformaTurtlebot2 y el brazo Widow-X supone una solu-cion de bajo coste muy interesante para laborato-rios docentes y de investigacion. Lamentablemen-te, el conjunto no dispone de un simulador comple-to en el popular framework de programacion ROS;tampoco el fabricante ofrece el modelado cinemati-co del brazo. En este artıculo se describe el trabajorealizado sobre ambas cuestiones para nuestro ro-bot CRUMB, y como se han implementado en Ga-zebo, un simulador realista para ROS donde tam-bien se ha anadido la posibilidad de incluir ruidosensorial simple, algo que la mayorıa de los simu-ladores no ofrecen. Se incluyen tambien algunosexperimentos de manipulacion y navegacion quedemuestran las posibilidades de nuestro trabajo.
Como parte del proyecto puente de investigaciondenominado CRUMB [1] (Cognitive-Robotics-sUpporting Mobile Base), financiado a traves delPlan Propio de Investigacion de la Universidad deMalaga, se adquirio un manipulador movil de ba-jo coste sobre el que implementar desarrollos deRobotica Cognitiva. Este robot esta formado poruna base movil Turtlebot2 [2] y un brazo manipu-lador Widow-X [3], mostrados en la figura 1.
Para facilitar que varios miembros del equipoinvestigador pudieran trabajar de manera si-multanea con esta plataforma robotica, se deci-dio crear un simulador realista compatible con elrobot real, de manera que los algoritmos desarro-llados en el primero pudieran utilizarse en el ro-bot CRUMB con mınimos cambios, y que se in-corporaran efectos suficientemente realistas en ci-nematica, dinamica y ruido de los sensores. Como
Figura 1: Manipulador movil CRUMB.
el robot debıa ser capaz de realizar tareas de mani-pulacion, en primer lugar era necesario obtener elmodelo cinematico del brazo manipulador Widow-X, no disponible por parte del fabricante, y resol-ver los problemas directo e inverso. Ademas, el si-mulador del robot completo debıa estar integradoen ROS [4], dada su popularidad como frameworkde programacion en la comunidad Robotica y portanto, su idoneidad para compartir desarrollos dediversos investigadores.
En este artıculo se presenta el trabajo realizadobajo los mencionados objetivos, concretamente elmodelado cinematico del brazo y la simulacionrealista del manipulador movil en Gazebo [5] (quese encarga de la dinamica realista), ası como laimplementacion de un interfaz de programacionpracticamente identico al del robot real (es decir,los topics de ROS necesarios) y la inclusion deruido simple en los sensores.
Actas de las XXXIX Jornadas de Automática, Badajoz, 5-7 de Septiembre de 2018
262
El texto se organiza de la siguiente forma: en laseccion 2 se describe el calculo del modelado ci-nematico directo e inverso del brazo manipuladorWidow-X ; en la seccion 3 se detalla como se ha im-plementado el simulador para CRUMB en ROS yGazebo, incluyendo el modelado geometrico y delos parametros fısicos y de control que hemos reali-zado; en la seccion 4 se muestran los experimentosrealizados con el simulador completo; finalmente,en la seccion 5 se resumen las conclusiones y laslıneas de trabajo futuras.
2. PROBLEMAS CINEMATICODIRECTO E INVERSO EN ELBRAZO WIDOW-X
Para realizar tareas de manipulacion (p.ej., pickand place) con el brazo Widow-X es necesario almenos su modelado cinematico. Este brazo cuentacon 5 g.d.l y una pinza paralela como efector final.
El modelo cinematico se basara en el producto delas matrices de paso correspondientes a cada unade las articulaciones. Para calcular dichas matricesde paso se necesitan los parametros de Denavit-Hartenberg [6], que son (ver figura 2):
ai: distancia desde el eje Zi hasta el eje Zi+1
medida a lo largo de Xi.
αi: angulo que forman los ejes Zi y Zi+1 me-dido sobre Xi.
di: distancia desde el eje Xi−1 hasta el eje Xi
medida a lo largo de Zi.
θi: angulo que forman los ejes Xi−1 y Xi me-dido sobre Zi.
En el caso del Widow-X, estos parametros son losmostrados en el cuadro 1. Con ellos se obtienenlas matrices de paso 1, 2, 3, 4 y 5.
El modelo cinematico sera el indicado en 6. El pun-to obtenido con el mismo sera el nombrado comoOm en la figura 3, que corresponde a la muneca.
T 05 = T 0
1 T12 T
23 T
34 T
45 (6)
El problema cinematico directo consiste basica-mente en la realizacion del producto anterior conuna postura articular dada. El inverso necesita laresolucion de las mismas ecuaciones cuando talpostura es desconocida. En nuestro caso se ha ob-tenido con un procedimiento algebraico mediantedesacoplo de la muneca respecto del brazo: en pri-mer lugar se obtienen las variables articulares θ1,
263
Figura 3: Posicion de la muneca y efector final.
θ2 y θ3 en funcion de un punto generico, y a con-tinuacion θ4 y θ5 para una orientacion generica dela muneca.
Para ello, partiendo de la matriz T 05 , se modifica la
ultima columna para introducir el punto genericode la muneca (px, py, pz), obteniendose la matrizTp05 de la ecuacion 7, en la que los elementos rijson los elementos ij del modelo cinematico.
Tp05 =
r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1
(7)
La matriz Tp05 debe ser igual a T 05 , con lo que se
obtiene la ecuacion 8d.
Tp05 = T 01 T
12 T
23 T
34 T
45 (8a)
[T 01 ]−1Tp05 = T 1
2 T23 T
34 T
45 (8b)
T 10 Tp
05 = T 1
2 T23 T
34 T
45 (8c)
Tp15 = T 12 T
23 T
34 T
45 (8d)
Resolviendo el sistema de ecuaciones resultante seobtienen las variables articulares θ1, θ2 y θ3 (ecua-ciones 9a, 9b y 9c, respectivamente).
θ1 = A tan 2(py, px) (9a)
θ2 = β +A tan 2(k1, k2)−
A tan 2(k,±√k21 + k22 + k2) (9b)
θ3 = θ2 −A sin
(−pz − d2sinβ
L3
)(9c)
donde k, k1 y k2 son, respectivamente, 10a, 10b y10c.
k = p2z + d22 − L23+
(px cos θ1 + py sin θ1)2 (10a)
k1 = 2px cos θ1d2 + 2py sin θ1d2 (10b)
k2 = 2pzd2 (10c)
Para obtener θ4 y θ5, se utiliza la orientacion dela muneca. Sea R0
5 la submatriz de rotacion deT 05 para una orientacion generica [n, o, a] (ecuacion
11).
R05 = [n, o, a] =
nx ox axny oy aynz oz az
(11)
Se tiene la igualdad 12:
R05 = R0
3 ·R35 (12)
donde R03 es conocido ya que es el producto en la
ecuacion 13a de las submatrices de rotacion mos-tradas, que dependen de θ1, θ2 y θ3, ya obtenidospreviamente, y R3
5 el producto de la ecuacion 13b,cuyo resultado depende de θ4 y θ5.
R03 = R0
1 ·R12 ·R2
3 (13a)
R35 = R3
4 ·R45 (13b)
Al resolver el sistema de ecuaciones resultante seobtienen las variables articulares de la muneca,ecuaciones 14a y 14b.
θ4 = A cos(−k3) − π
2(14a)
θ5 = A sin(nx sin θ1 − ny cos θ1) (14b)
donde k3 es:
k3 = az cos(θ2 − θ3)+
ax sin(θ2 − θ3) cos θ1+
ay sin(θ2 − θ3) sin θ1
(15)
Finalmente, se obtiene la postura del efector finalOr en funcion de la de la muneca Om, obtenida asu vez del modelo cinematico (figura 3), de dondese deduce la ecuacion 16.
~Pm = ~Pr − L4 · Z5 (16)
3. MODELADO DEL ROBOTPARA SU SIMULACION
En esta seccion se describe el modelado geometri-co, fısico y de controladores necesario para la simu-lacion en Gazebo del brazo manipulador Widow-X,ası como el de la base movil Turtlebot2 y del robotcompleto.
3.1. BRAZO WIDOW-X
El modelado del brazo Widow-X se divide en mo-delado geometrico, modelado de parametros fısi-cos, modelado de los controladores y configuracionde los plugins necesarios en Gazebo [7].
264
3.1.1. Modelado geometrico
La geometrıa del brazo se ha modelado partiendode los ficheros CAD .stl facilitados por RobotnikAutomation S.L.L. [8], distribuidores de este ma-nipulador. Dicho modelado se describe en formatoXML, donde se diferencian dos elementos: eslabo-nes y articulaciones.
En los eslabones se indican dos tipos de geo-metrıas: la visible y la de deteccion de colisiones,donde se anade el fichero .stl junto con el origende coordenadas y la orientacion del eslabon. Paraindicar la relacion entre eslabones se utilizan lasarticulaciones. En este trabajo se utilizan unionesde revolucion, prismaticas y fijas. En las dos pri-meras se indica el punto de contacto, el eje de giro,los lımites del movimiento y cuales son los eslabo-nes fijo y movil. Tambien se puede indicar si unapieza imita el movimiento de otra, como en el ca-so de la pinza. Para las uniones fijas se indican lasdos piezas que se unen, en este caso la base delbrazo con la superficie superior de la base movil.
Tambien es necesario anadir propiedades de colory autocolision en los ficheros de configuracion, pa-ra que el modelo utilice la deteccion de colisionespara no atravesarse a sı mismo y para mejorar suaspecto visual.
3.1.2. Modelado de parametros fısicos
Para que el modelo sea lo mas realista posible sehan incluido en el mismo parametros fısicos comola inercia, el amortiguamiento y la friccion.
Para cada uno de los eslabones se ha calculadola masa, el centro de masas y el tensor de inerciarespecto al centro de masas. El software utilizadoha sido MeshLab [9] para los centros de masa yV-REP [10] para la masa y el tensor de inercia.
Para comprobar que las inercias son correctas Ga-zebo dispone de un visor. En la figura 4 se observa,por ejemplo, como la geometrıa del brazo y la ge-nerada por la inercia son coincidentes.
El amortiguamiento y la friccion de los eslabonesse han obtenido experimentalmente bajo la premi-sa de que el brazo no presente vibraciones. Se haaumentado la resolucion de los metodos numericosdurante la simulacion para mejorar los resultadosobtenidos.
Finalmente, para poder realizar tareas con la pin-za hay que indicar cuales son las partes movilesy fijas de la misma, el tiempo para agarrar y sol-tar el objeto y el rozamiento y el contacto. Todosestos parametros se han obtenido de manera expe-rimental persiguiendo que las piezas no se quedenpegadas ni resbalen.
Figura 4: Widow-X en el visor de inercia de Ga-zebo.
3.1.3. Modelado de controladores
Se ha elegido un control de esfuerzo/posicion pa-ra la simulacion de las articulaciones, ya que ex-perimentalmente es el que mejores resultados haofrecido. El controlador PID se ha sintonizado demanera experimental.
Tambien se han incluido el esfuerzo y la velocidadmaxima de las articulaciones. Para estos datos sehan utilizado los del brazo Widow-X real.
3.1.4. Plugins Gazebo
Para anadir las funcionalidades necesarias para lasimulacion realista de los modelos en Gazebo hasido necesario incluir los siguientes plugins:
Ros Control : necesario para simular cualquierrobot, permite que los controladores puedanactuar sobre las articulaciones.
Mimic: necesario para las articulaciones dela pinza, donde las dos partes se mueven demanera simetrica y al mismo tiempo.
3.2. PLATAFORMA MOVILTURTLEBOT2
La plataforma Turtlebot2 esta basada en una ba-se diferencial Kobuki [11] que proporciona energıapara el ordenador externo, sensores (camara ki-nect, encoders magneticos incrementales, IMU,bumpers, sensores anticaıda y antivuelco), actua-dores, indicadores y botones. El modelo de laplataforma ya estaba disponible en el repositorioROS, junto con el stack de software de navegacionque se utiliza en el experimento 2. Sin embargo,para que su aspecto visual sea mas real se ha mo-dificado la geometrıa de colision para que coincidacon la de CRUMB.
265
3.3. MANIPULADOR MOVIL CRUMB
Uniendo los modelos del brazo Widow-X y dela plataforma Turtlebot2 se ha obtenido el robotcompleto CRUMB, mostrado en la figura 5.
Figura 5: Modelo completo de CRUMB en Gazebo.
Este modelo viene ya asociado en ROS con los to-pics de programacion mas importantes existentesen el robot real:
Comandos de actuacion y estado de todas lasarticulaciones del brazo
Lectura de los sensores IMU, camara kinect,bumpers, sensores anticaıda.
Datos de odometrıa de la plataforma.
Datos del estado completo del sistema, basemovil y brazo.
Sin embargo, otros topics del robot real han tenidoque ser incluidos adicionalmente:
Lectura de los sensores wheel drop y LEDsindicadores.
Lectura de todos los sensores de la plataformaen un mismo topic.
Tambien se han anadido topics utiles para simu-lacion que no existen en real, como la postura realde la plataforma, el tiempo real y el de simulacion.
Finalmente, para que el modelo sea mas realista,se ha implementado ruido simulado en el sensor
inercial y en la camara kinect, obteniendose ası 2modelos de CRUMB simulado distintos, uno conruido y otro sin ruido, una caracterıstica que no eshabitual en la simulacion con Gazebo y que puedeser muy util para realizar experimentos de diversostipos.
Como ejemplo de esta ultima caracterıstica, la fi-gura 6 muestra la lectura de la camara kinect sinruido y con ruido gaussiano.
Figura 6: Camara kinect sin ruido (izda) y conruido (dcha).
4. EXPERIMENTOS
Para validar el trabajo presentado aquı y mostrarsu utilidad, se han desarrollado tres aplicacionesexperimentales, que se detallan a continuacion.
4.1. EXPERIMENTO 1.PICK-AND-PLACE
En este experimento el robot permanece fijo enel escenario mostrado en la figura 7 mientras elbrazo va moviendo los objetos segun programa elusuario. Hay tres posiciones posibles de pick-and-place: dos para los objetos y una libre.
Figura 7: Escenario para pick-and-place.
En la figura 8 se observa como CRUMB va mo-viendo los objetos hacia el hueco libre. En la re-ferencia [12] se encuentra el vıdeo para ver la se-cuencia completa.
266
Figura 8: Secuencia de movimiento pick-and-place.
4.2. EXPERIMENTO 2. MAPEADODEL ENTORNO Y NAVEGACION
En el segundo experimento, con el stack de na-vegacion de ROS, CRUMB crea un mapa de suentorno para, posteriormente, navegar a una posi-cion indicada evitando obstaculos. En la figura 9se muestra el escenario creado en Gazebo.
Figura 9: Escenario para mapeado y navegacion.
Una vez lanzados los nodos del stack de navega-cion de ROS, se genera el mapa de la figura 10 conlos datos recopilados por la camara kinect.
Figura 10: Mapa creado del entorno.
En la figura 11 se observa como CRUMB es capazde ir a un punto indicado en el mapa esquivandolas paredes del entorno. Lo mas relevante respec-to al presente trabajo es que dicha navegacion se
realiza de manera fısicamente realista.
Figura 11: CRUMB llegando al punto objetivo.
Si se incluyeran nuevos objetos en el escenario,utilizando el mismo mapa, el robot llega al puntoobjetivo esquivando con exito estos nuevos obje-tos, como se aprecia en la figura 12.
Figura 12: CRUMB navega en un escenario conobstaculos.
En la referencia [13] se puede ver la secuencia com-pleta de este experimento.
4.3. EXPERIMENTO 3.MANIPULACION MOVIL
En este tercer experimento se combina la navega-cion con la manipulacion de objetos. En el esce-nario de la figura 13 CRUMB navega hasta unasuperficie donde se encuentra un objeto, lo recogey lo lleva hasta otra superficie para soltarlo.
Figura 13: Escenario para la manipulacion movil.
En la figura 14 se ve como CRUMB es capaz derealizar la tarea. En la referencia [14] se encuentra
267
el vıdeo con la secuencia completa de este experi-mento, donde se puede apreciar mejor el realismode la simulacion.
Figura 14: CRUMB realizando el experimento 3.
5. CONCLUSIONES Y LINEASDE TRABAJO FUTURAS
En este trabajo se ha presentado una simulacionen ROS y Gazebo del manipulador movil de bajocoste CRUMB, compuesto por un brazo Widow-Xy una plataforma Turtlebot2. Esta simulacion per-mite ejecutar las mismas aplicaciones que se eje-cutarıan en el robot real programandolas de modopracticamente identico gracias a que se han anadi-do todos los topics de ROS necesarios. Ademas, alincluir elementos geometricos (visuales y de coli-sion), parametros fısicos, de control y ruido en lossensores IMU y en la camara kinect, se ha conse-guido que el modelo tenga un aspecto muy realis-ta.
Este trabajo tambien ha incluido el modelado ci-nematico del brazo Widow-X, no disponible ante-riormente, ası como la resolucion de la cinematicadirecta e inversa del mismo.
Para validar la simulacion se han llevado a cabodiversos experimentos con exito: pick-and-place,creacion de mapas, navegacion con obstaculos ynavegacion combinada con manipulacion.
Tambien se han escrito tres manuales: de insta-lacion, uso y desarrollo, y se ha subido todo elcodigo necesario para su uso al popular reposito-rio de software GitHub del proyecto [15], dondeesta disponible publicamente.
El trabajo futuro incluye de manera natural el usode esta simulacion en docencia en ingenierıa, don-de se deberıa evaluar el aprendizaje del alumno ylas diferencias encontradas con el robot real; asi-mismo, este trabajo ha constituido una base im-portante del proyecto CRUMB, donde se pretende
en el futuro implementar diversas lıneas de inves-tigacion relacionadas con la Robotica Cognitivaen la que los integrantes del proyecto trabajan ac-tualmente, como el aprendizaje por refuerzo o lainferencia bayesiana aplicada a sistemas de per-cepcion roboticos.
Agradecimientos
Este trabajo ha sido financiado por el Plan Pro-pio de Investigacion de la Universidad de Malaga.Los autores agradecen a la empresa Robotnik Au-tomation S.L.L su amabilidad y disposicion, asıcomo los ficheros CAD del brazo. Tambien deseanagradecer a los companeros integrantes del pro-yecto CRUMB sus valiosas aportaciones durantela realizacion de este trabajo.
English summary
REALISTIC SIMULATION OFTHE LOW-COST TURTLEBOT2+ WIDOW-X MOBILE MANIPU-LATOR IN ROS
Abstract
The combination of a Turtlebot2 mobile-platform with a Widow-X robot-arm of-fers an interesting low-cost solution forresearch, teaching, or generic educatio-nal purposes. Unfortunately, neither doesthis hardware combination have a com-plete simulator in the robot programming-framework ROS, nor does the manufactu-rer offer a kinematic model of the robo-tic arm. In this article we propose a so-lution for both issues related to our mobi-le manipulator. Besides, we describe howrobot simulation was developed in Gaze-bo, a realistic simulator integrated withROS, and the possibility to include simplesensor-noise, which is not always availableon most simulators. Finally, we demons-trate the applicability of our simulator bymeans three experiments that involve pick-and-place, mapping, and navigation tasks.
Keywords: mobile manipulator, robot si-mulation, kinematic modelling, Turtlebot2,Widow-X, ROS, Gazebo.
268
Referencias
[1] Blog CRUMB. [Online]. Disponible:http://babel.isa.uma.es/crumb/, Accedi-do: 20/06/2018.
