Semana 27. Parametrizando y generalizando la caminata

Esta semana era bastante importante, ya que casi todo el peso y dificultad principal de este TFG se abordaría aquí, ya que mis tareas principales eran:

• Ajustar los tiempos en los movimientos para que queden suaves y naturales
• Hacer que NAO andase recto ajustando los rozamientos del mundo
• Solucionar cierta fatiga detectada la semana anterior (explicaremos esto más adelante)
• Caminar recto a distintas velocidades
• Conseguir caminar con velocidad angular para ahcer arcos

También quería matizar algunas cosas, así que me propuse las siguientes tareas secundarias:

• Arreglar por completo el editor para dejarlo zanjado
• Levantar a NAO desde cúbito supino

TAREAS PRINCIPALES

Solucionar rozamientos para que NAO ande recto y suavizar la fatiga

Comencé por esta tarea ya que me parecía la más sencilla, ya que llevaba con ella bastante tiempo, por lo que era sólo continuar hasta dar con los parámetros de rozamiento adecuados, tanto en el mundo como en los pies de NAO.

Para hacer esto, simplemente era editar los ficheros sdf del NAO y el mundo y ver qué combinación daba mejores resultados.

Como mientras estaba probando rozamientos tuve muchos casos de fatiga (NAO no responde a las publicaciones en los topics, como si tuviese las articulaciones bloqueadas, ejemplo en este vídeo):

Decidí tirarme de cabeza a solucionar este problema, cosa que conseguí añadiendo una calidad de servicio a mi programa intérprete de movimientos:

qos_profile = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_ALL,
    depth=1000
)

Cosa que solucionó bastante el tema de la fatiga y me daba muchas ejecuciones antes de que ocurriese de nuevo, por lo que decidí dejarlo así, ya que no era incómodo como antes y de seguro en la aplicación final no llegará a sobrecargarse de esa manera, ya que para probarlo lo ejecuté muchas veces seguidas. Dejo a continuación cómo funcionaba cuando lo arreglé, sin aun solucionar los rozamientos, pero a punto:

Como se puede haber, sigue habiendo fatiga, pero con menor frecuencia y menores consecuencias.

Una vez solucionada la fatiga, continué con el arreglo de los rozamientos.

Cuando lo conseguí, me di cuenta qde que la calidad de servicio no había solucionado tanto como yo creía, ya que tenía muchas cosas abiertas, y se seguía fatigando, por lo pensé que la fatiga se debía a eso y yo no tenía control sobre ello. Por lo que decidí seguir investigándo a ver si encontraba algo al respecto.

En cuanto a esto, dejo aquí una prueba de NAO andando recto y el tema de la fatiga:

Tras un rato de investigación, me di cuenta de que el error estaba en que en el código del intérprete de movimeintos ocurría lo siguiente:

if self.file_name.endswith(".csv"):
    for fotograma in self.datos:
        counter = counter + 1
        tiempo_actual = float(fotograma["#WEBOTS_MOTION"])
        fotograma["tiempo_de_duracion"] = tiempo_actual - tiempo_anterior
        tiempo_anterior = tiempo_actual
        
        # FUENTE DEL FALLO:
        for articulacion in fotograma:
            if articulacion != "#WEBOTS_MOTION" and articulacion != "V1.0":
                self.art_publishers[articulacion] = self.create_publisher(Float64, f'/{articulacion}/cmd_pos', qos_profile)

else:
    for fotograma in self.datos:
        counter = counter + 1
        tiempo_actual = fotograma["tiempo"]
        fotograma["tiempo_de_espera"] = tiempo_actual - tiempo_anterior
        tiempo_anterior = tiempo_actual
        
        # FUENTE DEL FALLO:
        for articulacion in fotograma["articulaciones"]:
            nombre = articulacion["articulacion"]
            self.art_publishers[nombre] = self.create_publisher(Float64, f'/{nombre}/cmd_pos', qos_profile)

Lo que hacía que, en lugar de crearse un solo publicador por articulación, se crearan vete tu a saber cuantos, pero demasiados, ya que se ejecutaba la creación de TODOS los publicadores por cada fotograma, cosa que sobrecargaba muchñisimo al sistema. Cosa que arreglé de la siguiente forma, con un sencillo contador, para que sólo se ejecutase una vez el segundo bucle for:

if self.file_name.endswith(".csv"):
    for fotograma in self.datos:
        counter = counter + 1 # SOLUCIÓN AL FALLO
        tiempo_actual = float(fotograma["#WEBOTS_MOTION"])
        fotograma["tiempo_de_duracion"] = tiempo_actual - tiempo_anterior
        tiempo_anterior = tiempo_actual
        
        # SOLUCIÓN AL FALLO
        if counter == 1:
            for articulacion in fotograma:
                if articulacion != "#WEBOTS_MOTION" and articulacion != "V1.0":
                    self.art_publishers[articulacion] = self.create_publisher(Float64, f'/{articulacion}/cmd_pos', qos_profile)

else:
    for fotograma in self.datos:
        counter = counter + 1 # SOLUCIÓN AL FALLO
        tiempo_actual = fotograma["tiempo"]
        fotograma["tiempo_de_espera"] = tiempo_actual - tiempo_anterior
        tiempo_anterior = tiempo_actual
        
        # SOLUCIÓN AL FALLO
        if counter == 1:
            for articulacion in fotograma["articulaciones"]:
                nombre = articulacion["articulacion"]
                self.art_publishers[nombre] = self.create_publisher(Float64, f'/{nombre}/cmd_pos', qos_profile)

Y, después de esto, junto a la calidad de servicio que puse, no me volvió a ocurrir la fatiga:

Sin embargo, como se puede observar, me dejó de caminar recto, así que de nuevo me puse a arreglarlo.

Tras un buen rato de pruebas, llegué a este resultado, que me pareció bastante decente:

Como estaba más o menos estable, decidí ar esto por terminado de momento y centrarme en las siguientes tareas, y, si era necesario, matizarlo un poco más en el futuro.

Ajustar tiempos para que los movientos queden suaves y naturales

Como se ve en los vídeos anteriores, los movientos ya son bastante suaves, por lo que decidí dejarlo así.

Probar distintas velocidades para caminar recto

Para esta tarea, recordé lo que me dijo mi tutor: “Una posible solución para esto es cambiar el timing”. Así que eso hice.

Probé 2 cosas muy simples, multiplicar los tiempos de cada fotograma por 2 para ir más lento, y dividir todos los tiempos entre 2 para ir más rápido.

Esto no fue nada complicado, ya que libreOffice interpreta los ficheros .csv como libros de cálculo, por lo que simplemente aplicando las fórmmulas adecuadas, tuve esto hecho en nada de tiempo.

A la hora de probar mis nuevos ficheros de movimiento, obtuve lo siguiente:

Lo que quiere decir que cambiar el timing de manera proporcional (multiplicar o dividir en la misma cantidad todos los timings), es decir, escalar los timings, es un método efectivo para cambiar la velocidad al NAO, la clave está en encontrar unos buenos valores.

Cosa que me alegró bastante, ya que eso significaba que para el código de andar, necesitaría sólo 1 parámetro para cambiar la velocidad, y este es el factor de escala, el número adecuado, o bien pensar en unos valores determinados para luego escalar de nuevo, y que la velocidad tenga un mínimo y un máximo.

A lo que me refiero con esto, mi idea era crear un fichero, walk.py, el cual sería un intérprete SÓLO para el patrón de caminar, y recibiría un argumento, V, el cual sería el valor a multiplicar por el tiempo, por ejemplo, 0.5(para ir más rápido) o 2 (para ir más lento). Pero, también me di cuenta de esto era contraintuitivo, ya que, si una persona indica “velocidad 0.5”, entiende que el robot se moverá más lento, y si indica, “velocidad 2”, entiende que se moverá más rápido, por lo que sería necesaario crear una interfaz amigable para el usuario, para que esas “normas de intuición” se cumpliesen. Por lo que sería necesario reescalar el número de entrada, de modo que, si entra un 0.5, habría que “convertirlo” en un 2, para que la velocidad se reduzca a la mitad, en lugar de aumentar. De manera análoga, habría que hacer que un 2 “se conviertese” en un 0.5, para que la velocidad aumente.

Para entender mejor esto, dibujé el siguiente esquema:

Tras un día de pensarlo, me di cuenta de que no hacía falta reescalar nada, ya que simplemente, en lugar de multiplicar siempre el tiempo, si lo dividía siempre no tendría el problema de “la contraintuición”, ya que, matemáticamente, dividir entre 0.5 da el mismo resultado que multiplicar por 2, y tendríamos un movimiento más lento, y, al dividir entre 2 directamente, ya tendríamos el movimiento rápido.

Así que, me pensé que sería buena práctica crear una librería para python llamada GreenNaoLib, dónde estarían todas las funciones relacionadas con el manejo de NAO en este proyecto. Aunque, por el momento, sólo creé un programa que coge la velocidad como argumento, ya que no tenía claro cómo hacer lo de la librería y quería preguntarle a mi tutor en la siguiente reunión. Dejo el resultado del programa a continuación:

También, como se puede ver en vídeo, se creó por fin el paquete de ros dónde se alojaría el resultado final del proyecto “grennao”.

Caminar en arco

Para caminar en arco, busqué información sobre la caminata de NAO, hasta que encontré el siguiente enlace, dónde se ve claramente que para poder hacer arcos, hay que rotar los pies.

Sin embargo, NAO no tiene un movimiento “AnkleYaw” (girar la punta del pie directamente), sólo tiene “AnklePitch” (levanta o baja la punta del pie) y “AnkleRoll” (rota hacia fuera y hacia adentro). Por lo que este movimiento sólo es posible utilizando “HipYawPitch”, se demuestra esto utilizando la demo predeterminada de gazebo para mostrar el movimiento de NAO:

Cómo se puede ver, ésta es la rotación del tobillo que buscamos, sin embargo, hay un pequeño problema, y es que este movimiento levanta la pierna del suelo, lo que produce inestabilidad:

Por lo que había que buscar una forma de compensar ese movimiento para que NAO no cayese en el intento.

Además, había que tener en cuenta que este movimiento sería más o menos amplio (más o menos giro de tobillo) dependiendo del parámetro W, de manera análoga a la V vista en el apartado anterior de este blog.

Aunque, por probar, hice exactamente lo mismo que para la velocidad lineal, pero, en vez de en el patrón de la caminata, en el patrón de girar 180 grados que tenía, el resultado fue el siguiente:

Y, ocurría lo mismo que en el anterior, giraba más o menos lento dependiendo del tiempo, si lo multiplicas por 2 (como en el video), va más lento, si lo divides, va mas rápido, solucionando el problema de la velocidad angular. Sin embargo, mi trabajo era juntar todo para que el robot pudiera andar en arco, es decir, debía girar y a la vez caminar recto, de modo que si le pasas velocidad angular 0, obtienes un movimiento recto.

TAREAS SECUNDARIAS

Zanjar el editor

Para dejar zanjado el editor, me di cuenta de que al principio, si movías muy rápido el slider, la articulación hacía muchísima fuerza y NAO salía disparado, además de todo el tema del peso de los pies.

Así que, lo que hice fue cambiar lo de la fuerza para que fuera constante, igual que en el NAO, que está a 0.2 aproximadamente, y lo cuadré para que la velocidad de la articulación en el editor fuera más o menos igual. Para ellos, sólo tuve que añadir maxvelocity=X en las lecturas del slider:

p.setJointMotorControl2(model,27, p.POSITION_CONTROL, targetPosition=R_hip_roll_value, maxVelocity=2)

Después, puse los pies con un pco más de peso (2.16184 concretamente). Y obtuve lo siguiente:

Cosa que da muchísima más coherencia a la hora de crear movimientos, no es exacto, pero está muchísimo mejor que antes y el NAO “real” replica los movimientos de forma mucho más exacta.

Levantar a NAO de cúbito supino

Una vez con el editor coherente, pude hacer que NAO se levantase desde cúbito supino, simplemente haciendo que se diera la vuelta y después levantándolo de cúbito prono.

Cabe destacar que hay veces (como la del vídeo) que hay que insistir un poco con el movimiento de cubito prono, pero al final siempres se levanta.

Con lo que podemos concluir que esta semana terminé las tareas secundarias.

Después de eso, ordené el directorio del editor de movimientos para tener por separado las cosas ya que qntes estaban todas juntas y era un poco tedioso distinguir los patrones creados de otras cosas.