Configuración del entorno de trabajo para trabajar con el KUKA LBR iiwa 7 R800

[mclopez-uma]

• [x] Características del KUKA LBR iiwa 7 R800 - Página del manipulador y acceso a la Hoja de Datos que viene en la misma https://www.kuka.com/es-es/productos-servicios/sistemas-de-robot/robot-industrial/lbr-iiwa
• [x] Configuración del paquete de ROS del manipulador - Seguir los pasos que vienen en https://github.com/IFL-CAMP/iiwa_stack/wiki/ para configuración del entorno y para trabajar con el simulador no con el robot real

[javilara4]

Ya he conseguido cargar el robot en el simulador. He investigado un poco y el problema estaba en que no había hecho source devel/setup.bash y por eso me daba error al cargar el robot en el simulador. Ahora carga sin problemas en el simulador como muestro en la imagen pero sin embargo, cuando se ejecuta el código que carga el robot en el simulador me da algunos pequeños fallos, que imagino se solucionarán al modificar el código de algunos de los archivos .launch que se ejecutan.

[mclopez-uma]

Hola Javier,

Como dices, el error tiene pinta de un problema de configuración de los launch. De hecho, he echado un vistazo rápido al repositorio y hay ficheros launch configurados para el iiwa 7 y otros para el 14... así que supongo que habrá más de un parámetro mal configurado.

Como primer paso, antes de intentar buscar y configurar los launch, seguiría un poco el resto de documentación que viene en la Wiki. A la derecha de la página principal (Home), aparecen varias secciones que te recomiendo que leas e intentes entender. Una de ellas (attachtool - https://github.com/IFL-CAMP/iiwa_stack/wiki/attachtool) define una herramienta y en https://github.com/SalvoVirga/iiwa_stack_examples/tree/master/iiwa_tool algunos paquetes asociados a la definición de herramientas y al movimiento del robot con ellas.

Como digo, intenta seguir las otras secciones a ver si los errores desaparecen y consigues mover el robot. Recuerda que el modelo que tenemos en el taller es el iiwa 7, así que configura los parámetros que necesites para este modelo.

Bueno, ya nos vas contando como vas.

Un saludo, Mª Carmen

[mclopez-uma]

Hola Javier,

Al final de esta página está el correo de la persona que ha implementado el iiwa_stack y los ejemplos, escríbele y cuéntale lo que hemos hablado, que según la stack está el iiwa_ros.hpp pero que en los ejemplos se busca un iiwa_ros.h... a ver qué te dice.

Un saludo, Mª Carmen

[javilara4]

Perfecto, gracias Mª Carmen. Acabo de mandarles un correo aunque parece que el tal Salvatore se ha borrado ese correo porque me ha llegado un aviso de que no le va a llegar, espero que al otro hombre sí le llegue. Igualmente he creado un Issue para toda la comunidad de GitHub que sigue este tema y espero que alguien conteste. Cualquier avance os comento.

[mclopez-uma]

Hola Javier,

He estado echándole un vistazo de nuevo al iiwa_stack y he visto que hay nodos ya creados que leen el estado del robot:

• https://github.com/IFL-CAMP/iiwa_stack/blob/master/iiwa_ros/src/main.cpp
• https://github.com/IFL-CAMP/iiwa_stack/blob/master/iiwa_hw/src/main.cpp

Yo probaría lo siguiente:

• lanzar el simulador de Gazebo
• lanzar (de uno en uno) los dos nodos que te he dicho antes y comprobar que no da error en su ejecución y ver qué saca por pantalla. Uno de ellos tiene print a consola.

Pruébalo y nos cuentas que vas obteniendo.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Tras lanzar el simulador y hacer ejecutables los dos nodos que me has comentado, los he ejecutado con rosrun y me sale lo siguiente

También he mirado ambos nodos en visual studio y parece que las librerías como ya pasaba antes, tampoco las coge bien.

He intentado probar algunas cosas pero la verdad que me encuentro bastante perdido ahora.

[javilara4]

Buenas tardes, he reservado tutoría para el miércoles a las 13:30 para ver si podemos seguir viendo esto un poco mejor. Si pudiera ser me gustaría tener ese día la tutoría online porque me viene regular ir a la escuela. Mi teams es Javier Lara Juárez y mi correo de la uma es javlarjua@uma.es

[mclopez-uma]

Hola Javier,

El error que te está dando al ejecutar los main.cpp es que, por algún motivo, el compilador está intentando compilar los comentarios iniciales de cada fichero, lo que en la captura aparece en verde.

Yo probaría a eliminar los comentarios, compilar y volver a ejecutar a ver qué pasa.

Por otro lado, ¿volviste a instalar el iiwa_stack desde cero? Es bastante raro que el compilador intente ejecutar comentarios, así que puede ser que algo no esté bien en la instalación. No obstante, prueba lo que te he dicho y nos cuentas qué ocurre.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Tras quitar los comentarios de ambos main.cpp y hacer ambos archivos ejecutables, los he intentado ejecutar de nuevo y me sigue dando error como se ve a continuación:

He instalado varias veces el espacio de trabajo iiwa_stack_ws pero el fallo persiste y he intentado probar distintas cosas pero no sé de donde viene ese fallo en ambos archivos.

[javilara4]

Buenas tardes

He investigado un poco más y parece ser que el repositorio que se encuentra en https://github.com/SalvoVirga/iiwa_stack_examples/tree/master/iiwa_tool tenía algunos errores. Investigando por internet he visto que no era solo mi problema (https://github.com/SalvoVirga/iiwa_stack_examples/issues/12 ), sino que el repositorio y sus ejemplos estaban algo anticuados y no se habían corregido errores. He conseguido encontrar el mismo repositorio pero actualizado y que sí funciona correctamente en el siguiente enlace (https://github.com/tassos/iiwa_stack_examples). Tras clonarlo en la misma localización y volver a hacer catkin build éste no daba ningún error.

También he probado a crear un archivo .launch que me ejecute el main.cpp de los paquetes iiwa_hw y iiwa_ros pero me da error y no me cuadra porque lo he hecho solo para el archivo main.cpp del paquete iiwa_ros y he hecho ejecutable el main.cpp previamente. Os dejo a continuación lo que he puesto dentro del .launch y el error que me da al lanzarlo.

[mclopez-uma]

Hola Javier,

¡Genial que hayas encontrado eso! Yo también he estado buscando porque en la Wiki hacen referencia a una clase iiwaRos.cpp que realmente no existe y he encontrado el siguiente issue https://github.com/IFL-CAMP/iiwa_stack/issues/185 en el que se indica que está deprecada (por eso no aparece) pero también da ejemplos de cómo leer/comandar la pose del robot que nos pueden venir muy bien.

En cuanto al main.cpp no puede lanzarse con un launch porque no es un nodo. Siento que te hayamos liado, pero no me había parado bien a mirar el código del fichero. Si te fijas en el código del main.cpp no aparece ninguna instrucción del tipo ros::NodeHandle n; que inicializa al nodo. Ese fichero es una aplicación normal de c++ y se ejecuta directamente.

Cuando se hace un catkin build del workspace se compilan y se crean los ejecutables de todas las aplicaciones que previamente se hayan configurado en el CMakeLists.txt del paquete. Si te fijas en el el CMakeLists.txt de iiwa_ros (https://github.com/IFL-CAMP/iiwa_stack/blob/master/iiwa_ros/CMakeLists.txt) en la línea 47 aparece una instrucción que indica que el main.cpp que aparece en la carpeta src de ese paquete se debe generar como ejecutable y se debe llamar _iiwa_rostest. La instrucción es la siguiente: add_executable(iiwa_ros_test src/main.cpp).

Los ejecutables se crean en la carpeta devel del workspace, en concreto en devel/lib. Si te fijas en dicha carpeta verás que contiene una carpeta iiwa_ros (también una iiwa_hw, perteneciente al otro main.cpp del que hablamos el otro día) y dentro de ésta el fichero _iiwa_rostest que es el ejecutable creado. Para ejecutarlo simplemente arrástralo a un terminal.

Bueno, creo que con lo que tú has encontrado y lo que te he comentado yo ya puedes modificar código (por ejemplo el main.cpp del iiwa_ros) e intentar leer el estado del robot y también comandarle alguna posición nueva. Ya nos vas contando.

Un saludo, Mª Carmen

[mclopez-uma]

Hola de nuevo,

No sé si ya habías visto que para poder comandar el robot en simulación debes lanzar iiwa_gazebo_with_sunrise.launch no el iiwa_gazebo.launch.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes, he vuelto!

He estado un poco ocupado con el máster y los exámenes estos últimos meses pero ya he terminado y está todo aprobado por lo que ya solo me queda el TFM que espero poder terminarlo ahora en verano para exponerlo en Septiembre. Estos días he avanzado un poco más, he seguido investigando y ya he conseguido mediante el uso de los topics del robot simulado, leer su estado y parámetros, he conseguido mandarle mensajes para que cambie su posición y alguna cosa más. También he creado un ejecutable a partir del cual mediante un bucle va y vuelve con un descanso de por medio de dos posiciones dadas, que publica por pantalla una vez llega a éstas.

Ahora en Julio mi intención es dedicarle todos los días un gran número de horas a esto para avanzar lo máximo posible y seguir así investigando todo lo posible.

Me gustaría ver si podríamos concretar a lo largo de esta semana alguna tutoría online (ya he vuelto a Granada) para seguir hablado de esto y ver qué más puedo ir haciendo.

Un saludo y muchas gracias.

[mclopez-uma]

Hola Javier,

Podríamos hablar el martes 11 a las 12h00, utilizaríamos la siguiente sala Meet: https://meet.google.com/fdu-nzkf-xsv.

Por favor, mándanos antes, al menos que lo tengamos el lunes a primera hora de la mañana, lo que ya llevas: el ejecutable (el código fuente por favor) y el código que has utilizado también para probar las funciones que indicas. De manera que podamos replicar lo que has conseguido hasta el momento.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Perfecto, nos vemos el martes día 11 a esa hora.

Les he enviado a través del correo la carpeta comprimida del espacio de trabajo del robot (iiwa_stack_ws) ya que he intentado subirla a Github pero eran demasiado archivos y me iba a llevar demasiado tiempo. En dicha carpeta encontrarán todo lo que he ido usando/creando este tiempo.

También les adjunto a continuación un documento Word que he ido creando con las cosas más relevantes con las que me he ido encontrando. También expongo en ese documento lo que he hecho para comandar el robot, junto con algunos comandos que también he probado.

Finalmente comentarles que hoy he estado investigando como crear un Movelt y como usarlo también para comandar el robot. Esto último lo he logrado esta tarde y he conseguido comandar mi robot en Movelt y posteriormente ejecutarlo para ver como hacia la trayectoria en Gazebo. Todo esto también queda explicado en el Word que les adjunto. Cualquier duda me comentáis. Seguiré investigando más cosas estos días.

Muchas gracias y nos vemos el martes.

Desarrollo TFM - Javier Lara Juárez.docx

[mclopez-uma]

Hola Javier,

Hemos dejado un documento llamado funcionalidades.docx en el repositorio. En él hemos detallado todo lo que habría que hacer en tu trabajo, léelo y mañana hablaremos sobre él.

Un saludo, Mª Carmen

[mclopez-uma]

Hola Javier,

Ya tengo los topics de estado que son diferentes en simulación y en real. En simulación utilizas /iiwa/joint_states que incluye posición articular, velocidad articular y esfuerzo articular. Sin embargo en el robot real esta información viene separada en distintos topics: \iiwa\state\JointPosition, \iiwa\state\JointVelocity, \iiwa\state\JointTorque, \iiwa\state\ExternalJointTorque

Un saludo, Mª Carmen

[javilara4]

Buenas noches,

He avanzado con lo que comentamos en la reunión del otro día. En el repositorio ya os he dejado el paquete iiwa_surgery con el .yalm, el .launch y los dos .py (uno para el nodo y otro para la clase iiwa_surgery).

El .yalm lo podéis encontrar en el la carpeta config y en éste como podéis observar, he puesto todas las variables miembro de la clase inicial mapeadas como parámetros del fichero .yaml, añadiendo además otro parámetro que indique si se está trabajando en modo libre o modo pivotaje alrededor del punto de fulcro (work_mode).

En el .launch he declarado el argumento simulation_mode, para que al lanzar el .launch se le indique si estamos en simulación (simulatio_mode:=true) o no (simulation_mode:=false). Después se leen los parámetros del .yalm. Una vez hecho se pregunta si estamos trabajando en modo simulación (simulation_mode = true) o si trabajamos en modo real (simulation_mode =false). En el caso de que trabajemos en modo de simulación se ejecuta el iiwa_gazebo_with_sunrise.launch y después nuestro nodo iiwa_control_node.py. La línea que pone es porque he hecho muchas pruebas para intentar introducir la herramienta que ya viene definida en el paquete iiwa_tool_description de iiwa_stack al robot en Gazebo, pero no lo he conseguido y tras mucho investigar sigo sin saber cómo ponerla. En el caso de que trabajemos en modo real (simulation_mode =true) se ejecuta automáticamente un roscore y posteriormente nuestro nodo. En este archivo faltaría creo intentar modificar de alguna manera lo de la herramienta para introducirla en gazebo. Nota: recuerdo que a la hora de ejecutar el .launch se ha de poner roslaunch iiwa_surgery simulation_mode:=true (si quiero trabajar en modo simulación) o roslaunch iiwa_surgery simulation_mode:=false (si quiero trabajar en modo real). He intentado usar el valor del parámetro simulation_mode del .yalm para hacer el if unless del .launch pero no se puede y por eso he creado un argumento específicamente para esto.

En la clase iiwa_control_class.py ya está todo lo que se solicitó a falta del implementar el movimiento cartesiano alrededor del punto de fulcro que todavía tengo que mirar cómo es.

En el nodo iiwa_control_node también creo que está más o menos terminado a falta quizás de algunas modificaciones.

Hoy he probado a lanzar el .launch en modo simulación y tras abrirse el gazebo con el robot he abierto una nueva terminal y he publicado en el topic /iiwa_surgery/command/joints un mensaje y tras esto el robot se ha movido en gazebo hasta dicha configuración. Con el topic iiwa_surgery/command/pose también lo he probado pero no consigo que se mueva.

Tengo varias cuestiones que me han ido surgiendo:

• No entiendo bien para qué hemos creado publicadores en el nodo y para qué se quieren usar.
• Por lo que dijo Mª Carmen en su comentario anterior quiero entender que para comandar el robot tanto en real como en simulación, los topics que tengo para comandar el robot son los mismos no? Tampoco entiendo muy bien para qué queremos saber los topics que nos dan el estado del robot (los que comenta Mª Carmen en su anterior mensaje) si realmente nosotros vamos solo a comandar el robot.
• Por mucho que he buscado y he probado, no sé añadir la herramienta a Gazebo.
• ¿He de añadir algo más en el .launch y en los dos .py?

Si pudierais también me gustaría tener otra tutoría por ver todos estos temas y más cosas de manera más directa con vosotras.

Muchas gracias y un saludo

[javilara4]

Buenas tardes de nuevo,

Este fin de semana he avanzado otro poquito más. He modificado los 2 archivos .py un poco más para que se adapten mejor a los requerimientos exigidos.

En iiwa_control_node.py he añadido las líneas de código que recogen el valor de los parámetros (líneas 15 a 21) del .yalm y cargados posteriormente en el .lauch y también he añadido las siguientes líneas de código (líneas 23 a 28) que mandan dichas variables con estos valores a la clase iiwa_surgery para su posterior uso. También he hecho alguna modificación en el método pose_command_callback de manera que en función del valor de la variable work_mode (que define si hay movimiento cartesiano libre o alrededor de un punto de fulcro) pues manda el msg recibido al método move_cartesian de la clase si es movimiento libre o al método move_cartesian_fulcrum si estamos en modo pivotaje.

Por otro lado en el archivo iiwa_control_class.py he conseguido que funcione el modo de movimiento libre articular (move_joint) y el modo de movimiento libre cartesiano (move_joint). Ya solo quedaría el modo de movimiento alrededor del punto de fulcro que lo desarrollaré estos días cuando tenga acceso al TFG de Pablo como he comentado en el otro hilo.

Sigo con las mismas dudas que os planteaba en el anterior mensaje, ya que éste finde por más vuelta que le he dado no sé sacarlas, sobre todo el tema de añadir la herramienta a gazebo.

Un saludo y muchas gracias

[mclopez-uma]

Hola Javier,

La idea de este TFM es tener una primera capa sobre el stack del IIWA7 sobre la que montar en futuros trabajos más funcionalidad, de ahí que aunque para este trabajo los publicadores no se vaya a utilizar tenemos que tenerlos implementados.

La idea es que a la finalización del trabajo se pueda comandar al robot utilizando iiwa_surgery/command/joints o iiwa_surgery/command/pose y pueda leer su posición con iiwa_surgery/output/joints, iiwa_surgery/output/ef_pose o iiwa_surgery/output/tcp_pose independientemente de que se esté trabajando en simulación o con el robot real. Cuando lo tengas implementado avísanos para que nosotras lo vayamos probando.

En cuanto a la inclusión de la herramienta en Gazebo, nosotras no lo hemos hecho antes así que sentimos no poder ayudarte más. En la página de Gazebo hay bastantes tutoriales, igual hay alguno que pueda servirte https://classic.gazebosim.org/tutorials.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Os escribo para ver si podría tener un día de estos una tutoría con vosotras. Estoy un poco agobiado porque llevo ya unos días sin parar de programar y no veo avances.

Estoy atascado con: -No consigo poner la herramienta al robot en gazebo -El topic /iiwa/state/CartesianPose me da la posición y orientación del efector final pero no hay ningún topic que me de la posición y orientación del TCP y no sé como modificar la información del efector final para hallar el TCP y viceversa igual para poder usar el movimiento libre cartesiano y también para el iiwa_surgery/output/tcp_pose. -No se bien como adaptar el código de Pablo para usarlo en mi clase.

Esta semana el único día que tengo un poco más liado es el jueves a partir de las 10:30 hasta las 14:00.

Muchas gracias de antemano

[mclopez-uma]

Hola Javier,

Esta semana nos es imposible tener una tutoría. Con respecto a los puntos en los que indicas que estás atascado:

• Herramienta - Gazebo

Como te comentamos, nosotras no lo hemos hecho antes. Buscando por ahí hemos encontrado las siguientes páginas que igual te pueden servir (si no las habías localizado tú antes) http://classic.gazebosim.org/tutorials?tut=attach_gripper&cat=build_robot -> Aquí le añaden una pinza a un robot móvil. http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -> Uso de modelos URDF en Gazebo. No sé si la herramienta que tienes está en URDF

Si no logras avanzar danos más información: formato del modelo de la herramienta, si has conseguido incluirla en Gazebo junto al robot pero no justo en el efector, algún error que te haya dado ...

• Código movimiento con punto de fulcro

¿Has logrado probar el nodo movimiento.py? Eso es lo primero que deberías hacer, para ello primero debes modificar un poco los subscriptores. Localiza los subscriptores que hay en el código, mínimo debe haber uno que reciba la posición del robot, modifícalo para que el topic sea el adecuado en nuestro caso. Si hay más subscriptores cambia el topic o si son de configuración igual simplemente se puede eliminar y configurar con parámetro. Es cierto que el código no está muy bien documentado, pero en la parte de Implementación de la memoria sí que está explicado su uso. En cuanto a los publicadores haz algo parecido. Una vez tengas adaptado la entrada y la salida a ese nodo deberías poder utilizarlo sin problemas. Para seguir la nomenclatura que hemos utilizado estaría bien que le cambiaras el nombre.

• Posición/Orientación del TCP

Para el movimiento con el punto de fulcro esto está resuelto en la clase de Pablo que tienes que utilizar, así que sería utilizar la información que él publica y después mapearla en los topics que ya definimos. Para el movimiento libre, tienes que aplicar una transformación que te calcule la posición del TCP a partir de la del EF o viceversa. ¿Has cursado alguna clase de robótica?

Espero que con lo que te hemos comentado puedas avanzar. Si sigues teniendo alguna duda, por favor, detalla más el problema que te vayas encontrado.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes,

Sin problema, pero si podéis sí me gustaría tener una tutoría la semana que viene de cara a ver todo lo realizado hasta ahora un poco más en detalle con vosotras y hablar también un poco de qué puntos he de tratar y cómo debo estructurar la documentación para ir empezando a desarrollarla durante el mes de Agosto.

Con respecto a los distintos problemas que tengo:

• El tema de la herramienta en el Gazebo lo seguiré investigando a ver si consigo algún avance durante este fin de semana

iiwa_control_class.py

• Con respecto al código movimiento con punto de fulcro: No he conseguido probar el nodo de movimiento .py porque llevo unos días intentanto incorporar la librería pytransform3d pero no he conseguido que funcione y usar así las funciones matrix_from_axis_angle y axis_angle_from_matrix. He estado investigando y parece que han actualizado todo el repositorio de pytransform3d (https://github.com/dfki-ric/pytransform3d/tree/master) y ahora las funciones que os comento se hallan en el siguiente archivo (https://github.com/dfki-ric/pytransform3d/blob/master/pytransform3d/rotations/_conversions.py). No sé como hacer para que esto funcione.

Por otro lado dentro del método de move_cartesian_fulcrum del archivo de mi clase (iiwa_control_class.py) he incorporado el código de Pablo con algunas modificaciones para adaptarse a mi código. Dentro de estas modificaciones algunas son:

1. He puesto 3 parametros (Ph1, Ph2 y Ph3) que representan los incrementos de posicion de la herramienta y que han de cambiarse manualmente en el codigo ya que no es un valor de entrada.
2. En el documento funcionalidades que me mandasteis con las cosas a hacer, comentabais que el dato de entrada para este metodo era la posicion del TCP. Sin embargo, Pablo usa como dato de entrada la posicion y orientacion del efector dinal y no del TCP. Además el tópic que uso para proporcionarme estos datos (iiwa_surgery/command/pose) me da un mensaje que incluye la posicion y orientacion del efector final del robot. Por todo esto creo que se debería usar los datos del efector final para este metodo.
3. En el código en la línea 140 he usado la variable self.fulcrum_point directamente para calcular Pf, ahorrándome el calculo que hace Pablo de fi y que luego incluye en esta ecuacion (fi = float(fulcro/Dt))
4. Al final, la posicion resultante del efector final se manda al topic /iiwa/command/CartesianPose que es el topic que usa el robot real y el simulado para mover su efector final hasta dicha posicion.
5. No he podido probarlo debido a lo que os he comentado de la libreria pytransform3d

• En el metodo de move_cartesian también me comentais que recibe como valor de entrada el TCP pero como os comento es más facil trabajar con la informacion del efector final que luego es la que realmente usa el robot para moverse. Puedo interpretar que usamos como valores de entrada la informacion (posicion y orientacion( del efector final tanto en este metodo como en el move_cartesian_fulcrum?

iiwa_control_node.py

• En este .py he usado el valor de la variable self.simulation_mode para el tema de usar unos topics u otros para luego mandar esta información por el topic iiwa_surgery/output/joints. Sin embargo, he estado viendo un vídeo (https://www.youtube.com/watch?v=z6XxUHEb5LA) en el que los topics /iiwa/joint_states, /iiwa/state/CartesianPose y /iiwa/state/CartesianWrench de estado del robot están disponibles tanto para el robot real como para el simulado. Los únicos topics que aparecen nuevos en el robot real son \iiwa\state\JointPosition, \iiwa\state\JointVelocity, \iiwa\state\JointTorque, \iiwa\state\ExternalJointTorque, \iiwa\state\JointPositionVelocity, \iiwa\state\buttonEvent. Por ello se puede usar el topic /iiwa/joint_states para hallar la información de los joints del robot y luego mandarla por iiwa_surgery/output/joints sin necesidad de diferenciar entre simulación y real. Algo parecido pasa con la posición cartesiana del estado del robot y es que como os he comentado solo tenemos los topics /iiwa/state/CartesianPose y /iiwa/state/CartesianWrench que me pueden dar dicha información y ambos se usan en simulacion y en realidad. Por ello puedo usar /iiwa/state/CartesianPose para mandar la informacion que me llega del efector final por iiwa_surgery/output/ef_pose y también modificar esta informacion para hallar la informacion del TCP y luego mandarla por iiwa_surgery/output/tcp_pose (esto lo he intentado pero no sé hacerlo).
• Yo he dado algo de robótica durante el grado pero hace ya mucho de eso y casi ni me acuerdo, por lo que el tema de convertir la información que me llega del EF a la del TCP no se bien como realizarlo.

En el repositorio está todo lo que llevo hecho hasta ahora, si podéis echadle un ojo por si veis algo más que deba modificar.

Muchas gracias y un saludo

[mclopez-uma]

Hola Javier,

Unos comentarios con respecto a lo que indicas:

• Los parámetros Ph1, Ph2 y Ph3 deben ser parámetros de entrada al algoritmo de Pablo, pero no se deben modificar manualmente en el código, tendrás que calcularlos a partir de la Pose comandada. Esos parámetros indican el incremento en la posición del TCP que se quiere aplicar. La idea es que al iiwa_control_node.py le llegue el topic que dijimos con la Pose del TCP y tendrás que calcular el incremento para pasárselo al código de Pablo.

• Como ya te dijimos trabajaremos con la Pose del TCP como parámetro de entrada, tendrás que aplicar las transformaciones adecuadas para pasar a EF tanto para comandar al robot como para usar el algoritmo de Pablo. Para ello vamos a considerar que la herramienta es un palo alineado con el EF, así que las rotaciones serían iguales y lo que cambia sería la posición. En el código de Pablo tienes el cálculo hecho para pasar de la posición del EF a la del TCP considerando una herramienta de este tipo.

• Con respecto a los topic de simulación/real yo te dije lo que me comentaron compañeros nuestros que han trabajado tanto en simulación como con el robot nuestro real... nosotras no hemos podido probarlo. Intentaremos hacerlo lo antes posible.

Por otro lado, en el repositorio te hemos dejado una guía para la redación de la memoria.

Si quieres podemos hablar el lunes 24 a las 11h00 en la siguiente sala Meet: https://meet.google.com/sbd-dnev-gjj

Un saludo, Mª Carmen

[javilara4]

Perfecto, el lunes hablamos a esa hora. Gracias y buen fin de semana.

[mclopez-uma]

Hola Javier,

He estado investigando un poco cómo hay que comandar al Kuka en orientación y tras analizar mucho el código (no lo he visto documentado en ningún sitio) he descubierto que se utilizan los cuaternios. Es decir, en el topic, /iiwa/command/CartesianPose la orientación hay que pasarla como un cuaternio.

En el stack hay métodos que pasan de cuaternios a matriz de rotación y viceversa, se encuentran en https://github.com/IFL-CAMP/iiwa_stack/blob/master/iiwa_ros_java/src/de/tum/in/camp/kuka/ros/Conversions.java son las funciones quatToMatrix y matrixToQuat. Están en java pero supongo que habrá alguna manera de utilizarlas desde python sino siempre puedes buscarte una librería por ahí o incluso picártelas tú ya que en el fichero que te indico se ve muy claro como están implementadas.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes,

Gracias por tu mensaje Mª Carmen. Al final debido a todos los problemas que he tenido con la librería pytransform3d, he creado un archivo conversions.py en el que he recogido de dicha libreria aquellas funciones que he creido más importantes y que me podrían ser de utilidad, que son:

check_axis_index check_quaternion check_matrix norm_axis_angle norm_vector norm_angle axis_angle_from_quaternion matrix_from_quaternion quaternion_from_matrix euler_from_quaternion

He incluido este .py dentro de la carpeta src de mi paquete y e importado las funciones que he creido convenientes tanto al nodo como a la clase. Tanto ayer como hoy he estado probando para ver si en el nodo consigo terminar el método cartesian_pose_callback y poder publicar así en iiwa_surgery/output/tcp_pose la información (posición y orientación) del TCP. Sin embargo, y tras poner en el achivo .yalm la variable tool_length: 1.0 (longitud de la herramienta =1) me sale los siguientes resultados tras ejecutar el nodo:

En esta imagen se puede ver arriba lo que se está publicando en iiwa_surgery/output/ef_pose y abajo lo que se publica en iiwa_surgery/output/tcp_pose. Como se ve la orientación es correcta puesto que al ser la herramienta un palo la orientación del EF y del TCP será la misma. Sin embargo, en la posición ocurre algo extraño puesto que en la posición inicial del robot en la que x e y deberían valer 0 y donde el único valor que se debería modificar es z añadiéndose la longitud de la herramienta, no es así y se obtiene un valor que no es el deseado. Tras analizar el código repetidamente no consigo dar con el error y quería ver si vosotras sabéis donde me he podido equivocar. He dejado todo el código actualizado en el repositorio.

En lo referente al archivo iiwa_control_class solo he modificado el tema de los incrementos como hablamos el otro día en la reunión en move_cartesian_fulcrum y voy a esperar a intentar solucionar el problema comentado antes en el nodo para seguir con dicho método ya que dicho problema afecta también aquí.

Muchas gracias de antemano y un saludo.

[irivas-uma]

Hola Javier, cuando haces la trasnformación al tcp te cambian las 3 coordenadas. Piensa que el mundo está en la base del robot, y lo que tu estás haciendo es sumarle al EF la longitud de la herramienta en el eje z. Pero salvo que la herramienta esté completamente vertical, te va a cambiar también la posición global de la "x" y la "y".

He mirado el código y en la línea 109 tienes esto: z = Rm[2]

esa Z que te sale es un vector? no controlo mucho python, pero tiene pinta de que es un único valor. Compruébalo.

Un saludo, Irene.

[javilara4]

Buenos días

Tras estas últimas semanas investigando y probando creo que ya tengo mi código terminado. He conseguido que funcionen todos los métodos en los que estoy trabajando y también he logrado añadir una herramienta al brazo en Gazebo y que ésta se mueva junto al efector final del robot al usar mi código. Os he dejado todo mi código en el repositorio en la carpeta iiwa_surgery. Además, he añadido dos paquetes más al repositorio (iiwa_gazebo y iiwa_tool_description), ambos peternecientes a iiwa_stack, pero a los que he tenido que añadir algunas modificaciones para poder incluir la herramienta al robot en Gazebo.

Tras haber conseguido todo ésto, me gustaría tener una reunión para revisar el código final y comentaros los resultados obtenidos antes de ir a probarlo a Málaga. También éstos días empezaré a escribir el proyecto.

Muchas gracias de antemano y espero su respuesta.

[mclopez-uma]

Hola Javier,

Antes de tener la reunión que propones queremos probar nosotras la funcionalidad, para ello necesitamos saber cómo lo has probado. Si has creado una aplicación (un main) que lance los métodos por favor pásanoslo, si no es así indícanos qué pruebas has hecho y cómo lo has hecho (qué comandos has lanzado).

Por otro lado, con respecto a los paquetes iiwa_gazebo e iiwa_tool_description ¿qué has modificado exactamente? Veo que aparecen dos .launch nuevos pero ¿has tocado algo más?

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Lo primero comentar que he vuelto a subir el archivo iiwa_control_node.py ya que le he hecho una pequeña modificación. Para probar el código he ido haciendo pruebas individuales de los distintos métodos. Como solo he realizado pruebas con el simulador Gazebo siempre he ejecutado al inicio en la consola el comando roslaunch iiwa_surgery iiwa_surgery.launch simulation_mode:=true. Mediante este comando se abre el robot en el simulador y se ejecuta el archivo iiwa_control_node.py

Las pruebas realizadas son las siguientes:

• Método move_joint: Tras ejecutarse el simulador, abrimos una nueva ventana de comandos. En esta nueva ventana para mandar un mensaje por el topic iiwa_surgery/command/joints se escribe el siguiente comando:

rostopic pub -1 /iiwa_surgery/command/joints iiwa_msgs/JointPosition "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' position: {a1: 0.0, a2: 0.8, a3: 0.0, a4: 0.8, a5: 0.7, a6: 0.8, a7: 0.0}"

Este mensaje primero llega al método joint_command_callback y posteriormente a través de éste, el mensaje es mandado al método move_joint de la clase, que manda dico mensaje a trvés del topic /iiwa/command/JointPosition, haciendo moverse al robot hasta la posición especificada de cada articulación, quedando el robot de la siguiente manera:

• Método move_cartesian: Antes de comprobar este método y antes de lanzar el .launch que abre el simulador, se ha de comprobar que el archivo .yalm tengamos lo siguiente: work_mode: "free" (esto hará que el robot se mueva libremente en el espacio cartesiano) y un valor de longitud de la herramienta como por ejemplo tool_length: 0.1. Tras ejecutarse el simulador, abrimos una nueva ventana de comandos. En esta nueva ventana para mandar un mensaje por el topic iiwa_surgery/command/pose se escribe el siguiente comando:

rostopic pub -1 /iiwa_surgery/command/pose geometry_msgs/PoseStamped "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 0.5 y: 0.0 z: 1.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0"

Este mensaje primero llega al método pose_command_callback. En esté primero comprueba si estamos en modo de movimiento libre (work_mode: "free") y posteriormente a través de este método, el mensaje es mandado al método move_cartesian de la clase, donde el mensaje que llega contiene la información relativa al TCP y esta es transformada mediante diversas operaciones al EF, la cual posteriormente es trasmitida al robot por el topic /iiwa/command/CartesianPose haciendo moverse al robot hasta la posición especificada del EF, quedando el robot de la siguiente manera para este ejemplo:

• Método move_cartesian_fulcrum: Antes de comprobar este método y antes de lanzar el .launch que abre el simulador, se ha de comprobar que el archivo .yalm tengamos lo siguiente: work_mode: "pivot" (esto hará que el robot se mueva en modo pivotaje alrededor del punto de fulcro), un valor de longitud de la herramienta como por ejemplo tool_length: 0.1 y un valor fulcrum_fi: 0.5 que indica en qué punto de la longitud de la herramienta está el punto de fulcro (este valor debe estar entre 0 y 1 . Tras ejecutarse el simulador, abrimos una nueva ventana de comandos. En esta nueva ventana para mandar un mensaje por el topic iiwa_surgery/command/pose se escribe el siguiente comando:

rostopic pub -1 /iiwa_surgery/command/pose geometry_msgs/PoseStamped "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 0.5 y: 0.0 z: 1.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0"

Este mensaje primero llega al método pose_command_callback. En esté primero comprueba si estamos en modo de pivotaje (work_mode: "pivot") y posteriormente como la variable self.last_positiones nula, a través de este método, el mensaje junto a j=0 y un vector increment_vector de 3 ceros, es mandado al método move_cartesian_fulcrum de la clase, donde el mensaje que llega contiene la información relativa al TCP (todo esto si es el primer mensaje que le llega al método). Esta información es transformada mediante diversas operaciones al EF, la cual posteriormente es trasmitida al robot por el topic /iiwa/command/CartesianPose haciendo moverse al robot hasta la posición especificada del EF, quedando el robot de la siguiente manera para este ejemplo:

En esta posición el punto de fulcro queda definido y a partir de ahora el robot se moverá en torno a ese punto mediante incrementos de posición. A partir de este momento si llegara otro mensaje al topic iiwa_surgery/command/pose como puede ser el siguiente mensaje:

rostopic pub -1 /iiwa_surgery/command/pose geometry_msgs/PoseStamped "header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: '' pose: position: x: 0.52 y: 0.0 z: 1.0 orientation: x: 0.0 y: 0.0 z: 0.0 w: 0.0"

Este mensaje llega de nuevo al método pose_command_callback. En esté primero comprueba otra vez si estamos en modo de pivotaje (work_mode: "pivot") y posteriormente como la variable self.last_position ya no es nula, a través de este método, el mensaje genera un vector increment_vector (que es la resta de la posición nueva menos la posición anterior) que junto a j=1 y el propio mensaje, son mandados al método move_cartesian_fulcrum de la clase, donde el mensaje que llega contiene el vector de incrementos de posición (increment_vector) y la variable j=1 que nos avisa que no es el primer mensaje que le llega al metodo. Con esta nueva información se realizan los incrementos de posición requeridos alrededor del punto de fulcro y se manda la nueva posición del EF al robot por el topic /iiwa/command/CartesianPose haciendo moverse al robot hasta la posición especificada y alrededor del punto de fulcro, quedando el robot de la siguiente manera para este ejemplo:

• Método joint_state_callback: Antes de comprobar este método y antes de lanzar el .launch que abre el simulador, se ha de comprobar que en el archivo .yalm tengamos lo siguiente: simulation_mode: true (esto nos dirá que estamos trabajando en simulación). En este método llega el mensaje que se recibe del topic /iiwa/joint_states. De este mensaje se recoge la información solo refenerente a la posición de las articulaciones y se traspasa dicha información junto al header a un mensaje del tipo JointPosition para posteriormente ser publicado a través del topic iiwa_surgery/output/joints. Tras ejecutarse el simulador, abrimos una nueva ventana de comandos. En esta nueva ventana para poder observarlo que se publica a través del topic iiwa_surgery/output/joints se escribe el comando rostopic echo /iiwa_surgery/output/joints, obteniendo como ejemplo lo siguiente:

• Método joint_position_callback: Antes de comprobar este método y antes de lanzar el .launch que abre el simulador, se ha de comprobar que en el archivo .yalm tengamos lo siguiente: simulation_mode: false (esto nos dirá que estamos trabajando con el robot real). En este método llega el mensaje que se recibe del topic /iiwa/state/JointPosition. De este mensaje se recoge la información y se manda directamente a través del topic iiwa_surgery/output/joints. Tras ejecutarse el simulador, abrimos una nueva ventana de comandos. En esta nueva ventana para poder observarlo que se publica a través del topic iiwa_surgery/output/joints se escribe el comando rostopic echo /iiwa_surgery/output/joints. Aún no se ha probado ya que no he tenido acceso aun al robot real pero creo que estaŕa bien puesto que el codigo referente a este metodo es muy simple, solo hay que publicar lo que se recibe.

• Método cartesian_pose_callback: Este es el último método a comprobar de nuestro código. Este método recibe los mensajes del tipo PoseStamped que llegan desde el robot a través del topic /iiwa/state/CartesianPose. Estos mensajes dan información referente al EF del robot. Por ello, al comienzo del metodo estos mensajes se pueden publicar directamente al topic iiwa_surgery/output/ef_pose que es aquel topic que nos dará la información referente al EF del robot y que nos llega de éste. Obtendriamos algo así como:

Posteriorme y en el mismo metodo, se hacen las transformaciones indicadas para obtener la información del TCP a partir de la información del EF que nos llega y luego publicamos la informacion del TCP como mensaje también del tipo PoseStamped a través del topic iiwa_surgery/output/tcp_pose . Obtendriamos algo así como:

Hay que tener en cuenta que para este último método se ha recogido información del robot en su estado incial, es decir cuando está completamente recto y en el eje Z. También se ha usado el valor de la variable tool_length: 0.1 del .yalm para el calculo del TCP.

[javilara4]

Además del mensaje anterior quería comentarles que ya me he matriculado de nuevo en el TFM y desde secretaria me indican que es necesario que el tutor me vuelva a inscribir en el TFM en la plataforma para así poder defenderlo en el presente curso.

Muchas gracias de antemano y espero su respuesta.

Un saludo

[javilara4]

Buenas de nuevo,

Se me ha olvidado comentaros el tema de la herramienta. Os expongo por aquí el proceso seguido para añadir la herramienta que he realizado:

ADICIÓN DE HERRAMIENTA AL EFECTOR FINAL DEL ROBOT

Dentro del ws iiwa_stack podemos encontrar el paquete iiwa_gazebo. Dentro de la carpeta launch, tenemos el archivo iiwa_world.launch. En este archivo hay unas lineas de código que hacen referencia a cargar un URDF en el ROS Parameter Server. Para poder introducir una herramienta se han de comentar las lineas siguientes:

y después introducir nuestro propio URDF de nuestra herramienta. A nosostros se nos da uno de ejemplo en el paquete iiwa_tool_description en la carpeta urdf (iiwa_tool.urdf.xacro). Este URDF se ha de llamar como viene indicado en la carpeta launch del mismo paquete (archivo iiwa7_tool_upload.launch). Se ha de coger las lineas de codigo:

y pegarlas en donde se han comentado las lineas del archivo iiwa_world.launch en el paquete iiwa_gazebo. Para que no haya confusión he creado un archivo iiwa_world_edited.launch para poder editarlo sin perder el original y otro archivo en el paquete iiwa_tool_description en la carpeta urdf con nombre iiwa_tool_edited.urdf.xacro para editarlo también y no perder el original.

Una vez hecho todo esto solo queda modificar el archivo iiwa_tool_edited.urdf.xacro para que el robot aparezca en el origen de coordenadas y no montado en una caja, y girar la herramienta -90º en el eje Y para que ésta quede alineada con el EF del robot.

Por último en el caso que queramos lanzarlo solo habrá que ir a la linea de comandos y hacer lo siguiente:

roslaunch iiwa_gazebo iiwa_world_edited.launch

ADICIÓN DE HERRAMIENTA AL EFECTOR FINAL DEL ROBOT PARA PODER USAR EL ROBOT EN MI CÓDIGO

Para nuestro trabajo vemos que en el paquete iiwa_surgery en la carpeta launch en el archivo iiwa_surgery.launch vemos como en la parte de simulación se llama al archivo iiwa_gazebo_with_sunrise.launch para poder controlar el robot. Sin embargo para poder cargar la herramienta junto al robot se ha cambiado esta linea de código por otra que llama ahora al archivo iiwa_gazebo_with_sunrise_edited.launch. Éste archivo es una copia del anterior (se encuentra en el paquete iiwa_gazebo carpeta launch) pero cambiando la linea:

por esta: Esta linea llama a su vez al archivo **iiwa_gazebo_edited.launch** que se encuentra en la misma carpeta que **iiwa_gazebo.launch** (se encuentra en el paquete iiwa_gazebo carpeta launch) pero cambiando la linea: por esta: Esta linea llama a su vez al archivo **iiwa_world_edited.launch** que se encuentra en la misma carpeta que **iiwa_world.launch** (se encuentra en el paquete iiwa_gazebo carpeta launch) pero cambiando la linea: por esta: Esta linea se ha cogido como se ha comentado del archivo **iiwa7_tool_upload.launch** (paquete iiwa_tool_description carpeta lauch) y que carga el archivo **iiwa_tool_edited.urdf.xacro** (paquete iiwa_tool_description carpeta urdf) que es una copia del archivo **iiwa_tool.urdf.xacro** pero cambiando lo siguiente: se comenta esto: y se descomenta esto: Todo esto para que el robot aparezca en el origen de coordenadas y no desplazado de éste. También se modifica lo siguiente: por lo siguiente: Esto se ha hecho porque la herramienta aparecía en el efector final del robot pero girada 90 grados con respecto al eje y por lo que en la descripción de la herramienta se modifica su posicion inicial para que aparezca alineada con el EF. Solo hay que girarla -90º en el eje Y (o lo que es lo mismo, girarla -1.5708 radianes en el eje Y)

[mclopez-uma]

Hola Javier,

He estado probando el código que tienes subido al repositorio y algo no se está haciendo bien en el movimiento cartesiano, tanto libre como con fulcro.

Con respecto al movimiento cartesiano libre, al llevar al robot a la posición (0,0,1.5) el robot debería estar en vertical sobre el eje z y desplazado según el valor de z. Como puedes ver en la figura, el TCP no está dispuesto de esa manera, la posicón X y la posición Y que deberían ser 0 no lo son y hacen que la herramienta esté inclinada.

En la imagen se pueden ver también los topics asociados al TCP (columna central) y al EF (columna de la derecha), como puedes ver la X y la Y no mantienen la posición cuando deberían hacerlo. La Z sí parece estar bien calculada porque la diferencia que existe es más o menos 0.3 que es el valor de la longitud que le había configurado.

Con respecto al movimiento con punto de fulcro, con la misma imagen que tú has mandado se puede ver que no se está ejecutando bien. Con movimiento en punto de fulcro el robot debe estar en una configuración de codo arriba, es decir la herramienta debe estar "mirando" hacia abajo. Además, aunque consideremos esa posición como válida, al enviarle la siguiente posición el punto coincidente con la mitad de la herramienta (puesto que se ha considerado fulcrum_fi como 0.5) no debe moverse de la posición en la que está y esto tampoco ocurre.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes Mª Carmen,

He estado mirando lo que comentas. En el movimiento libre cartesiano me parece que mi función sí funciona bien. He hecho dos pruebas y funciona correctamente. En la primera, teniendo una longitud de herramienta de 0.3, he intalroducido las coordenadas de TCP de (0,0,1.2) como se ve en el cuadro de arriba a la derecha. El robot se desplaza verticalmente hacia abajo en el eje z como le solicito. En el cuadro de abajo a la derecha se ve la información que da el robot sobre el EF y me da (0,0,0.9) que está bien puesto que el EF está 0.3 más abajo en el eje z que el TCP. En el cuadro de la abajo a la izquierda vemos la información que da el robot acerca del TCP y me da la información correcta siendo la posición que me da la misma que yo le he introducido. En la siguiente imagen se ve la prueba:

También he realizado la misma prueba pero ahora introduciendo como TCP las coordenadas (0,0,1.5) y manteniendo 0.3 de longitud de herramienta. Se observa que también funciona correctamente. Se observa en la siguiente imagen:

Con respecto al método move_cartesian_fulcrum hay que tener en cuenta que la primera posición que se le pasa al método será la que fije el punto de fulcro. Con esto me refiero a que si no modificamos la orientación en el primer mensaje que le enviamos, el robot es probable que tenga su herramienta mirando hacia arriba. Una vez que el robot recibe el primer mensaje y por tanto fija el punto de fulcro, los siguientes mensajes que reciba se usaran solo para los incrementos de posición y el movimiento en torno al punto de fulcro. Si seguís creyendo que no está bien implementado algunas de estas cosas me gustaría verlas con más tiempo en una reunión puesto que llevo todo agosto con esto y ya no sé que más cambiar.

Muchas gracias de antemano y un saludo.

[mclopez-uma]

Hola Javier,

He vuelto a probar el movimiento en cartesianas libre, he vuelto a bajarme tu código, y sigue pasando lo mismo si se comanda con nuestro topic /iiwa_surgery/command/pose; si se comanda con el topic del robot /iiwa/command/CartesianPose lo hace bien. ¿Tendremos versiones distintas? ¿estarás usando el topic del robot en vez del nuestro?

Con respecto al movimiento de cartesianas con punto de fulcro, adjunto un vídeo en el que se puede ver como el fulcro no se mantiene. En el vídeo se ve la posición inicial del robot, comandada justo después de lanzar roslaunch iiwa_surgery iiwa_surgery.launch simulation_mode:=true

rostopic pub -1 /iiwa_surgery/command/pose geometry_msgs/PoseStamped "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
pose:
position:
x: 0.73
y: 0.0
z: 0.21
orientation:
x: -0.04
y: 0.95
z: -0.02
w: 0.3"

Puedes ver una pegatina amarilla sobre mi pantalla que indica cuál es el punto de fulcro (más o menos a mitad de la herramienta, ya que fulcrum_fi es 0.5). Con el movimiento en cartesianas con punto de fulcro, ese punto marcado solo debe moverse a lo largo de la herramienta, la herramienta debe pivotar sobre él. Al robot se le comanda una posición: XYZ de la que se obvia la orientación, ya que ésta se debe calcular para que ese punto no se mueva. En el vídeo se puede ver que se comanda una nueva posición, aumento x que pasa de 0.73 a 0.83 y puedes ver cómo el punto marcado no permanece a lo largo de la herramienta.

No sé si es un problema de versiones entre tú código y el que nosotras tenemos o qué... ya nos dices.

Un saludo, Mª Carmen

https://github.com/irivas-uma/TFM_JavierLara/assets/4289786/cfd89c56-515e-4f77-9ac0-9c2cfb097508

[javilara4]

Buenas tardes

He revisado el tema del movimiento en cartesianas libre y funciona perfectamente con el código que he implementado. He realizado varias pruebas usando tanto el topic /iiwa_surgery/command/pose como el /iiwa/command/CartesianPose y ambos dan el mismo resultado.

Lo primero de todo es tener en cuenta que al topic /iiwa_surgery/command/pose se le pasa la información referente al TCP que luego será transformada en mi codigo para conseguir la posición y orientación del EF y enviar dicha información del EF al robot para que este se mueva hasta esa posición a través del topic /iiwa/command/CartesianPose. Como os comenté no existe un topic especifico para mandar directamente la información del TCP al robot y que este se mueva hasta ahí, sino que hay que mandarle la información del EF a través del topic /iiwa/command/CartesianPose que es el que usará para recibir dicha información y moverse hasta la posición del EF mandada a través de ese topic.

Dicho esto y usando una longitud de herramienta de 0.3 tenemos las siguientes pruebas:

PRIMERA PRUEBA

Como se observa en el cuadro de arriba a la derecha en la imagen, se ha usado el topic /iiwa/command/CartesianPose para mover el EF del robot hasta la posición (0 ,0, 0.9). Como se ve realiza correctamente su movimiento descendente vertical hacia abajo hasta la posición indicada.

Ahora en esta imagen observamos el mismo resultado pero ahora usando como se ve, el topic /iiwa_surgery/command/pose. Se ha introducido la posición del TCP de (0, 0, 1.2), obteniendo el mismo resultado que en la imagen anterior. Esto se debe a que nuestra herramienta tiene una longitud de 0.3, que restandola al eje z, se obtiene la posición del EF, es decir (0,0,0.9), que es la misma que la que se introduce en la imagen anterior para el topic /iiwa/command/CartesianPose. Este calculo lo realiza mi código y luego manda la posición del EF una vez obtenida a través del topic /iiwa/command/CartesianPose, obteniendo la misma posición final que la de la imagen anterior.

SEGUNDA PRUEBA

Aquí volvemos a repetir el procedimiento de antes pero ahora usando otras posiciones. En esta primera imagen he mandado la posición (0, 0, 0.8) a través del topic /iiwa/command/CartesianPose. Aquí se nota que algo falla puesto que le he mandado una posición del EF al robot a través de unos de sus topics (que ya vienen programados de forma predeterminada) y resulta que este se desplaza un poco del eje z como vemos. Esto puede deberse a las limitaciones de rotación de algunas articulaciones del robot, que ante la imposibilidad de seguir girando para llegar a la posición deseada, muevan el robot de otra manera que no se espera o se desea. Esto NO es mi culpa puesto que no estoy usando mi codigo para mandar esta posición, sino uno de los topics predeterminados del robot. Quizás también puede deberse a un problema del simulador Gazebo y esto se podría verificar una vez que se pruebe el robot de forma real. Sin embargo y como digo, esto no es un problema de mi código sino de algo externo a él.

Para refutar todo esto, he hecho la prueba con el topic /iiwa_surgery/command/pose. Se ha introducido la posición del TCP de (0, 0, 1.1), obteniendo el mismo resultado que en la imagen anterior. Esto se debe a que nuestra herramienta tiene una longitud de 0.3, que restandola al eje z, se obtiene la posición del EF, es decir (0,0,0.8), que es la misma que la que se introduce en la imagen anterior para el topic /iiwa/command/CartesianPose. Este calculo lo realiza mi codigo y luego manda la posición del EF una vez obtenida a través del topic /iiwa/command/CartesianPose, obteniendo la misma posición final que la de la imagen anterior.

Por todo ello, considero que este metodo está bien desarrollado. Probaré ahora el metodo move_cartesian_fulcrum y os mando otro mensaje.

Un saludo y gracias.

[javilara4]

Buenas de nuevo

Ya he mirado el tema del método move_cartesian_fulcrum. Antes de nada quería comentar que seguramente hayas usado para tus pruebas una longitud de herramienta de 0.3. Esto no debe ser así puesto que la herramienta es más pequeña. La herramienta se basa en 3 piezas unidas (una base, un cilindro y un pequeño cono que forma la punta de la herramienta). La altura del cilindro es de 0.1275 y aunque no sé la altura exacta del cono ni de la base, me atrevería a decir que proporcionalmente la herramienta en su conjunto mide un total de 0.24.

También quería comentar que he visto que una vez has mandado el primer mensaje y has fijado tu punto de fulcro, has vuelto a mandar otro mensaje haciendo un incremento en x del 0.1. Sabiendo que la herramienta mide como mucho 0.24 y que el punto de fulcro está como hemos establecido a la mitad de la herramienta (aproximandamente a 0,12 del EF), quizás hacer un incremento de 0.1 sea demasiado. Yo he estado haciendo incrementos de 0.02, 0.03 o así y me parece que funciona más o menos de forma correcta. Os dejo un video por aquí:

https://github.com/irivas-uma/TFM_JavierLara/assets/56400007/90be9672-a98d-4cf4-95a3-55043aef7a7d

Para la realización de este método he usado en gran parte el código de Pablo una vez que he se ha fijado el punto de fulcro, a excepción de algunos cambios que he debido hacer porque yo trabajo con cuaternios y Pablo no lo hacía. Por ello, puede ser que algo no esté bien pero le he dado varias vueltas y creo que está bien diseñado.

Cualquier cosa me comentáis como lo veís.

Muchas gracias de nuevo y un saludo.

[mclopez-uma]

Hola Javier,

Con respecto al movimiento en cartesianas libre, es cierto que es una limitación, pero es una limitación del simulador, de Gazebo, con el robot real el EF se queda bien alineado. Así que este movimiento, parece que ya estaría. Otra diferencia con el simulador y el robot real es que si se comanda una posición no válida, el simulador hay veces que no te informa y el robot real siempre lo hace.

Con respecto al movimiento en cartesianas con fulcro, para poder comprobar si el movimiento se está realizando correctamente alrededor del punto de fulcro hay que hacer lo siguiente (puedes hacer un pequeño nodo aparte en matlab o donde te resulte más fácil):

• Leer la posición del EF del robot, y con esa posición calcular el punto de fulcro y la del TCP, sumándole la distancia correspondiente en el eje Z.
• Dibujar las 3 posiciones: la del EF, la del punto de fulcro y la del TCP.
• La posición del punto de fulcro debe permanecer constante. Sino, es que hay algo que no se está haciendo bien.

Es decir, en primer lugar se calcula la posición del punto de fulcro sumándole a la posición inicial del robot la distancia correspondiente en el eje Z. Y esa posición del punto de fulcro ya permanece constante para hacer los cálculos para el movimiento alrededor del fulcro. Para comprobar si está bien hecho, se hace la inversa. Se lee la posición del EF y a partir de ahí se calcula el punto de fulcro, que debe permanecer constante.

El resultado de esta comprobación serviría también para justificar en la memoria que el movimiento implementado es válido.

Por otro lado, deberíamos poder configurar en Gazebo la longitud de la herramienta, supongo que será modificar un parámetro de alguno de los ficheros.

Con respecto al vídeo que mandaste, lo he replicado mandando las mismas posiciones y en la tercera posición el robot no se me va al mismo sitio que se te va a ti. Por favor, sube el workspace completo que yo pueda comprobar si hay alguna diferencia (debe de haberla) en algún sitio.

Finalmente, hoy he estado probando el Kuka de verdad y quería comentarte un par de cosas para tenerlas en cuenta:

• en la publicación de los topics el frame_id debe estar fijado a 'iiwa_link_0', es decir, se indica que el sistema de referencia sobre el que se está comandando la posición es el asociado a la articulación cero que es la de la base del robot. La orientación no se puede dejar a (0,0,0,0), el cuaternio tiene que tener un módulo igual a 1.
• he probado tu nodo con simulation_mode:=false y comandándolo en cartesianas movimiento libre parece que va bien. Lo único es que los topics ef_pose y tcp_pose no sacan nada, el de joint sí. He comprobado que lo tenías enlazado al topic del Kuka correcto, así que debe ser algo en la implementación de la callback que tienes.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Gracias por tu mensaje Mª Carmen y por comprobar que efectivamente era una limitación del Gazebo.

Me voy a poner a modificar el metodo de movimiento en torno al punto de fulcro a ver si puedo crear algo para comprobar lo que comentas ya sea mediante un grafico o imprimiento datos por pantalla.

Con respecto a lo de la herramienta me equivoqué en lo que os comenté el otro día. La herramienta viene definida de manera predeterminada como un solo cuerpo y tiene una longitud total de 0.1275 m (viene exlicado en https://github.com/IFL-CAMP/iiwa_stack/wiki/attachtool , donde se puede leer ''We have now a frame tool_link_ee at the tip of the tool, which in this case is just 12.75 cm long''). Su geometría como os comento viene ya predeterminada en un archivo .stl (en la ruta /iiwa_stack_ws/src/iiwa_stack/iiwa_stack_examples/iiwa_tool/iiwa_tool_description/meshes/visual). En el archivo iiwa_tool_edited.urdf.xacro (en la ruta /iiwa_stack_ws/src/iiwa_stack/iiwa_stack_examples/iiwa_tool/iiwa_tool_description/urdf) se carga este archivo .stl , pero no es posible la modificación de la geometría de la herramienta. En el caso de que si quisiera modificar la geometría de la herramienta habría que instalarse algún software de modelado 3D como Blender, SolidWorks o similar y modificarlo ahí.

He intentado subir el workspace con todo lo que tengo a github pero tiene demasiados archivos y subcarpetas por lo que es muy tedioso y me llevaria mucho tiempo subirlo todo. Por ello os he mandado por correo un enlace a wetransfer que tiene un zip con el workspace. Ahí está todo lo que tengo y todo lo que modifique de ahí lo subiré de forma individual al repositorio como he hecho hasta ahora si os parece bien.

En cuanto a las dos cosas que me comentas al final, tengo algunas dudas:

• Primero, no me ha quedado del todo claro a lo que te refieres con el cuaternio. ¿Debo poner alguna condición en mi código de forma que para que funcione tanto el movimiento en coordenadas cartesianas libres y el movimiento alrededor de punto de fulcro, tengo que introducir un cuaternio en la orientación que sea unitario?
• Segundo, he estado mirando el código y me parece extraño que tanto los topicos ef_pose y tcp_pose no saquen nada. En el archivo del nodo que es donde implemento su uso, defino ambos como publicadores y publico en ambos correctamente un mensaje del tipo PoseStamped que es el mismo tipo de mensaje que se recibe a través del topic /iiwa/state/CartesianPose. Además en el simulador funciona perfectamente como ya hemos visto. No tengo idea de qué puede estar pasando.

[javilara4]

Buenas de nuevo

He estado todo el día dándole vueltas al método del moviento alrededor del punto de fulcro y ya he conseguido encontrar el error por el que no se movía correctamente y solucionarlo. Ya funciona!. Además del correo que os he mandado esta tarde con todo el espacio de trabajo, he modificado esta tarde los archivos iiwa_surgery_params.yalm, iiwa_control_node.py y iiwa_control_class.py. Estos archivos los he vuelto a subir ahora al repositorio en el paquete iiwa_surgery ya corregidos. A continuación menciono los cambios realizados:

• iiwa_surgery_params.yalm: He puesto como longitud de la herramienta 0.1275, ya que es la longitud exacta que tiene la herramienta como os comento en el mensaje anterior.
• iiwa_control_node.py: He modificado la parte del cálculo de los incremetos de posición que luego se mandan al metodo move_cartesian_fulcrum. Para el primer mensaje que se recibe se manda un vector de incrementos =0. Una vez fijado la primera posición y con ella el punto de fulcro comenzamos a mandar nuevos mensaje y con estos calcularemos los incrementos de posición. Para dicho calculo se restará la primera posición del TCP que se manda menos la nueva posición del TCP y se mandará el vector resultante al método. De esta manera podemos ser mucho más exactos con los incrementos de posición que queremos tomando de referencia el primer TCP que se manda.
• iiwa_control_class.py: En este archivo se ha modificado como ya he dicho el método move_cartesian_fulcrum. En este ahora para el primer mensaje que le llega (posición y orientación del TCP), calcula el punto de fulcro y lo fija. Posteriormente y antes de que lleguen más mensaje se imprime la posición del punto de fulcro, del EF y del TCP. Luego una vez llegan más mensajes (incrementos), la herramienta se moverá hasta la posición indicada del TCP a partir de los incrementos y alrededor del punto de fulcro. Para comprobar que el proceso se ha llevado a cabo de manera satisfactoria y el movimiento es correcto, al final se imprimen las nuevas posiciones del EF y del TCP tras los incrementos de posición. También se comprueba si el punto de fulcro fijado en la primera iteración (Pf) se encuentra en la herramienta en su nueva posición. Para ello se comprueba si el punto de fulcro (Pf) entá entre los nuevos valores de EF (Pn) y de TCP (Ptn). Si esto es afirmativo, el sistema imprime el mensaje "El punto de fulcro (Pf) está entre EF (Pn) y TCP (Ptn)." y si no es correcto impirme "El punto de fulcro (Pf) no está entre EF (Pn) y TCP (Ptn).". He intentado hacer un gráfico como comentabas pero tras mucho intentarlo no lo he conseguido. A continuación os dejo un video con la demostración del correcto funcionamiento. La longitud de la herramienta se ha fijado en 0.1275 como ya he dicho y se han ido haciendo incrementos de 0.02, 0,03 o así.

https://github.com/irivas-uma/TFM_JavierLara/assets/56400007/6aebb15c-e19d-4fb4-ad5e-c325676f4c9a

[mclopez-uma]

Hola Javier,

El tema del movimiento en cartesianas con fulcro hay que comprobarlo adecuadamente tal y como te comenté el otro día. De hecho las gráficas que se obtengan (hay que obtener gráficas a longitudes de herramienta distinta) se incluirán en la memoria. Para hacerlas te recomiendo que almacenes los datos que necesites, para ello puedes utilizar un bag, que es un fichero de ROS que almacena los topics que le configures. Aquí tienes el enlace a la Wiki asociada a esto http://wiki.ros.org/Bags. Una vez hayas generado el bag con los datos necesarios, creo que lo más sencillo será analizarlo en Matlab (pero realmente puedes hacerlo donde veas). Matlab tiene una toolbox de ROS que permite leerlo y cargar los datos. Una vez cargados ya puedes trabajar con ellos. Te dejo un par de enlaces (hay más, ya miras cuál es el que necesitas): https://es.mathworks.com/help/ros/ref/rosbag.html, https://es.mathworks.com/help/ros/ug/work-with-rosbag-logfiles.html. Cuando tengas las gráficas que confirman que el movimiento es válido nos las pasas.

En cuanto a los cuaternios, no tienes que hacer nada en el código. Simplemente te lo decía para tenerlo en cuenta a la hora de hacer las pruebas con el robot real. Con el simulador no hay problemas en dejarlo todo a cero, pero con el robot real hay que darle un valor válido.

Un saludo, Mª Carmen

[javilara4]

Buenas tardes

Ya he obtenido los gráficos que me comentabas. Ayer estuve probando con los bags de ROS pero obtuve muchos problemas y tras horas probando decidí hacerlo de otra manera. He recogido los datos en un archivo .csv. En el archivo iiwa_control_class.py se puede observar como lo he implementado. Básicamente lo que he hecho es guardar los datos de punto de fulcro, EF en la primera posición, TCP en la primera posición, puntos EF tras los diversos incrementos y puntos TCP tras los diversos incrementos. Tras guardar tantas posiciones como he creido conveniente he metido este archivo en un script de matlab que he creado para que me genere un gráfico de dichos puntos.

En la carpeta CSV y grafico que he metido en el repositorio podéis observar los .csv que he obtenido, el script de matlab y los diversos gráficos obtenidos a partir de dichos gráficos. He hecho 5 pruebas, 3 pruebas con longitud de herramienta 0.1275 (datos_robot_1_l_0.1275.csv, datos_robot_2_l_0.1275 y datos_robot_3_l_0.1275), 1 prueba con longitud de herramienta 0.2 (datos_robot_l_0.2.csv) y 1 prueba con longitud de herramienta 0.3 (datos_robot_l_0.3.csv). Cada uno de estos csv como he comentado lo he introducido en el script de matlab y he obtenido su gráfico como os muestro a continuación:

PRUEBA 1 - LONGITUD DE HERRAMIENTA 0.1275

PRUEBA 2 - LONGITUD DE HERRAMIENTA 0.1275

PRUEBA 3 - LONGITUD DE HERRAMIENTA 0.1275

PRUEBA 4 - LONGITUD DE HERRAMIENTA 0.2

PRUEBA 5 - LONGITUD DE HERRAMIENTA 0.3

Todas estas imágenes se pueden encontrar como digo en la carpeta CSV y grafico del repositorio. Como se ve el método funciona perfectamente para varias longitudes de herramienta. Como me comentas incluiré estas imagenes en la memoria para verificar que el método funciona.

A partir de ahora ya solo quedaría redactar la memoria no?

Muchas gracias y espero vuestra respuesta. Un saludo

[mclopez-uma]

Hola Javier,

¿Qué problemas has tenido para generar el bag? Estaría bien poder utilizarlo.

En cuanto a las gráficas, parece que va todo bien sí. Aunque estaría bien tener más puntos por gráfica y más aleatorios, en casi todas las gráficas (salvo la 3, creo) se mantienen fijas las posiciones en dos ejes. Para ello y para optimizar las pruebas que hagas sobre el robot real, haz un nodo que vaya publicando posiciones, cambiado de posición cada cierto tiempo, de manera que las pruebas queden automatizadas. De cara a las pruebas reales habrá que obtener el bag y generar las gráficas también. Cuando lo tengas los subes al repositorio junto con el script de Matlab de manera que nosotras podamos comprobarlo también.

Independientemente de las pruebas en el robot real, además de la memoria (acabo de subir una actualización del documento asociado a su redacción), hay que comentar el código, generar el Doxigen y crear el manual de preparación del entorno y de uso de lo que tú has implementado.

Un saludo, Mª Carmen

[javilara4]

Buenas Mª Carmen

El otro día intenté desarrollar un bag que recogiera los datos que he recogido en el .csv, sin embargo el principal problema que encontré es que el bag está hecho para recoger datos que se publican en topics y en nuestro caso los datos que quiero recoger se calculan dentro del move_cartesian_fulcrum (punto de fulcro, EF en la primera posición, TCP en la primera posición, puntos EF tras los diversos incrementos y puntos TCP tras los diversos incrementos) y luego estos se quedan en el metodo y NO se envían a través de ningún topic. Intenté aún así guardarlos directamente en un bag a pesar de que no se envían por ningún topic pero solo obtenía errores y no encontraba la solución. Usando un archivo .CSV se puede obtener dichos datos con unas lineas de codigo mucho más sencillas y con un resultado muy bueno para despues poder sacar su gráfico en matlab. Si no le importa me gustaría dejarlo así como está y no usar el bag, ya que el otro día me llevó mucho trabajo poder introducir los datos en el .csv y luego crear el script de matlab para crear el gráfico.

En cuanto al nodo que comentas voy a intentar desarrollar ahora uno que de una posición y orientación al robot para conseguir la primera posición (TCP y EF iniciales) y con ellos el punto de fulcro y luego vaya pasando posiciones con incrementos en los tres ejes para que así tengamos datos más aleatorios.

Con respecto a lo que comentas de la memoria tengo varias dudas. Ya he escrito la introducción entera (aunque parece que debo cambiar algunas cosas de la introducción )y un poco del desarrollo teórico. En cuanto a la introducción he hablado un poco de la robotica medica, de su historia y de la cirugía minimamente invasiva, las metas del TFM y la estructura del TFM. Faltaría en esta parte hablar un poco del estado del arte de crear interfaces multimodales para robot quirúrgicos no?

En cuanto al apartado del desarrollo teórico he analizado y descrito el robot que se usa para este trabajo, he hablado de las vias de comunicación con el robot y como debemos conectarnos para usar ROS con él, he comentado las funcionalidades que debe tener en el contexto de la cirugía robótica y ahora estoy realizando el diseño modular de las primitivas del robot. En este diseño modular de las primitivas debo explicar lo que hace cada una de las funciones de mi codigo o solo eltema del movimiento articular, movimiento alrededor del punto de fulcro y movimiento cartesiano libre?

Debo comentar los códigos de todos los archivos que he generado para el paquete iiwa_surgery o para cuales? Estos codigos debo incluirlos en la memoria como anexos imagino no? El doxigen me dijiste que lo hiciera solo de los archivos iiwa_control_node.py y iiwa_control_class.py cierto? Esto una vez generado también se introduce como anexo o cómo? Y lo del tema del manual cómo debo realizarlo? Es otro documento que debo adjuntar en los anexos explicando los pasos de la instalación de la maquina virtual, ubuntu, los paquetes de ros, la conexion con el robot y el posterior uso?

Gracias y en cuanto tenga el nodo que me genere las posiciones aleatorias lo subo y os escribo de nuevo.

[javilara4]

Buenas de nuevo

Ya tenéis en la carpeta src del paquete iiwa_surgey un nodo llamado generate_pose_messages.py. Este nodo se puede ejecutar una vez hallamos hallamos lanzado la interfaz en modo simulado o real y cuando estemos en modo de pivotaje (pivot). Dentro de este nodo y abajo del todo se puede configurar una posición y orientación inicial para el TCP. También en el mismo nodo se puede configurar el incremento máximo mediante el cual se generarar posiciones aleatorias en los tres ejes (x,y,z). Una vez se halla lanzado el nodo, el robot se desplazará hasta la posición inical designada y posteriormente empezará a generar posiciones aleatorias alrededor del punto de fulcro. He hecho una prueba para una longitud de herramienta de 0.1275 m y posteriormente gracias a mi script de matlab he obtenido el siguiente grafico:

Espero que este sí sirva para justificar correctamente el funcionamiento del método.

También he estado mirando un poco lo del tema del doxygen y he visto que una vez se genere la documentación puede generarse de muchas maneras. Para este trabajo cómo se desea que se genere? en pdf, word, como página web?

Muchas gracias y un saludo

[javilara4]

Buenas

Ya he subido al repositorio todo el código que he generado para el paquete iiwa_surgery debidamente comentado y he generado el Doxygen para el archivo iiwa_control_node.py y iiwa_control_class.py. En el repositorio he dejado dos carpetas con la documentación Doxygen de ambos archivos. Para el nodo está la carpeta (Salida documentos doxygen nodo) y para la clase está la carpeta (Salida documentos doxygen clase). Para poder verlo como pagina web, hay que bajarse ambas carpetas y luego pinchar en el archivo index que se encuentra en la carpeta html para ambos archivos. También en la carpeta rtf se ha generado un archivo word con la misma información que se observa en index pero ahora en formato de texto (word).

[mclopez-uma]

Hola Javier,

Claro, tal y como te comenté los bags sólo almacenan topics que se estén publicando, que es justo lo que se necesita. Necesitas almacenar la posición del EF y posteriormente (fuera del nodo) hacer los cálculos que obtienen el punto de fulcro y el TCP, tal y como te expliqué en mi mensaje de hace una semana. Esos cálculos los debes incluir en el script de Matlab que has hecho, éste debe calcularlos y posteriormente representarlos. Sería hacer los cálculos a la inversa una vez que el robot se ha ido a la posición comandada.

Con respecto a la memoria, incluye lo que creas conveniente. Cuando tengas una primera versión ya la miramos nosotras. Ten en cuenta, que la redacción de la memoria va a conllevar bastantes revisiones hasta que se quede lista. Por cierto, ¿qué editor estás utilizando?

En cuanto al Doxigen hay que darle otra vuelta:

• por un lado no logro encontrar la documentación asociada a iiwa_control_class. Las dos carpetas, si no me equivoco, apuntan al nodo. Independientemente de esto, debería aparecer una única documentación que enlace las dos, puesto que el nodo utiliza los métodos de la clase; es decir, no debería aparecer dos carpetas totalmente distintas con dos index.html. Con un index.html se debería poder llegar a la documentación de los dos ficheros.
• Con respecto a iiwa_control_node lo que aparece como métodos y atributos públicos diría que son privados todos.
• No aparece ningún tipo de descripción asociada a los atributos, deben aparecer explicados qué definen y si lo necesitan la unidad correspondiente.
• En los métodos que son la mayoría callbacks hay que indicar para qué se utilizan, por ejemplo en pose_command_callback habría que explicar que se comporta de manera distinta dependiendo de la configuración libre o con fulcro. En cuanto al argumento de entrada, supongo que hay alguna manera de relacionarlo con la definición del tipo de mensaje y si no al menos incluir la url donde se define (puedes encontrarla buscando en Google)
• ¿A qué te refieres con "posición conjunta"? Creo que te refieres a posición articular.

Un saludo, Mª Carmen

[javilara4]

Buenas

Para el tema de la memoria estoy usando LATEX que me genera PDF tras compilación.

En cuanto al tema del bag, una vez ejecutado mi interfaz y antes de ejecutar mi archivo que crea posiciones aleatorias, he puesto a grabar los datos de EF que se publican en el topic /iiwa/command/CartesianPose dentro de mi metodo move_cartesian_fulcrum . Para ello he hecho lo siguiente:

rosbag record -O nombre_del_archivo.bag /iiwa/command/CartesianPose

Una vez he creído conveniente parar de grabar, se ha generado mi archivo .bag. He generado 2 archivos .bag como pruebas. Luego he generado un script de matlab que lee esos .bag y teniendo todos los puntos de EF saca mediante transformaciones los TCP y los une con una linea para ver gráficamente las posiciones de la herramienta como me comentabas que lo hiciera. Tanto dichos archivos, como el script de matlab y los gráficos obtenidos para cada prueba están en la carpeta Bag y grafico del repositorio.

A continuación os dejo uno de los gráficos obtenidos de mi script para comentar cosas que he visto:

Como se puede ver al calcular los TCP a partir de los EF que se mandan por /iiwa/command/CartesianPose de mi método move_cartesian_fulcrum, y aunque se comporta de manera parecida a un movimiento de pivoteo alrededor de un punto, salen posiciones que no se cortan en ningún punto claramente definido. Le he estado dando muchas vueltas tanto al script que he generado como a mi código de move_cartesian_fulcrum y no encuentro ningún fallo. Me da la sensación de que el código que generó Pablo para su TFG podría estar incorrecto.

Él, al igual que yo he hecho con el .csv para mis comprobaciones, ha dibujado las posiciones de TCP y EF tras incrementos que ha obtenido directamente de su código sin hacer el calculo luego a la inversa (como yo sí he hecho ahora). Esto que comento se ve en su memoria tanto en sus experimento 1 como en el 2. Yo he realizado las mismas comprobaciones que hizo él en su momento (que son los gráficos que he realizado a partir de los .csv) y funciona bien como ya hemos visto. Con todo esto vengo a decir que tanto los puntos TCP y EF con incrementos se calculan de manera adecuada siguiendo los pasos de Pablo y parece que sigue una trayectoria alrededor del punto de fulcro si solo nos fijamos en las posiciones que es lo que comprobó Pablo.

Sin embargo, Pablo no comprobó las orientaciones y esto se ve al intentar calcular los TCP a partir de los EF. Como se ve en la imagen que he compartido antes, éstas rectas no se cortan y esto tiene que ser casi seguro por la orientación de los puntos EF tras incrementos, ya que ésta se necesita para el calculo de los TCP (si se hace el calculo a la inversa). En la teoría, mi código funciona perfectamente porque genera bien la posición de los EF tras incrementos a partir de los TCP tras incrementos y todos estos puntos confluyen en el punto de fulcro. Sin embargo, en la práctica cuando se mandan los EF tras incremento por el topic /iiwa/command/CartesianPose para su publicación y al tener estos una orientación inesperada, generan un TCP distinto al que se calcula en mi método, no confluyendo las posiciones generadas en un punto (punto de fulcro).

Lo he revisado y revisado y quiero pensar que mi script no está mal y que lo que no funciona es la forma en la que Pablo saca las orientaciones de los puntos EF tras incremento. He intentado corregir esto de las orientaciones pero sin éxito.

Querría comentarles que este problema al parecer no proceder de mi trabajo, al que le llevo dedicando mucho tiempo, me gustaría dejar mi código como está, pues a través de los .csv puedo hacer las mismas comprobaciones que hizo Pablo. Necesito ponerme con la redacción de la memoria y finalizar el TFM para poder empezar a buscar trabajo cuanto antes.

Mañana revisaré el doxygen y lo corregiré con lo que me comentas.

Muchas gracias

[irivas-uma]

Hola Javier,

• En primer lugar, para hacer las pruebas deberías leer la posición del ef que te da el robot, no la que le comandas, porque esa es la posición real a la que se va el robot (deberían ser iguales, pero lo suyo es utilizar la real).
• Según tu gráfica, el robot no está realizando un movimiento de pivotaje, ya que todas las rectas deberían cortarse en el mismo punto. Por lo tanto, algo tienes que tener mal en tu código. El código de Pablo te lo pasamos para que te sirviera de ayuda. Pablo hizo sus comprobaciones, pero supongamos que el código de Pablo tuviera algún error. Si fuera así, forma parte de tu trabajo corregir lo que no esté bien para que te funcione, o picar tu la parte de Pablo si lo prefieres.
• En las comprobaciones de Pablo, él lee la posición del EF directamente del robot, y con eso calcula la posición del tcp. Ese es el cálculo a la inversa, ya que el cálculo directo es calcular la posición deseada del EF dada una posición del tcp. Cuando tu haces esa prueba te sale la gráfica en la que se ve que las rectas no se cortan en un punto, por lo que no está bien.
• Dices que " En la teoría, mi código funciona perfectamente porque genera bien la posición de los EF tras incrementos a partir de los TCP tras incrementos y todos estos puntos confluyen en el punto de fulcro." Lógicamente, si tu tienes un Punto de fulcro, y generas rectas uniendo ese punto de fulcro con distintas posiciones del tcp, todas se van a cortar en el mismo punto. Ya que estás forzando a que las rectas pasen por ahí. En la práctica no te funciona porque no estarás calculando bien la orientación del EF, y por tanto, aunque la posición del EF se la estés mandando bien, la orientación no.
• Por tanto, es evidente que tu script no está bien. Forma parte de tu trabajo averiguar si el problema está en tu código, en el de Pablo, o en alguna incompatibilidad que haya entre los dos. Pero lo que no puedes es dejar tu trabajo mal porque el código de otra persona esté mal (no creo que el código de Pablo esté mal, pero tu usas cuaternios y Pablo angle-axis, y podría ser que el problema venga de ahí. Si el robot no se mueve bien, no podemos probarlo con el robot real.

Dicho esto, revisando tu código veo varias cosas:

• Primero que para calcular la nueva posición del tcp usas como referencia la inicial. Eso no es así, al robot se le mandan incrementos sobre la posición que tiene actualmente. Es decir, si yo le digo que se mueva 5cm en x (por ejemplo), es sobre la posición actual, no sobre la que tenía al inicio.
• En la línea 257, ¿porque pasas de matriz de rotación a angle-axis, y luego de ahí a cuaternios?¿porqué no pasas de matriz de rotación a cuaternios directamente? Yo creo que tu problema está en la orientación, por lo que empezaría a mirar por ahí. Lo que yo haría es ir poniendo printf por el código y comprobaría poco a poco si los cálculos están bien. A partir del primer cálculo que haces para calcular el punto de fulcro, pondría un incremento, calcularía el EF (posición y orientación) y luego calcularía la posición del tcp como la posición del EF mas la longitud de la herramienta en el eje Z, a ver si te sale igual. Además, las conversiones que haces de orientación, las comprobaría en matlab para ver si están bien. Es decir, cogería a mano la matriz de rotación y en matlab calcularía el cuaternio correspondiente (quat2rotm), y en tú código pones un printf y ves si te sale lo mismo. Es decir, ir comprobando poco a poco si cada paso que vas dando está bien, para poder detectar en qué punto del programa está el fallo.

Entiendo que te quieras poner a buscar trabajo, y eso lo puedes hacer de forma paralela a terminar el trabajo ya que hasta diciembre de todas formas no puedes leer el TFM. Pero lo que no puedes es presentar un trabajo que no funciona y decir que el problema es de un TFG anterior.

Un saludo.