Despliegue de la política en ROS2
En esta sección se presupone que ROS2 está instalado y configurado en el sistema. En mi caso estoy utilizando ROS2 Humble, por lo que algunos detalles pueden variar ligeramente si se utiliza otra versión.
También se asumen conocimientos básicos sobre ROS2. A lo largo de esta sección puede que se usen conceptos sin explicarlos en profundidad, ya que el objetivo es mostrar cómo desplegar la política entrenada utilizando ROS2 e Isaac Sim, no introducir los fundamentos de ROS2 desde cero.
Objetivo
En los pasos anteriores he entrenado una política para hacer que un robot aprenda a moverse según un comando de velocidad lineal y angular, pero ahora mismo esa política solo puedo visualizarla utilizando el script play del Isaac Lab.
El punto de entrenar un robot con Isaac Lab es poder utilizar esa política entrenada para después ejecutarla en un robot "real", lo que se llama sim2real.
En el caso particular de este proyecto, el diseño que he hecho del robot no es para nada realista. Los parámetros de los actuadores y de los componentes del robot han sido extremadamente simplificados para hacer más sencillo el aprendizaje (inercia, fricción, límites de motor, etc). Es por ello que "construir" el robot con hardware real y hacer que sus parámetros físicos coincidan con los entrenados es casi imposible, lo que me quita la opción de hacer el sim2real para este proyecto.
¡Pero puedo hacer algo intermedio! Si bien no puedo hacer que la política funcione en un robot real, puedo hacer que funcione en un robot simulado utilizando Isaac Sim para la simulación y ROS2 para la ejecución de la política. Puede parecer confuso al principio, pero trataré de explicarlo lo mejor posible.
Isaac Lab está pensado principalmente para entrenar políticas, no para ejecutarlas directamente en un robot real. Hasta ahora, podía ejecutar la política con el script play del Isaac Lab. Esto me servía para visualizar el comportamiento de la política, pero no me permitía controlar el robot a mi antojo, no puedo enviarle comandos de control. Entonces, tenemos que sacar a Isaac Lab de la ecuación.

El output de Isaac Lab es una política en forma de archivo ".pt" o ".onnx". Este archivo contiene todos los pesos de la red neuronal entrenada. Esta política (.pt) puede cargarse en un script de python utilizando pytorch, de tal manera que puedo utilizarla para enviarle un tensor con las observaciones y me devolverá un tensor con las acciones.
Entonces, crearé un nodo de ROS2 que cargará la política al inicio. Este nodo estará suscrito al topic /simplerobot/observations, por donde recibirá los valores del tensor de observaciones. Cuando reciba los valores, se los pasará a la política cargada, que devolverá el tensor de acciones correspondiente a ese estado del robot. El nodo entonces, publicará los valores del tensor de las acciones en el topic /simplerobot/actions .
Por otro lado, necesito simular el robot y para ello utilizaré Isaac Sim.
Primero crearé un entorno exactamente igual al que teníamos en el entrenamiento, estableciendo los mismos ajustes y parámetros en los actuadores y componentes del robot.
Después, tendré que hacer la conexión del Isaac Sim con ROS2, utilizando para ello el plugin de ROSbridge y un Action graph.
El Action graph se encargará de dos cosas. La primera será obtener de la simulacion los valores del vector de observaciones para después publicarlos en el topic /simplerobot/observations. La segunda será suscribirse al topic /simplerobot/actions y aplicar las acciones a los actuadores del robot.

En esta arquitectura, ROS2 es quien ejecuta la política de control, mientras que Isaac Sim únicamente se encarga de simular el robot y proporcionar las observaciones del estado del sistema.
De esta manera se crea un bucle de control completo: Isaac Sim publica las observaciones del robot, el nodo de ROS2 ejecuta la política con esas observaciones y publica las acciones, y el Action Graph aplica esas acciones al robot simulado.
Preparación del entorno de simulación en Isaac Sim
Creación del entorno e importación del terreno
Lo primero que hay que hacer es abrir un proyecto nuevo de Isaac Sim (podemos abrir la plantilla Sunlight para tener la iluminación ya configurada).
Después, utilizando el navegador "Content", navego al directorio donde tengo el archivo USD del terreno que hemos utilizado en el entrenamiento y lo arrastro a la escena. Pulso sobre el terreno y me aseguro que, en el menú de Propiedades, los valores de Translate y Rotate son 0.0 (puede haberse colocado en una posición diferente al arrastrarlo a la escena).
Si en lugar del terreno que hice para entrenar planos inclinados, quisiese utilizar un terreno plano, podría simplemente agregar un plano haciendo click derecho sobre la escena Create > Physics > Ground Plane.
Importación y configuración del robot
Utilizando el navegador "Content", navego al directorio donde tengo el archivo USD del robot utilizado en el entrenamiento. La ruta de este archivo, junto con los parámetros que configuraremos ahora, se encuentran en el archivo simplerobot.py que creamos en la sección Creación del proyecto.
Al igual que antes, tengo que comprobar que la posición del robot sea la correcta. Para ello pulso sobre el robot en el Stage Tree y compruebo los valores de Translate y Rotation. En mi caso, como quiero que el robot aparezca un poco por encima del terreno para evitar fallos en el cálculo de las colisiones, voy a hacer que su posición inicial sea X: 0.0, Y: 0.0, Z: 0.3.

Lo siguiente que tengo que hacer es configurar los parámetros de las articulaciones para que coincidan con los del entrenamiento. Para ello selecciono ambos joints (left_joint y right_joint) y configuro los parámetros Max Force, Damping, Stiffness y Max Actuator Velocity con los mismos valores que en el simplerobot.py
Hay que tener en cuenta que este Max Actuator Velocity está expresado en rad/s en el simplerobot.py y en degrees/s en el Isaac Sim).

Configuración del PhysicsScene
Este ha sido uno de los quebraderos de cabeza más grandes de todo el proyecto y la razón por la cual, nada de lo que hacía terminaba de funcionar bien en la simulación: el PhysicsScene.
Este elemento permite modificar los parámetros de la simulación. Lo primero que hay que hacer es añadirlo, pulsando con el boton derecho sobre la escena 3D o el Stage Tree y haciendo click en Create > Physics > Physics Scene.
Una vez añadido a la escena, hay que cambiar el parámetro Time Steps Per Second.

Este parámetro define el número de pasos (steps) que se calculan en la simulación por segundo. Este valor tiene que coincidir con el valor que usé para entrenar la política, definido en el archivo simplerobot_env_cfg.py .

En este caso, veo que el valor de dt es 1 / 120, lo que indica que el número de pasos por segundo es 120, que es el valor que tendré que poner en el PhysicsScene del Isaac Sim.
Además, me tengo que fijar en el valor de decimation, que en el caso del entrenamiento era 2. Recordaré este valor, ya que lo utilizaré más adelante.
ROS2 Bridge
Para que Isaac Sim pueda suscribirse y publicar topics de ROS2, hay que instalar y habilitar la extension ROS 2 BRIDGE.

Para ello hay que abrir la ventana de Extensiones pulsando sobre el menú Window > Extensions. Después, hay que borrar lo que haya en el buscador y buscar "ros". Por último hay que instalar y habilitar la extensión llamada ROS 2 BRIDGE.
Action Graph
Un Action Graph es un sistema de nodos que permite crear flujos de ejecución dentro de la simulación. Cada nodo hace una cosa concreta y al conectarlos entre sí, se pueden realizar acciones como la extracción de datos, la comunicación con topics de ROS2 o ejecutar acciones durante la simulación.
Para crear un Action Graph, hay que pulsar sobre el Stage Tree con el boton derecho y después hacer click en Create > Visual Scripting > Action Graph. Haciendo click derecho sobre el objeto Action Graph en el Stage Tree y pulsando sobre Open Graph, se nos abrirá la ventana de edición.
Antes de continuar, vamos a repasar qué debería hacer el Action Graph.
Recopilar la información de la simulación necesaria para crear el vector de observaciones.
Crear y publicar los valores del tensor de observaciones en un topic de ROS.
Suscribirse al topic de ROS de los valores del tensor de acciones.
Ejecutar las acciones en los actuadores del robot.
Configuración del Action Graph
Para poder utilizar el nodo "On Physics Step", que utilizaremos más adelante es necesario cambiar el valor de la propiedad pipelineStage para que tenga el valor pipelineStageOnDemand . Si no lo hacemos, la ejecución del nodo On Physics Step fallará.

Creación del tensor de observaciones
El tensor de observaciones contenía 14 valores:
Vector de gravedad proyectado (x, y, z)
Velocidad angular (pitch, roll)
Velocidad de las ruedas (rueda1, rueda2)
Velocidad lineal
Velocidad angular (yaw)
Comando de velocidad (lineal, yaw)
El problema es que hay varios valores que no sé extraer directamente de la simulación, como es el caso del vector de la gravedad proyectado. Además, hay otros datos, como el comando de velocidad, que quiero controlar de manera externa (conectando un mando al PC para el input).
Llegados a este punto, y aunque lo ideal era publicar directamente los valores del tensor de observaciones para que el nodo de ROS solo tuviese que consumirlo, lo que voy a hacer es publicar los datos que son fácilmente extraíbles de la simulación y hacer los cálculos necesarios para obtener todos los valores del tensor de observaciones en el nodo de ROS.
Publicación de la odometría
Lo primero que vamos a publicar es la odometría del robot. Esto nos va a permitir calcular después la velocidad lineal, angular y el vector proyectado de la gravedad en el frame del robot.

Primero agregamos el nodo On Physics Step, que actuará como la señal de disparo para ejecutar otros nodos. Estos disparos se ejecutarán a la frecuencia que hemos definido anteriormente en el PhysicsScene (120 veces por segundo).
Después agregaremos el nodo Isaac Compute Odometry Node, y conectaremos la entrada Exec in con la salida Step del nodo On Physics Step. De esta manera, este nodo se ejecutará cada vez que se ejecute el cálculo de físicas del simulador. Para agregar el input Chassis Prim, en el cuadro de propiedades del nodo, pulsaremos sobre el boton "Add Target" y elegiremos el objeto "base_link" del robot.

Después, agregaremos el nodo ROS2 Publish Odometry. Al igual que antes, haremos la conexión Entre la entrada Exec In y el nodo On Physics Step. También conectaremos entre si las salidas Angular Velocity, Linear Velocity, Orientation y Position del nodo Isaac Compute Odometry Node con las entradas con el mismo nombre del nodo ROS2 Publish Odometry.
En las propiedades del nodo, podremos configurar el nombre del topic donde se publicará la odometría del robot. En mi caso he puesto que se publique en /simplerobot/odom

Si hemos hecho todo bien, si pulsamos sobre el botón Play de la simulación y hacemos un ros2 topic list debería aparecer listado el topic /simplerobot/odom. Y si hacemos un ros2 topic echo /simplerobot/odom deberíamos poder ver los datos publicados por el Action Graph.
Publicación del estado de las articulaciones
Como también necesitamos saber la velocidad a la que giran las ruedas, vamos a publicar en otro topic estos datos.

Para ello vamos a agregar los nodos On Physics Step, ROS2 Publish Joint State y Isaac Read Simulation Time. Conectaremos los nodos como muestro en la imagen superior y en las propiedades del nodo ROS2 Publish Joint State configuraremos el targetPrim seleccionando el base_link del robot y cambiaremos el nombre del topic por el que queramos, en mi caso /simplerobot/joint_states.
Control de los actuadores
Vamos a presuponer que el nodo de ROS2 va a publicar un mensaje en /simplerobot/actions con los dos valores del tensor de acciones y que va a ser del tipo msg_std/Float32MultiArray .

Lo primero que vamos a añadir es el nodo On Physics Step y el nodo ROS2 Subscriber para crear el suscriptor al topic.
Conectamos el Step con el Exec In y configuramos el nodo de ROS2 Subscriber para indicarle el tipo de mensaje que va a recibir y el nombre del topic.

A continuación agregamos el nodo Articulation Controller, que va a ser el encargado de aplicar las acciones a los actuadores. Lo conectamos con el nodo On Physics Step y, como la entrada de Velocity Command espera un dato de tipo Double, tendremos que agregar un nodo intermedio To Double entre esa entrada y la salida Data del ROS2 Subscriber (que devuelve un array de floats) para convertir los datos a un array de Doubles. Una vez conectado todo, configuraremos el nodo Articulation Controller, seleccionando de nuevo el base_link de nuestro robot para el targetPrim. Además, aunque no es obligatorio en este caso porque la dimensión del array de acciones y el orden coincide con el vector de las joints, yo he indicado también los nombres de las joints sobre las que tiene que actuar, en el mismo orden que cuando se entrenó la política.

Con esto hecho, ya tendríamos listo el Action Graph.
Nodo de ROS2
He creado un paquete nuevo de ROS2 llamado simplerobot en mi workspace de ROS.
Dentro de ese paquete, he creado el nodo policy_node :
Análisis del nodo
__init__
En esta función:
Se realiza la carga de la política (archivo .pt).
Se crean los suscriptores a los topics de odometría, estado de las articulaciones y los comandos de velocidad.
Se crea el publisher del topic de las acciones.
Se Inicializan buffers y variables que se utilizarán después.
Se crea el timer que ejecutará la función
control_loop. Recordemos que el valor de decimation utilizado durante el entrenamiento era 2. Esto significa que la política no se ejecuta en cada paso de la simulación, sino 1 vez cada 2 pasos de físicas. Es decir, la simulación funciona a 120 Hz, pero la política solo se evalúa a 60 Hz. Por eso, este timer se ejecutará a 60Hz.
Callbacks
Se crean los callbacks de las suscriptions. En el caso del cmd_callback, sólamente nos interesa la componente x de la velocidad lineal y la componente z de la velocidad angular (yaw).
control_loop
Esta función realiza las siguientes tareas:
Comprueba si existen valores de la odometría o el estado de las articulaciones. Si falta alguno de ellos, sale de la función.
Extrae los valores de las velocidades de las ruedas.
Obtiene la orientación actual del robot a partir de la odometría en forma de cuaternión y crea un objeto de rotación a partir de él.
Extrae la velocidad lineal del robot en el sistema de referencia global (world).
Extrae la velocidad angular del robot en el sistema de referencia global.
Transforma las velocidades lineales y angulares desde el sistema de referencia global al sistema de referencia del propio robot (body frame).
Calcula el vector de la gravedad proyectado en el sistema de referencia del robot.
Obtiene el comando de velocidad lineal y angular que se está enviando al robot.
Construye el vector de observaciones concatenando todos los valores en el mismo orden en el que se utilizaron durante el entrenamiento de la política.
Convierte el vector de observaciones en un tensor de PyTorch para poder pasarlo a la red neuronal de la política.
Ejecuta la política entrenada utilizando ese tensor de observaciones para obtener las acciones que debe realizar el robot.
Convierte el resultado de la red neuronal a un array de numpy.
Escala las acciones multiplicándolas por 0.25, que es el mismo factor utilizado durante el entrenamiento.
Crea un mensaje de tipo
Float32MultiArraycon los valores de las acciones.Publica ese mensaje en el topic
/simplerobot/actions
Main
Bloque típico de inicialización de un nodo de ROS2: inicializa ROS, ejecuta el nodo y gestiona su cierre.
Ejecución del stack Isaac Sim + ROS2
¡Ya estamos listos para probarlo todo junto!
Abro una terminal donde ejecutaré el nodo policy_node con el comando: ros2 run simplerobot_policy policy_node
Como quiero manejar el robot con un mando de Xbox, voy a utilizar un nodo de ros que sirve precisamente para eso: escucha las acciones de un mando y publica en el topic /cmd_vel un comando de velocidad: ros2 launch teleop_twist_joy teleop-launch.py joy_config:='xbox'
Ahora en el podemos pulsar sobre el botón play de la simulación y controlar el robot utilizando el mando:

Evaluación
No ha sido fácil, pero como se puede ver, he conseguido desplegar la política en un sistema ajeno a Isaac Lab como lo es ROS2 y el robot se comporta genial.
Esto significa que si tuviesemos ese robot construido en el mundo real, podríamos utilizar la política entrenada directamente sobre el robot real. Obviamente esto no sería "plug & play" y habría que hacer muchos ajustes, pero en un robot complejo, ahorra muchisimo tiempo en la creación del sistema de locomoción.
Última actualización