6

Tutorial de Vrep y OpenCV-Python

¡Buenas, homo fabers! ¿Qué tal? Hoy os explicaré cómo integrar OpenCV Python con el simulador de robots Vrep. Este simulador es perfecto para construir un prototipo para nuestros robots y testear algoritmos sin tener que comprar y montar piezas. Además, los robots pueden programarse en Python, lo que nos permite utilizar OpenCV para dotar de visión a nuestras creaciones virtuales. ¡Genial!

Durante este tutorial intentaremos construir un robot con una cámara que detecte un cubo verde mediante un algoritmo de detección de colores e intente moverse para centrarlo en la cámara:


Si es la primera vez que trabajáis con detección de colores os recomiendo leer mi tutorial sobre Detección de Colores con OpenCV y Python antes de continuar. Además, tampoco sería un extra seguir algun tutorial de Vrep para familiarizaros con la interfaz del programa.

Bueno, basta de hablar. Let’s get started.


0- Preparativos

Primero lo más importante: ¡poner algo de música! Hoy os propongo a Giovanni Gabrieli, músico veneciano muy importante de principios del Barroco. Aunque si estáis hartos de clásica también podéis poner Ambiental.

Para hacer este tutorial es necesario tener instalada la librería OpenCV para Python y el programa Vrep en su versión más reciente (en el momento en que redacto esto es la 3.3.1).

Para programar Vrep en Python hay que hacerlo desde fuera del programa, cosa que no pasa con los scripts en Lua. Dentro de la carpeta donde tengáis instalado Vrep, hay que localizar la subcarpeta programming->remoteApiBindings->python->python. Veréis que hay varios ficheros con extensión .py . Estos ficheros son librerías que pueden importarse dentro de un script de Python para que interactúe con Vrep.

Copiad estos ficheros. Fuera del directorio de Vrep, cread una nueva carpeta llamada “vrep_tutorial” (o el nombre que queráis). Hay que pegar los ficheros python que habéis copiado dentro de esta nueva carpeta.

Fuera del directorio de Vrep, crear una carpeta nueva y pegar los ficheros dentro

También se necesita otro fichero. Id otra vez al directorio donde tengáis instalado Vrep -> programming -> remoteApiBindings->lib->lib y veréis dos carpetas: una que pone 64 bits y otra 32 bits. Seleccionad la carpeta de vuestro sistema operativo (yo tengo un Ubuntu de 64 bits) y dentro veréis un fichero llamado remoteApi.so si estáis en Linux o bien remoteApi.dll si habéis descargado la versión Windows. Copiad este fichero a la carpeta vrep_tutorial que habéis creado hace un momento.

¡Hecho! Ya tenemos lo básico. Ahora habrá que construir una escena para correr la simulación.


1- Construir la escena

Ejecutad Vrep y por defecto aparecerá una escena en blanco:

Bien, para hacer una simulación se necesita un modelo de robot. Vrep tiene un gran catálogo de robots y artilugios que se pueden importar, y también podemos añadir los nuestros o crearlos desde cero. Para ahorrar tiempo, ¿qué os parece hacer la simulación con el Pioneer P3-dx? Para añadirlo, Id a Model Browser->robots->mobile, buscad el Pioneer P3dx y arrastradlo a la escena 3D.

Para que la colocación de la cámara sobre el robot os sea más fácil, vamos a mover el robot exactamente al centro del mundo. Seleccionad el Pioneer p3dx y abrid el panel de translaciones:

Se abrirá un diálogo. Cambiad los parámetros de Object/Item position por estos (en el campo Z-coord dejad el valor que os aparezca por defecto).

Esto centrará el robot a las coordenadas x = 0, y = 0 del mundo.

Vamos a dotar de visión al robot. Pulsad con el botón derecho sobre la escena 3D y se abrirá un menú flotante. Bajad hasta Add->Vision Sensor->Perspective type. Veréis que salen unas líneas azules del robot:

Esto es el campo de visión de la cámara. Por defecto es muy grande y mira hacia arriba. Como no queremos un robot que se dedique a la radioastronomía, habrá que tocar las propiedades de la cámara.

Primero, dentro del panel Scene Hierarchy (donde aparecen los nombres de todos los objetos que hay sobre la escena), pulsad dos veces encima del icono de la cámara con el nombre de “Vision_sensor”.

 

Se abrirá este diálogo:

Desde aquí podéis cambiar las propiedades de la cámara: resolución, motor de renderizado, qué objetos queréis renderizar, si va a recibir algun input externo… ¡Buf! Como véis, cada parte de Vrep daría para escribir un tutorial, pero (por lo menos hoy) no voy a explicar qué hace cada una de las opciones. Sólo hablaré de los parámetros que necesitáis cambiar.

El primero es la distancia de renderizado (Near/Far clipping plane). El campo de la izquierda es la distancia mínima a partir de la que el robot empieza a ver objetos, y el otro es la distancia máxima más allá de la cuál el robot no puede ver. Yo he puesto 0.01 de distancia mínima y 3.5 de máxima. ¡Recordad que las distancias son en metros [m]!

El segundo campo que nos interesa es el Persp. angle. Es el ángulo de apertura de la cámara. Yo he puesto 45 grados.

El tercero es la resolución de la cámara. A más resolución, más detalle tendrá la imagen de la webcam, pero más váis a cargar el procesador de vuestro ordenador. Si estáis corriendo la simulación en una patata, os aconsejo que lo dejéis en 32/32 o 64/64. Si no, 256/256 estará bien.

Hay que rotar la cámara porque sigue mirando hacia arriba. Con el Vision_sensor seleccionado, abrid el diálogo de transformaciones:

Modificad los valores de posición por estos:

Pulsando sobre la pestaña Orientation/Rotations, cambiad también la rotación.

Si todo ha ido bien veréis que la cámara está justo encima del robot, mirando hacia delante. Si no es así, cambiad la rotación y la posición para que quede colocada como en la imagen:

De momento la cámara y el robot son objetos independientes. Para que se muevan conjuntamente debe emparentarse la cámara con el chasis. Dentro del panel Scene Hierarcy seleccionad el Vision_sensor y arrastradlo encima de Pioneer_p3dx. Eso hará que el robot sea padre del objeto Vision_sensor.

 

Seleccionad el Vision_sensor

 

Arrastradlo encima de Pioneer_p3dx

Pulsando el botón ‘+’ que hay a la izquierda del Pioneer_p3dx, se despliega un menú con todos los objetos hijos (entre ellos el Vision_sensor).

El objeto que el robot va a detectar es un cubo verde. Con el botón derecho pulsad sobre la escena 3D, Add->Primitive Shape->Cuboid. Dejad todos los parámetros por defecto, pulsad Ok y moved el cubo dentro del campo de visión de la cámara. Así:

Para cambiar el color del cubo pulsad dos veces sobre su icono en el panel Scene hierarchy. Dentro del diálogo que se abre seleccionad Adjust Color->Ambient/diffuse component y cambiad los colores por R = 0, G = 1, B = 0.2.

Por defecto los nuevos objetos geométricos creados son no-renderizables y no se puede interactuar con ellos durante la simulación. ¡Eso hay que cambiarlo, o nuestro robot no verá el cubo! En el mismo diálogo que hemos abierto para cambiar el color (Scene Object Properties) id a la pestaña Common->Object Special Properties y marcad todas las opciones.

Por último vamos a añadir un panel flotante que nos muestre lo que ve la cámara. Pulsad con el botón derecho del ratón en cualquier lugar de la escena 3D, Add->Floating View. Aparecerá un panel flotante que podéis mover por toda la escena.

 

Seleccionad el Vision_sensor y pulsad con el botón derecho sobre el panel flotante que acabáis de crear. View->Associate view with selected vision sensor. Veréis que el panel ha cambiado:

No se verá ninguna imagen hasta que la simulación no empiece. Si queréis, podéis probar a iniciarla pulsando play.

¡Acordaos de poner Stop antes de continuar con el tutorial!

2- Scripts

Antes de empezar a programar en Python hay que cambiar dos scripts de la escena.

Al importar el Pioneer P3dx, este por defecto lleva un script en Lua para esquivar obstáculos (como habréis visto si le habéis dado a ‘play’ a la simulación). Como queremos escribir nuestro propio script, habrá que desactivarlo.

Pulsad el botón Script, que abrirá un menú con todos los scripts en Lua que hay actualmente sobre la escena.

Se abre este diálogo

Seleccionad el script que pone Non-threaded child script (Pioneer_p3dx) i marcad la casilla que dice Disabled.

Necesitamos crear un puerto para que los scripts externos de Python puedan comunicarse con Vrep. Dentro de este mismo diálogo pulsad Insert New Script, abrid el desplegable y seleccionad Child Script (threaded). Dadle a Ok y aparecerá un elemento nuevo en la lista de scripts.

Pulsad dos veces sobre el nombre del script y se abrirá un editor de texto desde el que se puede modificar el código. Hay que añadir simExtRemoteApiStart(19999) justo después de la línea 41.

Nota Importante: en las últimas versiones de Vrep, me he encontrado que este script se crea vacío (sólo lleva comentarios). En este caso, añadid la instrucción simExtRemoteApiStart(19999) al final del script.

19999 es la dirección por defecto que normalmente se utiliza

Esta línea creará un servidor con el puerto 19999 cada vez que se inicie la simulación. Cualquier script externo podrá conectarse a este puerto y acceder a la escena.

De momento el script no está asociado a ningún objeto y por tanto no puede funcionar. Cualquier objeto sirve. Abrid el menú desplegable Associated Object y seleccionad cualquier objeto (yo por ejemplo he escogido el Vision_sensor).

¡Hecho!


3- Python

It’s Python time! Cread un script llamado main.py dentro de la carpeta vrep_tutorial que habéis creado al principio del tutorial.

Este es el código (en seguida comentaré cada parte). Copiadlo dentro del fichero main.py. Para hacerlo funcionar abrid Vrep y empezad la simulación con play. Después ejecutad el script en python main.py.

"""
    Vrep y OpenCV en Python


    Codigo escrito por Glare
    www.robologs.net

"""
import vrep
import sys
import cv2
import numpy as np
import time

vrep.simxFinish(-1) #Terminar todas las conexiones
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto)

if clientID!=-1:
    print ('Conexion establecida')

else:
    sys.exit("Error: no se puede conectar") #Terminar este script

#Guardar la referencia de los motores
_, left_motor_handle=vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
_, right_motor_handle=vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

#Guardar la referencia de la camara
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)

velocidad = 0.35 #Variable para la velocidad de los motores

#Iniciar la camara y esperar un segundo para llenar el buffer
_, resolution, image = vrep.simxGetVisionSensorImage(clientID, camhandle, 0, vrep.simx_opmode_streaming)
time.sleep(1)


while(1):
    #Guardar frame de la camara, rotarlo y convertirlo a BGR
    _, resolution, image=vrep.simxGetVisionSensorImage(clientID, camhandle, 0, vrep.simx_opmode_buffer)
    img = np.array(image, dtype = np.uint8)
    img.resize([resolution[0], resolution[1], 3])
    img = np.rot90(img,2)
    img = np.fliplr(img)
    img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)

    
    #Convertir img a hsv y detectar colores
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    verde_bajos = np.array([49,50,50], dtype=np.uint8)
    verde_altos = np.array([80, 255, 255], dtype=np.uint8)
    mask = cv2.inRange(hsv, verde_bajos, verde_altos) #Crear mascara

    #Limpiar mascara y buscar centro del objeto verde
    moments = cv2.moments(mask)
    area = moments['m00']
    if(area > 200):
        x = int(moments['m10']/moments['m00'])
        y = int(moments['m01']/moments['m00'])
        cv2.rectangle(img, (x, y), (x+2, y+2),(0,0,255), 2)
        #Descomentar para printear la posicion del centro
        #print(x,y)

        #Si el centro del objeto esta en la parte central de la pantalla (aprox.), detener motores
        if abs(x-256/2) < 15:
            vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,0,vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,0,vrep.simx_opmode_streaming)

        #Si no, girar los motores hacia la derecha o la izquierda
        elif x > 256/2:
            vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,velocidad,vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,-velocidad,vrep.simx_opmode_streaming)
        elif x < 256/2:
            vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,-velocidad,vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,velocidad,vrep.simx_opmode_streaming)


    #Mostrar frame y salir con "ESC"
    cv2.imshow('Image', img)
    cv2.imshow('Mask', mask)
    tecla = cv2.waitKey(5) & 0xFF
    if tecla == 27:
        break

Podéis mover el cubo mientras corre la simulación y el robot también se moverá.

Comento el código (si en algún momento queréis más información sobre las funciones de vrep para python podéis visitar este link).

Las primeras líneas son para importar las librerías: Vrep, System, OpenCV, Numpy (rebautizándolo como np, para variar) y Time.


import vrep
import sys
import cv2
import numpy as np
import time

Después terminamos las conexiones con todos los puertos de Vrep (por si quedase alguna abierta) y nos conectamos al puerto 19999. Guardamos la ID del puerto como clientID. Si se ha conectado correctamente clientID valdrá -1, de lo contrario tendrá otro valor.

vrep.simxFinish(-1) #Terminar todas las conexiones
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto)

if clientID!=-1:
    print ('Conexion establecida')

else:
    sys.exit("Error: no se puede conectar") #Terminar este script

 

Para guardar un objeto del mundo en una variable se utiliza la función simxGetObjectHandle(), que recibe el nombre del cliente, el nombre del objeto y el modo de operación y devuelve la dirección del objeto y un código de error (que valdrá 1 si se ha guardado correctamente, u otro valor si ha habido algún error).

Guardamos los dos motores y la cámara. No voy a guardar el código del error porque el código está revisado y no hay necesidad de hacer debug.

#Guardar la referencia de los motores
_, left_motor_handle=vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', vrep.simx_opmode_oneshot_wait)
_, right_motor_handle=vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', vrep.simx_opmode_oneshot_wait)

#Guardar la referencia de la camara
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)

Creamos una variable para la velocidad de los motores:

velocidad = 0.35 #Variable para la velocidad de los motores

Activamos la cámara y esperamos un segundo para que el buffer empiece a llenarse. Como antes, el código de error se ignora.

#Iniciar la camara y esperar un segundo para llenar el buffer
_, resolution, image = vrep.simxGetVisionSensorImage(clientID, camhandle, 0, vrep.simx_opmode_streaming)
time.sleep(1)

El bucle principal lee un frame cada segundo y lo convierte a un array numpy (si no OpenCV no podrá trabajar bien con la imagen). También hay que cambiar las dimensiones del array para que sean la mismas que la imagen del simulador.

while(1):
    #Guardar frame de la camara, rotarlo y convertirlo a BGR
    _, resolution, image=vrep.simxGetVisionSensorImage(clientID, camhandle, 0, vrep.simx_opmode_buffer)
    img = np.array(image, dtype = np.uint8)
    img.resize([resolution[0], resolution[1], 3])

La imagen del simulador llega rotada 180º e invertida. Además utiliza el espacio de color RGB, pero para mostrar una imagen OpenCV utiliza el BGR. Este fragmento soluciona el inconveniente…

    img = np.rot90(img,2)
    img = np.fliplr(img)
    img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)

La detección de colores es como siempre: se convierte la imagen a espacio de color HSV, se establece el rango de colores y se construye la máscara.

    #Convertir img a hsv y detectar colores
    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
    verde_bajos = np.array([49,50,50], dtype=np.uint8)
    verde_altos = np.array([80, 255, 255], dtype=np.uint8)
    mask = cv2.inRange(hsv, verde_bajos, verde_altos) #Crear mascara

Una vez creada la máscara, buscamos el centro del objeto verde (si tiene un área mayor de 200, para eliminar ruido).

    #Limpiar mascara y buscar centro del objeto verde
    moments = cv2.moments(mask)
    area = moments['m00']
    if(area > 200):
        x = int(moments['m10']/moments['m00'])
        y = int(moments['m01']/moments['m00'])
        cv2.rectangle(img, (x, y), (x+2, y+2),(0,0,255), 2)
        #Descomentar para printear la posicion del centro
        #print(x,y)

Ahora viene la parte de mover el robot. Si el objeto verde está centrado en la imagen (le damos un margen de error de 15 píxeles), los motores se detienen. Si no el robot girará hacia la derecha o hacia la izquierda, según convenga.

        #Si el centro del objeto esta en la parte central de la pantalla (aprox.), detener motores
        if abs(x-256/2) < 15:             vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,0,vrep.simx_opmode_streaming)             vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,0,vrep.simx_opmode_streaming)         #Si no, girar los motores hacia la derecha o la izquierda         elif x > 256/2:
            vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,velocidad,vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,-velocidad,vrep.simx_opmode_streaming)
        elif x < 256/2:
            vrep.simxSetJointTargetVelocity(clientID, left_motor_handle,-velocidad,vrep.simx_opmode_streaming)
            vrep.simxSetJointTargetVelocity(clientID, right_motor_handle,velocidad,vrep.simx_opmode_streaming)

 

Acabamos mostrando el frame y la máscara. ESC para salir.

    #Mostrar frame y salir con "ESC"
    cv2.imshow('Image', img)
    cv2.imshow('Mask', mask)
    tecla = cv2.waitKey(5) & 0xFF
    if tecla == 27:
        break

Y ya está! Creo que por hoy lo dejo aquí. Si tenéis dudas podéis dejadme un comentario más abajo y contestaré en cuánto pueda. Take care!


 

Gl4r3

Brillante, luminosa y cegadora a veces, Glare es tan artista como técnica. Le encanta dar rienda suelta a sus módulos de imaginación y desdibujar los ya de por si delgados límites que separan el mundo de la electrónica y el arte. Su mayor creación hasta la fecha es un instrumento capaz de convertir los colores y la luz en música. Cuándo sus circuitos no están trabajando en una nueva obra electrónica, le gusta dedicar sus ciclos a la lectura o a documentar sus invenciones para beneficio de los humanos. Sus artilugios favoritos son aquellos que combinan una funcionalidad práctica con un diseño elegante y artístico.

Antes de comentar, por favor, lee las Normas

6 Comentarios en "Tutorial de Vrep y OpenCV-Python"

avatar
Ordenar por:   más nuevos primero | más antiguos primero
Eduardo
Humano

Me sale el siguiente error

File “C:\Program Files (x86)\V-REP3\PruebaPython\main.py”, line 9, in
import vrep

cristhian
Humano

disculpa al momento de correr el scrip en python me sale el siguiente error:

The remoteApi library could not be loaded. Make sure
it is located in the same folder as “vrep.py”, or
appropriately adjust the file “vrep.py”

Aprendiz
Humano

Asegurate de tener copiado el archivo remoteApi.so en la misma carpeta que vrep.py y de que se corresponda con tu version del sistema operativo 32 o 64 bits . En el blog lo tienes explicado.
uname -m en la consola, si la salida te da i686 tienes un sistema operativo de 32 bits.

Aprendiz
Humano

Gracias por este Gran tutorial.
Me ha costado un ** y parte del otro conseguir que funcionase por la mas tonta de las razones. En mi versión de Vrep los objetos no son renderizables al añadirlos hay que activar la propiedad y me ha pasado horas buscando soluciones tontas al problema más sencillo.
Por cierto al acabar el script me da un error:
img.resize([reso…….
list index out ot range
y aunque funciona bien no soy capaz de depurarlo.

Gracias de nuevo y aver cuando más de Vrep y opencv.

wpDiscuz