Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Prefacio: ........................................................................................................................ 4

BLOQUE 1: CONCEPTOS BÁSICOS................................... 6 1

CAPÍTULO: INTRODUCCIÓN A LA ROBÓTICA..................................... 7

1.1 1.2 1.3 1.4 1.5

Introducción. ...................................................................................................... 7 Historia de la robótica. ...................................................................................... 7 Bases de la robótica actual ................................................................................ 8 Definición de Robot............................................................................................ 9 Límites de la robótica actual........................................................................... 10

2

COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO. ................. 12

2.1 2.2 2.3 2.4 2.5 2.6 2.7 2.8 2.9 2.10

Introducción ..................................................................................................... 12 El Brazo Robot ................................................................................................. 12 Grados de libertad ........................................................................................... 12 Articulaciones................................................................................................... 14 Sistemas de coordenadas ................................................................................. 14 Volumen de trabajo ......................................................................................... 15 Actuadores ........................................................................................................ 16 El controlador................................................................................................... 18 Dispositivos sensoriales.................................................................................... 18 Elementos terminales....................................................................................... 21

3

CAPÍTULO: TRANSFORMACIONES 3D. ................................................. 23

3.1 3.2 3.3 3.4 3.5 3.6

Introducción ..................................................................................................... 23 Geometría 3D ................................................................................................... 23 Cambio de escala.............................................................................................. 25 Traslación ......................................................................................................... 25 Rotación ............................................................................................................ 25 Rotación alrededor de un eje arbitrario ........................................................ 26

4

CAPÍTULO: CINEMÁTICA. ........................................................................ 31

4.1 4.2 4.3 4.4

Introducción. .................................................................................................... 31 El problema Cinemático Directo .................................................................... 32 Algoritmo de Denavit-Hartenberg ................................................................. 33 Cinemática Inversa .......................................................................................... 37

5

CAPÍTULO: TOOLBOX DE ROBÓTICA................................................... 38

5.1 5.2

Introducción ..................................................................................................... 38 Representación de traslación 3D y orientación ............................................. 38

ÁLVARO GÓMEZ RAMOS.

-1-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 5.3 5.4

Cinemática directa ........................................................................................... 39 Cinemática inversa........................................................................................... 39

BLOQUE 2: DESARROLLO DEL SOFTWARE ................ 41 1

CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT. ............... 42

1.1 1.2 1.3 1.4 1.5 1.6

Introducción. .................................................................................................... 42 SICON-BOT. .................................................................................................... 43 Objetivos de la primera versión...................................................................... 43 Posibles mejoras futuras.................................................................................. 44 Requisitos mínimos. ......................................................................................... 44 Inicialización..................................................................................................... 45

2

CAPÍTULO: GRÁFICA DE SICON-BOT. .................................................. 46

2.1 2.2 2.3 2.4

Creación de ventana principal y menús desplegables. ................................. 46 Paneles y plots de representación ................................................................... 47 Eslabones y articulaciones............................................................................... 49 Botones, textos y cuadros editables. ............................................................... 54

3

CAPÍTULO: POSIBILIDADES DE SICON-BOT....................................... 56

3.1 3.2 3.3 3.4 3.5 3.6 3.7 3.8 3.9 3.10 3.10

Nuevo robot ...................................................................................................... 56 Calcular robot .................................................................................................. 58 Salvar Robot ..................................................................................................... 59 Cargar Robot.................................................................................................... 60 Parámetros D-H ............................................................................................... 62 Salvar D-H ........................................................................................................ 63 Cinemática directa ........................................................................................... 63 Cinemática inversa........................................................................................... 64 Posicionar Robot .............................................................................................. 66 Ayuda ................................................................................................................ 67 Herramientas.................................................................................................... 68

4

CAPÍTULO: PROGRAMACIÓN DE SICON-BOT.................................... 70

4.1 4.2 4.3 4.4 4.5 4.6 4.7 4.8 4.9 4.10 4.11

Introducción ..................................................................................................... 70 Inicialización de ventana inicial y menús desplegables ................................ 71 Entorno de creación de robots ........................................................................ 73 Añadir elemento de traslación ........................................................................ 76 Añadir elemento de rotación........................................................................... 78 Añadir elemento eslabón ................................................................................. 80 Añadir elemento base ...................................................................................... 81 Posicionar y dibujar por primera vez el elemento traslación ...................... 82 Posicionar y dibujar por primera vez el elemento rotación......................... 85 Posicionar y dibujar por primera vez el elemento eslabón .......................... 88 Posicionar y dibujar por primera vez el elemento base ............................... 91

ÁLVARO GÓMEZ RAMOS.

-2-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 4.12 4.12 4.13 4.14 4.15 4.16 4.17 4.18 4.19 4.20 4.21 4.22 4.23 4.24 4.25 4.26 4.27 4.28 4.28 4.29 4.30 4.31 4.32 4.33

Rotar punto respecto a ejes x e y que pasan por otro punto dado .............. 94 Rotar punto respecto a ejes dado por dos puntos ......................................... 96 Calcular robot .................................................................................................. 99 Obtención de los ejes de las articulaciones .................................................. 100 Obtención de “eje x”, “eje z” y punto de origen ......................................... 101 Obtención de “eje y”, normalizazión y dibujado de ejes............................ 106 Dibujar robot.................................................................................................. 107 Dibujar elemento eslabón.............................................................................. 108 Dibujar elemento traslación.......................................................................... 111 Dibujar elemento rotación ............................................................................ 114 Dibujar elemento base ................................................................................... 118 Salvar robot .................................................................................................... 119 Cargar robot................................................................................................... 119 Calculo de parámetros DH............................................................................ 120 Calculo de parámetros di ............................................................................... 123 Calculo de parámetros ai ............................................................................... 124 Calculo de parámetros θ i .............................................................................. 126 Calculo de parámetros α i .............................................................................. 126 Cinemática directa ......................................................................................... 127 Cinemática inversa......................................................................................... 129 Posicionar robot ............................................................................................. 134 Ayuda .............................................................................................................. 138 Función auxiliar: Multiplicar vectorialmente vectores simbólicos ........... 138 Función auxiliar: Redondear a cero los valores de las matrices ............... 138

BLOQUE 3: CONCLUSIONES ........................................... 139 1 2

Conclusiones. .................................................................................................. 140 Desarrollos futuros......................................................................................... 141

BLOQUE 4: ANEXOS .......................................................... 142 1

ANEXO: CÓDIGO FUENTE....................................................................... 143

ÁLVARO GÓMEZ RAMOS.

-3-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Prefacio: Durante los estudios universitarios hemos recibido una amplia formación en robótica, tanto teóricamente como en la práctica, aprendiendo a trabajar matemáticamente con los robots o a programarlos para que realicen diversas tareas. En el transcurso de esa formación hemos utilizado diversas herramientas informáticas que nos ayudan en diferente grado a tratar con los robots. Hoy en día la robótica se va extendiendo, y cada vez se generaliza más, con lo cual es conveniente crear ciertas herramientas que permitan acercar a usuarios menos expertos. El programa SICON-BOT surge con el objetivo de facilitar a los usuarios menos expertos el uso de aplicaciones informáticas, de hacer más cómodo el trabajo para los más expertos e integrar en una sola herramienta, otras herramientas, para hacer su uso más sencillo. En definitiva, SICON-BOT tiene tres objetivos principales: FACILIDAD-COMODIDAD-INTEGRACIÓN -

Facilidad: Con un entorno gráfico familiar, con diseño de robots en 3D, con ventanas, menús desplegables, botones, y demás funciones gráficas que son familiares a la mayoría de personas.

-

Comodidad: A la hora de integrar nuevas herramientas que nos permiten obtener automáticamente parámetros necesarios para otras herramientas, que hasta ahora teníamos que calcular a mano.

-

Integración: En un mismo software podemos utilizar funciones pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se resuelven en la programación interna.

En la primera versión del software SICON-BOT el objetivo es crear las bases de la herramienta, para que poco a poco puedan ir ampliándose en un futuro por nuevos usuarios interesados, para lo cual el código es accesible. En resumen los objetivos de la primera versión es el desarrollo desde cero del software, creando toda la programación necesaria para el entorno gráfico, el diseño de robots en 3D, el cálculo de ejes, el cálculo de matrices DH, el cálculo de la cinemática directa e inversa, y el posicionamiento del robot en 3D. Con esto tenemos en la primera versión los tres objetivos fundamentales: -

Facilidad: Con el entorno gráfico que incorpora, el diseño intuitivo de robots, el cálculo automático de parámetros mediante simple inserción de datos sencillos.

-

Comodidad: Púes realiza funciones como la obtención automática de los ejes o de la matriz DH.

ÁLVARO GÓMEZ RAMOS.

-4-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB -

Integración: Pues incorpora internamente funciones del toolbox de robótica como la cinemática directa e inversa.

ÁLVARO GÓMEZ RAMOS.

-5-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

BLOQUE 1: CONCEPTOS BÁSICOS

ÁLVARO GÓMEZ RAMOS.

-6-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1

CAPÍTULO: INTRODUCCIÓN A LA ROBÓTICA.

1.1

Introducción.

La Robótica describe todas las tecnologías asociadas con los robots. Sin embargo la definición de robot entraña más dificultad, pues existen muchas definiciones: • • • • •

Ingenio mecánico controlado electrónicamente, capaz de moverse y ejecutar de forma automática acciones diversas, siguiendo un programa establecido. Máquina que en apariencia o comportamiento imita a las personas o a sus acciones como, por ejemplo, en el movimiento de sus extremidades Un robot es una máquina que hace algo automáticamente en respuesta a su entorno. Un robot es un puñado de motores controlados por un programa de ordenador. Un robot es un ordenador con músculos.

Es cierto, como acabamos de observar, que los robots son difíciles de definir. Sin embargo, no es necesariamente un problema el que no esté todo el mundo de acuerdo sobre su definición. Quizás, Joseph Engelberg (padre de la robótica industrial) lo resumió inmejorablemente cuando dijo: "Puede que no se capaz de definirlo, pero sé cuándo veo uno". La imagen del robot como una máquina a semejanza del ser humano, subyace en el hombre desde hace muchos siglos, sin embargo, el robot industrial, que se conoce y emplea en nuestros días, no surge como consecuencia de la tendencia o afición de reproducir seres vivientes, sino de la necesidad. Inmersos en la era de la informatización, la imperiosa necesidad de aumentar la productividad y mejorar la calidad de los productos, ha hecho insuficiente la automatización industrial rígida, dominante en las primeras décadas del siglo XX, que estaba destinada a la fabricación de grandes series de una restringida gama de productos. Hoy día, más de la mitad de los productos que se fabrican corresponden a lotes de pocas unidades.

1.2

Historia de la robótica.

El concepto de robot se remonta casi al principio de la civilización, donde los mitos hablan de seres mecánicos dotados de vida. En la civilización griega aparecen figuras que se mueven mediante poleas y bombas hidráulicas y que se usan para propósitos estéticos y artísticos. Tal es el caso del Hero's Automatic Theater and Driver, una estatua de un dios alrededor de la cual rotaban pequeñas figuras de forma periódica, o del mítico Coloso de Rodas, que defendía el puerto de la ciudad. No obstante, la sociedad griega carecía de dos puntos básicos para un desarrollo satisfactorio de robots: necesidad y tecnología.

ÁLVARO GÓMEZ RAMOS.

-7-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB La civilización árabe cubre el primero de estos puntos al concebir el robot como un elemento para el confort del ser humano. Un ejemplo de este punto de vista pragmático es la Fuente del Pavo Real, un sencillo dispositivo que medía el nivel del agua vertida en un recipiente de forma que, al alcanzarse un cierto umbral, aparecía un autómata portando una pastilla de jabón y, transcurrido un cierto tiempo, una toalla. Durante el Renacimiento, los estudios de Leonardo da Vinci sobre anatomía del cuerpo humano aportaron un valioso conocimiento a la hora de desarrollar la mecánica del robot, principalmente los antropomorfos, permitiendo la construcción de junturas mecánicas mucho mejores e impulsando por tanto el desarrollo modular de máquinas complejas. No fue hasta finales de 1800, que se contempla de forma científica el concepto de autonomía e inteligencia artificial, cuando Nikola Tesla se propone crear una máquina capaz de tomar sus propias decisiones sin necesidad de un telecontrol. Más adelante, el escritor checo Karel Capek acuñaría el término robot como tal en su obra Rossum’s Universal Robots, que se estrenó en Praga en 1921. El término deriva de la palabra checa robota, que define un trabajo forzado o de carácter feudal y la obra trataba de la deshumanización en una sociedad tecnológica. En 1926 Fritz Lang tocaría el mismo tema, ésta vez con robots mecánicos, en la película Metrópolis. La robótica como ciencia que estudia el robot y su uso se concebiría más tarde, cuando en 1942 el escritor de origen ruso Isaac Asimov escribió el relato corto Runaround y la recopilación posterior Yo, Robot. Desde entonces, los robots no sólo han acaparado la atención de la ciencia ficción sino también la de un creciente número de investigadores, presentándose como una excelente alternativa para todos aquellos trabajos que por una u otra causa resultan indeseables para el ser humano.

1.3

Bases de la robótica actual

El inicio del desarrollo de la robótica actual puede fijarse en la industria textil del siglo XVIII, cuando en 1801 Joseph Jacquard inventa una máquina textil programable mediante tarjetas perforadas denominada Teal Programable y que se produciría en masa. Conforme la Revolución Industrial progresaba, continuó el desarrollo de estos agentes mecánicos, aunque el desarrollo de un verdadero robot no fue posible hasta los años cuarenta, con la aparición de la computadora y la cada vez mayor integración de los circuitos. Los primeros robots industriales fueron los Unimates, desarrollados por George Devol y Joe Engleberger. Las primeras patentes aparecen en 1946 y pertenecen a Devol, por sus muy primitivos robots capaces de trasladar maquinaria de un lugar a otro. También en 1946 aparece la primera computadora: J. Presper Ecker y John Maulchy construyen el Eniac en la universidad de Pensilvania y la primera máquina digital de propósito general se desarrolla en el MIT. Más tarde Devol, en 1954, diseña el primer robot programable y acuña el término “Autómata universal” que posteriormente recorta a unimation. Ese sería el nombre de la primera compañía de robótica, fundado por Engleberger, que se considera el padre de la robótica.

ÁLVARO GÓMEZ RAMOS.

-8-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1.4

Definición de Robot

Al hablar de de robots se piensa en un dispositivo fijo al suelo o al techo, con una cierta estructura mecánica que le confiere varios grados de libertad (generalmente de aspecto antropomorfo); con un computador encargado del control de los movimientos y cuya misión no va más allá de simple transportador de piezas, o a lo sumo, y con la ayuda de herramientas terminales para la manipulación de algún producto. Esta idea de la robótica abunda incluso en las definiciones más o menos oficiales. Según el instituto norteamericano Robot Industry Association (RIA) es la siguiente: -

“Se entiende por robot industrial a un manipulador multifuncional reprogramable, diseñado para desplazar materiales, piezas, herramientas o dispositivos especiales mediante movimientos programados variables que permiten llevar a cabo tareas diversas”.

Esta definición se queda algo desfasada porque las tareas que puede realizar un robot son más complejas que las recogidas. La Swedish Industrial Robot Association (SWIRA) define un robot como: -

“Una máquina manipuladora automáticamente controlada, reprogramable, multi-propósito con o sin locomoción para uso en aplicaciones industriales de automatización”.

Esta definición es mas completa pues incluye la posibilidad de movilidad y generaliza las operaciones posibles, además está actualmente considerada por la Norma ISO. Japón tiene su propia definición más amplia: -

“Un robot es todo aquel dispositivo mecánico con algún grado de libertad, destinado a su utilización industrial como manipulador”.

La definición mas precisa es la aportada por la Asociación Francesa de Normalización (AFNOR) aprobada en Agosto de 1983. AFNOR define en primer lugar el manipulador y a continuación el robot industrial. -

Manipulador: “Mecanismo compuesto generalmente de elementos en serie, articulados o deslizantes entre sí, cuyo objetivo es el agarre y desplazamiento de objetos siguiendo diversos grados de libertad. Es multifuncional y puede ser mandado directamente por un operador humano o por cualquier sistema lógico (levas, lógica neumática, lógica eléctrica cableada o bien programado)”.

-

Robot industrial: “Manipulador automático, con servo sistemas de posición, reprogramable, polivalente, capaz de posicionar y desplazar materiales, piezas útiles o dispositivos especiales a lo largo de movimientos variables y

ÁLVARO GÓMEZ RAMOS.

-9-

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB programables para la ejecución de tareas variadas. Estas máquinas polivalentes son generalmente concebidas para efectuar la misma función de manera cíclica y pueden ser adaptados a otras funciones sin modificación permanente del material”.

1.5

Límites de la robótica actual

Michael Knasel, director del Centro de Aplicaciones Robóticas de Science Application Inc., explicó en la exposición Robots-8 en Detroit (1984) que los robots deberían evolucionar durante cinco generaciones. Las dos primeras ya se habían manifestado en los ochenta e incluían la gestión de tareas repetitivas, pero estaban muy limitadas en cuanto a movimiento se refiere. La tercera generación incluiría visión artificial, la cuarta movilidad avanzada en exteriores e interiores y la quinta entraría en el dominio de la inteligencia artificial. Lamentablemente, si bien se han cumplido algunas de estas expectativas, aún no se han conseguido modelos lo suficientemente efectivos. Los límites de la robótica actual se pueden apreciar en contraste con las expectativas previstas por Joseph Engelberger para la robótica de los ochenta: -

Seis articulaciones de precisión entre la base del robot y el extremo de un manipulador Programación sencilla e intuitiva Adaptación al medio mediante estímulos sensoriales Margen de movimiento en rangos de 0.3 mm. Capacidad de manipular pesos de hasta 150 Kg. Control de movimiento punto a punto y de seguimiento de trayectorias Sincronización con blancos móviles Compatibilidad con ordenadores personales Alta fiabilidad Visión artificial que incluya al menos capacidad de orientación y reconocimiento Sensores táctiles Coordinación de las distintas partes de la unidad Capacidad de corrección de trayectorias en línea Movilidad Optimización de movimiento Conservación de la energía Apéndices de propósito general Comunicación por voz Seguridad de acuerdo a las leyes de Asimov de la robótica.

Engelberger aseguraba que los primeros diez puntos ya se habían conseguido a principios de los ochenta. Los sistemas sensoriales por visión y tacto, si bien existen, se limitan a aplicaciones muy básicas y específicas con bajo nivel de detalle debido a la enorme carga computacional inherente al procesado de imagen. En lo que respecta al movimiento, se progresa, aunque con menor precisión de la deseable debido a imperfecciones, deslizamiento y problemas mecánicos en general. La energía necesaria para operar una unidad robótica ha disminuido mucho desde el 79, y se han conseguido unidades capaces de funcionar con baterías solares, como las TEN-TOMUSHI de ÁLVARO GÓMEZ RAMOS.

- 10 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Sanyo. No obstante debe seguirse trabajando en este punto, especialmente en robots destinados a desplazarse en exteriores y entornos hostiles, donde la autonomía del móvil es un facto crítico. Respecto a la comunicación por voz, en tanto que existe la compatibilidad con ordenadores personales y éstos soportan paquetes de reconocimiento de voz distribuidos de forma comercial, es un problema resuelto de forma cada vez más satisfactoria.

ÁLVARO GÓMEZ RAMOS.

- 11 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2

COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO.

2.1

Introducción

La mayoría de robots tienen los siguientes componentes comunes (ver figura 2.1), aunque la estructura mecánica, eléctrica, y computacional de un robot puede variar apreciablemente: -

2.2

Un manipulador o brazo (la unidad mecánica), incluyendo una garra o elemento final. Un controlador (El módulo “inteligente”). Una unidad de potencia. Un sistema sensorial.

El Brazo Robot

El brazo robot consiste en una serie de miembros rígidos, llamados elementos, conectados por articulaciones. Al moverse una articulación se mueven también los siguientes elementos unidos a ella. El movimiento se debe a un actuador, que bien puede ir conectado directamente o a través de alguna transmisión mecánica (con el objetivo de producir una par o velocidad o una ganancia). El último elemento con el que termina el manipulador está diseñado para que pueda adaptarse una herramienta. El manipulador podría dividirse en tres partes: -

Los elementos mayores Los elementos menores (componentes de la muñeca) El elemento terminal (garra o herramienta)

Los elementos mayores lo forman los pares articulación-elemento, que sirven para posicionar el extremo del brazo en un punto del espacio, y que generalmente son los tres primeros. Los elementos menores están asociados con la orientación del elemento final, y junto con los elementos mayores se obtiene el posicionamiento y orientación del elemento final. El elemento final consiste en el mecanismo que permite desempeñar una función al robot, ya sea una garra, soldador, taladro, etc.

2.3

Grados de libertad

Se denomina grados de libertad del sistema al número de coordenadas independientes necesarias para expresar la posición de todas sus partes.

ÁLVARO GÓMEZ RAMOS.

- 12 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Para un elemento final típico de un robot, la garra o el punto central de la herramienta puede ponerse donde se quiera en el espacio, para lo cual se necesitan tres grados de libertad, y si se quiere poder orientar dicho punto en cualquier dirección necesitaremos otros tres grados de libertad más. Por lo tanto se necesitan seis grados de libertad para controlar el movimiento de cualquier elemento final con respecto a la base del robot permitiendo alcanzar cualquier posible orientación y posición dentro del espacio de trabajo del robot. Los tres primeros grados de libertad, que definen la posición, se denominan grados de libertad principales, y nos marcan el espacio de trabajo donde el robot puede trabajar y se controla mediante los actuadores situados en el cuerpo del robot. Los otros tres grados de libertad están localizados cerca del punto central de la herramienta del robot y sirven como “muñeca” para controlar la garra u orientación de la herramienta. Estos tres grados se denominan giro, elevación y guiñada. Cuando añadimos una nueva articulación podemos estar añadiendo un nuevo grado de libertad, pero esto no es siempre así, por eso cuando no ocurre se dice que añade un grado de movilidad, que a veces es necesario para dotar de una movilidad específica al brazo para poder evitar obstáculos situados dentro del campo de trabajo. Al final los grados de libertad se basan en si se puede desplazar y rotar el robot en el eje x, eje y, eje z. Otra razón para añadir nuevas articulaciones que no añadan grados de libertad adicionales, es debido a la limitación de rotación de las articulaciones rotativas, que generalmente no pueden rotar 360º por restricciones mecánicas. Así añadiendo nuevos grados de movilidad podemos compensar estas limitaciones.

ÁLVARO GÓMEZ RAMOS.

- 13 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2.4

Articulaciones

Existen cinco tipos de articulaciones disponibles, pero generalmente sólo se usan dos tipos. Aquí podemos ver los tipos existentes y los grados de libertad que tienen asociados.

2.5

Sistemas de coordenadas

Para mover el robot a cualquier punto hacen falta mínimo tres ejes, por lo que existen cuatro tipos de sistemas de coordenadas para mover los robots según el tipo de las articulaciones: -

3 ejes traslación -> Sistema Cartesiano 2 ejes traslación + 1 eje rotación -> Sistema Cilíndrico 1 eje traslación + 2 ejes rotación -> Sistema Polar 3 ejes rotación -> Sistema Cartesiano

ÁLVARO GÓMEZ RAMOS.

- 14 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2.6

Volumen de trabajo

Es el espacio sobre la que el robot se puede mover, es decir el conjunto de puntos donde se puede situar el extremo del robot. Las fronteras de este espacio solo pueden alcanzarse con una orientación. Normalmente para el cálculo del espacio del robot se tiene en cuenta el final de la muñeca y no la herramienta, porque esta no tiene porque ser fija. En definitiva, el volumen de trabajo del robot está determinado por las siguientes características físicas: - La configuración física del robot - El tamaño de los componentes del cuerpo, brazo y muñeca - Los límites de movimiento de las articulaciones del robot

ÁLVARO GÓMEZ RAMOS.

- 15 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2.7

Actuadores

Son los dispositivos que proporcionan la fuerza motriz para las articulaciones de los robots. Pueden ser de tres tipos según la fuente de energía: Eléctricos, Hidráulicos y Neumáticos. Los eléctricos son cada vez más empleados, pues tienen una gran controlabilidad con un mínimo mantenimiento. Dentro de las diferentes variedades, los más comunes son los servomotores de corriente continua, los motores paso a paso y los servomotores de corriente alterna.

Los actuadores hidráulicos están accionados por un fluido en movimiento, generalmente aceite a presión. Se asocia generalmente con robots grandes. Sus ventajas principales es que proporcionan mayor velocidad y fuerza, pero a cambio hacen el robot más grande y además pueden existir fugas de aceite.

ÁLVARO GÓMEZ RAMOS.

- 16 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

El accionamiento neumático hace uso de un medio fluido que es altamente compresible. Este fluido suele ser aire que está fácilmente disponible y no es inflamable. Este tipo de accionamiento está generalmente reservado para robots pequeños que poseen menos grados de libertad. Suelen estar limitados para operaciones de recogida y depósito con ciclos rápidos. Pueden ser fácilmente adaptados al accionamiento de dispositivos de pistón para proporcionar un movimiento de translación de articulaciones deslizantes. También pueden ser usados en actuadores rotatorios para articulaciones cilíndricas.

ÁLVARO GÓMEZ RAMOS.

- 17 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2.8

El controlador

El controlador es el dispositivo que proporciona la inteligencia necesaria para que actúe conforme al operador. Esencialmente consiste en: -

2.9

Una memoria para almacenar datos como posiciones prefijadas o el propio programa Un secuenciador que interpreta los datos de programa de la memoria. Una CPU que realiza los cálculos necesarios con los datos obtenidos con el secuenciador. Una interfaz que obtenga datos mediante sensores o sistemas de visión. Una interfaz que mande la información de movimiento a los actuadores. Una interfaz para equipos auxiliares, usado para comunicarse con otras unidades externas o dispositivos de control. Un dispositivo que permita aprender posiciones al robot u operaciones en forma de consola o botonera de enseñanza.

Dispositivos sensoriales Los sensores utilizados en robótica pueden dividirse en dos clases:

· Sensores internos: Son los dispositivos usados para medir la posición, velocidad o aceleración de las articulaciones del robot y/o del elemento final. Con esta información tratará más adelante el sistema de control para obtener el movimiento deseado. Pueden ser de diferentes clases: -

Potenciómetros: Es un elemento resistivo de valor máximo (R) que varía su valor según la posición de giro o el desplazamiento, con lo cual, midiendo su tensión inicial y su tensión final podemos saber en que posición está el robot, debido a la relación: r Vout = Vin . R

ÁLVARO GÓMEZ RAMOS.

- 18 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB -

Synchro: Es transductor rotacional que convierte desplazamientos angulares en voltaje de corriente alterna. Se basa en la fórmula: Vout (t ) = Vm sin(θ ) sin(ω ac t )

-

Resolvers: Es una forma especial de synchro por lo que a veces se le conoce como synchro-resolver. La mayor diferencia es que las bobinas del estator y del rotor están desplazadas mecánicamente 90º una respecto a otra en lugar de 120º como en el caso del synchro. Con el rotor excitado por un voltaje portador de alterna Vm sin(ω ac t ) , los dos voltajes de estator son:

V2 (t ) = Vm sin(θ ) sin(ω ac t ) V1 (t ) = Vm cos(θ ) sin(ω ac t ) - Codificadores ópticos: Realizan tareas de determinación de posición con relativa facilidad y sorprendente precisión, además de no necesitar contacto para obtener la medida. Pueden ser de 2 tipos: o Codificadores ópticos absolutos: Es capaz de memorizar la posición siempre, incluso recién conectado, por lo que no necesita ningún ciclo de calibración

ÁLVARO GÓMEZ RAMOS.

- 19 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB o Codificadores ópticos incrementales: Sólo son capaces de medir incrementos o decrementos sobre la posición actual, con lo que si no se conoce la posición actual no es posible conocer las futuras posiciones. Necesita de un ciclo de calibración para obtener una posición inicial sobre la que ir midiendo los cambios.

+ -

Tacómetros: Se aprovecha la variación de una tensión DC que crean los motores al variar sus velocidades para obtener precisamente la velocidad de giro.

· Sensores externos: Son dispositivos usados para monitorizar la relación geométrica y/o dinámica con su tarea, entorno, o los objetos que está manejando. Algunos ejemplos de estos sensores son: -

Dispositivos de proximidad: Nos indican si existen objetos cerca del sensor, e incluso, a que distancia se encuentran. Existen muchas tecnologías que

ÁLVARO GÓMEZ RAMOS.

- 20 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB hagan está labor: sensores de luz reflejada, sensores de ultrasonidos, sensores de barrido láser, etc.

-

Sensores de fuerza y par: Nos indican la fuerza que se está aplicando sobre un objeto.

2.10 Elementos terminales Es el elemento que dota de utilidad al robot, y por tanto es la parte más crítica de diseñar. Sus características más deseadas son: -

Diseño simple. Tamaño pequeño y poco peso. Capacidad para agarrar firmemente las piezas. Autocentrado Una configuración de sujeción habitual.

ÁLVARO GÓMEZ RAMOS.

- 21 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Existen muchos métodos para aportar energía al elemento terminal de un robot, como por ejemplo, cilindros de aire comprimido, electroimanes, o motores eléctricos.

· Dispositivos de aire comprimido: Es el método más utilizado debido a que son potentes y pequeños. Entre sus ventajas cabría destacar su pequeño tamaño y peso con relación a su potencia. También cabe destacar que no son caros y tienen buena fiabilidad. Entre sus desventajas es que hacen falta cilindros de aire comprimido o un compresor que encarece el producto. También pueden tener fugas o tener un respuesta muy lenta en circuitos de aire demasiado largos. · Cilindros hidráulicos: Se usan cuando hace falta fuerzas extremadamente altas para su tamaño, además de muy buenos amortiguamientos y tiempo de respuesta frente a los circuitos largos. Pero hace falta una bomba hidráulica para cada dispositivo hidráulico utilizado, además de ser susceptible de fugas.

ÁLVARO GÓMEZ RAMOS.

- 22 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3

CAPÍTULO: TRANSFORMACIONES 3D.

3.1

Introducción

Algunas aplicaciones gráficas son bidimensionales: dibujos y gráficos, algunos mapas y creaciones artísticas pueden ser entidades estrictamente bidimensionales. Pero vivimos en un mundo tridimensional, y en muchas aplicaciones de diseño debemos manejar y describir objetos tridimensionales. Si un arquitecto desease ver el aspecto real de la estructura, entonces un modelo tridimensional le permitiría observarla desde diferentes puntos de vista. Un diseñador de aviones podría desear analizar el comportamiento de la nave bajo fuerzas y tensiones tridimensionales. En este caso se necesita también una descripción tridimensional. Algunas aplicaciones de simulación, como el aterrizaje de un avión, también exigen una definición tridimensional del mundo. En este capítulo generalizaremos nuestro sistema para manejar modelos de objetos tridimensionales. Extenderemos las transformaciones que hemos visto para permitir traslaciones y rotaciones en el espacio tridimensional. Ya que la superficie de visualización es sólo bidimensional, debemos considerar la forma de proyectar nuestros objetos en esta superficie plana para formar la imagen. Se discutirán tanto las proyecciones paralelas como perspectivas.

3.2

Geometría 3D

Empecemos nuestra discusión sobre las tres dimensiones revisando los métodos de la geometría analítica para definir objetos. El objeto más sencillo es, por supuesto, un punto. Como en el caso de las dos dimensiones, podemos definir un punto estableciendo un sistema de coordenadas y listando las coordenadas del punto. Necesitamos un eje adicional de coordenadas para la tercera dimensión. Podemos disponer este tercer eje para que forme ángulo recto con los otros dos. Podemos utilizar este nuevo sistema de coordenadas para definir la ecuación de una recta. En dos dimensiones teníamos:

Mientras que en el caso tridimensional, son necesarias un par de ecuaciones

Son necesarias las coordenadas de dos puntos para construir estas ecuaciones y ÁLVARO GÓMEZ RAMOS.

. Por lo tanto, todavía son necesarios dos puntos para definir - 23 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB una recta, como cabría esperar. De forma paramétrica, donde cada variable se expresa en función de un parámetro u:

Nos gustaría también trabajar con planos. Un plano, de forma general, se especifica mediante una sola ecuación de la forma:

Cabe señalar que si dividimos por A la ecuación anterior tenemos

Esto significa que sólo son necesarias tres constantes para definir un plano. Las ecuaciones para un plano en particular pueden ser determinadas si conocemos las coordenadas de tres puntos (que no estén alineados)

,

y

. Podemos determina la ecuación de la siguiente manera, ya que cada punto del plano debe satisfacer la ecuación del plano

Tenemos así tres ecuaciones con tres incógnitas que podemos resolver con la ayuda de un poco de álgebra. Otra forma de especificar un plano es mediante un único punto que pertenezca al plano

y la dirección perpendicular al plano

recibe el nombre de vector normal. Por otro lado, si

. Este vector es un punto genérico del

plano, entonces es un vector de ese plano. Si tenemos en cuenta que estos dos vectores forman un ángulo recto, su producto escalar será nulo:

Esta expresión es válida cuando esta es la ecuación de ese plano.

ÁLVARO GÓMEZ RAMOS.

es un punto del plano, por lo tanto,

- 24 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.3

Cambio de escala

Ahora que sabemos expresar puntos, rectas y planos en tres dimensiones, consideremos los algoritmos que permiten al usuario modelar objetos tridimensionales. Empezaremos por generalizar las transformaciones bidimensionales que vimos en el capítulo anterior. Así, por lo que respecta al cambio de escala, la tercera coordenada que hemos introducido pueden tener su propio facto de escala, por lo tanto será necesaria una matriz ,

o de rango

3.4

si empleamos coordenadas homogéneas

Traslación

Igual que en el caso bidimensional, empleamos los elementos de la fila inferior de la matriz de transformación en coordenadas homogéneas para reflejar la traslación.

3.5

Rotación

Cuando consideramos la rotación de un objeto en dos dimensiones, vimos la matriz de rotación alrededor del origen

Podemos generalizar este concepto a una rotación tridimensional alrededor del eje z.

ÁLVARO GÓMEZ RAMOS.

- 25 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

En esta rotación, pensamos en el eje como fijo mientras que algún objeto se mueve en el espacio. Podemos pensar también que el objeto permanece inmóvil mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la dirección de la rotación. Fijar el eje y girar el objeto en sentido antihorario es lo mismo que fijar el objeto y mover el eje en sentido horario. La rotación alrededor de los ejes x e y se realiza con una matriz de transformación similar a la anterior.

Para hacer una rotación alrededor del eje y, del tal forma que el eje z se convierte en el x realizamos la siguiente matriz en coordenadas homogéneas

3.6

Rotación alrededor de un eje arbitrario

Para que las transformaciones tridimensionales nos sean un poco más familiares, resolvamos un sencillo problema. Aunque hemos visto matrices de transformación que nos permiten girar cualquier objeto alrededor de uno de los ejes de coordenadas, en general cualquier recta del espacio puede servir como eje de rotación. El problema consiste en establecer la matriz de rotación para un ángulo j alrededor de una recta cualquiera. Construiremos esta transformación a partir de las que ya conocemos. Moveremos primero el origen hasta que esté situado sobre la recta. Luego realizaremos rotaciones alrededor de los ejes x e y para alinear el eje z con la recta. De esta forma la rotación alrededor de la recta se convierte en un giro alrededor del eje z. Finalmente, realizaremos transformaciones inversas a la realizadas en los giros alrededor de los ejes x e y para deshacer, por último, la traslación del origen a su posición original. Lo primero que debemos hacer es escoger una representación adecuada de la recta que será el eje de rotación. Un punto de dicha recta, así como la dirección de la ÁLVARO GÓMEZ RAMOS.

- 26 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos ofrece la información necesaria para la traslación del origen, y la dirección nos informa de los ángulos de rotación correctos para alinear la recta con el eje z. Dada la recta:

Un punto de esta recta es vector

y su dirección está especificada por el

. La matriz de transformación que traslada el origen al eje de giro será:

Necesitaremos también la inversa de traslación para devolver el origen a su posición original una vez que las rotaciones se hayan completado.

El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente forma: el segmento de recta comprendido entre el origen y está en la dirección del eje arbitrario de rotación, y todo el segmento está situado sobre dicho eje. Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende desde

hasta

ÁLVARO GÓMEZ RAMOS.

.

- 27 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Si ahora realizamos un giro alrededor del eje x hasta que nuestro eje arbitrario quede en el plano xz, el segmento proyectado quedará alineado con el eje z. Cuanto vale el ángulo q ?

Sabemos que la longitud del segmento proyectado es

y, teniendo en cuenta la figura anterior, es fácil deducir las razones trigonométricas del ángulo q

Por lo tanto, la matriz de transformación para el giro alrededor del eje x será ÁLVARO GÓMEZ RAMOS.

- 28 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

La transformación inversa es una rotación de igual magnitud pero en sentido contrario. Como sabemos, cambiar el signo del ángulo significa cambiar el signo de los senos y conservar el de los cosenos.

La rotación alrededor de este eje no ha modificado la coordenada x. Sabemos también que la longitud total del segmento

no varía. La coordenada z será entonces

Necesitamos realizar ahora un giro de ángulo f alrededor del eje y para que el segmento quede alineado con el eje z.

Sus razones trigonométricas son

ÁLVARO GÓMEZ RAMOS.

- 29 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB La matriz de rotación en este caso será

La inversa de esta transformación será

Es posible ahora realizar el giro j alrededor del eje escogido. Ya que este eje está alineado con el eje z, la matriz correspondiente a esta rotación será

De esta forma ya somos capaces de realizar una rotación j alrededor de un eje arbitrario, mediante el producto de las transformaciones anteriores

ÁLVARO GÓMEZ RAMOS.

- 30 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4

CAPÍTULO: CINEMÁTICA.

4.1

Introducción.

La cinemática del brazo del robot trata con el estudio analítico de la geometría del movimiento de un brazo de robot con respecto a un sistema de coordenadas de referencia fijo sin considerar las fuerzas o momentos que originan el movimiento. Así, la cinemática se interesa por la descripción analítica del desplazamiento espacial del robot como una función del tiempo, en particular de las relaciones entre la posición de las variables de articulación y la posición y orientación del efecto final del brazo del robot. Hay dos problemas fundamentales en la cinemática del robot. El primer problema se suele conocer como el problema cinemático directo, mientras que el segundo es el problema cinemático inverso. Como las variables independientes en un robot son las variables de articulación, y una tarea se suele dar en términos del sistema de coordenadas de referencia, se utiliza de manera más frecuente el problema cinemático inverso. Denavit y Hartenberg en 1955 propusieron un enfoque sistemático y generalizado de utilizar álgebra matricial para describir y representar la geometría espacial de los elementos del brazo del robot con respecto la un sistema de referencia fijo. Este método utiliza una matriz de transformación homogénea 4 x 4 para describir la relación espacial entre dos elementos mecánicos rígidos adyacentes y reduce el problema cinemático directo a encontrar una matriz de transformación homogénea 4 x 4 que relaciona el desplazamiento espacial del sistema de coordenadas de la mano al sistema de coordenadas de referencia. Estas matrices de transformación homogéneas son también útiles en derivar las ecuaciones dinámicas de movimiento del brazo del robot. En general, el problema cinemático inverso se puede resolver mediante algunas técnicas. Los métodos utilizados más comúnmente son el algebraico matricial, iterativo o geométrico. · Cinemática Directa (ángulos para encontrar posición): Se conoce: a) La longitud de cada eslabón. b) El ángulo de cada articulación. Se busca: La posición de cualquier punto (coordenadas con respecto a la base) · Cinemática Inversa (posición para encontrar ángulos): Se conoce: a) La longitud de cada eslabón. b) La posición de cualquier punto (coordenadas con respecto a la base). Se busca: El ángulo de cada articulación necesitado para obtener la posición.

ÁLVARO GÓMEZ RAMOS.

- 31 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.2

El problema Cinemático Directo

El problema cinémático directo se reduce a encontrar la matriz de transformación homogénea (T) que relacione la posición y orientación del extremo del robot respecto a su sistema de referencia fijo (base del robot). La matriz T está en función de los parámetros de las articulaciones del robot. Para un robot de n grados de libertad tenemos:

x= y= z= α= β= γ =

f x (q1 f y (q1 f z (q1 f α (q1 f β (q1 f γ (q1

q2 q2 q2 q2 q2 q2

... ... ... ... ... ...

qn ) qn ) qn ) qn ) qn ) qn )

Donde:

q1...n = Son las variables de las articulaciones. Para articulaciones de rotación las variables son ángulos. Para articulaciones prismáticas las variables son distancias. x, y, z = Coordenadas de la posición del robot

α , β , γ = Ángulos de orientación de la muñeca Las funciones mencionadas pueden ser encontradas mediante métodos geométricos para el caso de robots de 2 grados de libertad (cada relación articulacióneslabón constituye un grado de libertad):

ÁLVARO GÓMEZ RAMOS.

- 32 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

x = l1 cos θ1 + l 2 cos(θ1 + θ 2 ) y = l1 senθ1 + l 2 sen(θ1 + θ 2 ) Para robots de más de 2 grados de libertad es difícil aplicar métodos geométricos para la solución de su cinemática directa. A cada eslabón se le asocia un sistema coordenado y utilizando transformaciones homogéneas es posible representar las rotaciones y traslaciones relativas entre los diferentes eslabones que componen el robot. Siendo la matriz:

Aii−1

Siendo i el nº de eslabón

La matriz de transformación homogénea que representa la posición y orientación relativa entre los sistemas asociados a dos eslabones consecutivos del robot. Se puede representar de forma parcial o total la cadena cinemática que forma el robot: n

A0n = ∏ Aii−1 i =1

Para el caso de un robot de 6 ejes, su cadena cinemática queda representada por la siguiente matriz de transformación homogénea:

T = A06 = A01 • A12 • A23 • A34 • A45 • A56

4.3

Algoritmo de Denavit-Hartenberg

En 1955 Denavit y Hartenberg propusieron un método matricial que permite establecer de manera sistemática un sistema de coordenadas. La representación de Denavit-Hartenberg (D-H) establece que seleccionándose adecuadamente los sistemas de coordenadas asociados a cada eslabón, será posible pasar de uno al siguiente mediante 4 transformaciones básicas que dependen exclusivamente de las características geométricas del eslabón. Reduciéndose al siguiente patrón de transformaciones que permiten relacionar el sistema de referencia del elemento i con respecto al sistema del elemento i-1: -

Rotación alrededor del eje z i −1 un ángulo θ i Traslación a lo largo de z i −1 una distancia d i Traslación a lo largo de x1 una distancia ai Rotación alrededor del eje x1 un ángulo α i

ÁLVARO GÓMEZ RAMOS.

- 33 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Aii−1 = T ( z ,θ i )T (0,0, d i )T (ai ,0,0)T ( x, α i ) Desarrollando la expresión:

Aii−1

⎡cos θ i ⎢ senθ i =⎢ ⎢ 0 ⎢ ⎣ 0

− senθ i cos θ i 0 0

0 0 1 0

0 ⎤ ⎡1 0⎥⎥ ⎢⎢0 0⎥ ⎢0 ⎥⎢ 1 ⎦ ⎣0

0 1 0 0

0 0 ⎤ ⎡1 0 0 ⎥⎥ ⎢⎢0 1 d i ⎥ ⎢0 ⎥⎢ 0 1 ⎦ ⎣0

0 1 0 0

0 a i ⎤ ⎡1 0 ⎥ ⎢ 0 0 ⎥ ⎢0 cos α i 1 0 ⎥ ⎢0 senα i ⎥⎢ 0 1 ⎦ ⎣0 0

0⎤ 0⎥⎥ 0⎥ ⎥ 1⎦

0 − senα i cos α i 0

Obtenemos la expresión general de DH, donde θ i , d i , ai , α i son los parámetros DH del eslabón i:

Aii−1

⎡cos θ i ⎢ senθ i =⎢ ⎢ 0 ⎢ ⎣ 0

− cos α i senθ i cos α i cos θ i senα i 0

senα i senθ i − senα i cos θ i cos α i 0

ai cos θ i ⎤ ai senθ i ⎥⎥ di ⎥ ⎥ 1 ⎦

Para que la matriz Aii−1 relacione los sistemas coordenados Oi y Oi −1 es necesario que los sistemas coordenados se determinen mediante los siguientes pasos: 1. Numerar y etiquetar el eslabón fijo (base) como 0. 2. Numerar y etiquetar los eslabones móviles desde 1 hasta el n eslabón móvil. 3. Localizar y numerar el eje de cada articulación y etiquetarla comenzando desde Z 0 hasta Z n −1 . Si la articulación es rotativa, el eje será su propio eje de giro. Si la articulación es prismática, el eje será a lo largo del cual se produce el desplazamiento.

Establecimiento del sistema coordenado de la base: 4. Establecer el sistema coordenado de la base estableciendo el origen como O0 en cualquier punto del eje Z 0 . Arbitrariamente establecer los ejes X 0 , Y0 respetando la regla de la mano derecha. Establecimiento de los sistemas coordenados de las demás articulaciones: 5. Localizar el origen Oi : a) En la intersección del eje Z i con la línea normal común a la intersección de Z i y Z i −1 . b) En la intersección de Z i y Z i −1 , si es que Z i y Z i −1 se intersectan. c) En la articulación i, si Z i y Z i −1 son paralelos.

ÁLVARO GÓMEZ RAMOS.

- 34 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 6. Establecer X i : a) A lo largo de la línea normal común entre los ejes Z i y Z i −1 que pasa por Oi . b) En la dirección normal al plano formado por Z i y Z i −1 , si es que estos dos ejes se intersectan. 7. Establecer Yi de acuerdo a la regla de la mano derecha. Establecimiento del sistema coordenado de la herramienta: 8. Localizar el sistema coordenado n-ésimo en el extremo del robot. Si es una articulación rotacional, establecer Z n a lo largo de la dirección Z n −1 y establecer el origen On de la manera que más convenga a lo largo de Z n , preferente en el centro de la pinza o la punta de cualquier herramienta que el robot tenga montada. 9. Establecer X n y Yn de acuerdo a la regla de la mano derecha. Si la herramienta es una pinza, es común establecer el eje Yn entre los “dedos” de la pinza y X n será ortonormal a Z n y Yn . Obtener las Matrices de Transformación Homogéneas 10. Crear una tabla con los parámetros D-H de los eslabones: Eslabón i

θi

ÁLVARO GÓMEZ RAMOS.

di

ai

αi

- 35 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Donde:

θ i = Es el ángulo formado por los ejes X i y X i −1 medido en un plano perpendicular a Z i −1 utilizando la regla de la mano derecha. Este es un parámetro variable en articulaciones rotatorias.

d i = Es la distancia a lo largo del eje Z i −1 desde el origen Oi −1 hasta la intersección del eje X i con el eje Z i −1 . Este es un parámetro variable en articulaciones prismáticas. ai = Para articulaciones rotatorias: es la distancia a lo largo del eje X i desde el origen Oi hasta la intersección del eje X i con el eje Z i −1 .

α i = Es el ángulo formado por los ejes Z i y Z i −1 medido en un plano perpendicular al eje X i utilizando la regla de la mano derecha. 11. Realizar la matriz D-H de transformación homogénea Aii−1 para cada eslabón de acuerdo a los datos de la tabla del punto anterior. 12. Obtener la matriz de transformación que relacione el sistema coordenado de la base con el sistema coordenado del extremo del robot, resultando en la posición y orientación del sistema coordenado de la herramienta expresado en coordenadas de la base. n

T = A = ∏ Aii−1 n 0

i =1

ÁLVARO GÓMEZ RAMOS.

- 36 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.4

Cinemática Inversa

El objetivo del problema cinemático inverso consiste en encontrar los valores que deben tomar las variables articulares del robot para que su extremo se posicione y oriente según una determinada localización espacial. La ecuación matemática que representa lo anterior es: q k = f k ( x, y , z , α , β , γ ) K = 1...n

Donde: q1...k = Son las variables de las articulaciones. Para articulaciones revolutas las variables son ángulos. Para articulaciones prismáticas las variables son distancias.

x, y, z = Coordenadas de la posición del extremo del robot.

α , β , γ = Ángulos de la orientación del extremo del robot. n = Número de grados de libertad A diferencia del problema cinemático directo donde de una manera sistemática e independiente de la configuración del robot se llega a una solución, en el problema cinemático inverso el mecanismo de solución es fuertemente dependiente de la configuración y con frecuencia la solución no es única. Normalmente los métodos geométricos nos permiten obtener normalmente los valores de las primeras variables, que son las que consiguen posicionar el extremo del robot en un punto determinado. También es posible recurrir a manipular directamente a las ecuaciones obtenidas del problema cinemático directo. En muchos robots de 6 grados de libertad es posible aplicar acoplamiento cinemático, para que los ejes dedicados al posicionamiento y los ejes dedicados a la orientación, sean tratados como dos problemas independientes.

ÁLVARO GÓMEZ RAMOS.

- 37 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

5

CAPÍTULO: TOOLBOX DE ROBÓTICA.

5.1

Introducción

El toolbox de robótica permite al usuario crear y manipular fácilmente tipos de datos fundamentales para la robótica, como pueden ser las transformaciones homogéneas, cuaternios y trayectorias. Incluye funciones para la cinemática directa e inversa, así como la dinámica. Se basa en un método general de representación de cinemática y dinámica de manipuladores a través de matrices. Estas comprenden, en el caso más simple, los parámetros de Denavit-Hartenberg del robot y pueden ser creadas por el usuario para cualquier manipulador. La descripción del manipulador puede ser mejorada, aumentando la matriz, para incluir parámetros inerciales de los eslabones, y parámetros de inercia y fricción de los motores.

5.2

Representación de traslación 3D y orientación

En coordenadas cartesianas la traslación puede ser representada por un vector de posición. Muchas representaciones de orientaciones en 3D han sido propuestas, pero la mas común en robótica son las matrices de rotación ortonormales. Una transformación homogénea es una matriz 4x4 que representa traslación y orientación y que además encaja muy bien con matlab, debido a la capacidad de manipulación de matrices de este. Las transformaciones homogéneas describen la relación entre la traslación y la rotación

Por ejemplo, para una traslación T = transl(0.5, 0.0, 0.0), una rotación en y T = roty(pi/2) y una rotación en z T = rotz(pi/2):

ÁLVARO GÓMEZ RAMOS.

- 38 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

5.3

Cinemática directa

La cinemática directa consiste en resolver la posición cartesiana y la orientación del efector final dado el valor de las articulaciones y conociendo la estructura del robot a través de la matriz de Denavit-Hartenberg. Existe una función en el toolbox para resolver la cinemática directa a través de la matriz DH, es la función fkine. FKINE(DH, Q) calcula la cinemática directa de una manipulador para un estado dado por el vector de variables articulares Q. Dicho manipulador quedará descrito por su matriz de parámetros de Denavit-Hartenberg (DH). Es posible utilizar esta función de dos modos: a) Si Q es un vector, entonces es interpretado como el vector de variables articulares para el que se pretende calcular el modelo directo, y FKINE devuelve la transformación homogénea T correspondiente al último enlace del manipulador. En este caso Q deberá ser un vector fila. b) Si Q es una matriz, cada fila será interpretada como un vector de variables articulares, y T será una matriz tridimensional 4x4xm, siendo m el número de filas de Q. T es lo que se denominará una trayectoria de transformaciones homogéneas. Para extraer la transformación homogénea (Ti) que hay "condensada" en la fila i-ésima de la matriz T, bastará con escribir: TI = T(:,:,i) Empleando la función FKINE según este último modo, es posible obtener todas las transformaciones homogéneas correspondientes a una trayectoria dada en el espacio de variables articulares.

5.4

Cinemática inversa

La cinemática inversa consiste en encontrar los valores articulares que me posicionan al robot en una determinada posición. El toolbox incluye una función para resolver la cinemática inversa de los robots, es la función ikine. Q = IKINE(DH, STOL, ILIMIT, T) Q = IKINE(DH, STOL, ILIMIT, T, Q0) Q = IKINE(DH, STOL, ILIMIT, T, Q0, M) Devuelve los valores de las variables articulares necesarios para que el efector final del manipulador tenga la posición y orientación dadas por la transformación T. La solución del problema cinemático inverso no es única en general, y es posible que para

ÁLVARO GÓMEZ RAMOS.

- 39 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB una misma orientación y posición deseadas, se obtengan soluciones distintas en función del vector inicial de variables articulares (Q0) que se le pase a IKINE. Es posible usar la función para que devuelva las variables articulares correspondientes a una sola posición y orientación, o bien a una trayectoria de posiciones y orientaciones. Eso dependerá del formato del parámetro T: - Si T es una transformación homogénea, entonces IKINE devuelve un vector fila(Q) con las variables articulares correspondientes a la posición y orientación indicadas en la matriz T. - Si T es una trayectoria de transformaciones homogéneas, entonces el resultado será una matriz (Q), en la que la fila i-ésima contendrá las variables articulares correspondientes a la transformación T (:, :, i ). La estimación inicial para Q en cada paso se toma de la solución obtenida en el paso anterior. Sea cual sea el formato de T, la estimación inicial para el vector de variables articulares será la dada en el parámetro Q0 (puede ser una columna o una fila), y en el caso de que no se lo demos, asume que es el vector nulo. Para el caso de un manipulador con menos de 6 grados de libertad el efector final no podrá alcanzar algunas posiciones y orientaciones. Esto normalmente lleva a una no convergencia de IKINE. Una solución consiste en especificar un vector (fila o columna) de pesos (M), cuyos elementos serán 0 para aquellos grados de libertad que en cartesianas estén restringidos, y 1 en otro caso. Los elementos de M se corresponden con las traslaciones a lo largo de los ejes X, Y y Z, y con las rotaciones entorno a los ejes X, Y y Z. Por ejemplo si el manipulador no se puede desplazar a lo largo del eje Z, ni rotar entorno a los ejes X e Y, M deberá ser el vector [1 1 0 0 0 1]. El número de elementos no nulos debe ser igual al número de grados de libertad del robot. ILIMIT es el número máximo de iteraciones que se ejecutarán en busca de una solución (un valor usual es 1000). STOL será la máxima diferencia que se admitirá entre la transformación correspondiente a las variables articulares solución y la transformación con la posición y orientación especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo uso de la función TR2DIFF.

ÁLVARO GÓMEZ RAMOS.

- 40 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

BLOQUE 2: DESARROLLO DEL SOFTWARE

ÁLVARO GÓMEZ RAMOS.

- 41 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1

CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT.

1.1

Introducción.

Durante los estudios universitarios hemos recibido una amplia formación en robótica, tanto teóricamente como en la práctica, aprendiendo a trabajar matemáticamente con los robots o a programarlos para que realicen diversas tareas. En el transcurso de esa formación hemos utilizado diversas herramientas informáticas que nos ayudan en diferente grado a tratar con los robots. Algunas herramientas nos aportan datos que usamos para corroborar con los nuestros propios, otras nos facilitan las operaciones matemáticas. Las hay que tratan sobre el control cinemático y dinámico del robot, facilitando el trabajo y ahorrando mucho tiempo al usuario. La principal herramienta utilizada es el software de programación MATLAB. MATLAB es la abreviatura de Matrix Laboratory (laboratorio de matrices). Es un programa de análisis numérico creado por The MathWorks en 1984. Está disponible para las plataformas Unix y Windows. Se pueden ampliar sus capacidades con Toolboxes, algunas de ellas están destinadas al procesado digital de señal, adquisición de datos, economía, inteligencia artificial, lógica difusa... También cuenta con otras herramientas como Simulink, que sirve para simular sistemas. Es precisamente uno de esos toolboxes, una de las herramientas más utilizadas en robótica, el Robotic Toolbox. Este toolbox consta de una serie de funciones que resultan más o menos útiles a la hora de trabajar en el control de los robots. Las funciones que incluye se dividen en varios grupos: - Transformaciones homogéneas - Cuaternios - Cinemática - Dinámica - Generación de trayectorias - Gráficas - Creación de modelos de robots - Demostraciones Todas estas funciones nos permiten trabajar con el robot, pero a través de facilitarle ciertos parámetros. Normalmente trabaja a través de un modelo propio que crea del robot a partir de la matriz DH que lo define. Esto supone que debemos conocer dicha matriz para poder realizar ciertos cálculos, lo cual implica ciertos conocimientos de robótica previos para poder trabajar.

ÁLVARO GÓMEZ RAMOS.

- 42 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1.2

SICON-BOT.

Hoy en día la robótica se va extendiendo, y cada vez se generaliza más, con lo cual es conveniente crear ciertas herramientas que permitan acercar a usuarios menos expertos, como pasó con la informática, que poco a poco se fue convirtiendo en un tema más generalista y se fueron desarrollando cada vez más unas herramientas más intuitivas y sencillas. Otra cuestión es la comodidad. Si disponemos de herramientas para trabajar con los robots de una manera más cómoda a partir del modelo matemático de éste, ¿por qué no tener también una herramienta que nos facilite la obtención del modelo matemático de una forma intuitiva y sencilla que no necesite grandes conocimientos previos?. El programa SICON-BOT surge de estas necesidades. Su objetivo es facilitar a los usuarios menos expertos, es hacer más cómodo el trabajo para los más expertos, e integrar en una sola herramienta, otras herramientas, para hacer su uso más sencillo. En definitiva, SICON-BOT tiene tres objetivos principales: FACILIDAD-COMODIDAD-INTEGRACIÓN

1.3

-

Facilidad: Con un entorno gráfico familiar, con diseño de robots en 3D, con ventanas, menús desplegables, botones, y demás funciones gráficas que son familiares a la mayoría de personas.

-

Comodidad: A la hora de integrar nuevas herramientas que nos permiten obtener automáticamente parámetros necesarios para otras herramientas, que hasta ahora teníamos que calcular a mano.

-

Integración: En un mismo software podemos utilizar funciones pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se resuelven en la programación interna.

Objetivos de la primera versión.

En la primera versión del software SICON-BOT el objetivo es crear las bases de la herramienta, para que poco a poco puedan ir ampliándose en un futuro por nuevos usuarios interesados, para lo cual el código es accesible. Se ha desarrollado enteramente en Matlab un software a través de elementos familiares, como ventanas y menús, al estilo Windows. Con él podremos dibujar en tres dimensiones el robot con el que queramos trabajar, introduciendo ciertos parámetros como la longitud de los tramos. Una vez dibujado el robot, podemos calcular sus ejes y sobre todo su matriz de Denavit-Hartenberg. Cuando ya tenemos calculada la matriz DH podemos posicionar el robot en las posiciones deseadas, o calcular su cinemática, tanto directa como inversa.

ÁLVARO GÓMEZ RAMOS.

- 43 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB El diseño del robot, es un diseño genérico y limitado a ciertos tipos de robots, púes se basa en la adición de elementos predefinidos, pero con esto podemos diseñar los robots más comunes. En resumen los objetivos de la primera versión es el desarrollo desde cero del software, creando toda la programación necesaria para el entorno gráfico, el diseño de robots en 3D, el cálculo de ejes, el cálculo de matrices DH, el cálculo de la cinemática directa e inversa, y el posicionamiento del robot en 3D. Con esto tenemos en la primera versión los tres objetivos fundamentales:

1.4

-

Facilidad: Con el entorno gráfico que incorpora, el diseño intuitivo de robots, el cálculo automático de parámetros mediante simple inserción de datos sencillos.

-

Comodidad: Púes realiza funciones como la obtención automática de los ejes o de la matriz DH.

-

Integración: Pues incorpora internamente funciones del toolbox de robótica como la cinemática directa e inversa.

Posibles mejoras futuras.

Existen una gran cantidad de posibles mejoras a añadir al programa, aquí pondremos algunas que serían de gran utilidad: -

1.5

Adición de dinámica Adición de movimiento en 3D siguiendo trayectorias Interacción con otros robots Posibilidad de diseñar un entorno de trabajo Cambio de plataforma de programación, para lograr una mayor eficacia, como podría ser el lenguaje de programación C o C++

Requisitos mínimos.

El software ha sido creado en matlab 7.0.0.19920 (R14), por lo cual necesita cumplir los requisitos mínimos de este software y su instalación: -

Sistemas operativos: Windows XP, Windows 2000 (Service Pack 3 or 4) o Windows NT 4.0 (Service Pack 5 or 6a) Procesadores: Pentium III, IV, Xeon, Pentium M, AMD Athlon, Athlon XP, Athlon MP Espacio en disco: 345 MB RAM: 256 MB (Minimum), 512 MB (Recommended)

ÁLVARO GÓMEZ RAMOS.

- 44 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB -

Gráfica: Tarjeta gráfica con 16-, 24-, o 32-bit OpenGL

También se necesita tener instalado el Robotics Toolbox versión 7 April-2002

1.6

Inicialización.

Una vez instalado MATLAB, el toolbox de robótica y SICON-BOT, debemos iniciar MATLAB,

elegir el directorio donde tenemos instalado SICON-BOT

y escribir la palabra “sicon”

ÁLVARO GÓMEZ RAMOS.

- 45 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2

CAPÍTULO: GRÁFICA DE SICON-BOT.

2.1

Creación de ventana principal y menús desplegables.

Para la creación de la ventana principal usamos el comando figure dentro de la función “sicon”, con la que creamos una ventana en windows, que sea visible, haga uso de opengl, de color de fondo blanco y otras opciones.

Al la ventana se le añade una imagen de fondo mediante:

Con “imread” cargamos una imagen del disco duro en una variable de matlab y con “subimage” mostramos la imagen en la ventana

ÁLVARO GÓMEZ RAMOS.

- 46 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Con “uimenu” creamos los menús desplegables y sus opciones, en este ejemplo creamos el menú desplegable llamado Archivo que contiene las opciones Nuevo Gráficamente, Salvar Robot, Salvar D-H, Cargar Robot y salir

2.2

Paneles y plots de representación Creamos una serie de elementos visuales, como: -

Paneles:

ÁLVARO GÓMEZ RAMOS.

- 47 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

-

Plots para representación de los elementos posibles del robot:

El resultado final es la ventana principal del programa, desde donde podremos diseñar el robot o ejecutar sus herramientas:

ÁLVARO GÓMEZ RAMOS.

- 48 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Elementos posibles del robot

2.3

Parámetros del robot

Plot principal

Eslabones y articulaciones.

Existen 4 tipos de elementos que están disponibles para diseñar nuestro robot, estos son: Articulación de traslación, articulación de rotación, eslabón y base.

Traslación

Rotación

Eslabón

Base

La ventana principal se ha dividido en una serie de subplots, donde se dibujaran diferentes elementos.

ÁLVARO GÓMEZ RAMOS.

- 49 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Con la orden subplot se divide la ventana en una matriz (m, n) de pequeños plots y después se elige el plot “p” deseado: SUBPLOT (m,n,p) En nuestro caso particular, sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]); creamos un subplot dividiendo la pantalla en 5 filas y 3 columnas, y seleccionamos las celdas 2, 3, 5, 6, 8, 9, 11, 12, 14, 15 para crear el subplot. 1

2

3

4

5

6

7 10 13

8 11 14

9 12 15

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

Para añadir un elemento a nuestro robot debemos introducir unos parámetros en función del elemento a añadir, y luego pulsar el botón correspondiente.

ÁLVARO GÓMEZ RAMOS.

- 50 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Con esto conseguiremos introducir un elemento de traslación en el plot de principal. Por defecto con los valores indicados en la figura, el elemento se dibujaría vertical y con una longitud inicial de 10 ud. El nombre de las variables en este caso debe ser obligatoriamente “t1, t2, t3, t4 o t5”, con lo cual el nº de elementos de traslación posibles en el robot es de 5.

Para poder añadir el elemento en otra posición, disponemos de dos parámetros

Con estos parámetros haremos rotar la figura vertical tantos grados como le indiquemos alrededor de los ejes “x” y “z” que pasan por el centro de la base del robot.

ÁLVARO GÓMEZ RAMOS.

- 51 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

z

x

y

Por ejemplo, si queremos que el elemento de traslación se dibuje apoyado sobre el eje “y”, tenemos que indicar un giro de 90º sobre el eje “x”

Para ver el efecto de un giro sobre el eje “z” haremos girar 90º sobre el eje “x” y 45 sobre el eje “z”

Con estos dos parámetros podemos posicionar cualquier elemento en la posición deseada. Si lo que queremos insertar es una articulación de rotación:

ÁLVARO GÓMEZ RAMOS.

- 52 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Tenemos que proporcionarle un nombre a la variable, aconsejamos usar q1, q2, q3, etc. Aunque la única limitación que existe es la de no usar los reservados para articulaciones de traslación ni otro que ya hayamos usado anteriormente. El elemento con los valores por defecto se dibujará también inicialmente vertical, aunque no nos hará falta indicarle longitud, pues esta es fija, y el elemento de rotación es incorpóreo, es decir no ocupa lugar, el elemento anterior al de rotación y su inmediatamente posterior están unidos.

El elemento de rotación hará girar a todos los elementos posteriores a él a través del eje que lo atraviesa desde el centro de la base inferior hasta el centro de su base superior.

Por eso es muy importante su colocación inicial.

Aunque gráficamente será igual girar 90º en el eje “x” o -90º, el robot resultante será totalmente distinto.

ÁLVARO GÓMEZ RAMOS.

- 53 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB El elemento eslabón es igual al de traslación sólo que su longitud indicada será siempre la misma. Y el elemento base, sirve para elevar al robot en su posición inicial. Los elementos se irán colocando uno tras el otro, y el punto inicial de uno se colocará en el punto inicial del siguiente. rotación traslación eslabón base

Con estos elementos y sus parámetros podemos dibujar una amplia gama de robots diferentes.

2.4

Botones, textos y cuadros editables.

Con estos elementos podemos interactuar con el software (botones), leer informaciones (textos) o indicar valores (cuadros editables)

Botones:

Aquí indicamos que queremos un botón pulsador “togglebutton” que al pulsar llame a la función “rot3d” Textos:

ÁLVARO GÓMEZ RAMOS.

- 54 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Le indicaremos que queremos un texto que diga “Rot. En X (º)” y en que posición lo queremos. Cuadros editables:

Elegimos la posición deseada del elemento y que valor inicial queremos que aparezca por defecto “0”.

ÁLVARO GÓMEZ RAMOS.

- 55 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3

CAPÍTULO: POSIBILIDADES DE SICON-BOT.

3.1

Nuevo robot

Para poder crear un nuevo robot, tenemos que iniciar el programa desde matlab escribiendo en el espacio de trabajo “sicon”, con lo que se nos abrirá la ventana de bienvenida.

Luego nos vamos al menú desplegable “Archivo” y seleccionamos la opción “Nuevo gráficamente”.

Al pulsar esta opción se abre la ventana principal de diseño del robot.

ÁLVARO GÓMEZ RAMOS.

- 56 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Ahora dependiendo del robot que queramos diseñar iremos introduciendo nuestros elementos siguiendo el siguiente orden: 1. Si es un elemento de base o eslabón no hace falta indicar nada en la casilla “Nombre Var”, pero si es un elemento de rotación si que debemos elegir un nombre para la variable que sea diferente a los anteriores proporcionados y diferente de t1, t2, t3, t4 y t5. Sin embargo si es un elemento de traslación debemos indicar un consecutivamente los nombres t1, t2, t3, t4 y t5.

2. Una vez resuelto el nombre de la variable elegiremos el parámetro longitud, que sólo tiene efecto en los elementos eslabón y base, que establece su longitud definitiva, y en el elemento traslación establece su longitud inicial.

3. Para terminar de parametrizar el eslabón debemos indicar la rotación en X y en Z, pudiendo así posicionar el elemento en cualquier posición, excepto el elemento base que es siempre vertical. Tener en cuenta que el sentido de rotación depende mucho de estos parámetros.

4. Una vez que tenemos el elemento totalmente parametrizado, sólo nos queda indicarle que tipo de elemento es y será añadido automáticamente tomando como punto inicial el final del elemento anterior o si es el primer elemento, será colocado en (0,0,0). Ejemplo: 1.

2.

3.

ÁLVARO GÓMEZ RAMOS.

- 57 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.

5.

El resultado es:

Así iremos diseñando nuestro robot paso a paso, elemento a elemento, hasta que tengamos dibujado nuestro robot por completo:

Una vez diseñado por completo el siguiente y obligatorio paso es calcular el robot.

3.2

Calcular robot

Para calcular el robot, o lo que es lo mismo, obtener los ejes del robot, debemos ir al menú desplegable “Ejecución” y seleccionar la opción “Calcular robot”.

ÁLVARO GÓMEZ RAMOS.

- 58 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Este proceso implicará un cálculo complejo con variables simbólicas, que dependiendo del ordenador y de la complejidad del robot, tardará más o menos tiempo. Al final el resultado serán los ejes del robot representados en el propio diseño y representando al eje “z” en azul, el eje “x” en rojo y el eje “y” en verde.

Una vez calculado los ejes, podemos realizar varias acciones como salvar nuestro robot, para usarlo en un futuro, calcular sus parámetros de Denavit-Hartenberg o ver su cinemática directa.

3.3

Salvar Robot

Para salvar el robot es necesario haber calculado sus ejes con anterioridad, y lo que esto implica, haberlo diseñado. Luego iremos al menú “Archivo” y elegiremos la opción “Salvar robot”.

Entonces se nos abrirá una ventana donde elegir en que lugar guardar el robot y con que nombre. Se guardará bajo la extensión “.mat” cuando le demos al botón guardar.

ÁLVARO GÓMEZ RAMOS.

- 59 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.4

Cargar Robot

Si tenemos un robot guardado previamente podemos recuperarlo cuando volvamos a utilizar el programa, para ello tenemos que iniciar el programa desde matlab escribiendo en el espacio de trabajo “sicon”, con lo que se nos abrirá la ventana de bienvenida.

Luego nos vamos al menú desplegable “Archivo” y seleccionamos la opción “Nuevo gráficamente”.

ÁLVARO GÓMEZ RAMOS.

- 60 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Al pulsar esta opción se abre la ventana principal de diseño del robot.

Luego vamos al menú desplegable “Archivo” y elegimos la opción cargar robot.

Se nos abrirá una ventana similar a la de salvar robot, donde buscaremos el lugar donde tenemos guardado el robot, lo seleccionaremos y pulsaremos el botón cargar, añadiendo así el robot al espacio de trabajo con los ejes ya calculados.

ÁLVARO GÓMEZ RAMOS.

- 61 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.5

Parámetros D-H

Una vez calculado el robot o cargado, podremos calcular sus parámetros de Denavit-Hartenberg o D-H, para ello iremos al menú desplegable “Ejecución” y elegiremos la opción “Parámetros D-H”

Entonces se nos abrirá una nueva ventana que contiene los parámetros D-H del robot representados de forma matricial: di

ai

tethai

alphai

… … …

Con estos tendremos calculados los parámetros D-H de una forma totalmente automática, con los nombres de las variables que le indicásemos. Una vez calculado los parámetros D-H, podremos acceder a nuevas opciones del programa como “Salvar D-H”, “Posicionar Robot” o “Cinemática Inversa”

ÁLVARO GÓMEZ RAMOS.

- 62 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.6

Salvar D-H

Con esto podremos exportar la matriz D-H y la matriz P_D_H, la primera nos indica los parámetros de Denavit-Hartenberg, la segunda además de incluir la matriz DH nos guarda también el tipo de articulación del robot, dando un 0 para rotación y un 1 para traslación. La matriz P_D_H es totalmente compatible con el toolbox de robótica de matlab. Para salvar D-H debemos seleccionar la opción “Salvar D-H” del menú desplegable “Archivo”, elegir donde guardarla y con que nombre.

El archivo guardado también tendrá la extensión “.mat”

3.7

Cinemática directa

Para poder ver la cinemática directa del robot debemos ir al menú desplegable “Ejecución” y seleccionar la opción “Cinemática directa”.

ÁLVARO GÓMEZ RAMOS.

- 63 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Se nos abrirá una nueva ventana que nos mostrará la cinemática directa del robot de la siguiente forma:

Así sabremos cual será la posición del extremo del robot a partir del valor de las variables.

3.8

Cinemática inversa

Para poder ver la cinemática inversa del robot debemos ir al menú desplegable “Ejecución” y seleccionar la opción “Cinemática inversa”.

Se nos abrirá la siguiente ventana, donde podremos indicar el valor de la posición final del extremo del robot que deseamos obtener, y pulsando el botón “calcular”, obtendremos el valor necesario de las variables para que el robot alcance dicha posición.

ÁLVARO GÓMEZ RAMOS.

- 64 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Posición deseada

Nombre de las variables

Valor calculado de las variables

Una vez pulsemos el botón “calcular” nos aparecerá otra nueva pantalla con un diseño esquemático del robot en la posición deseada. Ejemplo:

ÁLVARO GÓMEZ RAMOS.

- 65 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.9

Posicionar Robot

Para poder ver la cinemática inversa del robot debemos ir al menú desplegable “Ejecución” y seleccionar la opción “Posicionar robot”.

Se nos abrirá la siguiente ventana, donde podremos indicar el valor de las variables con las cuales queremos posicionar al robot, y pulsando el botón “intr.. Dato”, se abrirá una nueva ventana mostrando un diseño esquemático del robot en la posición calculada. Además si marcamos la opción “Ver Robot en 3D”, también se posicionará el robot de nuestro espacio de trabajo principal en dicha posición, teniendo en cuenta que el robot de la ventana emergente puede poseer cambios de variables para simplificar la matriz D-H.

ÁLVARO GÓMEZ RAMOS.

- 66 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.10 Ayuda Sicon-bot dispone de una pequeña ayuda para aprender a usar el programa. Dicha ayuda se encuentra en la opción “Ayuda Sicon” del menú desplegable “Ayuda”.

Se abrirá una ventana que contiene la ayuda disponible del software.

ÁLVARO GÓMEZ RAMOS.

- 67 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

3.10 Herramientas A la derecha del espacio de trabajo de sicon-bot disponemos de un panel de herramientas.

ÁLVARO GÓMEZ RAMOS.

- 68 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Existen tres herramientas disponibles: -

Rotar robot:

Nos permite rotar el robot en el espacio de trabajo principal. -

Ver/Ocultar ejes:

Nos permite ocultar los ejes calculados del robot o volver a verlos cuando deseemos. -

Ver/Ocultar robot:

Nos permite ocultar el robot calculados del robot o volver a verlos cuando deseemos

ÁLVARO GÓMEZ RAMOS.

- 69 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4

CAPÍTULO: PROGRAMACIÓN DE SICON-BOT.

4.1

Introducción

El proyecto de la creación de sicon-bot se basa en su mayor parte en la programación del software en el lenguaje MATLAB. Y esto incluye gran cantidad de funciones y de muy diversas estilos. Sicon-Bot es un software que pretende tener un aspecto gráfico accesible para todos los usuarios, y que se basa en la utilización de elementos visuales para interactuar con el software, mediante el uso de botones, gráficos, textos, diseños en 3D, etc. Para ello hace falta conocer la parte gráfica de MATLAB, un lenguaje usado normalmente para resolver la programación orientada a las matemáticas. La elección de MATLAB para programar el software es precisamente por su facilidad para ejecutar complejas operaciones matemáticas, pues aunque el aspecto visual es lo más destacado para el usuario, la base es la programación de funciones que implementen operaciones matemáticas complejas como la rotación y traslación de elementos en 3D y sobre todo el tratamiento con grandes matrices que contienen todos los datos del robot diseñado, pues este una vez calculado no será más que una serie de matrices numéricas o simbólicas con las que el software tiene que tratar. Al sopesar la facilidad matemática de MATLAB con la facilidad gráfica de otros lenguajes, pesa más la parte de MATLAB, por lo que el lenguaje de programación elegido es MATLAB. Esto implica que debemos realizar toda la parte gráfica con un lenguaje menos apto para ello. En resumen la programación de MATLAB tiene un recorrido de ida y vuelta, es decir, se crea un entorno gráfico accesible para el diseño en 3D de robots, luego el software convierte el robot gráfico en matrices, trabaja con ellas y muestra el resultado de forma gráfica. En resumen: Datos de entrada en forma gráfica Conversión

Datos de entrada en forma matricial Operación

Datos de salida en forma matricial Conversión

Datos de salida en forma gráfica

ÁLVARO GÓMEZ RAMOS.

- 70 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.2

Inicialización de ventana inicial y menús desplegables

La primera fase de la programación del software se centra en la creación del entorno gráfico necesario para el uso del programa. Para ello creamos la ventana principal a través del comando de matlab “figure”

Con este comando y las propiedades que le hemos indicado, se abre una ventana nueva, que nos devuelve un valor único y representativo que almacenamos en la variable “hf1” para poder realizar futuras operaciones con la ventana. Le hemos indicado ciertos parámetros a través de las propiedades, veamos algunos interesantes: -

: Indicamos que queremos que la ventana sea visible. : Indicamos que las unidades para indicarle la medida de la ventana, en este caso con “normalizad” queremos que (0, 0) sea la esquina inferior de abajo y (1, 1) la esquina superior derecha. : Elige el método de representación gráfica de los objetos, con “opengl” elegimos el método más potente de representación gráfica. : Elegimos que el color de fondo sea blanco. : Deshabilitamos la posibilidad de cambiar el tamaño de la ventana. : El nombre que aparecerá en la ventana será “SiconBot”. : Elimina que aparezca el número de la ventana siguiendo su orden de creación. : Eliminamos la posibilidad de que la ventana se pueda adosar al espacio de trabajo de matlab. : Eliminamos los menús desplegables que trae por defecto matlab. :_Indicamos la posición y tamaño de la pantalla.

ÁLVARO GÓMEZ RAMOS.

- 71 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Una vez creada la ventana principal, le queremos añadir la imagen del logo del software de fondo, para ello:

Con ello almacenamos la imagen en una variable de matlab y luego la mostramos con el comando “subimage” que nos permite mostrar en una ventana varias imágenes. Ahora creamos los menús desplegables y si son accesibles o no desde el principio:

Con “uimenu” creamos los menús, cuyo nombre indicamos a través de ‘Label’,’nombre’, por ejemplo: -

Creamos el menú llamado Archivo y guardamos su identificador en la variable “mfile”

Pero con “uimenu” también podemos crear las opciones que se crearán en el menú al desplegarse:

Al indicarle la variable mfile donde se almacena el indicador del menú, estamos asociando esta opción a dicho menú. Con label damos texto a la opción, con la propiedad “Callback” indicamos la rutina que se ejecutará cuando se pulse la opción, y con la propiedad ’Separator’,on’, indicamos que dibuje una línea para separar de las siguientes opciones posibles, para así crear subgrupos diferenciados.

Existen otras propiedades interesantes como para que la opción inicialmente no esté habilitada, y poder habilitarla cuando se cumpla alguna , que hace que la condición. También es interesante la propiedad ÁLVARO GÓMEZ RAMOS.

- 72 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB opción también se pueda ejecutar al pulsar la combinación de teclas “Alt + tecla elegida”.

4.3

Entorno de creación de robots

Cuando elegimos nuevo robot, nos aparece la ventana con el entorno que nos permite diseñar el robot

Lo primero que hacemos es cambiar el color de fondo: -

: Con esto decimos que la propiedad ‘Color’ cambie a la indicada en la ventana que representa el indicador “hf1”.

Luego se cambian las opciones de habilitación de los menús desplegables según nos interesen: -

: Habilitamos la opción Cargar del menú Archivo, se indica mediante el indicador de dicha opción.

Creamos unos paneles para colocar en ellos diferentes elementos y agrupar los que tienen algo en común. Esta acción la hacemos con el comando uipanel, guardando su indicador en una variable. Además le indicamos a través de las propiedades ‘BackgroundColor’ y ‘Position’, de que color será y donde estará colocado con respecto a los límites de la ventana principal, y le damos un nombre con ‘title’:

ÁLVARO GÓMEZ RAMOS.

- 73 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora crearemos los plots de la ventana, en concreto serán 6, uno principal donde se dibuja el robot, y 5 secundarios que nos mostrarán las figuras posibles a insertar.

Dividimos la pantalla en 15 partes e indicamos que el plot principal ocupará el espacio de las divisiones número 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15. Además con “xlabel” e “ylabel” modificamos propiedades de los ejes “x” e “y” respectivamente, como por ejemplo elegir un texto a mostrar y su color.

Luego creamos el resto de plots y hacemos que no se vean los ejes:

ÁLVARO GÓMEZ RAMOS.

- 74 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Lo siguiente será dibujar las figuras de referencia de las que podremos elegir para añadir a nuestro robot:

Para ello creamos una matriz con las coordenadas de los vértices y otra donde se le indican las caras de la figura a través de los vértices que la componen. Luego elegimos el plot donde queremos representar la figura, y ejecutamos el comando “patch” indicándole la matriz de vértices y la de caras y el color del objeto a través de la propiedad ‘FaceVertezCData’. También se le indica la manera de colorear el objeto, en este caso a través de ‘FaceColor’,’flat’, las caras se dibujarán por homogéneas.

Con

“Axis

xy’

indicamos que los ejes son cartesianos, y con especificamos el rango que queremos que muestren los ejes. “camorbit” elige la rotación de la cámara para mostrar el objeto, indicándole la rotación horizontal y vertical. Después de preparar los plots de representación secundarios vamos a añadir , consiguiendo que entre un rayo de luz iluminación al plot principal: que ilumina al objeto desde la derecha. El siguiente paso será la creación de botones, textos y cuadros editables, que lo haremos con el comando “uicontrol” y sus propiedades.

Con la propiedad ‘Style’ indicamos el tipo de elemento, que puede ser: -

Cajas de chequeo (Check boxes) Cuadros editables (Editable text fields) Cuadros de listas (List boxes) Menús emergentes (Pop-up menus) Botones de pulsar (Push buttons) Botones de elección (Radio buttons) Deslizadores (Sliders)

ÁLVARO GÓMEZ RAMOS.

- 75 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB -

Textos (Static text) Botones de estado (Toggle buttons)

Con la propiedad ‘CallBack” elegimos la rutina a ejecutar al activarse el elemento. Luego elegimos la posición a colocar.

Botones de pulsar Textos

Cuadros editables

Botones de estado

4.4

Añadir elemento de traslación

Para añadir un elemento de traslación en nuestro diseño del robot tan sólo tenemos que parametrizarlo correctamente y pulsar el botón correspondiente. Pero esto es a nivel de usuario, en la programación es un proceso más complejo que veremos a continuación. La función que coloca un elemento de traslación en nuestro robot es la siguiente:

ÁLVARO GÓMEZ RAMOS.

- 76 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Analicemos paso por paso la función. Primero elegimos las variables globales que nos van a ser útiles: -

MR: Es la matriz que contendrá los datos del robot, es decir es nuestro robot desde el punto de vista matemático. Q1: Es un vector que contiene los valores de las diferentes variables de traslación, según su orden de creación. n: Guarda el número de elementos que tiene nuestro robot. (x,y,z): Son las coordenadas del punto actual donde se añadirá el siguiente elemento del robot. htextoe: Cuadro editable que contiene el valor de la rotación sobre el eje de traslación del elemento. (Actualmente “0” por estar deshabilitada).

-

htexto: Cuadro editable que contiene el valor de la variable actual de traslación.

-

tras: Guarda el número de elementos de traslación que contiene el robot.

MR es la matriz simbólica que contiene el robot en forma matemática. Esta matriz es de (n+1 x 8), siendo n el número de elementos del robot. Las filas corresponden a los elementos del robot, reservando la última fila para un elemento ficticio situado en el extremo del robot, con la única finalidad de añadir un eje en el extremo del robot. Las columnas contienen datos que nos parametrizan de forma inequívoca al elemento en el robot: MR: (1 [2 3 4] [5 6 7] 8) MR: (tipo de elemento [posición inicial del elemento] [pto. Final] rot. En el eje) Luego hacemos que si n=0, es decir, es el primer elemento del robot, inicializamos la variable.

Lo siguiente es indicar que tenemos un elemento más en nuestro robot:

Obtenemos el valor inicial de la variable de traslación que le indicó el usuario:

Incrementamos en uno el número de elementos de traslación que contiene nuestro robot: ÁLVARO GÓMEZ RAMOS.

- 77 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Lo siguiente sería guardar el valor del punto inicial donde dibujar el elemento en forma simbólica:

El paso siguiente recurre a la llamada de una función que dibuja el robot a partir del punto inicial y los datos indicados y nos devuelve el nuevo valor inicial para el próximo elemento a añadir que coincide con el punto final del elemento inicial. Este dato nos lo devuelve en forma numérica y simbólica:

Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido, siendo un “1” para los elementos de traslación:

Almacenamos el punto final del elemento añadido, que como ya hemos indicado, coincide con el punto inicial del próximo elemento.

Luego guardamos el valor de rotación que le indicamos que deseamos que tenga la representación gráfica del elemento.

Ya tenemos parametrizado el elemento traslación del todo, pudiéndolo dibujar cuando se requiera.

4.5

Añadir elemento de rotación

Para añadir un elemento de rotación usamos el siguiente código:

ÁLVARO GÓMEZ RAMOS.

- 78 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Veamos las nuevas variables globales que nos hacen falta: -

Q2: Es un vector que contiene los valores de las diferentes variables de rotación, según su orden de creación. rotac: Guarda el número de elementos de traslación que contiene el robot.

Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot, inicializamos la variable.

Lo siguiente es indicar que tenemos un elemento más en nuestro robot:

Obtenemos el valor inicial de la variable de rotación que le indicó el usuario:

Incrementamos en uno el número de elementos de rotación que contiene nuestro robot: El elemento rotación no ocupa espacio, es decir el punto inicial y el final serán el mismo, por ello los siguiente sería llamar a una función que nos dibuja el elemento rotación y nos guarda en MR el punto final e inicial que son coincidentes:

Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido, siendo un “2” para los elementos de rotación:

ÁLVARO GÓMEZ RAMOS.

- 79 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Luego guardamos el valor de rotación que le indicamos que deseamos que tenga la representación gráfica del elemento.

Ya tenemos parametrizado el elemento rotación del todo, pudiéndolo dibujar cuando se requiera.

4.6

Añadir elemento eslabón

Para añadir un elemento eslabón usamos el siguiente código:

Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot, inicializamos la variable.

Lo siguiente es indicar que tenemos un elemento más en nuestro robot:

Luego llamamos a una función que nos dibuja el elemento base y nos guarda en MR el punto final e inicial:

ÁLVARO GÓMEZ RAMOS.

- 80 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido, siendo un “3” para los elementos eslabón:

Luego guardamos el valor de rotación que le indicamos que deseamos que tenga la representación gráfica del elemento.

Ya tenemos parametrizado el elemento eslabón del todo, pudiéndolo dibujar cuando se requiera.

4.7

Añadir elemento base

Para añadir un elemento base usamos el siguiente código:

Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot, inicializamos la variable.

Lo siguiente es indicar que tenemos un elemento más en nuestro robot:

ÁLVARO GÓMEZ RAMOS.

- 81 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Luego llamamos a una función que nos dibuja el elemento base y nos guarda en MR el punto final e inicial:

Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido, siendo un “4” para los elementos base:

Luego guardamos el valor de rotación que le indicamos que deseamos que tenga la representación gráfica del elemento.

Ya tenemos parametrizado el elemento base del todo, pudiéndolo dibujar cuando se requiera.

4.8

Posicionar y dibujar por primera vez el elemento traslación

Para dibujar el elemento traslación por primera usamos una función distinta que para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización, obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se dibujará a partir del punto inicial y el punto final obtenido.

Lo primero a realizar en la función es obtener los valores de parametrización que nos indico el usuario, en este caso necesitamos la longitud inicial y el nombre de la variable. La variable “num_simb_rot” guarda el número de variables de traslación que llevamos diseñadas. :

Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven para posicionar el robot adecuadamente:

ÁLVARO GÓMEZ RAMOS.

- 82 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8

Pto. final

X5

X7 X6 X4

Pto. inicial

X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.3, y-0.3, z) (x2,y2,z2) = (x+0.3, y-0.3, z) (x3,y3,z3) = (x+0.3, y+0.3, z) (x4,y4,z4) = (x-0.3, y+0.3, z) (x5,y5,z5) = (x-0.3, y-0.3, z+l) (x6,y6,z6) = (x+0.3, y-0.3, z+l) (x7,y7,z7) = (x+0.3, y+0.3, z+l) (x8,y8,z8) = (x-0.3, y+0.3, z+l) Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

ÁLVARO GÓMEZ RAMOS.

- 83 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento, luego sustituimos los valores simbólicos por los valores actuales, para tener el valor numérico del nuevo punto inicial:

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos que calcular su valor actual para posicionarlos:

ÁLVARO GÓMEZ RAMOS.

- 84 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.9

Posicionar y dibujar por primera vez el elemento rotación

Para dibujar el elemento rotación por primera usamos una función distinta que para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto inicial y los valores de giro indicadas por el usuario en la parametrización, obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se dibujará a partir del punto inicial y el punto final obtenido.

Lo primero a realizar en la función es obtener los valores de parametrización que nos indico el usuario, en este caso necesitamos el nombre de la variable. La variable “num_simb_rot2” guarda el número de variables de rotación que llevamos diseñadas. :

ÁLVARO GÓMEZ RAMOS.

- 85 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven para posicionar el robot adecuadamente:

A partir del valor del punto inicial, situamos el punto final coincidente:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8 X5

X7 X6

Pto. final Pto. inicial

X4 X1

X3 X2

La situación de los 8 vértices dependerá del centro de gravedad del elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.3, y-0.3, z-0.5) (x2,y2,z2) = (x+0.3, y-0.3, z-0.5) (x3,y3,z3) = (x+0.3, y+0.3, z-0.5) (x4,y4,z4) = (x-0.3, y+0.3, z-0.5) (x5,y5,z5) = (x-0.3, y-0.3, z+0.5) (x6,y6,z6) = (x+0.3, y-0.3, z+0.5) (x7,y7,z7) = (x+0.3, y+0.3, z+0.5) (x8,y8,z8) = (x-0.3, y+0.3, z+0.5) Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función: ÁLVARO GÓMEZ RAMOS.

- 86 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el centro de gravedad del elemento, éste rotará hasta posicionarse en su posición final.

En este caso el punto inicial no variará, por lo que coincidirá con el final. Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos que calcular su valor actual para posicionarlos:

ÁLVARO GÓMEZ RAMOS.

- 87 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.10 Posicionar y dibujar por primera vez el elemento eslabón Para dibujar el elemento eslabón por primera usamos una función distinta que para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización, obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se dibujará a partir del punto inicial y el punto final obtenido.

Lo primero a realizar en la función es obtener los valores de parametrización que nos indico el usuario, en este caso necesitamos la longitud:

ÁLVARO GÓMEZ RAMOS.

- 88 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven para posicionar el robot adecuadamente:

A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8

Pto. final

X5

X7 X6 X4

Pto. inicial

X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.3, y-0.3, z) (x2,y2,z2) = (x+0.3, y-0.3, z) (x3,y3,z3) = (x+0.3, y+0.3, z) (x4,y4,z4) = (x-0.3, y+0.3, z) (x5,y5,z5) = (x-0.3, y-0.3, z+l) (x6,y6,z6) = (x+0.3, y-0.3, z+l) (x7,y7,z7) = (x+0.3, y+0.3, z+l) (x8,y8,z8) = (x-0.3, y+0.3, z+l) Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

ÁLVARO GÓMEZ RAMOS.

- 89 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB “Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento:

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos que calcular su valor actual para posicionarlos:

ÁLVARO GÓMEZ RAMOS.

- 90 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.11 Posicionar y dibujar por primera vez el elemento base Para dibujar el elemento base por primera usamos una función distinta que para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización, obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se dibujará a partir del punto inicial y el punto final obtenido.

Lo primero a realizar en la función es obtener los valores de parametrización que nos indico el usuario, en este caso necesitamos la longitud inicial:

ÁLVARO GÓMEZ RAMOS.

- 91 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven para posicionar el robot adecuadamente:

A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8

Pto. final

X5

X7 X6 X4

Pto. inicial

X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.3, y-0.3, z) (x2,y2,z2) = (x+0.3, y-0.3, z) (x3,y3,z3) = (x+0.3, y+0.3, z) (x4,y4,z4) = (x-0.3, y+0.3, z) (x5,y5,z5) = (x-0.3, y-0.3, z+l) (x6,y6,z6) = (x+0.3, y-0.3, z+l) (x7,y7,z7) = (x+0.3, y+0.3, z+l) (x8,y8,z8) = (x-0.3, y+0.3, z+l) Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

ÁLVARO GÓMEZ RAMOS.

- 92 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB “Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento:

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos que calcular su valor actual para posicionarlos:

ÁLVARO GÓMEZ RAMOS.

- 93 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.12 Rotar punto respecto a ejes x e y que pasan por otro punto dado Este procedimiento lo resuelve por completo la función creada para la ocasión:

Para realizar esta operación matemática nos basamos en el concepto de rotación alrededor de los ejes “x”, “y” y “z” que pasan por el origen. Peor en nuestro caso necesitamos que roten alrededor de un punto que puede ser diferente al origen, para ello hacemos una traslación previa, restando al punto a rotar el punto por donde pasan los ejes, luego rotamos para luego volver a sumar lo anteriormente restado, es decir: Traslación al origen

Rotación

Deshacer traslación

Para realizar la traslación al origen hacemos la siguiente operación:

ÁLVARO GÓMEZ RAMOS.

- 94 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora el punto a tratar será el formado por las coordenadas (A,B,C). Lo siguiente es la rotación del punto. Cuando consideramos la rotación de un objeto en dos dimensiones, vimos la matriz de rotación alrededor del origen

Podemos generalizar este concepto a una rotación tridimensional alrededor del eje z.

En esta rotación, pensamos en el eje como fijo mientras que algún objeto se mueve en el espacio. Podemos pensar también que el objeto permanece inmóvil mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la dirección de la rotación. Fijar el eje y girar el objeto en sentido antihorario es lo mismo que fijar el objeto y mover el eje en sentido horario. La rotación alrededor de los ejes x e y se realiza con una matriz de transformación similar a la anterior.

Para hacer una rotación alrededor del eje y, del tal forma que el eje z se convierte en el x realizamos la siguiente matriz en coordenadas homogéneas

Y finalmente multiplicamos las matrices de rotación por el punto:

ÁLVARO GÓMEZ RAMOS.

- 95 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ya tenemos rotado el punto, pero debemos deshacer la traslación:

4.12 Rotar punto respecto a ejes dado por dos puntos Se resuelve mediante la función:

Lo primero que debemos hacer es escoger una representación adecuada de la recta que será el eje de rotación. Un punto de dicha recta, así como la dirección de la misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos ofrece la información necesaria para la traslación del origen, y la dirección nos informa de los ángulos de rotación correctos para alinear la recta con el eje z. Dada la recta

y su dirección está especificada por el vector

un punto de esta recta es . Para obtener los puntos

realizamos la siguiente operación:

El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente forma: el segmento de recta comprendido entre el origen y está en la dirección del eje arbitrario de rotación, y todo el segmento está situado sobre dicho eje. Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende desde

hasta

ÁLVARO GÓMEZ RAMOS.

.

- 96 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Si ahora realizamos un giro alrededor del eje x hasta que nuestro eje arbitrario quede en el plano xz, el segmento proyectado quedará alineado con el eje z. Cuanto vale el ángulo q ? Sabemos que la longitud del segmento proyectado es

y, teniendo en cuenta la figura anterior, es fácil deducir las razones trigonométricas del ángulo q

Por lo tanto, la matriz de transformación para el giro alrededor del eje x será

ÁLVARO GÓMEZ RAMOS.

- 97 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

La transformación inversa es una rotación de igual magnitud pero en sentido contrario. Como sabemos, cambiar el signo del ángulo significa cambiar el signo de los senos y conservar el de los cosenos.

La rotación alrededor de este eje no ha modificado la coordenada x. Sabemos también que la longitud total del segmento

no varía. La coordenada z será entonces

Necesitamos realizar ahora un giro de ángulo f alrededor del eje y para que el segmento quede alineado con el eje z.

Sus razones trigonométricas son

ÁLVARO GÓMEZ RAMOS.

- 98 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB La matriz de rotación en este caso será

La inversa de esta transformación será

Es posible ahora realizar el giro j alrededor del eje escogido. Ya que este eje está alineado con el eje z, la matriz correspondiente a esta rotación será:

De esta forma ya somos capaces de realizar una rotación j alrededor de un eje arbitrario, mediante el producto de las transformaciones anteriores:

4.13 Calcular robot Una vez dibujado el robot completamente necesitamos finalizarlo y calcular sus ejes. Todo esto se calculará a través de la opción “Calcula robot”, que llama a la función:

El primer paso es añadir un falso eslabón al final del último elemento, para obtener después un eje final:

Ya tenemos la matriz MR que contiene los datos de los elementos añadidos al robot. Dicha matriz es simbólica porque depende de los variable de longitud de los

ÁLVARO GÓMEZ RAMOS.

- 99 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB elementos de traslación, sin embargo, las variables de rotación no se ven reflejadas en esta matriz, teniendo que rotar externamente la matriz para posicionarla en su posición actual. Para ello utilizamos la función “rot” haciéndolo rotar alrededor del eje de rotación:

Una vez que tenemos la matriz MR ya con las variables de traslación y rotación, pasamos a ejecutar la función “MRtoMRvec”, que es una función que transforma la matriz con las coordenadas de los eslabones en una matriz con los ejes de las articulaciones de rotación y traslación y un punto de estos:

Ya tenemos el eje de las articulaciones y un punto, ahora vamos a buscar el “vector z” y “vector x” de la articulación y el punto donde se situará el eje, esto lo realizamos a través de la función “MRvectoMRperp”, que usando MRvec nos devuelve la matriz MRperp con los datos obtenidos:

Sólo nos falta obtener el “vector y” y dibujar los ejes, lo cual hacemos a través de la función “Plotejes”:

4.14 Obtención de los ejes de las articulaciones Este paso lo realizamos a través de la función “MRtoMRvec” que recorre la matriz MR en busca de articulaciones es decir, elementos de traslación o rotación:

ÁLVARO GÓMEZ RAMOS.

- 100 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Luego calcula su eje y un punto de este, si es de rotación resta el punto final del inicial y obtenemos el eje y cogemos el punto medio del inicial y el final como punto del eje:

Si es de traslación restamos el punto inicial al final y obtenemos el eje, y elegimos el punto final como punto del eje:

En el último elemento de MR que es el elemento ficticio añadido también calcula un eje según el punto inicial y final del elemento anterior de MR:

Al final se le añade a cada fila de MRvec un número identificando si es rotación (1), traslación (2) o es el eje final (0).

4.15 Obtención de “eje x”, “eje z” y punto de origen Para ello usamos la función “MRvectoMRperp” que transforma la matriz con los ejes principales (MRvec) de las articulaciones en una matriz que contiene la perpendicular a cada eje (MRperp). Para calcular el “eje x” nos basamos en Denavit-Hartenberg, que dice que se busque la recta perpendicular común a un eje y el siguiente, para ellos debemos averiguar primero si los ejes que entran en juego son coincidentes, paralelos, se cruzan o se cortan. La obtención de la posición relativa de dos rectas la realizamos mediante cálculo matemático. Los vectores directores y los puntos que utilizamos para escribir las ecuaciones de las rectas nos permiten calcular la posición relativa. Consideremos las matrices A y B formadas con estos elementos:

ÁLVARO GÓMEZ RAMOS.

- 101 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

cuando el rango de la matriz A es uno, es decir los vectores directores son iguales o proporcionales, las rectas son paralelas o coincidentes (la misma); serán coincidentes cuando el rango de B también sea 1 y paralelas cuando el rango de B sea 2. Cuando el rango de A es dos y el rango de B también dos: las rectas se cortan. El sistema que resulta al igualar sus ecuaciones paramétricas es compatible determinado. Cuando el rango de A es dos y el de B tres: las rectas se cruzan. El sistema es incompatible. El punto “P” será el punto del eje que guardamos en la matriz MRvec:

El punto “Q” será el punto del eje siguiente guardado en la matriz MRvec:

El vector “V” será el vector que guardamos en la matriz MRvec:

El vector “W” será el vector del eje siguiente guardado en la matriz MRvec:

Ahora creamos las matrices A y B que compararemos sus rangos para saber la posición relativa de los ejes intervinientes:

ÁLVARO GÓMEZ RAMOS.

- 102 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Comparamos el rango, para ello usamos el comando de matlab “rank” que nos devuelve el rango de una matriz, y comparamos ambos rangos:

Si las rectas se cruzan el vector z será el vector del eje que contiene VRvec, el punto de origen será el punto del eje actual que contiene MRvec, y el vector x será el producto vectorial de los vectores del eje actual y el siguiente:

Si las rectas se cortan el vector z será el vector del eje que contiene VRvec, el punto de origen es el punto de intersección de los dos ejes, que calculamos teniendo en cuenta las siguientes ecuaciones:

Tenemos todos los datos necesarios excepto λ que hallamos resolviendo el sistema de ecuaciones mediante la regla de Cramer. Un sistema de ecuaciones lineales se dice de Cramer cuando cumple las siguientes condiciones: -

Es un sistema cuadrado, con igual número de ecuaciones que de incógnitas. El determinante de la matriz de los coeficientes asociada es distinto de cero.

En consecuencia, un sistema de Cramer es siempre compatible determinado (tiene una solución única).

ÁLVARO GÓMEZ RAMOS.

- 103 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Para calcular la incógnita xi del sistema de ecuaciones lineales, se sustituye la columna i de la matriz de coeficientes por los términos independientes, se obtiene el determinante de la matriz resultante y se divide este valor por el del determinante de la matriz de los coeficientes. Por tanto, la solución de un sistema de Cramer se obtiene hallando cada incógnita Xi según la fórmula:

Siendo Ci la matriz resultante de sustituir la columna de la matriz de los coeficientes correspondiente a la incógnita por la de los términos independientes.

Una vez que tenemos λ hallamos el punto de corte de la siguiente manera:

Para ello empleamos el siguiente código:

Sólo nos falta el vector x, que será la perpendicular común a los ejes implicados. Si las rectas son paralelas debemos determinar el vector perpendicular a ambas rectas, hacemos que la recta pase por el punto P del primer eje, pero no tiene porque pasar por el punto Q, así que buscamos el punto Q’ del segundo eje por donde si pase:

Ya podemos obtener la perpendicular común:

ÁLVARO GÓMEZ RAMOS.

- 104 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

El último caso posible es que las rectas sean coincidentes, lo cual resolvemos dependiendo de varios casos concretos, es decir, pueden existir combinaciones no contempladas que no se resuelvan correctamente. Lo primero es ver si los ejes de la articulación actual y de la anterior coinciden o son paralelas y no es la primera articulación:

En este caso elegimos los ejes como los de la articulación anterior:

En el caso que no sea la primera articulación y no coincida con los ejes de la anterior:

Suponemos en este caso que sólo se contempla que el eje perpendicular al anterior, con lo cual el nuevo eje x será el eje z anterior:

Si la articulación es la primera de todas y vertical suponemos dos casos posibles, que sea eje de rotación, entonces el eje x estará en el plano perpendicular al eje de rotación:

Si la articulación es de traslación, el eje x será siempre (1 0 0):

Todos los demás casos no contemplados no podrán ser diseñados. Una vez calculados todos los ejes x y el punto donde situarlo, y teniendo previamente los ejes z de todas las articulaciones, sólo nos queda calcularlos de la última articulación ficticia. El eje x será la perpendicular común al eje anterior y el actual, situándolo en el último punto del robot:

ÁLVARO GÓMEZ RAMOS.

- 105 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.16 Obtención de “eje y”, normalización y dibujado de ejes Se realiza a través de la función “plotejes” que a partir de la matriz MRperp crea la matriz MRejes que contiene los ejes “x”, “y”, “z”, el punto donde se sitúan y el tipo de articulación.

El primer paso es crear unos ejes iniciales que serán siempre iguales, y estarán situados en (0, 0, 0):

Luego dibujamos dichos ejes iniciales:

Ahora, recorriendo la matriz entera, se calculan los ejes normalizados, es decir, ejes unitarios, para ello calculamos la longitud del vector que representa al eje y dividimos por sus coordenadas, lo mismo hacemos con el eje x, y luego calculamos el eje y como el producto vectorial de los ejes “z” y “x”:

Ahora para dibujar los ejes, primero convertimos la matriz simbólica en matriz numérica con los valores correspondientes a la posición actual:

ÁLVARO GÓMEZ RAMOS.

- 106 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Y al final se dibujan los ejes:

4.17 Dibujar robot Por diversas razones podemos necesitar dibujar el robot de nuevo, ya sea porque lo posicionamos en otra posición, o porque en un momento elegimos que no este visible y luego queremos que este visible de nuevo. Esta tarea se realiza a través de la función “dibubot” que a partir de la matriz MR nos dibuja el robot en la posición actual:

El primer paso es convertir la matriz MR simbólica en una matriz numérica, sustituyendo las variables por sus valores iniciales:

Luego recorremos la matriz MR, dibujando uno a uno los elementos dependiendo del tipo de elemento que sea. Para dibujar los elementos se usa una función diferente a la que los dibuja cuando son añadidos, porque ahora ya se dibujan simplemente conociendo el punto inicial y final del elemento:

ÁLVARO GÓMEZ RAMOS.

- 107 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.18 Dibujar elemento eslabón Para dibujar el elemento en cuestión utilizamos la función “eslabon2” que a partir del punto inicial y final lo dibuja:

Lo primero que necesitamos es la longitud del elemento, que calculamos viendo la distancia entre el punto inicial y el final:

Luego calculamos el ángulo x, que podrá ser positivo o negativo, para determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:

Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de esta sobre los ejes “x” (lx) e “y” (ly)

lx ly

ÁLVARO GÓMEZ RAMOS.

l1

- 108 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora tenemos los mismos datos que se introducen al parametrizar el elemento y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8

Pto. final

X5

X7 X6 X4

Pto. inicial

X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.5, y-0.5, z) (x2,y2,z2) = (x+0.5, y-0.5, z) (x3,y3,z3) = (x+0.5, y+0.5, z) (x4,y4,z4) = (x-0.5, y+0.5, z) (x5,y5,z5) = (x-0.5, y-0.5, z+l) (x6,y6,z6) = (x+0.5, y-0.5, z+l) (x7,y7,z7) = (x+0.5, y+0.5, z+l) (x8,y8,z8) = (x-0.5, y+0.5, z+l)

ÁLVARO GÓMEZ RAMOS.

- 109 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento:

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

ÁLVARO GÓMEZ RAMOS.

- 110 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.19 Dibujar elemento traslación Para dibujar el elemento en cuestión utilizamos la función “traslacion2” que a partir del punto inicial y final lo dibuja, los pasos son los mismos que para el elemento eslabón:

Lo primero que necesitamos es la longitud del elemento, que calculamos viendo la distancia entre el punto inicial y el final:

Luego calculamos el ángulo x, que podrá ser positivo o negativo, para determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:

ÁLVARO GÓMEZ RAMOS.

- 111 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de esta sobre los ejes “x” (lx) e “y” (ly)

lx ly

l1

Ahora tenemos los mismos datos que se introducen al parametrizar el elemento y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base.

ÁLVARO GÓMEZ RAMOS.

- 112 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB X8

Pto. final

X5

X7 X6 X4

Pto. inicial

X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.3, y-0.3, z) (x2,y2,z2) = (x+0.3, y-0.3, z) (x3,y3,z3) = (x+0.3, y+0.3, z) (x4,y4,z4) = (x-0.3, y+0.3, z) (x5,y5,z5) = (x-0.3, y-0.3, z+l) (x6,y6,z6) = (x+0.3, y-0.3, z+l) (x7,y7,z7) = (x+0.3, y+0.3, z+l) (x8,y8,z8) = (x-0.3, y+0.3, z+l) Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento:

ÁLVARO GÓMEZ RAMOS.

- 113 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.20 Dibujar elemento rotación Para dibujar el elemento en cuestión utilizamos la función “eje2” que a partir del punto inicial y final lo dibuja:

Lo primero que necesitamos es saber donde se situará el centro del elemento rotación, que será en el punto medio del punto inicial y final:

Lo siguiente que necesitamos es la longitud del elemento, que calculamos viendo la distancia entre el punto inicial y el final: ÁLVARO GÓMEZ RAMOS.

- 114 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Luego calculamos el ángulo x, que podrá ser positivo o negativo, para determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:

Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de esta sobre los ejes “x” (lx) e “y” (ly)

lx ly

ÁLVARO GÓMEZ RAMOS.

l1

- 115 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora tenemos los mismos datos que se introducen al parametrizar el elemento y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. X8 X5

Pto. final

X7 X6

Pto. inicial

X4 X1

X3 X2

La situación de los 8 vértices dependerán del punto inicial donde dibujar el elemento, suponiendo a éste vertical, así quedaría: (x1,y1,z1) = (x-0.4, y-0.4, z) (x2,y2,z2) = (x+0.4, y-0.4, z) (x3,y3,z3) = (x+0.4, y+0.4, z) (x4,y4,z4) = (x-0.4, y+0.4, z) (x5,y5,z5) = (x-0.4, y-0.4, z+1) (x6,y6,z6) = (x+0.4, y-0.4, z+1) (x7,y7,z7) = (x+0.4, y+0.4, z+1) (x8,y8,z8) = (x-0.4, y+0.4, z+1)

ÁLVARO GÓMEZ RAMOS.

- 116 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Ya tenemos identificado todos los vértices, pero en situación vertical con la longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:

“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del elemento, éste rotará hasta posicionarse en su posición final.

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial del siguiente elemento:

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha:

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch.

ÁLVARO GÓMEZ RAMOS.

- 117 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.21 Dibujar elemento base Para dibujar el elemento en cuestión utilizamos la función “base2” que a partir del punto inicial y final lo dibuja:

Lo primero que necesitamos es la longitud del elemento, que calculamos viendo la distancia entre el punto inicial y el final:

Ahora tenemos los mismos datos que se introducen al parametrizar el elemento y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial, situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada) en vertical:

Con esto tenemos el elemento situado en vertical y formado por una recta, pero necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada base. Pto. final

X8 X5 X4

Pto. inicial

X1

X7

X6 X3 X2

Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal, para ello usamos la función “rot” que es una función para resolver el giro de un punto (X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo de rotación igual a tetha: :

ÁLVARO GÓMEZ RAMOS.

- 118 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Y una vez que tenemos parametrizado el elemento en su posición inicial, lo dibujamos con el comando patch:

4.22 Salvar robot Una vez calculado el robot tenemos la posibilidad de poder salvarlo para recuperarlo más adelante y seguir trabajando con él sin necesidad de volver a diseñarlo. Para ello usamos la función “savebot” :

Usamos la función “uiputfile” para obtener un lugar donde guardar el archivo el nombre con el que lo haremos. El nombre se guardará en la variable “fname”:

Una vez indicados donde y con que nombre guardar los datos, procedemos a salvarlos con la función “save” a la que le indicamos el nombre del archivo a guardar y las variables a guardar en el archivo que se creará:

4.23 Cargar robot Si disponemos de un robot previamente salvado y necesitamos seguir interactuando con él, podremos hacerlo cargándolo al espacio de trabajo.. Para ello usamos la función “loadbot” :

ÁLVARO GÓMEZ RAMOS.

- 119 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Usamos la función “uiputfile” para obtener un lugar desde donde recuperar el archivo.: Una vez indicados donde y con que nombre se guardaron previamente los datos del robot, procedemos a cargarlos con la función “load” a la que le indicamos el nombre del archivo a recuperar:

Luego dibujaremos el robot con la función “dibubot” y sus ejes con la función “plotejes”:

4.24 Calculo de parámetros DH Para calcular los parámetros de Denavit-Hartenberg utilizaremos la función “Par_DH” creada para la ocasión:

Lo primero que debemos hacer es calcular los parámetros DH, cuyo cálculo lo resolveremos en funciones independientes:

Ya tenemos calculados los parámetros, ahora los mostraremos en una nueva pantalla que debemos crear:

ÁLVARO GÓMEZ RAMOS.

- 120 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Los datos de DH los representamos en forma matricial, teniendo la matriz resultante cuatro columnas correspondientes a los parámetros (a, d, α, θ) y un número de filas igual al número de articulaciones+1. Para la representación gráfica dividimos la pantalla en cuatro partes verticalmente y en nº articulaciones+1 en horizontal. Para saber el número de las articulaciones creamos la variable “tam”:

Ahora muestro los datos de la siguiente manera, los parámetros “d” los represento en la primera columna, es decir, dividiendo la pantalla en cuatro divisiones verticales y sabiendo que la longitud total es 1, “d” se representará en la división que va de 0 a 0’25. Primero represento el tipo de dato y luego sus valores, por lo que las divisiones en horizontal son el número de elementos más el nombre del tipo de dato:

Para el resto de datos seguimos el mismo método, pero colocándolos donde corresponde, es decir incrementando en una las divisiones verticales y con el mismo procedimiento en horizontal: a … … … …

ÁLVARO GÓMEZ RAMOS.

d … … … …

α … … … …

θ … … … …

- 121 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

ÁLVARO GÓMEZ RAMOS.

- 122 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ya hemos representado todos los valores, ahora los almacenamos en una matriz llamada “P_D_H”:

4.25 Calculo de parámetros di di es la distancia de xi-1 a xi medida a lo largo de zi

ÁLVARO GÓMEZ RAMOS.

- 123 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Sea una recta r determinada por un punto P y un vector exterior a la recta.

, y un punto Q

La distancia entre el punto y la recta es:

La distancia se calculará como la distancia de un punto a una recta, siendo “P” el vector xi. Con lo cual el el punto de los ejes i, “Q” el punto de los ejes i-1 y cálculo se resolverá:

4.26 Calculo de parámetros ai ai: Es la distancia de zi a zi+1 medida a lo largo de xi

ÁLVARO GÓMEZ RAMOS.

- 124 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Sea una recta r determinada por un punto P y un vector exterior a la recta.

, y un punto Q

La distancia entre el punto y la recta es:

La distancia se calculará como la distancia de un punto a una recta, siendo “P” el vector zi. Con lo cual el el punto de los ejes i, “Q” el punto de los ejes i-1 y cálculo se resolverá:

ÁLVARO GÓMEZ RAMOS.

- 125 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.27 Calculo de parámetros θ i θi: Es el ángulo de xi-1 a xi medida sobre zi (utilizando la regla de la mano derecha).

Es el ángulo que forman dos rectas es el ángulo que forman sus vectores directores:

Cuando la articulación es de rotación, indicamos que su valor sea el nombre de la variable, y si no es una articulación de rotación lo calculamos según la fórmula anterior teniendo en cuenta que V1 es xi-1 y que W1 es xi:

4.28 Calculo de parámetros α i αi: Es el ángulo de zi a zi+1 medida sobre xi (utilizando la regla de la mano derecha).

ÁLVARO GÓMEZ RAMOS.

- 126 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Es el ángulo que forman dos rectas es el ángulo que forman sus vectores directores:

Calculamos según la fórmula anterior teniendo en cuenta que V1 es xi y que W1 es xi+1:

4.28 Cinemática directa La cinemática directa consiste en resolver la posición cartesiana y la orientación del efector final dado el valor de las articulaciones y conociendo la estructura del robot a través de la matriz de Denavit-Hartenberg. Necesitamos obtener el valor del punto final del robot en función de sus variables articulares, lo cual obtenemos directamente de la matriz “MR” obteniendo el punto final del robot, ya que siempre expresamos los puntos en función de las variables. Esto lo hacemos a través de la función “cin_dir” creando una ventana nueva y mostramos la “x”, “y” y “z” desde MR(n,[5 6 7]) respectivamente y siendo “n” el número de elementos de “MR”

ÁLVARO GÓMEZ RAMOS.

- 127 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

ÁLVARO GÓMEZ RAMOS.

- 128 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.29 Cinemática inversa El objetivo del problema cinemático inverso consiste en encontrar los valores que deben tomar las variables articulares del robot para que su extremo se posicione y oriente según una determinada localización espacial. Para resolver el problema cinemática inverso utilizamos la función “cin_inv”. Lo primero a realizar es calcular el número de articulaciones, el tipo y el orden de ellas, siendo “a” el tipo de variables, y “Var” los nombres:

ÁLVARO GÓMEZ RAMOS.

- 129 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Luego debemos pasar a numérica la matriz simbólica con los parámetros DH:

Creo la matriz del robot para usarla en el toolbox de robótica:

Creo una matriz “DH” que contenga a la matriz “P_D_H” más el tipo de elemento:

Creamos la ventana para introducir datos:

ÁLVARO GÓMEZ RAMOS.

- 130 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

ÁLVARO GÓMEZ RAMOS.

- 131 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Creamos el robot según toolbox de robótica:

Ahora creamos una nueva función a la que se llamará cuando se pulse el botón calcular, y que nos realizará el cálculo de la cinemática inversa. El cálculo lo realizamos a través de la función ikine. IKINE calcula la cinemática inversa del manipulador Q = IKINE(DH, STOL, ILIMIT, T) Q = IKINE(DH, STOL, ILIMIT, T, Q0) Q = IKINE(DH, STOL, ILIMIT, T, Q0, M) Devuelve los valores de las variables articulares necesarios para que el efector final del manipulador tenga la posición y orientación dadas por la transformación T. La solución del problema cinemático inverso no es única en general, y es posible que para una misma orientación y posición deseadas, se obtengan soluciones distintas en función del vector inicial de variables articulares (Q0) que se le pase a IKINE. ÁLVARO GÓMEZ RAMOS.

- 132 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Es posible usar la función para que devuelva las variables articulares correspondientes a una sola posición y orientación, o bien a una trayectoria de posiciones y orientaciones. Eso dependerá del formato del parámetro T: -

Si T es una transformación homogénea, entonces IKINE devuelve un vector fila(Q) con las variables articulares correspondientes a la posición y orientación indicadas en la matriz T.

-

Si T es una trayectoria de transformaciones homogéneas, entonces el resultado será una matriz (Q), en la que la fila i-ésima contendrá las variables articulares correspondientes a la transformación T (:, :, i ). La estimación inicial para Q en cada paso se toma de la solución obtenida en el paso anterior.

Sea cual sea el formato de T, la estimación inicial para el vector de variables articulares será la dada en el parámetro Q0 (puede ser una columna o una fila), y en el caso de que no se lo demos, asume que es el vector nulo. Para el caso de un manipulador con menos de 6 grados de libertad el efector final no podrá alcanzar algunas posiciones y orientaciones. Esto normalmente lleva a una no convergencia de IKINE. Una solución consiste en especificar un vector (fila o columna) de pesos (M), cuyos elementos serán 0 para aquellos grados de libertad que en cartesianas estén restringidos, y 1 en otro caso. Los elementos de M se corresponden con las traslaciones a lo largo de los ejes X, Y y Z, y con las rotaciones entorno a los ejes X, Y y Z. Por ejemplo si el manipulador no se puede desplazar a lo largo del eje Z, ni rotar entorno a los ejes X e Y, M deberá ser el vector [1 1 0 0 0 1]. El número de elementos no nulos debe ser igual al número de grados de libertad del robot. ILIMIT es el número máximo de iteraciones que se ejecutarán en busca de una solución (un valor usual es 1000). STOL será la máxima diferencia que se admitirá entre la transformación correspondiente a las variables articulares solución y la transformación con la posición y orientación especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo uso de la función TR2DIFF. En nuestro caso usaremos la fórmula Q = IKINE(DH, STOL, ILIMIT, T, Q0, M) con: -

DH: El creado anteriormente. STOL: 1e-3. ILIMT: 1000. T: [1 0 0 str2num(get(xedit,'String'));0 1 0 str2num(get(yedit,'String'));0 0 1 str2num(get(zedit,'String'));0 0 0 1]. Q0: matriz de ceros del tamaño adecuado. M: [1 1 1 1 1 1].

ÁLVARO GÓMEZ RAMOS.

- 133 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ya tenemos los valores requeridos en “q” y procedemos a dibujar el robot en un nuevo “figure” en la posición resultante:

4.30 Posicionar robot Para posicionar el robot en la posición deseada usamos la función “env_bot” la cual realiza los cálculos necesarios:

Lo primero a realizar es calcular el número de articulaciones, el tipo y el orden de ellas. Para ello recorremos la matriz “MR” y cuando vemos una articulación la contabilizamos y guardamos su nombre:

Ahora sustituimos los nombres de las variables por el valor cero en la matriz “P_D_H” para tenerla en el mismo formato que usa el toolbox de robótica:

ÁLVARO GÓMEZ RAMOS.

- 134 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora creamos una nueva ventana para introducir los valores articulares con los que deseamos posicionar el robot, y también daremos la opción de elegir si lo queremos representar de forma esquemática solamente o también de forma tridimensional:

ÁLVARO GÓMEZ RAMOS.

- 135 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

Ahora creo una matriz del robot típica del toolbox:

Y ahora creo el robot en el formato del toolbox:

Ya tenemos la ventana en la que podemos pulsar el botón para posicionar el robot, lo cual ejecutará la función “intdata”. Lo primero a realizar en “intdata” es obtener los valores introducidos por el usuario para posicionar el robot:

Ahora dibujamos el robot de forma esquemática en una nueva ventana:

ÁLVARO GÓMEZ RAMOS.

- 136 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB Lo siguiente es comprobar si se quiere dibujar el robot en 3D, para ello comprobamos el valor de la variable “checkdata” que puede variar su valor a través de la función “check”:

Si la opción está activada, es decir, “checkdata=1”, cambiamos el valor de las variables al introducido por el usuario:

Preparamos el plot principal del programa para volver a dibujar el robot en la posición deseada:

Ejecutamos la función “dibubot” que nos dibujará el robot en la posición final en la que queríamos representarlo:

ÁLVARO GÓMEZ RAMOS.

- 137 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

4.31 Ayuda Para llamar a la ayuda se hace a través de la función “ayuda”, que abre una página web (previamente creada con toda la ayuda) en un explorador propio de matlab:

4.32 Función auxiliar: Multiplicar vectorialmente vectores simbólicos Para poder multiplicar vectores simbólicos hemos creado nuestra propia función ya que desconocemos la existencia de una función para dicho fin:

4.33 Función auxiliar: Redondear a cero los valores de las matrices Se ha creado una función que me recorre las matrices y cuando ve un valor menor que 0.0001 en alguno de sus términos, lo iguala a cero:

ÁLVARO GÓMEZ RAMOS.

- 138 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

BLOQUE 3: CONCLUSIONES

ÁLVARO GÓMEZ RAMOS.

- 139 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1

Conclusiones.

El proyecto nació con varios objetivos que según se fue desarrollando fueron variando. Se quería diseñar un software interactivo en el que se pudieran diseñar robots en tres dimensiones para luego poder obtener los ejes del robot y los parámetros de Denavit-Hartenberg. Esta sería la parte novedosa del software. Luego existiría otra parte del software que consistiese en la integración de herramientas ya existentes, para conseguir un paquete que incluya todas las herramientas que podamos considerar interesantes para el trabajo con robots. En esta sección se incluirían la resolución del problema cinemática directa e inversa, así como la dinámica directa e inversa. También se comentó la posibilidad de desarrollar ciertos bloques de simulink para la simulación, incluso la posibilidad de que alguno de estos bloques fuera desarrollado en el lenguaje de programación “C” para que tuviese una ejecución más rápida. La mayor parte del peso del proyecto se basa en la parte gráfica del software, sobre todo en lo que se trata de diseño en tres dimensiones. Otra de las cuestiones a las que más tiempo se ha dedicado es a la matemática realizada con simbólico. Es decir, lo difícil fue dibujar el robot según lo indique el usuario y poder posicionarlo o moverlo porque ello implica una matemática compleja de matrices simbólicas. En cuanto a la parte puramente relacionada con la robótica como puede ser la cinemática directa e inversa fue relativamente rápida su integración, porque ya teníamos unas matrices y variables previamente calculadas que nos facilitan el trabajo. El proyecto no tenía un fin claro, pues no se conocían con suficiente exactitud las dificultades que irían surgiendo, por lo que una vez desarrollado todo el diseño y cálculo de ejes y parámetros de Denavit-Hartenberg se optó por añadirle también la cinemática y dejar la dinámica y otras cuestiones para futuras ampliaciones. Es decir, se trata de un proyecto abierto con muchas posibilidades de ampliación, en el cual se han dejado sentadas unas bases más que suficientes para poder ser de cierta utilidad en esta su primera versión.

ÁLVARO GÓMEZ RAMOS.

- 140 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

2

Desarrollos futuros.

Como hemos dicho este es un proyecto abierto que podrá ser ampliado y modificado en un futuro. Lo primero a ampliar sería añadir la dinámica directa e inversa y la posibilidad de añadir bloques propios a simulink para poder simular el robot. Pero se pueden hacer muchas más cosas, como la posibilidad de mover el robot siguiendo trayectorias, o la inclusión de un segundo robot con el que interactuar, o incluso el diseño de elementos del entorno de trabajo. Quizás para este hecho sea mejor realizar un cambio en la plataforma de desarrollo y pasar de matlab a algún lenguaje más eficiente, sobre todo en la parte gráfica. Las posibilidades de ampliación son inmensas, y hacen de este software una herramienta muy interesante para el trabajo con robots. Se podría llegar a obtener un paquete de herramientas muy completo e integrado, consiguiendo un tipo de software poco desarrollado hasta el día de hoy. Sobre todo Sicon-Bot se pretende centrar en la ayuda al estudio de la robótica, es decir, una herramienta para el estudiante, pero con futuras ampliaciones podría llegar a ser de cierta utilidad profesional, sobre todo si se consigue compatibilizar con otras herramientas ya existentes.

ÁLVARO GÓMEZ RAMOS.

- 141 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

BLOQUE 4: ANEXOS

ÁLVARO GÓMEZ RAMOS.

- 142 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB

1

ANEXO: CÓDIGO FUENTE.

function ayuda %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función de ayuda %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% web index.htm

ÁLVARO GÓMEZ RAMOS.

- 143 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una base, situado entre un punto inicial y % otro final. La base dibujada llevará % el ángulo de giro sobre el eje punto inicial-final indicados por el usuario: % [xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global sp1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos datos de altura de la base %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi,ange); z2]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi,ange); z3]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi,ange); z4]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi,ange); z5]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi+l,ange); z6]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi+l,ange); z7]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi+l,ange); z8]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi+l,ange);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1)

ÁLVARO GÓMEZ RAMOS.

- 144 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 145 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=base(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una base, situado en un punto inicial y % que nos devuelve un punto final. La base dibujada llevará % los ángulos de inclinación indicados por el usuario: % [xf yf zf]=base(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos valor de altura de la base %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% l=str2num(get(htexto,'String')); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cogemos los valores de los ángulos y los pasamos a radianes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% angx=-str2num(get(htextox,'String'))*2*pi/360; angz=-str2num(get(htextoz,'String'))*2*pi/360; ange=str2num(get(htextoe,'String'))*2*pi/360; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi,yi,zi,xi-2,yi-2,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+2,yi-2,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+2,yi+2,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-2,yi+2,zi,angx,angz); z5]=rotar(xi,yi,zi,xi-2,yi-2,zi+l,angx,angz); z6]=rotar(xi,yi,zi,xi+2,yi-2,zi+l,angx,angz); z7]=rotar(xi,yi,zi,xi+2,yi+2,zi+l,angx,angz); z8]=rotar(xi,yi,zi,xi-2,yi+2,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);

ÁLVARO GÓMEZ RAMOS.

- 146 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 147 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function ai=calc_ai(MRejes) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % calculamos ai matemáticamente %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% n=size(MRejes,1); for i=1:n-1 P=MRejes(i,[10 11 12]); v=MRejes(i,[1 2 3]); Q=MRejes(i+1,[10 11 12]); w=MRejes(i+1,[1 2 3]); q4=simple(P(1)*v(1)+P(2)*v(2)+P(3)*v(3)); k1=Q(1)-P(1); k2=Q(2)-P(2); k3=Q(3)-P(3); mod_v1=sqrt(v(1)^2+v(2)^2+v(3)^2); A=[v(1)/mod_v1 w(1);v(2)/mod_v1 w(2);v(3)/mod_v1 w(3)]; B=[k1 v(1) w(1);k2 v(2) w(2); k3 v(3) w(3)]; if (rank(A)==1)&(rank(B)==1) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); ai(i)=mod_QP/mod_v; end if (rank(A)==1)&(rank(B)==2) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); ai(i)=mod_QP/mod_v; end if (rank(A)==2)&(rank(B)==2) a=[v(1) w(1) Q(1)-P(1);v(2) w(2) Q(2)-P(2);v(3) w(3) Q(3)P(3)]; b=cross(v,w); mod_QP=0; mod_v=0; deta=det(a); for j=1:3 mod_v=mod_v+b(j)^2; end

ÁLVARO GÓMEZ RAMOS.

- 148 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB mod_v=sqrt(mod_v); ai(i)=deta/mod_v; end if (rank(A)==2)&(rank(B)==3) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); ai(i)=mod_QP/mod_v; end end end

ÁLVARO GÓMEZ RAMOS.

- 149 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function alphai=calc_alphai(MRejes) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % calcula alphai matemáticamente %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% n=size(MRejes,1); clear alphai for i=1:n-1 v=MRejes(i,[1 2 3]); w=MRejes(i+1,[1 2 3]); alphai(i)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2 )^2+v(3)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2))))); x=cross(v,w); if (x(1)~=MRejes(i,4))|(x(2)~=MRejes(i,5))|(x(3)~=MRejes(i,6)) alphai(i)=-alphai(i); end end end

ÁLVARO GÓMEZ RAMOS.

- 150 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function di=calc_di(MRejes) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %Calculo di matemáticamente %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% n=size(MRejes,1); clear di for i=2:n P=MRejes(i-1,[10 11 12]); v=MRejes(i-1,[4 5 6]); Q=MRejes(i,[10 11 12]); w=MRejes(i,[4 5 6]); q4=simple(P(1)*v(1)+P(2)*v(2)+P(3)*v(3)); k1=Q(1)-P(1); k2=Q(2)-P(2); k3=Q(3)-P(3); mod_v1=sqrt(v(1)^2+v(2)^2+v(3)^2); A=[v(1)/mod_v1 w(1);v(2)/mod_v1 w(2);v(3)/mod_v1 w(3)]; B=[k1 v(1) w(1);k2 v(2) w(2); k3 v(3) w(3)]; if (rank(A)==1)&(rank(B)==1) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); di(i-1)=mod_QP/mod_v; end if (rank(A)==1)&(rank(B)==2) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); di(i-1)=mod_QP/mod_v; end if (rank(A)==2)&(rank(B)==2) a=[v(1) w(1) Q(1)-P(1);v(2) P(3)]; b=cross(v,w); mod_QP=0; mod_v=0; deta=det(a); for j=1:3 mod_v=mod_v+b(j)^2;

ÁLVARO GÓMEZ RAMOS.

%coincidentes

%paralelos

%se cortan w(2) Q(2)-P(2);v(3) w(3) Q(3)-

- 151 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB end mod_v=sqrt(mod_v); di(i-1)=deta/mod_v; end if (rank(A)==2)&(rank(B)==3) a=cross((Q-P),v); mod_QP=0; mod_v=0; for j=1:3 mod_QP=mod_QP+a(j)^2; end for j=1:3 mod_v=mod_v+v(j)^2; end mod_QP=sqrt(mod_QP); mod_v=sqrt(mod_v); di(i-1)=mod_QP/mod_v; end

%se cruzan

end end

ÁLVARO GÓMEZ RAMOS.

- 152 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function tethai=calc_tethai(MRejes) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos el valor de tethai %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global q1 q2 q3 t1 Q2_simb Q2_simb2 syms Q2_simb2 syms x positive n=size(MRejes,1); %Nº de ejes del robot clear tethai a=0; for i=2:n if MRejes(i,13)==1 %Si es de rotación hago cambio de variable para que el valor de tethai sea solo el nombre de la variable correspondiente a=a+1; tethai(i-1)= Q2_simb(a); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculo cual sería el valor de tethai sin el cambio de variable, % por si puedo necesitarlo mas adelante, y lo guardo en Q2_simb2(a) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% v=MRejes(i-1,[4 5 6]); w=MRejes(i,[4 5 6]); Q2_simb2(a)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v (2)^2+v(3)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2))))); x=cross(v,w); mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2))); x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x]; if (x(1)~=MRejes(i,1))|(x(2)~=MRejes(i,2))|(x(3)~=MRejes(i,3)) Q2_simb2(a)=2*pi-Q2_simb2(a); end Q2_simb2_aux(a)=subs(Q2_simb2(a),Q2_simb(a),0); Q2_simb2(a)=Q2_simb2(a)-Q2_simb2_aux(a);

else %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Resuelvo la matemática necesaria para calcular tethai %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% v=MRejes(i-1,[4 5 6]); w=MRejes(i,[4 5 6]); x=cross(v,w); tethai(i1)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2)^2+v(3 )^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2))))); mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2)));

ÁLVARO GÓMEZ RAMOS.

- 153 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x]; if (x(1)~=MRejes(i,1))|(x(2)~=MRejes(i,2))|(x(3)~=MRejes(i,3)) tethai(i-1)=-tethai(i-1); end end end end

ÁLVARO GÓMEZ RAMOS.

- 154 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function calcbot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculo los ejes del robot a partir de la matriz MR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MR n Q2_simb MRperp MRvec mfile_save graph_DH graph_calcbot htras heje hrot hgarra hbase MRejes t1 q1 q2 q3 MR_simb graph_cindir graph_cininv MR(n+1,:)= [3 MR(n,[5 6 7]) MR(n,[5 6 7]) 0]; %Añadimos un falso eslabón al final del robot con longitud 0 para que este termine en eslabón n=n+1; %Contamos este nuevo tramo añadido for i=1:n MRaux(i,:)=MR(i,:); %variable aux de MR end clear MR a=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % rotamos de forma simbólica el robot según las variables de rotación %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n if MRaux(i,1)==2 a=a+1; for j=i+1:n [MRaux(j,2) MRaux(j,3) MRaux(j,4)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6) ,MRaux(i,7),MRaux(j,2),MRaux(j,3),MRaux(j,4),Q2_simb(a)); [MRaux(j,5) MRaux(j,6) MRaux(j,7)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6) ,MRaux(i,7),MRaux(j,5),MRaux(j,6),MRaux(j,7),Q2_simb(a)); MRaux=simple(MRaux); end MRaux=simple(MRaux); end end MR=MRaux; MR_simb=MRaux; %Creamos valores simbólicos de rotación MRvec=simple(MRtoMRvec(MRaux)); %Paso de matriz con vector del eje de la articulacion MRperp=simple(MRvectoMRperp(MRvec,MRaux)); %Obtengo vector z + punto de origen + vector x MRejes=simple(plotejes(MRperp)); %Obtengo y los dibujo

MR con los matriz de robot a la matriz con el resto de ejes

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % habilitaciones de nuevas opciones del menu

ÁLVARO GÓMEZ RAMOS.

- 155 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% set(mfile_save,'Enable','on') set(graph_DH,'Enable','on') set(graph_calcbot,'Enable','off') set(htras,'Enable','off'); set(heje,'Enable','off'); set(hrot,'Enable','off'); set(hgarra,'Enable','off'); set(hbase,'Enable','off'); set(graph_cindir,'Enable','on'); end

ÁLVARO GÓMEZ RAMOS.

- 156 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function cin_dir %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Muestra la cinemática directa que se obtiene directamente del punto final % del último elemento del robot, que está almacenado en MR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MR_simb n P_D_H DH robotd2 Q2_simb Q1_simb figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Cinemática Inversa',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[0.15 0.15 0.6 0.6]); uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(MR_simb(n,5)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.4 0.7 .5 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(MR_simb(n,6)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.4 0.5 .5 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(MR_simb(n,7)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.4 0.3 .5 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String','x=',... 'backgroundcolor',[1 1 1],... 'ForegroundColor','b',... 'FontWeight','bold',... 'Units','normalized',...

ÁLVARO GÓMEZ RAMOS.

- 157 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Position',[.2 0.7 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String','y=',... 'backgroundcolor',[1 1 1],... 'ForegroundColor','b',... 'FontWeight','bold',... 'Units','normalized',... 'Position',[.2 0.5 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String','z=',... 'backgroundcolor',[1 1 1],... 'ForegroundColor','b',... 'FontWeight','bold',... 'Units','normalized',... 'Position',[.2 0.3 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel');

end

ÁLVARO GÓMEZ RAMOS.

- 158 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function cin_inv %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que calcula la cinemática inversa del robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2 hdata robotd2 hcheck checkdata DH tras rotac xedit yedit zedit n=size(P_D_H,1); checkdata=0; P_D_Haux=P_D_H; cont1=0; cont2=0;

%Nº de filas %Variable de checkbox

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el nº de articulaciones, el tipo y el orden % de ellas. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=2:size(MRejes,1) if MRejes(i,13)==1 a(i-1)=0; cont1=cont1+1; Var(i-1)=Q2_simb(cont1); end if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; Var(i-1)=Q1_simb(cont2); end end a(cont1+cont2+1)=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nueva ventana para introducir los datos de las variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot P_D_Haux=subs(P_D_Haux,Q1_simb(j),0); end for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Creo la matriz del robot para usarla en el toolbox de robótica %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n siconbot{i}=link([P_D_Haux(i,1) P_D_Haux(i,2) P_D_Haux(i,3) P_D_Haux(i,4) a(i)],'modified'); end

ÁLVARO GÓMEZ RAMOS.

- 159 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cálculo de DH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% DH=P_D_Haux; for i=1:(size(DH,1)) DH(i,5)=a(i); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % ventana para introducir datos %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Cinemática Inversa',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[0.15 0.15 0.6 0.6]); hintdata = uicontrol('style','pushbutton',... 'Callback',@intdata2,... 'backgroundcolor',[.724, .724, .724],... 'String','Calcular',... 'Units','normalized',... 'Position',[.8 .5 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String','x =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.1 0.7 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %introducir x xedit=uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.2 0.7 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel');

ÁLVARO GÓMEZ RAMOS.

- 160 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB uicontrol('Style', 'text', ... 'Callback',@label,... 'String','y =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.1 0.5 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Introducir y yedit=uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.2 0.5 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); uicontrol('Style', 'text', ... 'Callback',@label,... 'String','z =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.1 0.3 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Introducir z zedit=uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.2 0.3 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nos dibuja las variables y su valor y % las coloca en la ventana dependiendo del nº de variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% b=1/(n-1); c=1+b/2; for i=1:n-1 uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(Var(1,i)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',...

ÁLVARO GÓMEZ RAMOS.

- 161 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Position',[.4 c-b .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); hdata(i) = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.6 c-b .1 .05],... 'Enable','off',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); c=c-b; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % creamos el robot según toolbox de robótica %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% robotd2=robot(siconbot,'Robot de siconbot'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = intdata2(varargin) %Calcular cinemática inversa %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que nos calcula la cinemática inversa %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hdata robotd2

DH xedit yedit zedit tras rotac P_D_H

n=size(P_D_H,1); stol=1e-3; ilimit=1000; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Matriz de posición final deseada necesaria para "ikine" %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% TG = [1 0 0 str2num(get(xedit,'String'));0 1 0 str2num(get(yedit,'String'));0 0 1 str2num(get(zedit,'String'));0 0 0 1]; vectorq0=zeros(1,tras+rotac-1); %Posición inicial q=ikine(DH,stol,ilimit,TG,vectorq0,[1 1 1 1 1 1]); %Cálculo de cinemática Inversa %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el robot

ÁLVARO GÓMEZ RAMOS.

- 162 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% figure plot(robotd2,q) axis on AXIS([0 30 0 30 0 40]); camorbit(165,0) grid on

%Posicionar robot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Mostrar valores de variables articulares %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n-1 set(hdata(i),'String',num2str(q(i))); end end function V3=cross_sym(V1,V2) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % multiplica vectorialmente vectores simbólicos %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% V3=[det([V1(2) V1(3);V2(2) V2(3)]) det([V1(1) V1(3);V2(1) V2(3)]) det([V1(1) V1(2);V2(1) V2(2)])];

ÁLVARO GÓMEZ RAMOS.

- 163 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function dibubot(MR) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja el robot equivalente a la matriz MR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global n

sp1 num_simb_rot Q1_simb Q1 Q2_simb num_simb_rot2 Q2

subplot(sp1) MR=simple(MR);

%Seleccionamos subplot principal %Simplificamos MR

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos MR con valores reales %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot MR=subs(MR,Q1_simb(j),Q1(j)); variables end for j=1:num_simb_rot2 MR=subs(MR,Q2_simb(j),-Q2(j)); variables end

%Mr para valores concretos de las

%Mr para valores concretos de las

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibuja el tipo de elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n switch MR(i,1) case 1 traslac2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8)); case 2 eje2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8)); case 3 eslabon2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8)); case 4 base2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8)); case 5 garra2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8)); end end

ÁLVARO GÓMEZ RAMOS.

- 164 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=eje2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una eje de rotación, situado entre un punto inicial y % otro final. La base dibujada llevará % el ángulo de giro sobre el eje punto inicial-final indicados por el usuario: % [xf yf zf]=base(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global sp1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculo pto dnd se centrará el eje %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xi1=(xi+xf)/2; yi1=(yi+yf)/2; zi1=(zi+zf)/2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos la longitud del tramo para obtener el ángulo de giro % sobre el eje x a partir de zf y zi. Dependiendo del signo de % (yf-yi) el giro será en un sentido o en otro. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); % if (xi==xf)&(yi==yf) % angz=0; % angx=0; % else % angx=-(pi/2-asin((zf-zi)/l)); % lx=xf-xi; % ly=yf-yi; % l1=sqrt(lx^2+ly^2); % V=[lx/l1 ly/l1 0]*l; % xf1=V(1); % yf1=V(2); % zf1=V(3); % angz=-(pi/2-acos(xf1/l)); % end if (yf-yi)>0.0001 angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); end;

%ángulo positicvo %ángulo negativo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el giro sobre el eje z a partir del pto inicial y % final.

ÁLVARO GÓMEZ RAMOS.

- 165 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if (xi==xf)&(yi==yf) %Si tramo es vertical angz=0; %Ángulo y=0 else lx=xf-xi; %Longitud en x ly=yf-yi; %Longitud en y l1=sqrt(lx^2+ly^2); %Longitud en plano XY V=[lx/l1 ly/l1 0]*l; %Proyección en eje XY xf1=V(1); yf1=V(2); zf1=V(3); angz=(pi/2-acos(xf1/l)); %ángulo de y end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1-0.5,angx,angz); z2]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1-0.5,angx,angz); z3]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1-0.5,angx,angz); z4]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1-0.5,angx,angz); z5]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1+0.5,angx,angz); z6]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1+0.5,angx,angz); z7]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1+0.5,angx,angz); z8]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1+0.5,angx,angz);

[xf yf zf]=rotar(xi1,yi1,zi1,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); [x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange); end

ÁLVARO GÓMEZ RAMOS.

- 166 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[1 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 167 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=eje(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja un eje de rotación, situado en un punto inicial y % que nos devuelve un punto final. El eje dibujado llevará % los ángulos de inclinación indicados por el usuario: % [xf yf zf]=eje(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 n MR htextot1 num_simb_rot2 Q2_simb Q2 rotac num_simb_rot Q1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos datos de variable %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% num_simb_rot2=num_simb_rot2+1; l=str2num(get(htexto,'String')); a=get(htextot1,'String'); Q2_simb(num_simb_rot2)=a; L=sym(a); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cogemos los valores de los ángulos y los pasamos a radianes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% angx=-str2num(get(htextox,'String'))*2*pi/360; angz=-str2num(get(htextoz,'String'))*2*pi/360; ange=str2num(get(htextoe,'String'))*2*pi/360; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5

y1 y2 y3 y4 y5

z1]=rotar(xi,yi,zi,xi,yi-0.6,zi-0.5,angx,angz); z2]=rotar(xi,yi,zi,xi+0.6,yi,zi-0.5,angx,angz); z3]=rotar(xi,yi,zi,xi,yi+0.6,zi-0.5,angx,angz); z4]=rotar(xi,yi,zi,xi-0.6,yi,zi-0.5,angx,angz); z5]=rotar(xi,yi,zi,xi,yi-0.6,zi+0.5,angx,angz);

ÁLVARO GÓMEZ RAMOS.

- 168 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB [x6 y6 z6]=rotar(xi,yi,zi,xi+0.6,yi,zi+0.5,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi,yi+0.6,zi+0.5,angx,angz); [x8 y8 z8]=rotar(xi,yi,zi,xi-0.6,yi,zi+0.5,angx,angz); [xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x1,y1,z1,ange); z2]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x2,y2,z2,ange); z3]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x3,y3,z3,ange); z4]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x4,y4,z4,ange); z5]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x5,y5,z5,ange); z6]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x6,y6,z6,ange); z7]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x7,y7,z7,ange); z8]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x8,y8,z8,ange);

MR(n,[2 3 4])=[(x1+x3)/2 (y1+y3)/2 (z1+z3)/2]; MR(n,[5 6 7])=[(x5+x7)/2 (y5+y7)/2 (z5+z7)/2]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %sustituimos las variables por su valor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:num_simb_rot x1=subs(x1,Q1(i)); x2=subs(x2,Q1(i)); x3=subs(x3,Q1(i)); x4=subs(x4,Q1(i)); x5=subs(x5,Q1(i)); x6=subs(x6,Q1(i)); x7=subs(x7,Q1(i)); x8=subs(x8,Q1(i)); y1=subs(y1,Q1(i)); y2=subs(y2,Q1(i)); y3=subs(y3,Q1(i)); y4=subs(y4,Q1(i)); y5=subs(y5,Q1(i)); y6=subs(y6,Q1(i)); y7=subs(y7,Q1(i)); y8=subs(y8,Q1(i)); z1=subs(z1,Q1(i)); z2=subs(z2,Q1(i)); z3=subs(z3,Q1(i)); z4=subs(z4,Q1(i)); z5=subs(z5,Q1(i)); z6=subs(z6,Q1(i)); z7=subs(z7,Q1(i)); z8=subs(z8,Q1(i)); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento

ÁLVARO GÓMEZ RAMOS.

- 169 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[1 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 170 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function env_bot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Funcion que nos permite posicionar el robot en la posición % indicada, según el valor de las articulaciones %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2 hdata robotd2 hcheck checkdata DH n=size(P_D_H,1); checkdata=0; P_D_Haux=P_D_H; cont1=0; cont2=0;

%Nº de filas %Variable de checkbox %Contador %Contador

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el nº de articulaciones, el tipo y el orden % de ellas. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=2:size(MRejes,1) if MRejes(i,13)==1 a(i-1)=0; articular cont1=cont1+1; Var(i-1)=Q2_simb(cont1); articulares ordenadas end if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; Var(i-1)=Q1_simb(cont2); end end a(cont1+cont2+1)=0; ficticia al final del robot

%Articulación de traslacion %Vector con tipo de variable

%Vector con variables

%Creamos un articulación

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Damos valor cero a las variables articulares para adaptarlas al % formato del toolbox de robótica %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot P_D_Haux=subs(P_D_Haux,Q1_simb(j),0); end for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nueva ventana para introducir los datos de las variables

ÁLVARO GÓMEZ RAMOS.

- 171 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Introducir datos',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[0.15 0.15 0.6 0.6]); % Botón hintdata = uicontrol('style','pushbutton',... 'Callback',@intdata,... 'backgroundcolor',[.724, .724, .724],... 'String','Intr. Dato',... 'Units','normalized',... 'Position',[.6 .6 .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Checkbox hcheck = uicontrol('style','checkbox',... 'Callback',@check,... 'backgroundcolor',[1 1 1],... 'String','Ver Robot en 3D',... 'Units','normalized',... 'Position',[.6 .4 .15 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nos dibuja las variables para que le podamos asignar un valor y % las coloca en la ventana dependiendo del nº de variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% b=1/(n-1); %Variable para colocar la ventana c=1+b/2; %Variable con valor inicial para colocar la ventana for i=1:n-1 uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(Var(1,i)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[.2 c-b .1 .05],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel');

ÁLVARO GÓMEZ RAMOS.

- 172 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB hdata(i) = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.4 c-b .1 .05],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); c=c-b; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Creo la matriz del robot para usarla en el toolbox de robótica %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n siconbot{i}=link([P_D_Haux(i,1) P_D_Haux(i,2) P_D_Haux(i,3) P_D_Haux(i,4) a(i)],'modified'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % creamos el robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% robotd2=robot(siconbot,'Robot de siconbot'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = intdata(varargin) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que nos dibuja el robot según los datos introducidos y las % opciones elegidas %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hdata robotd2 MRejes Q2 Q1 MR sp1 hf1 n Q2_simb Q1_simb Q2_simb2 checkdata MR_simb

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos los valores de las articulaciones %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:size(hdata,2) vector(i)=str2num(get(hdata(i),'String')); end vector(i+1)=0; %Ultima articulación(Ficticia)=0 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 173 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB % Dibujamos el robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% figure plot(robotd2,vector) %dibuja el robot axis on AXIS([0 30 0 30 0 40]); camorbit(165,0) grid on if checkdata==1 % si queremos dibujarlo en 3D cont2=0; cont1=0; for i=1:size(hdata,2) if MRejes(i+1,13)==1 Q2(cont2+1)=str2num(get(hdata(i),'String')); %valor de variables cont2=cont2+1; end if MRejes(i+1,13)==2 Q1(cont1+1)=str2num(get(hdata(i),'String')); %Valor de variables cont1=cont1+1; end end figure(hf1) %Seleccionamos figure principal de siconbot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Preparamos el subplot principal %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% delete (sp1) sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]); xlabel('eje x'); ylabel('eje y'); axis on AXIS([-30 30 -30 30 0 40]); camorbit(165,0) grid on subplot(sp1) for i=1:n MRaux(i,:)=MR(i,:); %variable aux de MR end clear MR a=0; MR=MR_simb; dibubot(MR) %dibuja robot en 3D end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = check(varargin) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 174 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB % Función que detecta si se activa ver el robot en 3D %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global checkdata if checkdata==0 checkdata=1; else chackdata=0; end end

ÁLVARO GÓMEZ RAMOS.

- 175 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja un eslabón, situado entre un punto inicial y % otro final. La base dibujada llevará % el ángulo de giro sobre el eje punto inicial-final indicados por el usuario: % [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global sp1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos la longitud del tramo para obtener el ángulo de giro % sobre el eje x a partir de zf y zi. Dependiendo del signo de % (yf-yi) el giro será en un sentido o en otro. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); tramos if (yf-yi)>0.0001 angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); end;

%Longitud del %ángulo positicvo %ángulo negativo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el giro sobre el eje z a partir del pto inicial y % final. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if (xi==xf)&(yi==yf) angz=0; else lx=xf-xi; ly=yf-yi; l1=sqrt(lx^2+ly^2); V=[lx/l1 ly/l1 0]*l; xf1=V(1); yf1=V(2); zf1=V(3); angz=(pi/2-acos(xf1/l)); end

%Si tramo es vertical %Ángulo y=0 %Longitud en x %Longitud en y %Longitud en plano XY %Proyección en eje XY

%ángulo de y

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 176 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz); z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz); z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz); z8]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); [x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.8 .4 .125],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 177 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=eslabon(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una eslabón, situado en un punto inicial y % que nos devuelve un punto final. El eslabón dibujado llevará % los ángulos de inclinación indicados por el usuario: % [xf yf zf]=eslabon(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 tras Q1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el nº de elementos de traslacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if tras>1 tras2=tras-1; else tras2=tras; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos datos de longitud %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% l=str2num(get(htexto,'String')); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cogemos los valores de los ángulos y los pasamos a radianes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% angx=-str2num(get(htextox,'String'))*2*pi/360; angz=-str2num(get(htextoz,'String'))*2*pi/360; ange=str2num(get(htextoe,'String'))*2*pi/360; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados

ÁLVARO GÓMEZ RAMOS.

- 178 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz); z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz); z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz); z8]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); [x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %sustituimos las variables por su valor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% x1=subs(x1,Q1(tras2)); x2=subs(x2,Q1(tras2)); x3=subs(x3,Q1(tras2)); x4=subs(x4,Q1(tras2)); x5=subs(x5,Q1(tras2)); x6=subs(x6,Q1(tras2)); x7=subs(x7,Q1(tras2)); x8=subs(x8,Q1(tras2)); y1=subs(y1,Q1(tras2)); y2=subs(y2,Q1(tras2)); y3=subs(y3,Q1(tras2)); y4=subs(y4,Q1(tras2)); y5=subs(y5,Q1(tras2)); y6=subs(y6,Q1(tras2)); y7=subs(y7,Q1(tras2)); y8=subs(y8,Q1(tras2)); z1=subs(z1,Q1(tras2)); z2=subs(z2,Q1(tras2)); z3=subs(z3,Q1(tras2)); z4=subs(z4,Q1(tras2)); z5=subs(z5,Q1(tras2)); z6=subs(z6,Q1(tras2)); z7=subs(z7,Q1(tras2)); z8=subs(z8,Q1(tras2));

ÁLVARO GÓMEZ RAMOS.

- 179 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.8 .4 .125],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 180 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una garra, situado entre un punto inicial y % otro final. La garra dibujada llevará % el ángulo de giro sobre el eje punto inicial-final indicados por el usuario: % [xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global sp1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos la longitud del tramo para obtener el ángulo de giro % sobre el eje x a partir de zf y zi. Dependiendo del signo de % (yf-yi) el giro será en un sentido o en otro. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); angx=-(pi/2-asin((zf-zi)/l)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el giro sobre el eje z a partir del pto inicial y % final. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if (xi==xf)&(yi==yf) angz=0; else lx=xf-xi; ly=yf-yi; l1=sqrt(lx^2+ly^2); V=[lx/l1 ly/l1 0]*l; xf1=V(1); yf1=V(2); zf1=V(3); angz=-(pi/2-acos(xf1/l)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+l;

ÁLVARO GÓMEZ RAMOS.

- 181 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5

y1 y2 y3 y4 y5

z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5]; caras=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 182 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=garra(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja una garra, situada en un punto inicial y % que nos devuelve un punto final. La garra dibujada llevará % los ángulos de inclinación indicados por el usuario: % [xf yf zf]=garra(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 l=str2num(get(htexto,'String')); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cogemos los valores de los ángulos y los pasamos a radianes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% angx=-str2num(get(htextox,'String'))*2*pi/360; angz=-str2num(get(htextoz,'String'))*2*pi/360; ange=str2num(get(htextoe,'String'))*2*pi/360; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5

y1 y2 y3 y4 y5

z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);

ÁLVARO GÓMEZ RAMOS.

- 183 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB [x2 [x3 [x4 [x5 end

y2 y3 y4 y5

z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5]; caras=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0 0],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 184 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function loadbot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que carga un robot almacenado en disco %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 sp6 sp7 n num_simb_rot Q1_simb Q1 MRejes Q2_simb num_simb_rot2 Q2 htras heje hrot hgarra hbase graph_DH graph_calcbot MR_simb graph_cindir graph_cininv rotac tras %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % carga las variables del archivo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [fname,pname] = uigetfile('*.mat','Select File'); dbfile= strcat(pname,fname); if length(dbfile) == 0 return; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % carga las variables al workspace %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% load(fname) for i=1:n MRaux(i,:)=MR(i,:); end clear MR a=0; MR=MR_simb; dibubot(MR) exacta MRejes=simple(plotejes(MRperp)); dibujo

%variable aux de MR

%dibuja el robot en esa posicion %Obtengo el resto de ejes y los

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Habilitacion de nuevas opciones en el menu %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% set(htras,'Enable','off'); set(heje,'Enable','off'); set(hrot,'Enable','off'); set(hgarra,'Enable','off'); set(hbase,'Enable','off'); set(graph_DH,'Enable','on') set(graph_calcbot,'Enable','off') set(graph_cindir,'Enable','on') return

ÁLVARO GÓMEZ RAMOS.

- 185 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function MRvec=MRtoMRvec(MR) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que transforma la matriz con las coordenadas de los % eslabones en una matriz con los ejes de las articulaciones de % rotación y traslacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% num=0; n=size(MR,1); MRvec=sym([0 0 0 0 0 0 0]); for i=1:n x0=MR(i,2); y0=MR(i,3); z0=MR(i,4); if (MR(i,1)==1)|(MR(i,1)==2) x1=MR(i,5); y1=MR(i,6); z1=MR(i,7); num=num+1;

%eje rot o tras

if MR(i,1)==2 %si la articulacion es de rotacion el eje se calcula: MRvec(num,:)=simple([(x0+x1)/2 (y0+y1)/2 (z0+z1)/2 x1-x0 y1-y0 z1-z0 1]); else %si la articulacion es de traslaccion el eje se calcula: MRvec(num,:)=simple([x1 y1 z1 x1-x0 y1-y0 z1-z0 2]); end end if (i==n)&(MR(i,1)~=1)&(MR(i,1)~=2) %si es un ultimo eslabon y no es una articulacion x1=MR(i,5); y1=MR(i,6); z1=MR(i,7); num=num+1; MRvec(num,:)=simple([MR(i-1,2) MR(i-1,3) MR(i-1,4) MR(i-1,5)MR(i-1,2) MR(i-1,6)-MR(i-1,3) MR(i-1,7)-MR(i-1,4) 0]); end end end

ÁLVARO GÓMEZ RAMOS.

- 186 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function MRperp=MRvectoMRperp(MRvec,MR) % Función que transforma la matriz con los ejes principales de las articulaciones % en una matriz que contiene la perpendicular a cada eje global sp1 num_simb_rot Q1_simb Q1 num_simb_rot2 Q2_simb Q2 n=size(MRvec,1); %nº de articulaciones n2=size(MR,1); %nº elementos del robot MRperp=sym([0 0 0 0 0 0 0 0 0 0]); for i=1:n-1 p1=simple(MRvec(i,1)); p2=simple(MRvec(i,2)); p3=simple(MRvec(i,3)); q1=simple(MRvec(i+1,1)); q2=simple(MRvec(i+1,2)); q3=simple(MRvec(i+1,3)); % v1=subs(MRvec(i,4),'q1'); % v2=subs(MRvec(i,5),'q1'); % v3=subs(MRvec(i,6),'q1'); v1=simple(MRvec(i,4)); v2=simple(MRvec(i,5)); v3=simple(MRvec(i,6)); w1=simple(MRvec(i+1,4)); w2=simple(MRvec(i+1,5)); w3=simple(MRvec(i+1,6)); q4=simple(p1*v1+p2*v2+p3*v3); u(i,:)=cross(MRvec(i+1,[4 5 6]),MRvec(i,[4 5 6]));

k1=q1-p1; k2=q2-p2; k3=q3-p3; mod_v1=sqrt(v1^2+v2^2+v3^2); A=[v1/mod_v1 w1;v2/mod_v1 w2;v3/mod_v1 w3]; B=[k1 v1 w1;k2 v2 w2; k3 v3 w3]; %determinamos si los ejes son coincidentes, paralelos, se cortan o se cruzan if (rank(A)==1)&(rank(B)==1) %coincidentes rectas=0; end if (rank(A)==1)&(rank(B)==2) %paralelos rectas=1; end if (rank(A)==2)&(rank(B)==2) %se cortan rectas=3; end if (rank(A)==2)&(rank(B)==3) %se cruzan rectas=4; end if rectas==0

%coincidentes

if (i>1)&(cross(MRvec(i,[4 5 6]),MRvec(i-1,[4 5 6]))==[0 0 0]) %Si los ejes de eslabon actual y anterior coinciden

ÁLVARO GÓMEZ RAMOS.

- 187 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 MRperp(i-1,[7 8 9]) MRvec(i,7)]); %eje z igual a eje x actual y eje x igual al z anterior u(i,:)=simple(MRperp(i-1,[7 8 9])); %elseif (i1) MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 MRvec(i-1,[4 5 6]) MRvec(i,7)]); u(i,:)=simple(MRvec(i-1,[4 5 6])); end if i==1 if MRvec(i,7)==2 MRperp(i,:)=([MRvec(i,[4 5 6]) MRvec(i,[1 2 3]) 1 0 0 MRvec(i,7)]); end if MRvec(i,7)==1 MRperp(i,:)=([MRvec(i,[4 5 6]) MRvec(i,[1 2 3]) sin(Q2_simb(i)) cos(Q2_simb(i)) 0 MRvec(i,7)]); end end

end if rectas==1 % Paralelas q1_=simple(det([q1 0 0 -w1;q2 1 0 -w2;q3 0 1 -w3;q4 v2 v3 0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0])); q2_=simple(det([1 q1 0 -w1;0 q2 0 -w2;0 q3 1 -w3;v1 q4 v3 0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0])); q3_=simple(det([1 0 q1 -w1;0 1 q2 -w2;0 0 q3 -w3;v1 v2 q4 0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0])); u(i,:)=([simple(q1_-p1) simple(q2_-p2) simple(q3_p3)]/sqrt((q1_-p1)^2+(q2_-p2)^2+(q3_-p3)^2)); %sum(abs([simple(q1_-p1) simple(q2_-p2) simple(q3_p3)]).^2)^(1/2));%norm([q1_-p1 q2_-p2 q3_-p3]); MRperp(i,:)=simple([MRvec(i,[4 5 6]) p1 p2 p3 u(i,:) MRvec(i,7)]); end if rectas==3 %Cortan x=simple((det([k1 w1 u(1);k2 w2 u(2);k3 w3 u(3)])/det([v1 w1 u(i,1);v2 w2 u(i,2);v3 w3 u(i,3)]))); r1=simple(p1+abs(x)*v1); r2=simple(p2+abs(x)*v2); r3=simple(p3+abs(x)*v3); MRperp(i,:)=([MRvec(i,[4 5 6]) r1 r2 r3 u(i,:) MRvec(i,7)]); end if rectas==4 % Cruzan MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 u(i,:) MRvec(i,7)]); end

ÁLVARO GÓMEZ RAMOS.

- 188 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB end MRperp(i+1,:)=([MRvec(i+1,[4 5 6]) MR(n2,[5 6 7]) u(i,:) MRvec(i+1,7)]); %vector z + punto de origen + vector x end

ÁLVARO GÓMEZ RAMOS.

- 189 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function nuevo() %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función para dibujar un nuevo robot, cargar uno ya existente o % salvar uno nuevo al disco %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoy htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 sp6 sp7 MR mfile_save n mfile_load mfile_graf mfile_DH n MRperp MRvec t htextot1 num_simb_rot Q1_simb x y z tras hejes hbot graph_DH MRejes hrot3d Q1 rotac Q2_simb Q2 num_simb_rot2 graph_calcbot htras heje hrot hgarra hbase xinf xsup yinf ysup zinf zsup t1 q1 q2 q3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %inicializacion de variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% syms t1 t2 t3 t4 t5 positive x=0; y=0; z=0; n=0; tras=1; rotac=1; MR=sym([0 0 0 0 0 0 0 0]); num_simb_rot=0; num_simb_rot2=0; Q1_simb=sym('q'); Q1(tras)=0; Q2_simb=sym('q'); Q2(rotac)=0;

%Variables de traslacion % X inicial % Y inicial % Z inicial % Nº de piezas iniciales %Nº de elem de traslacion+1 %Nº de elem de rotación+1 %Matriz del robot %nº de elementos de traslacion %nº de elementos de rotacion %Variables de traslacion %Valores de traslacion %Variables de rotacion %Valores de rotacion

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % lim de los ejes del plot principal %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xinf=-30; xsup=30; yinf=-30; ysup=30; zinf=0; zsup=40; ejes=[-20 20 -20 20 0 20]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Color de fondo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% set(hf1,'Color',[.824, .824, .824]) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Habilita opciones del menu

ÁLVARO GÓMEZ RAMOS.

- 190 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% set(mfile_load,'Enable','on') set(mfile_graf,'Enable','off') set(mfile_DH,'Enable','off') set(graph_calcbot,'Enable','on') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Creación de paneles %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% hp0 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.0 0 .10 1]); hp1 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.10 0 .13 1],'title','Piezas'); hp2 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.38 0 .7 1],'title','Robot'); hp3 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.93 0 .07 1],'title','Herramientas'); hp4 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.23 0.8 .15 0.8],'title','traslacion'); hp5 = uipanel('BackgroundColor',[.824, .824, .824],... 'Position',[.23 0.5 .15 0.3],'title','Rotación'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Creación de Plots para la representación de las figuras %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]); xlabel('eje x','color','r'); ylabel('eje y','color','g'); sp2=subplot(5,3,1); sp3=subplot(5,3,4); sp4=subplot(5,3,7); sp5=subplot(5,3,10); sp6=subplot(5,3,13); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Borrado de ejes de los subplots %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% subplot(sp1); axis off subplot(sp2); axis off subplot(sp3); axis off subplot(sp4); axis off subplot(sp5); axis off subplot(sp6); axis off

ÁLVARO GÓMEZ RAMOS.

- 191 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujo de Figuras de referencia %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % traslacion vertices1=[x-0.3 y-0.3 z;x+0.3 y-0.3 z;x+0.3 y+0.3 z;x-0.3 y+0.3 z;x0.3 y-0.3 z+7;x+0.3 y-0.3 z+7;x+0.3 y+0.3 z+7;x-0.3 y+0.3 z+7]; caras1=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp2) patch('Vertices',vertices1,'Faces',caras1,'FaceVertexCData',[.4, .4, .125],'FaceColor','flat'); AXIS([-0.5 9.5 -0.5 9.5 0 10]); camorbit(55,-45) % Rotación vertices2=[x y-0.5 z;x+0.5 y z;x y+0.5 z;x-0.5 y z;x y-0.5 z+1;x+0.5 y z+1;x y+0.5 z+1;x-0.5 y z+1]; caras2=[1 2 3 4;5 6 7 8;1 5 6 2;2 6 7 3;7 8 4 3;8 4 1 5]; subplot(sp3) patch('Vertices',vertices2,'Faces',caras2,'FaceVertexCData',[1, 0, 0],'FaceColor','flat'); AXIS([-0.5 9.5 -0.5 9.5 0 10]); camorbit(55,-45) % Eslabón vertices3=[x-0.5 y-0.5 z;x+0.5 y-0.5 z;x+0.5 y+0.5 z;x-0.5 y+0.5 z;x0.5 y-0.5 z+7;x+0.5 y-0.5 z+7;x+0.5 y+0.5 z+7;x-0.5 y+0.5 z+7]; caras3=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp4) patch('Vertices',vertices3,'Faces',caras3,'FaceVertexCData',[.8, .4, .125],'FaceColor','flat'); axis xy AXIS([-0.5 9.5 -0.5 9.5 0 10]); camorbit(55,-45) % Base vertices4=[x-1 y-1 z;x+1 y-1 z;x+1 y+1 z;x-1 y+1 z;x-1 y-1 z+1;x+1 y-1 z+1;x+1 y+1 z+1;x-1 y+1 z+1]; caras4=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp5) patch('Vertices',vertices4,'Faces',caras4,'FaceVertexCData',[0, 0, 0],'FaceColor','flat'); axis xy AXIS([-1 8 -1 8 0 4]); camorbit(55,-15) % Garra vertices5=[x-0.5 y-0.5 z;x+0.5 y-0.5 z;x+0.5 y+0.5 z;x-0.5 y+0.5 z;x y z+1]; caras3=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5]; subplot(sp6) patch('Vertices',vertices3,'Faces',caras3,'FaceVertexCData',[.0, .1, .0],'FaceColor','flat'); axis xy AXIS([-0.5 9.5 -0.5 9.5 0 10]); camorbit(55,-45) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 192 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB % Plot principal para dibujar el robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% subplot(sp1) axis on AXIS([xinf xsup yinf ysup zinf zsup]); camorbit(165,0) % Cámara grid on camlight right camlight left lighting phong

%Límite de ejes

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Botones, Textos y cuadros editables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% rotate_struct = load([matlabroot,'/toolbox/matlab/icons/rotate.mat']); rotate_cdata = rotate_struct.cdata; imgejes = imread('ejes.TIF'); imgbot = imread('robot.TIF'); %boton de rotación hrot3d = uicontrol('Style','togglebutton',... 'Callback',@rot3d,... 'Tag','mypan',... 'Units','normalized',... 'Position',[0.95 0.94 0.03 0.03],... 'CData',rotate_cdata,... 'Interruptible','off',... 'BusyAction','cancel'); %boton de ver ejes hejes = uicontrol('Style','togglebutton',... 'Callback',@ver,... 'Tag','mypan',... 'Units','normalized',... 'Position',[0.95 0.84 0.03 0.03],... 'CData',imgejes,... 'Interruptible','off',... 'BusyAction','cancel'); %boton de representar robot hbot = uicontrol('Style','togglebutton',... 'Callback',@ver,... 'Tag','mypan',... 'Units','normalized',... 'Position',[0.95 0.74 0.03 0.03],... 'CData',imgbot,... 'value',1,... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir traslacion htras = uicontrol('style','pushbutton',... 'Callback',@colocar1,...

ÁLVARO GÓMEZ RAMOS.

- 193 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'backgroundcolor',[.724, .724, .724],... 'String','Traslación',... 'Units','normalized',... 'Position',[.11 .77 .06 .03],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir rotacion heje = uicontrol('style','pushbutton',... 'Callback',@colocar2,... 'backgroundcolor',[.724, .724, .724],... 'String','Eje Rotación',... 'Units','normalized',... 'Position',[.11 .6 .06 .03],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir Eslabon hrot = uicontrol('style','pushbutton',... 'Callback',@colocar3,... 'backgroundcolor',[.724, .724, .724],... 'String','Eslabón',... 'Units','normalized',... 'Position',[.11 .42 .06 .03],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir base hbase = uicontrol('style','pushbutton',... 'Callback',@colocar4,... 'backgroundcolor',[.724, .724, .724],... 'String','Base',... 'Units','normalized',... 'Position',[.11 .23 .06 .03],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir garra hgarra = uicontrol('style','pushbutton',... 'Callback',@colocar5,... 'backgroundcolor',[.724, .724, .724],... 'String','Garra',... 'Units','normalized',... 'Position',[.11 .08 .06 .03],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %longitud htexto = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],...

ÁLVARO GÓMEZ RAMOS.

- 194 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Position',[.31 .06 .05 .03],... 'Enable','on',... 'Interruptible','off',... 'String','10',... 'BusyAction','cancel'); hlabel = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','longitud (dm)',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.245 .0648 .05 .02],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %rot z htextoz = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .11 .05 .03],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabelz = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Rot. en Z (º)',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.245 .1148 .05 .02],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %rot x htextox = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .16 .05 .03],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabelx = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Rot. en X (º)',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.245 .1648 .05 .02],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel');

ÁLVARO GÓMEZ RAMOS.

- 195 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %Rot eje htextoe = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .21 .05 .03],... 'Enable','off',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabele = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Rot. eje (º)',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.245 .2148 .05 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %Limite traslacion htextot = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .92 .05 .03],... 'Enable','off',... 'Interruptible','off',... 'String','10',... 'BusyAction','cancel'); hlabelt = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Límite traslacion',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.232 .9248 .068 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %nombre variable tras htextot1 = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .88 .05 .03],... 'Enable','on',... 'Interruptible','off',... 'String','',... 'BusyAction','cancel'); hlabelt1 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Nombre Var',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',...

ÁLVARO GÓMEZ RAMOS.

- 196 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Position',[.232 .8848 .068 .018],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel');

hlabelt2 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','dm',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.36 .9248 .018 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %limite rot htextor1 = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .60 .05 .03],... 'Enable','off',... 'Interruptible','off',... 'String','-360',... 'BusyAction','cancel'); hlabelr1 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Límite Rot. -',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.232 .5548 .068 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %limite rot + htextor2 = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .55 .05 .03],... 'Enable','off',... 'Interruptible','off',... 'String','360',... 'BusyAction','cancel'); hlabelr2 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Límite Rot. +',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.232 .6048 .068 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel');

ÁLVARO GÓMEZ RAMOS.

- 197 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %nombre var rot htextor3 = uicontrol('Style', 'edit', ... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[.31 .65 .05 .03],... 'Enable','off',... 'Interruptible','off',... 'String','',... 'BusyAction','cancel'); hlabelr3 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','Nombre Var.',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.232 .6548 .068 .018],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); hlabelr4 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','º',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.36 .795 .018 .03],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); hlabelr4 = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','º',... 'backgroundcolor',[.824 .824 .824],... 'Units','normalized',... 'Position',[.36 .745 .018 .03],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Ejecutar la función del tipo de pieza elegida %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % colocar traslacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = colocar1(varargin) global MR Q1 n x y z htextoe htexto tras if n==0

ÁLVARO GÓMEZ RAMOS.

- 198 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB MR=sym([0 0 0 0 0 0 0 0]); end n=n+1; Q1(tras)=str2num(get(htexto,'String')); longitud tras=tras+1; MR(n,[2 3 4])=sym([x y z]); [x y z x1 y1 z1]=traslac(x,y,z); MR(n,1)=1; MR(n,[5 6 7])=[x y z]; MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; end

%Pto inicial %nº de elem %valor variable %nº elem tras %pto inicial tramo %pto final %eje traslacion %pto final tramos %rot eje

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Colocar rotacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = colocar2(varargin) global MR Q2 n x y z htextoe rotac if n==0 MR=sym([0 0 0 0 0 0 0 0]); end n=n+1; Q2(rotac)=str2num(get(htextoe,'String')); rotacion rotac=rotac+1; rotac [x y z]=eje(x,y,z); MR(n,1)=2; MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; end

%pto inicial %nº de elem %valor variable de %nº de elem de %nuevo pto final %eje rotac %rot eje

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Colocar eslabon %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = colocar3(varargin) global MR n x y z htextoe if n==0 MR=sym([0 0 0 0 0 0 0 0]); end n=n+1; MR(n,[2 3 4])=[x y z]; [x y z]=eslabon(x,y,z); MR(n,[5 6 7])=[x y z]; MR(n,1)=3; MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; end

%nº de elementos %pto inicial %pto final %tipo elem %rot eje

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % colocar base %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 199 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function varargout = colocar4(varargin) global MR n x y z htextoe if n==0 MR=sym([0 0 0 0 0 0 0 0]); end n=n+1; MR(n,[2 3 4])=[x y z]; [x y z]=base(x,y,z); MR(n,[5 6 7])=[x y z]; MR(n,1)=4; MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; end

%nº de elementos %pto inicial %pto final %tipo de elem %rot eje

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % colocar garra %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = colocar5(varargin) global MR n x y z htextoe if n==0 MR=sym([0 0 0 0 0 0 0 0]); end n=n+1; MR(n,[2 3 4])=[x y z]; [x y z]=garra(x,y,z); MR(n,[5 6 7])=[x y z]; MR(n,1)=5; MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; end

%nº de elemento %pto inicial %pto final %tipo de elem %rot eje

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % habilitar/deshabilitar rotacion 3D %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = rot3d(varargin) global hrot3d persistent enable_state; val = get(hrot3d,'Value'); switch val case 0 rotate3d off; case 1 rotate3d on; end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Habilitar o Deshabilitar ver ejes

ÁLVARO GÓMEZ RAMOS.

- 200 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% function varargout = ver(varargin) global hejes hbot sp1 MR MRperp MRejes n Q2_simb Q1_simb persistent enable_state; val1 = get(hejes,'Value'); val2 = get(hbot,'Value'); delete (sp1) sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]); xlabel('eje x'); ylabel('eje y'); axis on AXIS([-30 30 -30 30 0 40]); camorbit(165,0) grid on if val1==1 plotejes(MRperp); %mostrar ejes si esta activado end if val2==1 %si esta activada la opción de dibujar robot for i=1:n MRaux(i,:)=MR(i,:); %variable aux de MR end clear MR a=0; for i=1:n if MRaux(i,1)==2 a=a+1; %rotar tramos for j=i+1:n [MRaux(j,2) MRaux(j,3) MRaux(j,4)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6) ,MRaux(i,7),MRaux(j,2),MRaux(j,3),MRaux(j,4),Q2_simb(a)); [MRaux(j,5) MRaux(j,6) MRaux(j,7)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6) ,MRaux(i,7),MRaux(j,5),MRaux(j,6),MRaux(j,7),Q2_simb(a)); end MRaux=simple(MRaux); end end MR=MRaux; dibubot(MR); %dibujar robot end end

ÁLVARO GÓMEZ RAMOS.

- 201 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function Par_DH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que calcula los parámetros DH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MRejes P_D_H graph_robot Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2 t1 t2 t3 t4 t5 q1 q2 q3 graph_cininv mfile_saveDH P_D_H=sym([0 0 0 0]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nueva ventana %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% hf2=figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Parámetros D-H',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[0.25 0.25 0.5 0.5]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calcula los parámetros %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% di=simple(calc_di(MRejes)); ai=simple(calc_ai(MRejes)); tethai=simple(calc_tethai(MRejes)); alphai=simple(calc_alphai(MRejes)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % mostramos los valores %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% tam=1+(1/(size(di,2)+1)); %Variable para dividir la ventana según el nº de parámetros for i=1:size(di,2)+1 if i==1 hpar_di = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','di',... 'ForegroundColor','b',... 'FontWeight','bold',... 'backgroundcolor',[1 1 1],... 'Units','normalized',...

ÁLVARO GÓMEZ RAMOS.

- 202 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Position',[0 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); else hpar_di = uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(di(i-1)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); end end tam=1+(1/(size(di,2)+1)); for i=1:size(ai,2)+1 if i==1 hpar_di = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','ai',... 'ForegroundColor','b',... 'FontWeight','bold',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0.25 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); else hpar_ai = uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(ai(i-1)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0.25 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); end end tam=1+(1/(size(di,2)+1)); for i=1:size(tethai,2)+1 if i==1 hpar_di = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','tethai',... 'ForegroundColor','b',... 'FontWeight','bold',... 'backgroundcolor',[1 1 1],...

ÁLVARO GÓMEZ RAMOS.

- 203 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB 'Units','normalized',... 'Position',[0.5 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); else hpar_ai = uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(tethai(i-1)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0.5 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); end end tam=1+(1/(size(di,2)+1)); for i=1:size(alphai,2)+1 if i==1 hpar_di = uicontrol('Style', 'text', ... 'Callback',@label,... 'String','alphai',... 'ForegroundColor','b',... 'FontWeight','bold',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0.75 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); else hpar_ai = uicontrol('Style', 'text', ... 'Callback',@label,... 'String',char(alphai(i-1)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[0.75 0 .25 tam(1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); end end for i=1:size(alphai,2) P_D_H(i,:)=[alphai(i);ai(i);tethai(i);di(i)]; parametros de Denavit-Hartemberg end

%matriz con los

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % habilitación de nuevas opciones de los menus

ÁLVARO GÓMEZ RAMOS.

- 204 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% set(graph_robot,'Enable','on') set(graph_cininv,'Enable','on'); set(mfile_saveDH,'Enable','on'); end

ÁLVARO GÓMEZ RAMOS.

- 205 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function MRejes=plotejes(MRperp) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que nos muestra los ejes del robot a partir de la matriz % MRperp y los almacena en otra matriz. MRejes=plotejes(MRperp) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global sp1

num_simb_rot Q1_simb Q1 num_simb_rot2 Q2_simb Q2 t1

n=size(MRperp,1); %Nº de ejes clear MRejes subplot(sp1) MRejes(1,[1 2 3 4 5 6 7 8 9 10 11 12 13])=sym([0 0 1 1 0 0 0 1 0 0 0 0 0]); %ejes iniciales MRejes_aux=MRejes; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %sustitucion de valores de traslacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot MRejes_aux=subs(MRejes,Q1_simb(j),Q1(j)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %sustitucion de valores de rotacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot2 MRejes_aux=subs(MRejes,Q2_simb(j),Q2(j)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %dibuja ejes iniciales %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=0:0.15:3 hold on plot3(MRejes_aux(1,1)*j, MRejes_aux(1,2)*j, MRejes_aux(1,3)*j,'.b',MRejes_aux(1,4)*j, MRejes_aux(1,5)*j, MRejes_aux(1,6)*j,'-*r',MRejes_aux(1,7)*j, MRejes_aux(1,8)*j, MRejes_aux(1,9)*j,'-+g') hold off end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculo los ejes y los dibujo %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n

ÁLVARO GÓMEZ RAMOS.

- 206 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB lz=sqrt(MRperp(i,1)^2+MRperp(i,2)^2+MRperp(i,3)^2); if lz~=0 MRejes(i+1,[1 2 3])=[MRperp(i,1)/lz MRperp(i,2)/lz MRperp(i,3)/lz]; %eje Z else MRejes(i+1,[1 2 3])=MRejes(i,[1 2 3]); end lx=sqrt(MRperp(i,7)^2+MRperp(i,8)^2+MRperp(i,9)^2); if lx~=0 MRejes(i+1,[4 5 6])=[MRperp(i,7)/lx MRperp(i,8)/lx MRperp(i,9)/lx]; %eje X else MRejes(i+1,[4 5 6])=MRejes(i,[4 5 6]); end MRejes(i+1,[7 8 9])=cross(MRejes(i+1,[1 2 3]),MRejes(i+1,[4 5 6])); %eje y MRejes(i+1,[10 11 12])=MRperp(i,[4 5 6]); %punto de los ejes MRejes(i+1,13)=MRperp(i,10); %Tipo de articulación MRejes_aux=MRejes; %sustitucion de valores de traslacción for j=1:num_simb_rot MRejes_aux=subs(MRejes_aux,Q1_simb(j),Q1(j)); end %sustitucion de valores de rotacion for j=1:num_simb_rot2 MRejes_aux=subs(MRejes_aux,Q2_simb(j),Q2(j)); end %Dibujamos el eje for j=0:0.15:3 hold on plot3(MRejes_aux(i+1,10)+MRejes_aux(i+1,1)*j, MRejes_aux(i+1,11)+MRejes_aux(i+1,2)*j, MRejes_aux(i+1,12)+MRejes_aux(i+1,3)*j,'.b',MRejes_aux(i+1,10)+MRejes_ aux(i+1,4)*j, MRejes_aux(i+1,11)+MRejes_aux(i+1,5)*j, MRejes_aux(i+1,12)+MRejes_aux(i+1,6)*j,'*r',MRejes_aux(i+1,10)+MRejes_aux(i+1,7)*j, MRejes_aux(i+1,11)+MRejes_aux(i+1,8)*j, MRejes_aux(i+1,12)+MRejes_aux(i+1,9)*j,'-+g') hold off end end MRejes=expand(MRejes);

ÁLVARO GÓMEZ RAMOS.

- 207 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function mat=redondear(mat) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %redondeamos los valores de las matrices %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [n,m]=size(mat); for i=1:n for j=1:m if (mat(i,j)-0.0001 mat(i,j)=0; end end end end

ÁLVARO GÓMEZ RAMOS.

- 208 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función para resolver el giro de un punto (X,Y,Z) respecto a la % recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un ángulo % de rotación igual a tetha % [Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %

vectores de la recta que pasa pos los puntos A=X2-X1; B=Y2-Y1; C=Z2-Z1;

T=[1 0 0 0;0 1 0 0;0 0 1 0;-X1 -Y1 -Z1 1]; T_1=[1 0 0 0;0 1 0 0;0 0 1 0;X1 Y1 Z1 1]; Lp=sqrt(B^2+C^2); if (B==0)&(Lp==0) aux1=1; aux2=0; else aux1=B/Lp; aux2=C/Lp; end Rx=[1 0 0 0;0 aux2 aux1 0;0 -aux1 aux2 0;0 0 Rx_1=[1 0 0 0;0 aux2 -aux1 0;0 aux1 aux2 0;0 L=sqrt(A^2+B^2+C^2); Ry=[Lp/L 0 A/L 0;0 1 0 0;-A/L 0 Lp/L 0;0 0 0 Ry_1=[Lp/L 0 -A/L 0;0 1 0 0;A/L 0 Lp/L 0;0 0 Rz=[cos(-tetha) sin(-tetha) 0 0;-sin(-tetha) 0;0 0 0 1]; Rp=T*Rx*Ry*Rz*Ry_1*Rx_1*T_1; Res=[X Y Z 1]*Rp; Xf=Res(1,1); Yf=Res(1,2); Zf=Res(1,3);

0 1]; 0 0 1]; 1]; 0 1]; cos(-tetha) 0 0;0 0 1

end

ÁLVARO GÓMEZ RAMOS.

- 209 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que rota un punto Pf con respecto a los ejes x e y que % hacemos pasar por otro punto dado Pi: % [Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% A=xf-xi; B=yf-yi; C=zf-zi; Rx=[1 0 0 0;0 cos(angx) sin(angx) 0;0 -sin(angx) cos(angx) 0;0 0 0 1]; Rx=redondear(Rx); Ry=[cos(angy) 0 sin(angy) 0;0 1 0 0;-sin(angy) 0 cos(angy) 0;0 0 0 1]; Ry=redondear(Ry); Rz=[cos(angy) sin(angy) 0 0;-sin(angy) cos(angy) 0 0;0 0 1 0;0 0 0 1]; Rz=redondear(Rz); F=[A B C 1]*Rx*Rz; Xf=F(1,1)+xi; Yf=F(1,2)+yi; Zf=F(1,3)+zi;

ÁLVARO GÓMEZ RAMOS.

- 210 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function savebot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que salva un robot al disco %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global MR n num_simb_rot Q1_simb Q1 Q2_simb Q2 num_simb_rot2 MRvec MRperp MR_simb rotac tras [fname,pname] = uiputfile('*.mat','Select File'); dbfile= strcat(pname,fname); if length(dbfile) == 0 return; end save (fname,'MR','n','num_simb_rot','Q1_simb','Q1','Q2_simb','Q2','num_simb _rot2','MRperp','MRvec','MR_simb','rotac','tras') return

ÁLVARO GÓMEZ RAMOS.

- 211 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function saveDH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que salva un robot al disco, guardando la matriz P_D_H, % que contiene la matriz de Denavit-Hartemberg, y la matriz DH que es % igual que P_D_H pero le añadimos el tipo de rotación. DH es % compatible con el toolbox de robótica %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global

P_D_H DH MRejes num_simb_rot num_simb_rot2

Q1_simb Q2_simb

P_D_Haux=P_D_H; cont1=0; cont2=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %vemos tipo de articulación %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for i=2:size(MRejes,1) if MRejes(i,13)==1 a(i-1)=0; cont1=cont1+1; end if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; end end a(cont1+cont2+1)=0; eje

%Es un eje de rotacion %Tipo de articulación %nº de artic de rotación %Es un eje de traslacion %Tipo de articulación %nº de artic de traslacion

%Consideramos el eje final como otro

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Sustituimos variables articulares por cero %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% for j=1:num_simb_rot P_D_Haux=subs(P_D_Haux,Q1_simb(j),0); end for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % creamos DH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% DH=P_D_Haux; for i=1:(size(DH,1)) DH(i,5)=a(i); end

ÁLVARO GÓMEZ RAMOS.

- 212 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %Guardamos %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [fname,pname] = uiputfile('*.mat','Select File'); dbfile= strcat(pname,fname); if length(dbfile) == 0 return; end save (fname,'P_D_H','DH') return

ÁLVARO GÓMEZ RAMOS.

- 213 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function sicon() %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función inicial que dibuja el logo y crea los menus desplegables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% clc clear variables warning off img = imread('Logo.jpg'); %imagen principal scrsz = get(0,'ScreenSize'); global hf1 mfile_save mfile_load mfile_graf MR graph_DH MRejes graph_calcbot graph_robot graph_cindir graph_cininv mfile_saveDH %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Ventana principal %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% hf1=figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','off',... 'Name','Sicon-Bot',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','on',... 'BusyAction','cancel',... 'Position',[0 0.08 1 0.92]);

subimage(img) axis off

%mostrar imagen de fondo % Sin ejes

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Creamos los menus y submenus desplegables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% mfile= uimenu('Label','Archivo'); mfile_graf=uimenu(mfile,'Label','Nuevo Gráficamente','Callback','nuevo','Separator','on'); mfile_save=uimenu(mfile,'Label','Salvar Robot','Callback','Savebot','Separator','on','Enable','off','Accelerat or','S'); mfile_saveDH=uimenu(mfile,'Label','Salvar DH','Callback','SaveDH','Separator','off','Enable','off'); mfile_load=uimenu(mfile,'Label','Cargar Robot','Callback','loadbot','Separator','on','Enable','off','Accelerat or','L'); uimenu(mfile,'Label','Salir','Callback','clear all','Callback','close all','Separator','on','Accelerator','Q'); mopt= uimenu('Label','Configuracion');

ÁLVARO GÓMEZ RAMOS.

- 214 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB mopt_pardin=uimenu(mopt,'Label','Parámetros Dinámicos','Callback','par_din','Accelerator','X','Enable','off'); graph= uimenu('Label','Ejecucion'); graph_calcbot=uimenu(graph,'Label','Calcular Robot','Callback','calcbot','Enable','off'); graph_DH=uimenu(graph,'Label','Parámetros DH','Callback','Par_DH','Enable','off'); graph_robot=uimenu(graph,'Label','Posicionar Robot','Callback','env_bot','Enable','off'); graph_cindir=uimenu(graph,'Label','Cinemática Directa','Callback','cin_dir','Enable','off'); graph_cininv=uimenu(graph,'Label','Cinemática Inversa','Callback','cin_inv','Enable','off'); mopt= uimenu('Label','Ayuda'); uimenu(mopt,'Label','Ayuda Sicon','Callback','Ayuda','Accelerator','H','Enable','on'); uimenu(mopt,'Label','Sobre...','Callback','sobre','Enable','on'); end

ÁLVARO GÓMEZ RAMOS.

- 215 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function sobre %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Nos muestra una ventana con datos sobre el programa %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% img = imread('sicon.jpg'); %imagen de fondo figure('Visible','on',... 'Units','normalized',... 'Renderer','opengl',... 'Color',[1 1 1],... 'Resize','off',... 'Name','Sicon-Bot',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[0.4 0.4 0.35 0.35]); subimage(img) axis off end

%nos muestra la imagen

ÁLVARO GÓMEZ RAMOS.

- 216 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja un eslabón, situado entre un punto inicial y % otro final, se usa en la función dibubot. La base dibujada llevará % el ángulo de giro sobre el eje punto inicial-final indicados por el usuario: % [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% global sp1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos la longitud del tramo para obtener el ángulo de giro % sobre el eje x a partir de zf y zi. Dependiendo del signo de % (yf-yi) el giro será en un sentido o en otro. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%% l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo if (yf-yi)>0.0001 %ángulo positivo angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); %ángulo negativo end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el giro sobre el eje z a partir del pto inicial y % final. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if (xi==xf)&(yi==yf) angz=0; else lx=xf-xi; ly=yf-yi; l1=sqrt(lx^2+ly^2); V=[lx/l1 ly/l1 0]*l; xf1=V(1); yf1=V(2); zf1=V(3); angz=(pi/2-acos(xf1/l)); end

%si tramos es vertical %ángulo y=0 %longitud en x %longitud en y %longitud en plano x-y %Proyeccion en eje XY

%ángulo de y

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%

ÁLVARO GÓMEZ RAMOS.

- 217 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB xf=xi; yf=yi; zf=zi+l; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz); z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz); z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz); z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz); z8]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi+l,angx,angz);

[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); [x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.4, .4, .125],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 218 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB function [xf1 yf1 zf1 xf yf zf]=traslac(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Función que dibuja eje de traslacion, situado en un punto inicial y % que nos devuelve un punto final. El eslabón dibujado llevará % los ángulos de inclinación indicados por el usuario en la ventana % principal: % [xf yf zf]=traslac(xi,yi,zi) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5 htextot1 num_simb_rot Q1_simb Q1 tras syms a positive

%nombre de variable de traslacion

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Calculamos el nº de elementos de traslacion %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if tras>1 %nº de elementos de traslacion igual a tras-1 tras2=tras-1; else tras2=tras; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Obtenemos datos de variable %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% num_simb_rot=num_simb_rot+1; traslación l=str2num(get(htexto,'String')); a=get(htextot1,'String'); Q1_simb(num_simb_rot)=a; L=sym(a);

%Añadimos un elemento más de %longitud inicial del tramos %nombre de variable (traslacion) %matriz de nombre de variables %Longitud

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Cogemos los valores de los ángulos y los pasamos a radianes %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% angx=-str2num(get(htextox,'String'))*2*pi/360; angz=-str2num(get(htextoz,'String'))*2*pi/360; ange=str2num(get(htextoe,'String'))*2*pi/360; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Situamos el punto final a una distancia L en vertical respecto % al punto inicial

ÁLVARO GÓMEZ RAMOS.

- 219 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% xf=xi; yf=yi; zf=zi+L; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con los valores adecuados %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% [x1 [x2 [x3 [x4 [x5 [x6 [x7 [x8

y1 y2 y3 y4 y5 y6 y7 y8

z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz); z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz); z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz); z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz); z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz); z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz); z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz); z8]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi+l,angx,angz);

[xf1 yf1 zf1]=rotar(xi,yi,zi,xf,yf,zf,angx,angz); xf=subs(xf1,Q1(tras2)); %Valor real de xf, yf, zf yf=subs(yf1,Q1(tras2)); zf=subs(zf1,Q1(tras2)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Rotamos la figura con respecto al eje Xi-Xf %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% if ange [x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange); [x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% %sustituimos las variables por su valor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% x1=subs(x1,Q1(tras2)); x2=subs(x2,Q1(tras2)); x3=subs(x3,Q1(tras2)); x4=subs(x4,Q1(tras2)); x5=subs(x5,Q1(tras2)); x6=subs(x6,Q1(tras2)); x7=subs(x7,Q1(tras2)); x8=subs(x8,Q1(tras2)); y1=subs(y1,Q1(tras2)); y2=subs(y2,Q1(tras2)); y3=subs(y3,Q1(tras2));

ÁLVARO GÓMEZ RAMOS.

- 220 -

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de Robots Manipuladores en entorno MATLAB y4=subs(y4,Q1(tras2)); y5=subs(y5,Q1(tras2)); y6=subs(y6,Q1(tras2)); y7=subs(y7,Q1(tras2)); y8=subs(y8,Q1(tras2)); z1=subs(z1,Q1(tras2)); z2=subs(z2,Q1(tras2)); z3=subs(z3,Q1(tras2)); z4=subs(z4,Q1(tras2)); z5=subs(z5,Q1(tras2)); z6=subs(z6,Q1(tras2)); z7=subs(z7,Q1(tras2)); z8=subs(z8,Q1(tras2)); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% % Dibujamos el elemento %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%% vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7 z7;x8 y8 z8]; caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8]; subplot(sp1) patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.4, .4, .125],'FaceColor','flat'); return

ÁLVARO GÓMEZ RAMOS.

- 221 -