BLOQUE 1: CONCEPTOS BÁSICOS... 6

Transcripción

1 Prefacio:... 4 BLOQUE 1: CONCEPTOS BÁSICOS CAPÍTULO: INTRODUCCIÓN A LA ROBÓTICA Introducción Historia de la robótica Bases de la robótica actual Definición de Robot Límites de la robótica actual COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO Introducción El Brazo Robot Grados de libertad Articulaciones Sistemas de coordenadas Volumen de trabajo Actuadores El controlador Dispositivos sensoriales Elementos terminales CAPÍTULO: TRANSFORMACIONES 3D Introducción Geometría 3D Cambio de escala Traslación Rotación Rotación alrededor de un eje arbitrario CAPÍTULO: CINEMÁTICA Introducción El problema Cinemático Directo Algoritmo de Denavit-Hartenberg Cinemática Inversa CAPÍTULO: TOOLBOX DE ROBÓTICA Introducción Representación de traslación 3D y orientación ÁLVARO GÓMEZ RAMOS

2 5.3 Cinemática directa Cinemática inversa BLOQUE 2: DESARROLLO DEL SOFTWARE CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT Introducción SICON-BOT Objetivos de la primera versión Posibles mejoras futuras Requisitos mínimos Inicialización CAPÍTULO: GRÁFICA DE SICON-BOT Creación de ventana principal y menús desplegables Paneles y plots de representación Eslabones y articulaciones Botones, textos y cuadros editables CAPÍTULO: POSIBILIDADES DE SICON-BOT Nuevo robot Calcular robot Salvar Robot Cargar Robot Parámetros D-H Salvar D-H Cinemática directa Cinemática inversa Posicionar Robot Ayuda Herramientas CAPÍTULO: PROGRAMACIÓN DE SICON-BOT Introducción Inicialización de ventana inicial y menús desplegables Entorno de creación de robots Añadir elemento de traslación Añadir elemento de rotación Añadir elemento eslabón Añadir elemento base Posicionar y dibujar por primera vez el elemento traslación Posicionar y dibujar por primera vez el elemento rotación Posicionar y dibujar por primera vez el elemento eslabón Posicionar y dibujar por primera vez el elemento base ÁLVARO GÓMEZ RAMOS

3 4.12 Rotar punto respecto a ejes x e y que pasan por otro punto dado Rotar punto respecto a ejes dado por dos puntos Calcular robot Obtención de los ejes de las articulaciones Obtención de eje x, eje z y punto de origen Obtención de eje y, normalizazión y dibujado de ejes Dibujar robot Dibujar elemento eslabón Dibujar elemento traslación Dibujar elemento rotación Dibujar elemento base Salvar robot Cargar robot Calculo de parámetros DH Calculo de parámetros d i Calculo de parámetros a i Calculo de parámetros θ i Calculo de parámetros α i Cinemática directa Cinemática inversa Posicionar robot Ayuda Función auxiliar: Multiplicar vectorialmente vectores simbólicos Función auxiliar: Redondear a cero los valores de las matrices BLOQUE 3: CONCLUSIONES Conclusiones Desarrollos futuros BLOQUE 4: ANEXOS ANEXO: CÓDIGO FUENTE ÁLVARO GÓMEZ RAMOS

4 Prefacio: Durante los estudios universitarios hemos recibido una amplia formación en robótica, tanto teóricamente como en la práctica, aprio 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 extio, 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

5 - Integración: Pues incorpora internamente funciones del toolbox de robótica como la cinemática directa e inversa. ÁLVARO GÓMEZ RAMOS

6 BLOQUE 1: CONCEPTOS BÁSICOS ÁLVARO GÓMEZ RAMOS

7 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, siguio 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 tencia 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 defí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

8 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, permitio 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 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

9 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 entie 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 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 siguio 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

10 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

11 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

12 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: - Un manipulador o brazo (la unidad mecánica), incluyo una garra o elemento final. - Un controlador (El módulo inteligente ). - Una unidad de potencia. - Un sistema sensorial. 2.2 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 indepientes necesarias para expresar la posición de todas sus partes. ÁLVARO GÓMEZ RAMOS

13 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 permitio 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ñadio 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ñadio nuevos grados de movilidad podemos compensar estas limitaciones. ÁLVARO GÓMEZ RAMOS

14 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

15 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

16 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

17 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

18 2.8 El controlador El controlador es el dispositivo que proporciona la inteligencia necesaria para que actúe conforme al operador. Esencialmente consiste en: - 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 aprer posiciones al robot u operaciones en forma de consola o botonera de enseñanza. 2.9 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, midio su tensión inicial y su tensión final podemos saber en que posición está el robot, debido a la relación: r V out = V in. R ÁLVARO GÓMEZ RAMOS

19 - Synchro: Es transductor rotacional que convierte desplazamientos angulares en voltaje de corriente alterna. Se basa en la fórmula: V out ( t) = V sin( θ )sin( ω t) m ac - 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( ω act), los dos voltajes de estator son: V2 ( t) = Vm sin( θ )sin( ωact) V ( t) = V cos( θ )sin( ω ) 1 m act - Codificadores ópticos: Realizan tareas de determinación de posición con relativa facilidad y sorprente 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

20 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 midio 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

21 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 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

22 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

23 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. Exteremos 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 establecio 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. Por lo tanto, todavía son necesarios dos puntos para definir ÁLVARO GÓMEZ RAMOS

24 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 perpicular al plano. Este vector recibe el nombre de vector normal. Por otro lado, si 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. es un punto del plano, por lo tanto, ÁLVARO GÓMEZ RAMOS

25 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 si empleamos coordenadas homogéneas 3.4 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 eje z. Podemos generalizar este concepto a una rotación tridimensional alrededor del ÁLVARO GÓMEZ RAMOS

26 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

27 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 y su dirección está especificada por el vector. 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 comprido 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 extie desde hasta. ÁLVARO GÓMEZ RAMOS

28 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, tenio 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

29 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

30 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

31 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 indepientes 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

32 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 f f f f f x y z α β γ ( q1 q2... qn ) ( q1 q2... qn ) ( q1 q2... qn ) ( q1 q2... qn ) ( q1 q2... qn ) ( q q... q ) 1 2 n Donde: q 1...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

33 x = l 1 1 cosθ + l y = l senθ + l cos( θ + θ ) sen( θ + θ ) 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. Sio la matriz: i Ai 1 Sio 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. robot: Se puede representar de forma parcial o total la cadena cinemática que forma el A n 0 = n i= 1 A i 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 = A0 = A0 A1 A2 A3 A4 A5 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 depen 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 x 1 una distancia a i - Rotación alrededor del eje x 1 un ángulo α i ÁLVARO GÓMEZ RAMOS

34 A Desarrollando la expresión: = T ( z, θ ) T (0,0, d ) T ( a,0,0) T ( x, α ) i i 1 i i i i A i i 1 cosθ i = senθ i 0 0 senθ cosθ 0 0 i i d i ai cosα 0 i senα i 0 senα cosα 0 i i Obtenemos la expresión general de DH, donde θ i, d i, ai, α i son los parámetros DH del eslabón i: A i i 1 cosθ i = senθ i 0 0 cosα senθ cosα cosθ i 0 i senα i i i senα senθ i senα cosθ i cosα 0 i i i ai cosθ i a isenθ i d i 1 Para que la matriz A i i 1 relacione los sistemas coordenados O i y O i 1 es necesario que los sistemas coordenados se determinen mediante los siguientes pasos: 1. Numerar y etiquetar el eslabón fijo (base) como 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 establecio el origen como O 0 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 O i : 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

35 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 O i. 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 Y i 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 Y n de acuerdo a la regla de la mano derecha. Si la herramienta es una pinza, es común establecer el eje Y n entre los dedos de la pinza y X n será ortonormal a Z n y Y n. 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 d i a i α i ÁLVARO GÓMEZ RAMOS

36 Donde: θ i = Es el ángulo formado por los ejes X i y X i 1 medido en un plano perpicular 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 O i 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. a i = Para articulaciones rotatorias: es la distancia a lo largo del eje desde el origen O i 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 perpicular al eje X utilizando la regla de la mano derecha. i 11. Realizar la matriz D-H de transformación homogénea A i i 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. X i T = A n 0 = n i= 1 A i i 1 ÁLVARO GÓMEZ RAMOS

37 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: Donde: q k = ( x, y, z, α, β, γ ) f k K = 1...n q 1...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 indepiente 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 depiente 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 indepientes. ÁLVARO GÓMEZ RAMOS

38 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 compren, 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

39 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 conocio 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 prete 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, sio 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

40 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 deperá 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 contrá 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 [ ]. 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 hacio uso de la función TR2DIFF. ÁLVARO GÓMEZ RAMOS

41 BLOQUE 2: DESARROLLO DEL SOFTWARE ÁLVARO GÓMEZ RAMOS

42 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, aprio 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 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

43 1.2 SICON-BOT. Hoy en día la robótica se va extio, 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 convirtio 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 - 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. 1.3 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, introducio 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

44 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: - 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. 1.4 Posibles mejoras futuras. Existen una gran cantidad de posibles mejoras a añadir al programa, aquí pondremos algunas que serían de gran utilidad: - Adición de dinámica - Adición de movimiento en 3D siguio 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 (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 (Recommed) ÁLVARO GÓMEZ RAMOS

45 - 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 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

46 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

47 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

48 - 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

49 Elementos posibles del robot Parámetros del robot Plot principal 2.3 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

50 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,[ ]); creamos un subplot dividio 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 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

51 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

52 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

53 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

54 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. traslación rotación base eslabón 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

55 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

56 3 CAPÍTULO: POSIBILIDADES DE SICON-BOT. 3.1 Nuevo robot Para poder crear un nuevo robot, tenemos que iniciar el programa desde matlab escribio 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

57 Ahora depio del robot que queramos diseñar iremos introducio nuestros elementos siguio 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, pudio 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 depe 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: ÁLVARO GÓMEZ RAMOS

58 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: robot. Una vez diseñado por completo el siguiente y obligatorio paso es calcular el 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

59 Este proceso implicará un cálculo complejo con variables simbólicas, que depio 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

60 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 escribio 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

61 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ñadio así el robot al espacio de trabajo con los ejes ya calculados. ÁLVARO GÓMEZ RAMOS

62 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 tremos 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

63 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 D- H 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 trá 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

64 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, obtremos el valor necesario de las variables para que el robot alcance dicha posición. ÁLVARO GÓMEZ RAMOS

65 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

66 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, tenio en cuenta que el robot de la ventana emergente puede poseer cambios de variables para simplificar la matriz D-H. ÁLVARO GÓMEZ RAMOS

67 3.10 Ayuda Sicon-bot dispone de una pequeña ayuda para aprer 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

68 3.10 Herramientas A la derecha del espacio de trabajo de sicon-bot disponemos de un panel de herramientas. ÁLVARO GÓMEZ RAMOS

69 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

70 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 prete 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

71 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á Sicon- Bot. - : Elimina que aparezca el número de la ventana siguio 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

72 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 condición. También es interesante la propiedad, que hace que la ÁLVARO GÓMEZ RAMOS

73 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

74 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

75 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 iluminación al plot principal:, consiguio que entre un rayo de luz 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

76 - 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

77 Analicemos paso por paso la función. Primero elegimos las variables globales que nos van a ser útiles: - MR: Es la matriz que contrá 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), sio 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

78 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, sio 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

79 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: robot: Incrementamos en uno el número de elementos de rotación que contiene nuestro 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, sio un 2 para los elementos de rotación: ÁLVARO GÓMEZ RAMOS

80 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

81 Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido, sio 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

82 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, sio 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

83 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 deperán del punto inicial donde dibujar el elemento, suponio 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

84 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

85 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

86 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 Pto. final Pto. inicial X4 X6 X1 X3 X2 La situación de los 8 vértices deperá del centro de gravedad del elemento, suponio 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

87 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

88 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

89 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 deperán del punto inicial donde dibujar el elemento, suponio 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

90 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

91 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

92 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 deperán del punto inicial donde dibujar el elemento, suponio 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

93 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

94 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

95 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 eje z. Podemos generalizar este concepto a una rotación tridimensional alrededor del 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

96 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 un punto de esta recta es y su dirección está especificada por el vector. 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 comprido 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 extie desde hasta. ÁLVARO GÓMEZ RAMOS

97 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, tenio 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

98 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

99 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 depe de los variable de longitud de los ÁLVARO GÓMEZ RAMOS

100 elementos de traslación, sin embargo, las variables de rotación no se ven reflejadas en esta matriz, tenio 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

101 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) 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 perpicular a cada eje (MRperp). Para calcular el eje x nos basamos en Denavit-Hartenberg, que dice que se busque la recta perpicular 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

102 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

103 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 tenio en cuenta las siguientes ecuaciones: Tenemos todos los datos necesarios excepto λ que hallamos resolvio 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

104 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 indepientes, 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 X i según la fórmula: Sio 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 indepientes. 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 perpicular común a los ejes implicados. Si las rectas son paralelas debemos determinar el vector perpicular 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 perpicular común: ÁLVARO GÓMEZ RAMOS

105 El último caso posible es que las rectas sean coincidentes, lo cual resolvemos depio 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 perpicular 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 perpicular 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 tenio previamente los ejes z de todas las articulaciones, sólo nos queda calcularlos de la última articulación ficticia. El eje x será la perpicular común al eje anterior y el actual, situándolo en el último punto del robot: ÁLVARO GÓMEZ RAMOS

106 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, recorrio 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

107 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, sustituyo las variables por sus valores iniciales: Luego recorremos la matriz MR, dibujando uno a uno los elementos depio 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 conocio el punto inicial y final del elemento: ÁLVARO GÓMEZ RAMOS

108 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 vio 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 l1 ÁLVARO GÓMEZ RAMOS

109 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 deperán del punto inicial donde dibujar el elemento, suponio 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

110 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

111 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 vio 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

112 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

113 X8 Pto. final X5 X7 X6 X4 Pto. inicial X1 X3 X2 La situación de los 8 vértices deperán del punto inicial donde dibujar el elemento, suponio 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

114 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 vio la distancia entre el punto inicial y el final: ÁLVARO GÓMEZ RAMOS

115 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 l1 ÁLVARO GÓMEZ RAMOS

116 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 X6 X7 Pto. inicial X4 X1 X3 X2 La situación de los 8 vértices deperán del punto inicial donde dibujar el elemento, suponio 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

117 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

118 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 vio 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 X5 X8 X4 X7 Pto. inicial X1 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

119 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

120 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 indepientes: Ya tenemos calculados los parámetros, ahora los mostraremos en una nueva pantalla que debemos crear: ÁLVARO GÓMEZ RAMOS

121 Los datos de DH los representamos en forma matricial, tenio 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, dividio la pantalla en cuatro divisiones verticales y sabio que la longitud total es 1, d se representará en la división que va de 0 a 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 d α θ ÁLVARO GÓMEZ RAMOS

122 ÁLVARO GÓMEZ RAMOS

123 Ya hemos representado todos los valores, ahora los almacenamos en una matriz llamada P_D_H : 4.25 Calculo de parámetros d i di es la distancia de xi-1 a xi medida a lo largo de zi ÁLVARO GÓMEZ RAMOS

124 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, sio P el punto de los ejes i, Q el punto de los ejes i-1 y el vector xi. Con lo cual el cálculo se resolverá: 4.26 Calculo de parámetros a i ai: Es la distancia de zi a zi+1 medida a lo largo de xi ÁLVARO GÓMEZ RAMOS

125 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, sio P el punto de los ejes i, Q el punto de los ejes i-1 y el vector zi. Con lo cual el cálculo se resolverá: ÁLVARO GÓMEZ RAMOS

126 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 tenio 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

127 Es el ángulo que forman dos rectas es el ángulo que forman sus vectores directores: Calculamos según la fórmula anterior tenio 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 conocio 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 obtenio 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 sio n el número de elementos de MR ÁLVARO GÓMEZ RAMOS

128 ÁLVARO GÓMEZ RAMOS

129 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, sio a el tipo de variables, y Var los nombres: ÁLVARO GÓMEZ RAMOS

130 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

131 ÁLVARO GÓMEZ RAMOS

132 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

133 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 deperá 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 contrá 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 [ ]. 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 hacio 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: T: [1 0 0 str2num(get(xedit,'string'));0 1 0 str2num(get(yedit,'string'));0 0 1 str2num(get(zedit,'string')); ]. - Q0: matriz de ceros del tamaño adecuado. - M: [ ]. ÁLVARO GÓMEZ RAMOS

134 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

135 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

136 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

137 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

138 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 en alguno de sus términos, lo iguala a cero: ÁLVARO GÓMEZ RAMOS

139 BLOQUE 3: CONCLUSIONES ÁLVARO GÓMEZ RAMOS

140 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 surgio, 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

141 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 siguio 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, consiguio un tipo de software poco desarrollado hasta el día de hoy. Sobre todo Sicon-Bot se prete 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

142 BLOQUE 4: ANEXOS ÁLVARO GÓMEZ RAMOS

143 1 ANEXO: CÓDIGO FUENTE. function ayuda % Función de ayuda web index.htm ÁLVARO GÓMEZ RAMOS

144 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 y1 z1]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi,ange); [x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi,ange); [x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi,ange); [x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi,ange); [x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi+l,ange); [x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi+l,ange); [x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi+l,ange); [x8 y8 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=[ ; ; ; ; ; ]; subplot(sp1) ÁLVARO GÓMEZ RAMOS

145 patch('vertices',vertices,'faces',caras,'facevertexcdata',[0 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

146 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 y1 z1]=rotar(xi,yi,zi,xi-2,yi-2,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+2,yi-2,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+2,yi+2,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-2,yi+2,zi,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi-2,yi-2,zi+l,angx,angz); [x6 y6 z6]=rotar(xi,yi,zi,xi+2,yi-2,zi+l,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi+2,yi+2,zi+l,angx,angz); [x8 y8 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

147 % Rotamos la figura con respecto al eje Xi-Xf [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); % 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[0 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

148 function ai=calc_ai(mrejes) % calculamos ai matemáticamente n=size(mrejes,1); for i=1:n-1 P=MRejes(i,[ ]); v=mrejes(i,[1 2 3]); Q=MRejes(i+1,[ ]); 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; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); ai(i)=mod_qp/mod_v; 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; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); ai(i)=mod_qp/mod_v; 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; ÁLVARO GÓMEZ RAMOS

149 mod_v=sqrt(mod_v); ai(i)=deta/mod_v; 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; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); ai(i)=mod_qp/mod_v; ÁLVARO GÓMEZ RAMOS

150 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); ÁLVARO GÓMEZ RAMOS

151 function di=calc_di(mrejes) %Calculo di matemáticamente n=size(mrejes,1); clear di for i=2:n P=MRejes(i-1,[ ]); v=mrejes(i-1,[4 5 6]); Q=MRejes(i,[ ]); 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) %coincidentes a=cross((q-p),v); mod_qp=0; mod_v=0; for j=1:3 mod_qp=mod_qp+a(j)^2; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); di(i-1)=mod_qp/mod_v; if (rank(a)==1)&(rank(b)==2) %paralelos a=cross((q-p),v); mod_qp=0; mod_v=0; for j=1:3 mod_qp=mod_qp+a(j)^2; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); di(i-1)=mod_qp/mod_v; if (rank(a)==2)&(rank(b)==2) %se cortan 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; ÁLVARO GÓMEZ RAMOS

152 mod_v=sqrt(mod_v); di(i-1)=deta/mod_v; 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; for j=1:3 mod_v=mod_v+v(j)^2; mod_qp=sqrt(mod_qp); mod_v=sqrt(mod_v); di(i-1)=mod_qp/mod_v; %se cruzan ÁLVARO GÓMEZ RAMOS

153 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); 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(i- 1)=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

154 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); ÁLVARO GÓMEZ RAMOS

155 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 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); MRaux=simple(MRaux); MR=MRaux; MR_simb=MRaux; %Creamos MR con los valores simbólicos de rotación MRvec=simple(MRtoMRvec(MRaux)); %Paso de matriz de robot a matriz con vector del eje de la articulacion MRperp=simple(MRvectoMRperp(MRvec,MRaux)); %Obtengo la matriz con vector z + punto de origen + vector x MRejes=simple(plotejes(MRperp)); %Obtengo el resto de ejes y los dibujo % habilitaciones de nuevas opciones del menu ÁLVARO GÓMEZ RAMOS

156 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'); ÁLVARO GÓMEZ RAMOS

157 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',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Cinemática Inversa',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[ ]); uicontrol('style', 'text',... 'Callback',@label,... 'String',char(MR_simb(n,5)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'Callback',@label,... 'String',char(MR_simb(n,6)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'Callback',@label,... 'String',char(MR_simb(n,7)),... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... '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

158 .05],....05],....05],... 'Position',[ 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'String','y=',... 'backgroundcolor',[1 1 1],... 'ForegroundColor','b',... 'FontWeight','bold',... 'Units','normalized',... 'Position',[ 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'Callback',@label,... 'String','z=',... 'backgroundcolor',[1 1 1],... 'ForegroundColor','b',... 'FontWeight','bold',... 'Units','normalized',... 'Position',[ 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); ÁLVARO GÓMEZ RAMOS

159 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); if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; Var(i-1)=Q1_simb(cont2); 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); for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); % 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'); ÁLVARO GÓMEZ RAMOS

160 % Cálculo de DH DH=P_D_Haux; for i=1:(size(dh,1)) DH(i,5)=a(i); % ventana para introducir datos figure('visible','on',... 'Units','normalized',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Cinemática Inversa',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[ ]); hintdata = uicontrol('style','pushbutton',... 'Callback',@intdata2,... 'backgroundcolor',[.724,.724,.724],... 'String','Calcular',... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'Callback',@label,... 'String','x =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %introducir x xedit=uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); ÁLVARO GÓMEZ RAMOS

161 uicontrol('style', 'text',... 'String','y =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Introducir y yedit=uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); uicontrol('style', 'text',... 'Callback',@label,... 'String','z =',... 'backgroundcolor',[1 1 1],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Introducir z zedit=uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); % Nos dibuja las variables y su valor y % las coloca en la ventana depio 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

162 .05],... 'Position',[.4 c-b.1 '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; % creamos el robot según toolbox de robótica robotd2=robot(siconbot,'robot de siconbot'); 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')); ]; vectorq0=zeros(1,tras+rotac-1); %Posición inicial q=ikine(dh,stol,ilimit,tg,vectorq0,[ ]); %Cálculo de cinemática Inversa % Dibujamos el robot ÁLVARO GÓMEZ RAMOS

163 figure plot(robotd2,q) axis on AXIS([ ]); camorbit(165,0) grid on %Posicionar robot % Mostrar valores de variables articulares for i=1:n-1 set(hdata(i),'string',num2str(q(i))); 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

164 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 for j=1:num_simb_rot2 MR=subs(MR,Q2_simb(j),-Q2(j)); variables %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)); ÁLVARO GÓMEZ RAMOS

165 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. Depio 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)); % if (yf-yi)> angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); ; %ángulo positicvo %ángulo negativo % Calculamos el giro sobre el eje z a partir del pto inicial y % final. ÁLVARO GÓMEZ RAMOS

166 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 % 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 y1 z1]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1-0.5,angx,angz); [x2 y2 z2]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1-0.5,angx,angz); [x3 y3 z3]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1-0.5,angx,angz); [x4 y4 z4]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1-0.5,angx,angz); [x5 y5 z5]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1+0.5,angx,angz); [x6 y6 z6]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1+0.5,angx,angz); [x7 y7 z7]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1+0.5,angx,angz); [x8 y8 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); ÁLVARO GÓMEZ RAMOS

167 % 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[1 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

168 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 y1 z1]=rotar(xi,yi,zi,xi,yi-0.6,zi-0.5,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.6,yi,zi-0.5,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi,yi+0.6,zi-0.5,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.6,yi,zi-0.5,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi,yi-0.6,zi+0.5,angx,angz); ÁLVARO GÓMEZ RAMOS

169 [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 y1 z1]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x1,y1,z1,ange); [x2 y2 z2]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x2,y2,z2,ange); [x3 y3 z3]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x3,y3,z3,ange); [x4 y4 z4]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x4,y4,z4,ange); [x5 y5 z5]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x5,y5,z5,ange); [x6 y6 z6]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x6,y6,z6,ange); [x7 y7 z7]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x7,y7,z7,ange); [x8 y8 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)); % Dibujamos el elemento ÁLVARO GÓMEZ RAMOS

170 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[1 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

171 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 if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; Var(i-1)=Q1_simb(cont2); 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); for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); % Nueva ventana para introducir los datos de las variables ÁLVARO GÓMEZ RAMOS

172 figure('visible','on',... 'Units','normalized',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Introducir datos',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[ ]); % Botón hintdata = uicontrol('style','pushbutton',... 'Callback',@intdata,... 'backgroundcolor',[.724,.724,.724],... 'String','Intr. Dato',... 'Units','normalized',... 'Position',[ ],... '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',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); % Nos dibuja las variables para que le podamos asignar un valor y % las coloca en la ventana depio 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

173 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; % 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'); % creamos el robot robotd2=robot(siconbot,'robot de siconbot'); 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')); vector(i+1)=0; %Ultima articulación(ficticia)=0 ÁLVARO GÓMEZ RAMOS

174 % Dibujamos el robot figure plot(robotd2,vector) %dibuja el robot axis on AXIS([ ]); 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; if MRejes(i+1,13)==2 Q1(cont1+1)=str2num(get(hdata(i),'String')); %Valor de variables cont1=cont1+1; figure(hf1) %Seleccionamos figure principal de siconbot % Preparamos el subplot principal delete (sp1) sp1=subplot(5,3,[ ]); xlabel('eje x'); ylabel('eje y'); axis on AXIS([ ]); camorbit(165,0) grid on subplot(sp1) for i=1:n MRaux(i,:)=MR(i,:); %variable aux de MR clear MR a=0; MR=MR_simb; dibubot(mr) %dibuja robot en 3D function varargout = check(varargin) ÁLVARO GÓMEZ RAMOS

175 % Función que detecta si se activa ver el robot en 3D global checkdata if checkdata==0 checkdata=1; else chackdata=0; ÁLVARO GÓMEZ RAMOS

176 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. Depio 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)> angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); ; %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) %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 % Situamos el punto final a una distancia L en vertical respecto % al punto inicial ÁLVARO GÓMEZ RAMOS

177 xf=xi; yf=yi; zf=zi+l; % Rotamos la figura con los valores adecuados [x1 y1 z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz); [x6 y6 z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz); [x8 y8 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); % 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[ ],'facecolor','flat'); return ÁLVARO GÓMEZ RAMOS

178 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; % 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

179 [x1 y1 z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz); [x6 y6 z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz); [x8 y8 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); %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

180 % 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[ ],'facecolor','flat'); return ÁLVARO GÓMEZ RAMOS

181 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. Depio 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)); % 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

182 % Rotamos la figura con los valores adecuados [x1 y1 z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); [x5 y5 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); % Dibujamos el elemento vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5]; caras=[ ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[0 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

183 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 y1 z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz); [x5 y5 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

184 [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); % Dibujamos el elemento vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5]; caras=[ ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[0 0 0],'FaceColor','flat'); return ÁLVARO GÓMEZ RAMOS

185 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; % carga las variables al workspace load(fname) for i=1:n MRaux(i,:)=MR(i,:); 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

186 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([ ]); 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]); 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]); ÁLVARO GÓMEZ RAMOS

187 function MRperp=MRvectoMRperp(MRvec,MR) % Función que transforma la matriz con los ejes principales de las articulaciones % en una matriz que contiene la perpicular 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([ ]); 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; if (rank(a)==1)&(rank(b)==2) %paralelos rectas=1; if (rank(a)==2)&(rank(b)==2) %se cortan rectas=3; if (rank(a)==2)&(rank(b)==3) %se cruzan rectas=4; 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

188 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 (i<n-2)&(cross(mrvec(i+1,[4 5 6]),MRvec(i+2,[4 5 6]))==[0 0 0]) % MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 MRvec(i+2,[4 5 6])]); % u(i,:)=simple(mrvec(i+2,[4 5 6])); %elseif i==n-1 % MRperp(i,:)=([MRvec(n,[4 5 6]) MRvec(i,[1 2 3]) 0 0 0]); % u(i,:)=simple(mrvec(i,[1 2 3])); elseif (i>1) 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])); if i==1 if MRvec(i,7)==2 MRperp(i,:)=([MRvec(i,[4 5 6]) MRvec(i,[1 2 3]) MRvec(i,7)]); 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)]); if rectas==1 % Paralelas q1_=simple(det([q w1;q w2;q w3;q4 v2 v3 0])/det([ w1; w2; 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([ w1; w2; 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([ w1; w2; 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)]); 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)]); if rectas==4 % Cruzan MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 u(i,:) MRvec(i,7)]); ÁLVARO GÓMEZ RAMOS

189 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 ÁLVARO GÓMEZ RAMOS

190 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 %Variables de traslacion x=0; % X inicial y=0; % Y inicial z=0; % Z inicial n=0; % Nº de piezas iniciales tras=1; %Nº de elem de traslacion+1 rotac=1; %Nº de elem de rotación+1 MR=sym([ ]); %Matriz del robot num_simb_rot=0; %nº de elementos de traslacion num_simb_rot2=0; %nº de elementos de rotacion Q1_simb=sym('q'); %Variables de traslacion Q1(tras)=0; %Valores de traslacion Q2_simb=sym('q'); %Variables de rotacion Q2(rotac)=0; %Valores de rotacion % lim de los ejes del plot principal xinf=-30; xsup=30; yinf=-30; ysup=30; zinf=0; zsup=40; ejes=[ ]; % Color de fondo set(hf1,'color',[.824,.824,.824]) % Habilita opciones del menu ÁLVARO GÓMEZ RAMOS

191 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',[ ]); hp1 = uipanel('backgroundcolor',[.824,.824,.824],... 'Position',[ ],'title','Piezas'); hp2 = uipanel('backgroundcolor',[.824,.824,.824],... 'Position',[ ],'title','Robot'); hp3 = uipanel('backgroundcolor',[.824,.824,.824],... 'Position',[ ],'title','Herramientas'); hp4 = uipanel('backgroundcolor',[.824,.824,.824],... 'Position',[ ],'title','traslacion'); hp5 = uipanel('backgroundcolor',[.824,.824,.824],... 'Position',[ ],'title','Rotación'); % Creación de Plots para la representación de las figuras sp1=subplot(5,3,[ ]); 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

192 % 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;x- 0.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=[ ; ; ; ; ; ]; subplot(sp2) patch('vertices',vertices1,'faces',caras1,'facevertexcdata',[.4,.4,.125],'facecolor','flat'); AXIS([ ]); 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=[ ; ; ; ; ; ]; subplot(sp3) patch('vertices',vertices2,'faces',caras2,'facevertexcdata',[1, 0, 0],'FaceColor','flat'); AXIS([ ]); 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;x- 0.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=[ ; ; ; ; ; ]; subplot(sp4) patch('vertices',vertices3,'faces',caras3,'facevertexcdata',[.8,.4,.125],'facecolor','flat'); axis xy AXIS([ ]); 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=[ ; ; ; ; ; ]; subplot(sp5) patch('vertices',vertices4,'faces',caras4,'facevertexcdata',[0, 0, 0],'FaceColor','flat'); axis xy AXIS([ ]); 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=[ ; ; ; ; ]; subplot(sp6) patch('vertices',vertices3,'faces',caras3,'facevertexcdata',[.0,.1,.0],'facecolor','flat'); axis xy AXIS([ ]); camorbit(55,-45) ÁLVARO GÓMEZ RAMOS

193 % Plot principal para dibujar el robot subplot(sp1) axis on AXIS([xinf xsup yinf ysup zinf zsup]); %Límite de ejes camorbit(165,0) % Cámara grid on camlight right camlight left lighting phong % 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',[ ],... 'CData',rotate_cdata,... 'Interruptible','off',... 'BusyAction','cancel'); %boton de ver ejes hejes = uicontrol('style','togglebutton',... 'Callback',@ver,... 'Tag','mypan',... 'Units','normalized',... 'Position',[ ],... 'CData',imgejes,... 'Interruptible','off',... 'BusyAction','cancel'); %boton de representar robot hbot = uicontrol('style','togglebutton',... 'Callback',@ver,... 'Tag','mypan',... 'Units','normalized',... 'Position',[ ],... 'CData',imgbot,... 'value',1,... 'Interruptible','off',... 'BusyAction','cancel'); %Boton de añadir traslacion htras = uicontrol('style','pushbutton',... 'Callback',@colocar1,... ÁLVARO GÓMEZ RAMOS

194 .03],... 'backgroundcolor',[.724,.724,.724],... 'String','Traslación',... 'Units','normalized',... 'Position',[ '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',[ ],... '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',[ ],... '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',[ ],... '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',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %longitud htexto = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... ÁLVARO GÓMEZ RAMOS

195 .03],... 'Position',[ 'Enable','on',... 'Interruptible','off',... 'String','10',... 'BusyAction','cancel'); hlabel = uicontrol('style', 'text',... 'Callback',@label,... 'String','longitud (dm)',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %rot z htextoz = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabelz = uicontrol('style', 'text',... 'Callback',@label,... 'String','Rot. en Z (º)',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); %rot x htextox = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabelx = uicontrol('style', 'text',... 'Callback',@label,... 'String','Rot. en X (º)',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); ÁLVARO GÓMEZ RAMOS

196 %Rot eje htextoe = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'String','0',... 'BusyAction','cancel'); hlabele = uicontrol('style', 'text',... 'Callback',@label,... 'String','Rot. eje (º)',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %Limite traslacion htextot = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'String','10',... 'BusyAction','cancel'); hlabelt = uicontrol('style', 'text',... 'Callback',@label,... 'String','Límite traslacion',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %nombre variable tras htextot1 = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','on',... 'Interruptible','off',... 'String','',... 'BusyAction','cancel'); hlabelt1 = uicontrol('style', 'text',... 'Callback',@label,... 'String','Nombre Var',... 'backgroundcolor',[ ],... 'Units','normalized',... ÁLVARO GÓMEZ RAMOS

197 .018],... 'Position',[ 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); hlabelt2 = uicontrol('style', 'text',... 'Callback',@label,... 'String','dm',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %limite rot - htextor1 = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'String','-360',... 'BusyAction','cancel'); hlabelr1 = uicontrol('style', 'text',... 'Callback',@label,... 'String','Límite Rot. -',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); %limite rot + htextor2 = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'String','360',... 'BusyAction','cancel'); hlabelr2 = uicontrol('style', 'text',... 'Callback',@label,... 'String','Límite Rot. +',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); ÁLVARO GÓMEZ RAMOS

198 %nombre var rot htextor3 = uicontrol('style', 'edit',... 'Units','normalized',... 'backgroundcolor',[1 1 1],... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'String','',... 'BusyAction','cancel'); hlabelr3 = uicontrol('style', 'text',... 'Callback',@label,... 'String','Nombre Var.',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); hlabelr4 = uicontrol('style', 'text',... 'Callback',@label,... 'String','º',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); hlabelr4 = uicontrol('style', 'text',... 'Callback',@label,... 'String','º',... 'backgroundcolor',[ ],... 'Units','normalized',... 'Position',[ ],... 'Enable','off',... 'Interruptible','off',... 'BusyAction','cancel'); % 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

199 MR=sym([ ]); %Pto inicial n=n+1; %nº de elem Q1(tras)=str2num(get(htexto,'String')); %valor variable longitud tras=tras+1; %nº elem tras MR(n,[2 3 4])=sym([x y z]); %pto inicial tramo [x y z x1 y1 z1]=traslac(x,y,z); %pto final MR(n,1)=1; %eje traslacion MR(n,[5 6 7])=[x y z]; %pto final tramos MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %rot eje % Colocar rotacion function varargout = colocar2(varargin) global MR Q2 n x y z htextoe rotac if n==0 MR=sym([ ]); %pto inicial n=n+1; %nº de elem Q2(rotac)=str2num(get(htextoe,'String')); %valor variable de rotacion rotac=rotac+1; %nº de elem de rotac [x y z]=eje(x,y,z); %nuevo pto final MR(n,1)=2; %eje rotac MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %rot eje % Colocar eslabon function varargout = colocar3(varargin) global MR n x y z htextoe if n==0 MR=sym([ ]); n=n+1; %nº de elementos MR(n,[2 3 4])=[x y z]; %pto inicial [x y z]=eslabon(x,y,z); %pto final MR(n,[5 6 7])=[x y z]; MR(n,1)=3; %tipo elem MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %rot eje % colocar base ÁLVARO GÓMEZ RAMOS

200 function varargout = colocar4(varargin) global MR n x y z htextoe if n==0 MR=sym([ ]); n=n+1; %nº de elementos MR(n,[2 3 4])=[x y z]; %pto inicial [x y z]=base(x,y,z); %pto final MR(n,[5 6 7])=[x y z]; MR(n,1)=4; %tipo de elem MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %rot eje % colocar garra function varargout = colocar5(varargin) global MR n x y z htextoe if n==0 MR=sym([ ]); n=n+1; %nº de elemento MR(n,[2 3 4])=[x y z]; %pto inicial [x y z]=garra(x,y,z); %pto final MR(n,[5 6 7])=[x y z]; MR(n,1)=5; %tipo de elem MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %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; % Habilitar o Deshabilitar ver ejes ÁLVARO GÓMEZ RAMOS

201 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,[ ]); xlabel('eje x'); ylabel('eje y'); axis on AXIS([ ]); camorbit(165,0) grid on if val1==1 plotejes(mrperp); %mostrar ejes si esta activado if val2==1 %si esta activada la opción de dibujar robot for i=1:n MRaux(i,:)=MR(i,:); %variable aux de MR 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)); MRaux=simple(MRaux); MR=MRaux; dibubot(mr); %dibujar robot ÁLVARO GÓMEZ RAMOS

202 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([ ]); % Nueva ventana hf2=figure('visible','on',... 'Units','normalized',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','on',... 'Name','Parámetros D-H',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[ ]); % 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

203 'Position',[ 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',[ tam- (1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); 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',[ 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',[ tam- (1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); 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

204 (1/(size(di,2)+1))],... 'Units','normalized',... 'Position',[ tam- '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',[ tam- (1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); 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',[ 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',[ tam- (1/(size(di,2)+1))],... 'Enable','on',... 'Interruptible','off',... 'BusyAction','cancel'); tam=tam-(1/(size(di,2)+1)); for i=1:size(alphai,2) P_D_H(i,:)=[alphai(i);ai(i);tethai(i);di(i)]; parametros de Denavit-Hartemberg %matriz con los % habilitación de nuevas opciones de los menus ÁLVARO GÓMEZ RAMOS

205 set(graph_robot,'enable','on') set(graph_cininv,'enable','on'); set(mfile_savedh,'enable','on'); ÁLVARO GÓMEZ RAMOS

206 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,[ ])=sym([ ]); %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)); %sustitucion de valores de rotacion for j=1:num_simb_rot2 MRejes_aux=subs(MRejes,Q2_simb(j),Q2(j)); %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 % Calculo los ejes y los dibujo for i=1:n ÁLVARO GÓMEZ RAMOS

207 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]); 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]); MRejes(i+1,[7 8 9])=cross(MRejes(i+1,[1 2 3]),MRejes(i+1,[4 5 6])); %eje y MRejes(i+1,[ ])=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)); %sustitucion de valores de rotacion for j=1:num_simb_rot2 MRejes_aux=subs(MRejes_aux,Q2_simb(j),Q2(j)); %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 MRejes=expand(MRejes); ÁLVARO GÓMEZ RAMOS

208 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)> mat(i,j)=0; ÁLVARO GÓMEZ RAMOS

209 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=[ ; ; ;-X1 -Y1 -Z1 1]; T_1=[ ; ; ;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; Rx=[ ;0 aux2 aux1 0;0 -aux1 aux2 0; ]; Rx_1=[ ;0 aux2 -aux1 0;0 aux1 aux2 0; ]; L=sqrt(A^2+B^2+C^2); Ry=[Lp/L 0 A/L 0; ;-A/L 0 Lp/L 0; ]; Ry_1=[Lp/L 0 -A/L 0; ;A/L 0 Lp/L 0; ]; Rz=[cos(-tetha) sin(-tetha) 0 0;-sin(-tetha) cos(-tetha) 0 0; ; ]; 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); ÁLVARO GÓMEZ RAMOS

210 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=[ ;0 cos(angx) sin(angx) 0;0 -sin(angx) cos(angx) 0; ]; Rx=redondear(Rx); Ry=[cos(angy) 0 sin(angy) 0; ;-sin(angy) 0 cos(angy) 0; ]; Ry=redondear(Ry); Rz=[cos(angy) sin(angy) 0 0;-sin(angy) cos(angy) 0 0; ; ]; 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

211 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; 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

212 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; if MRejes(i,13)==2 a(i-1)=1; cont2=cont2+1; 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); for j=1:num_simb_rot2 P_D_Haux=subs(P_D_Haux,Q2_simb(j),0); % creamos DH DH=P_D_Haux; for i=1:(size(dh,1)) DH(i,5)=a(i); ÁLVARO GÓMEZ RAMOS

213 %Guardamos [fname,pname] = uiputfile('*.mat','select File'); dbfile= strcat(pname,fname); if length(dbfile) == 0 return; save (fname,'p_d_h','dh') return ÁLVARO GÓMEZ RAMOS

214 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',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','off',... 'Name','Sicon-Bot',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','on',... 'BusyAction','cancel',... 'Position',[ ]); 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 D- H','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

215 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 D- H','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'); ÁLVARO GÓMEZ RAMOS

216 function sobre % Nos muestra una ventana con datos sobre el programa img = imread('sicon.jpg'); %imagen de fondo figure('visible','on',... 'Units','normalized',... 'Rerer','opengl',... 'Color',[1 1 1],... 'Resize','off',... 'Name','Sicon-Bot',... 'NumberTitle','off',... 'DockControls','off',... 'MenuBar','none',... 'Interruptible','off',... 'BusyAction','cancel',... 'Position',[ ]); subimage(img) axis off %nos muestra la imagen ÁLVARO GÓMEZ RAMOS

217 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. Depio 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)> %ángulo positivo angx=-(pi/2-asin((zf-zi)/l)); else angx=(pi/2-asin((zf-zi)/l)); %ángulo negativo % Calculamos el giro sobre el eje z a partir del pto inicial y % final. if (xi==xf)&(yi==yf) %si tramos 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 x-y V=[lx/l1 ly/l1 0]*l; %Proyeccion en eje XY xf1=v(1); yf1=v(2); zf1=v(3); angz=(pi/2-acos(xf1/l)); %ángulo de y % Situamos el punto final a una distancia L en vertical respecto % al punto inicial ÁLVARO GÓMEZ RAMOS

218 xf=xi; yf=yi; zf=zi+l; % Rotamos la figura con los valores adecuados [x1 y1 z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz); [x6 y6 z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz); [x8 y8 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); % 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[.4,.4,.125],'facecolor','flat'); return ÁLVARO GÓMEZ RAMOS

219 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; % 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

220 xf=xi; yf=yi; zf=zi+l; % Rotamos la figura con los valores adecuados [x1 y1 z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz); [x2 y2 z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz); [x3 y3 z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz); [x4 y4 z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz); [x5 y5 z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz); [x6 y6 z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz); [x7 y7 z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz); [x8 y8 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); %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

221 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=[ ; ; ; ; ; ]; subplot(sp1) patch('vertices',vertices,'faces',caras,'facevertexcdata',[.4,.4,.125],'facecolor','flat'); return ÁLVARO GÓMEZ RAMOS