Semana 28. Creando la librería GreenNaoLib

Las tareas de esta semana eran bastantes y muy variadas:

• Eliminar las ondas del editor ya que eran inútiles
• Ver los límites de la velocidad lineal de NAO, para poner un valor mínimo y un máximo
• Hacer que NAO andase a más distancia
• Medir cuantitativamente las desviaciones a la hora de caminar
• Convertir mi nodo ros en una librería para python
• Leer un par de papers que me envió mi tutor
• Hacer andar a NAO en arco, sin tener en cuenta ningún tipo de parametrización
• Una vez hecho lo anterior, parametrizar V y W

Eliminar las ondas del editor

Esto no supuso ningún esfuerzo o dificultad, ya que solo era eliminar las partes del código relacionadas con las ondas. Una vez hecho éstas no aparecían al abrir el editor, ni al guardar los jsons.

Hacer que NAO ande a más distancia

Para esto simplemente hice que la secuencia de movimeinto se repitiese 3 veces, con un bucle for

Medir cuantitativamente las desviaciones a la hora de caminar

Para esta tarea, una vez nao caminando más distancia, lo puse a caminar a dstintas velocidades y comparé su posición incial y su posición final:

Velocidad 0.5

Posición incial

Posición final

Velocidad 1

Posición incial

Posición final

Velocidad 2

Posición incial

Posición final

Dar un mínimo y un máximo a la velocidad lineal de NAO

Para esto estuve probando valores hasta ver que o bien no se podía bajar más (mínimo), o bien no se podía subir más (máximo). Con lo que llegué a la conclusión de que la velocidad mínima sería de 0.35:

Y la máxima de 4.35:

Como se puede ver, aún resbala, pero eso lo investigaré más adelante.

Convertir el nodo ROS2 en una libreía para python

De esta tarea me sorprendió su sencillez, ya que creía que iba a ser muy complicada porque un nodo ROS2, aunque esté en python, se ejecuta de manera distinta que un programa normal de python (ros2 run package node vs. python3 programa.py). Sin embargo, el proceso fue de lo más sencillo. Ya que simplemente debía eliminar el main de mis nodos, y añadir rclpy.init() al principio, después, sólo era importar y llamar a las diferentes clases del nodo en un programa python normal y corriente:

import GreenNaoLib

GreenNaoLib.SetV(1.5)
GreenNaoLib.Interpreter("say_hi_L.json")

Después, sin ser necesario compilar con colcon, ni hacer source, simplemente lo lanzamos con python3, después de lanzar con ROS2 la simulación:

Como se puede ver, ahora podemos lanzar tanto el modo de caminar como el intérprete de movimientos muy cómodamente.

Mover a NAO en arco fijo

Para esta tarea, lo que estuve intentado fue cambiar el valor de ambos HipYawPitch del fichero de caminar, ya que estas estaban siempre a 0, y, como se explicó en la entrada de blog anterior, este movimiento es necesario para poder caminar en arco. Necesitaba hacer el arco en las 2 direcciones, a la izquierda y a la derecha. Pero comencé por el giro a la izquierda:

• Lo primero que intenté fue dejar LHipYawPitch a un valor fijo y RHipYawPitch a 0 pero no funcionó.
• Después probé a mezclar el LHipYawPitch del patrón “turn_left_40.csv” con el de caminar normal, y dejar RHipYawPitch a 0, pero tampoco funcionó
• Probé tambien a poner los valores de ambos HipYawPitch en los mismos valores que los HipRoll, pero con el signo inverso, cosa que funciono para los arcos a la izquierda, pero no para los de la derecha, en los cuales dejé el signo igual:

Cosa que no entendía por qué era así, pero bueno, una vez con el arco a la izquierda, me di por satisfecha, ya que aunque no tuviera giros a la derecha, ya podía paramerizar la velocidad angular. Así que eso hice.

Mover a NAO en arco parametrizable con V y W

Cómo se vió tambien en la entrada de blog anterior, la velocidad angular se parametriza de igual forma que la lineal, por lo que dejé preparada la función para ello, setW, la cual depende de encontrar el patrón de movimiento adecuado para andar en arco, el cual seguía buscando. Una vez encontrado el de los giros a la izquierda, probé la función setw y me di cuenta de que los límites debían ser distintos a setV, pro el cómo se comportaba, así que probé, y quedé en el que el mínimo sería 0.35 y el máximo 1.9:

Una vez parametrizado esto, volví a buscar el arco hacia la derecha, no sin antes cambiar la funcion de andar en arco para que girase a la izquierda si la W era negativa.

Giro a la derecha

Esto lo conseguí haciendo las siguientes modificaciones al fichero de arcos a la izquierda:

1. Dejé iguales los HipYawPitch
2. Intercambié los valores de RXPitch y LXPitch
3. Invertí los signos de los XXRoll

Y, cómo ya tenía parametrizada la W para la izquierda, no tuve que hacer nada más que probar que todo fuera correctamente:

Y, cómo se puede ver, todo va bien, excepto porque sigue habiendo problemas con el rozamiento del suelo.

Leer papers

Esta fue sin duda la tarea más fácil de todas, ya que simplemente era leer los papers, lo cuales trataban sobre soluciones a un editor de movimientos llamado KME, que es bastante similar al editor que tengo yo, pero explicado de forma más completa y profesional, para tener un par de referencias de cara a la memoria y saber cómo explicar adecuadamente mi editor.

EXTRA: Creando una funcion que lee el IMU y levanta a NAO si es necesario

Como bien dice en el título, me tomé la libertad de crear la función WakeUp, la cual se suscribe al imu, y, dependiendo de cómo esté NAO (boca arriba o boca abajo), llama a los movimientos necesarios para levantarlo, dejo el resultado a continuación:

Que, como se puede ver, funciona perfectamente, aunque esta versión no devolvía la orientación, simplemente “actuaba por su cuenta”, así que modifiqué todo para que en lugar de llamar a esta clase y ya está, se llamase a esta clase SOLO para leer la orientación de NAO, para que luego el usuario pudiera programar el comportamiento como quisiera, de esta forma:

import GreenNaoLib

orientation = GreenNaoLib.get_face()

if orientation == "face normal":
    # acción 1

elif orientation == "face up":
    # acción 2

elif orientation == "face down":
    # acción 3

else:
    # acción 4

Una vez implementado, me di cuenta de que necesitaría las acciones por hacer, así que decidí hacer lo explicado a continuación:

EXTRA 2: CReando funciones para patrones fijos de movimento

Aprovechando que había cogido práctica en esto de las librerías, me puse a hacer funciones para describir cada patrón de movimiento de NAO, para que así tuviera muchas funcionalidades, dejo una lista de las funciones que hay y una descripción de lo que hacen:

• wakeup_face_down(): Hace que Nao se levante desde cubito prono
• wakeup_face_up(): Hace que Nao se levante desde cubito supino
• stand_still(): Hace que Nao se quede en la posición estándar, de estar quieto
• say_hi(hand): Hace que Nao salude con la mano que se le pasa como argumento
• side_step(side): Hace que Nao ande de lado en la dirección que se le pasa com argumento
• turn(side, degrees): Hace que Nao gire en el sentido que se le pasa como primer argumento, los grados que se le pasan como segundo argumento
• go_backwards(): Hace que Nao retroceda un par de pasos

Ahora, mi librería GreenNaoLib tenía bastantes cosas, dando al usuario muchas cosas que hacer con Nao a la hora de programarlo, sin preocuparse absolutamente nada de ROS2, nada más que lanzar el simulador.

EXTRA 3: Tomando V negativa

Después de hacer las funciones para patrones fijos, me di cuenta de que para ir andando marcha atrás era mejor parametrizar de igual modo que hacia adelante la velocidad, así que borré la función go_backwards() y acabé haciendo que Nao camine hacia atrás si se le pasa una velocidad negativa a la función setV, con el mismo máximo y mínimo que hacia adelante (en este caso -0.35 mínimi y -4.35 máximo):

Después modifiqué el código para que andase el mismo número de pasos (30) tanto hacia adelante como hacia atrás.

EXTRA 4 : Parametrizando el número de pasos

Me pareció una buena práctica parametrizar también opcinalmente el número de pasos, así que, teniendo en cuenta que las caminatas son de 10 pasos, excepto marcha atrás, que son 2, parametricé de tal manera que el mínimo para las caminatas fuera de 10, y para ir marcha atrás fuera de 2, y luego repetir la secuencia x/10 veces (hacia adelante) o x*5 veces (hacia atrás), así también se puede controlar la distancia que quieres recorrer, y no sólo la velocidad a la que quieres hacerlo, como es un aparámetro opcional, si no lo modificas se queda en 10, que será nuestro estándar, tanto hacia adelante como hacia atrás:

EXTRA 5: Piner al día el fichero README.md

Como ya había avanzado muchísimo en el desarrollo del tfg, tenía algunas cosas que podía rellenar en el fichero README.md, así que eso hice, para que quedase más vistoso y explicativo.