Memoria Final PDF

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 Introducción. ...................................................................................................... 7

1.2 Historia de la robótica. ...................................................................................... 7
1.3 Bases de la robótica actual ................................................................................ 8
1.4 Definición de Robot............................................................................................ 9
1.5 Límites de la robótica actual........................................................................... 10
2 COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO. ................. 12
2.1 Introducción ..................................................................................................... 12

2.2 El Brazo Robot ................................................................................................. 12
2.3 Grados de libertad ........................................................................................... 12
2.4 Articulaciones................................................................................................... 14
2.5 Sistemas de coordenadas ................................................................................. 14
2.6 Volumen de trabajo ......................................................................................... 15
2.7 Actuadores ........................................................................................................ 16
2.8 El controlador................................................................................................... 18
2.9 Dispositivos sensoriales.................................................................................... 18
2.10 Elementos terminales....................................................................................... 21
3 CAPÍTULO: TRANSFORMACIONES 3D. ................................................. 23
3.1 Introducción ..................................................................................................... 23

3.2 Geometría 3D ................................................................................................... 23
3.3 Cambio de escala.............................................................................................. 25
3.4 Traslación ......................................................................................................... 25
3.5 Rotación ............................................................................................................ 25
3.6 Rotación alrededor de un eje arbitrario ........................................................ 26
4 CAPÍTULO: CINEMÁTICA. ........................................................................ 31
4.1 Introducción. .................................................................................................... 31

4.2 El problema Cinemático Directo .................................................................... 32
4.3 Algoritmo de Denavit-Hartenberg ................................................................. 33
4.4 Cinemática Inversa .......................................................................................... 37
5 CAPÍTULO: TOOLBOX DE ROBÓTICA................................................... 38
5.1 Introducción ..................................................................................................... 38

5.2 Representación de traslación 3D y orientación ............................................. 38
ÁLVARO GÓMEZ RAMOS. -1-

5.3 Cinemática directa ........................................................................................... 39

5.4 Cinemática inversa........................................................................................... 39
BLOQUE 2: DESARROLLO DEL SOFTWARE ................ 41
1 CAPÍTULO: CONCEPCIÓN DEL SOFTWARE SICON-BOT. ............... 42
1.1 Introducción. .................................................................................................... 42

1.2 SICON-BOT. .................................................................................................... 43
1.3 Objetivos de la primera versión...................................................................... 43
1.4 Posibles mejoras futuras.................................................................................. 44
1.5 Requisitos mínimos. ......................................................................................... 44
1.6 Inicialización..................................................................................................... 45
2 CAPÍTULO: GRÁFICA DE SICON-BOT. .................................................. 46
2.1 Creación de ventana principal y menús desplegables. ................................. 46

2.2 Paneles y plots de representación ................................................................... 47
2.3 Eslabones y articulaciones............................................................................... 49
2.4 Botones, textos y cuadros editables. ............................................................... 54
3 CAPÍTULO: POSIBILIDADES DE SICON-BOT....................................... 56
3.1 Nuevo robot ...................................................................................................... 56

3.2 Calcular robot .................................................................................................. 58
3.3 Salvar Robot ..................................................................................................... 59
3.4 Cargar Robot.................................................................................................... 60
3.5 Parámetros D-H ............................................................................................... 62
3.6 Salvar D-H ........................................................................................................ 63
3.7 Cinemática directa ........................................................................................... 63
3.8 Cinemática inversa........................................................................................... 64
3.9 Posicionar Robot .............................................................................................. 66
3.10 Ayuda ................................................................................................................ 67
3.10 Herramientas.................................................................................................... 68
4 CAPÍTULO: PROGRAMACIÓN DE SICON-BOT.................................... 70
4.1 Introducción ..................................................................................................... 70

4.2 Inicialización de ventana inicial y menús desplegables ................................ 71
4.3 Entorno de creación de robots ........................................................................ 73
4.4 Añadir elemento de traslación ........................................................................ 76
4.5 Añadir elemento de rotación........................................................................... 78
4.6 Añadir elemento eslabón ................................................................................. 80
4.7 Añadir elemento base ...................................................................................... 81
4.8 Posicionar y dibujar por primera vez el elemento traslación ...................... 82
4.9 Posicionar y dibujar por primera vez el elemento rotación......................... 85
4.10 Posicionar y dibujar por primera vez el elemento eslabón .......................... 88
4.11 Posicionar y dibujar por primera vez el elemento base ............................... 91

4.12 Rotar punto respecto a ejes x e y que pasan por otro punto dado .............. 94
4.12 Rotar punto respecto a ejes dado por dos puntos ......................................... 96
4.13 Calcular robot .................................................................................................. 99
4.14 Obtención de los ejes de las articulaciones .................................................. 100
4.15 Obtención de “eje x”, “eje z” y punto de origen ......................................... 101
4.16 Obtención de “eje y”, normalizazión y dibujado de ejes............................ 106
4.17 Dibujar robot.................................................................................................. 107
4.18 Dibujar elemento eslabón.............................................................................. 108
4.19 Dibujar elemento traslación.......................................................................... 111
4.20 Dibujar elemento rotación ............................................................................ 114
4.21 Dibujar elemento base ................................................................................... 118
4.22 Salvar robot .................................................................................................... 119
4.23 Cargar robot................................................................................................... 119
4.24 Calculo de parámetros DH............................................................................ 120
4.25 Calculo de parámetros di ............................................................................... 123
4.26 Calculo de parámetros ai ............................................................................... 124
4.27 Calculo de parámetros θ i .............................................................................. 126
4.28 Calculo de parámetros α i .............................................................................. 126
4.28 Cinemática directa ......................................................................................... 127
4.29 Cinemática inversa......................................................................................... 129
4.30 Posicionar robot ............................................................................................. 134
4.31 Ayuda .............................................................................................................. 138
4.32 Función auxiliar: Multiplicar vectorialmente vectores simbólicos ........... 138
4.33 Función auxiliar: Redondear a cero los valores de las matrices ............... 138
BLOQUE 3: CONCLUSIONES ........................................... 139

1 Conclusiones. .................................................................................................. 140
2 Desarrollos futuros......................................................................................... 141
BLOQUE 4: ANEXOS .......................................................... 142
1 ANEXO: CÓDIGO FUENTE....................................................................... 143

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.

- Integración: Pues incorpora internamente funciones del toolbox de robótica

como la cinemática directa e inversa.

BLOQUE 1: CONCEPTOS BÁSICOS

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.

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.

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

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 -

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.

2 COMPONENTES BÁSICOS DE UN SÍSTEMA ROBÓTICO.
2.1 Introducción
La mayoría de robots tienen los siguientes componentes comunes (ver figura
2.1), aunque la estructura mecánica, eléctrica, y computacional de un robot puede variar
apreciablemente:
- Un manipulador o brazo (la unidad mecánica), incluyendo una garra o

elemento final.
- Un controlador (El módulo “inteligente”).
- Una unidad de potencia.
- Un sistema sensorial.
2.2 El Brazo Robot

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

- Los elementos menores (componentes de la muñeca)
- El elemento terminal (garra o herramienta)
Los elementos mayores lo forman los pares articulación-elemento, que sirven

para posicionar el extremo del brazo en un punto del espacio, y que generalmente son
los tres primeros.
Los elementos menores están asociados con la orientación del elemento final, y
junto con los elementos mayores se obtiene el posicionamiento y orientación del
elemento final.
El elemento final consiste en el mecanismo que permite desempeñar una

función al robot, ya sea una garra, soldador, taladro, etc.
2.3 Grados de libertad

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

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.

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

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

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.

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.

2.8 El controlador
El controlador es el dispositivo que proporciona la inteligencia necesaria para

que actúe conforme al operador. Esencialmente consiste en:
- Una memoria para almacenar datos como posiciones prefijadas o el propio

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

Los sensores utilizados en robótica pueden dividirse en dos clases:
· Sensores internos: Son los dispositivos usados para medir la posición, velocidad o
aceleración de las articulaciones del robot y/o del elemento final. Con esta información
tratará más adelante el sistema de control para obtener el movimiento deseado. Pueden
ser de diferentes clases:
- Potenciómetros: Es un elemento resistivo de valor máximo (R) que varía su

valor según la posición de giro o el desplazamiento, con lo cual, 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

- 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

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

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.

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.

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 . Por lo tanto, todavía son necesarios dos puntos para definir

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 . Este vector

recibe el nombre de vector normal. Por otro lado, si es un punto genérico del
plano, entonces es un vector de ese plano. Si tenemos en cuenta

que estos dos vectores forman un ángulo recto, su producto escalar será nulo:
Esta expresión es válida cuando es un punto del plano, por lo tanto,

esta es la ecuación de ese plano.

3.3 Cambio de escala
Ahora que sabemos expresar puntos, rectas y planos en tres dimensiones,

consideremos los algoritmos que permiten al usuario modelar objetos tridimensionales.
Empezaremos por generalizar las transformaciones bidimensionales que vimos

en el capítulo anterior. Así, por lo que respecta al cambio de escala, la tercera
coordenada que hemos introducido pueden tener su propio facto de escala, por lo tanto
será necesaria una matriz ,
o de rango si empleamos coordenadas homogéneas
3.4 Traslación
Igual que en el caso bidimensional, empleamos los elementos de la fila inferior de la

matriz de transformación en coordenadas homogéneas para reflejar la traslación.
3.5 Rotación
Cuando consideramos la rotación de un objeto en dos dimensiones, vimos la

matriz de rotación alrededor del origen
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
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

misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos
ofrece la información necesaria para la traslación del origen, y la dirección nos informa
de los ángulos de rotación correctos para alinear la recta con el eje z.
Dada la recta:
Un punto de esta recta es y su dirección está especificada por el

vector .
La matriz de transformación que traslada el origen al eje de giro será:
Necesitaremos también la inversa de traslación para devolver el origen a su

posición original una vez que las rotaciones se hayan completado.
El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos

realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el
ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y
consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta 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 .

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á

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

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

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= f x (q1 q2 ... qn )
y= f y (q1 q2 ... qn )
z= f z (q1 q2 ... qn )
α= f α (q1 q2 ... qn )
β= f β (q1 q2 ... qn )
γ = f γ (q1 q2 ... 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ón-
eslabón constituye un grado de libertad):

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

Aii−1 = T ( z ,θ i )T (0,0, d i )T (ai ,0,0)T ( x, α i )
Desarrollando la expresión:
⎡cos θ i − senθ i 0 0 ⎤ ⎡1 0 0 0 ⎤ ⎡1 0 0 a i ⎤ ⎡1 0 0 0⎤
⎢ senθ cos θ i 0 0⎥⎥ ⎢⎢0 1 0 0 ⎥⎥ ⎢⎢0 1 ⎥ ⎢
0 0 ⎥ ⎢0 cos α i − senα i 0⎥⎥
Aii−1 =⎢ i
⎢ 0 0 1 0⎥ ⎢0 0 1 d i ⎥ ⎢0 0 1 0 ⎥ ⎢0 senα i cos α i 0⎥
⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥
⎣ 0 0 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1 ⎦ ⎣0 0 0 1⎦
Obtenemos la expresión general de DH, donde θ i , d i , ai , α i son los parámetros

DH del eslabón i:
⎡cos θ i − cos α i senθ i senα i senθ i ai cos θ i ⎤

⎢ senθ cos α i cos θ i − senα i cos θ i ai senθ i ⎥⎥
Aii−1 =⎢ i
⎢ 0 senα i cos α i di ⎥
⎢ ⎥
⎣ 0 0 0 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.

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 di ai αi

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

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.

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):

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

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.

BLOQUE 2: DESARROLLO DEL

SOFTWARE

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.

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
- Facilidad: Con un entorno gráfico familiar, con diseño de robots en 3D, con
ventanas, menús desplegables, botones, y demás funciones gráficas que son
familiares a la mayoría de personas.
- Comodidad: A la hora de integrar nuevas herramientas que nos permiten

obtener automáticamente parámetros necesarios para otras herramientas, que
hasta ahora teníamos que calcular a mano.
- Integración: En un mismo software podemos utilizar funciones

pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se
resuelven en la programación interna.
1.3 Objetivos de la primera versión.

En la primera versión del software SICON-BOT el objetivo es crear las bases de
la herramienta, para que poco a poco puedan ir ampliándose en un futuro por nuevos
usuarios interesados, para lo cual el código es accesible.
Se ha desarrollado enteramente en Matlab un software a través de elementos

familiares, como ventanas y menús, al estilo Windows. Con él podremos dibujar en tres
dimensiones el robot con el que queramos trabajar, 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.

El diseño del robot, es un diseño genérico y limitado a ciertos tipos de robots,

púes se basa en la adición de elementos predefinidos, pero con esto podemos diseñar
los robots más comunes.
En resumen los objetivos de la primera versión es el desarrollo desde cero del

software, creando toda la programación necesaria para el entorno gráfico, el diseño de
robots en 3D, el cálculo de ejes, el cálculo de matrices DH, el cálculo de la cinemática
directa e inversa, y el posicionamiento del robot en 3D.
Con esto tenemos en la primera versión los tres objetivos fundamentales:
- Facilidad: Con el entorno gráfico que incorpora, el diseño intuitivo de

robots, el cálculo automático de parámetros mediante simple inserción de
datos sencillos.
- Comodidad: Púes realiza funciones como la obtención automática de los

ejes o de la matriz DH.
- Integración: Pues incorpora internamente funciones del toolbox de robótica

como la cinemática directa e inversa.
1.4 Posibles mejoras futuras.

Existen una gran cantidad de posibles mejoras a añadir al programa, aquí
pondremos algunas que serían de gran utilidad:
- Adición de dinámica
- Adición de movimiento en 3D 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++
1.5 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)

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

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

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:

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

Elementos Parámetros Plot principal

posibles del del robot
robot
2.3 Eslabones y articulaciones.

Existen 4 tipos de elementos que están disponibles para diseñar nuestro robot,
estos son: Articulación de traslación, articulación de rotación, eslabón y base.
Traslación
Rotación
Eslabón
Base
La ventana principal se ha dividido en una serie de subplots, donde se dibujaran

diferentes elementos.

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 8 9
10 11 12
13 14 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.

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.

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:

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.

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:

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”.

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.

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.

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”.

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.

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”.

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.

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”

3.6 Salvar D-H

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

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

Posición Nombre Valor

deseada de las calculado
variables 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:

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.

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.

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

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

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

4.2 Inicialización de ventana inicial y menús desplegables

La primera fase de la programación del software se centra en la creación del
entorno gráfico necesario para el uso del programa.
Para ello creamos la ventana principal a través del comando de matlab “figure”
Con este comando y las propiedades que le hemos indicado, se abre una ventana
nueva, que nos devuelve un valor único y representativo que almacenamos en la
variable “hf1” para poder realizar futuras operaciones con la ventana.
Le hemos indicado ciertos parámetros a través de las propiedades, veamos

algunos interesantes:
- : Indicamos que queremos que la ventana sea visible.

- : Indicamos que las unidades para indicarle la
medida de la ventana, en este caso con “normalizad” queremos que (0, 0)
sea la esquina inferior de abajo y (1, 1) la esquina superior derecha.
- : Elige el método de representación gráfica de los
objetos, con “opengl” elegimos el método más potente de representación
gráfica.
- : Elegimos que el color de fondo sea blanco.
- : Deshabilitamos la posibilidad de cambiar el tamaño de la
ventana.
- : El nombre que aparecerá en la ventana será “Sicon-
Bot”.
- : Elimina que aparezca el número de la ventana
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.

Una vez creada la ventana principal, le queremos añadir la imagen del logo del
software de fondo, para ello:
Con ello almacenamos la imagen en una variable de matlab y luego la

mostramos con el comando “subimage” que nos permite mostrar en una ventana varias
imágenes.
Ahora creamos los menús desplegables y si son accesibles o no desde el

principio:
Con “uimenu” creamos los menús, cuyo nombre indicamos a través de

‘Label’,’nombre’, por ejemplo:
- Creamos el menú llamado Archivo y guardamos su identificador en la

variable “mfile”
Pero con “uimenu” también podemos crear las opciones que se crearán en el
menú al desplegarse:
Al indicarle la variable mfile donde se almacena el indicador del menú, estamos

asociando esta opción a dicho menú. Con label damos texto a la opción, con la
propiedad “Callback” indicamos la rutina que se ejecutará cuando se pulse la opción, y
con la propiedad ’Separator’,on’, indicamos que dibuje una línea para separar de las
siguientes opciones posibles, para así crear subgrupos diferenciados.
Existen otras propiedades interesantes como para que la

opción inicialmente no esté habilitada, y poder habilitarla cuando se cumpla alguna
condición. También es interesante la propiedad , que hace que la

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’:

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:

Lo siguiente será dibujar las figuras de referencia de las que podremos elegir
para añadir a nuestro robot:
Para ello creamos una matriz con las coordenadas de los vértices y otra donde se
le indican las caras de la figura a través de los vértices que la componen. Luego
elegimos el plot donde queremos representar la figura, y ejecutamos el comando
“patch” indicándole la matriz de vértices y la de caras y el color del objeto a través de
la propiedad ‘FaceVertezCData’. También se le indica la manera de colorear el objeto,
en este caso a través de ‘FaceColor’,’flat’, las caras se dibujarán por homogéneas.
Con “Axis xy’ indicamos que los ejes son cartesianos, y con
especificamos el rango que queremos que muestren los ejes.
“camorbit” elige la rotación de la cámara para mostrar el objeto, indicándole la rotación
horizontal y vertical.
Después de preparar los plots de representación secundarios vamos a añadir

iluminación al plot principal: , consiguiendo que entre un rayo de luz
que ilumina al objeto desde la derecha.
El siguiente paso será la creación de botones, textos y cuadros editables, que lo

haremos con el comando “uicontrol” y sus propiedades.
Con la propiedad ‘Style’ indicamos el tipo de elemento, que puede ser:
- Cajas de chequeo (Check boxes)

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

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

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:

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:

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,

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:

siendo un “2” para los elementos de rotación:

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:

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


siendo un “3” para los elementos eslabón:
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:


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

siendo un “4” para los elementos base:
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:

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
X1
Pto. inicial 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, 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:

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
inicial y los valores de giro indicadas por el usuario en la 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. :

A partir del valor del punto inicial, situamos el punto final coincidente:
base.
X8
X5
X7
Pto. final X6
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)


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.



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

que nos indico el usuario, en este caso necesitamos la longitud:

base.
X8
Pto. final X5
X7
X6
X4
X1
Pto. inicial X3
X2

(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)


del siguiente elemento:



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

que nos indico el usuario, en este caso necesitamos la longitud inicial:

base.
X8
Pto. final X5
X7
X6
X4
X1
Pto. inicial X3
X2

(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)





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:

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:

Ya tenemos rotado el punto, pero debemos deshacer la traslación:
4.12 Rotar punto respecto a ejes dado por dos puntos

Se resuelve mediante la función:
Lo primero que debemos hacer es escoger una representación adecuada de la

recta que será el eje de rotación. Un punto de dicha recta, así como la dirección de la
misma son suficientes para definir la recta (ecuación vectorial). El punto de la recta nos
ofrece la información necesaria para la traslación del origen, y la dirección nos informa
de los ángulos de rotación correctos para alinear la recta con el eje z.
Dada la recta
un punto de esta recta es y su dirección está especificada por el vector

. Para obtener los puntos realizamos la siguiente operación:
El siguiente paso en este proceso es la rotación alrededor del eje x. Deseamos

realizar un giro que sitúe al eje arbitrario de rotación en el plano xz. Para determinar el
ángulo de giro necesario, pongamos nuestro vector de dirección en el nuevo origen y
consideremos su proyección sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta 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 .

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á

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

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

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:

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:

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:

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).

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:

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:

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:

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:

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 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:
base.
X8
Pto. final X5
X7
X6
X4
X1
Pto. inicial X3
X2

(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)





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:




lx
ly l1

en vertical:
base.

X8
Pto. final X5
X7
X6
X4
X1
Pto. inicial X3
X2

(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)




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:



lx
ly l1


en vertical:
base.
X8
X5
X7
Pto. final X6
Pto. inicial X4
X1
X3
X2

(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)




dibujamos con el comando patch.

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:


en vertical:
base.
X8
Pto. final X5
X7
X4
X6
X1
Pto. inicial X3
X2



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” :

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:

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 d α θ
… … … …
… … … …
… … … …
… … … …


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

Sea una recta r determinada por un punto P y un vector , y un punto Q

exterior a la recta.
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 punto de los ejes i, “Q” el punto de los ejes i-1 y el vector xi. Con lo cual el
cálculo se resolverá:
4.26 Calculo de parámetros ai

ai: Es la distancia de zi a zi+1 medida a lo largo de xi

Sea una recta r determinada por un punto P y un vector , y un punto Q

exterior a la recta.
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 punto de los ejes i, “Q” el punto de los ejes i-1 y el vector zi. Con lo cual el
cálculo se resolverá:

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).

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:

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”



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:

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:


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.

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].

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:

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:

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:

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:

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:

BLOQUE 3: CONCLUSIONES

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.

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.

BLOQUE 4: ANEXOS

1 ANEXO: CÓDIGO FUENTE.
function ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Función de ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
web index.htm

function [xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Función que dibuja una base, situado entre un punto
inicial y
% otro final. La base dibujada llevará
% el ángulo de giro sobre el eje punto inicial-final
indicados por el usuario:
% [xf yf zf]=base2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obtenemos datos de altura de la base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Situamos el punto final a una distancia L en vertical
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rotamos la figura con respecto al eje Xi-Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi,ange);
[x2 y2 z2]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi,ange);
[x3 y3 z3]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi,ange);
[x4 y4 z4]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi,ange);
[x5 y5 z5]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi-2,zi+l,ange);
[x6 y6 z6]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi-2,zi+l,ange);
[x7 y7 z7]=rot(xi,yi,zi,xf,yf,zf,xi+2,yi+2,zi+l,ange);
[x8 y8 z8]=rot(xi,yi,zi,xf,yf,zf,xi-2,yi+2,zi+l,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Dibujamos el elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
vertices=[x1 y1 z1;x2 y2 z2;x3 y3 z3;x4 y4 z4;x5 y5 z5;x6 y6 z6;x7 y7

z7;x8 y8 z8];
caras=[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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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)
return

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
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_v=sqrt(mod_v);
ai(i)=mod_QP/mod_v;
end
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

mod_v=sqrt(mod_v);
ai(i)=deta/mod_v;
end
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_v=sqrt(mod_v);
ai(i)=mod_QP/mod_v;
end
end
end

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

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) %coincidentes
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_v=sqrt(mod_v);
di(i-1)=mod_QP/mod_v;
end
if (rank(A)==1)&(rank(B)==2) %paralelos
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_v=sqrt(mod_v);
end
if (rank(A)==2)&(rank(B)==2) %se cortan
a=[v(1) w(1) Q(1)-P(1);v(2) w(2) Q(2)-P(2);v(3) w(3) Q(3)-
P(3)];
b=cross(v,w);
mod_QP=0;
mod_v=0;
deta=det(a);
for j=1:3
mod_v=mod_v+b(j)^2;

end
mod_v=sqrt(mod_v);
di(i-1)=deta/mod_v;
end
if (rank(A)==2)&(rank(B)==3) %se cruzan
a=cross((Q-P),v);
mod_QP=0;
mod_v=0;
for j=1:3
end
for j=1:3
mod_v=mod_v+v(j)^2;
end
mod_v=sqrt(mod_v);
end
end
end

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]);
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];
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]);
x=cross(v,w);
tethai(i-
1)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2)^2+v(3
)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2)));

x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x];

tethai(i-1)=-tethai(i-1);
end
end
end
end

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

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

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],...
'Position',[.4 0.7 .5
.05],...
'Enable','on',...
'BusyAction','cancel');
'Position',[.4 0.5 .5
.05],...
'Enable','on',...
'Position',[.4 0.3 .5
.05],...
'Enable','on',...
'String','x=',...
'ForegroundColor','b',...
'FontWeight','bold',...

'Position',[.2 0.7 .1
.05],...
'Enable','on',...
'String','y=',...
'Position',[.2 0.5 .1
.05],...
'Enable','on',...
'String','z=',...
'Position',[.2 0.3 .1
.05],...
'Enable','on',...
end

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); %Nº de filas

checkdata=0; %Variable de checkbox
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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;
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
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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cálculo de DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
DH=P_D_Haux;
for i=1:(size(DH,1))
DH(i,5)=a(i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% ventana para introducir datos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
'Color',[1 1 1],...
'Resize','on',...
'Name','Cinemática Inversa',...
'Position',[0.15 0.15 0.6 0.6]);
hintdata = uicontrol('style','pushbutton',...
'Callback',@intdata2,...
'backgroundcolor',[.724, .724, .724],...
'String','Calcular',...
'Position',[.8 .5 .1 .05],...
'Enable','on',...

'String','x =',...
'Position',[.1 0.7 .1
.05],...
'Enable','on',...
%introducir x
xedit=uicontrol('Style', 'edit', ...
'Position',[.2 0.7 .1
.05],...
'Enable','on',...
'String','0',...


'String','y =',...
'Position',[.1 0.5 .1
.05],...
'Enable','on',...
% Introducir y
yedit=uicontrol('Style', 'edit', ...
'Position',[.2 0.5 .1
.05],...
'Enable','on',...
'String','0',...

'String','z =',...
'Position',[.1 0.3 .1
.05],...
'Enable','on',...
% Introducir z
zedit=uicontrol('Style', 'edit', ...
'Position',[.2 0.3 .1
.05],...
'Enable','on',...
'String','0',...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
'String',char(Var(1,i)),...

'Position',[.4 c-b .1
.05],...
'Enable','on',...
hdata(i) = uicontrol('Style', 'edit', ...

.05],...
'Enable','off',...
'String','0',...
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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
plot(robotd2,q) %Posicionar robot
axis on
AXIS([0 30 0 30 0 40]);
camorbit(165,0)
grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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)])];

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) %Seleccionamos subplot principal

MR=simple(MR); %Simplificamos MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculamos MR con valores reales
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
MR=subs(MR,Q1_simb(j),Q1(j)); %Mr para valores concretos de las
variables
end
MR=subs(MR,Q2_simb(j),-Q2(j)); %Mr para valores concretos de las
variables
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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

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
% [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 %ángulo positicvo
angx=-(pi/2-asin((zf-zi)/l));
else %ángulo negativo
angx=(pi/2-asin((zf-zi)/l));
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculamos el giro sobre el eje z a partir del pto inicial
y
% final.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf) %Si tramo es vertical

angz=0; %Ángulo y=0
else
lx=xf-xi; %Longitud en x
ly=yf-yi; %Longitud en y
l1=sqrt(lx^2+ly^2); %Longitud en plano XY
V=[lx/l1 ly/l1 0]*l; %Proyección en eje XY
xf1=V(1);
yf1=V(2);
zf1=V(3);
angz=(pi/2-acos(xf1/l)); %ángulo de y
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1 y1 z1]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1-0.5,angx,angz);
[x2 y2 z2]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1-0.5,angx,angz);
[x3 y3 z3]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1-0.5,angx,angz);
[x4 y4 z4]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1-0.5,angx,angz);
[x5 y5 z5]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1+0.5,angx,angz);
[x6 y6 z6]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1+0.5,angx,angz);
[x7 y7 z7]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1+0.5,angx,angz);
[x8 y8 z8]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1+0.5,angx,angz);
[xf yf zf]=rotar(xi1,yi1,zi1,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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)
return

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á
% [xf yf zf]=eje(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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;
a=get(htextot1,'String');
Q2_simb(num_simb_rot2)=a;
L=sym(a);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1 y1 z1]=rotar(xi,yi,zi,xi,yi-0.6,zi-0.5,angx,angz);
[x2 y2 z2]=rotar(xi,yi,zi,xi+0.6,yi,zi-0.5,angx,angz);
[x3 y3 z3]=rotar(xi,yi,zi,xi,yi+0.6,zi-0.5,angx,angz);
[x4 y4 z4]=rotar(xi,yi,zi,xi-0.6,yi,zi-0.5,angx,angz);
[x5 y5 z5]=rotar(xi,yi,zi,xi,yi-0.6,zi+0.5,angx,angz);

[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);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1 y1 z1]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x1,y1,z1,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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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)
return

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); %Nº de filas

checkdata=0; %Variable de checkbox
P_D_Haux=P_D_H;
cont1=0; %Contador
cont2=0; %Contador
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculamos el nº de articulaciones, el tipo y el orden
% de ellas.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if MRejes(i,13)==1 %Articulación de traslacion
a(i-1)=0; %Vector con tipo de variable
articular
cont1=cont1+1;
Var(i-1)=Q2_simb(cont1); %Vector con variables
articulares ordenadas
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
end
end
a(cont1+cont2+1)=0; %Creamos un articulación
ficticia al final del robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Damos valor cero a las variables articulares para adaptarlas
al
% formato del toolbox de robótica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nueva ventana para introducir los datos de las variables

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
'Color',[1 1 1],...
'Resize','on',...
'Name','Introducir datos',...
'Position',[0.15 0.15 0.6 0.6]);
% Botón
hintdata = uicontrol('style','pushbutton',...
'Callback',@intdata,...
'String','Intr. Dato',...
'Position',[.6 .6 .1 .05],...
'Enable','on',...
% Checkbox
hcheck = uicontrol('style','checkbox',...
'Callback',@check,...
'String','Ver Robot en
3D',...
'Position',[.6 .4 .15
.05],...
'Enable','on',...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
'String',char(Var(1,i)),...
.05],...
'Enable','on',...

hdata(i) = uicontrol('Style', 'edit', ...

.05],...
'Enable','on',...
'String','0',...
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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
end
clear MR
a=0;
MR=MR_simb;
dibubot(MR) %dibuja robot en 3D
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = check(varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

% Función que detecta si se activa ver el robot en 3D

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global checkdata
if checkdata==0
checkdata=1;
else
chackdata=0;
end
end

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
% [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
giro
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %Longitud del

tramos
if (yf-yi)>0.0001 %ángulo positicvo
else %ángulo negativo
angx=(pi/2-asin((zf-zi)/l));
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
y
% final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf) %Si tramo es vertical

angz=0; %Ángulo y=0
else
lx=xf-xi; %Longitud en x
ly=yf-yi; %Longitud en y
l1=sqrt(lx^2+ly^2); %Longitud en plano XY
V=[lx/l1 ly/l1 0]*l; %Proyección en eje XY
xf1=V(1);
yf1=V(2);
zf1=V(3);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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

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

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á
% [xf yf zf]=eslabon(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
tras Q1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Calculamos el nº de elementos de traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if tras>1
tras2=tras-1;
else tras2=tras;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obtenemos datos de longitud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
x1=subs(x1,Q1(tras2));
y1=subs(y1,Q1(tras2));
z1=subs(z1,Q1(tras2));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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
return

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á
% [xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
giro
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x5 y5 z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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)
return

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á
% [xf yf zf]=garra(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x5 y5 z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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)
return

function loadbot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Función que carga un robot almacenado en disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
end
clear MR
a=0;
MR=MR_simb;
dibubot(MR) %dibuja el robot en esa posicion

exacta
MRejes=simple(plotejes(MRperp)); %Obtengo el resto de ejes y los
dibujo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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

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) %eje rot o tras
x1=MR(i,5);
y1=MR(i,6);
z1=MR(i,7);
num=num+1;
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

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));
q1=simple(MRvec(i+1,1));
% v1=subs(MRvec(i,4),'q1');
v1=simple(MRvec(i,4));
w1=simple(MRvec(i+1,4));
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
%coincidentes
rectas=0;
end
%paralelos
rectas=1;
end
%se cortan
rectas=3;
end
%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

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

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

function nuevo()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Función para dibujar un nuevo robot, cargar uno ya existente o
% salvar uno nuevo al disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoy htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
sp6 sp7 MR mfile_save n mfile_load mfile_graf mfile_DH n MRperp MRvec
t htextot1 num_simb_rot Q1_simb x y z tras hejes hbot graph_DH MRejes
hrot3d Q1 rotac Q2_simb Q2 num_simb_rot2 graph_calcbot htras heje hrot
hgarra hbase xinf xsup yinf ysup zinf zsup t1 q1 q2 q3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%inicializacion de variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
syms t1 t2 t3 t4 t5 positive %Variables de traslacion

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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]);
'Position',[.10 0 .13 1],'title','Piezas');
'Position',[.38 0 .7 1],'title','Robot');
'Position',[.93 0 .07 1],'title','Herramientas');
'Position',[.23 0.8 .15 0.8],'title','traslacion');
'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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Dibujo de Figuras de referencia
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% traslacion
vertices1=[x-0.3 y-0.3 z;x+0.3 y-0.3 z;x+0.3 y+0.3 z;x-0.3 y+0.3 z;x-
0.3 y-0.3 z+7;x+0.3 y-0.3 z+7;x+0.3 y+0.3 z+7;x-0.3 y+0.3 z+7];
caras1=[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,
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,
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;x-
0.5 y-0.5 z+7;x+0.5 y-0.5 z+7;x+0.5 y+0.5 z+7;x-0.5 y+0.5 z+7];
caras3=[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)
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,
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)
axis xy
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

% Plot principal para dibujar el robot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(sp1)
axis on
AXIS([xinf xsup yinf ysup zinf zsup]); %Límite de ejes
camorbit(165,0) % Cámara
grid on
camlight right
camlight left
lighting phong
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Botones, Textos y cuadros editables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
rotate_struct = load([matlabroot,'/toolbox/matlab/icons/rotate.mat']);
rotate_cdata = rotate_struct.cdata;
imgejes = imread('ejes.TIF');
imgbot = imread('robot.TIF');
%boton de rotación
hrot3d = uicontrol('Style','togglebutton',...
'Callback',@rot3d,...
'Tag','mypan',...
'Position',[0.95 0.94 0.03
0.03],...
'CData',rotate_cdata,...
%boton de ver ejes

hejes = uicontrol('Style','togglebutton',...
'Callback',@ver,...
'Tag','mypan',...
'Position',[0.95 0.84 0.03
0.03],...
'CData',imgejes,...
%boton de representar robot

hbot = uicontrol('Style','togglebutton',...
'Callback',@ver,...
'Tag','mypan',...
'Position',[0.95 0.74 0.03
0.03],...
'CData',imgbot,...
'value',1,...
%Boton de añadir traslacion

htras = uicontrol('style','pushbutton',...
'Callback',@colocar1,...


'String','Traslación',...
'Position',[.11 .77 .06
.03],...
'Enable','on',...
%Boton de añadir rotacion

heje = uicontrol('style','pushbutton',...
'String','Eje Rotación',...
'Position',[.11 .6 .06
.03],...
'Enable','on',...
%Boton de añadir Eslabon

hrot = uicontrol('style','pushbutton',...
'String','Eslabón',...
'Position',[.11 .42 .06
.03],...
'Enable','on',...
%Boton de añadir base

hbase = uicontrol('style','pushbutton',...
'String','Base',...
'Position',[.11 .23 .06
.03],...
'Enable','on',...
%Boton de añadir garra

hgarra = uicontrol('style','pushbutton',...
'String','Garra',...
'Position',[.11 .08 .06
.03],...
'Enable','off',...
%longitud
htexto = uicontrol('Style', 'edit', ...

'Position',[.31 .06 .05

.03],...
'Enable','on',...
'String','10',...
hlabel = uicontrol('Style', 'text', ...

'String','longitud (dm)',...
'backgroundcolor',[.824 .824 .824],...
'Position',[.245 .0648 .05
.02],...
'Enable','on',...
%rot z
htextoz = uicontrol('Style', 'edit', ...
'Position',[.31 .11 .05
.03],...
'Enable','on',...
'String','0',...
hlabelz = uicontrol('Style', 'text', ...

'String','Rot. en Z (º)',...
'Position',[.245 .1148 .05
.02],...
'Enable','on',...
%rot x
htextox = uicontrol('Style', 'edit', ...
'Position',[.31 .16 .05
.03],...
'Enable','on',...
'String','0',...
hlabelx = uicontrol('Style', 'text', ...

'String','Rot. en X (º)',...
'Position',[.245 .1648 .05
.02],...
'Enable','on',...

%Rot eje
htextoe = uicontrol('Style', 'edit', ...
'Position',[.31 .21 .05
.03],...
'Enable','off',...
'String','0',...
hlabele = uicontrol('Style', 'text', ...

'String','Rot. eje (º)',...
'Position',[.245 .2148 .05
.018],...
'Enable','off',...
%Limite traslacion
htextot = uicontrol('Style', 'edit', ...
'Position',[.31 .92 .05
.03],...
'Enable','off',...
'String','10',...
hlabelt = uicontrol('Style', 'text', ...

'String','Límite traslacion',...
'Position',[.232 .9248 .068
.018],...
'Enable','off',...
%nombre variable tras

htextot1 = uicontrol('Style', 'edit', ...
'Position',[.31 .88 .05
.03],...
'Enable','on',...
'String','',...
hlabelt1 = uicontrol('Style', 'text', ...

'String','Nombre Var',...

'Position',[.232 .8848 .068

.018],...
'Enable','on',...
hlabelt2 = uicontrol('Style', 'text', ...

'String','dm',...
'Position',[.36 .9248 .018
.018],...
'Enable','off',...
%limite rot -
htextor1 = uicontrol('Style', 'edit', ...
'Position',[.31 .60 .05
.03],...
'Enable','off',...
'String','-360',...
hlabelr1 = uicontrol('Style', 'text', ...

'String','Límite Rot. -',...
'Position',[.232 .5548 .068
.018],...
'Enable','off',...
%limite rot +
'Position',[.31 .55 .05
.03],...
'Enable','off',...
'String','360',...

'String','Límite Rot. +',...
'Position',[.232 .6048 .068
.018],...
'Enable','off',...

%nombre var rot

'Position',[.31 .65 .05
.03],...
'Enable','off',...
'String','',...

'String','Nombre Var.',...
'Position',[.232 .6548 .068
.018],...
'Enable','off',...

'String','º',...
'Position',[.36 .795 .018
.03],...
'Enable','off',...
'String','º',...
'Position',[.36 .745 .018
.03],...
'Enable','off',...
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

MR=sym([0 0 0 0 0 0 0 0]); %Pto inicial

end
n=n+1; %nº de elem
Q1(tras)=str2num(get(htexto,'String')); %valor variable
longitud
tras=tras+1; %nº elem tras
MR(n,[2 3 4])=sym([x y z]); %pto inicial tramo
[x y z x1 y1 z1]=traslac(x,y,z); %pto final
MR(n,1)=1; %eje traslacion
MR(n,[5 6 7])=[x y z]; %pto final tramos
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360; %rot eje
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Colocar rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR Q2 n x y z htextoe rotac

if n==0
MR=sym([0 0 0 0 0 0 0 0]); %pto inicial
end
n=n+1; %nº de elem
Q2(rotac)=str2num(get(htextoe,'String')); %valor variable de
rotacion
rotac=rotac+1; %nº de elem de
rotac
[x y z]=eje(x,y,z); %nuevo pto final
MR(n,1)=2; %eje rotac
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Colocar eslabon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR n x y z htextoe
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1; %nº de elementos
MR(n,[2 3 4])=[x y z]; %pto inicial
[x y z]=eslabon(x,y,z); %pto final
MR(n,[5 6 7])=[x y z];
MR(n,1)=3; %tipo elem
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% colocar base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1; %nº de elementos
[x y z]=base(x,y,z); %pto final
MR(n,[5 6 7])=[x y z];
MR(n,1)=4; %tipo de elem
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% colocar garra
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1; %nº de elemento
[x y z]=garra(x,y,z); %pto final
MR(n,[5 6 7])=[x y z];
MR(n,1)=5; %tipo de elem
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
end
clear MR
a=0;
for i=1:n
if MRaux(i,1)==2
a=a+1;
%rotar tramos
for j=i+1:n
end
end
end
MR=MRaux;
dibubot(MR); %dibujar robot
end
end

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',...
'Color',[1 1 1],...
'Resize','on',...
'Name','Parámetros D-H',...
'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', ...
'String','di',...

'Position',[0 0 .25 tam-

(1/(size(di,2)+1))],...
'Enable','on',...
tam=tam-(1/(size(di,2)+1));
else
'String',char(di(i-1)),...
'Position',[0 0 .25 tam-
(1/(size(di,2)+1))],...
'Enable','on',...
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(ai,2)+1
if i==1
'String','ai',...
'Position',[0.25 0 .25 tam-
(1/(size(di,2)+1))],...
'Enable','on',...
else
hpar_ai = uicontrol('Style', 'text', ...
'String',char(ai(i-1)),...
(1/(size(di,2)+1))],...
'Enable','on',...
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(tethai,2)+1
if i==1
'String','tethai',...

(1/(size(di,2)+1))],...
'Enable','on',...
else
'String',char(tethai(i-1)),...
(1/(size(di,2)+1))],...
'Enable','on',...
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(alphai,2)+1
if i==1
'String','alphai',...
(1/(size(di,2)+1))],...
'Enable','on',...
else
'String',char(alphai(i-1)),...
(1/(size(di,2)+1))],...
'Enable','on',...
end
end
for i=1:size(alphai,2)
P_D_H(i,:)=[alphai(i);ai(i);tethai(i);di(i)]; %matriz con los
parametros de Denavit-Hartemberg
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% habilitación de nuevas opciones de los menus

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(graph_robot,'Enable','on')
set(graph_cininv,'Enable','on');
set(mfile_saveDH,'Enable','on');
end

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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
MRejes_aux=subs(MRejes,Q1_simb(j),Q1(j));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sustitucion de valores de rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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

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

MRejes_aux=subs(MRejes_aux,Q1_simb(j),Q1(j));
end
%sustitucion de valores de rotacion
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);

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

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 0 1];
Rx_1=[1 0 0 0;0 aux2 -aux1 0;0 aux1 aux2 0;0 0 0 1];
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 1];
Ry_1=[Lp/L 0 -A/L 0;0 1 0 0;A/L 0 Lp/L 0;0 0 0 1];
Rz=[cos(-tetha) sin(-tetha) 0 0;-sin(-tetha) cos(-tetha) 0 0;0 0 1
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);
end

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;

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');

save
(fname,'MR','n','num_simb_rot','Q1_simb','Q1','Q2_simb','Q2','num_simb
_rot2','MRperp','MRvec','MR_simb','rotac','tras')
return

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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if MRejes(i,13)==1 %Es un eje de rotacion
a(i-1)=0; %Tipo de articulación
cont1=cont1+1; %nº de artic de rotación
end
if MRejes(i,13)==2 %Es un eje de traslacion
a(i-1)=1; %Tipo de articulación
cont2=cont2+1; %nº de artic de traslacion
end
end
a(cont1+cont2+1)=0; %Consideramos el eje final como otro
eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sustituimos variables articulares por cero
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% creamos DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
DH=P_D_Haux;
for i=1:(size(DH,1))
DH(i,5)=a(i);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Guardamos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[fname,pname] = uiputfile('*.mat','Select File');

save (fname,'P_D_H','DH')
return

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',...
'Color',[1 1 1],...
'Resize','off',...
'Name','Sicon-Bot',...
'Interruptible','on',...
'Position',[0 0.08 1 0.92]);
subimage(img) %mostrar imagen de fondo

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

mopt_pardin=uimenu(mopt,'Label','Parámetros
Dinámicos','Callback','par_din','Accelerator','X','Enable','off');
graph= uimenu('Label','Ejecucion');
graph_calcbot=uimenu(graph,'Label','Calcular
Robot','Callback','calcbot','Enable','off');
graph_DH=uimenu(graph,'Label','Parámetros D-
H','Callback','Par_DH','Enable','off');
graph_robot=uimenu(graph,'Label','Posicionar
Robot','Callback','env_bot','Enable','off');
graph_cindir=uimenu(graph,'Label','Cinemática
Directa','Callback','cin_dir','Enable','off');
graph_cininv=uimenu(graph,'Label','Cinemática
Inversa','Callback','cin_inv','Enable','off');
mopt= uimenu('Label','Ayuda');
uimenu(mopt,'Label','Ayuda
Sicon','Callback','Ayuda','Accelerator','H','Enable','on');
uimenu(mopt,'Label','Sobre...','Callback','sobre','Enable','on');
end

function sobre
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nos muestra una ventana con datos sobre el programa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
img = imread('sicon.jpg'); %imagen de fondo

'Color',[1 1 1],...
'Resize','off',...
'Name','Sicon-Bot',...
'Position',[0.4 0.4 0.35 0.35]);
subimage(img) %nos muestra la imagen

axis off
end

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á
% [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
giro
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
l=sqrt(abs(xf-xi)^2+abs(yf-yi)^2+abs(zf-zi)^2); %longitud del tramo

if (yf-yi)>0.0001 %ángulo positivo
else
angx=(pi/2-asin((zf-zi)/l)); %ángulo negativo
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
y
% final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if (xi==xf)&(yi==yf) %si tramos es vertical

angz=0; %ángulo y=0
else
lx=xf-xi; %longitud en x
ly=yf-yi; %longitud en y
l1=sqrt(lx^2+ly^2); %longitud en plano x-y
V=[lx/l1 ly/l1 0]*l; %Proyeccion en eje XY
xf1=V(1);
yf1=V(2);
zf1=V(3);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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,
return

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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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; %Añadimos un elemento más de

traslación
l=str2num(get(htexto,'String')); %longitud inicial del tramos
a=get(htextot1,'String'); %nombre de variable (traslacion)
Q1_simb(num_simb_rot)=a; %matriz de nombre de variables
L=sym(a); %Longitud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
respecto
% al punto inicial

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+L;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[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));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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,
return

Memoria Final PDF

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Memoria Final PDF

Cargado por

Copyright:

Formatos disponibles

Diseño de una Plataforma Software Interactiva para la Simulación Cinemática de

Robots Manipuladores en entorno MATLAB

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

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

1.1 Introducción. ...................................................................................................... 7

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

2.1 Introducción ..................................................................................................... 12

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

3.1 Introducción ..................................................................................................... 23

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

4.1 Introducción. .................................................................................................... 31

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

5.1 Introducción ..................................................................................................... 38

ÁLVARO GÓMEZ RAMOS. -1-

5.3 Cinemática directa ........................................................................................... 39

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

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

1.1 Introducción. .................................................................................................... 42

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

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

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

3.1 Nuevo robot ...................................................................................................... 56

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

4.1 Introducción ..................................................................................................... 70

ÁLVARO GÓMEZ RAMOS. -2-

BLOQUE 3: CONCLUSIONES ........................................... 139

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

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

ÁLVARO GÓMEZ RAMOS. -3-

Durante los estudios universitarios hemos recibido una amplia formación en

Hoy en día la robótica se va extendiendo, y cada vez se generaliza más, con lo

El programa SICON-BOT surge con el objetivo de facilitar a los usuarios

- Comodidad: A la hora de integrar nuevas herramientas que nos permiten

- Integración: En un mismo software podemos utilizar funciones

En la primera versión del software SICON-BOT el objetivo es crear las bases de

En resumen los objetivos de la primera versión es el desarrollo desde cero del

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

- Comodidad: Púes realiza funciones como la obtención automática de los

ÁLVARO GÓMEZ RAMOS. -4-

- Integración: Pues incorpora internamente funciones del toolbox de robótica

ÁLVARO GÓMEZ RAMOS. -5-

BLOQUE 1: CONCEPTOS BÁSICOS

ÁLVARO GÓMEZ RAMOS. -6-

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

• Ingenio mecánico controlado electrónicamente, capaz de moverse y ejecutar de

Inmersos en la era de la informatización, la imperiosa necesidad de aumentar la

1.2 Historia de la robótica.

El concepto de robot se remonta casi al principio de la civilización, donde los

ÁLVARO GÓMEZ RAMOS. -7-

La civilización árabe cubre el primero de estos puntos al concebir el robot como

Durante el Renacimiento, los estudios de Leonardo da Vinci sobre anatomía del

No fue hasta finales de 1800, que se contempla de forma científica el concepto

1.3 Bases de la robótica actual

El inicio del desarrollo de la robótica actual puede fijarse en la industria textil

ÁLVARO GÓMEZ RAMOS. -8-

1.4 Definición de Robot

Según el instituto norteamericano Robot Industry Association (RIA) es la

- “Se entiende por robot industrial a un manipulador multifuncional

La Swedish Industrial Robot Association (SWIRA) define un robot como:

- “Una máquina manipuladora automáticamente controlada, reprogramable,

Esta definición es mas completa pues incluye la posibilidad de movilidad y

Japón tiene su propia definición más amplia: