Semanas de la 14 a la 18. Haciendo útil a NAO

Las tareas de estas semanas eran cortas, pero intensas, ya que debía incluirle una cámara a NAO, además de arreglar la forma de caminar que tenía, ya que, como se vió en la entrada anterior, era muy imperfecta.

Añadiendo la cámara

Añadir la cámara era una tarea sencilla, pero a mí se me hizo bastante cuesta arriba, ya que, lo único que tenía que hacer era añadir esto al modelo del NAO:

<link name="camera_rgb_frame">
    <inertial>
       <pose>0.15 0 0.25 0 0 0</pose>
       <inertia>
       <ixx>0.001</ixx>
       <ixy>0.000</ixy>
       <ixz>0.000</ixz>
       <iyy>0.001</iyy>
       <iyz>0.000</iyz>
       <izz>0.001</izz>
       </inertia>
       <mass>0.035</mass>
    </inertial>
    <pose>0.15 0 0.25 0 0 0</pose>
    <sensor name="camera" type="camera">
       <always_on>true</always_on>
       <visualize>true</visualize>
       <update_rate>30</update_rate>
       <topic>NAO/camera/image_raw</topic>
       <gz_frame_id>camera_rgb_frame</gz_frame_id>
       <camera name="intel_realsense_r200">
       <camera_info_topic>NAO/camera/camera_info</camera_info_topic>
       <horizontal_fov>1.02974</horizontal_fov>
       <image>
       <width>1920</width>
       <height>1080</height>
       <format>R8G8B8</format>
       </image>
       <clip>
       <near>0.02</near>
       <far>300</far>
       </clip>
       <noise>
       <type>gaussian</type>
       <!-- Noise is sampled independently per pixel on each frame.
       That pixel's noise value is added to each of its color
       channels, which at that point lie in the range [0,1]. -->
       <mean>0.0</mean>
       <stddev>0.007</stddev>
       </noise>
       </camera> 
      </sensor>
    </link>
    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
    	<render_engine>ogre2</render_engine>
    </plugin>

Pero, la parte del plugin tardé unos días en encontrarla, por lo que los topics de la cámara no aparecían en mi terminal a la hora de listar todos los topics, cosa que se me hizo cuesta arriba, y no fue hasta que un compañero de clase me enseñó un modelo de sensor en un modelo de robot que me di cuenta de este “código faltante”.

Una vez añadido eso correctamente, hice el nuevo bridge necesario en el launcher y ya éramos capaces de ver lo que NAO veía,o eso era lo que yo creía, ya que la cámara aparecía cómo topic y podíamos trabajar con él, pero, al probar con rviz a ver qué imgaen era la que obtenía, no visualizaba nada, sólo un espacio blanco.

Tras un buen rato de mirar todas las posibilidades, me di cuenta de que la cámara estaba cayendo por el infinto, ya que en su información de posición la coordenada z no dejaba de descender, cosa que hizo que me diese cuenta de que no había añadido un joint a la cabeza del nao y la cámara, así que, añadí lo siguiente a mi modelo:

<joint name="head_camera_joint" type="fixed">
    <parent>Head</parent>
    <child>camera_rgb_frame</child>
    <pose>0.15 0 0.25 0 0 0</pose>
</joint>

Una vez añadido este joint fixed (para que no se moviese), por fin las coordenadas de la cámara eran fijas, por lo que decidí ponerle un cubo delante a NAO y, con rviz, visualizar lo que NAO estaba viendo, dejo el resultado a continuación:

Cosa que me alegró bastante ya que por fin tenía la cámara del nao funcionando, y, por probar, decidí ponerle a andar un poco (aún sin arreglar la manera en la que camina) para ver cómo se comportaba la imagen, cosa que fue de la siguiente manera:

Arreglando la caminata

Para arreglar la caminata, decidí dejar de lado el crador de jsons para el moviento y dar paso a una neva técnica: Las ondas sinusoidales.

Pero, antes de eso, me tocó arreglar un poco el escenario ya que me di cuenta de que la fricción del suelo estaba demasiado alta como para considerar un suelo “normal” y la bajé a 0.9, también modifiqué las franjas amarillas del suelo ya que estaban definidas como cajas y entorpecían al robot, cosa que solucioné definiéndolas como plano.

Una vez arregladas esas 2 cosas, estábamos listos para implementar el seno.

Para ello, lo que hice fue conservar del código anterior la lectura del json, para poder crear los publicadores para cada una de las articulaciones, después, subí el número de argumentos a 3, para cambiar el número de pasos (secuencia de 2 pasos en realidad), la amplitud de la onda y el tiempo entre fotogramas, para así poder ajustar de forma más cómoda mi walking gate.

Después, el código es bastante sencillo, tenemos 3 funciones, una para inicializar los publicadores (init(self)), cómo de costumbre, otra para efectivamente poder publicar los mensajes para caminar (publish_message(self)) y una última para volver a la posición de parado (stand_still(self)), la cual se da leyendo el fichero stand.json y volcando los movimientos de éste en el robot, cómo hacíamos hasta ahora con los jsons.

En cuanto a la función publish_message(self), el código es el siguiente (para poderlo explicar mejor, las otras funciones las obviamos porque son lo que hemos estado haciendo hasta ahora en otras pruebas):

def publish_message(self):
    msg = Float64()
    step_time = self.tiempo  # Tiempo entre fotogramas
    step_amplitude = self.ampli  # Ajusta la amplitud del paso
    step_frequency = 2 * np.pi / (self.reps * 2)  # Frecuencia para una oscilación suave
    i = 0
    for repetition in range(self.reps):
        for idx, fotograma in enumerate(self.datos):
            i = i + 1
            time.sleep(step_time)
        
            phase = idx * step_frequency  # Determinar fase de la onda
            sinusoidal_offset = step_amplitude * np.sin(phase)  # Desplazamiento sinusoidal
        
            for articulacion in fotograma["articulaciones"]:
                nombre = articulacion["articulacion"]
                base_position = articulacion["posicion"]
            
                # Alternar entre piernas correctamente
                if "RHipPitch" in nombre or "RKneePitch" in nombre or "RAnklePitch" in nombre:
                    msg.data = base_position + sinusoidal_offset
                elif "LHipPitch" in nombre or "LKneePitch" in nombre or "LAnklePitch" in nombre:
                    msg.data = base_position - sinusoidal_offset  # Oposición de fase para la otra pierna
                else:
                    msg.data = base_position  # Mantener otras articulaciones sin cambios
            
                self.art_publishers[nombre].publish(msg)
        
            self.get_logger().info(f'Fotograma {i}')

    # Volver a posición de reposo
    self.stand_still()

Cómo se puede ver, lo que hace el programa es calcular la posición de las piernas en cada fotograma siguiendo el esquema del seno, de forma que las piernas quedan sincronizadas.

Ajunto a continuación el resultado de este nuevo modo de caminar, con amplitud 0.01 y tiempo entre pasos de 0.3:

Lo que me deja bastante satisfecha con la tarea de “reinventar” la forma de caminar, ya que queda bastante más estable que el anterior.