Memorias del XVIII Congreso Mexicano de Robotica 2016Universidad Autonoma de Sinaloa y Asociacion Mexicana de Robotica e Industria AC

XVIII COMRob 2016, ISBN: En tramite9–11 de Noviembre, 2016, Mazatlan, Sinaloa, Mexico

XVIIICOMRob2016/ID-015

IMPLEMENTACION DE HABILIDADES CULINARIAS EN UNHUMANOIDE NAO

Pablo Vera, Brian Medrano, Juan M. IbarraLaboratorio de Robotica y Vision Artificial

Depto de Control Automatico del CinvestavAv. IPN No. 2508, 07360 Mexico, DF

pvera@ctrl.cinvestav.mx, bmedrano@ctrl.cinvestav.mx,jibarra@cinvestav.mx

Mario Fuentes, Abraham ArrearanIngenierıa Mecatronica

UPIITA-IPN, UPSINmfts1@hotmail.com

ec112010@upsin.edu.mx

El objetivo de este trabajo es presentar la aplicacion desa-rrollada sobre un humanoide Nao para que pudiera participarde manera competitiva en el certamen Humabot Challenge 2014.Dicha aplicacion consta de tres tareas: encender y/o apagar laestufa, preparar ensalada de jitomate y hacer la lista de la com-pra. Estas tareas hacen uso intensivo de las capacidades de per-cepcion visual el robot, ademas de caminar, se requiere de mo-vimientos de los brazos para poder manipular el jitomate, pa-ra accionar los interruptores de la estufa y para abrir o cerrarlas puertas de la alacena. Ası, se presentan los algoritmos desa-rrollados ası como su implementacion en Python utilizando laslibrerıas de OpenCV, que permitiron la obtencion del segundolugar en esta competicion.

En esta ocasion el equipo dotMEX participo en el HumabotChallenge 2014 organizado como parte del 2014 IEEE-RAS In-ternational Conference on Humanoid Robots, organizado en no-viembre de 2014 en la ciudad de Madrid, Espana. Este certamenconsto de tres pruebas a desarrollar en un ambiente domestico:la cocina; consistentes en: i) manipular los controles de una es-tufa electrica en funcion de su estado; ii) Hacer la lista de lacompra en funcion de los productos en existencia dentro de laalacena; y iii) Preparar una ensalada de jitomate.

INTRODUCCION

El equipo dotMEX viene participando en diversas competi-ciones de futbol con robots humanoides, tanto en el ambito in-ternacional como en el nacional. Durante los ultimos 7 anos, he-mos participado en los Torneos Mexicanos de Robotica (TMR)de la Federacion Mexicana de Robotica (FMR) en los anos 2009,2011, 2012 y 2014; en la RoboCup en 2009, 2010 y 2012, asıcomo en la RoboWorld Cup organizada por FIRA en 2011 y2012 habiendo ganado esta ultima en la categorıa Hurocup Uni-ted Soccer. En 2012 participamos tambien en el International Ro-bot Contest (IRC2012) realizado en Seul, Corea ganando nueva-mente en la categorıa de futbol con humanoides.

En una primera etapa se utilizaron robots humanoides co-merciales hobbist level, como el Robonova I, a los cuales se lesagregaron dos grados de libertad en el cuello ası como una cabe-za con camara de video, sensores inerciales y una cierta potenciade computo extra. En algun momento tambien se le hicieron mo-dificaciones a la estructura mecanica de las piernas con el objetode mejorar su caminado y para cumplir con las normas RoboCupvigentes en aquel momento. Mas adelante se comenzo a utili-zar el Bioloid, humanoide comercial de la misma categorıa queel Robonova I, al cual debieron de practicarsele modificacionessimilares a nivel del cuello (2 gdl extras) y cabeza (camara devideo). Posteriormente se ha utilizado el robot humanoide Dar-win OP el cual cuenta con mejores prestaciones en el poder decomputo.

Copyright c© 2016 by AMRob

RESUMEN

1. REGLAS DEL HUMABOT CHALLENGE 20141.1. Pruebas

En el desafıo HumaBot el robot es una parte integral de lacasa y ayuda a sus ocupantes a vivir mejor. En la edicion 2014,se realizan las siguientes tres pruebas que se llevaran a cabo enla cocina de la casa, mostradas en la figura 1.

1. Apagar la estufa: el robot debe comprobar visualmente cualparrilla de la estufa se encuentra encendida y apagarla pre-sionando el boton correspondiente.

2. Lista de la compra: el robot debe reconocer que productos(dentro de un conjunto predefinido con 6 objetos comunes)estan disponible en los estantes, y que productos son los fal-tantes, y hacer una lista de la compra.

3. Preparacion de la comida: el robot debe agarrar un tomatey ponerlo en una ensaladera.

1.2. Reglas e Informacion tecnicaLa eleccion del metodo y lenguajes de programacion estan

en manos de los equipos. La intervencion humana esta prohibida,pero se permite la conexion con el robot a traves de Wi-Fi, aun-que se prohıbe teleoperar el robot. El codigo, debe presentarseal jurado, sin ningun tipo de restriccion. Se pueden utilizar mar-cas para la navegacion y el reconocimiento de objetos, ası comocualquier otro sistema para guiar el robot alrededor el entorno.Se permite que el robot caiga, pero debe levantarse y continuarcon la tarea por sı mismo. Cualquier intervencion humana du-rante la prueba dara lugar a una penalizacion que se aplica a losresultados de esta prueba.

1.3. NavegacionLa navegacion es un requisito para cada prueba. De hecho, el

robot debe comenzar en su posicion de base e ir a los elementoscon los que debe interactuar. La eleccion de la tecnica utilizadapor el robot para orientarse en la habitacion es libre. Las marcasde orientacion podran colocarse en algunas superficies (Armario,mesa). Los equipos participantes son libres de anadir marcas enel escenario, antes del inicio de la prueba.

1.4. AmbienteEl area de caminado es de 85 cm × 90 cm, rodeada por los

muebles (estufa, estante y mesa) y paredes de 45 cm de altura.El suelo es una plataforma de madera elevada para facilitar lamanipulacion de los objetos en las pruebas.

1.5. Objetos para lista de comprasEn esta prueba, se le pide al robot inspeccionar los estan-

tes y producir para el usuario una lista de los objetos faltantes.Algunos de los productos de interes se muestran en la figura 2.

FIGURA 1. ENTORNO DOMESTICO: LA COCINA

FIGURA 2. A) CUBOS DE CALDO, B) CLAVO C) CAFE, D) TE,E) PALOMITAS Y F) MUESLI

2. LOCALIZACIONDebido a que las reglas permiten agregar marcas al ambien-

te, se utiliza la marca mostrada en la figura 3, la cual es un cuadrode 10 cm de lado. Esta marca se coloca en el area donde se rea-lizara la prueba actual. Con la finalidad de generar un patron defacil reconocimiento, ası como de ser capaz de colocarse con unaorientacion especıfica se considero lo siguiente:

Utilizar cırculos como elementos constituyentes ya que elalgoritmo de transformada de Hough, ampliamente utiliza-do en problemas de vision puede resolverse en tiempo realy configurar parametros que ayuden a evitar posibles detec-ciones erroneas.Emplear 4 colores distintos, lo cual crea una orientacionunica del cuadrado a buscar. La seleccion de colores no serealizo de manera arbitraria, sino que se propuso emplear lastonalidades de rojo, azul, verde y negro debido a su rapidasegmentacion y bajas posibilidades de confusion entre sı.

Conocidas las medidas referentes a las distancias entre loscırculos del patron fısico original, es posible, mediante el uso deuna camara, establecer la correspondencia entre los puntos 3Ddel espacio fısico con los 2D de una imagen procesada en tiem-po real. Para ello se emplean las soluciones correspondientes al

Copyright c© 2016 by AMRob

FIGURA 3. MARCA PARA LOCALIZACION

problema Perspective-n-Point (PnP), en el cual mediante un con-junto de n puntos en el espacio 3D se puede estimar la posicionde la camara mediante la imagen que esta obtiene. Si bien unasolucion comun considera n = 3 como un valor optimo, usual-mente se sugiere trabajar con n > 3; los comandos disponiblesen OpenCV [4] incluyen una solucion al problema PnP, utilizan-do 4 puntos, de ahı que se haya seleccionado un patron con 4puntos: los centros de los cırculos usados en el patron [1] [2].

Previo al desarrollo del resto del algoritmo se necesitan co-nocer los parametros propios de la camara a emplear (matriz dela camara y coeficientes de distorsion), en este caso de la incor-porada en el robot NAO, cuyos parametros son los siguientes:

F =

795,80 0 326,030 796,41 230,220 0 1

K =

[0,454 −2,359 −0,0028 0,0042 4,7523

] (1)

La localizacion del robot se realiza mediante el algoritmo deestimacion de pose mostrado en el Algoritmo 2. Este algoritmousa como parametros el centroide de cada uno de los cırculos decolores,los cuales se calculan con el Algoritmo 1.

3. APAGAR ESTUFAEsta prueba consiste en determinar primero cual de las dos

parrillas se encuentra encendida y apagarla. La parrilla inferiorse controla con el boton de la izquierda y la parrilla superior conel boton de la derecha.

El primer paso es colocarse frente a la estufa para ello seutiliza el Algoritmo 2, especificandole que se quede a 30 cm dela marca, la distancia esta determinada de forma que pueda verla estufa y el boton, sin necesidad de moverse nuevamente.

Algoritmo 1: Centroide

1 Funcion centroide(imagen, color, tipo)Salida: centro

2 bin←segmentar(imagen, color)3 hsv←convertir bgr2hsv(imagen)4 if tipo=’circulos’ then5 cx, cy, r←Hough Circulos(bin)6 if tamano(cx)=1 then7 centro← cx, cy, r8 else9 centro← max([cx, cy],r)

10 else11 lin← Hough Lineas(bin)12 lin← ordena(lin)13 cx ← (lin[u]x− lin[0]x)/2+ lin[0]x14 cy ← (lin[u]y− lin[0]y)/2+ lin[0]y15 centro← [cx, cy]

Algoritmo 2: Estimacion de Pose

1 Funcion pose robot(imagen, F , K, x)Salida: rvec, tvec

2 board[0]← [0,0,0]3 board[1]← [0,x,0]4 board[2]← [x,0,0]5 board[3]← [x,x,0]6 centros[0]← centroide(imagen, azul, ’circulos’)7 centros[1]← centroide(imagen, verde, ’circulos’)8 centros[2]← centroide(imagen, rojo, ’circulos’)9 centros[3]← centroide(imagen, negro, ’circulos’)

10 rvec, tvec←solvePnP(board, centros, F , K)

Posteriormente se realiza el paneo para identificar la parrillaencendida, para ello se analizan las imagenes buscando cırculosde color rojo y estimando la posicion de su centro con respectoal sistema de referencia del robot, tambien se establece un um-bral u en la profundidad para poder identificar cual es la parrilladetectada, de acuerdo al Algoritmo 5 [8] [9].

Algoritmo 3: Posicion del objeto

1 Funcion pose obj(tilt, pan)Salida: x, y

2 x←0.1tan(90− tilt)sin(pan)3 y←0.1tan(90− tilt)cos(pan)

Copyright c© 2016 by AMRob

Algoritmo 4: Centrar objeto

1 Funcion centra obj(centro)Salida: tilt, pan

2 while centro[0], centro[1]) != (F[0,3],F[1,3]) do3 centrar cabeza(centro)4 [tilt, pan]←leer tilt pan()

Una vez identificada la parrilla se realiza un nuevo paneopara buscar el boton correspondiente, este es de forma rectangu-lar por los que se buscan lıneas a traves de la transformada deHough. Todas las lıneas verticales se ordenan por su posicion enla imagen y posteriormente se clasifican de acuerdo a la distanciaentre ellas, es decir si estan juntas pertenecen a un mismo objeto.Se considera como boton al mayor conjunto de lıneas y a este sele calcula el centroide para determinar la posicion del boton conrespecto al robot. Por ultimo, se debe presionar el boton corres-pondiente a la a parrilla detectada. En el Algoritmo 6 se muestrael pseudo-codigo de la aplicacion.

Algoritmo 5: Parrilla Encendida

1 Void parrilla(imagen, color,u)Salida: parrilla

2 centro←centroide(imagen,rojo)3 [x,y]←centra obj(centro)4 if y > u then5 parrilla← 16 else7 parrilla←−1

Algoritmo 6: Presiona boton

1 Void presiona boton(parrilla)2 boton←centroide(imagen, gris, ’lineas’)3 x,y← centra obj(boton)4 x← x+5parrilla5 mueve brazo1(x,y,35)

4. LISTA DE COMPRASEn esta tarea hay que realizar una lista de compras, con los

objetos faltantes en la alacena. Antes de realizar la prueba se rea-liza la etapa de entrenamiento del robot, lo cual consiste en reali-zar la base de datos de los objetos para su reconocimiento futuro,ecuacion (2)

D [m] =[

u v i j desc]

(2)

Donde m es numero de elementos en la base de datos, (u,v)el keypoint detectado, desc es el descriptor del Keypoint, i elidentificador del objeto (6 objetos) y j el identificador de la vista(4 vistas) del objeto.

Para realizar la deteccion de un Keypoint y generar su Des-criptor se utilizo el algoritmo FAST [5] debido a que es mas rapi-do en comparacion con otros como SIFT o SURF [6], ademas enesta competencia el tiempo de ejecucion es un factor en la eva-luacion del desempeno del robot. Previo a la deteccion de carac-terısticas se realiza un algoritmo para que identifique un objetopara unicamente determinar las caracterısticas de este y eliminarlas del ambiente donde se esta entrenando al robot, de acuerdo alAlgoritmo 7, mientras que la base de datos se genera con el Al-goritmo 8. En la figura 4 se muestra el objeto detectado durantela fase de entrenamiento, este objeto se encuentra encerrado porel recuadro de color rojo.

Algoritmo 7: Busca Objeto

1 Funcion Objeto(imagen,base)Salida: img recortada

2 img resta← imagen – base3 img umbral← umbraliza(img resta,4)4 img borde← detecta bordes(img umbral, ’canny’)5 rectangulo← min max bordes(img borde)6 img recortada← rec imagen(imagen, rectangulo)

Algoritmo 8: Base de Datos

1 Funcion caracteristicas(camara,num obj)Salida: D,imgenes

2 m = 03 base←obtener imagen(camara,’gray’)4 for i← 1 to num obj do5 for j← 1 to 4 do6 img←obtener imagen(camara)7 imgenes[i][ j]← Objeto(imgenes,base)8 kp, desc← FAST(img[i][ j])9 for k← 1 to tamano(kp) do

10 D[m]← [u, v, i, j,desc(k)]11 m = m+1

Como en la prueba anterior, lo primero es ubicarse frente ala alacena utilizando el Algoritmo 2 y deteniendo al robot a unadistancia de 50 cm.

Ubicado en la zona de busqueda realiza un paneo en el es-tante, y en cada imagen adquirida se realiza la busqueda de los 6objetos.

Copyright c© 2016 by AMRob

FIGURA 4. DETECCION DEL OBJETO EN ENTRENAMIENTO

Para ello se calculan los descriptores de la imagen actual conel algoritmo 8, donde i, j y obj son 0 para todas las caracterısti-cas, a continuacion se realiza el proceso de matching entre losdescriptores de la imagen actual y los almacenados en la basede datos, calculando la distancia euclidiana entre ambos descrip-tores y almacenandolos en una matriz de m× n, donde n es elnumero de caracterısticas encontradas en la imagen actual, comose muestra en la ecuacion 3.

dist =

|D[0]3−A[0]3| · · · |D[0]3−A[n]3|...

. . .|D[m]3−A[0]3| |D[m]3−A[n]3|

(3)

Algoritmo 9: Emparejamiento

1 Funcion matching(dist,U)Salida: match

2 d = 03 while d <U do4 d←min(dist)5 if d <U then6 agrega match(m,n,match)

De acuerdo a los resultados del matching al vector A se leagrega al elemento obj, el correspondiente valor del vector D. Secuentan la cantidad de caracterısticas de cada objeto. Si el totalde caracterısticas encontradas supera un umbral de matching (S)se establece que el objeto fue detectado. Y finalmente se realizala lista con los objetos que no se detectaron en ninguna imagen,en el algoritmo 10 se muestra el programa general.

5. PREPARACION DE LA COMIDAEsta tarea consiste en tomar un jitomate y colocarlo en un

recipiente cercano al jitomate, estos objetos se encuentran en unaposicion desconocida en la mesa a la derecha de la alacena.

Algoritmo 10: Lista de Compras

1 Void shopping list ()2 D←Cargar datos() img← obtener imagen()3 pose← pose robot(img, K,F ,10)4 mover robot(pose,50)5 for i← 1 to 9 do6 mover cabeza(i)7 img←obtener imagen()8 A←caracteristicas(img,0)9 dist←matriz distancias(D,A)

10 match←matching(dist)11 A← agrega num objeto(match,D)12 C←cuenta objetos(match, A)13 lista←agrega objetos detectado(C)

14 mostrar(lista)

Como en las demas pruebas es necesario llevar el robot a lamesa para ello se coloca la marca sobre la mesa y por medio delAlgoritmo 2 llevamos el robot hasta la mesa y le decimos que sedetenga 30 cm antes de ella.

Considerando que el jitomate es casi una esfera roja, aplica-mos el Algoritmo 11, para encontrar la posicion del jitomate conrespecto al robot.

Localizado el jitomate se procede a tomarlo, en esta ocasiony debido a que nuestro robot es la version de RoboCup no tienearticuladas las manos ni las munecas por lo que se trabajo en queel movimiento para agarra el jitomate fuera tomarlo con ambasmanos.Ya con el jitomate en las manos hay que mover el robothacia la posicion en la que se encuentra el recipiente tambienobtenida visualmente a una altura que libre dicho recipiente ysoltarlo.

Algoritmo 11: Busca Jitomate

1 Funcion jitomate (imagen)2 jitomate=centroide(imagen, red, ’circulos’)3 mueve brazo2(jitomate)4 mueve robot(0,30)5 mueve brazo3()

6. RESULTADOSEn esta competicion utilizamos al robot Nao version Robo-

cup, los programas se desarrollaron en Python [3] y con las li-brerıas de OpenCV [4]. Previo al concurso carecıamos del am-biente en donde se desarrollarıa la prueba, sin embargo se conta-ba con el ambiente virtual para el simulador Webots R© en el quese realizaran las pruebas correspondientes. Para el caso de lastareas de apagar la parrilla (seccion 3), para la lista de compras(seccion 4) y preparacion de la comida (seccion 5).

Copyright c© 2016 by AMRob

En la figura 5 se muestra la deteccion de la parrilla superior,esto en el simulador Webots

FIGURA 5. PRUEBA DE PARILLA ENCENDIDA

Para las pruebas correspondientes a la tarea de lista de com-pras se utilizaron 6 objetos de uso domestico, como una caja decereal, lata de cerveza, caja de te, botella de aceite, botella deagua, bolsa de cafe, con dichos objetos se realizaron las pruebasen el laboratorio, en la figura 6 se presenta una de las pruebas rea-lizadas en donde se aprecia que detecta los objetos cereal y cajade te, ası mismo la lista generada se presenta del lado izquierdode la imagen la lista generada por el programa.

FIGURA 6. LISTA DE COMPRAS

Se presentan en la figura 7 se muestra una fotografıa de latarea de preparacion de comida, en el evento Humanoids cha-llenger 2014. En la Tabla 1 se presentan los resultados de los 3primeros lugares de la competicion.

7. CONCLUSIONESLos resultados durante la competicion de los algoritmos im-

plementados fueron aceptables, dado que quedamos en Segundolugar, sin embargo la diferencia con el primer lugar fue mınimo

FIGURA 7. PREPARACION DE LA COMIDA

Team T1 T2 T3 Score

FIA ROBOTICS 18.56 5.83 - 24.39

DotMEX 16.06 8.14 - 24.20

UC3M RO-BOTICA - 7.56 - 7.56

CUADRO 1. RESULTADOS DE LA COMPETENCIA

y se debio al tiempo en que se realizaron las tareas. En donde setrataron satisfactoriamente problemas de localizacion, identifica-cion de objetos y manipulacion de objetos.

Referencias[1] J. Gao, Xiao-Shan; Hou, Xiao-Rong; Tang, Jianliang;

Cheng, Hang-Fei. Complete Solution Classification for thePerspective-Three-Point Problem, 2003.

[2] Gao Xiaoshan et all, New Algorithms for the Perspective-Three-Point Problem, Journal Computacional Sciences andTech. 2001.

[3] M. Lutz, Learning Python,O’Reilly,2013.[4] G. Bradaski, Learning OpenCV Computer Vision with the

OpenCV Library, O’Reilly, 2008.[5] E. Rosten and T. Drummond, Machine learning for high

speed corner detection, 9th Euproean Conference on Com-puter Vision, vol. 1, 2006, pp. 430–443.

[6] J. Zhao, L. Xue, G. Zue, Optimization Matching AlgorithmBased on improved Harris and SIFT,10thInternationalConference on Machine Learning and Cybernetics, vol. 1,2010, pp. 258 - 261.

[7] J. Ibarra, Apuntes del curso de Vision Artificial,Cinvestav.[8] M. Santoyo, J. Ibarra, Programming the Goalkeeper of 20

DOF for FIRA Cup, Intelligent Robotics Systems.[9] P. Vera, R. Carrillo, D. Canizo, J. Ibarra, ¿Que necesita un

robot humanoide para jugar Futbol?, Comrob, 2014.

Copyright c© 2016 by AMRob