search

ajardon / TFM-BorjaPerez

TFM Máster Robótica y Automatización uc3m
1 stars 0 forks source link

Comunicación vector fuerza Unity -> ROS2 #5

Open Borailuce99 opened 1 year ago

Borailuce99 commented 1 year ago

En Unity se ha implementado el código para obtener el vector fuerza que sería necesario aplicar en el robot IIWA para obtener la asistencia para mantener la esfera del usuario dentro del laberinto. Un poco en resumen sería:

imagen

Por ello, a partir de este Vector3 se debería de poder aplicar la fuerza en el robot IIWA. De esta forma, en Unity se va continuamente publicando este vector fuerza en el topic "/fb_force" como un mensaje de tipo RosMessageTypes.Geometry.PointMsg al que se deberá suscribir el control del IIWA y emplear este vector para aplicar el feedback de fuerza.

No obstante, si es necesario algún otro parámetro o es mejor enviar alguna otra información como la dirección, distancia, etc... para que la procese el robot, también es posible.

imontesino commented 1 year ago

En la rama tfm/borja de la repo https://github.com/roboticslab-uc3m/iiwa-ros2-examples he añadido un nuevo modo de control.

En el commit https://github.com/roboticslab-uc3m/iiwa-ros2-examples/commit/ccf5ba8ebea67596ab82ed0610f415d134cd47c2 estan los detalles.

Tienes que bajarte las repos iiwa-ros2-examples@tfm_borja, cobot-py y ros_iiwa_unity_msg.

Los mensajes tendras que compilarlos para Unity usando la ROS-Unity-Robotics Toolbox.

La idea es que lances el nodo simulado con ros2 launch dual_iiwa_control sim_robot_1_launch.py.

Desde unity lo que tienes que hacer es cambiar el modo de control a COLLISION_POINT_HAPTIC. Eso se hace creando un ROS2 Client con el servicio ControlModeService.srv con el campo control_mode con valor 7.

Te dejo aqui un ejemplo de como llamar a un servicio de ROS2 desde Unity : https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/service_call.md

Despues en el bucle de Unity que se encarge de detectar las colisiones mandas constantemente al topic /iiwa/id_1/collision_point del tipo CollisionPoint.msg.

El mensaje tiene dos campos:

imontesino commented 1 year ago

Con eso deberia bastar y deberias poder mover el robot simulado con el raton y comprobar que no se sale de las paredes del laberinto.

imontesino commented 1 year ago

@Borailuce99 Avisame en cuanto lo pruebes para aprovechar al maximo el tiempo que tenemos

Borailuce99 commented 1 year ago

Buenas. He estado siguiendo los pasos que has ido indicando y me han surgido algunas dudas y errores:

imontesino commented 1 year ago

@Borailuce99 Respondiendo a tus dudas:

imontesino commented 1 year ago

Por cierto, añádeme como colaborador a este repo

Borailuce99 commented 1 year ago
imontesino commented 1 year ago
ajardon commented 1 year ago
  • No, me refiero a que tienes que cambiar el modo de control a COLLISION_POINT_HAPTIC antes de mandar colisiones por el topic de colisiones. Cuando hacerlo es a discreción tuya según la lógica de tu programa que desconozco.
  • Exacto, cambiar de sistema de cordanadas Unity a ROS
  • Okey pues le dejamos eso a @ajardon

@imontesino invitado¡¡¡

imontesino commented 1 year ago

Vale esta listo el docker para tu demo. Clona la siguiente repo

Descargate la repo https://github.com/roboticslab-uc3m/roboasset-ros2-pkgs . Metete en la carpeta con la terminal, cambiate a la rama tfm_borja, y ejecuta los siguientes comandos para compilar el docker:

python3 generate_env_file.py
eval `ssh-agent -s`
ssh-add ~/.ssh/id_rsa
export DOCKER_BUILDKIT=1
docker compose build demo

Una vez compilado para lanzarlo ejecuta los siguientes:

sudo xhost +
docker compose run --rm labrynth-demo
Borailuce99 commented 1 year ago

Buenas. Ya he implementado el cambio de coordenadas que comentaste para pasar los puntos de colisión del sistema de Unity al del IIWA pero al ejecutar la demo del repo como comentabas, me salta un error con el ros_tcp-endpoint. Por si fuera útil, a continuación te envío una captura de toda la información que aparece en el terminal al ejecutar el "docker compose run --rm labrynth-demo" e iniciar la escena en Unity: errorRepo

imontesino commented 1 year ago

prueba a pararlo y lanzarlo otra vez, ese error tiene pinta de mala comunicacion unity ros

imontesino commented 1 year ago

@Borailuce99 Podrías subir tu código de Unity a GitHub? Creo que el error el que has creado el publisher con el tipo de dato incorrecto.

Borailuce99 commented 1 year ago

ya está subido. Es el "BallController.cs". Al principio del código están los tipos de mensajes de ROS que añado con el "using", por si es eso lo que quieres comprobar.

imontesino commented 1 year ago

Pues desconozco el origen del error, aparte de que en el código que has pasado hay muchas cosas comentadas que entiendo que será que estabas haciendo pruebas.

Prueba a bajarte la última versión del repo https://github.com/roboticslab-uc3m/roboasset-ros2-pkgs, esta vez en la rama main y recompilar el Docker. Esta versión está probada con Hugo, así que 100% no hay problema de conexión con Unity.

Así ideas rápidas:

Borailuce99 commented 1 year ago

He comprobado la IP que tenía en Unity y era "127.0.0.1:10000" así que la he cambiado a la que comentas, "0.0.0.0:10000" pero ahora me aparece el error en Unity de Connection to 0.0.0.0:10000 failed - System.ArgumentException: Addresses 0.0.0.0 (IPv4) and ::0 (IPv6) are unspecified addresses. You cannot use them as target address. Es posible que esa dirección no se pueda establecer para la conexión y tenga que ser un localhost (127.0.0.1) como antes? Estoy clonando y recompilando el docker que comentabas mientras, pero por si podía ser la IP el problema.

Borailuce99 commented 1 year ago

Al probar con la rama main ejecuto el docker y luego en un terminal el ros_tcp_endpoint, y al intentar conectarse desde Unity (con la IP 127.0.0.1 aunque en el terminal ponga el 0.0.0.0) me aparece el error No module named 'ros_iiwa_unity_msgs.ros_iiwa_unity_msgs_s__rosidl_typesupport_c. Ya desconozco si es un tipo de mensaje de la librería o que has creado tú. Además, siguiendo los pasos que hacía antes para las pruebas, ya no funciona y no consigo conectar ros con Unity para el funcionamiento de antes.

imontesino commented 1 year ago

El docker lleva el tcp endpoint integrado, no necesitas lanzar otro en una terminal. Mas aun, si lanzas otro puede dar problemas de comunicacion.

Borailuce99 commented 1 year ago

En el docker de la rama main también?

imontesino commented 1 year ago

Si, recuerda seguir estos pasos pero con la rama main

python3 generate_env_file.py
eval `ssh-agent -s`
ssh-add ~/.ssh/id_rsa
export DOCKER_BUILDKIT=1
docker compose build demo

Una vez compilado para lanzarlo ejecuta los siguientes:

sudo xhost +
docker compose run --rm labrynth-demo
Borailuce99 commented 1 year ago

Después de seguir esos pasos, he probado a ejecutar docker compose run --rm -t demo-sim-gui pero me aparece el error file 'full_sim_gui_demo.launch.py' was not found in the share directory of package 'dual_iiwa_control' which is at '/demo_ws/install/dual_iiwa_control/share/dual_iiwa_control'

imontesino commented 1 year ago

Eso es por que no se han recompilado los dockers. tienes dos opciones:

A parte, revisa los pasos que te he dicho que sigas, el comando que tienes que funcione con tu programa es:

sudo xhost +
docker compose run --rm labrynth-demo
Borailuce99 commented 1 year ago

Mientras se recompilaba, he modificado algunas cosas del código de Unity y al ejecutarlo, el servicio parece que funciona correctamente, en el terminal de vs me aparece: [robot_controller-1] [INFO] [1683543299.533012763] [iiwa_controler_id_1]: Got control mode: COLLISION_POINT_HAPTIC [robot_controller-1] [INFO] [1683543299.534154975] [iiwa_controler_id_1]: IK succeeded with joint positions: [ 0.00670754 0.42741828 -0.01142893 -1.60052035 0.00556711 1.13582927 -0.00670798] [robot_controller-1] [INFO] [1683543299.534541421] [iiwa_controler_id_1]: Switched to control mode: COLLISION_POINT_HAPTIC Por lo que entiendo que el mensaje de cambio de control a "COLLISION_POINT_HAPTIC" se está enviando correctamente.

Ahora el problema creo que siendo creo que el Publisher, que me salen diversos mensajes de que ese publisher ya está registrado, aunque al cambiar el código de Unity, si no poinía el "ros.RegisterPublisher" me salía otro error de que el publisher al que enviaba mensajes no estaba registrado. En el terminal me aparece el WARNING del Publisher ya registrado (entiendo que ya lo registras tú en el docker al ejecutarlo y por eso al registrarlo en Unity yo luego, salta el WARNING), pero el error aparece como 'NoneType' object has no attribute 'msg'entonces ya no entiendo muy bien dónde puede estar el fallo.

imontesino commented 1 year ago

No, el docker no registra nada. el docker solo lanza el tcp enpoint. Ese warning suele ser que estas lanzando varias veces el programa y por lo tanto estas creando el publisher varias veces.

En esos casos prueba a lanzar todo de nuevo. primero el docker y luego el script de Unity.

Si persiste ese error tendras que buscar que significa por que son cosas de como usar Unity tcp endpoint.

Borailuce99 commented 1 year ago

Buenas. Estoy intentado recompilar el docker y aunque haga con el --no-cache o borre el repo y vuelva a hacer git clone y vuelva a intentar el build, falla al intentar hacer sudo apt-get update y muestra los siguientes mensajes:

imagen

Después de eso intenta el sudo apt-get install -y software-properties-common, falla y termina la ejecución.

Edit: También he hecho git pull del repo y sigue fallando. He hecho git status para comprobar y me aparece que el repo lo tengo actualizado a la última versión en ambas ramas, la main y tfm-borja

Borailuce99 commented 1 year ago

He estado buscando y probando a ver cuál podía ser el problema de registrar 2 publishers y en el respositorio de ROS-TCP-Connector he encontrado un par de issues relacionadas:

Edit: Ahora mismo el rqt_graph aparece así imagen

Edit2: También me he dado cuenta que al hacer Ctrl+C en el docker de visual studio, si hago ros2 topic list en otro terminal, se ve que aunque el docker se pare, aún quedan los nodos de ros por un tiempo. Puede ser que esto afecte a que si paro el docker y relanzo unity muy pronto (antes de que se cierren los topics de ros anteriores) si luego ejecuto el docker vuelve a aparecer el error de doble Publisher registrado.

Borailuce99 commented 1 year ago

He probado ahora como ayer, ejecutando primero Unity y luego el docker, pero ahora ya no funciona y sigue fallando. No aparecen los nodos en rqt_graph como ayer ni nada. he probado con Debug.Log en varias partes pero el registerPublisher solo se ejecuta 1 vez, no tengo ni idea de por qué da el fallo de registrarse 2 veces.

No puede ser que en el docker se registren dos nodos con el mismo nombre? Y por otro lado, en el código de /demo_ws/install/ros_tcp_endpoint/lib/python3.8/site-packages/ros_tcp_endpoint/tcp_sender.py, en la línea 152 se podría añadir un if(node == null) antes de hacer el else self.parse_message_name(node.msg), por si igual el primer mensaje sí es nulo y por eso salta el error de NoneType has no attribute 'msg' y falla todo el programa luego.

Borailuce99 commented 1 year ago

Por cierto, de forma paralela a todo el intento de implementación esta, estoy haciendo el apartado de desarrollo de la parte del IIWA y diseñando el pipeline, me gustaría saber un poco más, aunque sea por encima y no muy en detalle, como funciona el apartado del labrynth-demo que has implementado @imontesino para añadirla al pipeline. Principalmente es la parte de recibir el mensaje de colisión en el /iiwa/id_1/collision_pointdel ROS-TCP y qué hace el robot con ese mensaje, cómo hace el control de las articulaciones para ejecutar esa función de fuerza, cómo el "controlador" procesa ese mensaje y qué envía luego al IIWA, etc... Pero repito, si puedes, tampoco es necesario que entres muy en detalles y especificaciones, solo es para tener una pequeña idea superficial para plasmarla en la memoria.

Gracias.

Borailuce99 commented 1 year ago

@imontesino He probado a modificar el código tcp_sender.py del docker demo-labrynth-demo. El cambio ha sido en la línea 152: topic_list.types = [ item[1][0].replace("/msg/", "/") if (len(item[1]) <= 1) else self.parse_message_name(node.msg) for item in topics_and_types ] CAMBIADO A topic_list.types = [ item[0][0].replace("/msg/", "/") for item in topics_and_types ] De esta forma, al ejecutar de nuevo el docker y Unity, tan solo sale un Warning de Only one message type per topic is supported, but found multiple types for topic /iiwa/id_1/ee_pose; maintaining geometry_msgs/Pose as the subscribed type. pero no parece que dé más problemas. Lo único que ahora el callback en Unity no se ejecuta, no sé si porque en el topic hay 2 tipos de mensajes geometry-msgs/msg/Pose y geometry_msgs/msg/PoseStamped o por qué puede ser.

Edit2: He modificado el script de Unity para que el subscriber sea de tipo PoseStamped y no Pose y ahora sí que recibe el mensaje, pero es de tipo PoseStamped.

imagen

FINALMENTE se ha comprobado que utilizando el PoseStamped en lugar de Pose, ya se ha conseguido que funcione correctamente la parte de recibir la posición y mover la esfera en la escena. Tan solo es necesario modificar el callback para sacar la Pose con el PoseStamped.pose y ya todo lo demás se ha dejado igual. Se ha comprobado que funciona correctamente el subscriber como se ve en el vídeo a continuación, pero al añadir el publisher, aparece un error de que no existe ningún modulo llamado 'roboasset_msgs' cuando ejecuto el ros2 topic echo del /collision_point.

MoveBall_solved-2023-05-15_13.16.18.zip

Borailuce99 commented 1 year ago

Así se ha comprobado que el problema no era el warning que aparecía, si no la definición del Subscriber ya que no existe ningún topic /ee_pose de tipo Pose sino que era PoseStamped. Lo siguiente ahora sería comprobar si se está enviando correctamente la información del topic /collision_point.

Por otro lado, ¿sería posible restringir el movimiento del robot en la simulación a un plano 2D, para facilitar las pruebas ya que en Unity la esfera al transformar la posición del iiwa, se restringe a un solo plano 2D.

imontesino commented 1 year ago

Geniaaaal!!! Si, seria posible restringirlo. Quiza sea interesante meter esos parametros en el mensaje? De momento podriamos probarlo solo restringiendo el plano vertical delante del robot.

Borailuce99 commented 1 year ago

Qué código he de modificar para restringir el movimiento?

Geniaaaal!!! Si, seria posible restringirlo. Quiza sea interesante meter esos parametros en el mensaje? De momento podriamos probarlo solo restringiendo el plano vertical delante del robot.

imontesino commented 1 year ago

Habria que añadir un par de lineas al robot-controller ademas de añadir una opcion en el servicio de comando, para poder selecionar que direccion bloquear. En tu caso seria la direccion $X$ (en ROS, en Unity no se a cual corresponderia)

Esta tarde lo hago.

imontesino commented 1 year ago

Por cierto, de forma paralela a todo el intento de implementación esta, estoy haciendo el apartado de desarrollo de la parte del IIWA y diseñando el pipeline, me gustaría saber un poco más, aunque sea por encima y no muy en detalle, como funciona el apartado del labrynth-demo que has implementado @imontesino para añadirla al pipeline. Principalmente es la parte de recibir el mensaje de colisión en el /iiwa/id_1/collision_pointdel ROS-TCP y qué hace el robot con ese mensaje, cómo hace el control de las articulaciones para ejecutar esa función de fuerza, cómo el "controlador" procesa ese mensaje y qué envía luego al IIWA, etc... Pero repito, si puedes, tampoco es necesario que entres muy en detalles y especificaciones, solo es para tener una pequeña idea superficial para plasmarla en la memoria.

Gracias.

Con respecto a esto lo mejor seria que te pasaras y te lo explique. El systema es algo complejo, sobretodo de explicar mediante mensajes.

imontesino commented 1 year ago

Por cierto @Borailuce99, he actualizado https://github.com/roboticslab-uc3m/roboasset-ros2-pkgs Ahora hay una herramienta por terminal (roboasset_cli) para crear dockers, descargarse los ultimos, correr las demos, etc. Si pudieras hecharle un vistazo, probarla y me dices si es intuitiva y si te funciona bien.

Las instrucciones estan el la seccion TLDR del README

boperezl commented 1 year ago
imontesino commented 1 year ago

He creado la rama tfm_borja en roboticslab-uc3m/roboasset-ros2-pkgs

https://github.com/roboticslab-uc3m/roboasset-ros2-pkgs

He añadido la funcionalidad de bloquear el movimiento en cualquier eje y rotación.

Cuando llamas al servicio set_control_mode el robot guarda la posición y rotación en la que está, y controlará esa posición en los ejes que están bloqueados, dejando libres los demás.

Para bloquear la rotación y el movimiento en $x$ rellena el campo request.options con un string de 6 caracteres con $0$'s para las direcciones fijas y con $1$'s para las direcciones bloqueadas, donde las direcciones son $(x,y,z,r_z,r_y,r_z)$.

En tu caso sería request.options = '100111'

imontesino commented 1 year ago

Para actualizar tu docker primero haz pull de roboticslab-uc3m/roboasset-ros2-pkgs para que tengas la nueva CLI.

Despues modifica el archivo repos/iiwa_control.repos en la linea que pone

  dual-iiwa-control:
    type: git
    url: git@github.com:roboticslab-uc3m/dual-iiwa-control.git
    version: main

y cambia main por tfm_borja.

Despues usa la cli para recompilar el docker image roboasset_demo y cuando te pregunte si quieres actualizar los paquetes dale a y

Borailuce99 commented 1 year ago

He hecho el git pull de roboasset-ros2-pkgs y luego ejecutado el robasset_cli, seleccionada la opción 2 (build a docker image), luego la imagen 1 (roboasset_demo) y de las opciones que aparecen, la opción y de Force rebuild pero me da error en el paso 5/7 con: === src/dual-iiwa-control (git) === Could not checkout ref 'tfm_borja': fatal: invalid reference: tfm_borja

imontesino commented 1 year ago

perdona, es tfm/borja

Borailuce99 commented 1 year ago

Vale, ahora ha fallado más adelante al hacer el apt. He probado la opcióin c de no-cache que ponía de usar si daba problemas el apt, pero ha vuelto a fallado también con executor failed running [/bin/sh -c apt-get install -y npm]: exit code: 100

imontesino commented 1 year ago

en que linea del docker?

imontesino commented 1 year ago

Vale, ahora ha fallado más adelante al hacer el apt. He probado la opcióin c de no-cache que ponía de usar si daba problemas el apt, pero ha vuelto a fallado también con executor failed running [/bin/sh -c apt-get install -y npm]: exit code: 100

prueba otra vez, es la primera vez que falla esal linea. Asegurate que tienes conexion a internet estable y sin vpn o cosas asi.

Borailuce99 commented 1 year ago

Okey, por otro lado al ejecutar con la opción y me aparecía este error:

apt: command [apt-get install -y ros-foxy-cv-bridge] failed apt: Failed to detect successful installation of [ros-foxy-cv-bridge]

executor failed running [/bin/sh -c . /opt/ros/foxy/setup.sh && rosdep install --from-paths src --ignore-src --rosdistro foxy -y -r --dependency-types='build' && colcon build --cmake-args -DBUILD_TESTING=OFF]: exit code: 1

Es el mismo error que me daba antes al instalarlo. No tengo ninguna VPN en el portátil y el internet debería ser estable, por si acaso, voy a conectarlo directamente al router con ethernet por si fuera ese el problema.

imontesino commented 1 year ago

He añadido un commit a https://github.com/roboticslab-uc3m/roboasset-ros2-pkgs para evitar ese error cuando recompilas localmente.

Borailuce99 commented 1 year ago

Finalmente, tras desconectarse el internet y luego apagarse el ordenador (-.-") ya ha funcionado el build correctamente! He ejecutado la imagen del labrynth-demo y me ha dado un error de "No protocol specified. cannot connect to X server". Pero al ejecutar la imagen del roboasset-demo, sim_demo parece que sí que funciona correctamente.

imontesino commented 1 year ago

hay que darle acceso a los docker a la pantalla corriendo el comando:

xhost +local:root

Tambien lo he añadido a la opcion Set-up environment de la CLI

imontesino commented 1 year ago

@Borailuce99 añadido Nvidia a la CLI. haz pull de la repo y prueba. Ahora la CLI es un programa que se instala. En el TLDR veien las intrucciones. es solo hacer pip install .

Despues en la CLi ejecuta el setup para asegurarte que tienes todo lode NVIDIA

Borailuce99 commented 1 year ago

He vuelto a probar lo del docker, pero me ha fallado al hacer el rebuild del docker (tanto con la opción y como con la c). Aparece el siguiente error: executor failed running [/bin/sh -c sudo apt-get install -y software-properties-common && sudo add-apt-repository ppa:ubuntu-toolchain-r/test && sudo apt-get update && sudo apt-get install -y gcc-11 g++-11]: exit code: 100 Seguido de un montón de traceback como este:

imagen imagen

Los pasos que he seguido son:

  1. git pull
  2. pip install .
  3. Modificar archivo repos > iiwa_control.repos para poner lo de tfm/borja
  4. roboasset-cli -d .
  5. Opción 3 de setup -> TODO SALE OK
  6. Opción 2 de Build -> con la opción de NVIDIA
  7. roboasset_demo Y ya he probado las 2 opciones que he comentado antes y nada.

No sé si se me ha llegado a pasar algún paso de los que hemos hablado en el lab.

imontesino commented 1 year ago

Te puedes estar quedando sin espacio en el disco duro?

si puedes copiar y pegar e error que sale encima de lo que has mandado, el de docker build