Simulación del robot KUKA YouBot en el entorno de CoppeliaSim

Equation Chapter 1 Section 1 Proyecto Fin de Carrera Ingeniería Electrónica, Robótica y Mecatrónica Simulación del robot KUKA YouBot en el entorno de CoppeliaSim Autor: Francisco José Rivero González Tutor: Manuel Vargas Villanueva Dpto. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 iii iii Proyecto Fin de Carrera Ingeniería Robótica, Electrónica y Mecatrónica Simulación del robot KUKA YouBot en el entorno de CoppeliaSim Autor: Francisco José Rivero González Tutor: Manuel Vargas Villanueva Profesor titular Dpto. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2021 v v Proyecto Fin de Carrera: Simulación del robot KUKA YouBot en el entorno de CoppeliaSim Autor: Francisco José Rivero González Tutor: Manuel Vargas Villanueva El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2021 El Secretario del Tribunal vii vii A mi familia A mis maestros A mis amigos A todos aquellos que han estado en los buenos momentos y, sobre todo, en los malos. ix ix Índice Agradecimientos ........................................................................................................................................... x Resumen ...................................................................................................................................................... xii Abstract ....................................................................................................................................................... xiv Índice .......................................................................................................................................................... xvi Índice de Figuras ....................................................................................................................................... xviii Notación...................................................................................................................................................... xxi Introducción ........................................................................................................................................... 1 1.1. Motivación ........................................................................................................................................................ 1 1.2. Estado del arte y objetivos............................................................................................................................... 2 1.3. Estructura del trabajo ...................................................................................................................................... 4 Entorno de simulación ........................................................................................................................... 5 2.1. Introducción a CoppeliaSim ............................................................................................................................. 5 2.2. Escena y jerarquía ............................................................................................................................................ 6 2.3. Scripts embebidos ............................................................................................................................................ 8 2.4. Formas primitivas ............................................................................................................................................. 9 2.5. Path ................................................................................................................................................................. 11 2.5.1. Trayectorias del brazo robótico ....................................................................................................... 11 2.5.2 Trayectoria de la base móvil ............................................................................................................. 12 2.6. Sensores .......................................................................................................................................................... 13 2.6.1 Visión .................................................................................................................................................. 13 2.6.2 Proximidad ......................................................................................................................................... 13 2.6.2.1 Tipo Haz ...................................................................................................................................... 14 Brazo robótico ..................................................................................................................................... 17 3.1. Morfología ...................................................................................................................................................... 17 xvii xvii 3.2. Uso del módulo de cinemática inversa en CoppeliaSim .............................................................................. 18 3.2.1 Configuración del modo IK ................................................................................................................ 19 3.2.1.1 Enlazar la muñeca con el objetivo ............................................................................................ 19 3.2.1.2 Añadir el grupo IK y el elemento IK .......................................................................................... 20 3.3. Control del brazo robótico ............................................................................................................................. 21 3.3.1 Movimiento coger/soltar pieza ........................................................................................................ 22 3.3.2 Apertura/cierre de la pinza ............................................................................................................... 23 Plataforma móvil ................................................................................................................................. 25 4.1 Características ............................................................................................................................................ 25 4.2 Modelo cinemático inverso de la base móvil ........................................................................................... 26 4.3 Implementación del modelo en Lua ......................................................................................................... 29 4.3.1 Obtención de la velocidad angular de las ruedas ............................................................................ 29 4.4 Control de las ruedas ................................................................................................................................. 32 4.5 Sensores asociados a la base .................................................................................................................... 33 4.6 Código de la rutina principal ..................................................................................................................... 36 Conclusiones y futuras mejoras ........................................................................................................... 37 Anexo A - Códigos ....................................................................................................................................... 39 Referencias ................................................................................................................................................. 46 ÍNDICE DE FIGURAS Figura 1: Robots en la industria automovilística. ...................................................................................................... 2 Figura 2: Spot, robot fabricado por Boston Dynamics. ............................................................................................ 3 Figura 3: KUKA YouBot. .......................................................................................................................................... 3 Figura 4: Modelo KUKA youBot en CoppeliaSim. ................................................................................................... 4 Figura 5: Ventana de inicio de CoppeliSim. 1: Escena, 2: Jerarquía de la escena, 3: Buscador de modelos, 4: Barra de herramientas 2, 5: Barra de herramientas 1, 6: Barra de menú, 7: Barra de estado. ................................ 5 Figura 6: Escena del proyecto. ................................................................................................................................... 7 Figura 7: Vista de la customización del tamaño de escena. ..................................................................................... 7 Figura 8: Vista de la jerarquía de escena. .................................................................................................................. 8 Figura 9: Vista de los distintos scripts; derecha: non-threaded child script, izquierda: threaded child script. ...... 9 Figura 10: Selección de una nueva forma primitiva. ................................................................................................ 9 Figura 11: Vista de las propiedades del objeto. ....................................................................................................... 10 Figura 12: Primera trayectoria izquierda; segunda trayectoria derecha. ............................................................... 11 Figura 13: Vista de la edición del elemento path. 1: Modo edición de caminos, 2: Puntos de control, 3: Cuadro de edición, 4: puntos de control y camino representado en la escena.................................................................... 12 Figura 14: Diferentes tipos de sensores de proximidad. ......................................................................................... 13 Figura 15: Sensores integrados en el robot youBot. ................................................................................................ 14 Figura 16: Propiedades de los sensores. .................................................................................................................. 14 Figura 17: Planos del brazo robótica de KUKA YouBot ....................................................................................... 17 Figura 18: Modelo del brazo en CoppeliaSim ........................................................................................................ 18 Figura 19: Selección del IK mode en los eslabones ................................................................................................ 19 Figura 20: Vista del proceso para enlazar el efector final y el objetivo ................................................................. 20 Figura 21: Vista de las pantallas para añadir el grupo IK ....................................................................................... 20 Figura 22: Planos e imagen de la base móvil .......................................................................................................... 25 xix xix Figura 23: Configuración de accionamiento de las ruedas Mecanum para el movimiento del robot. ................. 26 Figura 24: Ruedas en el modelo de CoppeliaSim ................................................................................................... 26 Figura 25: Modelo cinemático .................................................................................................................................. 27 Figura 26: Vista de los ejes en la trayectoria ........................................................................................................... 31 Figura 27: Captura de la barra de estado .................................................................................................................. 35 xxi xxi Notación CAD Diseño asistido por ordenador DLS Damped least squares tan Función tangente sin Función seno cos Función coseno IK Inverse Kinematic (Cinemática Inversa) 1 INTRODUCCIÓN Los países con la mayor densidad de robots tienen también las tasas de desempleo más bajas. La combinación correcta de tecnología y humanos impulsarán la prosperidad. Ulrich Spiesshofer, presidente y CEO de ABB oy en día cada vez es más frecuente ver robots en nuestro día a día y en nuestros trabajos. Los robots ya no son solo industriales y, por lo tanto, se pueden ver en un restaurante, bar o cafetería sirviendo bebidas en la barra o en la entrada de un museo para interactuar con los visitantes a modo de recepcionista. Este trabajo pretende mostrar la simulación de un robot real, llamado KUKA youBot, y que se encuentra en el mercado, a través del entorno de simulación de CoppeliaSim. Este software es gratuito y cuenta con una versión dedicada a la educación pensada para el uso de profesores, estudiantes, escuelas y universidades. 1.1. Motivación Personalmente he elegido este trabajo por varios motivos, uno de los principales era el reto de aprender a usar un entorno de simulación totalmente desconocido para mí como es CoppeliaSim y programarlo usando el lenguaje Lua. La curva de aprendizaje de este programa puede ser alta al principio debido a su alta complejidad y multitud de opciones. Por otro lado, también tenía la intención de poder simular y realizar un trabajo sobre un robot real, es decir, que exista en el mercado. Esto era algo que me llamaba la atención y me apetecía por poder ver cómo controlar un robot ya diseñado y fabricado. Además, durante mis prácticas en un entorno industrial que trabaja con elementos, descubrí que la logística (transporte, almacenaje, etc.) era una parte muy importante del proceso, por ello quería usar en este trabajo un robot manipulador móvil. Este tipo de robots combina una base móvil con un brazo robótico. Esta configuración permite desplazar al brazo robótico por distintas zonas aumentando su área de trabajo y versatilidad. Después de documentarme sobre el software CoppeliaSim y sus posibilidades descubrí, en la versión utilizada, el modelo del robot youBot. Este es un robot manipulador móvil que la empresa de robots KUKA había añadido para su simulación en este entorno. Por otro lado, no encontré casi ningún proyecto que utilizara este robot en particular u otros manipuladores móviles. Esto despertó aún más mi curiosidad y ganas de trabajar sobre el control de este robot. Con este proyecto pretendo crear una base de conocimiento para que ayude tanto H Introducción 2 a entender el software de CoppeliaSim como el uso de los robots manipuladores móviles, en concreto el robot KUKA youBot en este entorno. Todo lo que aquí se expone, puede desarrollar mejoras simuladas del robot que más tarde se puedan implementar en su versión real. 1.2. Estado del arte y objetivos El estudio y manejo de robots se lleva realizando desde hace varias décadas y desde entonces se han ido usando nuevos materiales, lenguajes de programación y configuraciones. Al principio se creaban robots para realizar tareas repetitivas y costosas en entornos industriales. Por ejemplo, son muy conocidos los brazos robóticos en las líneas de montaje de coches. Figura 1: Robots en la industria automovilística. El brazo robótico es uno de los muchos robots que existen e intenta imitar el brazo humano, de hecho, siempre se habla de sus partes haciendo referencia al hombro, codo y muñeca dependiendo del número de articulaciones. Al igual que ocurre con un brazo humano, los brazos robóticos se mueven con articulaciones rotatorias y se pueden equipar con distintas herramientas dependiendo del trabajo que vayan a realizar. Algunos ejemplos pueden ser una pistola de pintura, un soldador o simplemente una pinza para coger objetos. A parte de esos brazos, hay otros que intentan ser más humanos, por ello se asemejan en apariencia y poseen dedos. Estos robots se suelen denominar humanoides y se diseñan para imitar los movimientos y la apariencia humana. Por otro lado, se tienen los robots móviles que son aquellos capaces de desplazarse en un entorno de forma automática. Dentro de esta categoría se pueden distinguir entre los terrestres, marinos o acuáticos y aéreos. También existen robots híbridos que combinan algunas categorías anteriores y por supuesto, las configuraciones son muy variadas y a veces intentan imitar animales, tanto en aspecto como en movimiento. Centrándose en los robots móviles, se puede observar esto en robots con patas. 3 Figura 2: Spot, robot fabricado por Boston Dynamics. Además, dentro de los robots móviles terrestres se encuentran aquellos que se desplazan con ayuda de ruedas. En el mercado se pueden encontrar multitud de configuraciones y ruedas distintas. Últimamente, se está buscando combinar brazos robóticos con robots móviles para aumentar las capacidades y posibilidades de actuación. El objetivo de este trabajo consiste en programar el movimiento de un robot que combina una plataforma móvil con un brazo robótico. Dentro de los modelos disponibles en el entorno de simulación de CoppeliaSim, la opción elegida para el proyecto ha sido un robot llamado KUKA youBot, el cual es un robot manipulador móvil con ruedas omnidireccionales del tipo Mecanum. Se busca tanto programar el movimiento de la plataforma como el movimiento del brazo robótico. Para ello, se ha decidido que el robot siga una trayectoria en la que primero tomará un objeto de una mesa y posteriormente, la dejará en otra mesa antes de volver a su posición inicial. [1] A parte, se han añadido dos sensores tipo rayo en sus laterales para que el robot se sitúe a una distancia predefinida de las mesas. Con esto, se quiere mostrar la posibilidad de incorporar sensores al robot, los cuales no vienen de fábrica, para su mejor interacción con el entorno de su alrededor. Figura 3: KUKA YouBot. Fuente: maxongroup Entorno de simulación 10 Los objetos creados, se pueden rotar y trasladar por la escena además se le pueden asociar scripts. Como se observa en la figura 11, se pueden personalizar en tamaño y color. Uno de los rasgos más importantes es definir su interacción con el entorno. Esto significa que se pueden modificar sus propiedades dinámicas y físicas. Los objetos pueden verse afectados por la física (gravedad), se pueden habilitar o deshabilitar las colisiones con otros objetos y decidir que el objeto pueda ser detectado por sensores, incluso que sensores. Esto permite una gran configuración y realismo en la simulación. Para acceder al cuadro de las propiedades solo es necesario hacer clic en la figura a través del recuadro de la jerarquía (figura 8). Al hacer esto, aparecerá una ventana emergente y seleccionamos el apartado Common. La figura siguiente muestra la ventana emergente. Para este proyecto es importante que la mesa sea detectable y dentro de las propiedades esté seleccionada la opción laser, ya que el sensor del robot es de este tipo. Figura 11: Vista de las propiedades del objeto. Las propiedades especiales del objeto son: • Collidable: El objeto puede ser comprobado para saber si está en colisión con otros objetos. Esto no implica que reaccione frente a una colisión, para ello el objeto debe ser respondable. • Detectable: El objeto puede ser detectado por sensores de proximidad (ultrasonido, láser, etc.). • Renderable: El objeto puede ser visto por sensores de visión (cámaras con capacidad de procesamiento). • Measurable: El objeto puede ser usado para el cálculo de una distancia mínima. 11 2.5. Path El camino o path en inglés es otro de los objetos que CoppeliaSim permite usar y cuyo uso se ha implementado en este proyecto. Este tipo de objeto se puede añadir a la escena a través de [Menu bar → Add → Path] y se representa como una sucesión de puntos con orientación en el espacio. El entorno de CoppeliaSim permite seleccionar entre dos tipos de caminos, circular y de tipo segmento. Todos los caminos usados en este proyecto son de tipo segmento. Una vez creado, se puede modificar y usar funcionalidades internas que permiten conocer la posición global de sus puntos entre otras, tal y como se verá a continuación. [8] 2.5.1. Trayectorias del brazo robótico El movimiento del brazo que lleva incorporado el robot youBot se comentará en el capítulo siguiente. Ahora, se mencionará la trayectoria que sigue el brazo para coger el objeto de la mesa y dejarlo en su plataforma. Para este proceso es indispensable el objeto path. Se han creado dos caminos en los dos puntos donde el brazo se activa, el primer punto es donde se recoge al cubo que se transporta y el segundo punto es donde el cubo es depositado. El funcionamiento de los dos caminos es idéntico a la hora de diseñarlo, funcionalidad y propósito, por lo tanto, solo se mostrará la funcionalidad usando de ejemplo el camino creado en el primer punto. Figura 12: Primera trayectoria izquierda; segunda trayectoria derecha. En el primer punto se ha divido la trayectoria total en dos caminos llamados take y take1. El primer tramo (take) comienza cuando llega el robot al punto y termino una vez que coge el cubo. El segundo tramo (take1) es la continuación del anterior y termina cuando se deja el cubo en la plataforma del robot. Una vez que el robot llega al segundo punto se divide el proceso en tres tramos, estos son soltar, soltar1 y soltar2. Lo primero en este caso es coger el cubo de la base móvil (soltar), dejarlo en la mesa (soltar1) y por último volver a su posición inicial (soltar2). Para definir las propiedades del objeto path es necesario seleccionarlo después de crearse y hacer clic en el modo de edición de caminos. Este proceso se puede observar en la siguiente figura. Entorno de simulación 12 Figura 13: Vista de la edición del elemento path. 1: Modo edición de caminos, 2: Puntos de control, 3: Cuadro de edición, 4: puntos de control y camino representado en la escena. Las propiedades elegidas para ambos caminos se pueden ver en la figura anterior. Como se puede observar ambos caminos están en el espacio por lo que tienen altura, esto permite movimientos en el espacio 3D. Lo importante del camino es definir correctamente los puntos de control y el orden de estos. Cuando se crea un camino aparecen dos puntos de control indicando posición inicial y final. Para añadir más puntos y poder crear diferentes formas y curvas, solo es necesario seleccionar un punto y usar copiar (Ctrl+C) y pegar (Ctrl+V). Los puntos seleccionados cambian de color en la escena para poder localizarlos mejor, luego se trasladan y rotan como un objeto de la escena, manteniendo en este caso el enlace con el punto anterior y posterior. Hay que planificar y conocer la ruta que se quiere hacer y en qué sentido. Una de las condiciones que se necesitan para mover el brazo es seguir la trayectoria, antes descrita, siguiendo un orden. Los caminos se usan para describir la trayectoria de un punto durante la simulación. Gracias a unas funciones internas se puede determinar durante la simulación y modificar propiedades de ese punto como su posición y velocidad. El concepto usado para mover el brazo es el de seguir, mediante cinemática inversa, a ese punto mientras se desplaza por el camino. La trayectoria de un punto de referencia que se sitúa en la pinza del brazo será idéntica a la del punto en movimiento, siempre y cuando la configuración del brazo permita alcanzarla. 2.5.2 Trayectoria de la base móvil La trayectoria de la base móvil y por lo tanto del robot en la simulación, sigue la misma mecánica que la del brazo. En este caso se compone de tres elementos path denominados ruta1, ruta2 y ruta3. Al igual que para el brazo habrá un punto que recorra el path a una cierta velocidad. Ese punto se usará como referencia para los parámetros de entrada que necesita la función que mueve cada una de las ruedas del robot. Al ser un robot con 13 ruedas omnidireccionales del tipo Mecanum, el movimiento de cada rueda es independiente del resto. El código específico que permite conocer que velocidad angular hay que aplicar en cada momento a cada rueda, junto con las características de la base, se explican en detalle en el capítulo 4. 2.6. Sensores Este apartado está dedicado a los sensores que CoppeliaSim permite integrar en la escena. Dentro de este programa existen tres grupos de sensores: visión, proximidad y fuerza. Aunque solo se ha implementado el sensor de proximidad, se hará una breve introducción de los sensores de visión. 2.6.1 Visión Los sensores de visión pueden ser de tipo ortográfico (campo de visión rectangular) o perspectiva (campo de visión trapezoidal). Estos sensores simulan cámaras que pueden ser procesadas y obtienen imágenes de los objetos, siempre que entre sus propiedades este seleccionada esta opción. Las vistas de estos sensores pueden ser mostrada durante la simulación. El programa tiene un elemento “Cámara” que permite ver diferentes vistas de la escena, no hay que confundirlo con los sensores de visión. 2.6.2 Proximidad Los sensores de proximidad simulan sensores del tipo infrarrojo, capacitivo, ultrasonido o láser, que es el elegido en este proyecto. Su función, además de detectar a los objetos que se encuentren en su volumen de detección, es medir la mínima distancia con ellos. Según el volumen se pueden distinguir seis tipos, tal y como aparecen en la siguiente figura. Figura 14: Diferentes tipos de sensores de proximidad. Fuente: https://www.coppeliarobotics.com/helpFiles/en/proximitySensorDescription.htm Todos los sensores tienen las mismas funcionalidades y propiedades. Lo que los diferencia es el volumen detectable que puede ser personalizable dentro de los diferentes tipos que existen. Cada uno de ellos pueden servir para simular diferentes sensores reales, por ejemplo, para un sensor de ultrasonidos se usaría un sensor tipo cono, y dentro del mismo, se podrían seleccionar su rango de detección, offset y su radio. Entorno de simulación 14 2.6.2.1 Tipo Haz El sensor que se ha implementado en este proyecto es de tipo haz o rayo. Se ha colocado uno a cada lado del robot y simulan dos sensores láser. Estos sensores surgen como una mejora del movimiento de la base móvil. Con su uso se consigue que el robot detecte las mesas y se acerque o aleje una distancia dada previamente. El código asociado al sensor se explica en el capítulo 4. Figura 15: Sensores integrados en el robot youBot. Este sensor se crea con [Menu bar → Add → Proximity sensor → Ray type]. Una vez creado se mueve con la opción de traslación hasta la posición deseada en el robot. Para que el sensor se mueva junto con el robot es importante definir la jerarquía de ambos. Como se observa en la figura anterior, los dos sensores son hijos de la plataforma que, a su vez, es hija del modelo completo del robot (se representa como youBot en la jerarquía). Una vez realizado este proceso se han modificado ciertas propiedades. Haciendo doble clic sobre el sensor en la jerarquía de la escena se accede a sus propiedades. Hay que hacer clic en la figura pequeña, si se hace en el nombre, solo permite editarlo. Figura 16: Propiedades de los sensores. 15 En la figura anterior, aparecen los dos cuadros de propiedades del sensor. Es importante que se encuentren marcados las opciones de Laser (sensor subtype) y Ray (volumen type). 17 BRAZO ROBÓTICO ste capítulo trata sobre el brazo robótico y su control, una de las partes principales del robot KUKA youBot. A continuación, se detallan características del robot y se compara el robot real con su modelo en CoppeliaSim. Las técnicas de control explicadas para el funcionamiento de este brazo robótico pueden ser extrapoladas a otros brazos robóticos dentro del entorno de simulación. 3.1. Morfología El brazo integrado en el robot KUKA youBot tiene un total de 5 grados de libertad. Se compone de 5 articulaciones de rotación y sus correspondientes eslabones en serie. Es una mecánica típica en un brazo robótico. El efector final de la muñeca es una pinza que permite coger objetos de cierto tamaño. Las pinzas, a diferencia del brazo, tienen articulaciones prismáticas que permiten su cierre y apertura. Los rangos de movimiento y las dimensiones del robot se pueden observar en las siguientes figuras. Figura 17: Planos del brazo robótica de KUKA YouBot Fuente: https://www.generationrobots.com/img/Kuka-YouBot-Technical-Specs.pdf Todas estas especificaciones son trasladas al modelo que se encuentra en CoppeliaSim. Para obtener el modelo del robot, es necesario seleccionarlo en [Model browser → robots → mobile → KUKA YouBot.ttm]. En la figura siguiente se muestran las distintas articulaciones sobre el modelo. El modelo ya incluye las restricciones de rango de movimiento. E Brazo robótico 18 Figura 18: Modelo del brazo en CoppeliaSim Para el estudio de los brazos robóticos y su modelo cinemático, tradicionalmente se ha recurrido al método de Denavit-Hartenberg. Este método permite establecer los sistemas de referencia de los eslabones que forman el brazo robótico. Sin embargo, para este trabajo se ha optado por usar el módulo de cinemática inversa que incluye el software de CoppeliaSim. De este modo, no es necesario definir el modelo cinemático ni los parámetros del brazo. Lo único necesario es definir correctamente una serie de propiedades de los objetos que forman el brazo para acceder a su control posteriormente. Tal y como se observa en la figura anterior, es fundamental crear correctamente la jerarquía para evitar fallos y definir correctamente la cadena cinemática. 3.2. Uso del módulo de cinemática inversa en CoppeliaSim Antes de explicar cómo usar el módulo que integra CoppeliaSim de la cinemática inversa, es importante conocer el funcionamiento y lo que se pretende conseguir. La cinemática inversa es un conjunto de ecuaciones que permiten conocer la posición del robot (cada una de sus articulaciones) para alcanzar un punto con su efector final [9]. Por este motivo, es tan importante definir la cadena del robot al que se le va a aplicar este módulo. La base y el efector final deben empezar y finalizar la jerarquía en forma de árbol. La tarea de resolver la cinemática inversa o IK (Inverse Kinematic) es definida por un grupo IK que contiene uno o varios elementos IK [10]. Para resolver la cinemática de una simple cadena es necesario definir un grupo IK con algún elemento IK. Este grupo se encarga de definir las propiedades generales. Cada elemento IK representa una cadena cinemática simple con algunos de sus eslabones definidos en modo cinemática inversa (IK mode) [11]. Básicamente estas cadenas están formadas inicialmente por una base, una serie de eslabones y una muñeca o efector final. Este 19 objeto debe estar enlazado con un objetivo. El elemento objetivo representa la posición y/u orientación que debe alcanzar el efector final tras resolver la cinemática inversa. 3.2.1 Configuración del modo IK Lo primero es seleccionar los eslabones desde el 0 (youBotArmJoint0) hasta el 3 (youBotArmJoint3) y acceder a sus propiedades para elegir el modo de cinemática inversa. Se accede a las propiedades a través de la “lupa” que se encuentra a la izquierda. Este proceso se muestra en la siguiente imagen: Figura 19: Selección del IK mode en los eslabones Para el caso de los eslabones prismáticos de la pinza, se debe seleccionar el modo par/fuerza de la misma manera que se ha hecho en el caso anterior. Esto permitirá controlar la fuerza en lugar de la posición del eslabón, justo lo que se necesita para la apertura y cierre de la misma. 3.2.1.1 Enlazar la muñeca con el objetivo Ahora se crearán dos elementos en la escena denominados “Dummy”. La función de estos objetos es la de hacer de objetivo y posición final, respectivamente, de la cadena cinemática del brazo. Las propiedades de estos objetos permiten que puedan ser desplazados durante la simulación y, además, se puede acceder a su posición y orientación en todo momento. El objetivo es que un dummy sea perseguido por el otro. El dummy definido como posición final (será el punto que persiga), se coloca en medio de las pinzas y se representa visualmente como una esfera. Esto es así, para lograr un mejor agarre de los objetos a la hora de cogerlos y programar el movimiento. Después de crear estos objetos de referencia y posicionar el de la muñeca, se procede a enlazarlos. Seleccionando el dummy de la muñeca (youBot_armTip) se hace clic en el botón izquierdo con forma de lupa. Plataforma móvil 26 Figura 23: Configuración de accionamiento de las ruedas Mecanum para el movimiento del robot. Fuente: Mecanum_wheel 4.2 Modelo cinemático inverso de la base móvil Una vez que se añade el modelo a la escena, este tiene definidas las cuatro ruedas y cuáles son las delanteras y traseras. Esto se debe tener en cuenta para realizar el esquema que ayudará a obtener el modelo cinemático inverso. Figura 24: Ruedas en el modelo de CoppeliaSim Según la configuración del robot, los rodillos de la rueda de la esquina superior derecha tienen la misma orientación que los de la rueda inferior izquierda, lo mismo ocurre para el caso de las otras dos, pero en sentido contrario. Cómo se observa en la figura siguiente que hace referencia al modelo del robot, las ruedas 1 y 3 son iguales y lo mismo ocurre entre las ruedas 2 y 4. Se calcula el movimiento en función al punto central del robot. Debido a la mecánica de la rueda Mecanum, cuando giran hacia adelante o hacia atrás también se 27 produce un movimiento lateral. Según el esquema, al girar las cuatro ruedas hacia delante las fuerzas o velocidades laterales se contrarrestan evitando el movimiento lateral. Esto es importante para realizar el modelo y para entender el porqué de esta distribución de las ruedas. Si quisiéramos un desplazamiento, por ejemplo, hacia la derecha, las ruedas 1 y 3 rotarían hacia atrás y las ruedas 2 y 4 hacia delante. En este caso, se quiere obtener el modelo cinemático inverso de las ruedas. Esto permite obtener las velocidades angulares de cada rueda en función del movimiento del punto central [15]. Figura 25: Modelo cinemático Siguiendo como modelo el esquema anterior se obtiene que el movimiento del punto central se puede descomponer en una velocidad en el eje X, otra en el eje Y y una velocidad angular de rotación. (𝑣𝑥,𝑣𝑦,𝜔) (1) donde se tiene la relación: 𝑣𝑥=𝑣𝑠𝑖𝑛𝜃 𝑣𝑦=𝑣𝑐𝑜𝑠𝜃 (2) Por otro lado, al mover una rueda hacia adelante se sabe que también se produce un desplazamiento proporcional hacia un lateral. El movimiento de la rueda se puede igualar al movimiento del punto central de esta manera se obtiene la siguiente relación: 𝑣𝑖+𝑘𝑣𝑖𝑐𝑜𝑠𝛼𝑖=𝑣𝑦+𝑎𝑖𝜔 𝑘𝑣𝑖𝑠𝑖𝑛𝛼𝑖=𝑣𝑥+𝑏𝑖𝜔 (3) donde i representa el número de la rueda y 𝛼𝑖 es la inclinación de los rodillos, en este caso, 𝛼𝑖=45º: 𝑖={1,2,3,4} Plataforma móvil 28 Aplicando una pequeña transformación en la ecuación (3) se obtiene: 𝑡𝑎𝑛𝛼𝑖=𝑘𝑣𝑖𝑠𝑖𝑛𝛼𝑖 𝑘𝑣𝑖𝑐𝑜𝑠𝛼𝑖=𝑣𝑥+𝑏𝑖𝜔 −𝑣𝑖+𝑣𝑦+𝑎𝑖𝜔 (4) Se vuelve a transformar: 𝑣𝑖=𝑣𝑦+𝑎𝑖𝜔−𝑣𝑥+𝑏𝑖𝜔 𝑡𝑎𝑛𝛼𝑖 (5) Ahora se sustituyen los valores para cada rueda. Particularizando para el ángulo de orientación de los rodillos en este caso: 𝑣1=𝑣𝑦−𝑣𝑥+𝑎𝜔+𝑏𝜔 𝑣2=𝑣𝑦+𝑣𝑥−𝑎𝜔−𝑏𝜔 𝑣3=𝑣𝑦−𝑣𝑥−𝑎𝜔−𝑏𝜔 𝑣4=𝑣𝑦+𝑣𝑥+𝑎𝜔+𝑏𝜔 (6) La velocidad de la rueda se puede suponer: 𝑣𝑖=𝜔𝑖𝑅 (7) Finalmente se puede obtener las ecuaciones que permiten relacionar el movimiento de cada rueda con el movimiento del punto central. [𝜔1 𝜔2 𝜔3 𝜔4]=1𝑅[−1 1 (𝑎+𝑏) 1 1 −(𝑎+𝑏) −1 1 −(𝑎+𝑏) 1 1 (𝑎+𝑏)][𝑣𝑥 𝑣𝑦 𝜔] (8) Conociendo las dimensiones de la base móvil (a y b) y el radio de la rueda (R) se consigue la relación entre el movimiento de cada rueda y el punto central. A partir de estas ecuaciones se pueden obtener de forma clara los principales movimientos y como se ajustan a lo espero. Para un movimiento hacia adelante o atrás todas las ruedas tienen la misma velocidad y mismo sentido. Para un movimiento de rotación las ruedas en el mismo lado tienen la misma velocidad, pero sentidos opuestos a los del otro lado. Para un movimiento lateral las ruedas en esquinas opuestas tienen la misma velocidad, pero sentidos opuestos de giro con las otras [16]. 29 4.3 Implementación del modelo en Lua El siguiente paso es implementar las ecuaciones del modelo que se han deducido, en el entorno de simulación. El movimiento del robot y el modelo se incluyen dentro de un non-threaded child script asociado al objeto plataforma (ME_Platfo2_sub1). Las distintas constantes del robot que dependen de su geometría son las siguientes: La velocidad de las ruedas se asigna a la velocidad angular con el signo negativo debido al modelo de la simulación. Presentan esta particularidad en el software y es necesario para que se muevan correctamente las ruedas. 4.3.1 Obtención de la velocidad angular de las ruedas El siguiente paso tras implementar la función para obtener la velocidad angular de cada rueda, es calcular los valores de las variables de entrada de la función. Además, de las constantes (radio de las ruedas y medidas de la plataforma) es necesario conocer la velocidad lineal en los ejes X e Y y el giro del punto central del robot en cada instante. Esto con respecto a un punto de referencia que se encarga de marcar la trayectoria y que se mueve con velocidad constante, definida antes de iniciar cada tramo. Esta implementación se desarrolla dentro del mismo script, pero en la función predefinida de actuación. A las funciones que aparecen cuando se crea este tipo de script se denomina callback function, son cuatro y la única obligatoria es la de inicialización. En este caso, la función de actuación se ejecuta en cada paso de simulación, durante la fase de actuación y se encarga de manejar los diferentes actuadores de la simulación, como es el caso de las articulaciones que mueven las ruedas. -- youBot size parameters wheel_R = 0.05 -- 0.05 m a = 0.15 b = 0.235 -- Inverse kinematics function movementOmniwheel(vx, vy, omega, wheel_R, a, b) omega_1 = (vy - vx + (a+b)*omega)/wheel_R omega_2 = (vy + vx - (a+b)*omega)/wheel_R omega_3 = (vy - vx - (a+b)*omega)/wheel_R omega_4 = (vy + vx + (a+b)*omega)/wheel_R -- set the right direction for each wheel v_wheel_1 = -omega_1 v_wheel_2 = -omega_2 v_wheel_3 = -omega_3 v_wheel_4 = -omega_4 end Plataforma móvil 30 El punto que se mueve siguiendo la trayectoria de la base móvil está declarado en la variable youBot_target. El movimiento de este punto se programa en el código de la rutina principal. Este punto se mueve con una velocidad constante que va a ser diferente en cada trayectoria. Existen tres trayectorias que van desde el inicio hasta la mesa con el objeto, luego de esta mesa hasta la segunda mesa donde lo deposita y, por último, de esta segunda mesa hasta la posición inicial. La variable delta_pos almacena la posición relativa del objeto youBot_target con el robot you_bot. Por otro lado, la variable delta_ori almacena la orientación relativa del objeto youBot_target con el robot you_bot. La primera variable almacena la posición (x, y, z) y la segunda la orientación en ángulos de Euler (alpha (a), beta (b), gamma (g)). Estas variables nos permiten conocer los parámetros de entrada de la función, que son las velocidades lineales y el giro del punto central. Ahora es importante elegir bien las variables que necesitamos, ya que tenemos tres valores de posición y tres valores de orientación, pero solo necesitamos tres valores de entrada. Para las velocidades Vx y Vy se utilizan delta_pos[2] y delta_pos[3] respectivamente. Esto puede resultar extraño porque son las posiciones de los ejes Y e Z, pero tiene la siguiente explicación: cuando se crea el elemento path este se crea con un sistema de coordenadas en el que el eje X es vertical y el eje Z es el que sigue la trayectoria. A continuación, se muestra la vista del camino: function sysCall_actuation() delta_pos = sim.getObjectPosition(youBot_target, you_bot) delta_ori = sim.getObjectOrientation(youBot_target, you_bot) center_velocity = {delta_pos[2], delta_pos[3], delta_ori[1]} movementOmniwheel(center_velocity[1], center_velocity[2], center_velocity[3], wheel_R, a, b) -- Apply the desired wheel velocities sim.setJointTargetVelocity(wheel_joints[1], v_wheel_1) sim.setJointTargetVelocity(wheel_joints[2], v_wheel_2) sim.setJointTargetVelocity(wheel_joints[3], v_wheel_3) sim.setJointTargetVelocity(wheel_joints[4], v_wheel_4) end 31 Figura 26: Vista de los ejes en la trayectoria Para la variable delta_ori se toma su primer valor que hace a la rotación sobre el eje X. Ahora es el momento de introducir estos valores en la función que calcula la velocidad angular de cada rueda y obtener, por tanto, los valores de v_wheel_1, v_wheel_2, v_wheel_3 y v_wheel_4. Por último, se asocia cada valor a su rueda correspondiente usando la función sim.setJointTargetVelocity(), tal y como se observa en el código siguiente. Hay que tener en cuenta cada rueda del modelo de CoppeliSim a la hora de asociarlo a las definidas dentro de la función movementOmniwheels(), para no equivocarse con lo establecido en el esquema de la figura 25, que es de donde se han obtenido las ecuaciones. La asociación de las variables con las ruedas del robot en la simulación se realiza al principio del script, dentro de la función sysCall_init(). El código empleado para ello es el siguiente: -- Apply the desired wheel velocities sim.setJointTargetVelocity(wheel_joints[1], v_wheel_1) sim.setJointTargetVelocity(wheel_joints[2], v_wheel_2) sim.setJointTargetVelocity(wheel_joints[3], v_wheel_3) sim.setJointTargetVelocity(wheel_joints[4], v_wheel_4) -- Prepare initial values for four wheels wheel_joints = {-1,-1,-1,-1} -- front right, front left, rear left, rear right wheel_joints[1] = sim.getObjectHandle('rollingJoint_fr') wheel_joints[2] = sim.getObjectHandle('rollingJoint_fl') wheel_joints[3] = sim.getObjectHandle('rollingJoint_rl') wheel_joints[4] = sim.getObjectHandle('rollingJoint_rr') Plataforma móvil 32 Se observa que para evitar confusión se han asociado usando el esquema teórico, la rueda uno se asocia con la delantera derecha (fr, front-right), la dos con la delantera izquierda (fl, front left) y así hasta asociar las cuatro. 4.4 Control de las ruedas El modelo posee unos scripts asociados a cada rueda cuando se crea el modelo en escena. Estos scripts son idénticos, la única diferencia son las variables a las que hacen referencia que son las propias de la rueda asociada. Su función es la de permitir el movimiento de las ruedas durante la simulación y, por consiguiente, el movimiento completo del robot. Durante la realización de este proyecto surgieron varios errores al simular el robot. Tras un estudio del problema este se debía a un bug del software. Como solución se han implementado dos funciones que sustituyen a las funciones originarias en las ruedas (sim.setObjectPosition() y sim.setObjectOrientation()). Estos códigos se muestran a continuación y deben ser implementados si se usa este robot en una versión inferior a la 4.0.1. Como aclaración, este proyecto se ha realizado en la versión 4.0.0 pero ya existen versiones posteriores donde se ha corrige este error. Estas funciones han sido tomadas de esas versiones y la comunidad de CoppeliaSim como parche para esta versión. function __setObjectPosition__(a,b,c) -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1 if b==sim.handle_parent then b=sim.getObjectParent(a) end if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Parameter(sim.intparam_program_version)>=40001) then a=a+sim.handleflag_reljointbaseframe end return sim.setObjectPosition(a,b,c) end function __setObjectOrientation__(a,b,c) -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1 if b==sim.handle_parent then b=sim.getObjectParent(a) end if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Parameter(sim.intparam_program_version)>=40001) then a=a+sim.handleflag_reljointbaseframe end return sim.setObjectOrientation(a,b,c) end 33 4.5 Sensores asociados a la base El modelo presente de KUKA YouBot en CoppeliaSim no viene con ningún sensor integrado. En la realidad, tampoco presenta ningún sensor. Por este motivo, se ha querido añadir como mejora dos sensores de tipo láser en la simulación [17]. Concretamente, los sensores se encuentran en la base móvil del robot y son hijos de esta. Aquí no se entrará en profundidad en explicar estos sensores ya que fueron explicados en el apartado 2.6.2.1 y se muestran en la figura 15. Este apartado se centra en explicar el código que permite utilizar los sensores y usar su información para, en este caso, mover el robot. Todas las funciones y el código de los sensores se han programado en el threaded child script asociado al robot y que maneja la rutina que crea la secuencia de acciones del robot (movimiento y manipulación de la pieza). Como siempre se empieza por inicializar las variables que hacen referencia a ambos sensores: Los sensores están siempre leyendo información, pero solo la usaremos en los puntos donde están las mesas. Estos puntos coinciden con el final de la primera y segunda ruta. Una vez llegados a estos puntos, el robot se para y usa la información de los sensores activos. En la simulación se puede observar la activación de los sensores ya que al detectar un objeto el rayo pasa a parpadear en color amarillo. Para saber el sensor que está activo (el entorno y la prueba creada solo usa un sensor en cada punto) se usa la función sim.readProximitySensor() que devuelve un 1 si el sensor detecta algo. Se muestra el código utilizado para saber si el sensor activo es el situado en el lado derecho o izquierdo. Si el sensor se está activo se llama a una función definida para este proyecto que se llama sensor_distance. Esta función detecta a la distancia que se encuentra del punto y, aleja o acerca el robot hasta situarse a una cierta distancia predefinida. Además, se usa otra función creada para obtener la distancia que detecta el sensor, esta se denomina getDistance(). [18] sensor_l = sim.getObjectHandle('left_ray_sensor') sensor_r = sim.getObjectHandle('right_ray_sensor') if (sim.readProximitySensor(sensor_l)>0) then sensor_distance(sensor_l) end if (sim.readProximitySensor(sensor_r)>0) then sensor_distance(sensor_r) end Plataforma móvil 34 Esta función devuelve la distancia en metros detectada por el sensor. Este dato se obtiene también de la función sim.readProximitySensor() que es propia del software de CoppeliaSim. Esta función es capaz de devolver varios datos, pero solo interesa, para este caso, saber si el sensor detecta algo (valor almacenado en la variable local detected) y la distancia a la que se encuentra el objeto detectado (distance). Ahora se muestra la función sensor_distance en la cual se hace uso de la función anterior. Como se ve en el código, se usa la función print() que muestra las variables o texto por la barra de estado durante la simulación. Cada vez que se pone esta función se escribe una nueva línea. Luego se observa la distancia a la que se quiere que se sitúa el robot del objeto, para este proyecto se ha decidido que sea 0.1m. Para la simulación desarrollada, es importante que esta distancia sea adecuada para que el brazo robótico sea capaz de alcanzar el objeto de la mesa y luego, depositarlo en la otra. sensor_distance=function(sensor) dist = getDistance(sensor) printf ("Distancia primera") print (dist) p_dist = dist - 0.1 --Distancia que se quiere conseguir 0.1 p1=sim.getObjectPosition(youBot_target,-1) pfin = p1[1]-p_dist ptarget = {pfin, p1[2], p1[3]} sim.setObjectPosition(youBot_target, -1, ptarget) waitReachTarget() dist = getDistance(sensor) printf ("Distancia final") print (dist) end function getDistance(sensor) max_dist = 0.5 --0.5m distancia máxima asignada al sensor en la simulación local detected, distance detected,distance=sim.readProximitySensor(sensor) if (detected<0) then distance=max_dist end return distance end 35 Muchas de estas funciones han sido explicadas en capítulos anteriores, pero se hará una breve explicación del funcionamiento global de la función. Lo primero es calcular la distancia a la que está el robot del objeto para después, realizar el cálculo que permite saber cuánto se tiene que desplazar el robot para colocarse a la distancia establecida. Luego, este punto se asigna a la variable youBot_target para que el robot se mueva y siga a la nueva posición. En este caso, se ha implementado otra función para evitar cualquier problema en la transición entre movimiento de la plataforma y movimiento del brazo robótico, ya que se quiere mover el brazo solo cuando el robot este en su nueva posición. Esta función se ha denominado waitReachTarget() y su código es el siguiente: Esta función permite que no se avance a la siguiente línea de ejecución hasta que el punto de referencia y el robot se encuentren en la misma posición con un error máximo de 0.001 m. En esta función, se puede observar el uso de funciones matemáticas en CoppeliaSim, como es el caso de la raíz cuadrada (math.srqt). Una vez que se ha alcanzado el punto aparecerá también por la barra de estado (debajo de la escena) la nueva distancia que detecta el sensor (Distancia final). Esto permite comprobar que la distancia determinada ha sido alcanzada correctamente, tal y como se muestra en la imagen siguiente. Figura 27: Captura de la barra de estado Una vez ha sido alcanzado el punto, el robot se mantiene en la posición hasta que el brazo recoge o suelta la pieza en su correspondiente mesa. waitReachTarget=function() repeat sim.switchThread() p1=sim.getObjectPosition(youBot_target,-1) p2=sim.getObjectPosition(youBot_reference,-1) p={p2[1]-p1[1],p2[2]-p1[2]} pError=math.sqrt(p[1]*p[1]+p[2]*p[2]) until (pError<0.001) end Anexo A - Códigos 42 Non-threaded child script (ME_Platfo2_sub1) 43 Non-threaded child script (rollingJoint_rl) Non-threaded child script (rollingJoint_rr) Anexo A - Códigos 44 Non-threaded child script (rollingJoint_fl) Non-threaded child script (rollingJoint_fr) 45 Non-threaded child script (Rectangle7) Referencias 46 REFERENCIAS [1] KUKA Laboratories GmbH, «generationrobots,» [En línea]. Available: https://www.generationrobots.com/img/Kuka-YouBot-Technical-Specs.pdf. [Último acceso: 2021]. [2] CoppeliaSim, «Coppelia Robotics (Downloads),» [En línea]. Available: https://www.coppeliarobotics.com/downloads. [Último acceso: 2021]. [3] CoppeliaSim, «coppeliarobotics,» [En línea]. Available: https://www.coppeliarobotics.com/. [Último acceso: 2021]. [4] CoppeliaSim, «Coppelia Robotics (Regular API reference),» [En línea]. Available: https://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm. [Último acceso: 2021]. [5] CoppeliaSim, «coppeliarobotics,» [En línea]. Available: https://www.coppeliarobotics.com/helpFiles/. [Último acceso: 2021]. [6] R. Ierusalimschy, L. Henrique de Figueiredo y W. Celes, «lua.org,» 2007. [En línea]. Available: https://www.lua.org/manual/5.1/es/manual.html. [Último acceso: 2021]. [7] CoppeliaSim, «coppeliarobotics,» [En línea]. Available: https://www.coppeliarobotics.com/helpFiles/en/childScripts.htm. [Último acceso: 2021]. [8] L. Armesto, «Youtube (Creación de trayectorias/caminos en el suelo | CoppeliaSim (V-REP)),» 11 02 2020. [En línea]. Available: https://www.youtube.com/watch?v=0dh9quzGpW8&list=PLjzuoBhdtaXOYfcZOPS98uDTf4aAoDSRR &index=28. [Último acceso: 2021]. [9] CoppeliaSim, «coppeliarobotics,» [En línea]. Available: https://www.coppeliarobotics.com/helpFiles/en/solvingIkAndFk.htm. [Último acceso: 2021]. 47 [10] CoppeliaSim, «coppeliarobotics,» [En línea]. Available: https://www.coppeliarobotics.com/helpFiles/en/basicsOnIkGroupsAndIkElements.htm. [Último acceso: 2021]. [11] j. a. r. king, «Youtube (IK en CoppeliaSim (Tutorial)),» 8 06 2021. [En línea]. Available: https://www.youtube.com/watch?v=dB8ebrjUPdQ. [Último acceso: 2021]. [12] m. Ninja, «Youtube (Vrep (CoppeliaSim) Inverse kinematics tutorial (before version 4.2.0)),» 11 12 2018. [En línea]. Available: https://www.youtube.com/watch?v=eTd6mOk6Njw. [Último acceso: 2021]. [13] V.-R. Tutorials, «Youtube (Vrep script writing),» 5 10 2016. [En línea]. Available: https://www.youtube.com/watch?v=Zt21o1qsOB4. [Último acceso: 2021]. [14] B. Adamov, «Influence of mecanum wheels construction on accuracy of the omnidirectional platform navigation (on exanple of KUKA youBot robot),» de International Conference on Integrated Navigation Systems (ICINS), San Petersburgo, 2018. [15] Roboticoss, «Youtube (Robots Móviles Autónomos 5 : Modelo Cinemático y Simulación Robot Omnidireccional),» 31 10 2018. [En línea]. Available: https://www.youtube.com/watch?v=CSkHZhpEwsE. [Último acceso: 09 1 2021]. [16] N. Robotics, «Youtube (Modern Robotics, Chapter 13.2: Omnidirectional Wheeled Mobile Robots (Part 1 of 2)),» 10 05 2018. [En línea]. Available: https://www.youtube.com/watch?v=NcOT9hOsceE. [Último acceso: 2021]. [17] L. Armesto, "Youtube (Sensores de proximidad | CoppeliaSim (V-REP))," 3 02 2020. [Online]. Available: https://www.youtube.com/watch?v=vULeGmxff5c&list=PLjzuoBhdtaXOYfcZOPS98uDTf4aAoDSRR &index=13. [Accessed 2021]. [18] L. Armesto, «Youtube (Cómo Usar los Sensores de Proximidad para Permanecer entre Paredes | CoppeliaSim (V-REP)),» 12 02 2020. [En línea]. Available: https://www.youtube.com/watch?v=YKZmv7yA9jQ&list=PLjzuoBhdtaXOYfcZOPS98uDTf4aAoDSRR &index=26. [Último acceso: 2021].