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

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Prefacio: ........................................................................................................................ 4
BLOQUE 1: CONCEPTOS BÁSICOS................................... 6
1
CAPÍTULO: INTRODUCCIÓN A LA ROBÓTICA..................................... 7
1.1
1.2
1.3
1.4
1.5
Introducción. ...................................................................................................... 7
Historia de la robótica. ...................................................................................... 7
Bases de la robótica actual ................................................................................ 8
Definición de Robot............................................................................................ 9
Límites de la robótica actual........................................................................... 10
2
COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO. ................. 12
2.1
2.2
2.3
2.4
2.5
2.6
2.7
2.8
2.9
2.10
Introducción ..................................................................................................... 12
El Brazo Robot ................................................................................................. 12
Grados de libertad ........................................................................................... 12
Articulaciones................................................................................................... 14
Sistemas de coordenadas ................................................................................. 14
Volumen de trabajo ......................................................................................... 15
Actuadores ........................................................................................................ 16
El controlador................................................................................................... 18
Dispositivos sensoriales.................................................................................... 18
Elementos terminales....................................................................................... 21
3
CAPÍTULO: TRANSFORMACIONES 3D. ................................................. 23
3.1
3.2
3.3
3.4
3.5
3.6
Introducción ..................................................................................................... 23
Geometría 3D ................................................................................................... 23
Cambio de escala.............................................................................................. 25
Traslación ......................................................................................................... 25
Rotación ............................................................................................................ 25
Rotación alrededor de un eje arbitrario ........................................................ 26
4
CAPÍTULO: CINEMÁTICA. ........................................................................ 31
4.1
4.2
4.3
4.4
Introducción. .................................................................................................... 31
El problema Cinemático Directo .................................................................... 32
Algoritmo de Denavit-Hartenberg ................................................................. 33
Cinemática Inversa .......................................................................................... 37
5
CAPÍTULO: TOOLBOX DE ROBÓTICA................................................... 38
5.1
5.2
Introducción ..................................................................................................... 38
Representación de traslación 3D y orientación ............................................. 38
ÁLVARO GÓMEZ RAMOS.
-1-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
5.3
5.4
Cinemática directa ........................................................................................... 39
Cinemática inversa........................................................................................... 39
BLOQUE 2: DESARROLLO DEL SOFTWARE ................ 41
1
CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT. ............... 42
1.1
1.2
1.3
1.4
1.5
1.6
Introducción. .................................................................................................... 42
SICON-BOT. .................................................................................................... 43
Objetivos de la primera versión...................................................................... 43
Posibles mejoras futuras.................................................................................. 44
Requisitos mínimos. ......................................................................................... 44
Inicialización..................................................................................................... 45
2
CAPÍTULO: GRÁFICA DE SICON-BOT. .................................................. 46
2.1
2.2
2.3
2.4
Creación de ventana principal y menús desplegables. ................................. 46
Paneles y plots de representación ................................................................... 47
Eslabones y articulaciones............................................................................... 49
Botones, textos y cuadros editables. ............................................................... 54
3
CAPÍTULO: POSIBILIDADES DE SICON-BOT....................................... 56
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.10
Nuevo robot ...................................................................................................... 56
Calcular robot .................................................................................................. 58
Salvar Robot ..................................................................................................... 59
Cargar Robot.................................................................................................... 60
Parámetros D-H ............................................................................................... 62
Salvar D-H ........................................................................................................ 63
Cinemática directa ........................................................................................... 63
Cinemática inversa........................................................................................... 64
Posicionar Robot .............................................................................................. 66
Ayuda ................................................................................................................ 67
Herramientas.................................................................................................... 68
4
CAPÍTULO: PROGRAMACIÓN DE SICON-BOT.................................... 70
4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8
4.9
4.10
4.11
Introducción ..................................................................................................... 70
Inicialización de ventana inicial y menús desplegables ................................ 71
Entorno de creación de robots ........................................................................ 73
Añadir elemento de traslación ........................................................................ 76
Añadir elemento de rotación........................................................................... 78
Añadir elemento eslabón ................................................................................. 80
Añadir elemento base ...................................................................................... 81
Posicionar y dibujar por primera vez el elemento traslación ...................... 82
Posicionar y dibujar por primera vez el elemento rotación......................... 85
Posicionar y dibujar por primera vez el elemento eslabón .......................... 88
Posicionar y dibujar por primera vez el elemento base ............................... 91
ÁLVARO GÓMEZ RAMOS.
-2-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.12
4.12
4.13
4.14
4.15
4.16
4.17
4.18
4.19
4.20
4.21
4.22
4.23
4.24
4.25
4.26
4.27
4.28
4.28
4.29
4.30
4.31
4.32
4.33
Rotar punto respecto a ejes x e y que pasan por otro punto dado .............. 94
Rotar punto respecto a ejes dado por dos puntos ......................................... 96
Calcular robot .................................................................................................. 99
Obtención de los ejes de las articulaciones .................................................. 100
Obtención de “eje x”, “eje z” y punto de origen ......................................... 101
Obtención de “eje y”, normalizazión y dibujado de ejes............................ 106
Dibujar robot.................................................................................................. 107
Dibujar elemento eslabón.............................................................................. 108
Dibujar elemento traslación.......................................................................... 111
Dibujar elemento rotación ............................................................................ 114
Dibujar elemento base ................................................................................... 118
Salvar robot .................................................................................................... 119
Cargar robot................................................................................................... 119
Calculo de parámetros DH............................................................................ 120
Calculo de parámetros di ............................................................................... 123
Calculo de parámetros ai ............................................................................... 124
Calculo de parámetros θ i .............................................................................. 126
Calculo de parámetros α i .............................................................................. 126
Cinemática directa ......................................................................................... 127
Cinemática inversa......................................................................................... 129
Posicionar robot ............................................................................................. 134
Ayuda .............................................................................................................. 138
Función auxiliar: Multiplicar vectorialmente vectores simbólicos ........... 138
Función auxiliar: Redondear a cero los valores de las matrices ............... 138
BLOQUE 3: CONCLUSIONES ........................................... 139
1
2
Conclusiones. .................................................................................................. 140
Desarrollos futuros......................................................................................... 141
BLOQUE 4: ANEXOS .......................................................... 142
1
ANEXO: CÓDIGO FUENTE....................................................................... 143
ÁLVARO GÓMEZ RAMOS.
-3-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Prefacio:
Durante los estudios universitarios hemos recibido una amplia formación en
robótica, tanto teóricamente como en la práctica, aprendiendo a trabajar
matemáticamente con los robots o a programarlos para que realicen diversas tareas. En
el transcurso de esa formación hemos utilizado diversas herramientas informáticas que
nos ayudan en diferente grado a tratar con los robots.
Hoy en día la robótica se va extendiendo, y cada vez se generaliza más, con lo
cual es conveniente crear ciertas herramientas que permitan acercar a usuarios menos
expertos.
El programa SICON-BOT surge con el objetivo de facilitar a los usuarios
menos expertos el uso de aplicaciones informáticas, de hacer más cómodo el trabajo
para los más expertos e integrar en una sola herramienta, otras herramientas, para hacer
su uso más sencillo. En definitiva, SICON-BOT tiene tres objetivos principales:
FACILIDAD-COMODIDAD-INTEGRACIÓN
-
Facilidad: Con un entorno gráfico familiar, con diseño de robots en 3D, con
ventanas, menús desplegables, botones, y demás funciones gráficas que son
familiares a la mayoría de personas.
-
Comodidad: A la hora de integrar nuevas herramientas que nos permiten
obtener automáticamente parámetros necesarios para otras herramientas, que
hasta ahora teníamos que calcular a mano.
-
Integración: En un mismo software podemos utilizar funciones
pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se
resuelven en la programación interna.
En la primera versión del software SICON-BOT el objetivo es crear las bases de
la herramienta, para que poco a poco puedan ir ampliándose en un futuro por nuevos
usuarios interesados, para lo cual el código es accesible.
En resumen los objetivos de la primera versión es el desarrollo desde cero del
software, creando toda la programación necesaria para el entorno gráfico, el diseño de
robots en 3D, el cálculo de ejes, el cálculo de matrices DH, el cálculo de la cinemática
directa e inversa, y el posicionamiento del robot en 3D.
Con esto tenemos en la primera versión los tres objetivos fundamentales:
-
Facilidad: Con el entorno gráfico que incorpora, el diseño intuitivo de
robots, el cálculo automático de parámetros mediante simple inserción de
datos sencillos.
-
Comodidad: Púes realiza funciones como la obtención automática de los
ejes o de la matriz DH.
ÁLVARO GÓMEZ RAMOS.
-4-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
-
Integración: Pues incorpora internamente funciones del toolbox de robótica
como la cinemática directa e inversa.
ÁLVARO GÓMEZ RAMOS.
-5-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
BLOQUE 1: CONCEPTOS BÁSICOS
ÁLVARO GÓMEZ RAMOS.
-6-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1
CAPÍTULO: INTRODUCCIÓN A LA ROBÓTICA.
1.1
Introducción.
La Robótica describe todas las tecnologías asociadas con los robots. Sin
embargo la definición de robot entraña más dificultad, pues existen muchas
definiciones:
•
•
•
•
•
Ingenio mecánico controlado electrónicamente, capaz de moverse y ejecutar de
forma automática acciones diversas, siguiendo un programa establecido.
Máquina que en apariencia o comportamiento imita a las personas o a sus
acciones como, por ejemplo, en el movimiento de sus extremidades
Un robot es una máquina que hace algo automáticamente en respuesta a su
entorno.
Un robot es un puñado de motores controlados por un programa de ordenador.
Un robot es un ordenador con músculos.
Es cierto, como acabamos de observar, que los robots son difíciles de definir. Sin
embargo, no es necesariamente un problema el que no esté todo el mundo de acuerdo
sobre su definición. Quizás, Joseph Engelberg (padre de la robótica industrial) lo
resumió inmejorablemente cuando dijo: "Puede que no se capaz de definirlo, pero sé
cuándo veo uno".
La imagen del robot como una máquina a semejanza del ser humano, subyace en el
hombre desde hace muchos siglos, sin embargo, el robot industrial, que se conoce y
emplea en nuestros días, no surge como consecuencia de la tendencia o afición de
reproducir seres vivientes, sino de la necesidad.
Inmersos en la era de la informatización, la imperiosa necesidad de aumentar la
productividad y mejorar la calidad de los productos, ha hecho insuficiente la
automatización industrial rígida, dominante en las primeras décadas del siglo XX, que
estaba destinada a la fabricación de grandes series de una restringida gama de
productos. Hoy día, más de la mitad de los productos que se fabrican corresponden a
lotes de pocas unidades.
1.2
Historia de la robótica.
El concepto de robot se remonta casi al principio de la civilización, donde los
mitos hablan de seres mecánicos dotados de vida. En la civilización griega aparecen
figuras que se mueven mediante poleas y bombas hidráulicas y que se usan para
propósitos estéticos y artísticos. Tal es el caso del Hero's Automatic Theater and
Driver, una estatua de un dios alrededor de la cual rotaban pequeñas figuras de forma
periódica, o del mítico Coloso de Rodas, que defendía el puerto de la ciudad. No
obstante, la sociedad griega carecía de dos puntos básicos para un desarrollo
satisfactorio de robots: necesidad y tecnología.
ÁLVARO GÓMEZ RAMOS.
-7-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
La civilización árabe cubre el primero de estos puntos al concebir el robot como
un elemento para el confort del ser humano. Un ejemplo de este punto de vista
pragmático es la Fuente del Pavo Real, un sencillo dispositivo que medía el nivel del
agua vertida en un recipiente de forma que, al alcanzarse un cierto umbral, aparecía un
autómata portando una pastilla de jabón y, transcurrido un cierto tiempo, una toalla.
Durante el Renacimiento, los estudios de Leonardo da Vinci sobre anatomía del
cuerpo humano aportaron un valioso conocimiento a la hora de desarrollar la mecánica
del robot, principalmente los antropomorfos, permitiendo la construcción de junturas
mecánicas mucho mejores e impulsando por tanto el desarrollo modular de máquinas
complejas.
No fue hasta finales de 1800, que se contempla de forma científica el concepto
de autonomía e inteligencia artificial, cuando Nikola Tesla se propone crear una
máquina capaz de tomar sus propias decisiones sin necesidad de un telecontrol. Más
adelante, el escritor checo Karel Capek acuñaría el término robot como tal en su obra
Rossum’s Universal Robots, que se estrenó en Praga en 1921. El término deriva de la
palabra checa robota, que define un trabajo forzado o de carácter feudal y la obra
trataba de la deshumanización en una sociedad tecnológica. En 1926 Fritz Lang tocaría
el mismo tema, ésta vez con robots mecánicos, en la película Metrópolis.
La robótica como ciencia que estudia el robot y su uso se concebiría más tarde,
cuando en 1942 el escritor de origen ruso Isaac Asimov escribió el relato corto
Runaround y la recopilación posterior Yo, Robot. Desde entonces, los robots no sólo
han acaparado la atención de la ciencia ficción sino también la de un creciente número
de investigadores, presentándose como una excelente alternativa para todos aquellos
trabajos que por una u otra causa resultan indeseables para el ser humano.
1.3
Bases de la robótica actual
El inicio del desarrollo de la robótica actual puede fijarse en la industria textil
del siglo XVIII, cuando en 1801 Joseph Jacquard inventa una máquina textil
programable mediante tarjetas perforadas denominada Teal Programable y que se
produciría en masa. Conforme la Revolución Industrial progresaba, continuó el
desarrollo de estos agentes mecánicos, aunque el desarrollo de un verdadero robot no
fue posible hasta los años cuarenta, con la aparición de la computadora y la cada vez
mayor integración de los circuitos.
Los primeros robots industriales fueron los Unimates, desarrollados por George
Devol y Joe Engleberger. Las primeras patentes aparecen en 1946 y pertenecen a
Devol, por sus muy primitivos robots capaces de trasladar maquinaria de un lugar a
otro. También en 1946 aparece la primera computadora: J. Presper Ecker y John
Maulchy construyen el Eniac en la universidad de Pensilvania y la primera máquina
digital de propósito general se desarrolla en el MIT. Más tarde Devol, en 1954, diseña
el primer robot programable y acuña el término “Autómata universal” que
posteriormente recorta a unimation. Ese sería el nombre de la primera compañía de
robótica, fundado por Engleberger, que se considera el padre de la robótica.
ÁLVARO GÓMEZ RAMOS.
-8-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1.4
Definición de Robot
Al hablar de de robots se piensa en un dispositivo fijo al suelo o al techo, con
una cierta estructura mecánica que le confiere varios grados de libertad (generalmente
de aspecto antropomorfo); con un computador encargado del control de los
movimientos y cuya misión no va más allá de simple transportador de piezas, o a lo
sumo, y con la ayuda de herramientas terminales para la manipulación de algún
producto. Esta idea de la robótica abunda incluso en las definiciones más o menos
oficiales.
Según el instituto norteamericano Robot Industry Association (RIA) es la
siguiente:
-
“Se entiende por robot industrial a un manipulador multifuncional
reprogramable, diseñado para desplazar materiales, piezas, herramientas o
dispositivos especiales mediante movimientos programados variables que
permiten llevar a cabo tareas diversas”.
Esta definición se queda algo desfasada porque las tareas que puede realizar un
robot son más complejas que las recogidas.
La Swedish Industrial Robot Association (SWIRA) define un robot como:
-
“Una máquina manipuladora automáticamente controlada, reprogramable,
multi-propósito con o sin locomoción para uso en aplicaciones industriales
de automatización”.
Esta definición es mas completa pues incluye la posibilidad de movilidad y
generaliza las operaciones posibles, además está actualmente considerada por la Norma
ISO.
Japón tiene su propia definición más amplia:
-
“Un robot es todo aquel dispositivo mecánico con algún grado de libertad,
destinado a su utilización industrial como manipulador”.
La definición mas precisa es la aportada por la Asociación Francesa de
Normalización (AFNOR) aprobada en Agosto de 1983. AFNOR define en primer lugar
el manipulador y a continuación el robot industrial.
-
Manipulador: “Mecanismo compuesto generalmente de elementos en serie,
articulados o deslizantes entre sí, cuyo objetivo es el agarre y
desplazamiento de objetos siguiendo diversos grados de libertad. Es
multifuncional y puede ser mandado directamente por un operador humano
o por cualquier sistema lógico (levas, lógica neumática, lógica eléctrica
cableada o bien programado)”.
-
Robot industrial: “Manipulador automático, con servo sistemas de posición,
reprogramable, polivalente, capaz de posicionar y desplazar materiales,
piezas útiles o dispositivos especiales a lo largo de movimientos variables y
ÁLVARO GÓMEZ RAMOS.
-9-
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
programables para la ejecución de tareas variadas. Estas máquinas
polivalentes son generalmente concebidas para efectuar la misma función
de manera cíclica y pueden ser adaptados a otras funciones sin
modificación permanente del material”.
1.5
Límites de la robótica actual
Michael Knasel, director del Centro de Aplicaciones Robóticas de Science
Application Inc., explicó en la exposición Robots-8 en Detroit (1984) que los robots
deberían evolucionar durante cinco generaciones. Las dos primeras ya se habían
manifestado en los ochenta e incluían la gestión de tareas repetitivas, pero estaban muy
limitadas en cuanto a movimiento se refiere. La tercera generación incluiría visión
artificial, la cuarta movilidad avanzada en exteriores e interiores y la quinta entraría en
el dominio de la inteligencia artificial. Lamentablemente, si bien se han cumplido
algunas de estas expectativas, aún no se han conseguido modelos lo suficientemente
efectivos. Los límites de la robótica actual se pueden apreciar en contraste con las
expectativas previstas por Joseph Engelberger para la robótica de los ochenta:
-
Seis articulaciones de precisión entre la base del robot y el extremo de un
manipulador
Programación sencilla e intuitiva
Adaptación al medio mediante estímulos sensoriales
Margen de movimiento en rangos de 0.3 mm.
Capacidad de manipular pesos de hasta 150 Kg.
Control de movimiento punto a punto y de seguimiento de trayectorias
Sincronización con blancos móviles
Compatibilidad con ordenadores personales
Alta fiabilidad
Visión artificial que incluya al menos capacidad de orientación y
reconocimiento
Sensores táctiles
Coordinación de las distintas partes de la unidad
Capacidad de corrección de trayectorias en línea
Movilidad
Optimización de movimiento
Conservación de la energía
Apéndices de propósito general
Comunicación por voz
Seguridad de acuerdo a las leyes de Asimov de la robótica.
Engelberger aseguraba que los primeros diez puntos ya se habían conseguido a
principios de los ochenta. Los sistemas sensoriales por visión y tacto, si bien existen, se
limitan a aplicaciones muy básicas y específicas con bajo nivel de detalle debido a la
enorme carga computacional inherente al procesado de imagen. En lo que respecta al
movimiento, se progresa, aunque con menor precisión de la deseable debido a
imperfecciones, deslizamiento y problemas mecánicos en general. La energía necesaria
para operar una unidad robótica ha disminuido mucho desde el 79, y se han conseguido
unidades capaces de funcionar con baterías solares, como las TEN-TOMUSHI de
ÁLVARO GÓMEZ RAMOS.
- 10 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Sanyo. No obstante debe seguirse trabajando en este punto, especialmente en robots
destinados a desplazarse en exteriores y entornos hostiles, donde la autonomía del
móvil es un facto crítico. Respecto a la comunicación por voz, en tanto que existe la
compatibilidad con ordenadores personales y éstos soportan paquetes de
reconocimiento de voz distribuidos de forma comercial, es un problema resuelto de
forma cada vez más satisfactoria.
ÁLVARO GÓMEZ RAMOS.
- 11 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2
COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO.
2.1
Introducción
La mayoría de robots tienen los siguientes componentes comunes (ver figura
2.1), aunque la estructura mecánica, eléctrica, y computacional de un robot puede variar
apreciablemente:
-
2.2
Un manipulador o brazo (la unidad mecánica), incluyendo una garra o
elemento final.
Un controlador (El módulo “inteligente”).
Una unidad de potencia.
Un sistema sensorial.
El Brazo Robot
El brazo robot consiste en una serie de miembros rígidos, llamados elementos,
conectados por articulaciones. Al moverse una articulación se mueven también los
siguientes elementos unidos a ella. El movimiento se debe a un actuador, que bien
puede ir conectado directamente o a través de alguna transmisión mecánica (con el
objetivo de producir una par o velocidad o una ganancia). El último elemento con el
que termina el manipulador está diseñado para que pueda adaptarse una herramienta.
El manipulador podría dividirse en tres partes:
-
Los elementos mayores
Los elementos menores (componentes de la muñeca)
El elemento terminal (garra o herramienta)
Los elementos mayores lo forman los pares articulación-elemento, que sirven
para posicionar el extremo del brazo en un punto del espacio, y que generalmente son
los tres primeros.
Los elementos menores están asociados con la orientación del elemento final, y
junto con los elementos mayores se obtiene el posicionamiento y orientación del
elemento final.
El elemento final consiste en el mecanismo que permite desempeñar una
función al robot, ya sea una garra, soldador, taladro, etc.
2.3
Grados de libertad
Se denomina grados de libertad del sistema al número de coordenadas
independientes necesarias para expresar la posición de todas sus partes.
ÁLVARO GÓMEZ RAMOS.
- 12 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Para un elemento final típico de un robot, la garra o el punto central de la
herramienta puede ponerse donde se quiera en el espacio, para lo cual se necesitan tres
grados de libertad, y si se quiere poder orientar dicho punto en cualquier dirección
necesitaremos otros tres grados de libertad más. Por lo tanto se necesitan seis grados de
libertad para controlar el movimiento de cualquier elemento final con respecto a la base
del robot permitiendo alcanzar cualquier posible orientación y posición dentro del
espacio de trabajo del robot.
Los tres primeros grados de libertad, que definen la posición, se denominan
grados de libertad principales, y nos marcan el espacio de trabajo donde el robot puede
trabajar y se controla mediante los actuadores situados en el cuerpo del robot.
Los otros tres grados de libertad están localizados cerca del punto central de la
herramienta del robot y sirven como “muñeca” para controlar la garra u orientación de
la herramienta. Estos tres grados se denominan giro, elevación y guiñada.
Cuando añadimos una nueva articulación podemos estar añadiendo un nuevo
grado de libertad, pero esto no es siempre así, por eso cuando no ocurre se dice que
añade un grado de movilidad, que a veces es necesario para dotar de una movilidad
específica al brazo para poder evitar obstáculos situados dentro del campo de trabajo.
Al final los grados de libertad se basan en si se puede desplazar y rotar el robot en el eje
x, eje y, eje z.
Otra razón para añadir nuevas articulaciones que no añadan grados de libertad
adicionales, es debido a la limitación de rotación de las articulaciones rotativas, que
generalmente no pueden rotar 360º por restricciones mecánicas. Así añadiendo nuevos
grados de movilidad podemos compensar estas limitaciones.
ÁLVARO GÓMEZ RAMOS.
- 13 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2.4
Articulaciones
Existen cinco tipos de articulaciones disponibles, pero generalmente sólo se
usan dos tipos. Aquí podemos ver los tipos existentes y los grados de libertad que
tienen asociados.
2.5
Sistemas de coordenadas
Para mover el robot a cualquier punto hacen falta mínimo tres ejes, por lo que
existen cuatro tipos de sistemas de coordenadas para mover los robots según el tipo de
las articulaciones:
-
3 ejes traslación -> Sistema Cartesiano
2 ejes traslación + 1 eje rotación -> Sistema Cilíndrico
1 eje traslación + 2 ejes rotación -> Sistema Polar
3 ejes rotación -> Sistema Cartesiano
ÁLVARO GÓMEZ RAMOS.
- 14 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2.6
Volumen de trabajo
Es el espacio sobre la que el robot se puede mover, es decir el conjunto de
puntos donde se puede situar el extremo del robot. Las fronteras de este espacio solo
pueden alcanzarse con una orientación.
Normalmente para el cálculo del espacio del robot se tiene en cuenta el final de
la muñeca y no la herramienta, porque esta no tiene porque ser fija.
En definitiva, el volumen de trabajo del robot está determinado por las
siguientes características físicas:
- La configuración física del robot
- El tamaño de los componentes del cuerpo, brazo y muñeca
- Los límites de movimiento de las articulaciones del robot
ÁLVARO GÓMEZ RAMOS.
- 15 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2.7
Actuadores
Son los dispositivos que proporcionan la fuerza motriz para las articulaciones de
los robots. Pueden ser de tres tipos según la fuente de energía: Eléctricos, Hidráulicos y
Neumáticos.
Los eléctricos son cada vez más empleados, pues tienen una gran
controlabilidad con un mínimo mantenimiento. Dentro de las diferentes variedades, los
más comunes son los servomotores de corriente continua, los motores paso a paso y los
servomotores de corriente alterna.
Los actuadores hidráulicos están accionados por un fluido en movimiento,
generalmente aceite a presión. Se asocia generalmente con robots grandes. Sus ventajas
principales es que proporcionan mayor velocidad y fuerza, pero a cambio hacen el
robot más grande y además pueden existir fugas de aceite.
ÁLVARO GÓMEZ RAMOS.
- 16 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
El accionamiento neumático hace uso de un medio fluido que es altamente
compresible. Este fluido suele ser aire que está fácilmente disponible y no es
inflamable. Este tipo de accionamiento está generalmente reservado para robots
pequeños que poseen menos grados de libertad. Suelen estar limitados para operaciones
de recogida y depósito con ciclos rápidos. Pueden ser fácilmente adaptados al
accionamiento de dispositivos de pistón para proporcionar un movimiento de
translación de articulaciones deslizantes. También pueden ser usados en actuadores
rotatorios para articulaciones cilíndricas.
ÁLVARO GÓMEZ RAMOS.
- 17 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2.8
El controlador
El controlador es el dispositivo que proporciona la inteligencia necesaria para
que actúe conforme al operador. Esencialmente consiste en:
-
2.9
Una memoria para almacenar datos como posiciones prefijadas o el propio
programa
Un secuenciador que interpreta los datos de programa de la memoria.
Una CPU que realiza los cálculos necesarios con los datos obtenidos con el
secuenciador.
Una interfaz que obtenga datos mediante sensores o sistemas de visión.
Una interfaz que mande la información de movimiento a los actuadores.
Una interfaz para equipos auxiliares, usado para comunicarse con otras
unidades externas o dispositivos de control.
Un dispositivo que permita aprender posiciones al robot u operaciones en
forma de consola o botonera de enseñanza.
Dispositivos sensoriales
Los sensores utilizados en robótica pueden dividirse en dos clases:
· Sensores internos: Son los dispositivos usados para medir la posición, velocidad o
aceleración de las articulaciones del robot y/o del elemento final. Con esta información
tratará más adelante el sistema de control para obtener el movimiento deseado. Pueden
ser de diferentes clases:
-
Potenciómetros: Es un elemento resistivo de valor máximo (R) que varía su
valor según la posición de giro o el desplazamiento, con lo cual, midiendo su
tensión inicial y su tensión final podemos saber en que posición está el
robot, debido a la relación:
r
Vout = Vin .
R
ÁLVARO GÓMEZ RAMOS.
- 18 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
-
Synchro: Es transductor rotacional que convierte desplazamientos angulares
en voltaje de corriente alterna. Se basa en la fórmula:
Vout (t ) = Vm sin(θ ) sin(ω ac t )
-
Resolvers: Es una forma especial de synchro por lo que a veces se le conoce
como synchro-resolver. La mayor diferencia es que las bobinas del estator y
del rotor están desplazadas mecánicamente 90º una respecto a otra en lugar
de 120º como en el caso del synchro. Con el rotor excitado por un voltaje
portador de alterna Vm sin(ω ac t ) , los dos voltajes de estator son:
V2 (t ) = Vm sin(θ ) sin(ω ac t )
V1 (t ) = Vm cos(θ ) sin(ω ac t )
- Codificadores ópticos: Realizan tareas de determinación de posición con
relativa facilidad y sorprendente precisión, además de no necesitar contacto para
obtener la medida. Pueden ser de 2 tipos:
o Codificadores ópticos absolutos: Es capaz de memorizar la posición
siempre, incluso recién conectado, por lo que no necesita ningún
ciclo de calibración
ÁLVARO GÓMEZ RAMOS.
- 19 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
o Codificadores ópticos incrementales: Sólo son capaces de medir
incrementos o decrementos sobre la posición actual, con lo que si no
se conoce la posición actual no es posible conocer las futuras
posiciones. Necesita de un ciclo de calibración para obtener una
posición inicial sobre la que ir midiendo los cambios.
+
-
Tacómetros: Se aprovecha la variación de una tensión DC que crean los
motores al variar sus velocidades para obtener precisamente la velocidad de
giro.
· Sensores externos: Son dispositivos usados para monitorizar la relación geométrica
y/o dinámica con su tarea, entorno, o los objetos que está manejando. Algunos ejemplos
de estos sensores son:
-
Dispositivos de proximidad: Nos indican si existen objetos cerca del sensor,
e incluso, a que distancia se encuentran. Existen muchas tecnologías que
ÁLVARO GÓMEZ RAMOS.
- 20 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
hagan está labor: sensores de luz reflejada, sensores de ultrasonidos,
sensores de barrido láser, etc.
-
Sensores de fuerza y par: Nos indican la fuerza que se está aplicando sobre
un objeto.
2.10 Elementos terminales
Es el elemento que dota de utilidad al robot, y por tanto es la parte más crítica
de diseñar. Sus características más deseadas son:
-
Diseño simple.
Tamaño pequeño y poco peso.
Capacidad para agarrar firmemente las piezas.
Autocentrado
Una configuración de sujeción habitual.
ÁLVARO GÓMEZ RAMOS.
- 21 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Existen muchos métodos para aportar energía al elemento terminal de un robot,
como por ejemplo, cilindros de aire comprimido, electroimanes, o motores eléctricos.
· Dispositivos de aire comprimido: Es el método más utilizado debido a que son
potentes y pequeños. Entre sus ventajas cabría destacar su pequeño tamaño y peso con
relación a su potencia. También cabe destacar que no son caros y tienen buena
fiabilidad. Entre sus desventajas es que hacen falta cilindros de aire comprimido o un
compresor que encarece el producto. También pueden tener fugas o tener un respuesta
muy lenta en circuitos de aire demasiado largos.
· Cilindros hidráulicos: Se usan cuando hace falta fuerzas extremadamente altas para
su tamaño, además de muy buenos amortiguamientos y tiempo de respuesta frente a los
circuitos largos. Pero hace falta una bomba hidráulica para cada dispositivo hidráulico
utilizado, además de ser susceptible de fugas.
ÁLVARO GÓMEZ RAMOS.
- 22 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3
CAPÍTULO: TRANSFORMACIONES 3D.
3.1
Introducción
Algunas aplicaciones gráficas son bidimensionales: dibujos y gráficos, algunos mapas y
creaciones artísticas pueden ser entidades estrictamente bidimensionales. Pero vivimos
en un mundo tridimensional, y en muchas aplicaciones de diseño debemos manejar y
describir objetos tridimensionales. Si un arquitecto desease ver el aspecto real de la
estructura, entonces un modelo tridimensional le permitiría observarla desde diferentes
puntos de vista. Un diseñador de aviones podría desear analizar el comportamiento de
la nave bajo fuerzas y tensiones tridimensionales. En este caso se necesita también una
descripción tridimensional. Algunas aplicaciones de simulación, como el aterrizaje de
un avión, también exigen una definición tridimensional del mundo.
En este capítulo generalizaremos nuestro sistema para manejar modelos de
objetos tridimensionales. Extenderemos las transformaciones que hemos visto para
permitir traslaciones y rotaciones en el espacio tridimensional. Ya que la superficie de
visualización es sólo bidimensional, debemos considerar la forma de proyectar nuestros
objetos en esta superficie plana para formar la imagen. Se discutirán tanto las
proyecciones paralelas como perspectivas.
3.2
Geometría 3D
Empecemos nuestra discusión sobre las tres dimensiones revisando los métodos
de la geometría analítica para definir objetos. El objeto más sencillo es, por supuesto,
un punto. Como en el caso de las dos dimensiones, podemos definir un punto
estableciendo un sistema de coordenadas y listando las coordenadas del punto.
Necesitamos un eje adicional de coordenadas para la tercera dimensión. Podemos
disponer este tercer eje para que forme ángulo recto con los otros dos.
Podemos utilizar este nuevo sistema de coordenadas para definir la ecuación de
una recta. En dos dimensiones teníamos:
Mientras que en el caso tridimensional, son necesarias un par de ecuaciones
Son necesarias las coordenadas de dos puntos para construir estas ecuaciones
y
ÁLVARO GÓMEZ RAMOS.
. Por lo tanto, todavía son necesarios dos puntos para definir
- 23 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
una recta, como cabría esperar. De forma paramétrica, donde cada variable se expresa
en función de un parámetro u:
Nos gustaría también trabajar con planos. Un plano, de forma general, se
especifica mediante una sola ecuación de la forma:
Cabe señalar que si dividimos por A la ecuación anterior tenemos
Esto significa que sólo son necesarias tres constantes para definir un plano. Las
ecuaciones para un plano en particular pueden ser determinadas si conocemos las
coordenadas de tres puntos (que no estén alineados)
,
y
. Podemos determina la ecuación de la siguiente manera, ya que cada punto
del plano debe satisfacer la ecuación del plano
Tenemos así tres ecuaciones con tres incógnitas que podemos resolver con la
ayuda de un poco de álgebra.
Otra forma de especificar un plano es mediante un único punto que pertenezca
al plano
y la dirección perpendicular al plano
recibe el nombre de vector normal. Por otro lado, si
. Este vector
es un punto genérico del
plano, entonces
es un vector de ese plano. Si tenemos en cuenta
que estos dos vectores forman un ángulo recto, su producto escalar será nulo:
Esta expresión es válida cuando
esta es la ecuación de ese plano.
ÁLVARO GÓMEZ RAMOS.
es un punto del plano, por lo tanto,
- 24 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.3
Cambio de escala
Ahora que sabemos expresar puntos, rectas y planos en tres dimensiones,
consideremos los algoritmos que permiten al usuario modelar objetos tridimensionales.
Empezaremos por generalizar las transformaciones bidimensionales que vimos
en el capítulo anterior. Así, por lo que respecta al cambio de escala, la tercera
coordenada que hemos introducido pueden tener su propio facto de escala, por lo tanto
será necesaria una matriz
,
o de rango
3.4
si empleamos coordenadas homogéneas
Traslación
Igual que en el caso bidimensional, empleamos los elementos de la fila inferior de la
matriz de transformación en coordenadas homogéneas para reflejar la traslación.
3.5
Rotación
Cuando consideramos la rotación de un objeto en dos dimensiones, vimos la
matriz de rotación alrededor del origen
Podemos generalizar este concepto a una rotación tridimensional alrededor del
eje z.
ÁLVARO GÓMEZ RAMOS.
- 25 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
En esta rotación, pensamos en el eje como fijo mientras que algún objeto se
mueve en el espacio. Podemos pensar también que el objeto permanece inmóvil
mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la
dirección de la rotación. Fijar el eje y girar el objeto en sentido antihorario es lo mismo
que fijar el objeto y mover el eje en sentido horario.
La rotación alrededor de los ejes x e y se realiza con una matriz de
transformación similar a la anterior.
Para hacer una rotación alrededor del eje y, del tal forma que el eje z se
convierte en el x realizamos la siguiente matriz en coordenadas homogéneas
3.6
Rotación alrededor de un eje arbitrario
Para que las transformaciones tridimensionales nos sean un poco más
familiares, resolvamos un sencillo problema. Aunque hemos visto matrices de
transformación que nos permiten girar cualquier objeto alrededor de uno de los ejes de
coordenadas, en general cualquier recta del espacio puede servir como eje de rotación.
El problema consiste en establecer la matriz de rotación para un ángulo j alrededor de
una recta cualquiera. Construiremos esta transformación a partir de las que ya
conocemos. Moveremos primero el origen hasta que esté situado sobre la recta. Luego
realizaremos rotaciones alrededor de los ejes x e y para alinear el eje z con la recta. De
esta forma la rotación alrededor de la recta se convierte en un giro alrededor del eje z.
Finalmente, realizaremos transformaciones inversas a la realizadas en los giros
alrededor de los ejes x e y para deshacer, por último, la traslación del origen a su
posición original.
Lo primero que debemos hacer es escoger una representación adecuada de la
recta que será el eje de rotación. Un punto de dicha recta, así como la dirección de la
ÁLVARO GÓMEZ RAMOS.
- 26 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos
ofrece la información necesaria para la traslación del origen, y la dirección nos informa
de los ángulos de rotación correctos para alinear la recta con el eje z.
Dada la recta:
Un punto de esta recta es
vector
y su dirección está especificada por el
.
La matriz de transformación que traslada el origen al eje de giro será:
Necesitaremos también la inversa de traslación para devolver el origen a su
posición original una vez que las rotaciones se hayan completado.
El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos
realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el
ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y
consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta comprendido entre el origen
y
está en la
dirección del eje arbitrario de rotación, y todo el segmento está situado sobre dicho eje.
Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un
vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende
desde
hasta
ÁLVARO GÓMEZ RAMOS.
.
- 27 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Si ahora realizamos un giro alrededor del eje x hasta que nuestro eje arbitrario
quede en el plano xz, el segmento proyectado quedará alineado con el eje z. Cuanto vale
el ángulo q ?
Sabemos que la longitud del segmento proyectado es
y, teniendo en cuenta la figura anterior, es fácil deducir las razones trigonométricas del
ángulo q
Por lo tanto, la matriz de transformación para el giro alrededor del eje x será
ÁLVARO GÓMEZ RAMOS.
- 28 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
La transformación inversa es una rotación de igual magnitud pero en sentido
contrario. Como sabemos, cambiar el signo del ángulo significa cambiar el signo de los
senos y conservar el de los cosenos.
La rotación alrededor de este eje no ha modificado la coordenada x. Sabemos
también que la longitud total del segmento
no varía. La coordenada z será entonces
Necesitamos realizar ahora un giro de ángulo f alrededor del eje y para que el
segmento quede alineado con el eje z.
Sus razones trigonométricas son
ÁLVARO GÓMEZ RAMOS.
- 29 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
La matriz de rotación en este caso será
La inversa de esta transformación será
Es posible ahora realizar el giro j alrededor del eje escogido. Ya que este eje
está alineado con el eje z, la matriz correspondiente a esta rotación será
De esta forma ya somos capaces de realizar una rotación j alrededor de un eje
arbitrario, mediante el producto de las transformaciones anteriores
ÁLVARO GÓMEZ RAMOS.
- 30 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4
CAPÍTULO: CINEMÁTICA.
4.1
Introducción.
La cinemática del brazo del robot trata con el estudio analítico de la geometría
del movimiento de un brazo de robot con respecto a un sistema de coordenadas de
referencia fijo sin considerar las fuerzas o momentos que originan el movimiento. Así,
la cinemática se interesa por la descripción analítica del desplazamiento espacial del
robot como una función del tiempo, en particular de las relaciones entre la posición de
las variables de articulación y la posición y orientación del efecto final del brazo del
robot.
Hay dos problemas fundamentales en la cinemática del robot. El primer
problema se suele conocer como el problema cinemático directo, mientras que el
segundo es el problema cinemático inverso. Como las variables independientes en un
robot son las variables de articulación, y una tarea se suele dar en términos del sistema
de coordenadas de referencia, se utiliza de manera más frecuente el problema
cinemático inverso. Denavit y Hartenberg en 1955 propusieron un enfoque sistemático
y generalizado de utilizar álgebra matricial para describir y representar la geometría
espacial de los elementos del brazo del robot con respecto la un sistema de referencia
fijo. Este método utiliza una matriz de transformación homogénea 4 x 4 para describir
la relación espacial entre dos elementos mecánicos rígidos adyacentes y reduce el
problema cinemático directo a encontrar una matriz de transformación homogénea 4 x
4 que relaciona el desplazamiento espacial del sistema de coordenadas de la mano al
sistema de coordenadas de referencia. Estas matrices de transformación homogéneas
son también útiles en derivar las ecuaciones dinámicas de movimiento del brazo del
robot. En general, el problema cinemático inverso se puede resolver mediante algunas
técnicas. Los métodos utilizados más comúnmente son el algebraico matricial, iterativo
o geométrico.
· Cinemática Directa (ángulos para encontrar posición):
Se conoce:
a) La longitud de cada eslabón.
b) El ángulo de cada articulación.
Se busca: La posición de cualquier punto (coordenadas con respecto a la base)
· Cinemática Inversa (posición para encontrar ángulos):
Se conoce:
a) La longitud de cada eslabón.
b) La posición de cualquier punto (coordenadas con respecto a la base).
Se busca: El ángulo de cada articulación necesitado para obtener la posición.
ÁLVARO GÓMEZ RAMOS.
- 31 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.2
El problema Cinemático Directo
El problema cinémático directo se reduce a encontrar la matriz de
transformación homogénea (T) que relacione la posición y orientación del extremo del
robot respecto a su sistema de referencia fijo (base del robot). La matriz T está en
función de los parámetros de las articulaciones del robot. Para un robot de n grados de
libertad tenemos:
x=
y=
z=
α=
β=
γ =
f x (q1
f y (q1
f z (q1
f α (q1
f β (q1
f γ (q1
q2
q2
q2
q2
q2
q2
...
...
...
...
...
...
qn )
qn )
qn )
qn )
qn )
qn )
Donde:
q1...n = Son las variables de las articulaciones. Para articulaciones de rotación las
variables son ángulos. Para articulaciones prismáticas las variables son distancias.
x, y, z = Coordenadas de la posición del robot
α , β , γ = Ángulos de orientación de la muñeca
Las funciones mencionadas pueden ser encontradas mediante métodos
geométricos para el caso de robots de 2 grados de libertad (cada relación articulacióneslabón constituye un grado de libertad):
ÁLVARO GÓMEZ RAMOS.
- 32 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
x = l1 cos θ1 + l 2 cos(θ1 + θ 2 )
y = l1 senθ1 + l 2 sen(θ1 + θ 2 )
Para robots de más de 2 grados de libertad es difícil aplicar métodos
geométricos para la solución de su cinemática directa.
A cada eslabón se le asocia un sistema coordenado y utilizando
transformaciones homogéneas es posible representar las rotaciones y traslaciones
relativas entre los diferentes eslabones que componen el robot. Siendo la matriz:
Aii−1
Siendo i el nº de eslabón
La matriz de transformación homogénea que representa la posición y
orientación relativa entre los sistemas asociados a dos eslabones consecutivos del robot.
Se puede representar de forma parcial o total la cadena cinemática que forma el
robot:
n
A0n = ∏ Aii−1
i =1
Para el caso de un robot de 6 ejes, su cadena cinemática queda representada por
la siguiente matriz de transformación homogénea:
T = A06 = A01 • A12 • A23 • A34 • A45 • A56
4.3
Algoritmo de Denavit-Hartenberg
En 1955 Denavit y Hartenberg propusieron un método matricial que permite
establecer de manera sistemática un sistema de coordenadas. La representación de
Denavit-Hartenberg (D-H) establece que seleccionándose adecuadamente los sistemas
de coordenadas asociados a cada eslabón, será posible pasar de uno al siguiente
mediante 4 transformaciones básicas que dependen exclusivamente de las
características geométricas del eslabón.
Reduciéndose al siguiente patrón de transformaciones que permiten
relacionar el sistema de referencia del elemento i con respecto al sistema del
elemento i-1:
-
Rotación alrededor del eje z i −1 un ángulo θ i
Traslación a lo largo de z i −1 una distancia d i
Traslación a lo largo de x1 una distancia ai
Rotación alrededor del eje x1 un ángulo α i
ÁLVARO GÓMEZ RAMOS.
- 33 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Aii−1 = T ( z ,θ i )T (0,0, d i )T (ai ,0,0)T ( x, α i )
Desarrollando la expresión:
Aii−1
⎡cos θ i
⎢ senθ
i
=⎢
⎢ 0
⎢
⎣ 0
− senθ i
cos θ i
0
0
0
0
1
0
0 ⎤ ⎡1
0⎥⎥ ⎢⎢0
0⎥ ⎢0
⎥⎢
1 ⎦ ⎣0
0
1
0
0
0 0 ⎤ ⎡1
0 0 ⎥⎥ ⎢⎢0
1 d i ⎥ ⎢0
⎥⎢
0 1 ⎦ ⎣0
0
1
0
0
0 a i ⎤ ⎡1
0
⎥
⎢
0 0 ⎥ ⎢0 cos α i
1 0 ⎥ ⎢0 senα i
⎥⎢
0 1 ⎦ ⎣0
0
0⎤
0⎥⎥
0⎥
⎥
1⎦
0
− senα i
cos α i
0
Obtenemos la expresión general de DH, donde θ i , d i , ai , α i son los parámetros
DH del eslabón i:
Aii−1
⎡cos θ i
⎢ senθ
i
=⎢
⎢ 0
⎢
⎣ 0
− cos α i senθ i
cos α i cos θ i
senα i
0
senα i senθ i
− senα i cos θ i
cos α i
0
ai cos θ i ⎤
ai senθ i ⎥⎥
di ⎥
⎥
1 ⎦
Para que la matriz Aii−1 relacione los sistemas coordenados Oi y Oi −1 es
necesario que los sistemas coordenados se determinen mediante los siguientes pasos:
1. Numerar y etiquetar el eslabón fijo (base) como 0.
2. Numerar y etiquetar los eslabones móviles desde 1 hasta el n eslabón móvil.
3. Localizar y numerar el eje de cada articulación y etiquetarla comenzando desde Z 0
hasta Z n −1 . Si la articulación es rotativa, el eje será su propio eje de giro. Si la
articulación es prismática, el eje será a lo largo del cual se produce el desplazamiento.
Establecimiento del sistema coordenado de la base:
4. Establecer el sistema coordenado de la base estableciendo el origen como O0
en cualquier punto del eje Z 0 . Arbitrariamente establecer los ejes X 0 , Y0
respetando la regla de la mano derecha.
Establecimiento de los sistemas coordenados de las demás articulaciones:
5. Localizar el origen Oi :
a) En la intersección del eje Z i con la línea normal común a la
intersección de Z i y Z i −1 .
b) En la intersección de Z i y Z i −1 , si es que Z i y Z i −1 se intersectan.
c) En la articulación i, si Z i y Z i −1 son paralelos.
ÁLVARO GÓMEZ RAMOS.
- 34 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
6. Establecer X i :
a) A lo largo de la línea normal común entre los ejes Z i y Z i −1 que pasa
por Oi .
b) En la dirección normal al plano formado por Z i y Z i −1 , si es que estos
dos ejes se intersectan.
7. Establecer Yi de acuerdo a la regla de la mano derecha.
Establecimiento del sistema coordenado de la herramienta:
8. Localizar el sistema coordenado n-ésimo en el extremo del robot. Si es una
articulación rotacional, establecer Z n a lo largo de la dirección Z n −1 y establecer
el origen On de la manera que más convenga a lo largo de Z n , preferente en el
centro de la pinza o la punta de cualquier herramienta que el robot tenga
montada.
9. Establecer X n y Yn de acuerdo a la regla de la mano derecha. Si la
herramienta es una pinza, es común establecer el eje Yn entre los “dedos” de la
pinza y X n será ortonormal a Z n y Yn .
Obtener las Matrices de Transformación Homogéneas
10. Crear una tabla con los parámetros D-H de los eslabones:
Eslabón i
θi
ÁLVARO GÓMEZ RAMOS.
di
ai
αi
- 35 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Donde:
θ i = Es el ángulo formado por los ejes X i y X i −1 medido en un plano
perpendicular a Z i −1 utilizando la regla de la mano derecha. Este es un
parámetro variable en articulaciones rotatorias.
d i = Es la distancia a lo largo del eje Z i −1 desde el origen Oi −1 hasta la
intersección del eje X i con el eje Z i −1 . Este es un parámetro variable en
articulaciones prismáticas.
ai = Para articulaciones rotatorias: es la distancia a lo largo del eje X i
desde el origen Oi hasta la intersección del eje X i con el eje Z i −1 .
α i = Es el ángulo formado por los ejes Z i y Z i −1 medido en un plano
perpendicular al eje X i utilizando la regla de la mano derecha.
11. Realizar la matriz D-H de transformación homogénea Aii−1 para cada
eslabón de acuerdo a los datos de la tabla del punto anterior.
12. Obtener la matriz de transformación que relacione el sistema coordenado
de la base con el sistema coordenado del extremo del robot, resultando en
la posición y orientación del sistema coordenado de la herramienta
expresado en coordenadas de la base.
n
T = A = ∏ Aii−1
n
0
i =1
ÁLVARO GÓMEZ RAMOS.
- 36 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.4
Cinemática Inversa
El objetivo del problema cinemático inverso consiste en encontrar los valores
que deben tomar las variables articulares del robot para que su extremo se posicione y
oriente según una determinada localización espacial. La ecuación matemática que
representa lo anterior es:
q k = f k ( x, y , z , α , β , γ )
K = 1...n
Donde:
q1...k = Son las variables de las articulaciones. Para articulaciones
revolutas las variables son ángulos. Para articulaciones prismáticas las
variables son distancias.
x, y, z = Coordenadas de la posición del extremo del robot.
α , β , γ = Ángulos de la orientación del extremo del robot.
n = Número de grados de libertad
A diferencia del problema cinemático directo donde de una manera sistemática e
independiente de la configuración del robot se llega a una solución, en el problema
cinemático inverso el mecanismo de solución es fuertemente dependiente de la
configuración y con frecuencia la solución no es única.
Normalmente los métodos geométricos nos permiten obtener normalmente los
valores de las primeras variables, que son las que consiguen posicionar el extremo del
robot en un punto determinado.
También es posible recurrir a manipular directamente a las ecuaciones obtenidas
del problema cinemático directo.
En muchos robots de 6 grados de libertad es posible aplicar acoplamiento
cinemático, para que los ejes dedicados al posicionamiento y los ejes dedicados a la
orientación, sean tratados como dos problemas independientes.
ÁLVARO GÓMEZ RAMOS.
- 37 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
5
CAPÍTULO: TOOLBOX DE ROBÓTICA.
5.1
Introducción
El toolbox de robótica permite al usuario crear y manipular fácilmente tipos de
datos fundamentales para la robótica, como pueden ser las transformaciones
homogéneas, cuaternios y trayectorias. Incluye funciones para la cinemática directa e
inversa, así como la dinámica.
Se basa en un método general de representación de cinemática y dinámica de
manipuladores a través de matrices. Estas comprenden, en el caso más simple, los
parámetros de Denavit-Hartenberg del robot y pueden ser creadas por el usuario para
cualquier manipulador. La descripción del manipulador puede ser mejorada,
aumentando la matriz, para incluir parámetros inerciales de los eslabones, y parámetros
de inercia y fricción de los motores.
5.2
Representación de traslación 3D y orientación
En coordenadas cartesianas la traslación puede ser representada por un vector de
posición. Muchas representaciones de orientaciones en 3D han sido propuestas, pero la
mas común en robótica son las matrices de rotación ortonormales. Una transformación
homogénea es una matriz 4x4 que representa traslación y orientación y que además
encaja muy bien con matlab, debido a la capacidad de manipulación de matrices de
este.
Las transformaciones homogéneas describen la relación entre la traslación y la
rotación
Por ejemplo, para una traslación T = transl(0.5, 0.0, 0.0), una rotación en y T =
roty(pi/2) y una rotación en z T = rotz(pi/2):
ÁLVARO GÓMEZ RAMOS.
- 38 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
5.3
Cinemática directa
La cinemática directa consiste en resolver la posición cartesiana y la orientación
del efector final dado el valor de las articulaciones y conociendo la estructura del robot
a través de la matriz de Denavit-Hartenberg.
Existe una función en el toolbox para resolver la cinemática directa a través de
la matriz DH, es la función fkine.
FKINE(DH, Q) calcula la cinemática directa de una manipulador para un
estado dado por el vector de variables articulares Q. Dicho manipulador quedará
descrito por su matriz de parámetros de Denavit-Hartenberg (DH).
Es posible utilizar esta función de dos modos:
a) Si Q es un vector, entonces es interpretado como el vector de variables
articulares para el que se pretende calcular el modelo directo, y FKINE
devuelve la transformación homogénea T correspondiente al último enlace
del manipulador. En este caso Q deberá ser un vector fila.
b) Si Q es una matriz, cada fila será interpretada como un vector de variables
articulares, y T será una matriz tridimensional 4x4xm, siendo m el número de
filas de Q. T es lo que se denominará una trayectoria de transformaciones
homogéneas. Para extraer la transformación homogénea (Ti) que hay
"condensada" en la fila i-ésima de la matriz T, bastará con escribir:
TI = T(:,:,i)
Empleando la función FKINE según este último modo, es posible obtener todas
las transformaciones homogéneas correspondientes a una trayectoria dada en el espacio
de variables articulares.
5.4
Cinemática inversa
La cinemática inversa consiste en encontrar los valores articulares que me
posicionan al robot en una determinada posición.
El toolbox incluye una función para resolver la cinemática inversa de los robots,
es la función ikine.
Q = IKINE(DH, STOL, ILIMIT, T)
Q = IKINE(DH, STOL, ILIMIT, T, Q0)
Q = IKINE(DH, STOL, ILIMIT, T, Q0, M)
Devuelve los valores de las variables articulares necesarios para que el efector
final del manipulador tenga la posición y orientación dadas por la transformación T. La
solución del problema cinemático inverso no es única en general, y es posible que para
ÁLVARO GÓMEZ RAMOS.
- 39 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
una misma orientación y posición deseadas, se obtengan soluciones distintas en función
del vector inicial de variables articulares (Q0) que se le pase a IKINE.
Es posible usar la función para que devuelva las variables articulares correspondientes a
una sola posición y orientación, o bien a una trayectoria de posiciones y orientaciones.
Eso dependerá del formato del parámetro T:
- Si T es una transformación homogénea, entonces IKINE devuelve un vector
fila(Q) con las variables articulares correspondientes a la posición y orientación
indicadas en la matriz T.
- Si T es una trayectoria de transformaciones homogéneas, entonces el resultado
será una matriz (Q), en la que la fila i-ésima contendrá las variables articulares
correspondientes a la transformación T (:, :, i ). La estimación inicial para Q en
cada paso se toma de la solución obtenida en el paso anterior.
Sea cual sea el formato de T, la estimación inicial para el vector de variables
articulares será la dada en el parámetro Q0 (puede ser una columna o una fila), y en el
caso de que no se lo demos, asume que es el vector nulo.
Para el caso de un manipulador con menos de 6 grados de libertad el efector
final no podrá alcanzar algunas posiciones y orientaciones. Esto normalmente lleva a
una no convergencia de IKINE. Una solución consiste en especificar un vector (fila o
columna) de pesos (M), cuyos elementos serán 0 para aquellos grados de libertad que
en cartesianas estén restringidos, y 1 en otro caso. Los elementos de M se corresponden
con las traslaciones a lo largo de los ejes X, Y y Z, y con las rotaciones entorno a los
ejes X, Y y Z.
Por ejemplo si el manipulador no se puede desplazar a lo largo del eje Z, ni rotar
entorno a los ejes X e Y, M deberá ser el vector [1 1 0 0 0 1]. El número de elementos
no nulos debe ser igual al número de grados de libertad del robot.
ILIMIT es el número máximo de iteraciones que se ejecutarán en busca de una
solución (un valor usual es 1000).
STOL será la máxima diferencia que se admitirá entre la transformación
correspondiente a las variables articulares solución y la transformación con la posición
y orientación especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo
uso de la función TR2DIFF.
ÁLVARO GÓMEZ RAMOS.
- 40 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
BLOQUE 2: DESARROLLO DEL
SOFTWARE
ÁLVARO GÓMEZ RAMOS.
- 41 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1
CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT.
1.1
Introducción.
Durante los estudios universitarios hemos recibido una amplia formación en
robótica, tanto teóricamente como en la práctica, aprendiendo a trabajar
matemáticamente con los robots o a programarlos para que realicen diversas tareas.
En el transcurso de esa formación hemos utilizado diversas herramientas
informáticas que nos ayudan en diferente grado a tratar con los robots. Algunas
herramientas nos aportan datos que usamos para corroborar con los nuestros propios,
otras nos facilitan las operaciones matemáticas. Las hay que tratan sobre el control
cinemático y dinámico del robot, facilitando el trabajo y ahorrando mucho tiempo al
usuario. La principal herramienta utilizada es el software de programación MATLAB.
MATLAB es la abreviatura de Matrix Laboratory (laboratorio de matrices). Es
un programa de análisis numérico creado por The MathWorks en 1984. Está disponible
para las plataformas Unix y Windows.
Se pueden ampliar sus capacidades con Toolboxes, algunas de ellas están
destinadas al procesado digital de señal, adquisición de datos, economía, inteligencia
artificial, lógica difusa... También cuenta con otras herramientas como Simulink, que
sirve para simular sistemas.
Es precisamente uno de esos toolboxes, una de las herramientas más utilizadas
en robótica, el Robotic Toolbox. Este toolbox consta de una serie de funciones que
resultan más o menos útiles a la hora de trabajar en el control de los robots. Las
funciones que incluye se dividen en varios grupos:
- Transformaciones homogéneas
- Cuaternios
- Cinemática
- Dinámica
- Generación de trayectorias
- Gráficas
- Creación de modelos de robots
- Demostraciones
Todas estas funciones nos permiten trabajar con el robot, pero a través de
facilitarle ciertos parámetros. Normalmente trabaja a través de un modelo propio que
crea del robot a partir de la matriz DH que lo define. Esto supone que debemos conocer
dicha matriz para poder realizar ciertos cálculos, lo cual implica ciertos conocimientos
de robótica previos para poder trabajar.
ÁLVARO GÓMEZ RAMOS.
- 42 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1.2
SICON-BOT.
Hoy en día la robótica se va extendiendo, y cada vez se generaliza más, con lo
cual es conveniente crear ciertas herramientas que permitan acercar a usuarios menos
expertos, como pasó con la informática, que poco a poco se fue convirtiendo en un
tema más generalista y se fueron desarrollando cada vez más unas herramientas más
intuitivas y sencillas.
Otra cuestión es la comodidad. Si disponemos de herramientas para trabajar con
los robots de una manera más cómoda a partir del modelo matemático de éste, ¿por qué
no tener también una herramienta que nos facilite la obtención del modelo matemático
de una forma intuitiva y sencilla que no necesite grandes conocimientos previos?.
El programa SICON-BOT surge de estas necesidades. Su objetivo es facilitar a
los usuarios menos expertos, es hacer más cómodo el trabajo para los más expertos, e
integrar en una sola herramienta, otras herramientas, para hacer su uso más sencillo. En
definitiva, SICON-BOT tiene tres objetivos principales:
FACILIDAD-COMODIDAD-INTEGRACIÓN
1.3
-
Facilidad: Con un entorno gráfico familiar, con diseño de robots en 3D, con
ventanas, menús desplegables, botones, y demás funciones gráficas que son
familiares a la mayoría de personas.
-
Comodidad: A la hora de integrar nuevas herramientas que nos permiten
obtener automáticamente parámetros necesarios para otras herramientas, que
hasta ahora teníamos que calcular a mano.
-
Integración: En un mismo software podemos utilizar funciones
pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se
resuelven en la programación interna.
Objetivos de la primera versión.
En la primera versión del software SICON-BOT el objetivo es crear las bases de
la herramienta, para que poco a poco puedan ir ampliándose en un futuro por nuevos
usuarios interesados, para lo cual el código es accesible.
Se ha desarrollado enteramente en Matlab un software a través de elementos
familiares, como ventanas y menús, al estilo Windows. Con él podremos dibujar en tres
dimensiones el robot con el que queramos trabajar, introduciendo ciertos parámetros
como la longitud de los tramos.
Una vez dibujado el robot, podemos calcular sus ejes y sobre todo su matriz de
Denavit-Hartenberg. Cuando ya tenemos calculada la matriz DH podemos posicionar el
robot en las posiciones deseadas, o calcular su cinemática, tanto directa como inversa.
ÁLVARO GÓMEZ RAMOS.
- 43 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
El diseño del robot, es un diseño genérico y limitado a ciertos tipos de robots,
púes se basa en la adición de elementos predefinidos, pero con esto podemos diseñar
los robots más comunes.
En resumen los objetivos de la primera versión es el desarrollo desde cero del
software, creando toda la programación necesaria para el entorno gráfico, el diseño de
robots en 3D, el cálculo de ejes, el cálculo de matrices DH, el cálculo de la cinemática
directa e inversa, y el posicionamiento del robot en 3D.
Con esto tenemos en la primera versión los tres objetivos fundamentales:
1.4
-
Facilidad: Con el entorno gráfico que incorpora, el diseño intuitivo de
robots, el cálculo automático de parámetros mediante simple inserción de
datos sencillos.
-
Comodidad: Púes realiza funciones como la obtención automática de los
ejes o de la matriz DH.
-
Integración: Pues incorpora internamente funciones del toolbox de robótica
como la cinemática directa e inversa.
Posibles mejoras futuras.
Existen una gran cantidad de posibles mejoras a añadir al programa, aquí
pondremos algunas que serían de gran utilidad:
-
1.5
Adición de dinámica
Adición de movimiento en 3D siguiendo trayectorias
Interacción con otros robots
Posibilidad de diseñar un entorno de trabajo
Cambio de plataforma de programación, para lograr una mayor eficacia,
como podría ser el lenguaje de programación C o C++
Requisitos mínimos.
El software ha sido creado en matlab 7.0.0.19920 (R14), por lo cual necesita
cumplir los requisitos mínimos de este software y su instalación:
-
Sistemas operativos: Windows XP, Windows 2000 (Service Pack 3 or 4) o
Windows NT 4.0 (Service Pack 5 or 6a)
Procesadores: Pentium III, IV, Xeon, Pentium M, AMD Athlon, Athlon XP,
Athlon MP
Espacio en disco: 345 MB
RAM: 256 MB (Minimum), 512 MB (Recommended)
ÁLVARO GÓMEZ RAMOS.
- 44 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
-
Gráfica: Tarjeta gráfica con 16-, 24-, o 32-bit OpenGL
También se necesita tener instalado el Robotics Toolbox versión 7 April-2002
1.6
Inicialización.
Una vez instalado MATLAB, el toolbox de robótica y SICON-BOT, debemos
iniciar MATLAB,
elegir el directorio donde tenemos instalado SICON-BOT
y escribir la palabra “sicon”
ÁLVARO GÓMEZ RAMOS.
- 45 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2
CAPÍTULO: GRÁFICA DE SICON-BOT.
2.1
Creación de ventana principal y menús desplegables.
Para la creación de la ventana principal usamos el comando figure dentro de la
función “sicon”, con la que creamos una ventana en windows, que sea visible, haga uso
de opengl, de color de fondo blanco y otras opciones.
Al la ventana se le añade una imagen de fondo mediante:
Con “imread” cargamos una imagen del disco duro en una variable de matlab y
con “subimage” mostramos la imagen en la ventana
ÁLVARO GÓMEZ RAMOS.
- 46 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Con “uimenu” creamos los menús desplegables y sus opciones, en este ejemplo
creamos el menú desplegable llamado Archivo que contiene las opciones Nuevo
Gráficamente, Salvar Robot, Salvar D-H, Cargar Robot y salir
2.2
Paneles y plots de representación
Creamos una serie de elementos visuales, como:
-
Paneles:
ÁLVARO GÓMEZ RAMOS.
- 47 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
-
Plots para representación de los elementos posibles del robot:
El resultado final es la ventana principal del programa, desde donde podremos
diseñar el robot o ejecutar sus herramientas:
ÁLVARO GÓMEZ RAMOS.
- 48 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Elementos
posibles del
robot
2.3
Parámetros
del robot
Plot principal
Eslabones y articulaciones.
Existen 4 tipos de elementos que están disponibles para diseñar nuestro robot,
estos son: Articulación de traslación, articulación de rotación, eslabón y base.
Traslación
Rotación
Eslabón
Base
La ventana principal se ha dividido en una serie de subplots, donde se dibujaran
diferentes elementos.
ÁLVARO GÓMEZ RAMOS.
- 49 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Con la orden subplot se divide la ventana en una matriz (m, n) de pequeños
plots y después se elige el plot “p” deseado:
SUBPLOT (m,n,p)
En nuestro caso particular, sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]); creamos
un subplot dividiendo la pantalla en 5 filas y 3 columnas, y seleccionamos las celdas 2,
3, 5, 6, 8, 9, 11, 12, 14, 15 para crear el subplot.
1
2
3
4
5
6
7
10
13
8
11
14
9
12
15
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
Para añadir un elemento a nuestro robot debemos introducir unos parámetros en
función del elemento a añadir, y luego pulsar el botón correspondiente.
ÁLVARO GÓMEZ RAMOS.
- 50 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Con esto conseguiremos introducir un elemento de traslación en el plot de
principal. Por defecto con los valores indicados en la figura, el elemento se dibujaría
vertical y con una longitud inicial de 10 ud. El nombre de las variables en este caso
debe ser obligatoriamente “t1, t2, t3, t4 o t5”, con lo cual el nº de elementos de
traslación posibles en el robot es de 5.
Para poder añadir el elemento en otra posición, disponemos de dos parámetros
Con estos parámetros haremos rotar la figura vertical tantos grados como le
indiquemos alrededor de los ejes “x” y “z” que pasan por el centro de la base del robot.
ÁLVARO GÓMEZ RAMOS.
- 51 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
z
x
y
Por ejemplo, si queremos que el elemento de traslación se dibuje apoyado sobre
el eje “y”, tenemos que indicar un giro de 90º sobre el eje “x”
Para ver el efecto de un giro sobre el eje “z” haremos girar 90º sobre el eje “x” y
45 sobre el eje “z”
Con estos dos parámetros podemos posicionar cualquier elemento en la posición
deseada.
Si lo que queremos insertar es una articulación de rotación:
ÁLVARO GÓMEZ RAMOS.
- 52 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Tenemos que proporcionarle un nombre a la variable, aconsejamos usar q1, q2,
q3, etc. Aunque la única limitación que existe es la de no usar los reservados para
articulaciones de traslación ni otro que ya hayamos usado anteriormente.
El elemento con los valores por defecto se dibujará también inicialmente
vertical, aunque no nos hará falta indicarle longitud, pues esta es fija, y el elemento de
rotación es incorpóreo, es decir no ocupa lugar, el elemento anterior al de rotación y su
inmediatamente posterior están unidos.
El elemento de rotación hará girar a todos los elementos posteriores a él a través
del eje que lo atraviesa desde el centro de la base inferior hasta el centro de su base
superior.
Por eso es muy importante su colocación inicial.
Aunque gráficamente será igual girar 90º en el eje “x” o -90º, el robot resultante
será totalmente distinto.
ÁLVARO GÓMEZ RAMOS.
- 53 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
El elemento eslabón es igual al de traslación sólo que su longitud indicada será
siempre la misma.
Y el elemento base, sirve para elevar al robot en su posición inicial.
Los elementos se irán colocando uno tras el otro, y el punto inicial de uno se
colocará en el punto inicial del siguiente.
rotación
traslación
eslabón
base
Con estos elementos y sus parámetros podemos dibujar una amplia gama de
robots diferentes.
2.4
Botones, textos y cuadros editables.
Con estos elementos podemos interactuar con el software (botones), leer
informaciones (textos) o indicar valores (cuadros editables)
Botones:
Aquí indicamos que queremos un botón pulsador “togglebutton” que al pulsar
llame a la función “rot3d”
Textos:
ÁLVARO GÓMEZ RAMOS.
- 54 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Le indicaremos que queremos un texto que diga “Rot. En X (º)” y en que
posición lo queremos.
Cuadros editables:
Elegimos la posición deseada del elemento y que valor inicial queremos que
aparezca por defecto “0”.
ÁLVARO GÓMEZ RAMOS.
- 55 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3
CAPÍTULO: POSIBILIDADES DE SICON-BOT.
3.1
Nuevo robot
Para poder crear un nuevo robot, tenemos que iniciar el programa desde matlab
escribiendo en el espacio de trabajo “sicon”, con lo que se nos abrirá la ventana de
bienvenida.
Luego nos vamos al menú desplegable “Archivo” y seleccionamos la opción
“Nuevo gráficamente”.
Al pulsar esta opción se abre la ventana principal de diseño del robot.
ÁLVARO GÓMEZ RAMOS.
- 56 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora dependiendo del robot que queramos diseñar iremos introduciendo
nuestros elementos siguiendo el siguiente orden:
1. Si es un elemento de base o eslabón no hace falta indicar nada en la casilla
“Nombre Var”, pero si es un elemento de rotación si que debemos elegir un
nombre para la variable que sea diferente a los anteriores proporcionados y
diferente de t1, t2, t3, t4 y t5. Sin embargo si es un elemento de traslación
debemos indicar un consecutivamente los nombres t1, t2, t3, t4 y t5.
2. Una vez resuelto el nombre de la variable elegiremos el parámetro longitud,
que sólo tiene efecto en los elementos eslabón y base, que establece su
longitud definitiva, y en el elemento traslación establece su longitud inicial.
3. Para terminar de parametrizar el eslabón debemos indicar la rotación en X y
en Z, pudiendo así posicionar el elemento en cualquier posición, excepto el
elemento base que es siempre vertical. Tener en cuenta que el sentido de
rotación depende mucho de estos parámetros.
4. Una vez que tenemos el elemento totalmente parametrizado, sólo nos queda
indicarle que tipo de elemento es y será añadido automáticamente tomando
como punto inicial el final del elemento anterior o si es el primer elemento,
será colocado en (0,0,0).
Ejemplo:
1.
2.
3.
ÁLVARO GÓMEZ RAMOS.
- 57 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.
5.
El resultado es:
Así iremos diseñando nuestro robot paso a paso, elemento a elemento, hasta que
tengamos dibujado nuestro robot por completo:
Una vez diseñado por completo el siguiente y obligatorio paso es calcular el
robot.
3.2
Calcular robot
Para calcular el robot, o lo que es lo mismo, obtener los ejes del robot, debemos
ir al menú desplegable “Ejecución” y seleccionar la opción “Calcular robot”.
ÁLVARO GÓMEZ RAMOS.
- 58 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Este proceso implicará un cálculo complejo con variables simbólicas, que
dependiendo del ordenador y de la complejidad del robot, tardará más o menos tiempo.
Al final el resultado serán los ejes del robot representados en el propio diseño y
representando al eje “z” en azul, el eje “x” en rojo y el eje “y” en verde.
Una vez calculado los ejes, podemos realizar varias acciones como salvar
nuestro robot, para usarlo en un futuro, calcular sus parámetros de Denavit-Hartenberg
o ver su cinemática directa.
3.3
Salvar Robot
Para salvar el robot es necesario haber calculado sus ejes con anterioridad, y lo
que esto implica, haberlo diseñado. Luego iremos al menú “Archivo” y elegiremos la
opción “Salvar robot”.
Entonces se nos abrirá una ventana donde elegir en que lugar guardar el robot y
con que nombre. Se guardará bajo la extensión “.mat” cuando le demos al botón
guardar.
ÁLVARO GÓMEZ RAMOS.
- 59 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.4
Cargar Robot
Si tenemos un robot guardado previamente podemos recuperarlo cuando
volvamos a utilizar el programa, para ello tenemos que iniciar el programa desde
matlab escribiendo en el espacio de trabajo “sicon”, con lo que se nos abrirá la ventana
de bienvenida.
Luego nos vamos al menú desplegable “Archivo” y seleccionamos la opción
“Nuevo gráficamente”.
ÁLVARO GÓMEZ RAMOS.
- 60 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Al pulsar esta opción se abre la ventana principal de diseño del robot.
Luego vamos al menú desplegable “Archivo” y elegimos la opción cargar robot.
Se nos abrirá una ventana similar a la de salvar robot, donde buscaremos el
lugar donde tenemos guardado el robot, lo seleccionaremos y pulsaremos el botón
cargar, añadiendo así el robot al espacio de trabajo con los ejes ya calculados.
ÁLVARO GÓMEZ RAMOS.
- 61 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.5
Parámetros D-H
Una vez calculado el robot o cargado, podremos calcular sus parámetros de
Denavit-Hartenberg o D-H, para ello iremos al menú desplegable “Ejecución” y
elegiremos la opción “Parámetros D-H”
Entonces se nos abrirá una nueva ventana que contiene los parámetros D-H del
robot representados de forma matricial:
di
ai
tethai
alphai
…
…
…
Con estos tendremos calculados los parámetros D-H de una forma totalmente
automática, con los nombres de las variables que le indicásemos.
Una vez calculado los parámetros D-H, podremos acceder a nuevas opciones del
programa como “Salvar D-H”, “Posicionar Robot” o “Cinemática Inversa”
ÁLVARO GÓMEZ RAMOS.
- 62 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.6
Salvar D-H
Con esto podremos exportar la matriz D-H y la matriz P_D_H, la primera nos
indica los parámetros de Denavit-Hartenberg, la segunda además de incluir la matriz DH nos guarda también el tipo de articulación del robot, dando un 0 para rotación y un 1
para traslación. La matriz P_D_H es totalmente compatible con el toolbox de robótica
de matlab.
Para salvar D-H debemos seleccionar la opción “Salvar D-H” del menú
desplegable “Archivo”, elegir donde guardarla y con que nombre.
El archivo guardado también tendrá la extensión “.mat”
3.7
Cinemática directa
Para poder ver la cinemática directa del robot debemos ir al menú desplegable
“Ejecución” y seleccionar la opción “Cinemática directa”.
ÁLVARO GÓMEZ RAMOS.
- 63 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Se nos abrirá una nueva ventana que nos mostrará la cinemática directa del
robot de la siguiente forma:
Así sabremos cual será la posición del extremo del robot a partir del valor de las
variables.
3.8
Cinemática inversa
Para poder ver la cinemática inversa del robot debemos ir al menú desplegable
“Ejecución” y seleccionar la opción “Cinemática inversa”.
Se nos abrirá la siguiente ventana, donde podremos indicar el valor de la
posición final del extremo del robot que deseamos obtener, y pulsando el botón
“calcular”, obtendremos el valor necesario de las variables para que el robot alcance
dicha posición.
ÁLVARO GÓMEZ RAMOS.
- 64 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Posición
deseada
Nombre
de las
variables
Valor
calculado
de las
variables
Una vez pulsemos el botón “calcular” nos aparecerá otra nueva pantalla con un
diseño esquemático del robot en la posición deseada.
Ejemplo:
ÁLVARO GÓMEZ RAMOS.
- 65 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.9
Posicionar Robot
Para poder ver la cinemática inversa del robot debemos ir al menú desplegable
“Ejecución” y seleccionar la opción “Posicionar robot”.
Se nos abrirá la siguiente ventana, donde podremos indicar el valor de las
variables con las cuales queremos posicionar al robot, y pulsando el botón “intr..
Dato”, se abrirá una nueva ventana mostrando un diseño esquemático del robot en la
posición calculada. Además si marcamos la opción “Ver Robot en 3D”, también se
posicionará el robot de nuestro espacio de trabajo principal en dicha posición, teniendo
en cuenta que el robot de la ventana emergente puede poseer cambios de variables para
simplificar la matriz D-H.
ÁLVARO GÓMEZ RAMOS.
- 66 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.10 Ayuda
Sicon-bot dispone de una pequeña ayuda para aprender a usar el programa.
Dicha ayuda se encuentra en la opción “Ayuda Sicon” del menú desplegable “Ayuda”.
Se abrirá una ventana que contiene la ayuda disponible del software.
ÁLVARO GÓMEZ RAMOS.
- 67 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
3.10 Herramientas
A la derecha del espacio de trabajo de sicon-bot disponemos de un panel de
herramientas.
ÁLVARO GÓMEZ RAMOS.
- 68 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Existen tres herramientas disponibles:
-
Rotar robot:
Nos permite rotar el robot en el espacio de trabajo principal.
-
Ver/Ocultar ejes:
Nos permite ocultar los ejes calculados del robot o volver a verlos
cuando deseemos.
-
Ver/Ocultar robot:
Nos permite ocultar el robot calculados del robot o volver a verlos
cuando deseemos
ÁLVARO GÓMEZ RAMOS.
- 69 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4
CAPÍTULO: PROGRAMACIÓN DE SICON-BOT.
4.1
Introducción
El proyecto de la creación de sicon-bot se basa en su mayor parte en la
programación del software en el lenguaje MATLAB. Y esto incluye gran cantidad de
funciones y de muy diversas estilos.
Sicon-Bot es un software que pretende tener un aspecto gráfico accesible para
todos los usuarios, y que se basa en la utilización de elementos visuales para interactuar
con el software, mediante el uso de botones, gráficos, textos, diseños en 3D, etc. Para
ello hace falta conocer la parte gráfica de MATLAB, un lenguaje usado normalmente
para resolver la programación orientada a las matemáticas.
La elección de MATLAB para programar el software es precisamente por su
facilidad para ejecutar complejas operaciones matemáticas, pues aunque el aspecto
visual es lo más destacado para el usuario, la base es la programación de funciones que
implementen operaciones matemáticas complejas como la rotación y traslación de
elementos en 3D y sobre todo el tratamiento con grandes matrices que contienen todos
los datos del robot diseñado, pues este una vez calculado no será más que una serie de
matrices numéricas o simbólicas con las que el software tiene que tratar.
Al sopesar la facilidad matemática de MATLAB con la facilidad gráfica de
otros lenguajes, pesa más la parte de MATLAB, por lo que el lenguaje de programación
elegido es MATLAB. Esto implica que debemos realizar toda la parte gráfica con un
lenguaje menos apto para ello.
En resumen la programación de MATLAB tiene un recorrido de ida y vuelta, es
decir, se crea un entorno gráfico accesible para el diseño en 3D de robots, luego el
software convierte el robot gráfico en matrices, trabaja con ellas y muestra el resultado
de forma gráfica.
En resumen:
Datos de entrada en forma gráfica
Conversión
Datos de entrada en forma matricial
Operación
Datos de salida en forma matricial
Conversión
Datos de salida en forma gráfica
ÁLVARO GÓMEZ RAMOS.
- 70 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.2
Inicialización de ventana inicial y menús desplegables
La primera fase de la programación del software se centra en la creación del
entorno gráfico necesario para el uso del programa.
Para ello creamos la ventana principal a través del comando de matlab “figure”
Con este comando y las propiedades que le hemos indicado, se abre una ventana
nueva, que nos devuelve un valor único y representativo que almacenamos en la
variable “hf1” para poder realizar futuras operaciones con la ventana.
Le hemos indicado ciertos parámetros a través de las propiedades, veamos
algunos interesantes:
-
: Indicamos que queremos que la ventana sea visible.
: Indicamos que las unidades para indicarle la
medida de la ventana, en este caso con “normalizad” queremos que (0, 0)
sea la esquina inferior de abajo y (1, 1) la esquina superior derecha.
: Elige el método de representación gráfica de los
objetos, con “opengl” elegimos el método más potente de representación
gráfica.
: Elegimos que el color de fondo sea blanco.
: Deshabilitamos la posibilidad de cambiar el tamaño de la
ventana.
: El nombre que aparecerá en la ventana será “SiconBot”.
: Elimina que aparezca el número de la ventana
siguiendo su orden de creación.
: Eliminamos la posibilidad de que la ventana se
pueda adosar al espacio de trabajo de matlab.
: Eliminamos los menús desplegables que trae por
defecto matlab.
:_Indicamos la posición y tamaño de la
pantalla.
ÁLVARO GÓMEZ RAMOS.
- 71 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Una vez creada la ventana principal, le queremos añadir la imagen del logo del
software de fondo, para ello:
Con ello almacenamos la imagen en una variable de matlab y luego la
mostramos con el comando “subimage” que nos permite mostrar en una ventana varias
imágenes.
Ahora creamos los menús desplegables y si son accesibles o no desde el
principio:
Con “uimenu” creamos los menús, cuyo nombre indicamos a través de
‘Label’,’nombre’, por ejemplo:
-
Creamos el menú llamado Archivo y guardamos su identificador en la
variable “mfile”
Pero con “uimenu” también podemos crear las opciones que se crearán en el
menú al desplegarse:
Al indicarle la variable mfile donde se almacena el indicador del menú, estamos
asociando esta opción a dicho menú. Con label damos texto a la opción, con la
propiedad “Callback” indicamos la rutina que se ejecutará cuando se pulse la opción, y
con la propiedad ’Separator’,on’, indicamos que dibuje una línea para separar de las
siguientes opciones posibles, para así crear subgrupos diferenciados.
Existen otras propiedades interesantes como
para que la
opción inicialmente no esté habilitada, y poder habilitarla cuando se cumpla alguna
, que hace que la
condición. También es interesante la propiedad
ÁLVARO GÓMEZ RAMOS.
- 72 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
opción también se pueda ejecutar al pulsar la combinación de teclas “Alt + tecla
elegida”.
4.3
Entorno de creación de robots
Cuando elegimos nuevo robot, nos aparece la ventana con el entorno que nos
permite diseñar el robot
Lo primero que hacemos es cambiar el color de fondo:
-
: Con esto decimos que la
propiedad ‘Color’ cambie a la indicada en la ventana que representa el
indicador “hf1”.
Luego se cambian las opciones de habilitación de los menús desplegables según
nos interesen:
-
: Habilitamos la opción Cargar del menú
Archivo, se indica mediante el indicador de dicha opción.
Creamos unos paneles para colocar en ellos diferentes elementos y agrupar los
que tienen algo en común. Esta acción la hacemos con el comando uipanel, guardando
su indicador en una variable. Además le indicamos a través de las propiedades
‘BackgroundColor’ y ‘Position’, de que color será y donde estará colocado con respecto
a los límites de la ventana principal, y le damos un nombre con ‘title’:
ÁLVARO GÓMEZ RAMOS.
- 73 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora crearemos los plots de la ventana, en concreto serán 6, uno principal
donde se dibuja el robot, y 5 secundarios que nos mostrarán las figuras posibles a
insertar.
Dividimos la pantalla en 15 partes e indicamos que el plot principal ocupará el
espacio de las divisiones número 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15. Además
con “xlabel” e “ylabel” modificamos propiedades de los ejes “x” e “y” respectivamente,
como por ejemplo elegir un texto a mostrar y su color.
Luego creamos el resto de plots y hacemos que no se vean los ejes:
ÁLVARO GÓMEZ RAMOS.
- 74 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo siguiente será dibujar las figuras de referencia de las que podremos elegir
para añadir a nuestro robot:
Para ello creamos una matriz con las coordenadas de los vértices y otra donde se
le indican las caras de la figura a través de los vértices que la componen. Luego
elegimos el plot donde queremos representar la figura, y ejecutamos el comando
“patch” indicándole la matriz de vértices y la de caras y el color del objeto a través de
la propiedad ‘FaceVertezCData’. También se le indica la manera de colorear el objeto,
en este caso a través de ‘FaceColor’,’flat’, las caras se dibujarán por homogéneas.
Con
“Axis
xy’
indicamos que los ejes son cartesianos, y con
especificamos el rango que queremos que muestren los ejes.
“camorbit” elige la rotación de la cámara para mostrar el objeto, indicándole la rotación
horizontal y vertical.
Después de preparar los plots de representación secundarios vamos a añadir
, consiguiendo que entre un rayo de luz
iluminación al plot principal:
que ilumina al objeto desde la derecha.
El siguiente paso será la creación de botones, textos y cuadros editables, que lo
haremos con el comando “uicontrol” y sus propiedades.
Con la propiedad ‘Style’ indicamos el tipo de elemento, que puede ser:
-
Cajas de chequeo (Check boxes)
Cuadros editables (Editable text fields)
Cuadros de listas (List boxes)
Menús emergentes (Pop-up menus)
Botones de pulsar (Push buttons)
Botones de elección (Radio buttons)
Deslizadores (Sliders)
ÁLVARO GÓMEZ RAMOS.
- 75 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
-
Textos (Static text)
Botones de estado (Toggle buttons)
Con la propiedad ‘CallBack” elegimos la rutina a ejecutar al activarse el
elemento. Luego elegimos la posición a colocar.
Botones de pulsar
Textos
Cuadros editables
Botones de estado
4.4
Añadir elemento de traslación
Para añadir un elemento de traslación en nuestro diseño del robot tan sólo
tenemos que parametrizarlo correctamente y pulsar el botón correspondiente. Pero esto
es a nivel de usuario, en la programación es un proceso más complejo que veremos a
continuación.
La función que coloca un elemento de traslación en nuestro robot es la
siguiente:
ÁLVARO GÓMEZ RAMOS.
- 76 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Analicemos paso por paso la función. Primero elegimos las variables globales
que nos van a ser útiles:
-
MR: Es la matriz que contendrá los datos del robot, es decir es nuestro robot
desde el punto de vista matemático.
Q1: Es un vector que contiene los valores de las diferentes variables de
traslación, según su orden de creación.
n: Guarda el número de elementos que tiene nuestro robot.
(x,y,z): Son las coordenadas del punto actual donde se añadirá el siguiente
elemento del robot.
htextoe: Cuadro editable que contiene el valor de la rotación sobre el eje de
traslación del elemento. (Actualmente “0” por estar deshabilitada).
-
htexto: Cuadro editable que contiene el valor de la variable actual de
traslación.
-
tras: Guarda el número de elementos de traslación que contiene el robot.
MR es la matriz simbólica que contiene el robot en forma matemática. Esta
matriz es de (n+1 x 8), siendo n el número de elementos del robot. Las filas
corresponden a los elementos del robot, reservando la última fila para un elemento
ficticio situado en el extremo del robot, con la única finalidad de añadir un eje en el
extremo del robot. Las columnas contienen datos que nos parametrizan de forma
inequívoca al elemento en el robot:
MR: (1 [2 3 4] [5 6 7] 8)
MR: (tipo de elemento [posición inicial del elemento] [pto. Final] rot. En el eje)
Luego hacemos que si n=0, es decir, es el primer elemento del robot,
inicializamos la variable.
Lo siguiente es indicar que tenemos un elemento más en nuestro robot:
Obtenemos el valor inicial de la variable de traslación que le indicó el usuario:
Incrementamos en uno el número de elementos de traslación que contiene
nuestro robot:
ÁLVARO GÓMEZ RAMOS.
- 77 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo siguiente sería guardar el valor del punto inicial donde dibujar el elemento
en forma simbólica:
El paso siguiente recurre a la llamada de una función que dibuja el robot a partir
del punto inicial y los datos indicados y nos devuelve el nuevo valor inicial para el
próximo elemento a añadir que coincide con el punto final del elemento inicial. Este
dato nos lo devuelve en forma numérica y simbólica:
Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido,
siendo un “1” para los elementos de traslación:
Almacenamos el punto final del elemento añadido, que como ya hemos
indicado, coincide con el punto inicial del próximo elemento.
Luego guardamos el valor de rotación que le indicamos que deseamos que tenga
la representación gráfica del elemento.
Ya tenemos parametrizado el elemento traslación del todo, pudiéndolo dibujar
cuando se requiera.
4.5
Añadir elemento de rotación
Para añadir un elemento de rotación usamos el siguiente código:
ÁLVARO GÓMEZ RAMOS.
- 78 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Veamos las nuevas variables globales que nos hacen falta:
-
Q2: Es un vector que contiene los valores de las diferentes variables de
rotación, según su orden de creación.
rotac: Guarda el número de elementos de traslación que contiene el robot.
Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot,
inicializamos la variable.
Lo siguiente es indicar que tenemos un elemento más en nuestro robot:
Obtenemos el valor inicial de la variable de rotación que le indicó el usuario:
Incrementamos en uno el número de elementos de rotación que contiene nuestro
robot:
El elemento rotación no ocupa espacio, es decir el punto inicial y el final serán
el mismo, por ello los siguiente sería llamar a una función que nos dibuja el elemento
rotación y nos guarda en MR el punto final e inicial que son coincidentes:
Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido,
siendo un “2” para los elementos de rotación:
ÁLVARO GÓMEZ RAMOS.
- 79 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego guardamos el valor de rotación que le indicamos que deseamos que tenga
la representación gráfica del elemento.
Ya tenemos parametrizado el elemento rotación del todo, pudiéndolo dibujar
cuando se requiera.
4.6
Añadir elemento eslabón
Para añadir un elemento eslabón usamos el siguiente código:
Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot,
inicializamos la variable.
Lo siguiente es indicar que tenemos un elemento más en nuestro robot:
Luego llamamos a una función que nos dibuja el elemento base y nos guarda en
MR el punto final e inicial:
ÁLVARO GÓMEZ RAMOS.
- 80 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido,
siendo un “3” para los elementos eslabón:
Luego guardamos el valor de rotación que le indicamos que deseamos que tenga
la representación gráfica del elemento.
Ya tenemos parametrizado el elemento eslabón del todo, pudiéndolo dibujar
cuando se requiera.
4.7
Añadir elemento base
Para añadir un elemento base usamos el siguiente código:
Lo primero a hacer es que si n=0, es decir, es el primer elemento del robot,
inicializamos la variable.
Lo siguiente es indicar que tenemos un elemento más en nuestro robot:
ÁLVARO GÓMEZ RAMOS.
- 81 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego llamamos a una función que nos dibuja el elemento base y nos guarda en
MR el punto final e inicial:
Ahora indicaremos en la matriz MR el tipo de elemento que hemos añadido,
siendo un “4” para los elementos base:
Luego guardamos el valor de rotación que le indicamos que deseamos que tenga
la representación gráfica del elemento.
Ya tenemos parametrizado el elemento base del todo, pudiéndolo dibujar
cuando se requiera.
4.8
Posicionar y dibujar por primera vez el elemento traslación
Para dibujar el elemento traslación por primera usamos una función distinta que
para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto
inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización,
obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se
dibujará a partir del punto inicial y el punto final obtenido.
Lo primero a realizar en la función es obtener los valores de parametrización
que nos indico el usuario, en este caso necesitamos la longitud inicial y el nombre de la
variable. La variable “num_simb_rot” guarda el número de variables de traslación que
llevamos diseñadas. :
Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven
para posicionar el robot adecuadamente:
ÁLVARO GÓMEZ RAMOS.
- 82 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
A partir del valor del punto inicial, situamos el punto final momentáneamente a
una distancia “L” (anteriormente indicada) en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.3, y-0.3, z)
(x2,y2,z2) = (x+0.3, y-0.3, z)
(x3,y3,z3) = (x+0.3, y+0.3, z)
(x4,y4,z4) = (x-0.3, y+0.3, z)
(x5,y5,z5) = (x-0.3, y-0.3, z+l)
(x6,y6,z6) = (x+0.3, y-0.3, z+l)
(x7,y7,z7) = (x+0.3, y+0.3, z+l)
(x8,y8,z8) = (x-0.3, y+0.3, z+l)
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
ÁLVARO GÓMEZ RAMOS.
- 83 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento, luego sustituimos los valores simbólicos por los valores
actuales, para tener el valor numérico del nuevo punto inicial:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos
que calcular su valor actual para posicionarlos:
ÁLVARO GÓMEZ RAMOS.
- 84 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.9
Posicionar y dibujar por primera vez el elemento rotación
Para dibujar el elemento rotación por primera usamos una función distinta que
para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto
inicial y los valores de giro indicadas por el usuario en la parametrización,
obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se
dibujará a partir del punto inicial y el punto final obtenido.
Lo primero a realizar en la función es obtener los valores de parametrización
que nos indico el usuario, en este caso necesitamos el nombre de la variable. La
variable “num_simb_rot2” guarda el número de variables de rotación que llevamos
diseñadas. :
ÁLVARO GÓMEZ RAMOS.
- 85 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven
para posicionar el robot adecuadamente:
A partir del valor del punto inicial, situamos el punto final coincidente:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
X5
X7
X6
Pto. final
Pto. inicial
X4
X1
X3
X2
La situación de los 8 vértices dependerá del centro de gravedad del elemento,
suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.3, y-0.3, z-0.5)
(x2,y2,z2) = (x+0.3, y-0.3, z-0.5)
(x3,y3,z3) = (x+0.3, y+0.3, z-0.5)
(x4,y4,z4) = (x-0.3, y+0.3, z-0.5)
(x5,y5,z5) = (x-0.3, y-0.3, z+0.5)
(x6,y6,z6) = (x+0.3, y-0.3, z+0.5)
(x7,y7,z7) = (x+0.3, y+0.3, z+0.5)
(x8,y8,z8) = (x-0.3, y+0.3, z+0.5)
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
ÁLVARO GÓMEZ RAMOS.
- 86 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el centro de
gravedad del elemento, éste rotará hasta posicionarse en su posición final.
En este caso el punto inicial no variará, por lo que coincidirá con el final.
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos
que calcular su valor actual para posicionarlos:
ÁLVARO GÓMEZ RAMOS.
- 87 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.10 Posicionar y dibujar por primera vez el elemento eslabón
Para dibujar el elemento eslabón por primera usamos una función distinta que
para dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto
inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización,
obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se
dibujará a partir del punto inicial y el punto final obtenido.
Lo primero a realizar en la función es obtener los valores de parametrización
que nos indico el usuario, en este caso necesitamos la longitud:
ÁLVARO GÓMEZ RAMOS.
- 88 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven
para posicionar el robot adecuadamente:
A partir del valor del punto inicial, situamos el punto final momentáneamente a
una distancia “L” (anteriormente indicada) en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.3, y-0.3, z)
(x2,y2,z2) = (x+0.3, y-0.3, z)
(x3,y3,z3) = (x+0.3, y+0.3, z)
(x4,y4,z4) = (x-0.3, y+0.3, z)
(x5,y5,z5) = (x-0.3, y-0.3, z+l)
(x6,y6,z6) = (x+0.3, y-0.3, z+l)
(x7,y7,z7) = (x+0.3, y+0.3, z+l)
(x8,y8,z8) = (x-0.3, y+0.3, z+l)
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
ÁLVARO GÓMEZ RAMOS.
- 89 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos
que calcular su valor actual para posicionarlos:
ÁLVARO GÓMEZ RAMOS.
- 90 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.11 Posicionar y dibujar por primera vez el elemento base
Para dibujar el elemento base por primera usamos una función distinta que para
dibujarlo las siguientes veces, esto es porque la primera se dibuja a partir del punto
inicial y los valores de giro y longitudes indicadas por el usuario en la parametrización,
obteniéndose luego el punto final del robot. Sin embargo, a partir de entonces se
dibujará a partir del punto inicial y el punto final obtenido.
Lo primero a realizar en la función es obtener los valores de parametrización
que nos indico el usuario, en este caso necesitamos la longitud inicial:
ÁLVARO GÓMEZ RAMOS.
- 91 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo segundo sería obtener los ángulos indicados por el usuario, que nos sirven
para posicionar el robot adecuadamente:
A partir del valor del punto inicial, situamos el punto final momentáneamente a
una distancia “L” (anteriormente indicada) en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.3, y-0.3, z)
(x2,y2,z2) = (x+0.3, y-0.3, z)
(x3,y3,z3) = (x+0.3, y+0.3, z)
(x4,y4,z4) = (x-0.3, y+0.3, z)
(x5,y5,z5) = (x-0.3, y-0.3, z+l)
(x6,y6,z6) = (x+0.3, y-0.3, z+l)
(x7,y7,z7) = (x+0.3, y+0.3, z+l)
(x8,y8,z8) = (x-0.3, y+0.3, z+l)
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
ÁLVARO GÓMEZ RAMOS.
- 92 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
Ahora que tenemos los vértices posicionados pero de forma simbólica, tenemos
que calcular su valor actual para posicionarlos:
ÁLVARO GÓMEZ RAMOS.
- 93 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.12 Rotar punto respecto a ejes x e y que pasan por otro punto dado
Este procedimiento lo resuelve por completo la función creada para la ocasión:
Para realizar esta operación matemática nos basamos en el concepto de rotación
alrededor de los ejes “x”, “y” y “z” que pasan por el origen. Peor en nuestro caso
necesitamos que roten alrededor de un punto que puede ser diferente al origen, para ello
hacemos una traslación previa, restando al punto a rotar el punto por donde pasan los
ejes, luego rotamos para luego volver a sumar lo anteriormente restado, es decir:
Traslación al origen
Rotación
Deshacer traslación
Para realizar la traslación al origen hacemos la siguiente operación:
ÁLVARO GÓMEZ RAMOS.
- 94 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora el punto a tratar será el formado por las coordenadas (A,B,C).
Lo siguiente es la rotación del punto. Cuando consideramos la rotación de un
objeto en dos dimensiones, vimos la matriz de rotación alrededor del origen
Podemos generalizar este concepto a una rotación tridimensional alrededor del
eje z.
En esta rotación, pensamos en el eje como fijo mientras que algún objeto se
mueve en el espacio. Podemos pensar también que el objeto permanece inmóvil
mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la
dirección de la rotación. Fijar el eje y girar el objeto en sentido antihorario es lo mismo
que fijar el objeto y mover el eje en sentido horario.
La rotación alrededor de los ejes x e y se realiza con una matriz de
transformación similar a la anterior.
Para hacer una rotación alrededor del eje y, del tal forma que el eje z se
convierte en el x realizamos la siguiente matriz en coordenadas homogéneas
Y finalmente multiplicamos las matrices de rotación por el punto:
ÁLVARO GÓMEZ RAMOS.
- 95 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ya tenemos rotado el punto, pero debemos deshacer la traslación:
4.12 Rotar punto respecto a ejes dado por dos puntos
Se resuelve mediante la función:
Lo primero que debemos hacer es escoger una representación adecuada de la
recta que será el eje de rotación. Un punto de dicha recta, así como la dirección de la
misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos
ofrece la información necesaria para la traslación del origen, y la dirección nos informa
de los ángulos de rotación correctos para alinear la recta con el eje z.
Dada la recta
y su dirección está especificada por el vector
un punto de esta recta es
. Para obtener los puntos
realizamos la siguiente operación:
El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos
realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el
ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y
consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta comprendido entre el origen
y
está en la
dirección del eje arbitrario de rotación, y todo el segmento está situado sobre dicho eje.
Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un
vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende
desde
hasta
ÁLVARO GÓMEZ RAMOS.
.
- 96 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Si ahora realizamos un giro alrededor del eje x hasta que nuestro eje arbitrario
quede en el plano xz, el segmento proyectado quedará alineado con el eje z. Cuanto vale
el ángulo q ?
Sabemos que la longitud del segmento proyectado es
y, teniendo en cuenta la figura anterior, es fácil deducir las razones trigonométricas del
ángulo q
Por lo tanto, la matriz de transformación para el giro alrededor del eje x será
ÁLVARO GÓMEZ RAMOS.
- 97 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
La transformación inversa es una rotación de igual magnitud pero en sentido
contrario. Como sabemos, cambiar el signo del ángulo significa cambiar el signo de los
senos y conservar el de los cosenos.
La rotación alrededor de este eje no ha modificado la coordenada x. Sabemos
también que la longitud total del segmento
no varía. La coordenada z será entonces
Necesitamos realizar ahora un giro de ángulo f alrededor del eje y para que el
segmento quede alineado con el eje z.
Sus razones trigonométricas son
ÁLVARO GÓMEZ RAMOS.
- 98 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
La matriz de rotación en este caso será
La inversa de esta transformación será
Es posible ahora realizar el giro j alrededor del eje escogido. Ya que este eje
está alineado con el eje z, la matriz correspondiente a esta rotación será:
De esta forma ya somos capaces de realizar una rotación j alrededor de un eje
arbitrario, mediante el producto de las transformaciones anteriores:
4.13 Calcular robot
Una vez dibujado el robot completamente necesitamos finalizarlo y calcular sus
ejes. Todo esto se calculará a través de la opción “Calcula robot”, que llama a la
función:
El primer paso es añadir un falso eslabón al final del último elemento, para
obtener después un eje final:
Ya tenemos la matriz MR que contiene los datos de los elementos añadidos al
robot. Dicha matriz es simbólica porque depende de los variable de longitud de los
ÁLVARO GÓMEZ RAMOS.
- 99 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
elementos de traslación, sin embargo, las variables de rotación no se ven reflejadas en
esta matriz, teniendo que rotar externamente la matriz para posicionarla en su posición
actual. Para ello utilizamos la función “rot” haciéndolo rotar alrededor del eje de
rotación:
Una vez que tenemos la matriz MR ya con las variables de traslación y rotación,
pasamos a ejecutar la función “MRtoMRvec”, que es una función que transforma la
matriz con las coordenadas de los eslabones en una matriz con los ejes de las
articulaciones de rotación y traslación y un punto de estos:
Ya tenemos el eje de las articulaciones y un punto, ahora vamos a buscar el
“vector z” y “vector x” de la articulación y el punto donde se situará el eje, esto lo
realizamos a través de la función “MRvectoMRperp”, que usando MRvec nos devuelve
la matriz MRperp con los datos obtenidos:
Sólo nos falta obtener el “vector y” y dibujar los ejes, lo cual hacemos a través
de la función “Plotejes”:
4.14 Obtención de los ejes de las articulaciones
Este paso lo realizamos a través de la función “MRtoMRvec” que recorre la
matriz MR en busca de articulaciones es decir, elementos de traslación o rotación:
ÁLVARO GÓMEZ RAMOS.
- 100 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego calcula su eje y un punto de este, si es de rotación resta el punto final del
inicial y obtenemos el eje y cogemos el punto medio del inicial y el final como punto
del eje:
Si es de traslación restamos el punto inicial al final y obtenemos el eje, y
elegimos el punto final como punto del eje:
En el último elemento de MR que es el elemento ficticio añadido también
calcula un eje según el punto inicial y final del elemento anterior de MR:
Al final se le añade a cada fila de MRvec un número identificando si es rotación
(1), traslación (2) o es el eje final (0).
4.15 Obtención de “eje x”, “eje z” y punto de origen
Para ello usamos la función “MRvectoMRperp” que transforma la matriz con
los ejes principales (MRvec) de las articulaciones en una matriz que contiene la
perpendicular a cada eje (MRperp).
Para calcular el “eje x” nos basamos en Denavit-Hartenberg, que dice que se
busque la recta perpendicular común a un eje y el siguiente, para ellos debemos
averiguar primero si los ejes que entran en juego son coincidentes, paralelos, se cruzan
o se cortan. La obtención de la posición relativa de dos rectas la realizamos mediante
cálculo matemático.
Los vectores directores y los puntos que utilizamos para escribir las ecuaciones
de las rectas nos permiten calcular la posición relativa. Consideremos las matrices A y
B formadas con estos elementos:
ÁLVARO GÓMEZ RAMOS.
- 101 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
cuando el rango de la matriz A es uno, es decir los vectores directores son iguales o
proporcionales, las rectas son paralelas o coincidentes (la misma); serán coincidentes
cuando el rango de B también sea 1 y paralelas cuando el rango de B sea 2.
Cuando el rango de A es dos y el rango de B también dos: las rectas se cortan.
El sistema que resulta al igualar sus ecuaciones paramétricas es compatible
determinado. Cuando el rango de A es dos y el de B tres: las rectas se cruzan. El
sistema es incompatible.
El punto “P” será el punto del eje que guardamos en la matriz MRvec:
El punto “Q” será el punto del eje siguiente guardado en la matriz MRvec:
El vector “V” será el vector que guardamos en la matriz MRvec:
El vector “W” será el vector del eje siguiente guardado en la matriz MRvec:
Ahora creamos las matrices A y B que compararemos sus rangos para
saber la posición relativa de los ejes intervinientes:
ÁLVARO GÓMEZ RAMOS.
- 102 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Comparamos el rango, para ello usamos el comando de matlab “rank” que nos
devuelve el rango de una matriz, y comparamos ambos rangos:
Si las rectas se cruzan el vector z será el vector del eje que contiene VRvec, el
punto de origen será el punto del eje actual que contiene MRvec, y el vector x será el
producto vectorial de los vectores del eje actual y el siguiente:
Si las rectas se cortan el vector z será el vector del eje que contiene VRvec, el
punto de origen es el punto de intersección de los dos ejes, que calculamos teniendo en
cuenta las siguientes ecuaciones:
Tenemos todos los datos necesarios excepto λ que hallamos resolviendo el
sistema de ecuaciones mediante la regla de Cramer. Un sistema de ecuaciones lineales
se dice de Cramer cuando cumple las siguientes condiciones:
-
Es un sistema cuadrado, con igual número de ecuaciones que de incógnitas.
El determinante de la matriz de los coeficientes asociada es distinto de cero.
En consecuencia, un sistema de Cramer es siempre compatible determinado
(tiene una solución única).
ÁLVARO GÓMEZ RAMOS.
- 103 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Para calcular la incógnita xi del sistema de ecuaciones lineales, se sustituye la
columna i de la matriz de coeficientes por los términos independientes, se obtiene el
determinante de la matriz resultante y se divide este valor por el del determinante de la
matriz de los coeficientes. Por tanto, la solución de un sistema de Cramer se obtiene
hallando cada incógnita Xi según la fórmula:
Siendo Ci la matriz resultante de sustituir la columna de la matriz de los
coeficientes correspondiente a la incógnita por la de los términos independientes.
Una vez que tenemos λ hallamos el punto de corte de la siguiente manera:
Para ello empleamos el siguiente código:
Sólo nos falta el vector x, que será la perpendicular común a los ejes implicados.
Si las rectas son paralelas debemos determinar el vector perpendicular a ambas
rectas, hacemos que la recta pase por el punto P del primer eje, pero no tiene porque
pasar por el punto Q, así que buscamos el punto Q’ del segundo eje por donde si pase:
Ya podemos obtener la perpendicular común:
ÁLVARO GÓMEZ RAMOS.
- 104 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
El último caso posible es que las rectas sean coincidentes, lo cual resolvemos
dependiendo de varios casos concretos, es decir, pueden existir combinaciones no
contempladas que no se resuelvan correctamente.
Lo primero es ver si los ejes de la articulación actual y de la anterior coinciden o
son paralelas y no es la primera articulación:
En este caso elegimos los ejes como los de la articulación anterior:
En el caso que no sea la primera articulación y no coincida con los ejes de la
anterior:
Suponemos en este caso que sólo se contempla que el eje perpendicular al
anterior, con lo cual el nuevo eje x será el eje z anterior:
Si la articulación es la primera de todas y vertical suponemos dos casos
posibles, que sea eje de rotación, entonces el eje x estará en el plano perpendicular al
eje de rotación:
Si la articulación es de traslación, el eje x será siempre (1 0 0):
Todos los demás casos no contemplados no podrán ser diseñados.
Una vez calculados todos los ejes x y el punto donde situarlo, y teniendo
previamente los ejes z de todas las articulaciones, sólo nos queda calcularlos de la
última articulación ficticia. El eje x será la perpendicular común al eje anterior y el
actual, situándolo en el último punto del robot:
ÁLVARO GÓMEZ RAMOS.
- 105 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.16 Obtención de “eje y”, normalización y dibujado de ejes
Se realiza a través de la función “plotejes” que a partir de la matriz MRperp crea
la matriz MRejes que contiene los ejes “x”, “y”, “z”, el punto donde se sitúan y el tipo
de articulación.
El primer paso es crear unos ejes iniciales que serán siempre iguales, y estarán
situados en (0, 0, 0):
Luego dibujamos dichos ejes iniciales:
Ahora, recorriendo la matriz entera, se calculan los ejes normalizados, es decir,
ejes unitarios, para ello calculamos la longitud del vector que representa al eje y
dividimos por sus coordenadas, lo mismo hacemos con el eje x, y luego calculamos el
eje y como el producto vectorial de los ejes “z” y “x”:
Ahora para dibujar los ejes, primero convertimos la matriz simbólica en matriz
numérica con los valores correspondientes a la posición actual:
ÁLVARO GÓMEZ RAMOS.
- 106 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y al final se dibujan los ejes:
4.17 Dibujar robot
Por diversas razones podemos necesitar dibujar el robot de nuevo, ya sea porque
lo posicionamos en otra posición, o porque en un momento elegimos que no este visible
y luego queremos que este visible de nuevo. Esta tarea se realiza a través de la función
“dibubot” que a partir de la matriz MR nos dibuja el robot en la posición actual:
El primer paso es convertir la matriz MR simbólica en una matriz numérica,
sustituyendo las variables por sus valores iniciales:
Luego recorremos la matriz MR, dibujando uno a uno los elementos
dependiendo del tipo de elemento que sea. Para dibujar los elementos se usa una
función diferente a la que los dibuja cuando son añadidos, porque ahora ya se dibujan
simplemente conociendo el punto inicial y final del elemento:
ÁLVARO GÓMEZ RAMOS.
- 107 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.18 Dibujar elemento eslabón
Para dibujar el elemento en cuestión utilizamos la función “eslabon2” que a
partir del punto inicial y final lo dibuja:
Lo primero que necesitamos es la longitud del elemento, que calculamos viendo
la distancia entre el punto inicial y el final:
Luego calculamos el ángulo x, que podrá ser positivo o negativo, para
determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que
consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:
Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es
vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se
calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de
esta sobre los ejes “x” (lx) e “y” (ly)
lx
ly
ÁLVARO GÓMEZ RAMOS.
l1
- 108 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora tenemos los mismos datos que se introducen al parametrizar el elemento
y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial,
situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada)
en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.5, y-0.5, z)
(x2,y2,z2) = (x+0.5, y-0.5, z)
(x3,y3,z3) = (x+0.5, y+0.5, z)
(x4,y4,z4) = (x-0.5, y+0.5, z)
(x5,y5,z5) = (x-0.5, y-0.5, z+l)
(x6,y6,z6) = (x+0.5, y-0.5, z+l)
(x7,y7,z7) = (x+0.5, y+0.5, z+l)
(x8,y8,z8) = (x-0.5, y+0.5, z+l)
ÁLVARO GÓMEZ RAMOS.
- 109 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
ÁLVARO GÓMEZ RAMOS.
- 110 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.19 Dibujar elemento traslación
Para dibujar el elemento en cuestión utilizamos la función “traslacion2” que a
partir del punto inicial y final lo dibuja, los pasos son los mismos que para el elemento
eslabón:
Lo primero que necesitamos es la longitud del elemento, que calculamos viendo
la distancia entre el punto inicial y el final:
Luego calculamos el ángulo x, que podrá ser positivo o negativo, para
determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que
consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:
ÁLVARO GÓMEZ RAMOS.
- 111 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es
vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se
calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de
esta sobre los ejes “x” (lx) e “y” (ly)
lx
ly
l1
Ahora tenemos los mismos datos que se introducen al parametrizar el elemento
y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial,
situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada)
en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
ÁLVARO GÓMEZ RAMOS.
- 112 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.3, y-0.3, z)
(x2,y2,z2) = (x+0.3, y-0.3, z)
(x3,y3,z3) = (x+0.3, y+0.3, z)
(x4,y4,z4) = (x-0.3, y+0.3, z)
(x5,y5,z5) = (x-0.3, y-0.3, z+l)
(x6,y6,z6) = (x+0.3, y-0.3, z+l)
(x7,y7,z7) = (x+0.3, y+0.3, z+l)
(x8,y8,z8) = (x-0.3, y+0.3, z+l)
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento:
ÁLVARO GÓMEZ RAMOS.
- 113 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.20 Dibujar elemento rotación
Para dibujar el elemento en cuestión utilizamos la función “eje2” que a partir
del punto inicial y final lo dibuja:
Lo primero que necesitamos es saber donde se situará el centro del elemento
rotación, que será en el punto medio del punto inicial y final:
Lo siguiente que necesitamos es la longitud del elemento, que calculamos
viendo la distancia entre el punto inicial y el final:
ÁLVARO GÓMEZ RAMOS.
- 114 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego calculamos el ángulo x, que podrá ser positivo o negativo, para
determinar su signo, miramos la “y” del punto inicial y final, si yf-yi>0 implica que
consideraremos el giro sobre x menos que cero, en el caso contrario será positivo:
Lo siguiente es calcular el giro sobre el eje z, que será cero si z el elemento es
vertical, es decir, la “x” y la “y” del punto inicial y final coinciden. Si no es vertical, se
calculará con la proyección del eje sobre el plano XY (l1) y la proyección a su vez de
esta sobre los ejes “x” (lx) e “y” (ly)
lx
ly
ÁLVARO GÓMEZ RAMOS.
l1
- 115 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora tenemos los mismos datos que se introducen al parametrizar el elemento
y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial,
situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada)
en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
X8
X5
Pto. final
X7
X6
Pto. inicial
X4
X1
X3
X2
La situación de los 8 vértices dependerán del punto inicial donde dibujar el
elemento, suponiendo a éste vertical, así quedaría:
(x1,y1,z1) = (x-0.4, y-0.4, z)
(x2,y2,z2) = (x+0.4, y-0.4, z)
(x3,y3,z3) = (x+0.4, y+0.4, z)
(x4,y4,z4) = (x-0.4, y+0.4, z)
(x5,y5,z5) = (x-0.4, y-0.4, z+1)
(x6,y6,z6) = (x+0.4, y-0.4, z+1)
(x7,y7,z7) = (x+0.4, y+0.4, z+1)
(x8,y8,z8) = (x-0.4, y+0.4, z+1)
ÁLVARO GÓMEZ RAMOS.
- 116 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ya tenemos identificado todos los vértices, pero en situación vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vértices
alrededor del eje x y el eje z según los valores deseados, para ello usamos la función:
“Rotar” es una función que rota un punto Pf con respecto a los ejes x e y que
hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el punto inicial del
elemento, éste rotará hasta posicionarse en su posición final.
Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch.
ÁLVARO GÓMEZ RAMOS.
- 117 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.21 Dibujar elemento base
Para dibujar el elemento en cuestión utilizamos la función “base2” que a partir
del punto inicial y final lo dibuja:
Lo primero que necesitamos es la longitud del elemento, que calculamos viendo
la distancia entre el punto inicial y el final:
Ahora tenemos los mismos datos que se introducen al parametrizar el elemento
y podemos seguir el mismo sistema para dibujarlo. A partir del valor del punto inicial,
situamos el punto final momentáneamente a una distancia “L” (anteriormente indicada)
en vertical:
Con esto tenemos el elemento situado en vertical y formado por una recta, pero
necesitamos representarlo en 3D, para ello necesitamos obtener 8 vértices, 4 por cada
base.
Pto. final
X8
X5
X4
Pto. inicial
X1
X7
X6
X3
X2
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,
para ello usamos la función “rot” que es una función para resolver el giro de un punto
(X,Y,Z) respecto a la recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo de rotación igual a tetha:
:
ÁLVARO GÓMEZ RAMOS.
- 118 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Y una vez que tenemos parametrizado el elemento en su posición inicial, lo
dibujamos con el comando patch:
4.22 Salvar robot
Una vez calculado el robot tenemos la posibilidad de poder salvarlo para
recuperarlo más adelante y seguir trabajando con él sin necesidad de volver a diseñarlo.
Para ello usamos la función “savebot” :
Usamos la función “uiputfile” para obtener un lugar donde guardar el archivo el
nombre con el que lo haremos. El nombre se guardará en la variable “fname”:
Una vez indicados donde y con que nombre guardar los datos, procedemos a
salvarlos con la función “save” a la que le indicamos el nombre del archivo a guardar y
las variables a guardar en el archivo que se creará:
4.23 Cargar robot
Si disponemos de un robot previamente salvado y necesitamos seguir
interactuando con él, podremos hacerlo cargándolo al espacio de trabajo..
Para ello usamos la función “loadbot” :
ÁLVARO GÓMEZ RAMOS.
- 119 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Usamos la función “uiputfile” para obtener un lugar desde donde recuperar el
archivo.:
Una vez indicados donde y con que nombre se guardaron previamente los datos
del robot, procedemos a cargarlos con la función “load” a la que le indicamos el
nombre del archivo a recuperar:
Luego dibujaremos el robot con la función “dibubot” y sus ejes con la función
“plotejes”:
4.24 Calculo de parámetros DH
Para calcular los parámetros de Denavit-Hartenberg utilizaremos la función
“Par_DH” creada para la ocasión:
Lo primero que debemos hacer es calcular los parámetros DH, cuyo cálculo lo
resolveremos en funciones independientes:
Ya tenemos calculados los parámetros, ahora los mostraremos en una nueva
pantalla que debemos crear:
ÁLVARO GÓMEZ RAMOS.
- 120 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Los datos de DH los representamos en forma matricial, teniendo la matriz
resultante cuatro columnas correspondientes a los parámetros (a, d, α, θ) y un número
de filas igual al número de articulaciones+1. Para la representación gráfica dividimos la
pantalla en cuatro partes verticalmente y en nº articulaciones+1 en horizontal. Para
saber el número de las articulaciones creamos la variable “tam”:
Ahora muestro los datos de la siguiente manera, los parámetros “d” los
represento en la primera columna, es decir, dividiendo la pantalla en cuatro divisiones
verticales y sabiendo que la longitud total es 1, “d” se representará en la división que va
de 0 a 0’25. Primero represento el tipo de dato y luego sus valores, por lo que las
divisiones en horizontal son el número de elementos más el nombre del tipo de dato:
Para el resto de datos seguimos el mismo método, pero colocándolos donde
corresponde, es decir incrementando en una las divisiones verticales y con el mismo
procedimiento en horizontal:
a
…
…
…
…
ÁLVARO GÓMEZ RAMOS.
d
…
…
…
…
α
…
…
…
…
θ
…
…
…
…
- 121 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
ÁLVARO GÓMEZ RAMOS.
- 122 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ya hemos representado todos los valores, ahora los almacenamos en una matriz
llamada “P_D_H”:
4.25 Calculo de parámetros di
di es la distancia de xi-1 a xi medida a lo largo de zi
ÁLVARO GÓMEZ RAMOS.
- 123 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Sea una recta r determinada por un punto P y un vector
exterior a la recta.
, y un punto Q
La distancia entre el punto y la recta es:
La distancia se calculará como la distancia de un punto a una recta, siendo “P”
el vector xi. Con lo cual el
el punto de los ejes i, “Q” el punto de los ejes i-1 y
cálculo se resolverá:
4.26 Calculo de parámetros ai
ai: Es la distancia de zi a zi+1 medida a lo largo de xi
ÁLVARO GÓMEZ RAMOS.
- 124 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Sea una recta r determinada por un punto P y un vector
exterior a la recta.
, y un punto Q
La distancia entre el punto y la recta es:
La distancia se calculará como la distancia de un punto a una recta, siendo “P”
el vector zi. Con lo cual el
el punto de los ejes i, “Q” el punto de los ejes i-1 y
cálculo se resolverá:
ÁLVARO GÓMEZ RAMOS.
- 125 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.27 Calculo de parámetros θ i
θi: Es el ángulo de xi-1 a xi medida sobre zi (utilizando la regla de la mano
derecha).
Es el ángulo que forman dos rectas es el ángulo que forman sus vectores directores:
Cuando la articulación es de rotación, indicamos que su valor sea el nombre de
la variable, y si no es una articulación de rotación lo calculamos según la fórmula
anterior teniendo en cuenta que V1 es xi-1 y que W1 es xi:
4.28 Calculo de parámetros α i
αi: Es el ángulo de zi a zi+1 medida sobre xi (utilizando la regla de la mano
derecha).
ÁLVARO GÓMEZ RAMOS.
- 126 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Es el ángulo que forman dos rectas es el ángulo que forman sus vectores directores:
Calculamos según la fórmula anterior teniendo en cuenta que V1 es xi y que
W1 es xi+1:
4.28 Cinemática directa
La cinemática directa consiste en resolver la posición cartesiana y la orientación
del efector final dado el valor de las articulaciones y conociendo la estructura del robot
a través de la matriz de Denavit-Hartenberg.
Necesitamos obtener el valor del punto final del robot en función de sus
variables articulares, lo cual obtenemos directamente de la matriz “MR” obteniendo el
punto final del robot, ya que siempre expresamos los puntos en función de las variables.
Esto lo hacemos a través de la función “cin_dir” creando una ventana nueva y
mostramos la “x”, “y” y “z” desde MR(n,[5 6 7]) respectivamente y siendo “n” el
número de elementos de “MR”
ÁLVARO GÓMEZ RAMOS.
- 127 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
ÁLVARO GÓMEZ RAMOS.
- 128 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.29 Cinemática inversa
El objetivo del problema cinemático inverso consiste en encontrar los valores
que deben tomar las variables articulares del robot para que su extremo se posicione y
oriente según una determinada localización espacial.
Para resolver el problema cinemática inverso utilizamos la función “cin_inv”.
Lo primero a realizar es calcular el número de articulaciones, el tipo y el orden de ellas,
siendo “a” el tipo de variables, y “Var” los nombres:
ÁLVARO GÓMEZ RAMOS.
- 129 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Luego debemos pasar a numérica la matriz simbólica con los parámetros DH:
Creo la matriz del robot para usarla en el toolbox de robótica:
Creo una matriz “DH” que contenga a la matriz “P_D_H” más el tipo de
elemento:
Creamos la ventana para introducir datos:
ÁLVARO GÓMEZ RAMOS.
- 130 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
ÁLVARO GÓMEZ RAMOS.
- 131 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Creamos el robot según toolbox de robótica:
Ahora creamos una nueva función a la que se llamará cuando se pulse el botón
calcular, y que nos realizará el cálculo de la cinemática inversa. El cálculo lo
realizamos a través de la función ikine. IKINE calcula la cinemática inversa del
manipulador
Q = IKINE(DH, STOL, ILIMIT, T)
Q = IKINE(DH, STOL, ILIMIT, T, Q0)
Q = IKINE(DH, STOL, ILIMIT, T, Q0, M)
Devuelve los valores de las variables articulares necesarios para que el efector
final del manipulador tenga la posición y orientación dadas por la transformación T. La
solución del problema cinemático inverso no es única en general, y es posible que para
una misma orientación y posición deseadas, se obtengan soluciones distintas en función
del vector inicial de variables articulares (Q0) que se le pase a IKINE.
ÁLVARO GÓMEZ RAMOS.
- 132 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Es posible usar la función para que devuelva las variables articulares
correspondientes a una sola posición y orientación, o bien a una trayectoria de
posiciones y orientaciones. Eso dependerá del formato del parámetro T:
-
Si T es una transformación homogénea, entonces IKINE devuelve un
vector fila(Q) con las variables articulares correspondientes a la posición
y orientación indicadas en la matriz T.
-
Si T es una trayectoria de transformaciones homogéneas, entonces el
resultado será una matriz (Q), en la que la fila i-ésima contendrá las
variables articulares correspondientes a la transformación T (:, :, i ). La
estimación inicial para Q en cada paso se toma de la solución obtenida
en el paso anterior.
Sea cual sea el formato de T, la estimación inicial para el vector de variables
articulares será la dada en el parámetro Q0 (puede ser una columna o una fila), y en el
caso de que no se lo demos, asume que es el vector nulo.
Para el caso de un manipulador con menos de 6 grados de libertad el efector
final no podrá alcanzar algunas posiciones y orientaciones. Esto normalmente lleva a
una no convergencia de IKINE. Una solución consiste en especificar un vector (fila o
columna) de pesos (M), cuyos elementos serán 0 para aquellos grados de libertad que
en cartesianas estén restringidos, y 1 en otro caso. Los elementos de M se corresponden
con las traslaciones a lo largo de los ejes X, Y y Z, y con las rotaciones entorno a los
ejes X, Y y Z.
Por ejemplo si el manipulador no se puede desplazar a lo largo del eje Z, ni rotar
entorno a los ejes X e Y, M deberá ser el vector [1 1 0 0 0 1]. El número de elementos
no nulos debe ser igual al número de grados de libertad del robot.
ILIMIT es el número máximo de iteraciones que se ejecutarán en busca de una
solución (un valor usual es 1000).
STOL será la máxima diferencia que se admitirá entre la transformación
correspondiente a las variables articulares solución y la transformación con la posición
y orientación especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo
uso de la función TR2DIFF.
En nuestro caso usaremos la fórmula Q = IKINE(DH, STOL, ILIMIT, T, Q0,
M) con:
-
DH: El creado anteriormente.
STOL: 1e-3.
ILIMT: 1000.
T: [1 0 0 str2num(get(xedit,'String'));0 1 0 str2num(get(yedit,'String'));0 0 1
str2num(get(zedit,'String'));0 0 0 1].
Q0: matriz de ceros del tamaño adecuado.
M: [1 1 1 1 1 1].
ÁLVARO GÓMEZ RAMOS.
- 133 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ya tenemos los valores requeridos en “q” y procedemos a dibujar el robot en un
nuevo “figure” en la posición resultante:
4.30 Posicionar robot
Para posicionar el robot en la posición deseada usamos la función “env_bot” la
cual realiza los cálculos necesarios:
Lo primero a realizar es calcular el número de articulaciones, el tipo y el orden
de ellas. Para ello recorremos la matriz “MR” y cuando vemos una articulación la
contabilizamos y guardamos su nombre:
Ahora sustituimos los nombres de las variables por el valor cero en la matriz
“P_D_H” para tenerla en el mismo formato que usa el toolbox de robótica:
ÁLVARO GÓMEZ RAMOS.
- 134 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora creamos una nueva ventana para introducir los valores articulares con los
que deseamos posicionar el robot, y también daremos la opción de elegir si lo queremos
representar de forma esquemática solamente o también de forma tridimensional:
ÁLVARO GÓMEZ RAMOS.
- 135 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Ahora creo una matriz del robot típica del toolbox:
Y ahora creo el robot en el formato del toolbox:
Ya tenemos la ventana en la que podemos pulsar el botón para posicionar el
robot, lo cual ejecutará la función “intdata”. Lo primero a realizar en “intdata” es
obtener los valores introducidos por el usuario para posicionar el robot:
Ahora dibujamos el robot de forma esquemática en una nueva ventana:
ÁLVARO GÓMEZ RAMOS.
- 136 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
Lo siguiente es comprobar si se quiere dibujar el robot en 3D, para ello
comprobamos el valor de la variable “checkdata” que puede variar su valor a través de
la función “check”:
Si la opción está activada, es decir, “checkdata=1”, cambiamos el valor de las
variables al introducido por el usuario:
Preparamos el plot principal del programa para volver a dibujar el robot en la
posición deseada:
Ejecutamos la función “dibubot” que nos dibujará el robot en la posición final
en la que queríamos representarlo:
ÁLVARO GÓMEZ RAMOS.
- 137 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
4.31 Ayuda
Para llamar a la ayuda se hace a través de la función “ayuda”, que abre una
página web (previamente creada con toda la ayuda) en un explorador propio de matlab:
4.32 Función auxiliar: Multiplicar vectorialmente vectores simbólicos
Para poder multiplicar vectores simbólicos hemos creado nuestra propia función
ya que desconocemos la existencia de una función para dicho fin:
4.33 Función auxiliar: Redondear a cero los valores de las matrices
Se ha creado una función que me recorre las matrices y cuando ve un valor
menor que 0.0001 en alguno de sus términos, lo iguala a cero:
ÁLVARO GÓMEZ RAMOS.
- 138 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
BLOQUE 3: CONCLUSIONES
ÁLVARO GÓMEZ RAMOS.
- 139 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1
Conclusiones.
El proyecto nació con varios objetivos que según se fue desarrollando fueron
variando.
Se quería diseñar un software interactivo en el que se pudieran diseñar robots en
tres dimensiones para luego poder obtener los ejes del robot y los parámetros de
Denavit-Hartenberg. Esta sería la parte novedosa del software.
Luego existiría otra parte del software que consistiese en la integración de
herramientas ya existentes, para conseguir un paquete que incluya todas las
herramientas que podamos considerar interesantes para el trabajo con robots. En esta
sección se incluirían la resolución del problema cinemática directa e inversa, así como
la dinámica directa e inversa. También se comentó la posibilidad de desarrollar ciertos
bloques de simulink para la simulación, incluso la posibilidad de que alguno de estos
bloques fuera desarrollado en el lenguaje de programación “C” para que tuviese una
ejecución más rápida.
La mayor parte del peso del proyecto se basa en la parte gráfica del software,
sobre todo en lo que se trata de diseño en tres dimensiones. Otra de las cuestiones a las
que más tiempo se ha dedicado es a la matemática realizada con simbólico. Es decir, lo
difícil fue dibujar el robot según lo indique el usuario y poder posicionarlo o moverlo
porque ello implica una matemática compleja de matrices simbólicas.
En cuanto a la parte puramente relacionada con la robótica como puede ser la
cinemática directa e inversa fue relativamente rápida su integración, porque ya
teníamos unas matrices y variables previamente calculadas que nos facilitan el trabajo.
El proyecto no tenía un fin claro, pues no se conocían con suficiente exactitud
las dificultades que irían surgiendo, por lo que una vez desarrollado todo el diseño y
cálculo de ejes y parámetros de Denavit-Hartenberg se optó por añadirle también la
cinemática y dejar la dinámica y otras cuestiones para futuras ampliaciones.
Es decir, se trata de un proyecto abierto con muchas posibilidades de
ampliación, en el cual se han dejado sentadas unas bases más que suficientes para poder
ser de cierta utilidad en esta su primera versión.
ÁLVARO GÓMEZ RAMOS.
- 140 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
2
Desarrollos futuros.
Como hemos dicho este es un proyecto abierto que podrá ser ampliado y
modificado en un futuro.
Lo primero a ampliar sería añadir la dinámica directa e inversa y la posibilidad
de añadir bloques propios a simulink para poder simular el robot.
Pero se pueden hacer muchas más cosas, como la posibilidad de mover el robot
siguiendo trayectorias, o la inclusión de un segundo robot con el que interactuar, o
incluso el diseño de elementos del entorno de trabajo. Quizás para este hecho sea mejor
realizar un cambio en la plataforma de desarrollo y pasar de matlab a algún lenguaje
más eficiente, sobre todo en la parte gráfica.
Las posibilidades de ampliación son inmensas, y hacen de este software una
herramienta muy interesante para el trabajo con robots. Se podría llegar a obtener un
paquete de herramientas muy completo e integrado, consiguiendo un tipo de software
poco desarrollado hasta el día de hoy.
Sobre todo Sicon-Bot se pretende centrar en la ayuda al estudio de la robótica,
es decir, una herramienta para el estudiante, pero con futuras ampliaciones podría llegar
a ser de cierta utilidad profesional, sobre todo si se consigue compatibilizar con otras
herramientas ya existentes.
ÁLVARO GÓMEZ RAMOS.
- 141 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
BLOQUE 4: ANEXOS
ÁLVARO GÓMEZ RAMOS.
- 142 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
1
ANEXO: CÓDIGO FUENTE.
function ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función de ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
web index.htm
ÁLVARO GÓMEZ RAMOS.
- 143 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una base, situado entre un punto
inicial y
%
otro final. La base dibujada llevará
%
el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
%
[xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos datos de altura de la base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi,ange);
z2]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi,ange);
z3]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi,ange);
z4]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi,ange);
z5]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi+l,ange);
z6]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi+l,ange);
z7]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi+l,ange);
z8]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi+l,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
ÁLVARO GÓMEZ RAMOS.
- 144 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 145 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=base(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una base, situado en un punto inicial y
%
que nos devuelve un punto final. La base dibujada llevará
%
los ángulos de inclinación indicados por el usuario:
%
[xf yf zf]=base(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos valor de altura de la base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
l=str2num(get(htexto,'String'));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cogemos los valores de los ángulos y los pasamos a
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angx=-str2num(get(htextox,'String'))*2*pi/360;
angz=-str2num(get(htextoz,'String'))*2*pi/360;
ange=str2num(get(htextoe,'String'))*2*pi/360;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-2,yi-2,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+2,yi-2,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+2,yi+2,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-2,yi+2,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-2,yi-2,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+2,yi-2,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+2,yi+2,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-2,yi+2,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
ÁLVARO GÓMEZ RAMOS.
- 146 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 147 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function ai=calc_ai(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
calculamos ai matemáticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(MRejes,1);
for i=1:n-1
P=MRejes(i,[10 11 12]);
v=MRejes(i,[1 2 3]);
Q=MRejes(i+1,[10 11 12]);
w=MRejes(i+1,[1 2 3]);
q4=simple(P(1)*v(1)+P(2)*v(2)+P(3)*v(3));
k1=Q(1)-P(1);
k2=Q(2)-P(2);
k3=Q(3)-P(3);
mod_v1=sqrt(v(1)^2+v(2)^2+v(3)^2);
A=[v(1)/mod_v1 w(1);v(2)/mod_v1 w(2);v(3)/mod_v1 w(3)];
B=[k1 v(1) w(1);k2 v(2) w(2); k3 v(3) w(3)];
if (rank(A)==1)&(rank(B)==1)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
ai(i)=mod_QP/mod_v;
end
if (rank(A)==1)&(rank(B)==2)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
ai(i)=mod_QP/mod_v;
end
if (rank(A)==2)&(rank(B)==2)
a=[v(1) w(1) Q(1)-P(1);v(2) w(2) Q(2)-P(2);v(3) w(3) Q(3)P(3)];
b=cross(v,w);
mod_QP=0;
mod_v=0;
deta=det(a);
for j=1:3
mod_v=mod_v+b(j)^2;
end
ÁLVARO GÓMEZ RAMOS.
- 148 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
mod_v=sqrt(mod_v);
ai(i)=deta/mod_v;
end
if (rank(A)==2)&(rank(B)==3)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
ai(i)=mod_QP/mod_v;
end
end
end
ÁLVARO GÓMEZ RAMOS.
- 149 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function alphai=calc_alphai(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
calcula alphai matemáticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(MRejes,1);
clear alphai
for i=1:n-1
v=MRejes(i,[1 2 3]);
w=MRejes(i+1,[1 2 3]);
alphai(i)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2
)^2+v(3)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
x=cross(v,w);
if (x(1)~=MRejes(i,4))|(x(2)~=MRejes(i,5))|(x(3)~=MRejes(i,6))
alphai(i)=-alphai(i);
end
end
end
ÁLVARO GÓMEZ RAMOS.
- 150 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function di=calc_di(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculo di matemáticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(MRejes,1);
clear di
for i=2:n
P=MRejes(i-1,[10 11 12]);
v=MRejes(i-1,[4 5 6]);
Q=MRejes(i,[10 11 12]);
w=MRejes(i,[4 5 6]);
q4=simple(P(1)*v(1)+P(2)*v(2)+P(3)*v(3));
k1=Q(1)-P(1);
k2=Q(2)-P(2);
k3=Q(3)-P(3);
mod_v1=sqrt(v(1)^2+v(2)^2+v(3)^2);
A=[v(1)/mod_v1 w(1);v(2)/mod_v1 w(2);v(3)/mod_v1 w(3)];
B=[k1 v(1) w(1);k2 v(2) w(2); k3 v(3) w(3)];
if (rank(A)==1)&(rank(B)==1)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
di(i-1)=mod_QP/mod_v;
end
if (rank(A)==1)&(rank(B)==2)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
di(i-1)=mod_QP/mod_v;
end
if (rank(A)==2)&(rank(B)==2)
a=[v(1) w(1) Q(1)-P(1);v(2)
P(3)];
b=cross(v,w);
mod_QP=0;
mod_v=0;
deta=det(a);
for j=1:3
mod_v=mod_v+b(j)^2;
ÁLVARO GÓMEZ RAMOS.
%coincidentes
%paralelos
%se cortan
w(2) Q(2)-P(2);v(3) w(3) Q(3)-
- 151 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
end
mod_v=sqrt(mod_v);
di(i-1)=deta/mod_v;
end
if (rank(A)==2)&(rank(B)==3)
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
mod_QP=mod_QP+a(j)^2;
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_QP=sqrt(mod_QP);
mod_v=sqrt(mod_v);
di(i-1)=mod_QP/mod_v;
end
%se cruzan
end
end
ÁLVARO GÓMEZ RAMOS.
- 152 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function tethai=calc_tethai(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos el valor de tethai
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global q1 q2 q3 t1 Q2_simb Q2_simb2
syms Q2_simb2
syms x positive
n=size(MRejes,1);
%Nº de ejes del robot
clear tethai
a=0;
for i=2:n
if MRejes(i,13)==1
%Si es de rotación hago cambio de
variable para que el valor de tethai sea solo el nombre de la variable
correspondiente
a=a+1;
tethai(i-1)= Q2_simb(a);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculo cual sería el valor de tethai sin el cambio de
variable,
%
por si puedo necesitarlo mas adelante, y lo guardo en
Q2_simb2(a)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
v=MRejes(i-1,[4 5 6]);
w=MRejes(i,[4 5 6]);
Q2_simb2(a)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v
(2)^2+v(3)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
x=cross(v,w);
mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2)));
x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x];
if (x(1)~=MRejes(i,1))|(x(2)~=MRejes(i,2))|(x(3)~=MRejes(i,3))
Q2_simb2(a)=2*pi-Q2_simb2(a);
end
Q2_simb2_aux(a)=subs(Q2_simb2(a),Q2_simb(a),0);
Q2_simb2(a)=Q2_simb2(a)-Q2_simb2_aux(a);
else
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Resuelvo la matemática necesaria para calcular tethai
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
v=MRejes(i-1,[4 5 6]);
w=MRejes(i,[4 5 6]);
x=cross(v,w);
tethai(i1)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2)^2+v(3
)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2)));
ÁLVARO GÓMEZ RAMOS.
- 153 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x];
if (x(1)~=MRejes(i,1))|(x(2)~=MRejes(i,2))|(x(3)~=MRejes(i,3))
tethai(i-1)=-tethai(i-1);
end
end
end
end
ÁLVARO GÓMEZ RAMOS.
- 154 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function calcbot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculo los ejes del robot a partir de la matriz MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR n Q2_simb MRperp MRvec mfile_save graph_DH graph_calcbot
htras heje hrot hgarra hbase MRejes t1 q1 q2 q3 MR_simb graph_cindir
graph_cininv
MR(n+1,:)= [3 MR(n,[5 6 7]) MR(n,[5 6 7]) 0];
%Añadimos un falso
eslabón al final del robot con longitud 0 para que este termine en
eslabón
n=n+1;
%Contamos este
nuevo tramo añadido
for i=1:n
MRaux(i,:)=MR(i,:); %variable aux de MR
end
clear MR
a=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
rotamos de forma simbólica el robot según las variables de
rotación
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
if MRaux(i,1)==2
a=a+1;
for j=i+1:n
[MRaux(j,2) MRaux(j,3)
MRaux(j,4)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6)
,MRaux(i,7),MRaux(j,2),MRaux(j,3),MRaux(j,4),Q2_simb(a));
[MRaux(j,5) MRaux(j,6)
MRaux(j,7)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6)
,MRaux(i,7),MRaux(j,5),MRaux(j,6),MRaux(j,7),Q2_simb(a));
MRaux=simple(MRaux);
end
MRaux=simple(MRaux);
end
end
MR=MRaux;
MR_simb=MRaux;
%Creamos
valores simbólicos de rotación
MRvec=simple(MRtoMRvec(MRaux));
%Paso de
matriz con vector del eje de la articulacion
MRperp=simple(MRvectoMRperp(MRvec,MRaux)); %Obtengo
vector z + punto de origen + vector x
MRejes=simple(plotejes(MRperp));
%Obtengo
y los dibujo
MR con los
matriz de robot a
la matriz con
el resto de ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
habilitaciones de nuevas opciones del menu
ÁLVARO GÓMEZ RAMOS.
- 155 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(mfile_save,'Enable','on')
set(graph_DH,'Enable','on')
set(graph_calcbot,'Enable','off')
set(htras,'Enable','off');
set(heje,'Enable','off');
set(hrot,'Enable','off');
set(hgarra,'Enable','off');
set(hbase,'Enable','off');
set(graph_cindir,'Enable','on');
end
ÁLVARO GÓMEZ RAMOS.
- 156 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function cin_dir
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Muestra la cinemática directa que se obtiene directamente del punto
final
% del último elemento del robot, que está almacenado en MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR_simb n P_D_H DH robotd2 Q2_simb Q1_simb
figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','on',...
'Name','Cinemática Inversa',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','off',...
'BusyAction','cancel',...
'Position',[0.15 0.15 0.6 0.6]);
uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(MR_simb(n,5)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.4 0.7 .5
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(MR_simb(n,6)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.4 0.5 .5
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(MR_simb(n,7)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.4 0.3 .5
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','x=',...
'backgroundcolor',[1 1 1],...
'ForegroundColor','b',...
'FontWeight','bold',...
'Units','normalized',...
ÁLVARO GÓMEZ RAMOS.
- 157 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Position',[.2 0.7 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','y=',...
'backgroundcolor',[1 1 1],...
'ForegroundColor','b',...
'FontWeight','bold',...
'Units','normalized',...
'Position',[.2 0.5 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','z=',...
'backgroundcolor',[1 1 1],...
'ForegroundColor','b',...
'FontWeight','bold',...
'Units','normalized',...
'Position',[.2 0.3 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
end
ÁLVARO GÓMEZ RAMOS.
- 158 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function cin_inv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que calcula la cinemática inversa del robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2
hdata robotd2 hcheck checkdata DH tras rotac xedit yedit zedit
n=size(P_D_H,1);
checkdata=0;
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%Nº de filas
%Variable de checkbox
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el nº de articulaciones, el tipo y el orden
%
de ellas.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:size(MRejes,1)
if MRejes(i,13)==1
a(i-1)=0;
cont1=cont1+1;
Var(i-1)=Q2_simb(cont1);
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
Var(i-1)=Q1_simb(cont2);
end
end
a(cont1+cont2+1)=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nueva ventana para introducir los datos de las variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot
P_D_Haux=subs(P_D_Haux,Q1_simb(j),0);
end
for j=1:num_simb_rot2
P_D_Haux=subs(P_D_Haux,Q2_simb(j),0);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creo la matriz del robot para usarla en el toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
siconbot{i}=link([P_D_Haux(i,1) P_D_Haux(i,2) P_D_Haux(i,3)
P_D_Haux(i,4) a(i)],'modified');
end
ÁLVARO GÓMEZ RAMOS.
- 159 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cálculo de DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
DH=P_D_Haux;
for i=1:(size(DH,1))
DH(i,5)=a(i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
ventana para introducir datos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','on',...
'Name','Cinemática Inversa',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','off',...
'BusyAction','cancel',...
'Position',[0.15 0.15 0.6 0.6]);
hintdata = uicontrol('style','pushbutton',...
'Callback',@intdata2,...
'backgroundcolor',[.724, .724, .724],...
'String','Calcular',...
'Units','normalized',...
'Position',[.8 .5 .1 .05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','x =',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.1 0.7 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%introducir x
xedit=uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.2 0.7 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
ÁLVARO GÓMEZ RAMOS.
- 160 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','y =',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.1 0.5 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
% Introducir y
yedit=uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.2 0.5 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
uicontrol('Style', 'text', ...
'Callback',@label,...
'String','z =',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.1 0.3 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
% Introducir z
zedit=uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.2 0.3 .1
.05],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nos dibuja las variables y su valor y
%
las coloca en la ventana dependiendo del nº de variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
b=1/(n-1);
c=1+b/2;
for i=1:n-1
uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(Var(1,i)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
ÁLVARO GÓMEZ RAMOS.
- 161 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Position',[.4 c-b .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
hdata(i) = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.6 c-b .1
.05],...
'Enable','off',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
c=c-b;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
creamos el robot según toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
robotd2=robot(siconbot,'Robot de siconbot');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = intdata2(varargin) %Calcular cinemática inversa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que nos calcula la cinemática inversa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hdata robotd2
DH xedit yedit zedit tras rotac P_D_H
n=size(P_D_H,1);
stol=1e-3;
ilimit=1000;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Matriz de posición final deseada necesaria para "ikine"
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
TG = [1 0 0 str2num(get(xedit,'String'));0 1 0
str2num(get(yedit,'String'));0 0 1 str2num(get(zedit,'String'));0 0 0
1];
vectorq0=zeros(1,tras+rotac-1);
%Posición inicial
q=ikine(DH,stol,ilimit,TG,vectorq0,[1 1 1 1 1 1]); %Cálculo de
cinemática Inversa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el robot
ÁLVARO GÓMEZ RAMOS.
- 162 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
plot(robotd2,q)
axis on
AXIS([0 30 0 30 0 40]);
camorbit(165,0)
grid on
%Posicionar robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Mostrar valores de variables articulares
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n-1
set(hdata(i),'String',num2str(q(i)));
end
end
function V3=cross_sym(V1,V2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
multiplica vectorialmente vectores simbólicos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
V3=[det([V1(2) V1(3);V2(2) V2(3)]) det([V1(1) V1(3);V2(1) V2(3)])
det([V1(1) V1(2);V2(1) V2(2)])];
ÁLVARO GÓMEZ RAMOS.
- 163 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function dibubot(MR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja el robot equivalente a la matriz MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global n
sp1 num_simb_rot Q1_simb Q1 Q2_simb num_simb_rot2 Q2
subplot(sp1)
MR=simple(MR);
%Seleccionamos subplot principal
%Simplificamos MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos MR con valores reales
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot
MR=subs(MR,Q1_simb(j),Q1(j));
variables
end
for j=1:num_simb_rot2
MR=subs(MR,Q2_simb(j),-Q2(j));
variables
end
%Mr para valores concretos de las
%Mr para valores concretos de las
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibuja el tipo de elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
switch MR(i,1)
case
1
traslac2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
2
eje2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
3
eslabon2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
4
base2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
5
garra2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
end
end
ÁLVARO GÓMEZ RAMOS.
- 164 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=eje2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una eje de rotación, situado entre un
punto inicial y
%
otro final. La base dibujada llevará
%
el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
%
[xf yf zf]=base(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculo pto dnd se centrará el eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xi1=(xi+xf)/2;
yi1=(yi+yf)/2;
zi1=(zi+zf)/2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos la longitud del tramo para obtener el ángulo de
giro
%
sobre el eje x a partir de zf y zi. Dependiendo del signo
de
%
(yf-yi) el giro será en un sentido o en otro.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2);
% if (xi==xf)&(yi==yf)
%
angz=0;
%
angx=0;
% else
%
angx=-(pi/2-asin((zf-zi)/l));
%
lx=xf-xi;
%
ly=yf-yi;
%
l1=sqrt(lx^2+ly^2);
%
V=[lx/l1 ly/l1 0]*l;
%
xf1=V(1);
%
yf1=V(2);
%
zf1=V(3);
%
angz=-(pi/2-acos(xf1/l));
% end
if (yf-yi)>0.0001
angx=-(pi/2-asin((zf-zi)/l));
else
angx=(pi/2-asin((zf-zi)/l));
end;
%ángulo positicvo
%ángulo negativo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el giro sobre el eje z a partir del pto inicial
y
%
final.
ÁLVARO GÓMEZ RAMOS.
- 165 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf)
%Si tramo es vertical
angz=0;
%Ángulo y=0
else
lx=xf-xi;
%Longitud en x
ly=yf-yi;
%Longitud en y
l1=sqrt(lx^2+ly^2);
%Longitud en plano XY
V=[lx/l1 ly/l1 0]*l;
%Proyección en eje XY
xf1=V(1);
yf1=V(2);
zf1=V(3);
angz=(pi/2-acos(xf1/l));
%ángulo de y
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1-0.5,angx,angz);
z2]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1-0.5,angx,angz);
z3]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1-0.5,angx,angz);
z4]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1-0.5,angx,angz);
z5]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1+0.5,angx,angz);
z6]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1+0.5,angx,angz);
z7]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1+0.5,angx,angz);
z8]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1+0.5,angx,angz);
[xf yf zf]=rotar(xi1,yi1,zi1,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
end
ÁLVARO GÓMEZ RAMOS.
- 166 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[1 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 167 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=eje(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja un eje de rotación, situado en un punto
inicial y
%
que nos devuelve un punto final. El eje dibujado llevará
%
los ángulos de inclinación indicados por el usuario:
%
[xf yf zf]=eje(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
n MR htextot1 num_simb_rot2 Q2_simb Q2 rotac num_simb_rot Q1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos datos de variable
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
num_simb_rot2=num_simb_rot2+1;
l=str2num(get(htexto,'String'));
a=get(htextot1,'String');
Q2_simb(num_simb_rot2)=a;
L=sym(a);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cogemos los valores de los ángulos y los pasamos a
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angx=-str2num(get(htextox,'String'))*2*pi/360;
angz=-str2num(get(htextoz,'String'))*2*pi/360;
ange=str2num(get(htextoe,'String'))*2*pi/360;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
y1
y2
y3
y4
y5
z1]=rotar(xi,yi,zi,xi,yi-0.6,zi-0.5,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.6,yi,zi-0.5,angx,angz);
z3]=rotar(xi,yi,zi,xi,yi+0.6,zi-0.5,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.6,yi,zi-0.5,angx,angz);
z5]=rotar(xi,yi,zi,xi,yi-0.6,zi+0.5,angx,angz);
ÁLVARO GÓMEZ RAMOS.
- 168 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
[x6 y6 z6]=rotar(xi,yi,zi,xi+0.6,yi,zi+0.5,angx,angz);
[x7 y7 z7]=rotar(xi,yi,zi,xi,yi+0.6,zi+0.5,angx,angz);
[x8 y8 z8]=rotar(xi,yi,zi,xi-0.6,yi,zi+0.5,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x1,y1,z1,ange);
z2]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x2,y2,z2,ange);
z3]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x3,y3,z3,ange);
z4]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x4,y4,z4,ange);
z5]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x5,y5,z5,ange);
z6]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x6,y6,z6,ange);
z7]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x7,y7,z7,ange);
z8]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x8,y8,z8,ange);
MR(n,[2 3 4])=[(x1+x3)/2 (y1+y3)/2 (z1+z3)/2];
MR(n,[5 6 7])=[(x5+x7)/2 (y5+y7)/2 (z5+z7)/2];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustituimos las variables por su valor
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:num_simb_rot
x1=subs(x1,Q1(i));
x2=subs(x2,Q1(i));
x3=subs(x3,Q1(i));
x4=subs(x4,Q1(i));
x5=subs(x5,Q1(i));
x6=subs(x6,Q1(i));
x7=subs(x7,Q1(i));
x8=subs(x8,Q1(i));
y1=subs(y1,Q1(i));
y2=subs(y2,Q1(i));
y3=subs(y3,Q1(i));
y4=subs(y4,Q1(i));
y5=subs(y5,Q1(i));
y6=subs(y6,Q1(i));
y7=subs(y7,Q1(i));
y8=subs(y8,Q1(i));
z1=subs(z1,Q1(i));
z2=subs(z2,Q1(i));
z3=subs(z3,Q1(i));
z4=subs(z4,Q1(i));
z5=subs(z5,Q1(i));
z6=subs(z6,Q1(i));
z7=subs(z7,Q1(i));
z8=subs(z8,Q1(i));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
ÁLVARO GÓMEZ RAMOS.
- 169 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[1 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 170 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function env_bot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcion que nos permite posicionar el robot en la posición
%
indicada, según el valor de las articulaciones
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2
hdata robotd2 hcheck checkdata DH
n=size(P_D_H,1);
checkdata=0;
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%Nº de filas
%Variable de checkbox
%Contador
%Contador
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el nº de articulaciones, el tipo y el orden
%
de ellas.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:size(MRejes,1)
if MRejes(i,13)==1
a(i-1)=0;
articular
cont1=cont1+1;
Var(i-1)=Q2_simb(cont1);
articulares ordenadas
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
Var(i-1)=Q1_simb(cont2);
end
end
a(cont1+cont2+1)=0;
ficticia al final del robot
%Articulación de traslacion
%Vector con tipo de variable
%Vector con variables
%Creamos un articulación
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Damos valor cero a las variables articulares para adaptarlas
al
%
formato del toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot
P_D_Haux=subs(P_D_Haux,Q1_simb(j),0);
end
for j=1:num_simb_rot2
P_D_Haux=subs(P_D_Haux,Q2_simb(j),0);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nueva ventana para introducir los datos de las variables
ÁLVARO GÓMEZ RAMOS.
- 171 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','on',...
'Name','Introducir datos',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','off',...
'BusyAction','cancel',...
'Position',[0.15 0.15 0.6 0.6]);
%
Botón
hintdata = uicontrol('style','pushbutton',...
'Callback',@intdata,...
'backgroundcolor',[.724, .724, .724],...
'String','Intr. Dato',...
'Units','normalized',...
'Position',[.6 .6 .1 .05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%
Checkbox
hcheck = uicontrol('style','checkbox',...
'Callback',@check,...
'backgroundcolor',[1 1 1],...
'String','Ver Robot en
3D',...
'Units','normalized',...
'Position',[.6 .4 .15
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nos dibuja las variables para que le podamos asignar un valor
y
%
las coloca en la ventana dependiendo del nº de variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
b=1/(n-1);
%Variable para colocar la ventana
c=1+b/2;
%Variable con valor inicial para
colocar la ventana
for i=1:n-1
uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(Var(1,i)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[.2 c-b .1
.05],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
ÁLVARO GÓMEZ RAMOS.
- 172 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
hdata(i) = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.4 c-b .1
.05],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
c=c-b;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creo la matriz del robot para usarla en el toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
siconbot{i}=link([P_D_Haux(i,1) P_D_Haux(i,2) P_D_Haux(i,3)
P_D_Haux(i,4) a(i)],'modified');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
creamos el robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
robotd2=robot(siconbot,'Robot de siconbot');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = intdata(varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que nos dibuja el robot según los datos introducidos y
las
%
opciones elegidas
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hdata robotd2 MRejes Q2 Q1 MR sp1 hf1 n Q2_simb Q1_simb
Q2_simb2 checkdata MR_simb
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos los valores de las articulaciones
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:size(hdata,2)
vector(i)=str2num(get(hdata(i),'String'));
end
vector(i+1)=0;
%Ultima articulación(Ficticia)=0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 173 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%
Dibujamos el robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
plot(robotd2,vector)
%dibuja el robot
axis on
AXIS([0 30 0 30 0 40]);
camorbit(165,0)
grid on
if checkdata==1
% si queremos dibujarlo en 3D
cont2=0;
cont1=0;
for i=1:size(hdata,2)
if MRejes(i+1,13)==1
Q2(cont2+1)=str2num(get(hdata(i),'String'));
%valor de
variables
cont2=cont2+1;
end
if MRejes(i+1,13)==2
Q1(cont1+1)=str2num(get(hdata(i),'String'));
%Valor de
variables
cont1=cont1+1;
end
end
figure(hf1)
%Seleccionamos figure principal de
siconbot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Preparamos el subplot principal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
delete (sp1)
sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]);
xlabel('eje x');
ylabel('eje y');
axis on
AXIS([-30 30 -30 30 0 40]);
camorbit(165,0)
grid on
subplot(sp1)
for i=1:n
MRaux(i,:)=MR(i,:); %variable aux de MR
end
clear MR
a=0;
MR=MR_simb;
dibubot(MR)
%dibuja robot en 3D
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = check(varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 174 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%
Función que detecta si se activa ver el robot en 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global checkdata
if checkdata==0
checkdata=1;
else
chackdata=0;
end
end
ÁLVARO GÓMEZ RAMOS.
- 175 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja un eslabón, situado entre un punto
inicial y
%
otro final. La base dibujada llevará
%
el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
%
[xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos la longitud del tramo para obtener el ángulo de
giro
%
sobre el eje x a partir de zf y zi. Dependiendo del signo
de
%
(yf-yi) el giro será en un sentido o en otro.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2);
tramos
if (yf-yi)>0.0001
angx=-(pi/2-asin((zf-zi)/l));
else
angx=(pi/2-asin((zf-zi)/l));
end;
%Longitud del
%ángulo positicvo
%ángulo negativo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el giro sobre el eje z a partir del pto inicial
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf)
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
V=[lx/l1 ly/l1 0]*l;
xf1=V(1);
yf1=V(2);
zf1=V(3);
angz=(pi/2-acos(xf1/l));
end
%Si tramo es vertical
%Ángulo y=0
%Longitud en x
%Longitud en y
%Longitud en plano XY
%Proyección en eje XY
%ángulo de y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 176 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.8 .4
.125],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 177 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=eslabon(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una eslabón, situado en un punto
inicial y
%
que nos devuelve un punto final. El eslabón dibujado
llevará
%
los ángulos de inclinación indicados por el usuario:
%
[xf yf zf]=eslabon(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
tras Q1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el nº de elementos de traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if tras>1
tras2=tras-1;
else tras2=tras;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos datos de longitud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
l=str2num(get(htexto,'String'));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cogemos los valores de los ángulos y los pasamos a
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angx=-str2num(get(htextox,'String'))*2*pi/360;
angz=-str2num(get(htextoz,'String'))*2*pi/360;
ange=str2num(get(htextoe,'String'))*2*pi/360;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
ÁLVARO GÓMEZ RAMOS.
- 178 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustituimos las variables por su valor
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
x1=subs(x1,Q1(tras2));
x2=subs(x2,Q1(tras2));
x3=subs(x3,Q1(tras2));
x4=subs(x4,Q1(tras2));
x5=subs(x5,Q1(tras2));
x6=subs(x6,Q1(tras2));
x7=subs(x7,Q1(tras2));
x8=subs(x8,Q1(tras2));
y1=subs(y1,Q1(tras2));
y2=subs(y2,Q1(tras2));
y3=subs(y3,Q1(tras2));
y4=subs(y4,Q1(tras2));
y5=subs(y5,Q1(tras2));
y6=subs(y6,Q1(tras2));
y7=subs(y7,Q1(tras2));
y8=subs(y8,Q1(tras2));
z1=subs(z1,Q1(tras2));
z2=subs(z2,Q1(tras2));
z3=subs(z3,Q1(tras2));
z4=subs(z4,Q1(tras2));
z5=subs(z5,Q1(tras2));
z6=subs(z6,Q1(tras2));
z7=subs(z7,Q1(tras2));
z8=subs(z8,Q1(tras2));
ÁLVARO GÓMEZ RAMOS.
- 179 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.8 .4
.125],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 180 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una garra, situado entre un punto
inicial y
%
otro final. La garra dibujada llevará
%
el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
%
[xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos la longitud del tramo para obtener el ángulo de
giro
%
sobre el eje x a partir de zf y zi. Dependiendo del signo
de
%
(yf-yi) el giro será en un sentido o en otro.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2);
angx=-(pi/2-asin((zf-zi)/l));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el giro sobre el eje z a partir del pto inicial
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf)
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
V=[lx/l1 ly/l1 0]*l;
xf1=V(1);
yf1=V(2);
zf1=V(3);
angz=-(pi/2-acos(xf1/l));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
ÁLVARO GÓMEZ RAMOS.
- 181 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
y1
y2
y3
y4
y5
z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5];
caras=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 182 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=garra(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja una garra, situada en un punto inicial
y
%
que nos devuelve un punto final. La garra dibujada llevará
%
los ángulos de inclinación indicados por el usuario:
%
[xf yf zf]=garra(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
l=str2num(get(htexto,'String'));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cogemos los valores de los ángulos y los pasamos a
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angx=-str2num(get(htextox,'String'))*2*pi/360;
angz=-str2num(get(htextoz,'String'))*2*pi/360;
ange=str2num(get(htextoe,'String'))*2*pi/360;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
y1
y2
y3
y4
y5
z1]=rotar(xi,yi,zi,xi-0.5,yi-0.5,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.5,yi-0.5,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.5,yi+0.5,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.5,yi+0.5,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
ÁLVARO GÓMEZ RAMOS.
- 183 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
[x2
[x3
[x4
[x5
end
y2
y3
y4
y5
z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5];
caras=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0
0],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 184 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function loadbot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que carga un robot almacenado en disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
sp6 sp7 n
num_simb_rot Q1_simb Q1 MRejes Q2_simb num_simb_rot2 Q2
htras heje hrot hgarra hbase graph_DH graph_calcbot MR_simb
graph_cindir graph_cininv rotac tras
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
carga las variables del archivo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[fname,pname] = uigetfile('*.mat','Select File');
dbfile= strcat(pname,fname);
if length(dbfile) == 0 return; end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
carga las variables al workspace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
load(fname)
for i=1:n
MRaux(i,:)=MR(i,:);
end
clear MR
a=0;
MR=MR_simb;
dibubot(MR)
exacta
MRejes=simple(plotejes(MRperp));
dibujo
%variable aux de MR
%dibuja el robot en esa posicion
%Obtengo el resto de ejes y los
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilitacion de nuevas opciones en el menu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(htras,'Enable','off');
set(heje,'Enable','off');
set(hrot,'Enable','off');
set(hgarra,'Enable','off');
set(hbase,'Enable','off');
set(graph_DH,'Enable','on')
set(graph_calcbot,'Enable','off')
set(graph_cindir,'Enable','on')
return
ÁLVARO GÓMEZ RAMOS.
- 185 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function MRvec=MRtoMRvec(MR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que transforma la matriz con las coordenadas de los
%
eslabones en una matriz con los ejes de las articulaciones de
%
rotación y traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
num=0;
n=size(MR,1);
MRvec=sym([0 0 0 0 0 0 0]);
for i=1:n
x0=MR(i,2);
y0=MR(i,3);
z0=MR(i,4);
if (MR(i,1)==1)|(MR(i,1)==2)
x1=MR(i,5);
y1=MR(i,6);
z1=MR(i,7);
num=num+1;
%eje rot o tras
if MR(i,1)==2
%si la articulacion es de rotacion el eje se calcula:
MRvec(num,:)=simple([(x0+x1)/2 (y0+y1)/2 (z0+z1)/2 x1-x0
y1-y0 z1-z0 1]);
else
%si la articulacion es de traslaccion el eje se calcula:
MRvec(num,:)=simple([x1 y1 z1 x1-x0 y1-y0 z1-z0 2]);
end
end
if (i==n)&(MR(i,1)~=1)&(MR(i,1)~=2) %si es un ultimo eslabon y no
es una articulacion
x1=MR(i,5);
y1=MR(i,6);
z1=MR(i,7);
num=num+1;
MRvec(num,:)=simple([MR(i-1,2) MR(i-1,3) MR(i-1,4) MR(i-1,5)MR(i-1,2) MR(i-1,6)-MR(i-1,3) MR(i-1,7)-MR(i-1,4) 0]);
end
end
end
ÁLVARO GÓMEZ RAMOS.
- 186 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function MRperp=MRvectoMRperp(MRvec,MR)
%
Función que transforma la matriz con los ejes principales de
las articulaciones
%
en una matriz que contiene la perpendicular a cada eje
global sp1 num_simb_rot Q1_simb Q1 num_simb_rot2 Q2_simb Q2
n=size(MRvec,1);
%nº de articulaciones
n2=size(MR,1); %nº elementos del robot
MRperp=sym([0 0 0 0 0 0 0 0 0 0]);
for i=1:n-1
p1=simple(MRvec(i,1));
p2=simple(MRvec(i,2));
p3=simple(MRvec(i,3));
q1=simple(MRvec(i+1,1));
q2=simple(MRvec(i+1,2));
q3=simple(MRvec(i+1,3));
%
v1=subs(MRvec(i,4),'q1');
%
v2=subs(MRvec(i,5),'q1');
%
v3=subs(MRvec(i,6),'q1');
v1=simple(MRvec(i,4));
v2=simple(MRvec(i,5));
v3=simple(MRvec(i,6));
w1=simple(MRvec(i+1,4));
w2=simple(MRvec(i+1,5));
w3=simple(MRvec(i+1,6));
q4=simple(p1*v1+p2*v2+p3*v3);
u(i,:)=cross(MRvec(i+1,[4 5 6]),MRvec(i,[4 5 6]));
k1=q1-p1;
k2=q2-p2;
k3=q3-p3;
mod_v1=sqrt(v1^2+v2^2+v3^2);
A=[v1/mod_v1 w1;v2/mod_v1 w2;v3/mod_v1 w3];
B=[k1 v1 w1;k2 v2 w2; k3 v3 w3];
%determinamos si los ejes son coincidentes, paralelos, se cortan o
se cruzan
if (rank(A)==1)&(rank(B)==1)
%coincidentes
rectas=0;
end
if (rank(A)==1)&(rank(B)==2)
%paralelos
rectas=1;
end
if (rank(A)==2)&(rank(B)==2)
%se cortan
rectas=3;
end
if (rank(A)==2)&(rank(B)==3)
%se cruzan
rectas=4;
end
if rectas==0
%coincidentes
if (i>1)&(cross(MRvec(i,[4 5 6]),MRvec(i-1,[4 5 6]))==[0 0 0])
%Si los ejes de eslabon actual y anterior coinciden
ÁLVARO GÓMEZ RAMOS.
- 187 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 MRperp(i-1,[7 8
9]) MRvec(i,7)]); %eje z igual a eje x actual y eje x igual al z
anterior
u(i,:)=simple(MRperp(i-1,[7 8 9]));
%elseif (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]));
end
if i==1
if MRvec(i,7)==2
MRperp(i,:)=([MRvec(i,[4 5 6]) MRvec(i,[1 2 3]) 1 0 0
MRvec(i,7)]);
end
if MRvec(i,7)==1
MRperp(i,:)=([MRvec(i,[4 5 6]) MRvec(i,[1 2 3])
sin(Q2_simb(i)) cos(Q2_simb(i)) 0 MRvec(i,7)]);
end
end
end
if rectas==1
% Paralelas
q1_=simple(det([q1 0 0 -w1;q2 1 0 -w2;q3 0 1 -w3;q4 v2 v3
0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0]));
q2_=simple(det([1 q1 0 -w1;0 q2 0 -w2;0 q3 1 -w3;v1 q4 v3
0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0]));
q3_=simple(det([1 0 q1 -w1;0 1 q2 -w2;0 0 q3 -w3;v1 v2 q4
0])/det([1 0 0 -w1; 0 1 0 -w2; 0 0 1 -w3; v1 v2 v3 0]));
u(i,:)=([simple(q1_-p1) simple(q2_-p2) simple(q3_p3)]/sqrt((q1_-p1)^2+(q2_-p2)^2+(q3_-p3)^2));
%sum(abs([simple(q1_-p1) simple(q2_-p2) simple(q3_p3)]).^2)^(1/2));%norm([q1_-p1 q2_-p2 q3_-p3]);
MRperp(i,:)=simple([MRvec(i,[4 5 6]) p1 p2 p3 u(i,:)
MRvec(i,7)]);
end
if rectas==3
%Cortan
x=simple((det([k1 w1 u(1);k2 w2 u(2);k3 w3 u(3)])/det([v1 w1
u(i,1);v2 w2 u(i,2);v3 w3 u(i,3)])));
r1=simple(p1+abs(x)*v1);
r2=simple(p2+abs(x)*v2);
r3=simple(p3+abs(x)*v3);
MRperp(i,:)=([MRvec(i,[4 5 6]) r1 r2 r3 u(i,:) MRvec(i,7)]);
end
if rectas==4
% Cruzan
MRperp(i,:)=([MRvec(i,[4 5 6]) p1 p2 p3 u(i,:) MRvec(i,7)]);
end
ÁLVARO GÓMEZ RAMOS.
- 188 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
end
MRperp(i+1,:)=([MRvec(i+1,[4 5 6]) MR(n2,[5 6 7]) u(i,:)
MRvec(i+1,7)]); %vector z + punto de origen + vector x
end
ÁLVARO GÓMEZ RAMOS.
- 189 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function nuevo()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función para dibujar un nuevo robot, cargar uno ya existente o
%
salvar uno nuevo al disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoy htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
sp6 sp7 MR mfile_save n mfile_load mfile_graf mfile_DH n MRperp MRvec
t htextot1 num_simb_rot Q1_simb x y z tras hejes hbot graph_DH MRejes
hrot3d Q1 rotac Q2_simb Q2 num_simb_rot2 graph_calcbot htras heje hrot
hgarra hbase xinf xsup yinf ysup zinf zsup t1 q1 q2 q3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%inicializacion de variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
syms t1 t2 t3 t4 t5 positive
x=0;
y=0;
z=0;
n=0;
tras=1;
rotac=1;
MR=sym([0 0 0 0 0 0 0 0]);
num_simb_rot=0;
num_simb_rot2=0;
Q1_simb=sym('q');
Q1(tras)=0;
Q2_simb=sym('q');
Q2(rotac)=0;
%Variables de traslacion
%
X inicial
%
Y inicial
%
Z inicial
%
Nº de piezas iniciales
%Nº de elem de traslacion+1
%Nº de elem de rotación+1
%Matriz del robot
%nº de elementos de traslacion
%nº de elementos de rotacion
%Variables de traslacion
%Valores de traslacion
%Variables de rotacion
%Valores de rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
lim de los ejes del plot principal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xinf=-30;
xsup=30;
yinf=-30;
ysup=30;
zinf=0;
zsup=40;
ejes=[-20 20 -20 20 0 20];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Color de fondo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(hf1,'Color',[.824, .824, .824])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilita opciones del menu
ÁLVARO GÓMEZ RAMOS.
- 190 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(mfile_load,'Enable','on')
set(mfile_graf,'Enable','off')
set(mfile_DH,'Enable','off')
set(graph_calcbot,'Enable','on')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creación de paneles
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
hp0 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.0 0 .10 1]);
hp1 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.10 0 .13 1],'title','Piezas');
hp2 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.38 0 .7 1],'title','Robot');
hp3 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.93 0 .07 1],'title','Herramientas');
hp4 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.23 0.8 .15 0.8],'title','traslacion');
hp5 = uipanel('BackgroundColor',[.824, .824, .824],...
'Position',[.23 0.5 .15 0.3],'title','Rotación');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creación de Plots para la representación de las figuras
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]);
xlabel('eje x','color','r');
ylabel('eje y','color','g');
sp2=subplot(5,3,1);
sp3=subplot(5,3,4);
sp4=subplot(5,3,7);
sp5=subplot(5,3,10);
sp6=subplot(5,3,13);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Borrado de ejes de los subplots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(sp1);
axis off
subplot(sp2);
axis off
subplot(sp3);
axis off
subplot(sp4);
axis off
subplot(sp5);
axis off
subplot(sp6);
axis off
ÁLVARO GÓMEZ RAMOS.
- 191 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujo de Figuras de referencia
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% traslacion
vertices1=[x-0.3 y-0.3 z;x+0.3 y-0.3 z;x+0.3 y+0.3 z;x-0.3 y+0.3 z;x0.3 y-0.3 z+7;x+0.3 y-0.3 z+7;x+0.3 y+0.3 z+7;x-0.3 y+0.3 z+7];
caras1=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp2)
patch('Vertices',vertices1,'Faces',caras1,'FaceVertexCData',[.4, .4,
.125],'FaceColor','flat');
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
% Rotación
vertices2=[x y-0.5 z;x+0.5 y z;x y+0.5 z;x-0.5 y z;x y-0.5 z+1;x+0.5 y
z+1;x y+0.5 z+1;x-0.5 y z+1];
caras2=[1 2 3 4;5 6 7 8;1 5 6 2;2 6 7 3;7 8 4 3;8 4 1 5];
subplot(sp3)
patch('Vertices',vertices2,'Faces',caras2,'FaceVertexCData',[1, 0,
0],'FaceColor','flat');
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
% Eslabón
vertices3=[x-0.5 y-0.5 z;x+0.5 y-0.5 z;x+0.5 y+0.5 z;x-0.5 y+0.5 z;x0.5 y-0.5 z+7;x+0.5 y-0.5 z+7;x+0.5 y+0.5 z+7;x-0.5 y+0.5 z+7];
caras3=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp4)
patch('Vertices',vertices3,'Faces',caras3,'FaceVertexCData',[.8, .4,
.125],'FaceColor','flat');
axis xy
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
% Base
vertices4=[x-1 y-1 z;x+1 y-1 z;x+1 y+1 z;x-1 y+1 z;x-1 y-1 z+1;x+1 y-1
z+1;x+1 y+1 z+1;x-1 y+1 z+1];
caras4=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp5)
patch('Vertices',vertices4,'Faces',caras4,'FaceVertexCData',[0, 0,
0],'FaceColor','flat');
axis xy
AXIS([-1 8 -1 8 0 4]);
camorbit(55,-15)
% Garra
vertices5=[x-0.5 y-0.5 z;x+0.5 y-0.5 z;x+0.5 y+0.5 z;x-0.5 y+0.5 z;x y
z+1];
caras3=[1 2 3 4;1 2 5 5;2 3 5 5;3 4 5 5;1 4 5 5];
subplot(sp6)
patch('Vertices',vertices3,'Faces',caras3,'FaceVertexCData',[.0, .1,
.0],'FaceColor','flat');
axis xy
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 192 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%
Plot principal para dibujar el robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(sp1)
axis on
AXIS([xinf xsup yinf ysup zinf zsup]);
camorbit(165,0) % Cámara
grid on
camlight right
camlight left
lighting phong
%Límite de ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Botones, Textos y cuadros editables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
rotate_struct = load([matlabroot,'/toolbox/matlab/icons/rotate.mat']);
rotate_cdata = rotate_struct.cdata;
imgejes = imread('ejes.TIF');
imgbot = imread('robot.TIF');
%boton de rotación
hrot3d = uicontrol('Style','togglebutton',...
'Callback',@rot3d,...
'Tag','mypan',...
'Units','normalized',...
'Position',[0.95 0.94 0.03
0.03],...
'CData',rotate_cdata,...
'Interruptible','off',...
'BusyAction','cancel');
%boton de ver ejes
hejes = uicontrol('Style','togglebutton',...
'Callback',@ver,...
'Tag','mypan',...
'Units','normalized',...
'Position',[0.95 0.84 0.03
0.03],...
'CData',imgejes,...
'Interruptible','off',...
'BusyAction','cancel');
%boton de representar robot
hbot = uicontrol('Style','togglebutton',...
'Callback',@ver,...
'Tag','mypan',...
'Units','normalized',...
'Position',[0.95 0.74 0.03
0.03],...
'CData',imgbot,...
'value',1,...
'Interruptible','off',...
'BusyAction','cancel');
%Boton de añadir traslacion
htras = uicontrol('style','pushbutton',...
'Callback',@colocar1,...
ÁLVARO GÓMEZ RAMOS.
- 193 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'backgroundcolor',[.724, .724, .724],...
'String','Traslación',...
'Units','normalized',...
'Position',[.11 .77 .06
.03],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%Boton de añadir rotacion
heje = uicontrol('style','pushbutton',...
'Callback',@colocar2,...
'backgroundcolor',[.724, .724, .724],...
'String','Eje Rotación',...
'Units','normalized',...
'Position',[.11 .6 .06
.03],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%Boton de añadir Eslabon
hrot = uicontrol('style','pushbutton',...
'Callback',@colocar3,...
'backgroundcolor',[.724, .724, .724],...
'String','Eslabón',...
'Units','normalized',...
'Position',[.11 .42 .06
.03],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%Boton de añadir base
hbase = uicontrol('style','pushbutton',...
'Callback',@colocar4,...
'backgroundcolor',[.724, .724, .724],...
'String','Base',...
'Units','normalized',...
'Position',[.11 .23 .06
.03],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%Boton de añadir garra
hgarra = uicontrol('style','pushbutton',...
'Callback',@colocar5,...
'backgroundcolor',[.724, .724, .724],...
'String','Garra',...
'Units','normalized',...
'Position',[.11 .08 .06
.03],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
%longitud
htexto = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
ÁLVARO GÓMEZ RAMOS.
- 194 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Position',[.31 .06 .05
.03],...
'Enable','on',...
'Interruptible','off',...
'String','10',...
'BusyAction','cancel');
hlabel = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','longitud (dm)',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.245 .0648 .05
.02],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%rot z
htextoz = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .11 .05
.03],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
hlabelz = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Rot. en Z (º)',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.245 .1148 .05
.02],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
%rot x
htextox = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .16 .05
.03],...
'Enable','on',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
hlabelx = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Rot. en X (º)',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.245 .1648 .05
.02],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
ÁLVARO GÓMEZ RAMOS.
- 195 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%Rot eje
htextoe = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .21 .05
.03],...
'Enable','off',...
'Interruptible','off',...
'String','0',...
'BusyAction','cancel');
hlabele = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Rot. eje (º)',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.245 .2148 .05
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
%Limite traslacion
htextot = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .92 .05
.03],...
'Enable','off',...
'Interruptible','off',...
'String','10',...
'BusyAction','cancel');
hlabelt = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Límite traslacion',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.232 .9248 .068
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
%nombre variable
tras
htextot1 = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .88 .05
.03],...
'Enable','on',...
'Interruptible','off',...
'String','',...
'BusyAction','cancel');
hlabelt1 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Nombre Var',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
ÁLVARO GÓMEZ RAMOS.
- 196 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Position',[.232 .8848 .068
.018],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
hlabelt2 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','dm',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.36 .9248 .018
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
%limite rot htextor1 = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .60 .05
.03],...
'Enable','off',...
'Interruptible','off',...
'String','-360',...
'BusyAction','cancel');
hlabelr1 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Límite Rot. -',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.232 .5548 .068
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
%limite rot +
htextor2 = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .55 .05
.03],...
'Enable','off',...
'Interruptible','off',...
'String','360',...
'BusyAction','cancel');
hlabelr2 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Límite Rot. +',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.232 .6048 .068
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
ÁLVARO GÓMEZ RAMOS.
- 197 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%nombre var rot
htextor3 = uicontrol('Style', 'edit', ...
'Units','normalized',...
'backgroundcolor',[1 1 1],...
'Position',[.31 .65 .05
.03],...
'Enable','off',...
'Interruptible','off',...
'String','',...
'BusyAction','cancel');
hlabelr3 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','Nombre Var.',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.232 .6548 .068
.018],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
hlabelr4 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','º',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.36 .795 .018
.03],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
hlabelr4 = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','º',...
'backgroundcolor',[.824 .824 .824],...
'Units','normalized',...
'Position',[.36 .745 .018
.03],...
'Enable','off',...
'Interruptible','off',...
'BusyAction','cancel');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Ejecutar la función del tipo de pieza elegida
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = colocar1(varargin)
global MR Q1 n x y z htextoe htexto tras
if n==0
ÁLVARO GÓMEZ RAMOS.
- 198 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
Q1(tras)=str2num(get(htexto,'String'));
longitud
tras=tras+1;
MR(n,[2 3 4])=sym([x y z]);
[x y z x1 y1 z1]=traslac(x,y,z);
MR(n,1)=1;
MR(n,[5 6 7])=[x y z];
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%Pto inicial
%nº de elem
%valor variable
%nº elem tras
%pto inicial tramo
%pto final
%eje traslacion
%pto final tramos
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Colocar rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = colocar2(varargin)
global MR Q2 n x y z htextoe rotac
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
Q2(rotac)=str2num(get(htextoe,'String'));
rotacion
rotac=rotac+1;
rotac
[x y z]=eje(x,y,z);
MR(n,1)=2;
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%pto inicial
%nº de elem
%valor variable de
%nº de elem de
%nuevo pto final
%eje rotac
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Colocar eslabon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = colocar3(varargin)
global MR n x y z htextoe
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
MR(n,[2 3 4])=[x y z];
[x y z]=eslabon(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=3;
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%nº de elementos
%pto inicial
%pto final
%tipo elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 199 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function varargout = colocar4(varargin)
global MR n x y z htextoe
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
MR(n,[2 3 4])=[x y z];
[x y z]=base(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=4;
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%nº de elementos
%pto inicial
%pto final
%tipo de elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar garra
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = colocar5(varargin)
global MR n x y z htextoe
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
MR(n,[2 3 4])=[x y z];
[x y z]=garra(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=5;
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%nº de elemento
%pto inicial
%pto final
%tipo de elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
habilitar/deshabilitar rotacion 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = rot3d(varargin)
global hrot3d
persistent enable_state;
val = get(hrot3d,'Value');
switch val
case 0
rotate3d off;
case 1
rotate3d on;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilitar o Deshabilitar ver ejes
ÁLVARO GÓMEZ RAMOS.
- 200 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = ver(varargin)
global hejes hbot sp1 MR MRperp MRejes n Q2_simb Q1_simb
persistent enable_state;
val1 = get(hejes,'Value');
val2 = get(hbot,'Value');
delete (sp1)
sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]);
xlabel('eje x');
ylabel('eje y');
axis on
AXIS([-30 30 -30 30 0 40]);
camorbit(165,0)
grid on
if val1==1
plotejes(MRperp);
%mostrar ejes si esta activado
end
if val2==1 %si esta activada la opción de dibujar robot
for i=1:n
MRaux(i,:)=MR(i,:); %variable aux de MR
end
clear MR
a=0;
for i=1:n
if MRaux(i,1)==2
a=a+1;
%rotar tramos
for j=i+1:n
[MRaux(j,2) MRaux(j,3)
MRaux(j,4)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6)
,MRaux(i,7),MRaux(j,2),MRaux(j,3),MRaux(j,4),Q2_simb(a));
[MRaux(j,5) MRaux(j,6)
MRaux(j,7)]=rot(MRaux(i,2),MRaux(i,3),MRaux(i,4),MRaux(i,5),MRaux(i,6)
,MRaux(i,7),MRaux(j,5),MRaux(j,6),MRaux(j,7),Q2_simb(a));
end
MRaux=simple(MRaux);
end
end
MR=MRaux;
dibubot(MR);
%dibujar robot
end
end
ÁLVARO GÓMEZ RAMOS.
- 201 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function Par_DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que calcula los parámetros DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MRejes P_D_H graph_robot Q1_simb Q1 Q2_simb Q2 num_simb_rot
num_simb_rot2 t1 t2 t3 t4 t5 q1 q2 q3 graph_cininv mfile_saveDH
P_D_H=sym([0 0 0 0]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nueva ventana
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
hf2=figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','on',...
'Name','Parámetros D-H',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','off',...
'BusyAction','cancel',...
'Position',[0.25 0.25 0.5 0.5]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calcula los parámetros
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
di=simple(calc_di(MRejes));
ai=simple(calc_ai(MRejes));
tethai=simple(calc_tethai(MRejes));
alphai=simple(calc_alphai(MRejes));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
mostramos los valores
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
tam=1+(1/(size(di,2)+1));
%Variable para dividir la ventana
según el nº de parámetros
for i=1:size(di,2)+1
if i==1
hpar_di = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','di',...
'ForegroundColor','b',...
'FontWeight','bold',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
ÁLVARO GÓMEZ RAMOS.
- 202 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Position',[0 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
else
hpar_di = uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(di(i-1)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(ai,2)+1
if i==1
hpar_di = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','ai',...
'ForegroundColor','b',...
'FontWeight','bold',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0.25 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
else
hpar_ai = uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(ai(i-1)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0.25 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(tethai,2)+1
if i==1
hpar_di = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','tethai',...
'ForegroundColor','b',...
'FontWeight','bold',...
'backgroundcolor',[1 1 1],...
ÁLVARO GÓMEZ RAMOS.
- 203 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
'Units','normalized',...
'Position',[0.5 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
else
hpar_ai = uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(tethai(i-1)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0.5 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(alphai,2)+1
if i==1
hpar_di = uicontrol('Style', 'text', ...
'Callback',@label,...
'String','alphai',...
'ForegroundColor','b',...
'FontWeight','bold',...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0.75 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
else
hpar_ai = uicontrol('Style', 'text', ...
'Callback',@label,...
'String',char(alphai(i-1)),...
'backgroundcolor',[1 1 1],...
'Units','normalized',...
'Position',[0.75 0 .25 tam(1/(size(di,2)+1))],...
'Enable','on',...
'Interruptible','off',...
'BusyAction','cancel');
tam=tam-(1/(size(di,2)+1));
end
end
for i=1:size(alphai,2)
P_D_H(i,:)=[alphai(i);ai(i);tethai(i);di(i)];
parametros de Denavit-Hartemberg
end
%matriz con los
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
habilitación de nuevas opciones de los menus
ÁLVARO GÓMEZ RAMOS.
- 204 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(graph_robot,'Enable','on')
set(graph_cininv,'Enable','on');
set(mfile_saveDH,'Enable','on');
end
ÁLVARO GÓMEZ RAMOS.
- 205 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function MRejes=plotejes(MRperp)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que nos muestra los ejes del robot a partir de la
matriz
%
MRperp y los almacena en otra matriz. MRejes=plotejes(MRperp)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
num_simb_rot Q1_simb Q1 num_simb_rot2 Q2_simb Q2 t1
n=size(MRperp,1);
%Nº de ejes
clear MRejes
subplot(sp1)
MRejes(1,[1 2 3 4 5 6 7 8 9 10 11 12 13])=sym([0 0 1 1 0 0 0 1 0 0 0 0
0]);
%ejes iniciales
MRejes_aux=MRejes;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustitucion de valores de traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot
MRejes_aux=subs(MRejes,Q1_simb(j),Q1(j));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustitucion de valores de rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot2
MRejes_aux=subs(MRejes,Q2_simb(j),Q2(j));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%dibuja ejes iniciales
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=0:0.15:3
hold on
plot3(MRejes_aux(1,1)*j, MRejes_aux(1,2)*j,
MRejes_aux(1,3)*j,'.b',MRejes_aux(1,4)*j, MRejes_aux(1,5)*j,
MRejes_aux(1,6)*j,'-*r',MRejes_aux(1,7)*j, MRejes_aux(1,8)*j,
MRejes_aux(1,9)*j,'-+g')
hold off
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculo los ejes y los dibujo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
ÁLVARO GÓMEZ RAMOS.
- 206 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
lz=sqrt(MRperp(i,1)^2+MRperp(i,2)^2+MRperp(i,3)^2);
if lz~=0
MRejes(i+1,[1 2 3])=[MRperp(i,1)/lz MRperp(i,2)/lz
MRperp(i,3)/lz]; %eje Z
else
MRejes(i+1,[1 2 3])=MRejes(i,[1 2 3]);
end
lx=sqrt(MRperp(i,7)^2+MRperp(i,8)^2+MRperp(i,9)^2);
if lx~=0
MRejes(i+1,[4 5 6])=[MRperp(i,7)/lx MRperp(i,8)/lx
MRperp(i,9)/lx]; %eje X
else
MRejes(i+1,[4 5 6])=MRejes(i,[4 5 6]);
end
MRejes(i+1,[7 8 9])=cross(MRejes(i+1,[1 2 3]),MRejes(i+1,[4 5
6])); %eje y
MRejes(i+1,[10 11 12])=MRperp(i,[4 5 6]);
%punto de los ejes
MRejes(i+1,13)=MRperp(i,10);
%Tipo de articulación
MRejes_aux=MRejes;
%sustitucion de valores de traslacción
for j=1:num_simb_rot
MRejes_aux=subs(MRejes_aux,Q1_simb(j),Q1(j));
end
%sustitucion de valores de rotacion
for j=1:num_simb_rot2
MRejes_aux=subs(MRejes_aux,Q2_simb(j),Q2(j));
end
%Dibujamos el eje
for j=0:0.15:3
hold on
plot3(MRejes_aux(i+1,10)+MRejes_aux(i+1,1)*j,
MRejes_aux(i+1,11)+MRejes_aux(i+1,2)*j,
MRejes_aux(i+1,12)+MRejes_aux(i+1,3)*j,'.b',MRejes_aux(i+1,10)+MRejes_
aux(i+1,4)*j, MRejes_aux(i+1,11)+MRejes_aux(i+1,5)*j,
MRejes_aux(i+1,12)+MRejes_aux(i+1,6)*j,'*r',MRejes_aux(i+1,10)+MRejes_aux(i+1,7)*j,
MRejes_aux(i+1,11)+MRejes_aux(i+1,8)*j,
MRejes_aux(i+1,12)+MRejes_aux(i+1,9)*j,'-+g')
hold off
end
end
MRejes=expand(MRejes);
ÁLVARO GÓMEZ RAMOS.
- 207 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function mat=redondear(mat)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%redondeamos los valores de las matrices
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[n,m]=size(mat);
for i=1:n
for j=1:m
if (mat(i,j)<0.0001)&mat(i,j)>-0.0001
mat(i,j)=0;
end
end
end
end
ÁLVARO GÓMEZ RAMOS.
- 208 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función para resolver el giro de un punto (X,Y,Z) respecto a
la
%
recta que pasa por los puntos (X1,Y1,Z1) y (X2,Y2,Z2) con un
ángulo
%
de rotación igual a tetha
%
[Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
vectores de la recta que pasa pos los puntos
A=X2-X1;
B=Y2-Y1;
C=Z2-Z1;
T=[1 0 0 0;0 1 0 0;0 0 1 0;-X1 -Y1 -Z1 1];
T_1=[1 0 0 0;0 1 0 0;0 0 1 0;X1 Y1 Z1 1];
Lp=sqrt(B^2+C^2);
if (B==0)&(Lp==0)
aux1=1;
aux2=0;
else
aux1=B/Lp;
aux2=C/Lp;
end
Rx=[1 0 0 0;0 aux2 aux1 0;0 -aux1 aux2 0;0 0
Rx_1=[1 0 0 0;0 aux2 -aux1 0;0 aux1 aux2 0;0
L=sqrt(A^2+B^2+C^2);
Ry=[Lp/L 0 A/L 0;0 1 0 0;-A/L 0 Lp/L 0;0 0 0
Ry_1=[Lp/L 0 -A/L 0;0 1 0 0;A/L 0 Lp/L 0;0 0
Rz=[cos(-tetha) sin(-tetha) 0 0;-sin(-tetha)
0;0 0 0 1];
Rp=T*Rx*Ry*Rz*Ry_1*Rx_1*T_1;
Res=[X Y Z 1]*Rp;
Xf=Res(1,1);
Yf=Res(1,2);
Zf=Res(1,3);
0 1];
0 0 1];
1];
0 1];
cos(-tetha) 0 0;0 0 1
end
ÁLVARO GÓMEZ RAMOS.
- 209 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que rota un punto Pf con respecto a los ejes x e y
que
%
hacemos pasar por otro punto dado Pi:
%
[Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
A=xf-xi;
B=yf-yi;
C=zf-zi;
Rx=[1 0 0 0;0 cos(angx) sin(angx) 0;0 -sin(angx) cos(angx) 0;0 0 0 1];
Rx=redondear(Rx);
Ry=[cos(angy) 0 sin(angy) 0;0 1 0 0;-sin(angy) 0 cos(angy) 0;0 0 0 1];
Ry=redondear(Ry);
Rz=[cos(angy) sin(angy) 0 0;-sin(angy) cos(angy) 0 0;0 0 1 0;0 0 0 1];
Rz=redondear(Rz);
F=[A B C 1]*Rx*Rz;
Xf=F(1,1)+xi;
Yf=F(1,2)+yi;
Zf=F(1,3)+zi;
ÁLVARO GÓMEZ RAMOS.
- 210 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function savebot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que salva un robot al disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR n num_simb_rot Q1_simb Q1 Q2_simb Q2 num_simb_rot2 MRvec
MRperp MR_simb rotac tras
[fname,pname] = uiputfile('*.mat','Select File');
dbfile= strcat(pname,fname);
if length(dbfile) == 0 return; end
save
(fname,'MR','n','num_simb_rot','Q1_simb','Q1','Q2_simb','Q2','num_simb
_rot2','MRperp','MRvec','MR_simb','rotac','tras')
return
ÁLVARO GÓMEZ RAMOS.
- 211 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function saveDH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que salva un robot al disco, guardando la matriz
P_D_H,
%
que contiene la matriz de Denavit-Hartemberg, y la matriz DH
que es
%
igual que P_D_H pero le añadimos el tipo de rotación. DH es
%
compatible con el toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global
P_D_H DH MRejes num_simb_rot num_simb_rot2
Q1_simb Q2_simb
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%vemos tipo de articulación
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:size(MRejes,1)
if MRejes(i,13)==1
a(i-1)=0;
cont1=cont1+1;
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
end
end
a(cont1+cont2+1)=0;
eje
%Es un eje de rotacion
%Tipo de articulación
%nº de artic de rotación
%Es un eje de traslacion
%Tipo de articulación
%nº de artic de traslacion
%Consideramos el eje final como otro
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sustituimos variables articulares por cero
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_simb_rot
P_D_Haux=subs(P_D_Haux,Q1_simb(j),0);
end
for j=1:num_simb_rot2
P_D_Haux=subs(P_D_Haux,Q2_simb(j),0);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% creamos DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
DH=P_D_Haux;
for i=1:(size(DH,1))
DH(i,5)=a(i);
end
ÁLVARO GÓMEZ RAMOS.
- 212 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Guardamos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[fname,pname] = uiputfile('*.mat','Select File');
dbfile= strcat(pname,fname);
if length(dbfile) == 0 return; end
save (fname,'P_D_H','DH')
return
ÁLVARO GÓMEZ RAMOS.
- 213 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function sicon()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función inicial que dibuja el logo y crea los menus
desplegables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear variables
warning off
img = imread('Logo.jpg');
%imagen principal
scrsz = get(0,'ScreenSize');
global hf1 mfile_save mfile_load mfile_graf MR graph_DH MRejes
graph_calcbot graph_robot graph_cindir graph_cininv mfile_saveDH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ventana principal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
hf1=figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','off',...
'Name','Sicon-Bot',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','on',...
'BusyAction','cancel',...
'Position',[0 0.08 1 0.92]);
subimage(img)
axis off
%mostrar imagen de fondo
% Sin ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creamos los menus y submenus desplegables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
mfile= uimenu('Label','Archivo');
mfile_graf=uimenu(mfile,'Label','Nuevo
Gráficamente','Callback','nuevo','Separator','on');
mfile_save=uimenu(mfile,'Label','Salvar
Robot','Callback','Savebot','Separator','on','Enable','off','Accelerat
or','S');
mfile_saveDH=uimenu(mfile,'Label','Salvar DH','Callback','SaveDH','Separator','off','Enable','off');
mfile_load=uimenu(mfile,'Label','Cargar
Robot','Callback','loadbot','Separator','on','Enable','off','Accelerat
or','L');
uimenu(mfile,'Label','Salir','Callback','clear
all','Callback','close all','Separator','on','Accelerator','Q');
mopt= uimenu('Label','Configuracion');
ÁLVARO GÓMEZ RAMOS.
- 214 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
mopt_pardin=uimenu(mopt,'Label','Parámetros
Dinámicos','Callback','par_din','Accelerator','X','Enable','off');
graph= uimenu('Label','Ejecucion');
graph_calcbot=uimenu(graph,'Label','Calcular
Robot','Callback','calcbot','Enable','off');
graph_DH=uimenu(graph,'Label','Parámetros DH','Callback','Par_DH','Enable','off');
graph_robot=uimenu(graph,'Label','Posicionar
Robot','Callback','env_bot','Enable','off');
graph_cindir=uimenu(graph,'Label','Cinemática
Directa','Callback','cin_dir','Enable','off');
graph_cininv=uimenu(graph,'Label','Cinemática
Inversa','Callback','cin_inv','Enable','off');
mopt= uimenu('Label','Ayuda');
uimenu(mopt,'Label','Ayuda
Sicon','Callback','Ayuda','Accelerator','H','Enable','on');
uimenu(mopt,'Label','Sobre...','Callback','sobre','Enable','on');
end
ÁLVARO GÓMEZ RAMOS.
- 215 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function sobre
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nos muestra una ventana con datos sobre el programa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
img = imread('sicon.jpg'); %imagen de fondo
figure('Visible','on',...
'Units','normalized',...
'Renderer','opengl',...
'Color',[1 1 1],...
'Resize','off',...
'Name','Sicon-Bot',...
'NumberTitle','off',...
'DockControls','off',...
'MenuBar','none',...
'Interruptible','off',...
'BusyAction','cancel',...
'Position',[0.4 0.4 0.35 0.35]);
subimage(img)
axis off
end
%nos muestra la imagen
ÁLVARO GÓMEZ RAMOS.
- 216 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja un eslabón, situado entre un punto
inicial y
%
otro final, se usa en la función dibubot. La base dibujada
llevará
%
el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
%
[xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos la longitud del tramo para obtener el ángulo de
giro
%
sobre el eje x a partir de zf y zi. Dependiendo del signo
de
%
(yf-yi) el giro será en un sentido o en otro.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo
if (yf-yi)>0.0001
%ángulo positivo
angx=-(pi/2-asin((zf-zi)/l));
else
angx=(pi/2-asin((zf-zi)/l));
%ángulo negativo
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el giro sobre el eje z a partir del pto inicial
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf)
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
V=[lx/l1 ly/l1 0]*l;
xf1=V(1);
yf1=V(2);
zf1=V(3);
angz=(pi/2-acos(xf1/l));
end
%si tramos es vertical
%ángulo y=0
%longitud en x
%longitud en y
%longitud en plano x-y
%Proyeccion en eje XY
%ángulo de y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
ÁLVARO GÓMEZ RAMOS.
- 217 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.4, .4,
.125],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 218 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
function [xf1 yf1 zf1 xf yf zf]=traslac(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Función que dibuja eje de traslacion, situado en un punto
inicial y
%
que nos devuelve un punto final. El eslabón dibujado
llevará
%
los ángulos de inclinación indicados por el usuario en la
ventana
%
principal:
%
[xf yf zf]=traslac(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoz htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
htextot1 num_simb_rot Q1_simb Q1 tras
syms a positive
%nombre de variable de traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el nº de elementos de traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if tras>1
%nº de elementos de traslacion igual a tras-1
tras2=tras-1;
else tras2=tras;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Obtenemos datos de variable
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
num_simb_rot=num_simb_rot+1;
traslación
l=str2num(get(htexto,'String'));
a=get(htextot1,'String');
Q1_simb(num_simb_rot)=a;
L=sym(a);
%Añadimos un elemento más de
%longitud inicial del tramos
%nombre de variable (traslacion)
%matriz de nombre de variables
%Longitud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Cogemos los valores de los ángulos y los pasamos a
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angx=-str2num(get(htextox,'String'))*2*pi/360;
angz=-str2num(get(htextoz,'String'))*2*pi/360;
ange=str2num(get(htextoe,'String'))*2*pi/360;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Situamos el punto final a una distancia L en vertical
respecto
%
al punto inicial
ÁLVARO GÓMEZ RAMOS.
- 219 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+L;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con los valores adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-0.3,yi-0.3,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+0.3,yi-0.3,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+0.3,yi+0.3,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-0.3,yi+0.3,zi+l,angx,angz);
[xf1 yf1 zf1]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
xf=subs(xf1,Q1(tras2));
%Valor real de xf, yf, zf
yf=subs(yf1,Q1(tras2));
zf=subs(zf1,Q1(tras2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,x2,y2,z2,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,x3,y3,z3,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,x4,y4,z4,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,x5,y5,z5,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,x6,y6,z6,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,x7,y7,z7,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,x8,y8,z8,ange);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustituimos las variables por su valor
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
x1=subs(x1,Q1(tras2));
x2=subs(x2,Q1(tras2));
x3=subs(x3,Q1(tras2));
x4=subs(x4,Q1(tras2));
x5=subs(x5,Q1(tras2));
x6=subs(x6,Q1(tras2));
x7=subs(x7,Q1(tras2));
x8=subs(x8,Q1(tras2));
y1=subs(y1,Q1(tras2));
y2=subs(y2,Q1(tras2));
y3=subs(y3,Q1(tras2));
ÁLVARO GÓMEZ RAMOS.
- 220 -
Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de
Robots Manipuladores en entorno MATLAB
y4=subs(y4,Q1(tras2));
y5=subs(y5,Q1(tras2));
y6=subs(y6,Q1(tras2));
y7=subs(y7,Q1(tras2));
y8=subs(y8,Q1(tras2));
z1=subs(z1,Q1(tras2));
z2=subs(z2,Q1(tras2));
z3=subs(z3,Q1(tras2));
z4=subs(z4,Q1(tras2));
z5=subs(z5,Q1(tras2));
z6=subs(z6,Q1(tras2));
z7=subs(z7,Q1(tras2));
z8=subs(z8,Q1(tras2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7
z7;x8 y8 z8];
caras=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp1)
patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[.4, .4,
.125],'FaceColor','flat');
return
ÁLVARO GÓMEZ RAMOS.
- 221 -