Memoria Final PDF

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de
Robots Manipuladores en entorno MATLAB
Prefacio: ........................................................................................................................ 4
BLOQUE 1: CONCEPTOS BSICOS................................... 6

1
CAPTULO: INTRODUCCIN A LA ROBTICA..................................... 7
1.1
1.2
1.3
1.4
1.5
Introduccin. ...................................................................................................... 7
Historia de la robtica. ...................................................................................... 7
Bases de la robtica actual ................................................................................ 8
Definicin de Robot............................................................................................ 9
Lmites de la robtica actual........................................................................... 10
COMPONENTES BSICOS DE UN SSTEMA ROBTICO. ................. 12
2.1
2.2
2.3
2.4
2.5
2.6
2.7
2.8
2.9
2.10
Introduccin ..................................................................................................... 12
El Brazo Robot ................................................................................................. 12
Grados de libertad ........................................................................................... 12
Articulaciones................................................................................................... 14
Sistemas de coordenadas ................................................................................. 14
Volumen de trabajo ......................................................................................... 15
Actuadores ........................................................................................................ 16
El controlador................................................................................................... 18
Dispositivos sensoriales.................................................................................... 18
Elementos terminales....................................................................................... 21
CAPTULO: TRANSFORMACIONES 3D. ................................................. 23
3.1
3.2
3.3
3.4
3.5
3.6
Introduccin ..................................................................................................... 23
Geometra 3D ................................................................................................... 23
Cambio de escala.............................................................................................. 25
Traslacin ......................................................................................................... 25
Rotacin ............................................................................................................ 25
Rotacin alrededor de un eje arbitrario ........................................................ 26
CAPTULO: CINEMTICA. ........................................................................ 31
4.1
4.2
4.3
4.4
Introduccin. .................................................................................................... 31
El problema Cinemtico Directo .................................................................... 32
Algoritmo de Denavit-Hartenberg ................................................................. 33
Cinemtica Inversa .......................................................................................... 37
CAPTULO: TOOLBOX DE ROBTICA................................................... 38
5.1
5.2
Introduccin ..................................................................................................... 38
Representacin de traslacin 3D y orientacin ............................................. 38
LVARO GMEZ RAMOS.
-1-

5.3
5.4
Cinemtica directa ........................................................................................... 39

Cinemtica inversa........................................................................................... 39
BLOQUE 2: DESARROLLO DEL SOFTWARE ................ 41

1
CAPTULO: CONCEPCIN DEL SOFTWARE SICON-BOT. ............... 42
1.1
1.2
1.3
1.4
1.5
1.6
Introduccin. .................................................................................................... 42
SICON-BOT. .................................................................................................... 43
Objetivos de la primera versin...................................................................... 43
Posibles mejoras futuras.................................................................................. 44
Requisitos mnimos. ......................................................................................... 44
Inicializacin..................................................................................................... 45
CAPTULO: GRFICA DE SICON-BOT. .................................................. 46
2.1
2.2
2.3
2.4
Creacin de ventana principal y mens desplegables. ................................. 46

Paneles y plots de representacin ................................................................... 47
Eslabones y articulaciones............................................................................... 49
Botones, textos y cuadros editables. ............................................................... 54
CAPTULO: POSIBILIDADES DE SICON-BOT....................................... 56
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.10
Nuevo robot ...................................................................................................... 56

Calcular robot .................................................................................................. 58
Salvar Robot ..................................................................................................... 59
Cargar Robot.................................................................................................... 60
Parmetros D-H ............................................................................................... 62
Salvar D-H ........................................................................................................ 63
Cinemtica directa ........................................................................................... 63
Cinemtica inversa........................................................................................... 64
Posicionar Robot .............................................................................................. 66
Ayuda ................................................................................................................ 67
Herramientas.................................................................................................... 68
CAPTULO: PROGRAMACIN DE SICON-BOT.................................... 70
4.1
4.2
4.3
4.4
4.5
4.6
4.7
4.8
4.9
4.10
4.11
Introduccin ..................................................................................................... 70
Inicializacin de ventana inicial y mens desplegables ................................ 71
Entorno de creacin de robots ........................................................................ 73
Aadir elemento de traslacin ........................................................................ 76
Aadir elemento de rotacin........................................................................... 78
Aadir elemento eslabn ................................................................................. 80
Aadir elemento base ...................................................................................... 81
Posicionar y dibujar por primera vez el elemento traslacin ...................... 82
Posicionar y dibujar por primera vez el elemento rotacin......................... 85
Posicionar y dibujar por primera vez el elemento eslabn .......................... 88
Posicionar y dibujar por primera vez el elemento base ............................... 91
LVARO GMEZ RAMOS.
-2-

4.12
4.12
4.13
4.14
4.15
4.16
4.17
4.18
4.19
4.20
4.21
4.22
4.23
4.24
4.25
4.26
4.27
4.28
4.28
4.29
4.30
4.31
4.32
4.33
Rotar punto respecto a ejes x e y que pasan por otro punto dado .............. 94
Rotar punto respecto a ejes dado por dos puntos ......................................... 96
Calcular robot .................................................................................................. 99
Obtencin de los ejes de las articulaciones .................................................. 100
Obtencin de eje x, eje z y punto de origen ......................................... 101
Obtencin de eje y, normalizazin y dibujado de ejes............................ 106
Dibujar robot.................................................................................................. 107
Dibujar elemento eslabn.............................................................................. 108
Dibujar elemento traslacin.......................................................................... 111
Dibujar elemento rotacin ............................................................................ 114
Dibujar elemento base ................................................................................... 118
Salvar robot .................................................................................................... 119
Cargar robot................................................................................................... 119
Calculo de parmetros DH............................................................................ 120
Calculo de parmetros di ............................................................................... 123
Calculo de parmetros ai ............................................................................... 124
Calculo de parmetros i .............................................................................. 126
Calculo de parmetros i .............................................................................. 126
Cinemtica directa ......................................................................................... 127
Cinemtica inversa......................................................................................... 129
Posicionar robot ............................................................................................. 134
Ayuda .............................................................................................................. 138
Funcin auxiliar: Multiplicar vectorialmente vectores simblicos ........... 138
Funcin auxiliar: Redondear a cero los valores de las matrices ............... 138
BLOQUE 3: CONCLUSIONES ........................................... 139

1
2
Conclusiones. .................................................................................................. 140

Desarrollos futuros......................................................................................... 141
BLOQUE 4: ANEXOS .......................................................... 142

1
ANEXO: CDIGO FUENTE....................................................................... 143
LVARO GMEZ RAMOS.
-3-

Prefacio:
Durante los estudios universitarios hemos recibido una amplia formacin en
robtica, tanto tericamente como en la prctica, aprendiendo a trabajar
matemticamente con los robots o a programarlos para que realicen diversas tareas. En
el transcurso de esa formacin hemos utilizado diversas herramientas informticas que
nos ayudan en diferente grado a tratar con los robots.
Hoy en da la robtica se va extendiendo, y cada vez se generaliza ms, 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 informticas, de hacer ms cmodo el trabajo
para los ms expertos e integrar en una sola herramienta, otras herramientas, para hacer
su uso ms sencillo. En definitiva, SICON-BOT tiene tres objetivos principales:
FACILIDAD-COMODIDAD-INTEGRACIN
-
Facilidad: Con un entorno grfico familiar, con diseo de robots en 3D, con
ventanas, mens desplegables, botones, y dems funciones grficas que son
familiares a la mayora de personas.
Comodidad: A la hora de integrar nuevas herramientas que nos permiten

obtener automticamente parmetros necesarios para otras herramientas, que
hasta ahora tenamos que calcular a mano.
Integracin: En un mismo software podemos utilizar funciones

pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se
resuelven en la programacin interna.
En la primera versin del software SICON-BOT el objetivo es crear las bases de

la herramienta, para que poco a poco puedan ir amplindose en un futuro por nuevos
usuarios interesados, para lo cual el cdigo es accesible.
En resumen los objetivos de la primera versin es el desarrollo desde cero del
software, creando toda la programacin necesaria para el entorno grfico, el diseo de
robots en 3D, el clculo de ejes, el clculo de matrices DH, el clculo de la cinemtica
directa e inversa, y el posicionamiento del robot en 3D.
Con esto tenemos en la primera versin los tres objetivos fundamentales:
-
Facilidad: Con el entorno grfico que incorpora, el diseo intuitivo de

robots, el clculo automtico de parmetros mediante simple insercin de
datos sencillos.
Comodidad: Pes realiza funciones como la obtencin automtica de los

ejes o de la matriz DH.
LVARO GMEZ RAMOS.
-4-

-
Integracin: Pues incorpora internamente funciones del toolbox de robtica

como la cinemtica directa e inversa.
LVARO GMEZ RAMOS.
-5-

BLOQUE 1: CONCEPTOS BSICOS
LVARO GMEZ RAMOS.
-6-

CAPTULO: INTRODUCCIN A LA ROBTICA.
1.1
Introduccin.
La Robtica describe todas las tecnologas asociadas con los robots. Sin
embargo la definicin de robot entraa ms dificultad, pues existen muchas
definiciones:
Ingenio mecnico controlado electrnicamente, capaz de moverse y ejecutar de

forma automtica acciones diversas, siguiendo un programa establecido.
Mquina 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 mquina que hace algo automticamente en respuesta a su
entorno.
Un robot es un puado de motores controlados por un programa de ordenador.
Un robot es un ordenador con msculos.
Es cierto, como acabamos de observar, que los robots son difciles de definir. Sin
embargo, no es necesariamente un problema el que no est todo el mundo de acuerdo
sobre su definicin. Quizs, Joseph Engelberg (padre de la robtica industrial) lo
resumi inmejorablemente cuando dijo: "Puede que no se capaz de definirlo, pero s
cundo veo uno".
La imagen del robot como una mquina 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 das, no surge como consecuencia de la tendencia o aficin de
reproducir seres vivientes, sino de la necesidad.
Inmersos en la era de la informatizacin, la imperiosa necesidad de aumentar la
productividad y mejorar la calidad de los productos, ha hecho insuficiente la
automatizacin industrial rgida, dominante en las primeras dcadas del siglo XX, que
estaba destinada a la fabricacin de grandes series de una restringida gama de
productos. Hoy da, ms de la mitad de los productos que se fabrican corresponden a
lotes de pocas unidades.
1.2
Historia de la robtica.
El concepto de robot se remonta casi al principio de la civilizacin, donde los

mitos hablan de seres mecnicos dotados de vida. En la civilizacin griega aparecen
figuras que se mueven mediante poleas y bombas hidrulicas y que se usan para
propsitos estticos y artsticos. Tal es el caso del Hero's Automatic Theater and
Driver, una estatua de un dios alrededor de la cual rotaban pequeas figuras de forma
peridica, o del mtico Coloso de Rodas, que defenda el puerto de la ciudad. No
obstante, la sociedad griega careca de dos puntos bsicos para un desarrollo
satisfactorio de robots: necesidad y tecnologa.
LVARO GMEZ RAMOS.
-7-

La civilizacin 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
pragmtico es la Fuente del Pavo Real, un sencillo dispositivo que meda el nivel del
agua vertida en un recipiente de forma que, al alcanzarse un cierto umbral, apareca un
autmata portando una pastilla de jabn y, transcurrido un cierto tiempo, una toalla.
Durante el Renacimiento, los estudios de Leonardo da Vinci sobre anatoma del
cuerpo humano aportaron un valioso conocimiento a la hora de desarrollar la mecnica
del robot, principalmente los antropomorfos, permitiendo la construccin de junturas
mecnicas mucho mejores e impulsando por tanto el desarrollo modular de mquinas
complejas.
No fue hasta finales de 1800, que se contempla de forma cientfica el concepto
de autonoma e inteligencia artificial, cuando Nikola Tesla se propone crear una
mquina capaz de tomar sus propias decisiones sin necesidad de un telecontrol. Ms
adelante, el escritor checo Karel Capek acuara el trmino robot como tal en su obra
Rossums Universal Robots, que se estren en Praga en 1921. El trmino deriva de la
palabra checa robota, que define un trabajo forzado o de carcter feudal y la obra
trataba de la deshumanizacin en una sociedad tecnolgica. En 1926 Fritz Lang tocara
el mismo tema, sta vez con robots mecnicos, en la pelcula Metrpolis.
La robtica como ciencia que estudia el robot y su uso se concebira ms tarde,
cuando en 1942 el escritor de origen ruso Isaac Asimov escribi el relato corto
Runaround y la recopilacin posterior Yo, Robot. Desde entonces, los robots no slo
han acaparado la atencin de la ciencia ficcin sino tambin la de un creciente nmero
de investigadores, presentndose 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 robtica actual
El inicio del desarrollo de la robtica actual puede fijarse en la industria textil

del siglo XVIII, cuando en 1801 Joseph Jacquard inventa una mquina textil
programable mediante tarjetas perforadas denominada Teal Programable y que se
producira en masa. Conforme la Revolucin Industrial progresaba, continu el
desarrollo de estos agentes mecnicos, aunque el desarrollo de un verdadero robot no
fue posible hasta los aos cuarenta, con la aparicin de la computadora y la cada vez
mayor integracin 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. Tambin en 1946 aparece la primera computadora: J. Presper Ecker y John
Maulchy construyen el Eniac en la universidad de Pensilvania y la primera mquina
digital de propsito general se desarrolla en el MIT. Ms tarde Devol, en 1954, disea
el primer robot programable y acua el trmino Autmata universal que
posteriormente recorta a unimation. Ese sera el nombre de la primera compaa de
robtica, fundado por Engleberger, que se considera el padre de la robtica.
LVARO GMEZ RAMOS.
-8-

1.4
Definicin de Robot
Al hablar de de robots se piensa en un dispositivo fijo al suelo o al techo, con

una cierta estructura mecnica que le confiere varios grados de libertad (generalmente
de aspecto antropomorfo); con un computador encargado del control de los
movimientos y cuya misin no va ms all de simple transportador de piezas, o a lo
sumo, y con la ayuda de herramientas terminales para la manipulacin de algn
producto. Esta idea de la robtica abunda incluso en las definiciones ms o menos
oficiales.
Segn el instituto norteamericano Robot Industry Association (RIA) es la
siguiente:
-
Se entiende por robot industrial a un manipulador multifuncional

reprogramable, diseado para desplazar materiales, piezas, herramientas o
dispositivos especiales mediante movimientos programados variables que
permiten llevar a cabo tareas diversas.
Esta definicin se queda algo desfasada porque las tareas que puede realizar un
robot son ms complejas que las recogidas.
La Swedish Industrial Robot Association (SWIRA) define un robot como:
-
Una mquina manipuladora automticamente controlada, reprogramable,

multi-propsito con o sin locomocin para uso en aplicaciones industriales
de automatizacin.
Esta definicin es mas completa pues incluye la posibilidad de movilidad y

generaliza las operaciones posibles, adems est actualmente considerada por la Norma
ISO.
Japn tiene su propia definicin ms amplia:
-
Un robot es todo aquel dispositivo mecnico con algn grado de libertad,

destinado a su utilizacin industrial como manipulador.
La definicin mas precisa es la aportada por la Asociacin Francesa de

Normalizacin (AFNOR) aprobada en Agosto de 1983. AFNOR define en primer lugar
el manipulador y a continuacin 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 lgico (levas, lgica neumtica, lgica elctrica
cableada o bien programado).
Robot industrial: Manipulador automtico, con servo sistemas de posicin,

reprogramable, polivalente, capaz de posicionar y desplazar materiales,
piezas tiles o dispositivos especiales a lo largo de movimientos variables y
LVARO GMEZ RAMOS.
-9-

programables para la ejecucin de tareas variadas. Estas mquinas
polivalentes son generalmente concebidas para efectuar la misma funcin
de manera cclica y pueden ser adaptados a otras funciones sin
modificacin permanente del material.
1.5
Lmites de la robtica actual
Michael Knasel, director del Centro de Aplicaciones Robticas de Science

Application Inc., explic en la exposicin Robots-8 en Detroit (1984) que los robots
deberan evolucionar durante cinco generaciones. Las dos primeras ya se haban
manifestado en los ochenta e incluan la gestin de tareas repetitivas, pero estaban muy
limitadas en cuanto a movimiento se refiere. La tercera generacin incluira visin
artificial, la cuarta movilidad avanzada en exteriores e interiores y la quinta entrara en
el dominio de la inteligencia artificial. Lamentablemente, si bien se han cumplido
algunas de estas expectativas, an no se han conseguido modelos lo suficientemente
efectivos. Los lmites de la robtica actual se pueden apreciar en contraste con las
expectativas previstas por Joseph Engelberger para la robtica de los ochenta:
-
Seis articulaciones de precisin entre la base del robot y el extremo de un

manipulador
Programacin sencilla e intuitiva
Adaptacin al medio mediante estmulos 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
Sincronizacin con blancos mviles
Compatibilidad con ordenadores personales
Alta fiabilidad
Visin artificial que incluya al menos capacidad de orientacin y
reconocimiento
Sensores tctiles
Coordinacin de las distintas partes de la unidad
Capacidad de correccin de trayectorias en lnea
Movilidad
Optimizacin de movimiento
Conservacin de la energa
Apndices de propsito general
Comunicacin por voz
Seguridad de acuerdo a las leyes de Asimov de la robtica.
Engelberger aseguraba que los primeros diez puntos ya se haban conseguido a

principios de los ochenta. Los sistemas sensoriales por visin y tacto, si bien existen, se
limitan a aplicaciones muy bsicas y especficas 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 precisin de la deseable debido a
imperfecciones, deslizamiento y problemas mecnicos en general. La energa necesaria
para operar una unidad robtica ha disminuido mucho desde el 79, y se han conseguido
unidades capaces de funcionar con bateras solares, como las TEN-TOMUSHI de
LVARO GMEZ RAMOS.
- 10 -

Sanyo. No obstante debe seguirse trabajando en este punto, especialmente en robots
destinados a desplazarse en exteriores y entornos hostiles, donde la autonoma del
mvil es un facto crtico. Respecto a la comunicacin 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 ms satisfactoria.
LVARO GMEZ RAMOS.
- 11 -

COMPONENTES BSICOS DE UN SSTEMA ROBTICO.
2.1
Introduccin
La mayora de robots tienen los siguientes componentes comunes (ver figura

2.1), aunque la estructura mecnica, elctrica, y computacional de un robot puede variar
apreciablemente:
-
2.2
Un manipulador o brazo (la unidad mecnica), incluyendo una garra o

elemento final.
Un controlador (El mdulo inteligente).
Una unidad de potencia.
Un sistema sensorial.
El Brazo Robot
El brazo robot consiste en una serie de miembros rgidos, llamados elementos,

conectados por articulaciones. Al moverse una articulacin se mueven tambin los
siguientes elementos unidos a ella. El movimiento se debe a un actuador, que bien
puede ir conectado directamente o a travs de alguna transmisin mecnica (con el
objetivo de producir una par o velocidad o una ganancia). El ltimo elemento con el
que termina el manipulador est diseado para que pueda adaptarse una herramienta.
El manipulador podra dividirse en tres partes:
-
Los elementos mayores

Los elementos menores (componentes de la mueca)
El elemento terminal (garra o herramienta)
Los elementos mayores lo forman los pares articulacin-elemento, que sirven

para posicionar el extremo del brazo en un punto del espacio, y que generalmente son
los tres primeros.
Los elementos menores estn asociados con la orientacin del elemento final, y
junto con los elementos mayores se obtiene el posicionamiento y orientacin del
elemento final.
El elemento final consiste en el mecanismo que permite desempear una
funcin al robot, ya sea una garra, soldador, taladro, etc.
2.3
Grados de libertad
Se denomina grados de libertad del sistema al nmero de coordenadas

independientes necesarias para expresar la posicin de todas sus partes.
LVARO GMEZ RAMOS.
- 12 -

Para un elemento final tpico 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 direccin
necesitaremos otros tres grados de libertad ms. 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 orientacin y posicin dentro del
espacio de trabajo del robot.
Los tres primeros grados de libertad, que definen la posicin, 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 estn localizados cerca del punto central de la
herramienta del robot y sirven como mueca para controlar la garra u orientacin de
la herramienta. Estos tres grados se denominan giro, elevacin y guiada.
Cuando aadimos una nueva articulacin podemos estar aadiendo un nuevo
grado de libertad, pero esto no es siempre as, por eso cuando no ocurre se dice que
aade un grado de movilidad, que a veces es necesario para dotar de una movilidad
especfica al brazo para poder evitar obstculos 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 razn para aadir nuevas articulaciones que no aadan grados de libertad
adicionales, es debido a la limitacin de rotacin de las articulaciones rotativas, que
generalmente no pueden rotar 360 por restricciones mecnicas. As aadiendo nuevos
grados de movilidad podemos compensar estas limitaciones.
LVARO GMEZ RAMOS.
- 13 -

2.4
Articulaciones
Existen cinco tipos de articulaciones disponibles, pero generalmente slo 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 mnimo tres ejes, por lo que
existen cuatro tipos de sistemas de coordenadas para mover los robots segn el tipo de
las articulaciones:
-
3 ejes traslacin -> Sistema Cartesiano

2 ejes traslacin + 1 eje rotacin -> Sistema Cilndrico
1 eje traslacin + 2 ejes rotacin -> Sistema Polar
3 ejes rotacin -> Sistema Cartesiano
LVARO GMEZ RAMOS.
- 14 -

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 orientacin.
Normalmente para el clculo del espacio del robot se tiene en cuenta el final de
la mueca y no la herramienta, porque esta no tiene porque ser fija.
En definitiva, el volumen de trabajo del robot est determinado por las
siguientes caractersticas fsicas:
- La configuracin fsica del robot
- El tamao de los componentes del cuerpo, brazo y mueca
- Los lmites de movimiento de las articulaciones del robot
LVARO GMEZ RAMOS.
- 15 -

2.7
Actuadores
Son los dispositivos que proporcionan la fuerza motriz para las articulaciones de
los robots. Pueden ser de tres tipos segn la fuente de energa: Elctricos, Hidrulicos y
Neumticos.
Los elctricos son cada vez ms empleados, pues tienen una gran
controlabilidad con un mnimo mantenimiento. Dentro de las diferentes variedades, los
ms comunes son los servomotores de corriente continua, los motores paso a paso y los
servomotores de corriente alterna.
Los actuadores hidrulicos estn accionados por un fluido en movimiento,

generalmente aceite a presin. Se asocia generalmente con robots grandes. Sus ventajas
principales es que proporcionan mayor velocidad y fuerza, pero a cambio hacen el
robot ms grande y adems pueden existir fugas de aceite.
LVARO GMEZ RAMOS.
- 16 -

El accionamiento neumtico hace uso de un medio fluido que es altamente

compresible. Este fluido suele ser aire que est fcilmente disponible y no es
inflamable. Este tipo de accionamiento est generalmente reservado para robots
pequeos que poseen menos grados de libertad. Suelen estar limitados para operaciones
de recogida y depsito con ciclos rpidos. Pueden ser fcilmente adaptados al
accionamiento de dispositivos de pistn para proporcionar un movimiento de
translacin de articulaciones deslizantes. Tambin pueden ser usados en actuadores
rotatorios para articulaciones cilndricas.
LVARO GMEZ RAMOS.
- 17 -

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

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

programa
Un secuenciador que interpreta los datos de programa de la memoria.
Una CPU que realiza los clculos necesarios con los datos obtenidos con el
secuenciador.
Una interfaz que obtenga datos mediante sensores o sistemas de visin.
Una interfaz que mande la informacin 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 enseanza.
Dispositivos sensoriales
Los sensores utilizados en robtica pueden dividirse en dos clases:
Sensores internos: Son los dispositivos usados para medir la posicin, velocidad o
aceleracin de las articulaciones del robot y/o del elemento final. Con esta informacin
tratar ms adelante el sistema de control para obtener el movimiento deseado. Pueden
ser de diferentes clases:
-
Potencimetros: Es un elemento resistivo de valor mximo (R) que vara su

valor segn la posicin de giro o el desplazamiento, con lo cual, midiendo su
tensin inicial y su tensin final podemos saber en que posicin est el
robot, debido a la relacin:
r
Vout = Vin .
R
LVARO GMEZ RAMOS.
- 18 -

-
Synchro: Es transductor rotacional que convierte desplazamientos angulares

en voltaje de corriente alterna. Se basa en la frmula:
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 estn desplazadas mecnicamente 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 determinacin de posicin con
relativa facilidad y sorprendente precisin, adems de no necesitar contacto para
obtener la medida. Pueden ser de 2 tipos:
o Codificadores pticos absolutos: Es capaz de memorizar la posicin
siempre, incluso recin conectado, por lo que no necesita ningn
ciclo de calibracin
LVARO GMEZ RAMOS.
- 19 -

o Codificadores pticos incrementales: Slo son capaces de medir
incrementos o decrementos sobre la posicin actual, con lo que si no
se conoce la posicin actual no es posible conocer las futuras
posiciones. Necesita de un ciclo de calibracin para obtener una
posicin inicial sobre la que ir midiendo los cambios.
+
-
Tacmetros: Se aprovecha la variacin de una tensin DC que crean los

motores al variar sus velocidades para obtener precisamente la velocidad de
giro.
Sensores externos: Son dispositivos usados para monitorizar la relacin geomtrica

y/o dinmica 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 tecnologas que
LVARO GMEZ RAMOS.
- 20 -

hagan est labor: sensores de luz reflejada, sensores de ultrasonidos,
sensores de barrido lser, 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 ms crtica
de disear. Sus caractersticas ms deseadas son:
-
Diseo simple.
Tamao pequeo y poco peso.
Capacidad para agarrar firmemente las piezas.
Autocentrado
Una configuracin de sujecin habitual.
LVARO GMEZ RAMOS.
- 21 -

Existen muchos mtodos para aportar energa al elemento terminal de un robot,
como por ejemplo, cilindros de aire comprimido, electroimanes, o motores elctricos.
Dispositivos de aire comprimido: Es el mtodo ms utilizado debido a que son

potentes y pequeos. Entre sus ventajas cabra destacar su pequeo tamao y peso con
relacin a su potencia. Tambin 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. Tambin pueden tener fugas o tener un respuesta
muy lenta en circuitos de aire demasiado largos.
Cilindros hidrulicos: Se usan cuando hace falta fuerzas extremadamente altas para
su tamao, adems de muy buenos amortiguamientos y tiempo de respuesta frente a los
circuitos largos. Pero hace falta una bomba hidrulica para cada dispositivo hidrulico
utilizado, adems de ser susceptible de fugas.
LVARO GMEZ RAMOS.
- 22 -

CAPTULO: TRANSFORMACIONES 3D.
3.1
Introduccin
Algunas aplicaciones grficas son bidimensionales: dibujos y grficos, algunos mapas y

creaciones artsticas pueden ser entidades estrictamente bidimensionales. Pero vivimos
en un mundo tridimensional, y en muchas aplicaciones de diseo debemos manejar y
describir objetos tridimensionales. Si un arquitecto desease ver el aspecto real de la
estructura, entonces un modelo tridimensional le permitira observarla desde diferentes
puntos de vista. Un diseador de aviones podra desear analizar el comportamiento de
la nave bajo fuerzas y tensiones tridimensionales. En este caso se necesita tambin una
descripcin tridimensional. Algunas aplicaciones de simulacin, como el aterrizaje de
un avin, tambin exigen una definicin tridimensional del mundo.
En este captulo 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
visualizacin es slo bidimensional, debemos considerar la forma de proyectar nuestros
objetos en esta superficie plana para formar la imagen. Se discutirn tanto las
proyecciones paralelas como perspectivas.
3.2
Geometra 3D
Empecemos nuestra discusin sobre las tres dimensiones revisando los mtodos
de la geometra analtica para definir objetos. El objeto ms 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 dimensin. Podemos
disponer este tercer eje para que forme ngulo recto con los otros dos.
Podemos utilizar este nuevo sistema de coordenadas para definir la ecuacin de
una recta. En dos dimensiones tenamos:
Mientras que en el caso tridimensional, son necesarias un par de ecuaciones
Son necesarias las coordenadas de dos puntos para construir estas ecuaciones
y
LVARO GMEZ RAMOS.
. Por lo tanto, todava son necesarios dos puntos para definir

- 23 -

una recta, como cabra esperar. De forma paramtrica, donde cada variable se expresa
en funcin de un parmetro u:
Nos gustara tambin trabajar con planos. Un plano, de forma general, se

especifica mediante una sola ecuacin de la forma:
Cabe sealar que si dividimos por A la ecuacin anterior tenemos
Esto significa que slo 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 estn alineados)
. Podemos determina la ecuacin de la siguiente manera, ya que cada punto

del plano debe satisfacer la ecuacin del plano
Tenemos as tres ecuaciones con tres incgnitas 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 direccin perpendicular al plano
recibe el nombre de vector normal. Por otro lado, si
. Este vector
es un punto genrico 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 expresin es vlida cuando

esta es la ecuacin de ese plano.
LVARO GMEZ RAMOS.
es un punto del plano, por lo tanto,
- 24 -

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 captulo anterior. As, por lo que respecta al cambio de escala, la tercera
coordenada que hemos introducido pueden tener su propio facto de escala, por lo tanto
ser necesaria una matriz
,
o de rango
3.4
si empleamos coordenadas homogneas
Traslacin
Igual que en el caso bidimensional, empleamos los elementos de la fila inferior de la

matriz de transformacin en coordenadas homogneas para reflejar la traslacin.
3.5
Rotacin
Cuando consideramos la rotacin de un objeto en dos dimensiones, vimos la

matriz de rotacin alrededor del origen
Podemos generalizar este concepto a una rotacin tridimensional alrededor del

eje z.
LVARO GMEZ RAMOS.
- 25 -

En esta rotacin, pensamos en el eje como fijo mientras que algn objeto se
mueve en el espacio. Podemos pensar tambin que el objeto permanece inmvil
mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la
direccin de la rotacin. 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 rotacin alrededor de los ejes x e y se realiza con una matriz de
transformacin similar a la anterior.
Para hacer una rotacin alrededor del eje y, del tal forma que el eje z se
convierte en el x realizamos la siguiente matriz en coordenadas homogneas
3.6
Rotacin alrededor de un eje arbitrario
Para que las transformaciones tridimensionales nos sean un poco ms

familiares, resolvamos un sencillo problema. Aunque hemos visto matrices de
transformacin 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 rotacin.
El problema consiste en establecer la matriz de rotacin para un ngulo j alrededor de
una recta cualquiera. Construiremos esta transformacin 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 rotacin 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 traslacin del origen a su
posicin original.
Lo primero que debemos hacer es escoger una representacin adecuada de la
recta que ser el eje de rotacin. Un punto de dicha recta, as como la direccin de la
LVARO GMEZ RAMOS.
- 26 -

misma son suficientes para definir la recta (ecuacin vectorial). El punto de la recta nos
ofrece la informacin necesaria para la traslacin del origen, y la direccin nos informa
de los ngulos de rotacin correctos para alinear la recta con el eje z.
Dada la recta:
Un punto de esta recta es

vector
y su direccin est especificada por el
.
La matriz de transformacin que traslada el origen al eje de giro ser:
Necesitaremos tambin la inversa de traslacin para devolver el origen a su

posicin original una vez que las rotaciones se hayan completado.
El siguiente paso en este proceso es la rotacin alrededor del eje x. Deseamos

realizar un giro que site al eje arbitrario de rotacin en el plano xz. Para determinar el
ngulo de giro necesario, pongamos nuestro vector de direccin en el nuevo origen y
consideremos su proyeccin sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta comprendido entre el origen
y
est en la
direccin del eje arbitrario de rotacin, y todo el segmento est situado sobre dicho eje.
Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un
vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende
desde
hasta
LVARO GMEZ RAMOS.
- 27 -

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 fcil deducir las razones trigonomtricas del
ngulo q
Por lo tanto, la matriz de transformacin para el giro alrededor del eje x ser
LVARO GMEZ RAMOS.
- 28 -

La transformacin inversa es una rotacin 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 rotacin alrededor de este eje no ha modificado la coordenada x. Sabemos

tambin que la longitud total del segmento
no vara. 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 trigonomtricas son
LVARO GMEZ RAMOS.
- 29 -

La matriz de rotacin en este caso ser
La inversa de esta transformacin 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 rotacin ser
De esta forma ya somos capaces de realizar una rotacin j alrededor de un eje

arbitrario, mediante el producto de las transformaciones anteriores
LVARO GMEZ RAMOS.
- 30 -

CAPTULO: CINEMTICA.
4.1
Introduccin.
La cinemtica del brazo del robot trata con el estudio analtico de la geometra
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 cinemtica se interesa por la descripcin analtica del desplazamiento espacial del
robot como una funcin del tiempo, en particular de las relaciones entre la posicin de
las variables de articulacin y la posicin y orientacin del efecto final del brazo del
robot.
Hay dos problemas fundamentales en la cinemtica del robot. El primer
problema se suele conocer como el problema cinemtico directo, mientras que el
segundo es el problema cinemtico inverso. Como las variables independientes en un
robot son las variables de articulacin, y una tarea se suele dar en trminos del sistema
de coordenadas de referencia, se utiliza de manera ms frecuente el problema
cinemtico inverso. Denavit y Hartenberg en 1955 propusieron un enfoque sistemtico
y generalizado de utilizar lgebra matricial para describir y representar la geometra
espacial de los elementos del brazo del robot con respecto la un sistema de referencia
fijo. Este mtodo utiliza una matriz de transformacin homognea 4 x 4 para describir
la relacin espacial entre dos elementos mecnicos rgidos adyacentes y reduce el
problema cinemtico directo a encontrar una matriz de transformacin homognea 4 x
4 que relaciona el desplazamiento espacial del sistema de coordenadas de la mano al
sistema de coordenadas de referencia. Estas matrices de transformacin homogneas
son tambin tiles en derivar las ecuaciones dinmicas de movimiento del brazo del
robot. En general, el problema cinemtico inverso se puede resolver mediante algunas
tcnicas. Los mtodos utilizados ms comnmente son el algebraico matricial, iterativo
o geomtrico.
Cinemtica Directa (ngulos para encontrar posicin):
Se conoce:
a) La longitud de cada eslabn.
b) El ngulo de cada articulacin.
Se busca: La posicin de cualquier punto (coordenadas con respecto a la base)
Cinemtica Inversa (posicin para encontrar ngulos):
Se conoce:
a) La longitud de cada eslabn.
b) La posicin de cualquier punto (coordenadas con respecto a la base).
Se busca: El ngulo de cada articulacin necesitado para obtener la posicin.
LVARO GMEZ RAMOS.
- 31 -

4.2
El problema Cinemtico Directo
El problema cinmtico directo se reduce a encontrar la matriz de

transformacin homognea (T) que relacione la posicin y orientacin del extremo del
robot respecto a su sistema de referencia fijo (base del robot). La matriz T est en
funcin de los parmetros de las articulaciones del robot. Para un robot de n grados de
libertad tenemos:
x=
y=
z=
=
=
=
f x (q1
f y (q1
f z (q1
f (q1
f (q1
f (q1
q2
q2
q2
q2
q2
q2
...
...
...
...
...
...
qn )
qn )
qn )
qn )
qn )
qn )
Donde:
q1...n = Son las variables de las articulaciones. Para articulaciones de rotacin las
variables son ngulos. Para articulaciones prismticas las variables son distancias.
x, y, z = Coordenadas de la posicin del robot
, , = ngulos de orientacin de la mueca

Las funciones mencionadas pueden ser encontradas mediante mtodos
geomtricos para el caso de robots de 2 grados de libertad (cada relacin articulacineslabn constituye un grado de libertad):
LVARO GMEZ RAMOS.
- 32 -

x = l1 cos 1 + l 2 cos(1 + 2 )
y = l1 sen1 + l 2 sen(1 + 2 )
Para robots de ms de 2 grados de libertad es difcil aplicar mtodos
geomtricos para la solucin de su cinemtica directa.
A cada eslabn se le asocia un sistema coordenado y utilizando
transformaciones homogneas es posible representar las rotaciones y traslaciones
relativas entre los diferentes eslabones que componen el robot. Siendo la matriz:
Aii1
Siendo i el n de eslabn
La matriz de transformacin homognea que representa la posicin y

orientacin relativa entre los sistemas asociados a dos eslabones consecutivos del robot.
Se puede representar de forma parcial o total la cadena cinemtica que forma el
robot:
n
A0n = Aii1
i =1
Para el caso de un robot de 6 ejes, su cadena cinemtica queda representada por

la siguiente matriz de transformacin homognea:
T = A06 = A01 A12 A23 A34 A45 A56
4.3
Algoritmo de Denavit-Hartenberg
En 1955 Denavit y Hartenberg propusieron un mtodo matricial que permite

establecer de manera sistemtica un sistema de coordenadas. La representacin de
Denavit-Hartenberg (D-H) establece que seleccionndose adecuadamente los sistemas
de coordenadas asociados a cada eslabn, ser posible pasar de uno al siguiente
mediante 4 transformaciones bsicas que dependen exclusivamente de las
caractersticas geomtricas del eslabn.
Reducindose al siguiente patrn de transformaciones que permiten
relacionar el sistema de referencia del elemento i con respecto al sistema del
elemento i-1:
-
Rotacin alrededor del eje z i 1 un ngulo i

Traslacin a lo largo de z i 1 una distancia d i
Traslacin a lo largo de x1 una distancia ai
Rotacin alrededor del eje x1 un ngulo i
LVARO GMEZ RAMOS.
- 33 -

Aii1 = T ( z , i )T (0,0, d i )T (ai ,0,0)T ( x, i )

Desarrollando la expresin:
Aii1
cos i
sen
i
=
0
sen i
cos i
0
0
0
0
1
0
0 1
0 0
0 0
1 0
0
1
0
0
0 0 1
0 0 0
1 d i 0
0 1 0
0
1
0
0
0 a i 1
0
0 0 0 cos i
1 0 0 sen i
0 1 0
0
0
0
0
0
sen i
cos i
0
Obtenemos la expresin general de DH, donde i , d i , ai , i son los parmetros

DH del eslabn i:
Aii1
cos i
sen
i
=
0
cos i sen i
cos i cos i
sen i
0
sen i sen i
sen i cos i
cos i
0
ai cos i
ai sen i
di
Para que la matriz Aii1 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 eslabn fijo (base) como 0.
2. Numerar y etiquetar los eslabones mviles desde 1 hasta el n eslabn mvil.
3. Localizar y numerar el eje de cada articulacin y etiquetarla comenzando desde Z 0
hasta Z n 1 . Si la articulacin es rotativa, el eje ser su propio eje de giro. Si la
articulacin es prismtica, 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 dems articulaciones:
5. Localizar el origen Oi :
a) En la interseccin del eje Z i con la lnea normal comn a la
interseccin de Z i y Z i 1 .
b) En la interseccin de Z i y Z i 1 , si es que Z i y Z i 1 se intersectan.
c) En la articulacin i, si Z i y Z i 1 son paralelos.
LVARO GMEZ RAMOS.
- 34 -

6. Establecer X i :
a) A lo largo de la lnea normal comn entre los ejes Z i y Z i 1 que pasa
por Oi .
b) En la direccin 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
articulacin rotacional, establecer Z n a lo largo de la direccin Z n 1 y establecer
el origen On de la manera que ms 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 comn establecer el eje Yn entre los dedos de la
pinza y X n ser ortonormal a Z n y Yn .
Obtener las Matrices de Transformacin Homogneas
10. Crear una tabla con los parmetros D-H de los eslabones:
Eslabn i
LVARO GMEZ RAMOS.
di
ai
- 35 -

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
parmetro variable en articulaciones rotatorias.
d i = Es la distancia a lo largo del eje Z i 1 desde el origen Oi 1 hasta la

interseccin del eje X i con el eje Z i 1 . Este es un parmetro variable en
articulaciones prismticas.
ai = Para articulaciones rotatorias: es la distancia a lo largo del eje X i
desde el origen Oi hasta la interseccin 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 transformacin homognea Aii1 para cada
eslabn de acuerdo a los datos de la tabla del punto anterior.
12. Obtener la matriz de transformacin que relacione el sistema coordenado
de la base con el sistema coordenado del extremo del robot, resultando en
la posicin y orientacin del sistema coordenado de la herramienta
expresado en coordenadas de la base.
n
T = A = Aii1
n
0
i =1
LVARO GMEZ RAMOS.
- 36 -

4.4
Cinemtica Inversa
El objetivo del problema cinemtico inverso consiste en encontrar los valores

que deben tomar las variables articulares del robot para que su extremo se posicione y
oriente segn una determinada localizacin espacial. La ecuacin matemtica 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 prismticas las
variables son distancias.
x, y, z = Coordenadas de la posicin del extremo del robot.
, , = ngulos de la orientacin del extremo del robot.

n = Nmero de grados de libertad
A diferencia del problema cinemtico directo donde de una manera sistemtica e
independiente de la configuracin del robot se llega a una solucin, en el problema
cinemtico inverso el mecanismo de solucin es fuertemente dependiente de la
configuracin y con frecuencia la solucin no es nica.
Normalmente los mtodos geomtricos nos permiten obtener normalmente los
valores de las primeras variables, que son las que consiguen posicionar el extremo del
robot en un punto determinado.
Tambin es posible recurrir a manipular directamente a las ecuaciones obtenidas
del problema cinemtico directo.
En muchos robots de 6 grados de libertad es posible aplicar acoplamiento
cinemtico, para que los ejes dedicados al posicionamiento y los ejes dedicados a la
orientacin, sean tratados como dos problemas independientes.
LVARO GMEZ RAMOS.
- 37 -

CAPTULO: TOOLBOX DE ROBTICA.
5.1
Introduccin
El toolbox de robtica permite al usuario crear y manipular fcilmente tipos de

datos fundamentales para la robtica, como pueden ser las transformaciones
homogneas, cuaternios y trayectorias. Incluye funciones para la cinemtica directa e
inversa, as como la dinmica.
Se basa en un mtodo general de representacin de cinemtica y dinmica de
manipuladores a travs de matrices. Estas comprenden, en el caso ms simple, los
parmetros de Denavit-Hartenberg del robot y pueden ser creadas por el usuario para
cualquier manipulador. La descripcin del manipulador puede ser mejorada,
aumentando la matriz, para incluir parmetros inerciales de los eslabones, y parmetros
de inercia y friccin de los motores.
5.2
Representacin de traslacin 3D y orientacin
En coordenadas cartesianas la traslacin puede ser representada por un vector de

posicin. Muchas representaciones de orientaciones en 3D han sido propuestas, pero la
mas comn en robtica son las matrices de rotacin ortonormales. Una transformacin
homognea es una matriz 4x4 que representa traslacin y orientacin y que adems
encaja muy bien con matlab, debido a la capacidad de manipulacin de matrices de
este.
Las transformaciones homogneas describen la relacin entre la traslacin y la
rotacin
Por ejemplo, para una traslacin T = transl(0.5, 0.0, 0.0), una rotacin en y T =
roty(pi/2) y una rotacin en z T = rotz(pi/2):
LVARO GMEZ RAMOS.
- 38 -

5.3
Cinemtica directa
La cinemtica directa consiste en resolver la posicin cartesiana y la orientacin

del efector final dado el valor de las articulaciones y conociendo la estructura del robot
a travs de la matriz de Denavit-Hartenberg.
Existe una funcin en el toolbox para resolver la cinemtica directa a travs de
la matriz DH, es la funcin fkine.
FKINE(DH, Q) calcula la cinemtica directa de una manipulador para un
estado dado por el vector de variables articulares Q. Dicho manipulador quedar
descrito por su matriz de parmetros de Denavit-Hartenberg (DH).
Es posible utilizar esta funcin 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 transformacin homognea 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 nmero de
filas de Q. T es lo que se denominar una trayectoria de transformaciones
homogneas. Para extraer la transformacin homognea (Ti) que hay
"condensada" en la fila i-sima de la matriz T, bastar con escribir:
TI = T(:,:,i)
Empleando la funcin FKINE segn este ltimo modo, es posible obtener todas
las transformaciones homogneas correspondientes a una trayectoria dada en el espacio
de variables articulares.
5.4
Cinemtica inversa
La cinemtica inversa consiste en encontrar los valores articulares que me

posicionan al robot en una determinada posicin.
El toolbox incluye una funcin para resolver la cinemtica inversa de los robots,
es la funcin 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 posicin y orientacin dadas por la transformacin T. La
solucin del problema cinemtico inverso no es nica en general, y es posible que para
LVARO GMEZ RAMOS.
- 39 -

una misma orientacin y posicin deseadas, se obtengan soluciones distintas en funcin
del vector inicial de variables articulares (Q0) que se le pase a IKINE.
Es posible usar la funcin para que devuelva las variables articulares correspondientes a
una sola posicin y orientacin, o bien a una trayectoria de posiciones y orientaciones.
Eso depender del formato del parmetro T:
- Si T es una transformacin homognea, entonces IKINE devuelve un vector
fila(Q) con las variables articulares correspondientes a la posicin y orientacin
indicadas en la matriz T.
- Si T es una trayectoria de transformaciones homogneas, entonces el resultado
ser una matriz (Q), en la que la fila i-sima contendr las variables articulares
correspondientes a la transformacin T (:, :, i ). La estimacin inicial para Q en
cada paso se toma de la solucin obtenida en el paso anterior.
Sea cual sea el formato de T, la estimacin inicial para el vector de variables
articulares ser la dada en el parmetro 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 solucin consiste en especificar un vector (fila o
columna) de pesos (M), cuyos elementos sern 0 para aquellos grados de libertad que
en cartesianas estn 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 nmero de elementos
no nulos debe ser igual al nmero de grados de libertad del robot.
ILIMIT es el nmero mximo de iteraciones que se ejecutarn en busca de una
solucin (un valor usual es 1000).
STOL ser la mxima diferencia que se admitir entre la transformacin
correspondiente a las variables articulares solucin y la transformacin con la posicin
y orientacin especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo
uso de la funcin TR2DIFF.
LVARO GMEZ RAMOS.
- 40 -

BLOQUE 2: DESARROLLO DEL

SOFTWARE
LVARO GMEZ RAMOS.
- 41 -

CAPTULO: CONCEPCIN DEL SOFTWARE SICON-BOT.
1.1
Introduccin.
Durante los estudios universitarios hemos recibido una amplia formacin en

robtica, tanto tericamente como en la prctica, aprendiendo a trabajar
matemticamente con los robots o a programarlos para que realicen diversas tareas.
En el transcurso de esa formacin hemos utilizado diversas herramientas
informticas 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 matemticas. Las hay que tratan sobre el control
cinemtico y dinmico del robot, facilitando el trabajo y ahorrando mucho tiempo al
usuario. La principal herramienta utilizada es el software de programacin MATLAB.
MATLAB es la abreviatura de Matrix Laboratory (laboratorio de matrices). Es
un programa de anlisis numrico creado por The MathWorks en 1984. Est disponible
para las plataformas Unix y Windows.
Se pueden ampliar sus capacidades con Toolboxes, algunas de ellas estn
destinadas al procesado digital de seal, adquisicin de datos, economa, inteligencia
artificial, lgica difusa... Tambin cuenta con otras herramientas como Simulink, que
sirve para simular sistemas.
Es precisamente uno de esos toolboxes, una de las herramientas ms utilizadas
en robtica, el Robotic Toolbox. Este toolbox consta de una serie de funciones que
resultan ms o menos tiles a la hora de trabajar en el control de los robots. Las
funciones que incluye se dividen en varios grupos:
- Transformaciones homogneas
- Cuaternios
- Cinemtica
- Dinmica
- Generacin de trayectorias
- Grficas
- Creacin de modelos de robots
- Demostraciones
Todas estas funciones nos permiten trabajar con el robot, pero a travs de
facilitarle ciertos parmetros. Normalmente trabaja a travs 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 clculos, lo cual implica ciertos conocimientos
de robtica previos para poder trabajar.
LVARO GMEZ RAMOS.
- 42 -

1.2
SICON-BOT.
Hoy en da la robtica se va extendiendo, y cada vez se generaliza ms, con lo

cual es conveniente crear ciertas herramientas que permitan acercar a usuarios menos
expertos, como pas con la informtica, que poco a poco se fue convirtiendo en un
tema ms generalista y se fueron desarrollando cada vez ms unas herramientas ms
intuitivas y sencillas.
Otra cuestin es la comodidad. Si disponemos de herramientas para trabajar con
los robots de una manera ms cmoda a partir del modelo matemtico de ste, por qu
no tener tambin una herramienta que nos facilite la obtencin del modelo matemtico
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 ms cmodo el trabajo para los ms expertos, e
integrar en una sola herramienta, otras herramientas, para hacer su uso ms sencillo. En
definitiva, SICON-BOT tiene tres objetivos principales:
FACILIDAD-COMODIDAD-INTEGRACIN
1.3
Facilidad: Con un entorno grfico familiar, con diseo de robots en 3D, con
ventanas, mens desplegables, botones, y dems funciones grficas que son
familiares a la mayora de personas.
Comodidad: A la hora de integrar nuevas herramientas que nos permiten

obtener automticamente parmetros necesarios para otras herramientas, que
hasta ahora tenamos que calcular a mano.
Integracin: En un mismo software podemos utilizar funciones

pertenecientes a otras herramientas, sin necesidad de conocerlas, porque se
resuelven en la programacin interna.
Objetivos de la primera versin.
En la primera versin del software SICON-BOT el objetivo es crear las bases de

la herramienta, para que poco a poco puedan ir amplindose en un futuro por nuevos
usuarios interesados, para lo cual el cdigo es accesible.
Se ha desarrollado enteramente en Matlab un software a travs de elementos
familiares, como ventanas y mens, al estilo Windows. Con l podremos dibujar en tres
dimensiones el robot con el que queramos trabajar, introduciendo ciertos parmetros
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 cinemtica, tanto directa como inversa.
LVARO GMEZ RAMOS.
- 43 -

El diseo del robot, es un diseo genrico y limitado a ciertos tipos de robots,
pes se basa en la adicin de elementos predefinidos, pero con esto podemos disear
los robots ms comunes.
En resumen los objetivos de la primera versin es el desarrollo desde cero del
software, creando toda la programacin necesaria para el entorno grfico, el diseo de
robots en 3D, el clculo de ejes, el clculo de matrices DH, el clculo de la cinemtica
directa e inversa, y el posicionamiento del robot en 3D.
Con esto tenemos en la primera versin los tres objetivos fundamentales:
1.4
Facilidad: Con el entorno grfico que incorpora, el diseo intuitivo de

robots, el clculo automtico de parmetros mediante simple insercin de
datos sencillos.
Comodidad: Pes realiza funciones como la obtencin automtica de los

ejes o de la matriz DH.
Integracin: Pues incorpora internamente funciones del toolbox de robtica

como la cinemtica directa e inversa.
Posibles mejoras futuras.
Existen una gran cantidad de posibles mejoras a aadir al programa, aqu

pondremos algunas que seran de gran utilidad:
-
1.5
Adicin de dinmica
Adicin de movimiento en 3D siguiendo trayectorias
Interaccin con otros robots
Posibilidad de disear un entorno de trabajo
Cambio de plataforma de programacin, para lograr una mayor eficacia,
como podra ser el lenguaje de programacin C o C++
Requisitos mnimos.
El software ha sido creado en matlab 7.0.0.19920 (R14), por lo cual necesita

cumplir los requisitos mnimos de este software y su instalacin:
-
Sistemas operativos: Windows XP, Windows 2000 (Service Pack 3 or 4) o

Windows NT 4.0 (Service Pack 5 or 6a)
Procesadores: Pentium III, IV, Xeon, Pentium M, AMD Athlon, Athlon XP,
Athlon MP
Espacio en disco: 345 MB
RAM: 256 MB (Minimum), 512 MB (Recommended)
LVARO GMEZ RAMOS.
- 44 -

-
Grfica: Tarjeta grfica con 16-, 24-, o 32-bit OpenGL
Tambin se necesita tener instalado el Robotics Toolbox versin 7 April-2002
1.6
Inicializacin.
Una vez instalado MATLAB, el toolbox de robtica y SICON-BOT, debemos

iniciar MATLAB,
elegir el directorio donde tenemos instalado SICON-BOT
y escribir la palabra sicon
LVARO GMEZ RAMOS.
- 45 -

CAPTULO: GRFICA DE SICON-BOT.
2.1
Creacin de ventana principal y mens desplegables.
Para la creacin de la ventana principal usamos el comando figure dentro de la

funcin 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 aade una imagen de fondo mediante:
Con imread cargamos una imagen del disco duro en una variable de matlab y
con subimage mostramos la imagen en la ventana
LVARO GMEZ RAMOS.
- 46 -

Con uimenu creamos los mens desplegables y sus opciones, en este ejemplo
creamos el men desplegable llamado Archivo que contiene las opciones Nuevo
Grficamente, Salvar Robot, Salvar D-H, Cargar Robot y salir
2.2
Paneles y plots de representacin

Creamos una serie de elementos visuales, como:
-
Paneles:
LVARO GMEZ RAMOS.
- 47 -

Plots para representacin de los elementos posibles del robot:
El resultado final es la ventana principal del programa, desde donde podremos

disear el robot o ejecutar sus herramientas:
LVARO GMEZ RAMOS.
- 48 -

Elementos
posibles del
robot
2.3
Parmetros
del robot
Plot principal
Eslabones y articulaciones.
Existen 4 tipos de elementos que estn disponibles para disear nuestro robot,
estos son: Articulacin de traslacin, articulacin de rotacin, eslabn y base.
Traslacin
Rotacin
Eslabn
Base
La ventana principal se ha dividido en una serie de subplots, donde se dibujaran

diferentes elementos.
LVARO GMEZ RAMOS.
- 49 -

Con la orden subplot se divide la ventana en una matriz (m, n) de pequeos

plots y despus 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
7
10
13
8
11
14
9
12
15
10
11
12
13
14
15
Para aadir un elemento a nuestro robot debemos introducir unos parmetros en

funcin del elemento a aadir, y luego pulsar el botn correspondiente.
LVARO GMEZ RAMOS.
- 50 -

Con esto conseguiremos introducir un elemento de traslacin en el plot de

principal. Por defecto con los valores indicados en la figura, el elemento se dibujara
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
traslacin posibles en el robot es de 5.
Para poder aadir el elemento en otra posicin, disponemos de dos parmetros
Con estos parmetros haremos rotar la figura vertical tantos grados como le
indiquemos alrededor de los ejes x y z que pasan por el centro de la base del robot.
LVARO GMEZ RAMOS.
- 51 -

Por ejemplo, si queremos que el elemento de traslacin 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 parmetros podemos posicionar cualquier elemento en la posicin

deseada.
Si lo que queremos insertar es una articulacin de rotacin:
LVARO GMEZ RAMOS.
- 52 -

Tenemos que proporcionarle un nombre a la variable, aconsejamos usar q1, q2,
q3, etc. Aunque la nica limitacin que existe es la de no usar los reservados para
articulaciones de traslacin ni otro que ya hayamos usado anteriormente.
El elemento con los valores por defecto se dibujar tambin inicialmente
vertical, aunque no nos har falta indicarle longitud, pues esta es fija, y el elemento de
rotacin es incorpreo, es decir no ocupa lugar, el elemento anterior al de rotacin y su
inmediatamente posterior estn unidos.
El elemento de rotacin har girar a todos los elementos posteriores a l a travs

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 colocacin inicial.
Aunque grficamente ser igual girar 90 en el eje x o -90, el robot resultante

ser totalmente distinto.
LVARO GMEZ RAMOS.
- 53 -

El elemento eslabn es igual al de traslacin slo que su longitud indicada ser
siempre la misma.
Y el elemento base, sirve para elevar al robot en su posicin inicial.
Los elementos se irn colocando uno tras el otro, y el punto inicial de uno se
colocar en el punto inicial del siguiente.
rotacin
traslacin
eslabn
base
Con estos elementos y sus parmetros 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 botn pulsador togglebutton que al pulsar

llame a la funcin rot3d
Textos:
LVARO GMEZ RAMOS.
- 54 -

Le indicaremos que queremos un texto que diga Rot. En X () y en que

posicin lo queremos.
Cuadros editables:
Elegimos la posicin deseada del elemento y que valor inicial queremos que
aparezca por defecto 0.
LVARO GMEZ RAMOS.
- 55 -

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

Nuevo grficamente.
Al pulsar esta opcin se abre la ventana principal de diseo del robot.
LVARO GMEZ RAMOS.
- 56 -

Ahora dependiendo del robot que queramos disear iremos introduciendo
nuestros elementos siguiendo el siguiente orden:
1. Si es un elemento de base o eslabn no hace falta indicar nada en la casilla
Nombre Var, pero si es un elemento de rotacin 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 traslacin
debemos indicar un consecutivamente los nombres t1, t2, t3, t4 y t5.
2. Una vez resuelto el nombre de la variable elegiremos el parmetro longitud,

que slo tiene efecto en los elementos eslabn y base, que establece su
longitud definitiva, y en el elemento traslacin establece su longitud inicial.
3. Para terminar de parametrizar el eslabn debemos indicar la rotacin en X y

en Z, pudiendo as posicionar el elemento en cualquier posicin, excepto el
elemento base que es siempre vertical. Tener en cuenta que el sentido de
rotacin depende mucho de estos parmetros.
4. Una vez que tenemos el elemento totalmente parametrizado, slo nos queda
indicarle que tipo de elemento es y ser aadido automticamente tomando
como punto inicial el final del elemento anterior o si es el primer elemento,
ser colocado en (0,0,0).
Ejemplo:
1.
2.
3.
LVARO GMEZ RAMOS.
- 57 -

4.
5.
El resultado es:
As iremos diseando nuestro robot paso a paso, elemento a elemento, hasta que
tengamos dibujado nuestro robot por completo:
Una vez diseado 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 Ejecucin y seleccionar la opcin Calcular robot.
LVARO GMEZ RAMOS.
- 58 -

Este proceso implicar un clculo complejo con variables simblicas, que

dependiendo del ordenador y de la complejidad del robot, tardar ms o menos tiempo.
Al final el resultado sern los ejes del robot representados en el propio diseo 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 parmetros de Denavit-Hartenberg
o ver su cinemtica directa.
3.3
Salvar Robot
Para salvar el robot es necesario haber calculado sus ejes con anterioridad, y lo
que esto implica, haberlo diseado. Luego iremos al men Archivo y elegiremos la
opcin Salvar robot.
Entonces se nos abrir una ventana donde elegir en que lugar guardar el robot y
con que nombre. Se guardar bajo la extensin .mat cuando le demos al botn
guardar.
LVARO GMEZ RAMOS.
- 59 -

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 opcin

Nuevo grficamente.
LVARO GMEZ RAMOS.
- 60 -

Al pulsar esta opcin se abre la ventana principal de diseo del robot.
Luego vamos al men desplegable Archivo y elegimos la opcin 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 botn
cargar, aadiendo as el robot al espacio de trabajo con los ejes ya calculados.
LVARO GMEZ RAMOS.
- 61 -

3.5
Parmetros D-H
Una vez calculado el robot o cargado, podremos calcular sus parmetros de

Denavit-Hartenberg o D-H, para ello iremos al men desplegable Ejecucin y
elegiremos la opcin Parmetros D-H
Entonces se nos abrir una nueva ventana que contiene los parmetros D-H del
robot representados de forma matricial:
di
ai
tethai
alphai
Con estos tendremos calculados los parmetros D-H de una forma totalmente
automtica, con los nombres de las variables que le indicsemos.
Una vez calculado los parmetros D-H, podremos acceder a nuevas opciones del
programa como Salvar D-H, Posicionar Robot o Cinemtica Inversa
LVARO GMEZ RAMOS.
- 62 -

3.6
Salvar D-H
Con esto podremos exportar la matriz D-H y la matriz P_D_H, la primera nos
indica los parmetros de Denavit-Hartenberg, la segunda adems de incluir la matriz DH nos guarda tambin el tipo de articulacin del robot, dando un 0 para rotacin y un 1
para traslacin. La matriz P_D_H es totalmente compatible con el toolbox de robtica
de matlab.
Para salvar D-H debemos seleccionar la opcin Salvar D-H del men
desplegable Archivo, elegir donde guardarla y con que nombre.
El archivo guardado tambin tendr la extensin .mat
3.7
Cinemtica directa
Para poder ver la cinemtica directa del robot debemos ir al men desplegable
Ejecucin y seleccionar la opcin Cinemtica directa.
LVARO GMEZ RAMOS.
- 63 -

Se nos abrir una nueva ventana que nos mostrar la cinemtica directa del
robot de la siguiente forma:
As sabremos cual ser la posicin del extremo del robot a partir del valor de las
variables.
3.8
Cinemtica inversa
Para poder ver la cinemtica inversa del robot debemos ir al men desplegable
Ejecucin y seleccionar la opcin Cinemtica inversa.
Se nos abrir la siguiente ventana, donde podremos indicar el valor de la

posicin final del extremo del robot que deseamos obtener, y pulsando el botn
calcular, obtendremos el valor necesario de las variables para que el robot alcance
dicha posicin.
LVARO GMEZ RAMOS.
- 64 -

Posicin
deseada
Nombre
de las
variables
Valor
calculado
de las
variables
Una vez pulsemos el botn calcular nos aparecer otra nueva pantalla con un
diseo esquemtico del robot en la posicin deseada.
Ejemplo:
LVARO GMEZ RAMOS.
- 65 -

3.9
Posicionar Robot
Para poder ver la cinemtica inversa del robot debemos ir al men desplegable
Ejecucin y seleccionar la opcin 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 botn intr..
Dato, se abrir una nueva ventana mostrando un diseo esquemtico del robot en la
posicin calculada. Adems si marcamos la opcin Ver Robot en 3D, tambin se
posicionar el robot de nuestro espacio de trabajo principal en dicha posicin, teniendo
en cuenta que el robot de la ventana emergente puede poseer cambios de variables para
simplificar la matriz D-H.
LVARO GMEZ RAMOS.
- 66 -

3.10 Ayuda
Sicon-bot dispone de una pequea ayuda para aprender a usar el programa.
Dicha ayuda se encuentra en la opcin Ayuda Sicon del men desplegable Ayuda.
Se abrir una ventana que contiene la ayuda disponible del software.
LVARO GMEZ RAMOS.
- 67 -

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

Existen tres herramientas disponibles:
-
Rotar robot:
Nos permite rotar el robot en el espacio de trabajo principal.

-
Ver/Ocultar ejes:
Nos permite ocultar los ejes calculados del robot o volver a verlos
cuando deseemos.
-
Ver/Ocultar robot:
Nos permite ocultar el robot calculados del robot o volver a verlos

cuando deseemos
LVARO GMEZ RAMOS.
- 69 -

CAPTULO: PROGRAMACIN DE SICON-BOT.
4.1
Introduccin
El proyecto de la creacin de sicon-bot se basa en su mayor parte en la

programacin 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 grfico accesible para
todos los usuarios, y que se basa en la utilizacin de elementos visuales para interactuar
con el software, mediante el uso de botones, grficos, textos, diseos en 3D, etc. Para
ello hace falta conocer la parte grfica de MATLAB, un lenguaje usado normalmente
para resolver la programacin orientada a las matemticas.
La eleccin de MATLAB para programar el software es precisamente por su
facilidad para ejecutar complejas operaciones matemticas, pues aunque el aspecto
visual es lo ms destacado para el usuario, la base es la programacin de funciones que
implementen operaciones matemticas complejas como la rotacin y traslacin de
elementos en 3D y sobre todo el tratamiento con grandes matrices que contienen todos
los datos del robot diseado, pues este una vez calculado no ser ms que una serie de
matrices numricas o simblicas con las que el software tiene que tratar.
Al sopesar la facilidad matemtica de MATLAB con la facilidad grfica de
otros lenguajes, pesa ms la parte de MATLAB, por lo que el lenguaje de programacin
elegido es MATLAB. Esto implica que debemos realizar toda la parte grfica con un
lenguaje menos apto para ello.
En resumen la programacin de MATLAB tiene un recorrido de ida y vuelta, es
decir, se crea un entorno grfico accesible para el diseo en 3D de robots, luego el
software convierte el robot grfico en matrices, trabaja con ellas y muestra el resultado
de forma grfica.
En resumen:
Datos de entrada en forma grfica
Conversin
Datos de entrada en forma matricial

Operacin
Datos de salida en forma matricial

Conversin
Datos de salida en forma grfica
LVARO GMEZ RAMOS.
- 70 -

4.2
Inicializacin de ventana inicial y mens desplegables
La primera fase de la programacin del software se centra en la creacin del

entorno grfico necesario para el uso del programa.
Para ello creamos la ventana principal a travs 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 parmetros a travs 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 mtodo de representacin grfica de los
objetos, con opengl elegimos el mtodo ms potente de representacin
grfica.
: Elegimos que el color de fondo sea blanco.
: Deshabilitamos la posibilidad de cambiar el tamao de la
ventana.
: El nombre que aparecer en la ventana ser SiconBot.
: Elimina que aparezca el nmero de la ventana
siguiendo su orden de creacin.
: Eliminamos la posibilidad de que la ventana se
pueda adosar al espacio de trabajo de matlab.
: Eliminamos los mens desplegables que trae por
defecto matlab.
:_Indicamos la posicin y tamao de la
pantalla.
LVARO GMEZ RAMOS.
- 71 -

Una vez creada la ventana principal, le queremos aadir 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
imgenes.
Ahora creamos los mens desplegables y si son accesibles o no desde el
principio:
Con uimenu creamos los mens, cuyo nombre indicamos a travs de

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

variable mfile
Pero con uimenu tambin podemos crear las opciones que se crearn en el
men al desplegarse:
Al indicarle la variable mfile donde se almacena el indicador del men, estamos

asociando esta opcin a dicho men. Con label damos texto a la opcin, con la
propiedad Callback indicamos la rutina que se ejecutar cuando se pulse la opcin, y
con la propiedad Separator,on, indicamos que dibuje una lnea para separar de las
siguientes opciones posibles, para as crear subgrupos diferenciados.
Existen otras propiedades interesantes como

para que la
opcin inicialmente no est habilitada, y poder habilitarla cuando se cumpla alguna
, que hace que la
condicin. Tambin es interesante la propiedad
LVARO GMEZ RAMOS.
- 72 -

opcin tambin se pueda ejecutar al pulsar la combinacin de teclas Alt + tecla
elegida.
4.3
Entorno de creacin de robots
Cuando elegimos nuevo robot, nos aparece la ventana con el entorno que nos
permite disear 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 habilitacin de los mens desplegables segn

nos interesen:
-
: Habilitamos la opcin Cargar del men

Archivo, se indica mediante el indicador de dicha opcin.
Creamos unos paneles para colocar en ellos diferentes elementos y agrupar los
que tienen algo en comn. Esta accin la hacemos con el comando uipanel, guardando
su indicador en una variable. Adems le indicamos a travs de las propiedades
BackgroundColor y Position, de que color ser y donde estar colocado con respecto
a los lmites de la ventana principal, y le damos un nombre con title:
LVARO GMEZ RAMOS.
- 73 -

Ahora crearemos los plots de la ventana, en concreto sern 6, uno principal

donde se dibuja el robot, y 5 secundarios que nos mostrarn las figuras posibles a
insertar.
Dividimos la pantalla en 15 partes e indicamos que el plot principal ocupar el

espacio de las divisiones nmero 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15. Adems
con xlabel e ylabel modificamos propiedades de los ejes x e y respectivamente,
como por ejemplo elegir un texto a mostrar y su color.
Luego creamos el resto de plots y hacemos que no se vean los ejes:
LVARO GMEZ RAMOS.
- 74 -

Lo siguiente ser dibujar las figuras de referencia de las que podremos elegir
para aadir a nuestro robot:
Para ello creamos una matriz con las coordenadas de los vrtices y otra donde se
le indican las caras de la figura a travs de los vrtices que la componen. Luego
elegimos el plot donde queremos representar la figura, y ejecutamos el comando
patch indicndole la matriz de vrtices y la de caras y el color del objeto a travs de
la propiedad FaceVertezCData. Tambin se le indica la manera de colorear el objeto,
en este caso a travs de FaceColor,flat, las caras se dibujarn por homogneas.
Con
Axis
xy
indicamos que los ejes son cartesianos, y con

especificamos el rango que queremos que muestren los ejes.
camorbit elige la rotacin de la cmara para mostrar el objeto, indicndole la rotacin
horizontal y vertical.
Despus de preparar los plots de representacin secundarios vamos a aadir
, consiguiendo que entre un rayo de luz
iluminacin al plot principal:
que ilumina al objeto desde la derecha.
El siguiente paso ser la creacin 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)
Mens emergentes (Pop-up menus)
Botones de pulsar (Push buttons)
Botones de eleccin (Radio buttons)
Deslizadores (Sliders)
LVARO GMEZ RAMOS.
- 75 -

-
Textos (Static text)

Botones de estado (Toggle buttons)
Con la propiedad CallBack elegimos la rutina a ejecutar al activarse el

elemento. Luego elegimos la posicin a colocar.
Botones de pulsar
Textos
Cuadros editables
Botones de estado
4.4
Aadir elemento de traslacin
Para aadir un elemento de traslacin en nuestro diseo del robot tan slo
tenemos que parametrizarlo correctamente y pulsar el botn correspondiente. Pero esto
es a nivel de usuario, en la programacin es un proceso ms complejo que veremos a
continuacin.
La funcin que coloca un elemento de traslacin en nuestro robot es la
siguiente:
LVARO GMEZ RAMOS.
- 76 -

Analicemos paso por paso la funcin. 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 matemtico.
Q1: Es un vector que contiene los valores de las diferentes variables de
traslacin, segn su orden de creacin.
n: Guarda el nmero de elementos que tiene nuestro robot.
(x,y,z): Son las coordenadas del punto actual donde se aadir el siguiente
elemento del robot.
htextoe: Cuadro editable que contiene el valor de la rotacin sobre el eje de
traslacin del elemento. (Actualmente 0 por estar deshabilitada).
htexto: Cuadro editable que contiene el valor de la variable actual de

traslacin.
tras: Guarda el nmero de elementos de traslacin que contiene el robot.
MR es la matriz simblica que contiene el robot en forma matemtica. Esta

matriz es de (n+1 x 8), siendo n el nmero 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 aadir un eje en el
extremo del robot. Las columnas contienen datos que nos parametrizan de forma
inequvoca al elemento en el robot:
MR: (1 [2 3 4] [5 6 7] 8)
MR: (tipo de elemento [posicin 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 ms en nuestro robot:
Obtenemos el valor inicial de la variable de traslacin que le indic el usuario:
Incrementamos en uno el nmero de elementos de traslacin que contiene

nuestro robot:
LVARO GMEZ RAMOS.
- 77 -

Lo siguiente sera guardar el valor del punto inicial donde dibujar el elemento
en forma simblica:
El paso siguiente recurre a la llamada de una funcin que dibuja el robot a partir
del punto inicial y los datos indicados y nos devuelve el nuevo valor inicial para el
prximo elemento a aadir que coincide con el punto final del elemento inicial. Este
dato nos lo devuelve en forma numrica y simblica:
Ahora indicaremos en la matriz MR el tipo de elemento que hemos aadido,

siendo un 1 para los elementos de traslacin:
Almacenamos el punto final del elemento aadido, que como ya hemos

indicado, coincide con el punto inicial del prximo elemento.
Luego guardamos el valor de rotacin que le indicamos que deseamos que tenga
la representacin grfica del elemento.
Ya tenemos parametrizado el elemento traslacin del todo, pudindolo dibujar

cuando se requiera.
4.5
Aadir elemento de rotacin
Para aadir un elemento de rotacin usamos el siguiente cdigo:
LVARO GMEZ RAMOS.
- 78 -

Veamos las nuevas variables globales que nos hacen falta:

-
Q2: Es un vector que contiene los valores de las diferentes variables de

rotacin, segn su orden de creacin.
rotac: Guarda el nmero de elementos de traslacin 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 rotacin que le indic el usuario:
Incrementamos en uno el nmero de elementos de rotacin que contiene nuestro

robot:
El elemento rotacin no ocupa espacio, es decir el punto inicial y el final sern
el mismo, por ello los siguiente sera llamar a una funcin que nos dibuja el elemento
rotacin y nos guarda en MR el punto final e inicial que son coincidentes:

siendo un 2 para los elementos de rotacin:
LVARO GMEZ RAMOS.
- 79 -

Ya tenemos parametrizado el elemento rotacin del todo, pudindolo dibujar

cuando se requiera.
4.6
Aadir elemento eslabn
Para aadir un elemento eslabn usamos el siguiente cdigo:

Luego llamamos a una funcin que nos dibuja el elemento base y nos guarda en
MR el punto final e inicial:
LVARO GMEZ RAMOS.
- 80 -

siendo un 3 para los elementos eslabn:
Ya tenemos parametrizado el elemento eslabn del todo, pudindolo dibujar

cuando se requiera.
4.7
Aadir elemento base
Para aadir un elemento base usamos el siguiente cdigo:

LVARO GMEZ RAMOS.
- 81 -

Luego llamamos a una funcin 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, pudindolo dibujar

cuando se requiera.
4.8
Posicionar y dibujar por primera vez el elemento traslacin
Para dibujar el elemento traslacin por primera usamos una funcin 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 parametrizacin,
obtenindose 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 funcin es obtener los valores de parametrizacin

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 nmero de variables de traslacin que
llevamos diseadas. :
Lo segundo sera obtener los ngulos indicados por el usuario, que nos sirven
para posicionar el robot adecuadamente:
LVARO GMEZ RAMOS.
- 82 -

A partir del valor del punto inicial, situamos el punto final momentneamente 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 vrtices, 4 por cada
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
X3
X2
La situacin de los 8 vrtices dependern del punto inicial donde dibujar el

elemento, suponiendo a ste vertical, as quedara:
(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 vrtices, pero en situacin vertical con la
longitud inicial indicada por el usuario. Ahora queremos girar todos los vrtices
alrededor del eje x y el eje z segn los valores deseados, para ello usamos la funcin:
Rotar es una funcin 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 posicin final.
LVARO GMEZ RAMOS.
- 83 -

Ahora rotaremos el punto final en vertical, para obtener el nuevo punto inicial
del siguiente elemento, luego sustituimos los valores simblicos por los valores
actuales, para tener el valor numrico del nuevo punto inicial:
Luego tenemos la posibilidad de rotar la figura alrededor de su eje principal,

para ello usamos la funcin rot que es una funcin 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 rotacin igual a tetha:
:
Ahora que tenemos los vrtices posicionados pero de forma simblica, tenemos
que calcular su valor actual para posicionarlos:
LVARO GMEZ RAMOS.
- 84 -

Y una vez que tenemos parametrizado el elemento en su posicin inicial, lo

dibujamos con el comando patch:
4.9
Posicionar y dibujar por primera vez el elemento rotacin
Para dibujar el elemento rotacin por primera usamos una funcin distinta que
inicial y los valores de giro indicadas por el usuario en la parametrizacin,

que nos indico el usuario, en este caso necesitamos el nombre de la variable. La
variable num_simb_rot2 guarda el nmero de variables de rotacin que llevamos
diseadas. :
LVARO GMEZ RAMOS.
- 85 -

A partir del valor del punto inicial, situamos el punto final coincidente:
base.
X8
X5
X7
X6
Pto. final
Pto. inicial
X4
X1
X3
X2
La situacin de los 8 vrtices depender del centro de gravedad del elemento,

suponiendo a ste vertical, as quedara:
(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)
LVARO GMEZ RAMOS.
- 86 -

hacemos pasar por otro punto dado Pi. Si hacemos Pi coincidir con el centro de
gravedad del elemento, ste rotar hasta posicionarse en su posicin final.
En este caso el punto inicial no variar, por lo que coincidir con el final.
:
LVARO GMEZ RAMOS.
- 87 -


4.10 Posicionar y dibujar por primera vez el elemento eslabn

Para dibujar el elemento eslabn por primera usamos una funcin distinta que

que nos indico el usuario, en este caso necesitamos la longitud:
LVARO GMEZ RAMOS.
- 88 -

base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
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)
LVARO GMEZ RAMOS.
- 89 -

del siguiente elemento:

:
LVARO GMEZ RAMOS.
- 90 -


4.11 Posicionar y dibujar por primera vez el elemento base

Para dibujar el elemento base por primera usamos una funcin 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:
LVARO GMEZ RAMOS.
- 91 -

base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
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)
LVARO GMEZ RAMOS.
- 92 -


:
LVARO GMEZ RAMOS.
- 93 -


4.12 Rotar punto respecto a ejes x e y que pasan por otro punto dado
Este procedimiento lo resuelve por completo la funcin creada para la ocasin:
Para realizar esta operacin matemtica nos basamos en el concepto de rotacin

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 traslacin 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:
Traslacin al origen
Rotacin
Deshacer traslacin
Para realizar la traslacin al origen hacemos la siguiente operacin:
LVARO GMEZ RAMOS.
- 94 -

Ahora el punto a tratar ser el formado por las coordenadas (A,B,C).

Lo siguiente es la rotacin del punto. Cuando consideramos la rotacin de un
objeto en dos dimensiones, vimos la matriz de rotacin alrededor del origen
Podemos generalizar este concepto a una rotacin tridimensional alrededor del

eje z.
En esta rotacin, pensamos en el eje como fijo mientras que algn objeto se
mueve en el espacio. Podemos pensar tambin que el objeto permanece inmvil
mientras los ejes se mueven. La diferencia entre uno y otro planteamiento es la
direccin de la rotacin. 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 rotacin alrededor de los ejes x e y se realiza con una matriz de
transformacin similar a la anterior.
Para hacer una rotacin alrededor del eje y, del tal forma que el eje z se
convierte en el x realizamos la siguiente matriz en coordenadas homogneas
Y finalmente multiplicamos las matrices de rotacin por el punto:
LVARO GMEZ RAMOS.
- 95 -

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

Se resuelve mediante la funcin:
Lo primero que debemos hacer es escoger una representacin adecuada de la

recta que ser el eje de rotacin. Un punto de dicha recta, as como la direccin de la
misma son suficientes para definir la recta (ecuacin vectorial). El punto de la recta nos
ofrece la informacin necesaria para la traslacin del origen, y la direccin nos informa
de los ngulos de rotacin correctos para alinear la recta con el eje z.
Dada la recta
y su direccin est especificada por el vector
un punto de esta recta es

. Para obtener los puntos
realizamos la siguiente operacin:
El siguiente paso en este proceso es la rotacin alrededor del eje x. Deseamos

realizar un giro que site al eje arbitrario de rotacin en el plano xz. Para determinar el
ngulo de giro necesario, pongamos nuestro vector de direccin en el nuevo origen y
consideremos su proyeccin sobre el plano yz. Podemos imaginar esto de la siguiente
forma: el segmento de recta comprendido entre el origen
y
est en la
direccin del eje arbitrario de rotacin, y todo el segmento est situado sobre dicho eje.
Si ahora iluminamos este segmento con un haz de luz paralela al eje x y echamos un
vistazo a la sombra que proyecta sobre el plano yz comprobamos que esta se extiende
desde
hasta
LVARO GMEZ RAMOS.
- 96 -

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 fcil deducir las razones trigonomtricas del
ngulo q
Por lo tanto, la matriz de transformacin para el giro alrededor del eje x ser
LVARO GMEZ RAMOS.
- 97 -

La transformacin inversa es una rotacin 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 rotacin alrededor de este eje no ha modificado la coordenada x. Sabemos

tambin que la longitud total del segmento
no vara. 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 trigonomtricas son
LVARO GMEZ RAMOS.
- 98 -

La matriz de rotacin en este caso ser
La inversa de esta transformacin 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 rotacin ser:
De esta forma ya somos capaces de realizar una rotacin 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 travs de la opcin Calcula robot, que llama a la
funcin:
El primer paso es aadir un falso eslabn al final del ltimo elemento, para
obtener despus un eje final:
Ya tenemos la matriz MR que contiene los datos de los elementos aadidos al

robot. Dicha matriz es simblica porque depende de los variable de longitud de los
LVARO GMEZ RAMOS.
- 99 -

elementos de traslacin, sin embargo, las variables de rotacin no se ven reflejadas en
esta matriz, teniendo que rotar externamente la matriz para posicionarla en su posicin
actual. Para ello utilizamos la funcin rot hacindolo rotar alrededor del eje de
rotacin:
Una vez que tenemos la matriz MR ya con las variables de traslacin y rotacin,
pasamos a ejecutar la funcin MRtoMRvec, que es una funcin que transforma la
matriz con las coordenadas de los eslabones en una matriz con los ejes de las
articulaciones de rotacin y traslacin 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 articulacin y el punto donde se situar el eje, esto lo
realizamos a travs de la funcin MRvectoMRperp, que usando MRvec nos devuelve
la matriz MRperp con los datos obtenidos:
Slo nos falta obtener el vector y y dibujar los ejes, lo cual hacemos a travs
de la funcin Plotejes:
4.14 Obtencin de los ejes de las articulaciones

Este paso lo realizamos a travs de la funcin MRtoMRvec que recorre la
matriz MR en busca de articulaciones es decir, elementos de traslacin o rotacin:
LVARO GMEZ RAMOS.
- 100 -

Luego calcula su eje y un punto de este, si es de rotacin 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 traslacin 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 aadido tambin

calcula un eje segn el punto inicial y final del elemento anterior de MR:
Al final se le aade a cada fila de MRvec un nmero identificando si es rotacin

(1), traslacin (2) o es el eje final (0).
4.15 Obtencin de eje x, eje z y punto de origen

Para ello usamos la funcin 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 comn 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 obtencin de la posicin relativa de dos rectas la realizamos mediante
clculo matemtico.
Los vectores directores y los puntos que utilizamos para escribir las ecuaciones
de las rectas nos permiten calcular la posicin relativa. Consideremos las matrices A y
B formadas con estos elementos:
LVARO GMEZ RAMOS.
- 101 -

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); sern coincidentes
cuando el rango de B tambin sea 1 y paralelas cuando el rango de B sea 2.
Cuando el rango de A es dos y el rango de B tambin dos: las rectas se cortan.
El sistema que resulta al igualar sus ecuaciones paramtricas 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 posicin relativa de los ejes intervinientes:
LVARO GMEZ RAMOS.
- 102 -

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 interseccin 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 nmero de ecuaciones que de incgnitas.

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 solucin nica).
LVARO GMEZ RAMOS.
- 103 -

Para calcular la incgnita xi del sistema de ecuaciones lineales, se sustituye la
columna i de la matriz de coeficientes por los trminos 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 solucin de un sistema de Cramer se obtiene
hallando cada incgnita Xi segn la frmula:
Siendo Ci la matriz resultante de sustituir la columna de la matriz de los

coeficientes correspondiente a la incgnita por la de los trminos independientes.
Una vez que tenemos hallamos el punto de corte de la siguiente manera:
Para ello empleamos el siguiente cdigo:
Slo nos falta el vector x, que ser la perpendicular comn 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 comn:
LVARO GMEZ RAMOS.
- 104 -

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 articulacin actual y de la anterior coinciden o
son paralelas y no es la primera articulacin:
En este caso elegimos los ejes como los de la articulacin anterior:
En el caso que no sea la primera articulacin y no coincida con los ejes de la

anterior:
Suponemos en este caso que slo se contempla que el eje perpendicular al

anterior, con lo cual el nuevo eje x ser el eje z anterior:
Si la articulacin es la primera de todas y vertical suponemos dos casos

posibles, que sea eje de rotacin, entonces el eje x estar en el plano perpendicular al
eje de rotacin:
Si la articulacin es de traslacin, el eje x ser siempre (1 0 0):
Todos los dems casos no contemplados no podrn ser diseados.

Una vez calculados todos los ejes x y el punto donde situarlo, y teniendo
previamente los ejes z de todas las articulaciones, slo nos queda calcularlos de la
ltima articulacin ficticia. El eje x ser la perpendicular comn al eje anterior y el
actual, situndolo en el ltimo punto del robot:
LVARO GMEZ RAMOS.
- 105 -

4.16 Obtencin de eje y, normalizacin y dibujado de ejes

Se realiza a travs de la funcin plotejes que a partir de la matriz MRperp crea
la matriz MRejes que contiene los ejes x, y, z, el punto donde se sitan y el tipo
de articulacin.
El primer paso es crear unos ejes iniciales que sern siempre iguales, y estarn
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 simblica en matriz
numrica con los valores correspondientes a la posicin actual:
LVARO GMEZ RAMOS.
- 106 -

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 posicin, o porque en un momento elegimos que no este visible
y luego queremos que este visible de nuevo. Esta tarea se realiza a travs de la funcin
dibubot que a partir de la matriz MR nos dibuja el robot en la posicin actual:
El primer paso es convertir la matriz MR simblica en una matriz numrica,

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
funcin diferente a la que los dibuja cuando son aadidos, porque ahora ya se dibujan
simplemente conociendo el punto inicial y final del elemento:
LVARO GMEZ RAMOS.
- 107 -

4.18 Dibujar elemento eslabn

Para dibujar el elemento en cuestin utilizamos la funcin 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 proyeccin del eje sobre el plano XY (l1) y la proyeccin a su vez de
esta sobre los ejes x (lx) e y (ly)
lx
ly
LVARO GMEZ RAMOS.
l1
- 108 -

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 momentneamente a una distancia L (anteriormente indicada)
en vertical:
base.
X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
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)
LVARO GMEZ RAMOS.
- 109 -


:
LVARO GMEZ RAMOS.
- 110 -


4.19 Dibujar elemento traslacin

Para dibujar el elemento en cuestin utilizamos la funcin traslacion2 que a
partir del punto inicial y final lo dibuja, los pasos son los mismos que para el elemento
eslabn:


LVARO GMEZ RAMOS.
- 111 -

lx
ly
l1

en vertical:
base.
LVARO GMEZ RAMOS.
- 112 -

X8
Pto. final
X5
X7
X6
X4
Pto. inicial
X1
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)
LVARO GMEZ RAMOS.
- 113 -

:

4.20 Dibujar elemento rotacin

Para dibujar el elemento en cuestin utilizamos la funcin eje2 que a partir
del punto inicial y final lo dibuja:
Lo primero que necesitamos es saber donde se situar el centro del elemento

rotacin, que ser en el punto medio del punto inicial y final:
Lo siguiente que necesitamos es la longitud del elemento, que calculamos

viendo la distancia entre el punto inicial y el final:
LVARO GMEZ RAMOS.
- 114 -



lx
ly
LVARO GMEZ RAMOS.
l1
- 115 -


en vertical:
base.
X8
X5
Pto. final
X7
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)
LVARO GMEZ RAMOS.
- 116 -



dibujamos con el comando patch.
LVARO GMEZ RAMOS.
- 117 -

4.21 Dibujar elemento base

Para dibujar el elemento en cuestin utilizamos la funcin base2 que a partir
del punto inicial y final lo dibuja:


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

:
LVARO GMEZ RAMOS.
- 118 -


4.22 Salvar robot

Una vez calculado el robot tenemos la posibilidad de poder salvarlo para
recuperarlo ms adelante y seguir trabajando con l sin necesidad de volver a disearlo.
Para ello usamos la funcin savebot :
Usamos la funcin 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 funcin 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 cargndolo al espacio de trabajo..
Para ello usamos la funcin loadbot :
LVARO GMEZ RAMOS.
- 119 -

Usamos la funcin 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 funcin load a la que le indicamos el
nombre del archivo a recuperar:
Luego dibujaremos el robot con la funcin dibubot y sus ejes con la funcin
plotejes:
4.24 Calculo de parmetros DH

Para calcular los parmetros de Denavit-Hartenberg utilizaremos la funcin
Par_DH creada para la ocasin:
Lo primero que debemos hacer es calcular los parmetros DH, cuyo clculo lo
resolveremos en funciones independientes:
Ya tenemos calculados los parmetros, ahora los mostraremos en una nueva

pantalla que debemos crear:
LVARO GMEZ RAMOS.
- 120 -

Los datos de DH los representamos en forma matricial, teniendo la matriz
resultante cuatro columnas correspondientes a los parmetros (a, d, , ) y un nmero
de filas igual al nmero de articulaciones+1. Para la representacin grfica dividimos la
pantalla en cuatro partes verticalmente y en n articulaciones+1 en horizontal. Para
saber el nmero de las articulaciones creamos la variable tam:
Ahora muestro los datos de la siguiente manera, los parmetros 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 divisin que va
de 0 a 025. Primero represento el tipo de dato y luego sus valores, por lo que las
divisiones en horizontal son el nmero de elementos ms el nombre del tipo de dato:
Para el resto de datos seguimos el mismo mtodo, pero colocndolos donde

corresponde, es decir incrementando en una las divisiones verticales y con el mismo
procedimiento en horizontal:
a
LVARO GMEZ RAMOS.
- 121 -

LVARO GMEZ RAMOS.
- 122 -

Ya hemos representado todos los valores, ahora los almacenamos en una matriz
llamada P_D_H:
4.25 Calculo de parmetros di

di es la distancia de xi-1 a xi medida a lo largo de zi
LVARO GMEZ RAMOS.
- 123 -

Sea una recta r determinada por un punto P y un vector
exterior a la recta.
, y un punto Q
La distancia entre el punto y la recta es:
La distancia se calcular como la distancia de un punto a una recta, siendo P

el vector xi. Con lo cual el
el punto de los ejes i, Q el punto de los ejes i-1 y
clculo se resolver:
4.26 Calculo de parmetros ai

ai: Es la distancia de zi a zi+1 medida a lo largo de xi
LVARO GMEZ RAMOS.
- 124 -

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

exterior a la recta.
, y un punto Q
La distancia entre el punto y la recta es:
La distancia se calcular como la distancia de un punto a una recta, siendo P

el vector zi. Con lo cual el
el punto de los ejes i, Q el punto de los ejes i-1 y
clculo se resolver:
LVARO GMEZ RAMOS.
- 125 -

4.27 Calculo de parmetros 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 articulacin es de rotacin, indicamos que su valor sea el nombre de

la variable, y si no es una articulacin de rotacin lo calculamos segn la frmula
anterior teniendo en cuenta que V1 es xi-1 y que W1 es xi:
4.28 Calculo de parmetros i

i: Es el ngulo de zi a zi+1 medida sobre xi (utilizando la regla de la mano
derecha).
LVARO GMEZ RAMOS.
- 126 -

Es el ngulo que forman dos rectas es el ngulo que forman sus vectores directores:
Calculamos segn la frmula anterior teniendo en cuenta que V1 es xi y que

W1 es xi+1:
4.28 Cinemtica directa

La cinemtica directa consiste en resolver la posicin cartesiana y la orientacin
del efector final dado el valor de las articulaciones y conociendo la estructura del robot
a travs de la matriz de Denavit-Hartenberg.
Necesitamos obtener el valor del punto final del robot en funcin 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 funcin de las variables.
Esto lo hacemos a travs de la funcin cin_dir creando una ventana nueva y
mostramos la x, y y z desde MR(n,[5 6 7]) respectivamente y siendo n el
nmero de elementos de MR
LVARO GMEZ RAMOS.
- 127 -

LVARO GMEZ RAMOS.
- 128 -

4.29 Cinemtica inversa

El objetivo del problema cinemtico inverso consiste en encontrar los valores
que deben tomar las variables articulares del robot para que su extremo se posicione y
oriente segn una determinada localizacin espacial.
Para resolver el problema cinemtica inverso utilizamos la funcin cin_inv.
Lo primero a realizar es calcular el nmero de articulaciones, el tipo y el orden de ellas,
siendo a el tipo de variables, y Var los nombres:
LVARO GMEZ RAMOS.
- 129 -

Luego debemos pasar a numrica la matriz simblica con los parmetros DH:
Creo la matriz del robot para usarla en el toolbox de robtica:
Creo una matriz DH que contenga a la matriz P_D_H ms el tipo de

elemento:
Creamos la ventana para introducir datos:
LVARO GMEZ RAMOS.
- 130 -

LVARO GMEZ RAMOS.
- 131 -

Creamos el robot segn toolbox de robtica:
Ahora creamos una nueva funcin a la que se llamar cuando se pulse el botn
calcular, y que nos realizar el clculo de la cinemtica inversa. El clculo lo
realizamos a travs de la funcin ikine. IKINE calcula la cinemtica 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 posicin y orientacin dadas por la transformacin T. La
solucin del problema cinemtico inverso no es nica en general, y es posible que para
una misma orientacin y posicin deseadas, se obtengan soluciones distintas en funcin
del vector inicial de variables articulares (Q0) que se le pase a IKINE.
LVARO GMEZ RAMOS.
- 132 -

Es posible usar la funcin para que devuelva las variables articulares
correspondientes a una sola posicin y orientacin, o bien a una trayectoria de
posiciones y orientaciones. Eso depender del formato del parmetro T:
-
Si T es una transformacin homognea, entonces IKINE devuelve un

vector fila(Q) con las variables articulares correspondientes a la posicin
y orientacin indicadas en la matriz T.
Si T es una trayectoria de transformaciones homogneas, entonces el

resultado ser una matriz (Q), en la que la fila i-sima contendr las
variables articulares correspondientes a la transformacin T (:, :, i ). La
estimacin inicial para Q en cada paso se toma de la solucin obtenida
en el paso anterior.
Sea cual sea el formato de T, la estimacin inicial para el vector de variables

articulares ser la dada en el parmetro 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 solucin consiste en especificar un vector (fila o
columna) de pesos (M), cuyos elementos sern 0 para aquellos grados de libertad que
en cartesianas estn 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 nmero de elementos
no nulos debe ser igual al nmero de grados de libertad del robot.
ILIMIT es el nmero mximo de iteraciones que se ejecutarn en busca de una
solucin (un valor usual es 1000).
STOL ser la mxima diferencia que se admitir entre la transformacin
correspondiente a las variables articulares solucin y la transformacin con la posicin
y orientacin especificadas (un valor usual es 1e-6). Dicha diferencia se mide haciendo
uso de la funcin TR2DIFF.
En nuestro caso usaremos la frmula 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 tamao adecuado.
M: [1 1 1 1 1 1].
LVARO GMEZ RAMOS.
- 133 -

Ya tenemos los valores requeridos en q y procedemos a dibujar el robot en un

nuevo figure en la posicin resultante:
4.30 Posicionar robot

Para posicionar el robot en la posicin deseada usamos la funcin env_bot la
cual realiza los clculos necesarios:
Lo primero a realizar es calcular el nmero de articulaciones, el tipo y el orden

de ellas. Para ello recorremos la matriz MR y cuando vemos una articulacin 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 robtica:
LVARO GMEZ RAMOS.
- 134 -

Ahora creamos una nueva ventana para introducir los valores articulares con los
que deseamos posicionar el robot, y tambin daremos la opcin de elegir si lo queremos
representar de forma esquemtica solamente o tambin de forma tridimensional:
LVARO GMEZ RAMOS.
- 135 -

Ahora creo una matriz del robot tpica del toolbox:
Y ahora creo el robot en el formato del toolbox:
Ya tenemos la ventana en la que podemos pulsar el botn para posicionar el

robot, lo cual ejecutar la funcin 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 esquemtica en una nueva ventana:
LVARO GMEZ RAMOS.
- 136 -

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 travs de
la funcin check:
Si la opcin 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

posicin deseada:
Ejecutamos la funcin dibubot que nos dibujar el robot en la posicin final

en la que queramos representarlo:
LVARO GMEZ RAMOS.
- 137 -

4.31 Ayuda
Para llamar a la ayuda se hace a travs de la funcin ayuda, que abre una
pgina web (previamente creada con toda la ayuda) en un explorador propio de matlab:
4.32 Funcin auxiliar: Multiplicar vectorialmente vectores simblicos

Para poder multiplicar vectores simblicos hemos creado nuestra propia funcin
ya que desconocemos la existencia de una funcin para dicho fin:
4.33 Funcin auxiliar: Redondear a cero los valores de las matrices

Se ha creado una funcin que me recorre las matrices y cuando ve un valor
menor que 0.0001 en alguno de sus trminos, lo iguala a cero:
LVARO GMEZ RAMOS.
- 138 -

BLOQUE 3: CONCLUSIONES
LVARO GMEZ RAMOS.
- 139 -

Conclusiones.
El proyecto naci con varios objetivos que segn se fue desarrollando fueron
variando.
Se quera disear un software interactivo en el que se pudieran disear robots en
tres dimensiones para luego poder obtener los ejes del robot y los parmetros de
Denavit-Hartenberg. Esta sera la parte novedosa del software.
Luego existira otra parte del software que consistiese en la integracin de
herramientas ya existentes, para conseguir un paquete que incluya todas las
herramientas que podamos considerar interesantes para el trabajo con robots. En esta
seccin se incluiran la resolucin del problema cinemtica directa e inversa, as como
la dinmica directa e inversa. Tambin se coment la posibilidad de desarrollar ciertos
bloques de simulink para la simulacin, incluso la posibilidad de que alguno de estos
bloques fuera desarrollado en el lenguaje de programacin C para que tuviese una
ejecucin ms rpida.
La mayor parte del peso del proyecto se basa en la parte grfica del software,
sobre todo en lo que se trata de diseo en tres dimensiones. Otra de las cuestiones a las
que ms tiempo se ha dedicado es a la matemtica realizada con simblico. Es decir, lo
difcil fue dibujar el robot segn lo indique el usuario y poder posicionarlo o moverlo
porque ello implica una matemtica compleja de matrices simblicas.
En cuanto a la parte puramente relacionada con la robtica como puede ser la
cinemtica directa e inversa fue relativamente rpida su integracin, porque ya
tenamos unas matrices y variables previamente calculadas que nos facilitan el trabajo.
El proyecto no tena un fin claro, pues no se conocan con suficiente exactitud
las dificultades que iran surgiendo, por lo que una vez desarrollado todo el diseo y
clculo de ejes y parmetros de Denavit-Hartenberg se opt por aadirle tambin la
cinemtica y dejar la dinmica y otras cuestiones para futuras ampliaciones.
Es decir, se trata de un proyecto abierto con muchas posibilidades de
ampliacin, en el cual se han dejado sentadas unas bases ms que suficientes para poder
ser de cierta utilidad en esta su primera versin.
LVARO GMEZ RAMOS.
- 140 -

Desarrollos futuros.
Como hemos dicho este es un proyecto abierto que podr ser ampliado y
modificado en un futuro.
Lo primero a ampliar sera aadir la dinmica directa e inversa y la posibilidad
de aadir bloques propios a simulink para poder simular el robot.
Pero se pueden hacer muchas ms cosas, como la posibilidad de mover el robot
siguiendo trayectorias, o la inclusin de un segundo robot con el que interactuar, o
incluso el diseo de elementos del entorno de trabajo. Quizs para este hecho sea mejor
realizar un cambio en la plataforma de desarrollo y pasar de matlab a algn lenguaje
ms eficiente, sobre todo en la parte grfica.
Las posibilidades de ampliacin son inmensas, y hacen de este software una
herramienta muy interesante para el trabajo con robots. Se podra llegar a obtener un
paquete de herramientas muy completo e integrado, consiguiendo un tipo de software
poco desarrollado hasta el da de hoy.
Sobre todo Sicon-Bot se pretende centrar en la ayuda al estudio de la robtica,
es decir, una herramienta para el estudiante, pero con futuras ampliaciones podra llegar
a ser de cierta utilidad profesional, sobre todo si se consigue compatibilizar con otras
herramientas ya existentes.
LVARO GMEZ RAMOS.
- 141 -

BLOQUE 4: ANEXOS
LVARO GMEZ RAMOS.
- 142 -

ANEXO: CDIGO FUENTE.
function ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin de ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
web index.htm
LVARO GMEZ RAMOS.
- 143 -

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

patch('Vertices',vertices,'Faces',caras,'FaceVertexCData',[0 0
0],'FaceColor','flat');
return
LVARO GMEZ RAMOS.
- 145 -

function [xf yf zf]=base(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja una base, situado en un punto inicial y
%
que nos devuelve un punto final. La base dibujada llevar
%
los ngulos de inclinacin 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
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi,yi,zi,xi-2,yi-2,zi,angx,angz);
z2]=rotar(xi,yi,zi,xi+2,yi-2,zi,angx,angz);
z3]=rotar(xi,yi,zi,xi+2,yi+2,zi,angx,angz);
z4]=rotar(xi,yi,zi,xi-2,yi+2,zi,angx,angz);
z5]=rotar(xi,yi,zi,xi-2,yi-2,zi+l,angx,angz);
z6]=rotar(xi,yi,zi,xi+2,yi-2,zi+l,angx,angz);
z7]=rotar(xi,yi,zi,xi+2,yi+2,zi+l,angx,angz);
z8]=rotar(xi,yi,zi,xi-2,yi+2,zi+l,angx,angz);
[xf yf zf]=rotar(xi,yi,zi,xf,yf,zf,angx,angz);
LVARO GMEZ RAMOS.
- 146 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 147 -

function ai=calc_ai(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
calculamos ai matemticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 148 -

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
LVARO GMEZ RAMOS.
- 149 -

function alphai=calc_alphai(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
calcula alphai matemticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
n=size(MRejes,1);
clear alphai
for i=1:n-1
v=MRejes(i,[1 2 3]);
w=MRejes(i+1,[1 2 3]);
alphai(i)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2
)^2+v(3)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
x=cross(v,w);
if (x(1)~=MRejes(i,4))|(x(2)~=MRejes(i,5))|(x(3)~=MRejes(i,6))
alphai(i)=-alphai(i);
end
end
end
LVARO GMEZ RAMOS.
- 150 -

function di=calc_di(MRejes)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Calculo di matemticamente
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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)];
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
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
a=[v(1) w(1) Q(1)-P(1);v(2)
P(3)];
b=cross(v,w);
mod_QP=0;
mod_v=0;
deta=det(a);
for j=1:3
mod_v=mod_v+b(j)^2;
LVARO GMEZ RAMOS.
%coincidentes
%paralelos
%se cortan
w(2) Q(2)-P(2);v(3) w(3) Q(3)-
- 151 -

end
mod_v=sqrt(mod_v);
di(i-1)=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);
end
%se cruzan
end
end
LVARO GMEZ RAMOS.
- 152 -

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 rotacin 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 sera 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 matemtica necesaria para calcular tethai
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
v=MRejes(i-1,[4 5 6]);
x=cross(v,w);
tethai(i1)=simple(acos((v(1)*w(1)+v(2)*w(2)+v(3)*w(3))/(sqrt(v(1)^2+v(2)^2+v(3
)^2)*(sqrt(w(1)^2+w(2)^2+w(3)^2)))));
mod_x=simple(abs(sqrt(x(1)^2+x(2)^2+x(3)^2)));
LVARO GMEZ RAMOS.
- 153 -

x=[x(1)/mod_x x(2)/mod_x x(3)/mod_x];
tethai(i-1)=-tethai(i-1);
end
end
end
end
LVARO GMEZ RAMOS.
- 154 -

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];
%Aadimos un falso
eslabn al final del robot con longitud 0 para que este termine en
eslabn
n=n+1;
%Contamos este
nuevo tramo aadido
for i=1:n
MRaux(i,:)=MR(i,:); %variable aux de MR
end
clear MR
a=0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
rotamos de forma simblica el robot segn las variables de
rotacin
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
valores simblicos de rotacin
MRvec=simple(MRtoMRvec(MRaux));
%Paso de
matriz con vector del eje de la articulacion
MRperp=simple(MRvectoMRperp(MRvec,MRaux)); %Obtengo
vector z + punto de origen + vector x
MRejes=simple(plotejes(MRperp));
%Obtengo
y los dibujo
MR con los
matriz de robot a
la matriz con
el resto de ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
habilitaciones de nuevas opciones del menu
LVARO GMEZ RAMOS.
- 155 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(mfile_save,'Enable','on')
set(graph_DH,'Enable','on')
set(graph_calcbot,'Enable','off')
set(htras,'Enable','off');
set(heje,'Enable','off');
set(hrot,'Enable','off');
set(hgarra,'Enable','off');
set(hbase,'Enable','off');
set(graph_cindir,'Enable','on');
end
LVARO GMEZ RAMOS.
- 156 -

function cin_dir
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Muestra la cinemtica 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','Cinemtica 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',...
LVARO GMEZ RAMOS.
- 157 -

'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
LVARO GMEZ RAMOS.
- 158 -

function cin_inv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que calcula la cinemtica inversa del robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2
hdata robotd2 hcheck checkdata DH tras rotac xedit yedit zedit
n=size(P_D_H,1);
checkdata=0;
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%N de filas
%Variable de checkbox
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el n de articulaciones, el tipo y el orden
%
de ellas.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=2:size(MRejes,1)
if MRejes(i,13)==1
a(i-1)=0;
cont1=cont1+1;
Var(i-1)=Q2_simb(cont1);
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
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 robtica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
siconbot{i}=link([P_D_Haux(i,1) P_D_Haux(i,2) P_D_Haux(i,3)
P_D_Haux(i,4) a(i)],'modified');
end
LVARO GMEZ RAMOS.
- 159 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Clculo 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','Cinemtica 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',...
LVARO GMEZ RAMOS.
- 160 -

'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)),...
LVARO GMEZ RAMOS.
- 161 -

'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 segn toolbox de robtica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
robotd2=robot(siconbot,'Robot de siconbot');
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = intdata2(varargin) %Calcular cinemtica inversa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que nos calcula la cinemtica 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 posicin 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);
%Posicin inicial
q=ikine(DH,stol,ilimit,TG,vectorq0,[1 1 1 1 1 1]); %Clculo de
cinemtica Inversa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujamos el robot
LVARO GMEZ RAMOS.
- 162 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
plot(robotd2,q)
axis on
AXIS([0 30 0 30 0 40]);
camorbit(165,0)
grid on
%Posicionar robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Mostrar valores de variables articulares
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n-1
set(hdata(i),'String',num2str(q(i)));
end
end
function V3=cross_sym(V1,V2)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
multiplica vectorialmente vectores simblicos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
V3=[det([V1(2) V1(3);V2(2) V2(3)]) det([V1(1) V1(3);V2(1) V2(3)])
det([V1(1) V1(2);V2(1) V2(2)])];
LVARO GMEZ RAMOS.
- 163 -

function dibubot(MR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja el robot equivalente a la matriz MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global n
sp1 num_simb_rot Q1_simb Q1 Q2_simb num_simb_rot2 Q2
subplot(sp1)
MR=simple(MR);
%Seleccionamos subplot principal

%Simplificamos MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos MR con valores reales
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
MR=subs(MR,Q1_simb(j),Q1(j));
variables
end
MR=subs(MR,Q2_simb(j),-Q2(j));
variables
end
%Mr para valores concretos de las
%Mr para valores concretos de las
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibuja el tipo de elemento
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
switch MR(i,1)
case
1
traslac2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
2
eje2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
3
eslabon2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
4
base2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
case
5
garra2(MR(i,2),MR(i,3),MR(i,4),MR(i,5),MR(i,6),MR(i,7),MR(i,8));
end
end
LVARO GMEZ RAMOS.
- 164 -

function [xf yf zf]=eje2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja una eje de rotacin, 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
else
angx=(pi/2-asin((zf-zi)/l));
end;
%ngulo positicvo
%ngulo negativo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el giro sobre el eje z a partir del pto inicial
y
%
final.
LVARO GMEZ RAMOS.
- 165 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
%Proyeccin 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
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1-0.5,angx,angz);
z2]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1-0.5,angx,angz);
z3]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1-0.5,angx,angz);
z4]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1-0.5,angx,angz);
z5]=rotar(xi1,yi1,zi1,xi1,yi1-0.6,zi1+0.5,angx,angz);
z6]=rotar(xi1,yi1,zi1,xi1+0.6,yi1,zi1+0.5,angx,angz);
z7]=rotar(xi1,yi1,zi1,xi1,yi1+0.6,zi1+0.5,angx,angz);
z8]=rotar(xi1,yi1,zi1,xi1-0.6,yi1,zi1+0.5,angx,angz);
[xf yf zf]=rotar(xi1,yi1,zi1,xf,yf,zf,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
[x1 y1 z1]=rot(xi,yi,zi,xf,yf,zf,x1,y1,z1,ange);
end
LVARO GMEZ RAMOS.
- 166 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 167 -

function [xf yf zf]=eje(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja un eje de rotacin, 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
[x2
[x3
[x4
[x5
y1
y2
y3
y4
y5
z1]=rotar(xi,yi,zi,xi,yi-0.6,zi-0.5,angx,angz);
z2]=rotar(xi,yi,zi,xi+0.6,yi,zi-0.5,angx,angz);
z3]=rotar(xi,yi,zi,xi,yi+0.6,zi-0.5,angx,angz);
z4]=rotar(xi,yi,zi,xi-0.6,yi,zi-0.5,angx,angz);
z5]=rotar(xi,yi,zi,xi,yi-0.6,zi+0.5,angx,angz);
LVARO GMEZ RAMOS.
- 168 -

[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
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
z1]=rot(xi,yi,zi-0.5,xf,yf,zf+0.5,x1,y1,z1,ange);
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
LVARO GMEZ RAMOS.
- 169 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 170 -

function env_bot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcion que nos permite posicionar el robot en la posicin
%
indicada, segn el valor de las articulaciones
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MRejes P_D_H Q1_simb Q1 Q2_simb Q2 num_simb_rot num_simb_rot2
hdata robotd2 hcheck checkdata DH
n=size(P_D_H,1);
checkdata=0;
P_D_Haux=P_D_H;
cont1=0;
cont2=0;
%N de filas
%Variable de checkbox
%Contador
%Contador
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calculamos el n de articulaciones, el tipo y el orden
%
de ellas.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if MRejes(i,13)==1
a(i-1)=0;
articular
cont1=cont1+1;
articulares ordenadas
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
end
end
a(cont1+cont2+1)=0;
ficticia al final del robot
%Articulacin de traslacion
%Vector con tipo de variable
%Vector con variables
%Creamos un articulacin
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Damos valor cero a las variables articulares para adaptarlas
al
%
formato del toolbox de robtica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Nueva ventana para introducir los datos de las variables
LVARO GMEZ RAMOS.
- 171 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
'Color',[1 1 1],...
'Resize','on',...
'Name','Introducir datos',...
'Position',[0.15 0.15 0.6 0.6]);
%
Botn
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',...
LVARO GMEZ RAMOS.
- 172 -

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 robtica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que nos dibuja el robot segn 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 articulacin(Ficticia)=0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 173 -

%
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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 174 -

%
Funcin que detecta si se activa ver el robot en 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global checkdata
if checkdata==0
checkdata=1;
else
chackdata=0;
end
end
LVARO GMEZ RAMOS.
- 175 -

function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja un eslabn, situado entre un punto
inicial y
%
%
%
[xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global sp1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
giro
%
de
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
tramos
if (yf-yi)>0.0001
else
end;
%Longitud del
%ngulo positicvo
%ngulo negativo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
xf1=V(1);
yf1=V(2);
zf1=V(3);
end
%Si tramo es vertical

%ngulo y=0
%Longitud en x
%Longitud en y
%Longitud en plano XY
%Proyeccin en eje XY
%ngulo de y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 176 -

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

function [xf yf zf]=eslabon(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja una eslabn, situado en un punto
inicial y
%
que nos devuelve un punto final. El eslabn 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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
LVARO GMEZ RAMOS.
- 178 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
x1=subs(x1,Q1(tras2));
y1=subs(y1,Q1(tras2));
z1=subs(z1,Q1(tras2));
LVARO GMEZ RAMOS.
- 179 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 180 -

function [xf yf zf]=garra2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
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;
LVARO GMEZ RAMOS.
- 181 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
y1
y2
y3
y4
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
LVARO GMEZ RAMOS.
- 182 -

function [xf yf zf]=garra(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
y1
y2
y3
y4
y5
z5]=rotar(xi,yi,zi,xi,yi,zi+l,angx,angz);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if ange
LVARO GMEZ RAMOS.
- 183 -

[x2
[x3
[x4
[x5
end
y2
y3
y4
y5
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 184 -

function loadbot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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
MRaux(i,:)=MR(i,:);
end
clear MR
a=0;
MR=MR_simb;
dibubot(MR)
exacta
MRejes=simple(plotejes(MRperp));
dibujo
%variable aux de MR
%dibuja el robot en esa posicion

%Obtengo el resto de ejes y los
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilitacion de nuevas opciones en el menu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(htras,'Enable','off');
set(heje,'Enable','off');
set(hrot,'Enable','off');
set(hgarra,'Enable','off');
set(hbase,'Enable','off');
set(graph_DH,'Enable','on')
set(graph_calcbot,'Enable','off')
set(graph_cindir,'Enable','on')
return
LVARO GMEZ RAMOS.
- 185 -

function MRvec=MRtoMRvec(MR)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que transforma la matriz con las coordenadas de los
%
eslabones en una matriz con los ejes de las articulaciones de
%
rotacin y traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
num=0;
n=size(MR,1);
MRvec=sym([0 0 0 0 0 0 0]);
for i=1:n
x0=MR(i,2);
y0=MR(i,3);
z0=MR(i,4);
if (MR(i,1)==1)|(MR(i,1)==2)
x1=MR(i,5);
y1=MR(i,6);
z1=MR(i,7);
num=num+1;
%eje rot o tras
if MR(i,1)==2
%si la articulacion es de rotacion el eje se calcula:
MRvec(num,:)=simple([(x0+x1)/2 (y0+y1)/2 (z0+z1)/2 x1-x0
y1-y0 z1-z0 1]);
else
%si la articulacion es de traslaccion el eje se calcula:
MRvec(num,:)=simple([x1 y1 z1 x1-x0 y1-y0 z1-z0 2]);
end
end
if (i==n)&(MR(i,1)~=1)&(MR(i,1)~=2) %si es un ultimo eslabon y no
es una articulacion
x1=MR(i,5);
y1=MR(i,6);
z1=MR(i,7);
num=num+1;
MRvec(num,:)=simple([MR(i-1,2) MR(i-1,3) MR(i-1,4) MR(i-1,5)MR(i-1,2) MR(i-1,6)-MR(i-1,3) MR(i-1,7)-MR(i-1,4) 0]);
end
end
end
LVARO GMEZ RAMOS.
- 186 -

function MRperp=MRvectoMRperp(MRvec,MR)
%
Funcin 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
LVARO GMEZ RAMOS.
- 187 -

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
LVARO GMEZ RAMOS.
- 188 -

end
MRperp(i+1,:)=([MRvec(i+1,[4 5 6]) MR(n2,[5 6 7]) u(i,:)
MRvec(i+1,7)]); %vector z + punto de origen + vector x
end
LVARO GMEZ RAMOS.
- 189 -

function nuevo()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin para dibujar un nuevo robot, cargar uno ya existente o
%
salvar uno nuevo al disco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global hf1 htextox htextoy htextoz htexto htextoe sp1 sp2 sp3 sp4 sp5
sp6 sp7 MR mfile_save n mfile_load mfile_graf mfile_DH n MRperp MRvec
t htextot1 num_simb_rot Q1_simb x y z tras hejes hbot graph_DH MRejes
hrot3d Q1 rotac Q2_simb Q2 num_simb_rot2 graph_calcbot htras heje hrot
hgarra hbase xinf xsup yinf ysup zinf zsup t1 q1 q2 q3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%inicializacion de variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
syms t1 t2 t3 t4 t5 positive
x=0;
y=0;
z=0;
n=0;
tras=1;
rotac=1;
MR=sym([0 0 0 0 0 0 0 0]);
num_simb_rot=0;
num_simb_rot2=0;
Q1_simb=sym('q');
Q1(tras)=0;
Q2_simb=sym('q');
Q2(rotac)=0;
%Variables de traslacion
%
X inicial
%
Y inicial
%
Z inicial
%
N de piezas iniciales
%N de elem de traslacion+1
%N de elem de rotacin+1
%Matriz del robot
%n de elementos de traslacion
%n de elementos de rotacion
%Variables de traslacion
%Valores de traslacion
%Variables de rotacion
%Valores de rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
lim de los ejes del plot principal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xinf=-30;
xsup=30;
yinf=-30;
ysup=30;
zinf=0;
zsup=40;
ejes=[-20 20 -20 20 0 20];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Color de fondo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(hf1,'Color',[.824, .824, .824])
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilita opciones del menu
LVARO GMEZ RAMOS.
- 190 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(mfile_load,'Enable','on')
set(mfile_graf,'Enable','off')
set(mfile_DH,'Enable','off')
set(graph_calcbot,'Enable','on')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creacin 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','Rotacin');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creacin de Plots para la representacin de las figuras
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
sp1=subplot(5,3,[2 3 5 6 8 9 11 12 14 15]);
xlabel('eje x','color','r');
ylabel('eje y','color','g');
sp2=subplot(5,3,1);
sp3=subplot(5,3,4);
sp4=subplot(5,3,7);
sp5=subplot(5,3,10);
sp6=subplot(5,3,13);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Borrado de ejes de los subplots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(sp1);
axis off
subplot(sp2);
axis off
subplot(sp3);
axis off
subplot(sp4);
axis off
subplot(sp5);
axis off
subplot(sp6);
axis off
LVARO GMEZ RAMOS.
- 191 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Dibujo de Figuras de referencia
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% traslacion
vertices1=[x-0.3 y-0.3 z;x+0.3 y-0.3 z;x+0.3 y+0.3 z;x-0.3 y+0.3 z;x0.3 y-0.3 z+7;x+0.3 y-0.3 z+7;x+0.3 y+0.3 z+7;x-0.3 y+0.3 z+7];
caras1=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp2)
patch('Vertices',vertices1,'Faces',caras1,'FaceVertexCData',[.4, .4,
AXIS([-0.5 9.5 -0.5 9.5 0 10]);
camorbit(55,-45)
% Rotacin
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)
% Eslabn
vertices3=[x-0.5 y-0.5 z;x+0.5 y-0.5 z;x+0.5 y+0.5 z;x-0.5 y+0.5 z;x0.5 y-0.5 z+7;x+0.5 y-0.5 z+7;x+0.5 y+0.5 z+7;x-0.5 y+0.5 z+7];
caras3=[1 2 3 4;1 2 6 5;2 3 7 6;3 4 8 7;1 4 8 5;5 6 7 8];
subplot(sp4)
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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 192 -

%
Plot principal para dibujar el robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(sp1)
axis on
AXIS([xinf xsup yinf ysup zinf zsup]);
camorbit(165,0) % Cmara
grid on
camlight right
camlight left
lighting phong
%Lmite de ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Botones, Textos y cuadros editables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
rotate_struct = load([matlabroot,'/toolbox/matlab/icons/rotate.mat']);
rotate_cdata = rotate_struct.cdata;
imgejes = imread('ejes.TIF');
imgbot = imread('robot.TIF');
%boton de rotacin
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 aadir traslacion
htras = uicontrol('style','pushbutton',...
'Callback',@colocar1,...
LVARO GMEZ RAMOS.
- 193 -

'String','Traslacin',...
'Position',[.11 .77 .06
.03],...
'Enable','on',...
%Boton de aadir rotacion
heje = uicontrol('style','pushbutton',...
'String','Eje Rotacin',...
'Position',[.11 .6 .06
.03],...
'Enable','on',...
%Boton de aadir Eslabon
hrot = uicontrol('style','pushbutton',...
'String','Eslabn',...
'Position',[.11 .42 .06
.03],...
'Enable','on',...
%Boton de aadir base
hbase = uicontrol('style','pushbutton',...
'String','Base',...
'Position',[.11 .23 .06
.03],...
'Enable','on',...
%Boton de aadir garra
hgarra = uicontrol('style','pushbutton',...
'String','Garra',...
'Position',[.11 .08 .06
.03],...
'Enable','off',...
%longitud
htexto = uicontrol('Style', 'edit', ...
LVARO GMEZ RAMOS.
- 194 -

'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',...
LVARO GMEZ RAMOS.
- 195 -

%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','Lmite 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',...
LVARO GMEZ RAMOS.
- 196 -

'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','Lmite Rot. -',...
'Position',[.232 .5548 .068
.018],...
'Enable','off',...
%limite rot +
htextor2 = uicontrol('Style', 'edit', ...
'Position',[.31 .55 .05
.03],...
'Enable','off',...
'String','360',...
'String','Lmite Rot. +',...
'Position',[.232 .6048 .068
.018],...
'Enable','off',...
LVARO GMEZ RAMOS.
- 197 -

%nombre var rot
htextor3 = uicontrol('Style', 'edit', ...
'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 funcin del tipo de pieza elegida
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar traslacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = colocar1(varargin)
global MR Q1 n x y z htextoe htexto tras
if n==0
LVARO GMEZ RAMOS.
- 198 -

MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
Q1(tras)=str2num(get(htexto,'String'));
longitud
tras=tras+1;
MR(n,[2 3 4])=sym([x y z]);
[x y z x1 y1 z1]=traslac(x,y,z);
MR(n,1)=1;
MR(n,[5 6 7])=[x y z];
MR(n,8)=str2num(get(htextoe,'String'))*2*pi/360;
end
%Pto inicial
%n de elem
%valor variable
%n elem tras
%pto inicial tramo
%pto final
%eje traslacion
%pto final tramos
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Colocar rotacion
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
global MR Q2 n x y z htextoe rotac
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
Q2(rotac)=str2num(get(htextoe,'String'));
rotacion
rotac=rotac+1;
rotac
[x y z]=eje(x,y,z);
MR(n,1)=2;
end
%pto inicial
%n de elem
%valor variable de
%n de elem de
%nuevo pto final
%eje rotac
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
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;
MR(n,[2 3 4])=[x y z];
[x y z]=eslabon(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=3;
end
%n de elementos
%pto inicial
%pto final
%tipo elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 199 -

if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
MR(n,[2 3 4])=[x y z];
[x y z]=base(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=4;
end
%n de elementos
%pto inicial
%pto final
%tipo de elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
colocar garra
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if n==0
MR=sym([0 0 0 0 0 0 0 0]);
end
n=n+1;
MR(n,[2 3 4])=[x y z];
[x y z]=garra(x,y,z);
MR(n,[5 6 7])=[x y z];
MR(n,1)=5;
end
%n de elemento
%pto inicial
%pto final
%tipo de elem
%rot eje
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
habilitar/deshabilitar rotacion 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
function varargout = rot3d(varargin)
global hrot3d
persistent enable_state;
val = get(hrot3d,'Value');
switch val
case 0
rotate3d off;
case 1
rotate3d on;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Habilitar o Deshabilitar ver ejes
LVARO GMEZ RAMOS.
- 200 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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 opcin 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
LVARO GMEZ RAMOS.
- 201 -

function Par_DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que calcula los parmetros 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','Parmetros D-H',...
'Position',[0.25 0.25 0.5 0.5]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Calcula los parmetros
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
segn el n de parmetros
for i=1:size(di,2)+1
if i==1
hpar_di = uicontrol('Style', 'text', ...
'String','di',...
LVARO GMEZ RAMOS.
- 202 -

'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)),...
'Enable','on',...
end
end
tam=1+(1/(size(di,2)+1));
for i=1:size(tethai,2)+1
if i==1
'String','tethai',...
LVARO GMEZ RAMOS.
- 203 -

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
set(graph_robot,'Enable','on')
set(graph_cininv,'Enable','on');
set(mfile_saveDH,'Enable','on');
end
LVARO GMEZ RAMOS.
- 205 -

function MRejes=plotejes(MRperp)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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
LVARO GMEZ RAMOS.
- 206 -

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 articulacin
MRejes_aux=MRejes;
%sustitucion de valores de traslaccin
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);
LVARO GMEZ RAMOS.
- 207 -

function mat=redondear(mat)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%redondeamos los valores de las matrices
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[n,m]=size(mat);
for i=1:n
for j=1:m
if (mat(i,j)<0.0001)&mat(i,j)>-0.0001
mat(i,j)=0;
end
end
end
end
LVARO GMEZ RAMOS.
- 208 -

function [Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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 rotacin igual a tetha
%
[Xf Yf Zf]=rot(X1,Y1,Z1,X2,Y2,Z2,X,Y,Z,tetha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
vectores de la recta que pasa pos los puntos

A=X2-X1;
B=Y2-Y1;
C=Z2-Z1;
T=[1 0 0 0;0 1 0 0;0 0 1 0;-X1 -Y1 -Z1 1];

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

function [Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que rota un punto Pf con respecto a los ejes x e y
que
%
hacemos pasar por otro punto dado Pi:
%
[Xf Yf Zf]=Rotar(xi,yi,zi,xf,yf,zf,angx,angy)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
A=xf-xi;
B=yf-yi;
C=zf-zi;
Rx=[1 0 0 0;0 cos(angx) sin(angx) 0;0 -sin(angx) cos(angx) 0;0 0 0 1];
Rx=redondear(Rx);
Ry=[cos(angy) 0 sin(angy) 0;0 1 0 0;-sin(angy) 0 cos(angy) 0;0 0 0 1];
Ry=redondear(Ry);
Rz=[cos(angy) sin(angy) 0 0;-sin(angy) cos(angy) 0 0;0 0 1 0;0 0 0 1];
Rz=redondear(Rz);
F=[A B C 1]*Rx*Rz;
Xf=F(1,1)+xi;
Yf=F(1,2)+yi;
Zf=F(1,3)+zi;
LVARO GMEZ RAMOS.
- 210 -

function savebot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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
LVARO GMEZ RAMOS.
- 211 -

function saveDH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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 aadimos el tipo de rotacin. DH es
%
compatible con el toolbox de robtica
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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 articulacin
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
if MRejes(i,13)==1
a(i-1)=0;
cont1=cont1+1;
end
if MRejes(i,13)==2
a(i-1)=1;
cont2=cont2+1;
end
end
a(cont1+cont2+1)=0;
eje
%Es un eje de rotacion

%n de artic de rotacin
%Es un eje de traslacion
%n de artic de traslacion
%Consideramos el eje final como otro
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
LVARO GMEZ RAMOS.
- 212 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Guardamos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[fname,pname] = uiputfile('*.mat','Select File');
save (fname,'P_D_H','DH')
return
LVARO GMEZ RAMOS.
- 213 -

function sicon()
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin 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)
axis off
%mostrar imagen de fondo

% Sin ejes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Creamos los menus y submenus desplegables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
mfile= uimenu('Label','Archivo');
mfile_graf=uimenu(mfile,'Label','Nuevo
Grficamente','Callback','nuevo','Separator','on');
mfile_save=uimenu(mfile,'Label','Salvar
Robot','Callback','Savebot','Separator','on','Enable','off','Accelerat
or','S');
mfile_saveDH=uimenu(mfile,'Label','Salvar DH','Callback','SaveDH','Separator','off','Enable','off');
mfile_load=uimenu(mfile,'Label','Cargar
Robot','Callback','loadbot','Separator','on','Enable','off','Accelerat
or','L');
uimenu(mfile,'Label','Salir','Callback','clear
all','Callback','close all','Separator','on','Accelerator','Q');
mopt= uimenu('Label','Configuracion');
LVARO GMEZ RAMOS.
- 214 -

mopt_pardin=uimenu(mopt,'Label','Parmetros
Dinmicos','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','Parmetros DH','Callback','Par_DH','Enable','off');
graph_robot=uimenu(graph,'Label','Posicionar
Robot','Callback','env_bot','Enable','off');
graph_cindir=uimenu(graph,'Label','Cinemtica
Directa','Callback','cin_dir','Enable','off');
graph_cininv=uimenu(graph,'Label','Cinemtica
Inversa','Callback','cin_inv','Enable','off');
mopt= uimenu('Label','Ayuda');
uimenu(mopt,'Label','Ayuda
Sicon','Callback','Ayuda','Accelerator','H','Enable','on');
uimenu(mopt,'Label','Sobre...','Callback','sobre','Enable','on');
end
LVARO GMEZ RAMOS.
- 215 -

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)
axis off
end
%nos muestra la imagen
LVARO GMEZ RAMOS.
- 216 -

function [xf yf zf]=eslabon2(xi,yi,zi,xf,yf,zf,ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja un eslabn, situado entre un punto
inicial y
%
otro final, se usa en la funcin 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
%ngulo negativo
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
y
%
final.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
angz=0;
else
lx=xf-xi;
ly=yf-yi;
l1=sqrt(lx^2+ly^2);
xf1=V(1);
yf1=V(2);
zf1=V(3);
end
%si tramos es vertical

%ngulo y=0
%longitud en x
%longitud en y
%longitud en plano x-y
%Proyeccion en eje XY
%ngulo de y
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
respecto
%
al punto inicial
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 217 -

xf=xi;
yf=yi;
zf=zi+l;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 218 -

function [xf1 yf1 zf1 xf yf zf]=traslac(xi,yi,zi)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
Funcin que dibuja eje de traslacion, situado en un punto
inicial y
%
que nos devuelve un punto final. El eslabn dibujado
llevar
%
los ngulos de inclinacin 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;
traslacin
a=get(htextot1,'String');
Q1_simb(num_simb_rot)=a;
L=sym(a);
%Aadimos un elemento ms de
%longitud inicial del tramos
%nombre de variable (traslacion)
%matriz de nombre de variables
%Longitud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
radianes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
respecto
%
al punto inicial
LVARO GMEZ RAMOS.
- 219 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
xf=xi;
yf=yi;
zf=zi+L;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
[x1
[x2
[x3
[x4
[x5
[x6
[x7
[x8
y1
y2
y3
y4
y5
y6
y7
y8
[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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
LVARO GMEZ RAMOS.
- 220 -

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
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
LVARO GMEZ RAMOS.
- 221 -

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

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Robots Manipuladores en entorno MATLAB

BLOQUE 1: CONCEPTOS BSICOS................................... 6

CAPTULO: INTRODUCCIN A LA ROBTICA..................................... 7

COMPONENTES BSICOS DE UN SSTEMA ROBTICO. ................. 12

CAPTULO: TRANSFORMACIONES 3D. ................................................. 23

CAPTULO: CINEMTICA. ........................................................................ 31

CAPTULO: TOOLBOX DE ROBTICA................................................... 38

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Cinemtica directa ........................................................................................... 39

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

CAPTULO: CONCEPCIN DEL SOFTWARE SICON-BOT. ............... 42

CAPTULO: GRFICA DE SICON-BOT. .................................................. 46

Creacin de ventana principal y mens desplegables. ................................. 46

CAPTULO: POSIBILIDADES DE SICON-BOT....................................... 56

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

CAPTULO: PROGRAMACIN DE SICON-BOT.................................... 70

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

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

Conclusiones. .................................................................................................. 140

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

ANEXO: CDIGO FUENTE....................................................................... 143

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

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

Integracin: En un mismo software podemos utilizar funciones

En la primera versin del software SICON-BOT el objetivo es crear las bases de

Facilidad: Con el entorno grfico que incorpora, el diseo intuitivo de

Comodidad: Pes realiza funciones como la obtencin automtica de los

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Integracin: Pues incorpora internamente funciones del toolbox de robtica

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

BLOQUE 1: CONCEPTOS BSICOS

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

CAPTULO: INTRODUCCIN A LA ROBTICA.

Ingenio mecnico controlado electrnicamente, capaz de moverse y ejecutar de

El concepto de robot se remonta casi al principio de la civilizacin, donde los

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Bases de la robtica actual

El inicio del desarrollo de la robtica actual puede fijarse en la industria textil

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Al hablar de de robots se piensa en un dispositivo fijo al suelo o al techo, con

Se entiende por robot industrial a un manipulador multifuncional

Una mquina manipuladora automticamente controlada, reprogramable,

Esta definicin es mas completa pues incluye la posibilidad de movilidad y

Un robot es todo aquel dispositivo mecnico con algn grado de libertad,

La definicin mas precisa es la aportada por la Asociacin Francesa de

Manipulador: Mecanismo compuesto generalmente de elementos en serie,

Robot industrial: Manipulador automtico, con servo sistemas de posicin,

LVARO GMEZ RAMOS.

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Lmites de la robtica actual

Michael Knasel, director del Centro de Aplicaciones Robticas de Science

Seis articulaciones de precisin entre la base del robot y el extremo de un

Engelberger aseguraba que los primeros diez puntos ya se haban conseguido a

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de