Memoria Final

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de
Robots Manipuladores en entorno MATLAB

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

5.3 Cinemtica directa........................................................................................... 39
5.4 Cinemtica inversa........................................................................................... 39
BLOQUE 2: DESARROLLO DEL SOFTWARE................ 41
1 CAPTULO: CONCEPCIN DEL SOFTWARE SICON-BOT. ............... 42
1.1 Introduccin. .................................................................................................... 42
1.2 SICON-BOT. .................................................................................................... 43
1.3 Objetivos de la primera versin...................................................................... 43
1.4 Posibles mejoras futuras.................................................................................. 44
1.5 Requisitos mnimos. ......................................................................................... 44
1.6 Inicializacin..................................................................................................... 45
2 CAPTULO: GRFICA DE SICON-BOT. .................................................. 46
2.1 Creacin de ventana principal y mens desplegables. ................................. 46
2.2 Paneles y plots de representacin................................................................... 47
2.3 Eslabones y articulaciones............................................................................... 49
2.4 Botones, textos y cuadros editables. ............................................................... 54
3 CAPTULO: POSIBILIDADES DE SICON-BOT....................................... 56
3.1 Nuevo robot ...................................................................................................... 56
3.2 Calcular robot .................................................................................................. 58
3.3 Salvar Robot ..................................................................................................... 59
3.4 Cargar Robot.................................................................................................... 60
3.5 Parmetros D-H............................................................................................... 62
3.6 Salvar D-H........................................................................................................ 63
3.7 Cinemtica directa........................................................................................... 63
3.8 Cinemtica inversa........................................................................................... 64
3.9 Posicionar Robot .............................................................................................. 66
3.10 Ayuda ................................................................................................................ 67
3.10 Herramientas.................................................................................................... 68
4 CAPTULO: PROGRAMACIN DE SICON-BOT.................................... 70
4.1 Introduccin ..................................................................................................... 70
4.2 Inicializacin de ventana inicial y mens desplegables ................................ 71
4.3 Entorno de creacin de robots........................................................................ 73
4.4 Aadir elemento de traslacin........................................................................ 76
4.5 Aadir elemento de rotacin........................................................................... 78
4.6 Aadir elemento eslabn................................................................................. 80
4.7 Aadir elemento base ...................................................................................... 81
4.8 Posicionar y dibujar por primera vez el elemento traslacin...................... 82
4.9 Posicionar y dibujar por primera vez el elemento rotacin......................... 85
4.10 Posicionar y dibujar por primera vez el elemento eslabn.......................... 88
4.11 Posicionar y dibujar por primera vez el elemento base ............................... 91

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


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.


- Integracin: Pues incorpora internamente funciones del toolbox de robtica
como la cinemtica directa e inversa.


BLOQUE 1: CONCEPTOS BSICOS

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


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.

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

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

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.

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

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

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


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.



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



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



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.



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.


2.8 El controlador

El controlador es el dispositivo que proporciona la inteligencia necesaria para
que acte conforme al operador. Esencialmente consiste en:

- Una memoria para almacenar datos como posiciones prefijadas o el propio
programa
- Un secuenciador que interpreta los datos de programa de la memoria.
- Una CPU que realiza los 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.

2.9 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:
in out
V
R
r
V = .



- Synchro: Es transductor rotacional que convierte desplazamientos angulares
en voltaje de corriente alterna. Se basa en la frmula:

) sin( ) sin( ) ( t V t V
ac m out
=

- 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 ) sin( t V
ac m
, los dos voltajes de estator son:

) sin( ) sin( ) (
2
t V t V
ac m
=
) sin( ) cos( ) (
1
t V t V
ac m
=
- 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


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

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.


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.


3 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 . Por lo tanto, todava son necesarios dos puntos para definir

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) , y
. 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 . Este vector
recibe el nombre de vector normal. Por otro lado, si 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 es un punto del plano, por lo tanto,
esta es la ecuacin de ese plano.


3.3 Cambio de escala
Ahora que sabemos expresar puntos, rectas y planos en tres dimensiones,
consideremos los algoritmos que permiten al usuario modelar objetos tridimensionales.
Empezaremos por generalizar las transformaciones bidimensionales que vimos
en el 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 si empleamos coordenadas homogneas

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


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

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 y su direccin est especificada por el
vector .
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 .


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


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


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


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:

( )
( )
( )
( )
( )
( )
n
n
n
n z
n y
n x
q q q f
q q q f
q q q f
q q q f z
q q q f y
q q q f x
...
...
...
...
...
...
2 1
2 1
2 1
2 1
2 1
2 1
=
=
=
=
=
=

Donde:

n
q
... 1
= 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 articulacin-
eslabn constituye un grado de libertad):


) (
) cos( cos
2 1 2 1 1
2 1 2 1 1

+ + =
+ + =
sen l sen l y
l l x

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:

i
i
A
1
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
i
i
i
n
A A
1
1 0

Para el caso de un robot de 6 ejes, su cadena cinemtica queda representada por
la siguiente matriz de transformacin homognea:

6
5
5
4
4
3
3
2
2
1
1
0
6
0
A A A A A A A T = =

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
1 i
z un ngulo
i

- Traslacin a lo largo de
1 i
z una distancia
i
d
- Traslacin a lo largo de
1
x una distancia
i
a
- Rotacin alrededor del eje
1
x un ngulo
i



) , ( ) 0 , 0 , ( ) , 0 , 0 ( ) , (
1 i i i i
i
i
x T a T d T z T A =

Desarrollando la expresin:

1 0 0 0
0 cos 0
0 cos 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 1 0
0 0 1
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
1 0 0 0
0 1 0 0
0 0 cos
0 0 cos
1
i i
i i
i
i
i i
i i
i
i
sen
sen
a
d
sen
sen
A

Obtenemos la expresin general de DH, donde
i i i i
a d , , , son los parmetros
DH del eslabn i:

1 0 0 0
cos 0
cos cos cos
cos cos cos
1
i i i
i i i i i i i
i i i i i i i
i
i
d sen
sen a sen sen
a sen sen sen
A

Para que la matriz
i
i
A
1
relacione los sistemas coordenados
i
O y
1 i
O 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
0
Z
hasta
1 n
Z . 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
0
O
en cualquier punto del eje
0
Z . Arbitrariamente establecer los ejes
0 0
,Y X
respetando la regla de la mano derecha.

Establecimiento de los sistemas coordenados de las dems articulaciones:

5. Localizar el origen
i
O :

a) En la interseccin del eje
i
Z con la lnea normal comn a la
interseccin de
i
Z y
1 i
Z .
b) En la interseccin de
i
Z y
1 i
Z , si es que
i
Z y
1 i
Z se intersectan.
c) En la articulacin i, si
i
Z y
1 i
Z son paralelos.


6. Establecer
i
X :

a) A lo largo de la lnea normal comn entre los ejes
i
Z y
1 i
Z que pasa
por
i
O .
b) En la direccin normal al plano formado por
i
Z y
1 i
Z , si es que estos
dos ejes se intersectan.

7. Establecer
i
Y 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
n
Z a lo largo de la direccin
1 n
Z y establecer
el origen
n
O de la manera que ms convenga a lo largo de
n
Z , preferente en el
centro de la pinza o la punta de cualquier herramienta que el robot tenga
montada.

9. Establecer
n
X y
n
Y de acuerdo a la regla de la mano derecha. Si la
herramienta es una pinza, es comn establecer el eje
n
Y entre los dedos de la
pinza y
n
X ser ortonormal a
n
Z y
n
Y .

Obtener las Matrices de Transformacin Homogneas

10. Crear una tabla con los parmetros D-H de los eslabones:

Eslabn i
i

i
d
i
a
i



Donde:

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

i
d = Es la distancia a lo largo del eje
1 i
Z desde el origen
1 i
O hasta la
interseccin del eje
i
X con el eje
1 i
Z . Este es un parmetro variable en
articulaciones prismticas.

i
a = Para articulaciones rotatorias: es la distancia a lo largo del eje
i
X
desde el origen
i
O hasta la interseccin del eje
i
X con el eje
1 i
Z .

i
= Es el ngulo formado por los ejes
i
Z y
1 i
Z medido en un plano
perpendicular al eje
i
X utilizando la regla de la mano derecha.

11. Realizar la matriz D-H de transformacin homognea
i
i
A
1
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
i
i
i
n
A A T
1
1 0


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:

( ) , , , , , z y x f q
k k
=
n K ... 1 =
Donde:

k
q
... 1
= Son las variables de las articulaciones. Para articulaciones
revolutas las variables son ngulos. Para articulaciones prismticas las
variables son distancias.

z y x , , = 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.

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


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

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.


BLOQUE 2: DESARROLLO DEL
SOFTWARE

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


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

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

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


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:

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

1.4 Posibles mejoras futuras.

Existen una gran cantidad de posibles mejoras a aadir al programa, aqu
pondremos algunas que seran de gran utilidad:

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

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

- 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


2 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



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:



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



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

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

Elementos
posibles del
robot
Parmetros
del robot
Plot principal
Traslacin
Rotacin
Eslabn
Base


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 2 3
4 5 6
7 8 9
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.

1 2 3
4 5 6
7 8 9
10 11 12
13 14 15


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.


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:

x y
z

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.


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.

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:

rotacin
traslacin
eslabn
base


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.

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


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.


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.



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.


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.



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.


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


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 D-
H 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


Para poder ver la cinemtica directa del robot debemos ir al men desplegable
Ejecucin y seleccionar la opcin Cinemtica directa.


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.


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.



Una vez pulsemos el botn calcular nos aparecer otra nueva pantalla con un
diseo esquemtico del robot en la posicin deseada.

Ejemplo:

Posicin
deseada
Nombre
de las
variables
Valor
calculado
de las
variables


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.



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.



3.10 Herramientas

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


Existen tres herramientas disponibles:

- Rotar robot:

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

- Ver/Ocultar ejes:

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

- Ver/Ocultar robot:

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

4 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


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 Sicon-
Bot.
- : 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.


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
condicin. Tambin es interesante la propiedad , que hace que la

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:



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:


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
iluminacin al plot principal: , consiguiendo que entre un rayo de luz
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)

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


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:


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:



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:



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:


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:





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:


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.

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.

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


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:



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




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

base.

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)

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


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.


:





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:




base.


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


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


del siguiente elemento:


:





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:




base.


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


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




:





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:



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:



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

un punto de esta recta es y su direccin est especificada por el vector
. 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 .


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


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


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

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:


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:


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:


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

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


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:


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:


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:


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)

l1
lx
ly


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.


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

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





:




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:





en vertical:

base.

l1
lx
ly



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




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


:


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:




l1
lx
ly


en vertical:

base.


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





dibujamos con el comando patch.


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



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 :


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:


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 d





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

4.25 Calculo de parmetros d
i

di es la distancia de xi-1 a xi medida a lo largo de zi


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

La distancia entre el punto y la recta es:

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

4.26 Calculo de parmetros a
i

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



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

La distancia entre el punto y la recta es:

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


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:

i

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



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

Calculamos segn la frmula anterior teniendo en cuenta que V1 es xi y que
W1 es xi+1:


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






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:


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:





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.

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



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:



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:



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:


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:


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:



BLOQUE 3: CONCLUSIONES

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


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


BLOQUE 4: ANEXOS

1 ANEXO: CDIGO FUENTE.

f unct i on ayuda

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n de ayuda
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

web i ndex. ht m

f unct i on [ xf yf zf ] =base2( xi , yi , zi , xf , yf , zf , ange)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una base, si t uado ent r e un punt o
i ni ci al y
% ot r o f i nal . La base di buj ada l l evar
% el ngul o de gi r o sobr e el ej e punt o i ni ci al - f i nal
i ndi cados por el usuar i o:
% [ xf yf zf ] =base2( xi , yi , zi , xf , yf , zf , ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos dat os de al t ur a de l a base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

l =sqr t ( abs( xf - xi ) ^2+abs( yf - yi ) ^2+abs( zf - zi ) ^2) ; %l ongi t ud del t r amo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Si t uamos el punt o f i nal a una di st anci a L en ver t i cal
r espect o
% al punt o i ni ci al
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +l ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rot amos l a f i gur a con r espect o al ej e Xi - Xf
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Di buj amos el el ement o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

ver t i ces=[ x1 y1 z1; x2 y2 z2; x3 y3 z3; x4 y4 z4; x5 y5 z5; x6 y6 z6; x7 y7
z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)

pat ch( ' Ver t i ces' , ver t i ces, ' Faces' , car as, ' FaceVer t exCDat a' , [ 0 0
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on [ xf yf zf ] =base( xi , yi , zi )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una base, si t uado en un punt o i ni ci al y
% que nos devuel ve un punt o f i nal . La base di buj ada l l evar
% l os ngul os de i ncl i naci n i ndi cados por el usuar i o:
% [ xf yf zf ] =base( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal hf 1 ht ext ox ht ext oz ht ext oz ht ext o ht ext oe sp1 sp2 sp3 sp4 sp5

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos val or de al t ur a de l a base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

l =st r 2num( get ( ht ext o, ' St r i ng' ) ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cogemos l os val or es de l os ngul os y l os pasamos a
r adi anes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

angx=- st r 2num( get ( ht ext ox, ' St r i ng' ) ) *2*pi / 360;
angz=- st r 2num( get ( ht ext oz, ' St r i ng' ) ) *2*pi / 360;
ange=st r 2num( get ( ht ext oe, ' St r i ng' ) ) *2*pi / 360;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +l ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rot amos l a f i gur a con l os val or es adecuados
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

[ x1 y1 z1] =r ot ar ( xi , yi , zi , xi - 2, yi - 2, zi , angx, angz) ;
[ x2 y2 z2] =r ot ar ( xi , yi , zi , xi +2, yi - 2, zi , angx, angz) ;
[ x3 y3 z3] =r ot ar ( xi , yi , zi , xi +2, yi +2, zi , angx, angz) ;
[ x4 y4 z4] =r ot ar ( xi , yi , zi , xi - 2, yi +2, zi , angx, angz) ;
[ x5 y5 z5] =r ot ar ( xi , yi , zi , xi - 2, yi - 2, zi +l , angx, angz) ;
[ x6 y6 z6] =r ot ar ( xi , yi , zi , xi +2, yi - 2, zi +l , angx, angz) ;
[ x7 y7 z7] =r ot ar ( xi , yi , zi , xi +2, yi +2, zi +l , angx, angz) ;
[ x8 y8 z8] =r ot ar ( xi , yi , zi , xi - 2, yi +2, zi +l , angx, angz) ;

[ xf yf zf ] =r ot ar ( xi , yi , zi , xf , yf , zf , angx, angz) ;


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

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

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on ai =cal c_ai ( MRej es)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% cal cul amos ai mat emt i cament e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

n=si ze( MRej es, 1) ;
f or i =1: n- 1
P=MRej es( i , [ 10 11 12] ) ;
v=MRej es( i , [ 1 2 3] ) ;
Q=MRej es( i +1, [ 10 11 12] ) ;
w=MRej es( i +1, [ 1 2 3] ) ;

q4=si mpl e( 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=sqr t ( 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) ] ;

i f ( r ank( A) ==1) &( r ank( B) ==1)
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
mod_QP=mod_QP+a( j ) ^2;
end
f or j =1: 3
mod_v=mod_v+v( j ) ^2;
end
mod_QP=sqr t ( mod_QP) ;
mod_v=sqr t ( mod_v) ;
ai ( i ) =mod_QP/ mod_v;
end
i f ( r ank( A) ==1) &( r ank( B) ==2)
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
end
f or j =1: 3
end
end
i f ( r ank( A) ==2) &( r ank( B) ==2)
a=[ v( 1) w( 1) Q( 1) - P( 1) ; v( 2) w( 2) Q( 2) - P( 2) ; v( 3) w( 3) Q( 3) -
P( 3) ] ;
b=cr oss( v, w) ;
mod_QP=0;
mod_v=0;
det a=det ( a) ;
f or j =1: 3
mod_v=mod_v+b( j ) ^2;
end

ai ( i ) =det a/ mod_v;
end
i f ( r ank( A) ==2) &( r ank( B) ==3)
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
end
f or j =1: 3
end
end
end
end

f unct i on al phai =cal c_al phai ( MRej es)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% cal cul a al phai mat emt i cament e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

cl ear al phai
f or i =1: n- 1
v=MRej es( i , [ 1 2 3] ) ;
w=MRej es( i +1, [ 1 2 3] ) ;

al phai ( i ) =si mpl e( acos( ( v( 1) *w( 1) +v( 2) *w( 2) +v( 3) *w( 3) ) / ( sqr t ( v( 1) ^2+v( 2
) ^2+v( 3) ^2) *( sqr t ( w( 1) ^2+w( 2) ^2+w( 3) ^2) ) ) ) ) ;
x=cr oss( v, w) ;
i f ( x( 1) ~=MRej es( i , 4) ) | ( x( 2) ~=MRej es( i , 5) ) | ( x( 3) ~=MRej es( i , 6) )
al phai ( i ) =- al phai ( i ) ;
end

end
end

f unct i on di =cal c_di ( MRej es)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Cal cul o di mat emt i cament e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

cl ear di
f or i =2: n
P=MRej es( i - 1, [ 10 11 12] ) ;
v=MRej es( i - 1, [ 4 5 6] ) ;
Q=MRej es( i , [ 10 11 12] ) ;
w=MRej es( i , [ 4 5 6] ) ;

q4=si mpl e( 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=sqr t ( 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) ] ;

i f ( r ank( A) ==1) &( r ank( B) ==1) %coi nci dent es
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
end
f or j =1: 3
end
di ( i - 1) =mod_QP/ mod_v;
end
i f ( r ank( A) ==1) &( r ank( B) ==2) %par al el os
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
end
f or j =1: 3
end
end
i f ( r ank( A) ==2) &( r ank( B) ==2) %se cor t an
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=cr oss( v, w) ;
mod_QP=0;
mod_v=0;
det a=det ( a) ;
f or j =1: 3
mod_v=mod_v+b( j ) ^2;

end
di ( i - 1) =det a/ mod_v;
end
i f ( r ank( A) ==2) &( r ank( B) ==3) %se cr uzan
a=cr oss( ( Q- P) , v) ;
mod_QP=0;
mod_v=0;
f or j =1: 3
end
f or j =1: 3
end
end
end
end

f unct i on t et hai =cal c_t et hai ( MRej es)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos el val or de t et hai
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal q1 q2 q3 t 1 Q2_si mb Q2_si mb2

syms Q2_si mb2
syms x posi t i ve
n=si ze( MRej es, 1) ; %N de ej es del r obot
cl ear t et hai
a=0;
f or i =2: n
i f MRej es( i , 13) ==1 %Si es de r ot aci n hago cambi o de
var i abl e par a que el val or de t et hai sea sol o el nombr e de l a var i abl e
cor r espondi ent e
a=a+1;
t et hai ( i - 1) = Q2_si mb( a) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul o cual ser a el val or de t et hai si n el cambi o de
var i abl e,
% por si puedo necesi t ar l o mas adel ant e, y l o guar do en
Q2_si mb2( a)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

v=MRej es( i - 1, [ 4 5 6] ) ;
w=MRej es( i , [ 4 5 6] ) ;

Q2_si mb2( a) =si mpl e( acos( ( v( 1) *w( 1) +v( 2) *w( 2) +v( 3) *w( 3) ) / ( sqr t ( v( 1) ^2+v
( 2) ^2+v( 3) ^2) *( sqr t ( w( 1) ^2+w( 2) ^2+w( 3) ^2) ) ) ) ) ;
x=cr oss( v, w) ;
mod_x=si mpl e( abs( sqr t ( x( 1) ^2+x( 2) ^2+x( 3) ^2) ) ) ;
x=[ x( 1) / mod_x x( 2) / mod_x x( 3) / mod_x] ;
Q2_si mb2( a) =2*pi - Q2_si mb2( a) ;
end
Q2_si mb2_aux( a) =subs( Q2_si mb2( a) , Q2_si mb( a) , 0) ;
Q2_si mb2( a) =Q2_si mb2( a) - Q2_si mb2_aux( a) ;

el se

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Resuel vo l a mat emt i ca necesar i a par a cal cul ar t et hai
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

v=MRej es( i - 1, [ 4 5 6] ) ;
w=MRej es( i , [ 4 5 6] ) ;
x=cr oss( v, w) ;
t et hai ( i -
1) =si mpl e( acos( ( v( 1) *w( 1) +v( 2) *w( 2) +v( 3) *w( 3) ) / ( sqr t ( v( 1) ^2+v( 2) ^2+v( 3
) ^2) *( sqr t ( w( 1) ^2+w( 2) ^2+w( 3) ^2) ) ) ) ) ;
mod_x=si mpl e( abs( sqr t ( x( 1) ^2+x( 2) ^2+x( 3) ^2) ) ) ;

x=[ x( 1) / mod_x x( 2) / mod_x x( 3) / mod_x] ;
t et hai ( i - 1) =- t et hai ( i - 1) ;
end
end
end
end

f unct i on cal cbot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Cal cul o l os ej es del r obot a par t i r de l a mat r i z MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MR n Q2_si mb MRper p MRvec mf i l e_save gr aph_DH gr aph_cal cbot
ht r as hej e hr ot hgar r a hbase MRej es t 1 q1 q2 q3 MR_si mb gr aph_ci ndi r
gr aph_ci ni nv

MR( n+1, : ) = [ 3 MR( n, [ 5 6 7] ) MR( n, [ 5 6 7] ) 0] ; %Aadi mos un f al so
esl abn al f i nal del r obot con l ongi t ud 0 par a que est e t er mi ne en
esl abn
n=n+1; %Cont amos est e
nuevo t r amo aadi do

f or i =1: n
MRaux( i , : ) =MR( i , : ) ; %var i abl e aux de MR
end
cl ear MR
a=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% r ot amos de f or ma si mbl i ca el r obot segn l as var i abl es de
r ot aci n
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: n
i f MRaux( i , 1) ==2
a=a+1;
f or j =i +1: n
[ MRaux( j , 2) MRaux( j , 3)
MRaux( j , 4) ] =r ot ( 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_si mb( a) ) ;
MRaux=si mpl e( MRaux) ;
end
end
end

MR=MRaux;
MR_si mb=MRaux; %Cr eamos MR con l os
val or es si mbl i cos de r ot aci n
MRvec=si mpl e( MRt oMRvec( MRaux) ) ; %Paso de mat r i z de r obot a
mat r i z con vect or del ej e de l a ar t i cul aci on
MRper p=si mpl e( MRvect oMRper p( MRvec, MRaux) ) ; %Obt engo l a mat r i z con
vect or z + punt o de or i gen + vect or x
MRej es=si mpl e( pl ot ej es( MRper p) ) ; %Obt engo el r est o de ej es
y l os di buj o

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% habi l i t aci ones de nuevas opci ones del menu

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

set ( mf i l e_save, ' Enabl e' , ' on' )
set ( gr aph_DH, ' Enabl e' , ' on' )
set ( gr aph_cal cbot , ' Enabl e' , ' of f ' )
set ( ht r as, ' Enabl e' , ' of f ' ) ;
set ( hej e, ' Enabl e' , ' of f ' ) ;
set ( hr ot , ' Enabl e' , ' of f ' ) ;
set ( hgar r a, ' Enabl e' , ' of f ' ) ;
set ( hbase, ' Enabl e' , ' of f ' ) ;
set ( gr aph_ci ndi r , ' Enabl e' , ' on' ) ;

end

f unct i on ci n_di r

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Muest r a l a ci nemt i ca di r ect a que se obt i ene di r ect ament e del punt o
f i nal
%del l t i mo el ement o del r obot , que est al macenado en MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MR_si mb n P_D_H DH r obot d2 Q2_si mb Q1_si mb

f i gur e( ' Vi si bl e' , ' on' , . . .
' Uni t s' , ' nor mal i zed' , . . .
' Render er ' , ' opengl ' , . . .
' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' on' , . . .
' Name' , ' Ci nemt i ca I nver sa' , . . .
' Number Ti t l e' , ' of f ' , . . .
' DockCont r ol s' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' I nt er r upt i bl e' , ' of f ' , . . .
' BusyAct i on' , ' cancel ' , . . .
' Posi t i on' , [ 0. 15 0. 15 0. 6 0. 6] ) ;
ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' Cal l back' , @l abel , . . .
' St r i ng' , char ( MR_si mb( n, 5) ) , . . .
' backgr oundcol or ' , [ 1 1 1] , . . .
' Posi t i on' , [ . 4 0. 7 . 5
. 05] , . . .
' Enabl e' , ' on' , . . .
' BusyAct i on' , ' cancel ' ) ;
' Posi t i on' , [ . 4 0. 5 . 5
. 05] , . . .
' Enabl e' , ' on' , . . .
' Posi t i on' , [ . 4 0. 3 . 5
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' x=' , . . .
' For egr oundCol or ' , ' b' , . . .
' Font Wei ght ' , ' bol d' , . . .

' Posi t i on' , [ . 2 0. 7 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' y=' , . . .
' Posi t i on' , [ . 2 0. 5 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' z=' , . . .
' Posi t i on' , [ . 2 0. 3 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .

end

f unct i on ci n_i nv
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que cal cul a l a ci nemt i ca i nver sa del r obot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MRej es P_D_H Q1_si mb Q1 Q2_si mb Q2 num_si mb_r ot num_si mb_r ot 2
hdat a r obot d2 hcheck checkdat a DH t r as r ot ac xedi t yedi t zedi t

n=si ze( P_D_H, 1) ; %N de f i l as
checkdat a=0; %Var i abl e de checkbox
P_D_Haux=P_D_H;
cont 1=0;
cont 2=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos el n de ar t i cul aci ones, el t i po y el or den
% de el l as.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =2: si ze( MRej es, 1)
i f MRej es( i , 13) ==1
a( i - 1) =0;
cont 1=cont 1+1;
Var ( i - 1) =Q2_si mb( cont 1) ;
end
i f MRej es( i , 13) ==2
a( i - 1) =1;
cont 2=cont 2+1;
Var ( i - 1) =Q1_si mb( cont 2) ;
end
end
a( cont 1+cont 2+1) =0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nueva vent ana par a i nt r oduci r l os dat os de l as var i abl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or j =1: num_si mb_r ot
P_D_Haux=subs( P_D_Haux, Q1_si mb( j ) , 0) ;
end
f or j =1: num_si mb_r ot 2
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cr eo l a mat r i z del r obot par a usar l a en el t ool box de r obt i ca
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: n
si conbot {i }=l i nk( [ P_D_Haux( i , 1) P_D_Haux( i , 2) P_D_Haux( i , 3)
P_D_Haux( i , 4) a( i ) ] , ' modi f i ed' ) ;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cl cul o de DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

DH=P_D_Haux;
f or i =1: ( si ze( DH, 1) )
DH( i , 5) =a( i ) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% vent ana par a i nt r oduci r dat os
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' on' , . . .
' Name' , ' Ci nemt i ca I nver sa' , . . .
' Number Ti t l e' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' Posi t i on' , [ 0. 15 0. 15 0. 6 0. 6] ) ;
hi nt dat a = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' Cal l back' , @i nt dat a2, . . .
' backgr oundcol or ' , [ . 724, . 724, . 724] , . . .
' St r i ng' , ' Cal cul ar ' , . . .
' Posi t i on' , [ . 8 . 5 . 1 . 05] , . . .
' Enabl e' , ' on' , . . .

' St r i ng' , ' x =' , . . .
' Posi t i on' , [ . 1 0. 7 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .

%i nt r oduci r x
xedi t =ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 2 0. 7 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .


' St r i ng' , ' y =' , . . .
' Posi t i on' , [ . 1 0. 5 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .

%I nt r oduci r y
yedi t =ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 2 0. 5 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .

' St r i ng' , ' z =' , . . .
' Posi t i on' , [ . 1 0. 3 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .

%I nt r oduci r z
zedi t =ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 2 0. 3 . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nos di buj a l as var i abl es y su val or y
% l as col oca en l a vent ana dependi endo del n de var i abl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

b=1/ ( n- 1) ;
c=1+b/ 2;

f or i =1: n- 1
' St r i ng' , char ( Var ( 1, i ) ) , . . .

' Posi t i on' , [ . 4 c- b . 1
. 05] , . . .
' Enabl e' , ' on' , . . .

hdat a( i ) = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 6 c- b . 1
. 05] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' 0' , . . .
c=c- b;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% cr eamos el r obot segn t ool box de r obt i ca
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

r obot d2=r obot ( si conbot , ' Robot de si conbot ' ) ;
end

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

f unct i on var ar gout = i nt dat a2( var ar gi n) %Cal cul ar ci nemt i ca i nver sa

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que nos cal cul a l a ci nemt i ca i nver sa
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal hdat a r obot d2 DH xedi t yedi t zedi t t r as r ot ac P_D_H

n=si ze( P_D_H, 1) ;
st ol =1e- 3;
i l i mi t =1000;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Mat r i z de posi ci n f i nal deseada necesar i a par a " i ki ne"
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

TG = [ 1 0 0 st r 2num( get ( xedi t , ' St r i ng' ) ) ; 0 1 0
st r 2num( get ( yedi t , ' St r i ng' ) ) ; 0 0 1 st r 2num( get ( zedi t , ' St r i ng' ) ) ; 0 0 0
1] ;
vect or q0=zer os( 1, t r as+r ot ac- 1) ; %Posi ci n i ni ci al
q=i ki ne( DH, st ol , i l i mi t , TG, vect or q0, [ 1 1 1 1 1 1] ) ; %Cl cul o de
ci nemt i ca I nver sa

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Di buj amos el r obot

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

f i gur e
pl ot ( r obot d2, q) %Posi ci onar r obot
axi s on
AXI S( [ 0 30 0 30 0 40] ) ;
camor bi t ( 165, 0)
gr i d on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Most r ar val or es de var i abl es ar t i cul ar es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
f or i =1: n- 1
set ( hdat a( i ) , ' St r i ng' , num2st r ( q( i ) ) ) ;
end

end
f unct i on V3=cr oss_sym( V1, V2)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% mul t i pl i ca vect or i al ment e vect or es si mbl i cos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

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

f unct i on di bubot ( MR)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a el r obot equi val ent e a l a mat r i z MR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal n sp1 num_si mb_r ot Q1_si mb Q1 Q2_si mb num_si mb_r ot 2 Q2

subpl ot ( sp1) %Sel ecci onamos subpl ot pr i nci pal
MR=si mpl e( MR) ; %Si mpl i f i camos MR

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos MR con val or es r eal es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

MR=subs( MR, Q1_si mb( j ) , Q1( j ) ) ; %Mr par a val or es concr et os de l as
var i abl es
end
MR=subs( MR, Q2_si mb( j ) , - Q2( j ) ) ; %Mr par a val or es concr et os de l as
var i abl es
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Di buj a el t i po de el ement o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: n
swi t ch MR( i , 1)
case 1

t r asl ac2( MR( i , 2) , MR( i , 3) , MR( i , 4) , MR( i , 5) , MR( i , 6) , MR( i , 7) , MR( i , 8) ) ;
case 2

ej e2( MR( i , 2) , MR( i , 3) , MR( i , 4) , MR( i , 5) , MR( i , 6) , MR( i , 7) , MR( i , 8) ) ;
case 3

esl abon2( 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

gar r a2( MR( i , 2) , MR( i , 3) , MR( i , 4) , MR( i , 5) , MR( i , 6) , MR( i , 7) , MR( i , 8) ) ;
end
end


f unct i on [ xf yf zf ] =ej e2( xi , yi , zi , xf , yf , zf , ange)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una ej e de r ot aci n, si t uado ent r e un
punt o i ni ci al y
% [ xf yf zf ] =base( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul o pt o dnd se cent r ar el ej e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xi 1=( xi +xf ) / 2;
yi 1=( yi +yf ) / 2;
zi 1=( zi +zf ) / 2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos l a l ongi t ud del t r amo par a obt ener el ngul o de
gi r o
% sobr e el ej e x a par t i r de zf y zi . Dependi endo del si gno
de
% ( yf - yi ) el gi r o ser en un sent i do o en ot r o.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

l =sqr t ( abs( xf - xi ) ^2+abs( yf - yi ) ^2+abs( zf - zi ) ^2) ;
%i f ( xi ==xf ) &( yi ==yf )
% angz=0;
% angx=0;
%el se
% angx=- ( pi / 2- asi n( ( zf - zi ) / l ) ) ;
% l x=xf - xi ;
% l y=yf - yi ;
% l 1=sqr t ( l x^2+l y^2) ;
% V=[ l x/ l 1 l y/ l 1 0] *l ;
% xf 1=V( 1) ;
% yf 1=V( 2) ;
% zf 1=V( 3) ;
% angz=- ( pi / 2- acos( xf 1/ l ) ) ;
%end
i f ( yf - yi ) >0. 0001 %ngul o posi t i cvo
angx=- ( pi / 2- asi n( ( zf - zi ) / l ) ) ;
el se %ngul o negat i vo
angx=( pi / 2- asi n( ( zf - zi ) / l ) ) ;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos el gi r o sobr e el ej e z a par t i r del pt o i ni ci al
y
% f i nal .

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

i f ( xi ==xf ) &( yi ==yf ) %Si t r amo es ver t i cal
angz=0; %ngul o y=0
el se
l x=xf - xi ; %Longi t ud en x
l y=yf - yi ; %Longi t ud en y
l 1=sqr t ( l x^2+l y^2) ; %Longi t ud en pl ano XY
V=[ l x/ l 1 l y/ l 1 0] *l ; %Pr oyecci n en ej e XY
xf 1=V( 1) ;
yf 1=V( 2) ;
zf 1=V( 3) ;
angz=( pi / 2- acos( xf 1/ l ) ) ; %ngul o de y
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +1;

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

[ x1 y1 z1] =r ot ar ( xi 1, yi 1, zi 1, xi 1, yi 1- 0. 6, zi 1- 0. 5, angx, angz) ;
[ x2 y2 z2] =r ot ar ( xi 1, yi 1, zi 1, xi 1+0. 6, yi 1, zi 1- 0. 5, angx, angz) ;
[ x3 y3 z3] =r ot ar ( xi 1, yi 1, zi 1, xi 1, yi 1+0. 6, zi 1- 0. 5, angx, angz) ;
[ x4 y4 z4] =r ot ar ( xi 1, yi 1, zi 1, xi 1- 0. 6, yi 1, zi 1- 0. 5, angx, angz) ;
[ x5 y5 z5] =r ot ar ( xi 1, yi 1, zi 1, xi 1, yi 1- 0. 6, zi 1+0. 5, angx, angz) ;
[ x6 y6 z6] =r ot ar ( xi 1, yi 1, zi 1, xi 1+0. 6, yi 1, zi 1+0. 5, angx, angz) ;
[ x7 y7 z7] =r ot ar ( xi 1, yi 1, zi 1, xi 1, yi 1+0. 6, zi 1+0. 5, angx, angz) ;
[ x8 y8 z8] =r ot ar ( xi 1, yi 1, zi 1, xi 1- 0. 6, yi 1, zi 1+0. 5, angx, angz) ;

[ xf yf zf ] =r ot ar ( xi 1, yi 1, zi 1, xf , yf , zf , angx, angz) ;

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

i f ange
end

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on [ xf yf zf ] =ej e( xi , yi , zi )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a un ej e de r ot aci n, si t uado en un punt o
i ni ci al y
% que nos devuel ve un punt o f i nal . El ej e di buj ado l l evar
% [ xf yf zf ] =ej e( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

n MR ht ext ot 1 num_si mb_r ot 2 Q2_si mb Q2 r ot ac num_si mb_r ot Q1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos dat os de var i abl e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

num_si mb_r ot 2=num_si mb_r ot 2+1;
a=get ( ht ext ot 1, ' St r i ng' ) ;
Q2_si mb( num_si mb_r ot 2) =a;
L=sym( a) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r adi anes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi ;

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

[ x1 y1 z1] =r ot ar ( xi , yi , zi , xi , yi - 0. 6, zi - 0. 5, angx, angz) ;
[ x2 y2 z2] =r ot ar ( xi , yi , zi , xi +0. 6, yi , zi - 0. 5, angx, angz) ;
[ x3 y3 z3] =r ot ar ( xi , yi , zi , xi , yi +0. 6, zi - 0. 5, angx, angz) ;
[ x4 y4 z4] =r ot ar ( xi , yi , zi , xi - 0. 6, yi , zi - 0. 5, angx, angz) ;
[ x5 y5 z5] =r ot ar ( xi , yi , zi , xi , yi - 0. 6, zi +0. 5, angx, angz) ;

[ x6 y6 z6] =r ot ar ( xi , yi , zi , xi +0. 6, yi , zi +0. 5, angx, angz) ;
[ x7 y7 z7] =r ot ar ( xi , yi , zi , xi , yi +0. 6, zi +0. 5, angx, angz) ;
[ x8 y8 z8] =r ot ar ( xi , yi , zi , xi - 0. 6, yi , zi +0. 5, angx, angz) ;


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

[ x1 y1 z1] =r ot ( 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] ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sust i t ui mos l as var i abl es por su val or
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: num_si mb_r ot
x1=subs( x1, Q1( i ) ) ;
x2=subs( x2, Q1( i ) ) ;
x3=subs( x3, Q1( i ) ) ;
x4=subs( x4, Q1( i ) ) ;
x5=subs( x5, Q1( i ) ) ;
x6=subs( x6, Q1( i ) ) ;
x7=subs( x7, Q1( i ) ) ;
x8=subs( x8, Q1( i ) ) ;
y1=subs( y1, Q1( i ) ) ;
y2=subs( y2, Q1( i ) ) ;
y3=subs( y3, Q1( i ) ) ;
y4=subs( y4, Q1( i ) ) ;
y5=subs( y5, Q1( i ) ) ;
y6=subs( y6, Q1( i ) ) ;
y7=subs( y7, Q1( i ) ) ;
y8=subs( y8, Q1( i ) ) ;
z1=subs( z1, Q1( i ) ) ;
z2=subs( z2, Q1( i ) ) ;
z3=subs( z3, Q1( i ) ) ;
z4=subs( z4, Q1( i ) ) ;
z5=subs( z5, Q1( i ) ) ;
z6=subs( z6, Q1( i ) ) ;
z7=subs( z7, Q1( i ) ) ;
z8=subs( z8, Q1( i ) ) ;
end

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

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on env_bot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci on que nos per mi t e posi ci onar el r obot en l a posi ci n
% i ndi cada, segn el val or de l as ar t i cul aci ones
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MRej es P_D_H Q1_si mb Q1 Q2_si mb Q2 num_si mb_r ot num_si mb_r ot 2
hdat a r obot d2 hcheck checkdat a DH

n=si ze( P_D_H, 1) ; %N de f i l as
checkdat a=0; %Var i abl e de checkbox
P_D_Haux=P_D_H;
cont 1=0; %Cont ador
cont 2=0; %Cont ador

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos el n de ar t i cul aci ones, el t i po y el or den
% de el l as.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f MRej es( i , 13) ==1 %Ar t i cul aci n de t r asl aci on
a( i - 1) =0; %Vect or con t i po de var i abl e
ar t i cul ar
cont 1=cont 1+1;
Var ( i - 1) =Q2_si mb( cont 1) ; %Vect or con var i abl es
ar t i cul ar es or denadas
end
i f MRej es( i , 13) ==2
a( i - 1) =1;
cont 2=cont 2+1;
Var ( i - 1) =Q1_si mb( cont 2) ;
end
end
a( cont 1+cont 2+1) =0; %Cr eamos un ar t i cul aci n
f i ct i ci a al f i nal del r obot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Damos val or cer o a l as var i abl es ar t i cul ar es par a adapt ar l as
al
% f or mat o del t ool box de r obt i ca
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nueva vent ana par a i nt r oduci r l os dat os de l as var i abl es

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

' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' on' , . . .
' Name' , ' I nt r oduci r dat os' , . . .
' Number Ti t l e' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' Posi t i on' , [ 0. 15 0. 15 0. 6 0. 6] ) ;

% Bot n
hi nt dat a = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' Cal l back' , @i nt dat a, . . .
' St r i ng' , ' I nt r . Dat o' , . . .
' Posi t i on' , [ . 6 . 6 . 1 . 05] , . . .
' Enabl e' , ' on' , . . .
% Checkbox
hcheck = ui cont r ol ( ' st yl e' , ' checkbox' , . . .
' Cal l back' , @check, . . .
' St r i ng' , ' Ver Robot en
3D' , . . .
' Posi t i on' , [ . 6 . 4 . 15
. 05] , . . .
' Enabl e' , ' on' , . . .

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nos di buj a l as var i abl es par a que l e podamos asi gnar un val or
y
% l as col oca en l a vent ana dependi endo del n de var i abl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

b=1/ ( n- 1) ; %Var i abl e par a col ocar l a vent ana
c=1+b/ 2; %Var i abl e con val or i ni ci al par a
col ocar l a vent ana
f or i =1: n- 1
' St r i ng' , char ( Var ( 1, i ) ) , . . .
' Posi t i on' , [ . 2 c- b . 1
. 05] , . . .
' Enabl e' , ' on' , . . .


hdat a( i ) = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 4 c- b . 1
. 05] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .
c=c- b;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cr eo l a mat r i z del r obot par a usar l a en el t ool box de r obt i ca
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: n
si conbot {i }=l i nk( [ P_D_Haux( i , 1) P_D_Haux( i , 2) P_D_Haux( i , 3)
P_D_Haux( i , 4) a( i ) ] , ' modi f i ed' ) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% cr eamos el r obot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

r obot d2=r obot ( si conbot , ' Robot de si conbot ' ) ;
end

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

f unct i on var ar gout = i nt dat a( var ar gi n)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que nos di buj a el r obot segn l os dat os i nt r oduci dos y
l as
% opci ones el egi das
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal hdat a r obot d2 MRej es Q2 Q1 MR sp1 hf 1 n Q2_si mb Q1_si mb
Q2_si mb2 checkdat a MR_si mb

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos l os val or es de l as ar t i cul aci ones
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: si ze( hdat a, 2)
vect or ( i ) =st r 2num( get ( hdat a( i ) , ' St r i ng' ) ) ;
end
vect or ( i +1) =0; %Ul t i ma ar t i cul aci n( Fi ct i ci a) =0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

% Di buj amos el r obot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f i gur e
pl ot ( r obot d2, vect or ) %di buj a el r obot
axi s on
AXI S( [ 0 30 0 30 0 40] ) ;
gr i d on
i f checkdat a==1 %si quer emos di buj ar l o en 3D
cont 2=0;
cont 1=0;
f or i =1: si ze( hdat a, 2)
i f MRej es( i +1, 13) ==1
Q2( cont 2+1) =st r 2num( get ( hdat a( i ) , ' St r i ng' ) ) ; %val or de
var i abl es
cont 2=cont 2+1;
end
i f MRej es( i +1, 13) ==2
Q1( cont 1+1) =st r 2num( get ( hdat a( i ) , ' St r i ng' ) ) ; %Val or de
var i abl es
cont 1=cont 1+1;
end
end
f i gur e( hf 1) %Sel ecci onamos f i gur e pr i nci pal de
si conbot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pr epar amos el subpl ot pr i nci pal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

del et e ( sp1)
sp1=subpl ot ( 5, 3, [ 2 3 5 6 8 9 11 12 14 15] ) ;
xl abel ( ' ej e x' ) ;
yl abel ( ' ej e y' ) ;
axi s on
AXI S( [ - 30 30 - 30 30 0 40] ) ;
gr i d on
subpl ot ( sp1)

f or i =1: n
end
cl ear MR
a=0;
MR=MR_si mb;
di bubot ( MR) %di buj a r obot en 3D
end
end

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

f unct i on var ar gout = check( var ar gi n)

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

% Funci n que det ect a si se act i va ver el r obot en 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal checkdat a

i f checkdat a==0
checkdat a=1;
el se
chackdat a=0;
end
end

f unct i on [ xf yf zf ] =esl abon2( xi , yi , zi , xf , yf , zf , ange)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a un esl abn, si t uado ent r e un punt o
i ni ci al y
% [ xf yf zf ] =esl abon2( xi , yi , zi , xf , yf , zf , ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
gi r o
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

l =sqr t ( abs( xf - xi ) ^2+abs( yf - yi ) ^2+abs( zf - zi ) ^2) ; %Longi t ud del
t r amos
i f ( yf - yi ) >0. 0001 %ngul o posi t i cvo
el se %ngul o negat i vo
angx=( pi / 2- asi n( ( zf - zi ) / l ) ) ;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
y
% f i nal .
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f ( xi ==xf ) &( yi ==yf ) %Si t r amo es ver t i cal
angz=0; %ngul o y=0
el se
l x=xf - xi ; %Longi t ud en x
l y=yf - yi ; %Longi t ud en y
l 1=sqr t ( l x^2+l y^2) ; %Longi t ud en pl ano XY
V=[ l x/ l 1 l y/ l 1 0] *l ; %Pr oyecci n en ej e XY
xf 1=V( 1) ;
yf 1=V( 2) ;
zf 1=V( 3) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


xf =xi ;
yf =yi ;
zf =zi +l ;

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

[ x1 y1 z1] =r ot ar ( xi , yi , zi , xi - 0. 5, yi - 0. 5, zi , angx, angz) ;
[ x2 y2 z2] =r ot ar ( xi , yi , zi , xi +0. 5, yi - 0. 5, zi , angx, angz) ;
[ x3 y3 z3] =r ot ar ( xi , yi , zi , xi +0. 5, yi +0. 5, zi , angx, angz) ;
[ x4 y4 z4] =r ot ar ( xi , yi , zi , xi - 0. 5, yi +0. 5, zi , angx, angz) ;
[ x5 y5 z5] =r ot ar ( xi , yi , zi , xi - 0. 5, yi - 0. 5, zi +l , angx, angz) ;
[ x6 y6 z6] =r ot ar ( xi , yi , zi , xi +0. 5, yi - 0. 5, zi +l , angx, angz) ;
[ x7 y7 z7] =r ot ar ( xi , yi , zi , xi +0. 5, yi +0. 5, zi +l , angx, angz) ;
[ x8 y8 z8] =r ot ar ( xi , yi , zi , xi - 0. 5, yi +0. 5, zi +l , angx, angz) ;


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

i f ange
end

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
pat ch( ' Ver t i ces' , ver t i ces, ' Faces' , car as, ' FaceVer t exCDat a' , [ . 8 . 4
. 125] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on [ xf yf zf ] =esl abon( xi , yi , zi )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una esl abn, si t uado en un punt o
i ni ci al y
% que nos devuel ve un punt o f i nal . El esl abn di buj ado
l l evar
% [ xf yf zf ] =esl abon( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

t r as Q1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos el n de el ement os de t r asl aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f t r as>1
t r as2=t r as- 1;
el se t r as2=t r as;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos dat os de l ongi t ud
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r adi anes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +l ;

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

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



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

i f ange
end

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

x1=subs( x1, Q1( t r as2) ) ;
x2=subs( x2, Q1( t r as2) ) ;
x3=subs( x3, Q1( t r as2) ) ;
x4=subs( x4, Q1( t r as2) ) ;
x5=subs( x5, Q1( t r as2) ) ;
x6=subs( x6, Q1( t r as2) ) ;
x7=subs( x7, Q1( t r as2) ) ;
x8=subs( x8, Q1( t r as2) ) ;
y1=subs( y1, Q1( t r as2) ) ;
y2=subs( y2, Q1( t r as2) ) ;
y3=subs( y3, Q1( t r as2) ) ;
y4=subs( y4, Q1( t r as2) ) ;
y5=subs( y5, Q1( t r as2) ) ;
y6=subs( y6, Q1( t r as2) ) ;
y7=subs( y7, Q1( t r as2) ) ;
y8=subs( y8, Q1( t r as2) ) ;
z1=subs( z1, Q1( t r as2) ) ;
z2=subs( z2, Q1( t r as2) ) ;
z3=subs( z3, Q1( t r as2) ) ;
z4=subs( z4, Q1( t r as2) ) ;
z5=subs( z5, Q1( t r as2) ) ;
z6=subs( z6, Q1( t r as2) ) ;
z7=subs( z7, Q1( t r as2) ) ;
z8=subs( z8, Q1( t r as2) ) ;


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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
pat ch( ' Ver t i ces' , ver t i ces, ' Faces' , car as, ' FaceVer t exCDat a' , [ . 8 . 4
. 125] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on [ xf yf zf ] =gar r a2( xi , yi , zi , xf , yf , zf , ange)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una gar r a, si t uado ent r e un punt o
i ni ci al y
% ot r o f i nal . La gar r a di buj ada l l evar
% [ xf yf zf ] =gar r a2( xi , yi , zi , xf , yf , zf , ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
gi r o
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

l =sqr t ( abs( xf - xi ) ^2+abs( yf - yi ) ^2+abs( zf - zi ) ^2) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
y
% f i nal .
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f ( xi ==xf ) &( yi ==yf )
angz=0;
el se
l x=xf - xi ;
l y=yf - yi ;
l 1=sqr t ( l x^2+l y^2) ;
V=[ l x/ l 1 l y/ l 1 0] *l ;
xf 1=V( 1) ;
yf 1=V( 2) ;
zf 1=V( 3) ;
angz=- ( pi / 2- acos( xf 1/ l ) ) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +l ;


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

[ x5 y5 z5] =r ot ar ( xi , yi , zi , xi , yi , zi +l , angx, angz) ;


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

i f ange

end

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

ver t i ces=[ x1 y1 z1; x2 y2 z2; x3 y3 z3; x4 y4 z4; x5 y5 z5] ;
car as=[ 1 2 3 4; 1 2 5 5; 2 3 5 5; 3 4 5 5; 1 4 5 5] ;
subpl ot ( sp1)
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n


f unct i on [ xf yf zf ] =gar r a( xi , yi , zi )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a una gar r a, si t uada en un punt o i ni ci al
y
% que nos devuel ve un punt o f i nal . La gar r a di buj ada l l evar
% [ xf yf zf ] =gar r a( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r adi anes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xf =xi ;
yf =yi ;
zf =zi +l ;

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

[ x5 y5 z5] =r ot ar ( xi , yi , zi , xi , yi , zi +l , angx, angz) ;


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

i f ange

end

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

ver t i ces=[ x1 y1 z1; x2 y2 z2; x3 y3 z3; x4 y4 z4; x5 y5 z5] ;
car as=[ 1 2 3 4; 1 2 5 5; 2 3 5 5; 3 4 5 5; 1 4 5 5] ;
subpl ot ( sp1)
0] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n


f unct i on l oadbot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que car ga un r obot al macenado en di sco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

sp6 sp7 n num_si mb_r ot Q1_si mb Q1 MRej es Q2_si mb num_si mb_r ot 2 Q2
ht r as hej e hr ot hgar r a hbase gr aph_DH gr aph_cal cbot MR_si mb
gr aph_ci ndi r gr aph_ci ni nv r ot ac t r as

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% car ga l as var i abl es del ar chi vo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

[ f name, pname] = ui get f i l e( ' *. mat ' , ' Sel ect Fi l e' ) ;
dbf i l e= st r cat ( pname, f name) ;
i f l engt h( dbf i l e) == 0 r et ur n; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% car ga l as var i abl es al wor kspace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

l oad( f name)
f or i =1: n
end
cl ear MR
a=0;
MR=MR_si mb;

di bubot ( MR) %di buj a el r obot en esa posi ci on
exact a
MRej es=si mpl e( pl ot ej es( MRper p) ) ; %Obt engo el r est o de ej es y l os
di buj o

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Habi l i t aci on de nuevas opci ones en el menu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

set ( ht r as, ' Enabl e' , ' of f ' ) ;
set ( hej e, ' Enabl e' , ' of f ' ) ;
set ( hr ot , ' Enabl e' , ' of f ' ) ;
set ( hgar r a, ' Enabl e' , ' of f ' ) ;
set ( hbase, ' Enabl e' , ' of f ' ) ;
set ( gr aph_DH, ' Enabl e' , ' on' )
set ( gr aph_cal cbot , ' Enabl e' , ' of f ' )
set ( gr aph_ci ndi r , ' Enabl e' , ' on' )
r et ur n

f unct i on MRvec=MRt oMRvec( MR)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que t r ansf or ma l a mat r i z con l as coor denadas de l os
% esl abones en una mat r i z con l os ej es de l as ar t i cul aci ones de
% r ot aci n y t r asl aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

num=0;
n=si ze( MR, 1) ;
MRvec=sym( [ 0 0 0 0 0 0 0] ) ;
f or i =1: n
x0=MR( i , 2) ;
y0=MR( i , 3) ;
z0=MR( i , 4) ;
i f ( MR( i , 1) ==1) | ( MR( i , 1) ==2) %ej e r ot o t r as
x1=MR( i , 5) ;
y1=MR( i , 6) ;
z1=MR( i , 7) ;
num=num+1;

i f MR( i , 1) ==2
%si l a ar t i cul aci on es de r ot aci on el ej e se cal cul a:
MRvec( num, : ) =si mpl e( [ ( x0+x1) / 2 ( y0+y1) / 2 ( z0+z1) / 2 x1- x0
y1- y0 z1- z0 1] ) ;
el se
%si l a ar t i cul aci on es de t r asl acci on el ej e se cal cul a:
MRvec( num, : ) =si mpl e( [ x1 y1 z1 x1- x0 y1- y0 z1- z0 2] ) ;
end
end
i f ( i ==n) &( MR( i , 1) ~=1) &( MR( i , 1) ~=2) %si es un ul t i mo esl abon y no
es una ar t i cul aci on
x1=MR( i , 5) ;
y1=MR( i , 6) ;
z1=MR( i , 7) ;
num=num+1;
MRvec( num, : ) =si mpl e( [ 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

f unct i on MRper p=MRvect oMRper p( MRvec, MR)

% Funci n que t r ansf or ma l a mat r i z con l os ej es pr i nci pal es de
l as ar t i cul aci ones
% en una mat r i z que cont i ene l a per pendi cul ar a cada ej e

gl obal sp1 num_si mb_r ot Q1_si mb Q1 num_si mb_r ot 2 Q2_si mb Q2
n=si ze( MRvec, 1) ; %n de ar t i cul aci ones
n2=si ze( MR, 1) ; %n el ement os del r obot
MRper p=sym( [ 0 0 0 0 0 0 0 0 0 0] ) ;
f or i =1: n- 1
p1=si mpl e( MRvec( i , 1) ) ;
q1=si mpl e( MRvec( i +1, 1) ) ;
% v1=subs( MRvec( i , 4) , ' q1' ) ;
% v2=subs( MRvec( i , 5) , ' q1' ) ;
% v3=subs( MRvec( i , 6) , ' q1' ) ;
v1=si mpl e( MRvec( i , 4) ) ;
w1=si mpl e( MRvec( i +1, 4) ) ;
q4=si mpl e( p1*v1+p2*v2+p3*v3) ;
u( i , : ) =cr oss( MRvec( i +1, [ 4 5 6] ) , MRvec( i , [ 4 5 6] ) ) ;

k1=q1- p1;
k2=q2- p2;
k3=q3- p3;
mod_v1=sqr t ( 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] ;

%det er mi namos si l os ej es son coi nci dent es, par al el os, se cor t an o
se cr uzan
i f ( r ank( A) ==1) &( r ank( B) ==1)
%coi nci dent es
r ect as=0;
end
i f ( r ank( A) ==1) &( r ank( B) ==2)
%par al el os
r ect as=1;
end
i f ( r ank( A) ==2) &( r ank( B) ==2)
%se cor t an
r ect as=3;
end
i f ( r ank( A) ==2) &( r ank( B) ==3)
%se cr uzan
r ect as=4;
end

i f r ect as==0 %coi nci dent es

i f ( i >1) &( cr oss( MRvec( i , [ 4 5 6] ) , MRvec( i - 1, [ 4 5 6] ) ) ==[ 0 0 0] )
%Si l os ej es de esl abon act ual y ant er i or coi nci den

MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) p1 p2 p3 MRper p( i - 1, [ 7 8
9] ) MRvec( i , 7) ] ) ; %ej e z i gual a ej e x act ual y ej e x i gual al z
ant er i or
u( i , : ) =si mpl e( MRper p( i - 1, [ 7 8 9] ) ) ;
%el sei f ( i <n- 2) &( cr oss( MRvec( i +1, [ 4 5 6] ) , MRvec( i +2, [ 4 5
6] ) ) ==[ 0 0 0] )
% MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) p1 p2 p3 MRvec( i +2, [ 4 5
6] ) ] ) ;
% u( i , : ) =si mpl e( MRvec( i +2, [ 4 5 6] ) ) ;
%el sei f i ==n- 1
% MRper p( i , : ) =( [ MRvec( n, [ 4 5 6] ) MRvec( i , [ 1 2 3] ) 0 0 0] ) ;
% u( i , : ) =si mpl e( MRvec( i , [ 1 2 3] ) ) ;
el sei f ( i >1)
MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) p1 p2 p3 MRvec( i - 1, [ 4 5 6] )
MRvec( i , 7) ] ) ;
u( i , : ) =si mpl e( MRvec( i - 1, [ 4 5 6] ) ) ;
end
i f i ==1
i f MRvec( i , 7) ==2
MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) MRvec( i , [ 1 2 3] ) 1 0 0
MRvec( i , 7) ] ) ;
end
i f MRvec( i , 7) ==1
MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) MRvec( i , [ 1 2 3] )
si n( Q2_si mb( i ) ) cos( Q2_si mb( i ) ) 0 MRvec( i , 7) ] ) ;
end

end

end

i f r ect as==1 %Par al el as
q1_=si mpl e( 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_=si mpl e( 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_=si mpl e( 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 , : ) =( [ si mpl e( q1_- p1) si mpl e( q2_- p2) si mpl e( q3_-
p3) ] / sqr t ( ( q1_- p1) ^2+( q2_- p2) ^2+( q3_- p3) ^2) ) ;
%sum( abs( [ si mpl e( q1_- p1) si mpl e( q2_- p2) si mpl e( q3_-
p3) ] ) . ^2) ^( 1/ 2) ) ; %nor m( [ q1_- p1 q2_- p2 q3_- p3] ) ;
MRper p( i , : ) =si mpl e( [ MRvec( i , [ 4 5 6] ) p1 p2 p3 u( i , : )
MRvec( i , 7) ] ) ;
end

i f r ect as==3 %Cor t an
x=si mpl e( ( 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) ] ) ) ) ;

r 1=si mpl e( p1+abs( x) *v1) ;
MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) r 1 r 2 r 3 u( i , : ) MRvec( i , 7) ] ) ;
end
i f r ect as==4 %Cr uzan
MRper p( i , : ) =( [ MRvec( i , [ 4 5 6] ) p1 p2 p3 u( i , : ) MRvec( i , 7) ] ) ;
end


end
MRper p( i +1, : ) =( [ MRvec( i +1, [ 4 5 6] ) MR( n2, [ 5 6 7] ) u( i , : )
MRvec( i +1, 7) ] ) ; %vect or z + punt o de or i gen + vect or x
end

f unct i on nuevo( )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n par a di buj ar un nuevo r obot , car gar uno ya exi st ent e o
% sal var uno nuevo al di sco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal hf 1 ht ext ox ht ext oy ht ext oz ht ext o ht ext oe sp1 sp2 sp3 sp4 sp5
sp6 sp7 MR mf i l e_save n mf i l e_l oad mf i l e_gr af mf i l e_DH n MRper p MRvec
t ht ext ot 1 num_si mb_r ot Q1_si mb x y z t r as hej es hbot gr aph_DH MRej es
hr ot 3d Q1 r ot ac Q2_si mb Q2 num_si mb_r ot 2 gr aph_cal cbot ht r as hej e hr ot
hgar r a hbase xi nf xsup yi nf ysup zi nf zsup t 1 q1 q2 q3

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%i ni ci al i zaci on de var i abl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

syms t 1 t 2 t 3 t 4 t 5 posi t i ve %Var i abl es de t r asl aci on
x=0; % X i ni ci al
y=0; % Y i ni ci al
z=0; % Z i ni ci al
n=0; % N de pi ezas i ni ci al es
t r as=1; %N de el emde t r asl aci on+1
r ot ac=1; %N de el emde r ot aci n+1
MR=sym( [ 0 0 0 0 0 0 0 0] ) ; %Mat r i z del r obot
num_si mb_r ot =0; %n de el ement os de t r asl aci on
num_si mb_r ot 2=0; %n de el ement os de r ot aci on
Q1_si mb=sym( ' q' ) ; %Var i abl es de t r asl aci on
Q1( t r as) =0; %Val or es de t r asl aci on
Q2_si mb=sym( ' q' ) ; %Var i abl es de r ot aci on
Q2( r ot ac) =0; %Val or es de r ot aci on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% l i mde l os ej es del pl ot pr i nci pal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

xi nf =- 30;
xsup=30;
yi nf =- 30;
ysup=30;
zi nf =0;
zsup=40;
ej es=[ - 20 20 - 20 20 0 20] ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Col or de f ondo
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

set ( hf 1, ' Col or ' , [ . 824, . 824, . 824] )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Habi l i t a opci ones del menu

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

set ( mf i l e_l oad, ' Enabl e' , ' on' )
set ( mf i l e_gr af , ' Enabl e' , ' of f ' )
set ( mf i l e_DH, ' Enabl e' , ' of f ' )
set ( gr aph_cal cbot , ' Enabl e' , ' on' )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cr eaci n de panel es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

hp0 = ui panel ( ' Backgr oundCol or ' , [ . 824, . 824, . 824] , . . .
' Posi t i on' , [ . 0 0 . 10 1] ) ;
' Posi t i on' , [ . 10 0 . 13 1] , ' t i t l e' , ' Pi ezas' ) ;
' Posi t i on' , [ . 38 0 . 7 1] , ' t i t l e' , ' Robot ' ) ;
' Posi t i on' , [ . 93 0 . 07 1] , ' t i t l e' , ' Her r ami ent as' ) ;
' Posi t i on' , [ . 23 0. 8 . 15 0. 8] , ' t i t l e' , ' t r asl aci on' ) ;
' Posi t i on' , [ . 23 0. 5 . 15 0. 3] , ' t i t l e' , ' Rot aci n' ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cr eaci n de Pl ot s par a l a r epr esent aci n de l as f i gur as
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

sp1=subpl ot ( 5, 3, [ 2 3 5 6 8 9 11 12 14 15] ) ;
xl abel ( ' ej e x' , ' col or ' , ' r ' ) ;
yl abel ( ' ej e y' , ' col or ' , ' g' ) ;
sp2=subpl ot ( 5, 3, 1) ;
sp3=subpl ot ( 5, 3, 4) ;
sp4=subpl ot ( 5, 3, 7) ;
sp5=subpl ot ( 5, 3, 10) ;
sp6=subpl ot ( 5, 3, 13) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bor r ado de ej es de l os subpl ot s
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

subpl ot ( sp1) ;
axi s of f
subpl ot ( sp2) ;
axi s of f
subpl ot ( sp3) ;
axi s of f
subpl ot ( sp4) ;
axi s of f
subpl ot ( sp5) ;
axi s of f
subpl ot ( sp6) ;
axi s of f


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Di buj o de Fi gur as de r ef er enci a
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%t r asl aci on
ver t i ces1=[ x- 0. 3 y- 0. 3 z; x+0. 3 y- 0. 3 z; x+0. 3 y+0. 3 z; x- 0. 3 y+0. 3 z; x-
0. 3 y- 0. 3 z+7; x+0. 3 y- 0. 3 z+7; x+0. 3 y+0. 3 z+7; x- 0. 3 y+0. 3 z+7] ;
car as1=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp2)
pat ch( ' Ver t i ces' , ver t i ces1, ' Faces' , car as1, ' FaceVer t exCDat a' , [ . 4, . 4,
. 125] , ' FaceCol or ' , ' f l at ' ) ;
AXI S( [ - 0. 5 9. 5 - 0. 5 9. 5 0 10] ) ;
camor bi t ( 55, - 45)

%Rot aci n
ver t i ces2=[ 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] ;
car as2=[ 1 2 3 4; 5 6 7 8; 1 5 6 2; 2 6 7 3; 7 8 4 3; 8 4 1 5] ;
subpl ot ( sp3)
pat ch( ' Ver t i ces' , ver t i ces2, ' Faces' , car as2, ' FaceVer t exCDat a' , [ 1, 0,
0] , ' FaceCol or ' , ' f l at ' ) ;
AXI S( [ - 0. 5 9. 5 - 0. 5 9. 5 0 10] ) ;
camor bi t ( 55, - 45)

%Esl abn
ver t i ces3=[ x- 0. 5 y- 0. 5 z; x+0. 5 y- 0. 5 z; x+0. 5 y+0. 5 z; x- 0. 5 y+0. 5 z; x-
0. 5 y- 0. 5 z+7; x+0. 5 y- 0. 5 z+7; x+0. 5 y+0. 5 z+7; x- 0. 5 y+0. 5 z+7] ;
car as3=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp4)
. 125] , ' FaceCol or ' , ' f l at ' ) ;
axi s xy
AXI S( [ - 0. 5 9. 5 - 0. 5 9. 5 0 10] ) ;
camor bi t ( 55, - 45)

%Base
ver t i ces4=[ 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] ;
car as4=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp5)
pat ch( ' Ver t i ces' , ver t i ces4, ' Faces' , car as4, ' FaceVer t exCDat a' , [ 0, 0,
0] , ' FaceCol or ' , ' f l at ' ) ;
axi s xy
AXI S( [ - 1 8 - 1 8 0 4] ) ;
camor bi t ( 55, - 15)

%Gar r a
ver t i ces5=[ 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] ;
car as3=[ 1 2 3 4; 1 2 5 5; 2 3 5 5; 3 4 5 5; 1 4 5 5] ;
subpl ot ( sp6)
. 0] , ' FaceCol or ' , ' f l at ' ) ;
axi s xy
AXI S( [ - 0. 5 9. 5 - 0. 5 9. 5 0 10] ) ;
camor bi t ( 55, - 45)

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

% Pl ot pr i nci pal par a di buj ar el r obot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

subpl ot ( sp1)
axi s on
AXI S( [ xi nf xsup yi nf ysup zi nf zsup] ) ; %L mi t e de ej es
camor bi t ( 165, 0) %Cmar a
gr i d on
caml i ght r i ght
caml i ght l ef t
l i ght i ng phong

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bot ones, Text os y cuadr os edi t abl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

r ot at e_st r uct = l oad( [ mat l abr oot , ' / t ool box/ mat l ab/ i cons/ r ot at e. mat ' ] ) ;
r ot at e_cdat a = r ot at e_st r uct . cdat a;
i mgej es = i mr ead( ' ej es. TI F' ) ;
i mgbot = i mr ead( ' r obot . TI F' ) ;

%bot on de r ot aci n
hr ot 3d = ui cont r ol ( ' St yl e' , ' t oggl ebut t on' , . . .
' Cal l back' , @r ot 3d, . . .
' Tag' , ' mypan' , . . .
' Posi t i on' , [ 0. 95 0. 94 0. 03
0. 03] , . . .
' CDat a' , r ot at e_cdat a, . . .

%bot on de ver ej es
hej es = ui cont r ol ( ' St yl e' , ' t oggl ebut t on' , . . .
' Cal l back' , @ver , . . .
' Tag' , ' mypan' , . . .
' Posi t i on' , [ 0. 95 0. 84 0. 03
0. 03] , . . .
' CDat a' , i mgej es, . . .

%bot on de r epr esent ar r obot
hbot = ui cont r ol ( ' St yl e' , ' t oggl ebut t on' , . . .
' Cal l back' , @ver , . . .
' Tag' , ' mypan' , . . .
' Posi t i on' , [ 0. 95 0. 74 0. 03
0. 03] , . . .
' CDat a' , i mgbot , . . .
' val ue' , 1, . . .

%Bot on de aadi r t r asl aci on
ht r as = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' Cal l back' , @col ocar 1, . . .

' St r i ng' , ' Tr asl aci n' , . . .
' Posi t i on' , [ . 11 . 77 . 06
. 03] , . . .
' Enabl e' , ' on' , . . .

%Bot on de aadi r r ot aci on
hej e = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' St r i ng' , ' Ej e Rot aci n' , . . .
' Posi t i on' , [ . 11 . 6 . 06
. 03] , . . .
' Enabl e' , ' on' , . . .

%Bot on de aadi r Esl abon
hr ot = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' St r i ng' , ' Esl abn' , . . .
' Posi t i on' , [ . 11 . 42 . 06
. 03] , . . .
' Enabl e' , ' on' , . . .

%Bot on de aadi r base
hbase = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' St r i ng' , ' Base' , . . .
' Posi t i on' , [ . 11 . 23 . 06
. 03] , . . .
' Enabl e' , ' on' , . . .

%Bot on de aadi r gar r a
hgar r a = ui cont r ol ( ' st yl e' , ' pushbut t on' , . . .
' St r i ng' , ' Gar r a' , . . .
' Posi t i on' , [ . 11 . 08 . 06
. 03] , . . .
' Enabl e' , ' of f ' , . . .

%l ongi t ud
ht ext o = ui cont r ol ( ' St yl e' , ' edi t ' , . . .

' Posi t i on' , [ . 31 . 06 . 05
. 03] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 10' , . . .

hl abel = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' l ongi t ud ( dm) ' , . . .
' backgr oundcol or ' , [ . 824 . 824 . 824] , . . .
' Posi t i on' , [ . 245 . 0648 . 05
. 02] , . . .
' Enabl e' , ' on' , . . .

%r ot z
ht ext oz = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 11 . 05
. 03] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .

hl abel z = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' Rot . en Z ( ) ' , . . .
' Posi t i on' , [ . 245 . 1148 . 05
. 02] , . . .
' Enabl e' , ' on' , . . .

%r ot x
ht ext ox = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 16 . 05
. 03] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' 0' , . . .

hl abel x = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' Rot . en X ( ) ' , . . .
' Posi t i on' , [ . 245 . 1648 . 05
. 02] , . . .
' Enabl e' , ' on' , . . .


%Rot ej e
ht ext oe = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 21 . 05
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' 0' , . . .

hl abel e = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' Rot . ej e ( ) ' , . . .
' Posi t i on' , [ . 245 . 2148 . 05
. 018] , . . .
' Enabl e' , ' of f ' , . . .

%Li mi t e t r asl aci on
ht ext ot = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 92 . 05
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' 10' , . . .

hl abel t = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' L mi t e t r asl aci on' , . . .
' Posi t i on' , [ . 232 . 9248 . 068
. 018] , . . .
' Enabl e' , ' of f ' , . . .

%nombr e var i abl e t r as
ht ext ot 1 = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 88 . 05
. 03] , . . .
' Enabl e' , ' on' , . . .
' St r i ng' , ' ' , . . .

hl abel t 1 = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' Nombr e Var ' , . . .

' Posi t i on' , [ . 232 . 8848 . 068
. 018] , . . .
' Enabl e' , ' on' , . . .

hl abel t 2 = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' dm' , . . .
' Posi t i on' , [ . 36 . 9248 . 018
. 018] , . . .
' Enabl e' , ' of f ' , . . .

%l i mi t e r ot -
ht ext or 1 = ui cont r ol ( ' St yl e' , ' edi t ' , . . .
' Posi t i on' , [ . 31 . 60 . 05
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' - 360' , . . .

hl abel r 1 = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' L mi t e Rot . - ' , . . .
' Posi t i on' , [ . 232 . 5548 . 068
. 018] , . . .
' Enabl e' , ' of f ' , . . .

%l i mi t e r ot +
' Posi t i on' , [ . 31 . 55 . 05
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' 360' , . . .

' St r i ng' , ' L mi t e Rot . +' , . . .
' Posi t i on' , [ . 232 . 6048 . 068
. 018] , . . .
' Enabl e' , ' of f ' , . . .


%nombr e var r ot
' Posi t i on' , [ . 31 . 65 . 05
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' ' , . . .

' St r i ng' , ' Nombr e Var . ' , . . .
' Posi t i on' , [ . 232 . 6548 . 068
. 018] , . . .
' Enabl e' , ' of f ' , . . .

' St r i ng' , ' ' , . . .
' Posi t i on' , [ . 36 . 795 . 018
. 03] , . . .
' Enabl e' , ' of f ' , . . .
' St r i ng' , ' ' , . . .
' Posi t i on' , [ . 36 . 745 . 018
. 03] , . . .
' Enabl e' , ' of f ' , . . .
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Ej ecut ar l a f unci n del t i po de pi eza el egi da
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% col ocar t r asl aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
f unct i on var ar gout = col ocar 1( var ar gi n)

gl obal MR Q1 n x y z ht ext oe ht ext o t r as

i f n==0

MR=sym( [ 0 0 0 0 0 0 0 0] ) ; %Pt o i ni ci al
end
n=n+1; %n de el em
Q1( t r as) =st r 2num( get ( ht ext o, ' St r i ng' ) ) ; %val or var i abl e
l ongi t ud
t r as=t r as+1; %n el emt r as
MR( n, [ 2 3 4] ) =sym( [ x y z] ) ; %pt o i ni ci al t r amo
[ x y z x1 y1 z1] =t r asl ac( x, y, z) ; %pt o f i nal
MR( n, 1) =1; %ej e t r asl aci on
MR( n, [ 5 6 7] ) =[ x y z] ; %pt o f i nal t r amos
MR( n, 8) =st r 2num( get ( ht ext oe, ' St r i ng' ) ) *2*pi / 360; %r ot ej e
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Col ocar r ot aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


gl obal MR Q2 n x y z ht ext oe r ot ac
i f n==0
MR=sym( [ 0 0 0 0 0 0 0 0] ) ; %pt o i ni ci al
end
n=n+1; %n de el em
Q2( r ot ac) =st r 2num( get ( ht ext oe, ' St r i ng' ) ) ; %val or var i abl e de
r ot aci on
r ot ac=r ot ac+1; %n de el emde
r ot ac
[ x y z] =ej e( x, y, z) ; %nuevo pt o f i nal
MR( n, 1) =2; %ej e r ot ac
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Col ocar esl abon
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


gl obal MR n x y z ht ext oe

i f n==0
MR=sym( [ 0 0 0 0 0 0 0 0] ) ;
end
n=n+1; %n de el ement os
MR( n, [ 2 3 4] ) =[ x y z] ; %pt o i ni ci al
[ x y z] =esl abon( x, y, z) ; %pt o f i nal
MR( n, [ 5 6 7] ) =[ x y z] ;
MR( n, 1) =3; %t i po el em
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% col ocar base
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%




i f n==0
MR=sym( [ 0 0 0 0 0 0 0 0] ) ;
end
n=n+1; %n de el ement os
[ x y z] =base( x, y, z) ; %pt o f i nal
MR( n, [ 5 6 7] ) =[ x y z] ;
MR( n, 1) =4; %t i po de el em
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% col ocar gar r a
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%



i f n==0
MR=sym( [ 0 0 0 0 0 0 0 0] ) ;
end
n=n+1; %n de el ement o
[ x y z] =gar r a( x, y, z) ; %pt o f i nal
MR( n, [ 5 6 7] ) =[ x y z] ;
MR( n, 1) =5; %t i po de el em
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% habi l i t ar / deshabi l i t ar r ot aci on 3D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f unct i on var ar gout = r ot 3d( var ar gi n)

gl obal hr ot 3d

per si st ent enabl e_st at e;
val = get ( hr ot 3d, ' Val ue' ) ;
swi t ch val
case 0
r ot at e3d of f ;
case 1
r ot at e3d on;
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Habi l i t ar o Deshabi l i t ar ver ej es

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

f unct i on var ar gout = ver ( var ar gi n)

gl obal hej es hbot sp1 MR MRper p MRej es n Q2_si mb Q1_si mb

per si st ent enabl e_st at e;
val 1 = get ( hej es, ' Val ue' ) ;
val 2 = get ( hbot , ' Val ue' ) ;
del et e ( sp1)
sp1=subpl ot ( 5, 3, [ 2 3 5 6 8 9 11 12 14 15] ) ;
xl abel ( ' ej e x' ) ;
yl abel ( ' ej e y' ) ;
axi s on
AXI S( [ - 30 30 - 30 30 0 40] ) ;
gr i d on

i f val 1==1
pl ot ej es( MRper p) ; %most r ar ej es si est a act i vado
end
i f val 2==1 %si est a act i vada l a opci n de di buj ar r obot
f or i =1: n
end
cl ear MR
a=0;
f or i =1: n
i f MRaux( i , 1) ==2
a=a+1;
%r ot ar t r amos
f or j =i +1: n
end
end
end
MR=MRaux;
di bubot ( MR) ; %di buj ar r obot
end
end


f unct i on Par _DH

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que cal cul a l os par met r os DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MRej es P_D_H gr aph_r obot Q1_si mb Q1 Q2_si mb Q2 num_si mb_r ot
num_si mb_r ot 2 t 1 t 2 t 3 t 4 t 5 q1 q2 q3 gr aph_ci ni nv mf i l e_saveDH

P_D_H=sym( [ 0 0 0 0] ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nueva vent ana
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

hf 2=f i gur e( ' Vi si bl e' , ' on' , . . .
' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' on' , . . .
' Name' , ' Par met r os D- H' , . . .
' Number Ti t l e' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' Posi t i on' , [ 0. 25 0. 25 0. 5 0. 5] ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul a l os par met r os
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

di =si mpl e( cal c_di ( MRej es) ) ;
ai =si mpl e( cal c_ai ( MRej es) ) ;
t et hai =si mpl e( cal c_t et hai ( MRej es) ) ;
al phai =si mpl e( cal c_al phai ( MRej es) ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% most r amos l os val or es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

t am=1+( 1/ ( si ze( di , 2) +1) ) ; %Var i abl e par a di vi di r l a vent ana
segn el n de par met r os
f or i =1: si ze( di , 2) +1
i f i ==1
hpar _di = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , ' di ' , . . .

' Posi t i on' , [ 0 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
el se
' St r i ng' , char ( di ( i - 1) ) , . . .
' Posi t i on' , [ 0 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
end
end

t am=1+( 1/ ( si ze( di , 2) +1) ) ;
f or i =1: si ze( ai , 2) +1
i f i ==1
' St r i ng' , ' ai ' , . . .
' Posi t i on' , [ 0. 25 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
el se
hpar _ai = ui cont r ol ( ' St yl e' , ' t ext ' , . . .
' St r i ng' , char ( ai ( i - 1) ) , . . .
' Posi t i on' , [ 0. 25 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
end
end

t am=1+( 1/ ( si ze( di , 2) +1) ) ;
f or i =1: si ze( t et hai , 2) +1
i f i ==1
' St r i ng' , ' t et hai ' , . . .

' Posi t i on' , [ 0. 5 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
el se
' St r i ng' , char ( t et hai ( i - 1) ) , . . .
' Posi t i on' , [ 0. 5 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
end
end

t am=1+( 1/ ( si ze( di , 2) +1) ) ;
f or i =1: si ze( al phai , 2) +1
i f i ==1
' St r i ng' , ' al phai ' , . . .
' Posi t i on' , [ 0. 75 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
el se
' St r i ng' , char ( al phai ( i - 1) ) , . . .
' Posi t i on' , [ 0. 75 0 . 25 t am-
( 1/ ( si ze( di , 2) +1) ) ] , . . .
' Enabl e' , ' on' , . . .
t am=t am- ( 1/ ( si ze( di , 2) +1) ) ;
end
end

f or i =1: si ze( al phai , 2)
P_D_H( i , : ) =[ al phai ( i ) ; ai ( i ) ; t et hai ( i ) ; di ( i ) ] ; %mat r i z con l os
par amet r os de Denavi t - Har t ember g
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% habi l i t aci n de nuevas opci ones de l os menus

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

set ( gr aph_r obot , ' Enabl e' , ' on' )
set ( gr aph_ci ni nv, ' Enabl e' , ' on' ) ;
set ( mf i l e_saveDH, ' Enabl e' , ' on' ) ;
end

f unct i on MRej es=pl ot ej es( MRper p)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que nos muest r a l os ej es del r obot a par t i r de l a
mat r i z
% MRper p y l os al macena en ot r a mat r i z. MRej es=pl ot ej es( MRper p)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1 num_si mb_r ot Q1_si mb Q1 num_si mb_r ot 2 Q2_si mb Q2 t 1

n=si ze( MRper p, 1) ; %N de ej es
cl ear MRej es
subpl ot ( sp1)
MRej es( 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] ) ; %ej es i ni ci al es
MRej es_aux=MRej es;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sust i t uci on de val or es de t r asl aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

MRej es_aux=subs( MRej es, Q1_si mb( j ) , Q1( j ) ) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%sust i t uci on de val or es de r ot aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

MRej es_aux=subs( MRej es, Q2_si mb( j ) , Q2( j ) ) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%di buj a ej es i ni ci al es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or j =0: 0. 15: 3
hol d on
pl ot 3( MRej es_aux( 1, 1) *j , MRej es_aux( 1, 2) *j ,
MRej es_aux( 1, 3) *j , ' . b' , MRej es_aux( 1, 4) *j , MRej es_aux( 1, 5) *j ,
MRej es_aux( 1, 6) *j , ' - *r ' , MRej es_aux( 1, 7) *j , MRej es_aux( 1, 8) *j ,
MRej es_aux( 1, 9) *j , ' - +g' )
hol d of f
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul o l os ej es y l os di buj o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

f or i =1: n

l z=sqr t ( MRper p( i , 1) ^2+MRper p( i , 2) ^2+MRper p( i , 3) ^2) ;
i f l z~=0
MRej es( i +1, [ 1 2 3] ) =[ MRper p( i , 1) / l z MRper p( i , 2) / l z
MRper p( i , 3) / l z] ; %ej e Z
el se
MRej es( i +1, [ 1 2 3] ) =MRej es( i , [ 1 2 3] ) ;
end
l x=sqr t ( MRper p( i , 7) ^2+MRper p( i , 8) ^2+MRper p( i , 9) ^2) ;
i f l x~=0
MRej es( i +1, [ 4 5 6] ) =[ MRper p( i , 7) / l x MRper p( i , 8) / l x
MRper p( i , 9) / l x] ; %ej e X
el se
MRej es( i +1, [ 4 5 6] ) =MRej es( i , [ 4 5 6] ) ;
end
MRej es( i +1, [ 7 8 9] ) =cr oss( MRej es( i +1, [ 1 2 3] ) , MRej es( i +1, [ 4 5
6] ) ) ; %ej e y
MRej es( i +1, [ 10 11 12] ) =MRper p( i , [ 4 5 6] ) ;
%punt o de l os ej es
MRej es( i +1, 13) =MRper p( i , 10) ;
%Ti po de ar t i cul aci n
MRej es_aux=MRej es;

%sust i t uci on de val or es de t r asl acci n
MRej es_aux=subs( MRej es_aux, Q1_si mb( j ) , Q1( j ) ) ;
end
%sust i t uci on de val or es de r ot aci on
MRej es_aux=subs( MRej es_aux, Q2_si mb( j ) , Q2( j ) ) ;
end
%Di buj amos el ej e
f or j =0: 0. 15: 3
hol d on
pl ot 3( MRej es_aux( i +1, 10) +MRej es_aux( i +1, 1) *j ,
MRej es_aux( i +1, 11) +MRej es_aux( i +1, 2) *j ,
MRej es_aux( i +1, 12) +MRej es_aux( i +1, 3) *j , ' . b' , MRej es_aux( i +1, 10) +MRej es_
aux( i +1, 4) *j , MRej es_aux( i +1, 11) +MRej es_aux( i +1, 5) *j ,
MRej es_aux( i +1, 12) +MRej es_aux( i +1, 6) *j , ' -
*r ' , MRej es_aux( i +1, 10) +MRej es_aux( i +1, 7) *j ,
MRej es_aux( i +1, 11) +MRej es_aux( i +1, 8) *j ,
MRej es_aux( i +1, 12) +MRej es_aux( i +1, 9) *j , ' - +g' )
hol d of f
end
end

MRej es=expand( MRej es) ;


f unct i on mat =r edondear ( mat )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%r edondeamos l os val or es de l as mat r i ces
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

[ n, m] =si ze( mat ) ;
f or i =1: n
f or j =1: m
i f ( mat ( i , j ) <0. 0001) &mat ( i , j ) >- 0. 0001
mat ( i , j ) =0;
end
end
end
end

f unct i on [ Xf Yf Zf ] =r ot ( X1, Y1, Z1, X2, Y2, Z2, X, Y, Z, t et ha)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n par a r esol ver el gi r o de un punt o ( X, Y, Z) r espect o a
l a
% r ect a que pasa por l os punt os ( X1, Y1, Z1) y ( X2, Y2, Z2) con un
ngul o
% de r ot aci n i gual a t et ha
% [ Xf Yf Zf ] =r ot ( X1, Y1, Z1, X2, Y2, Z2, X, Y, Z, t et ha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

% vect or es de l a r ect a que pasa pos l os punt os
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=sqr t ( B^2+C^2) ;
i f ( B==0) &( Lp==0)
aux1=1;
aux2=0;
el se
aux1=B/ Lp;
aux2=C/ Lp;
end
Rx=[ 1 0 0 0; 0 aux2 aux1 0; 0 - aux1 aux2 0; 0 0 0 1] ;
Rx_1=[ 1 0 0 0; 0 aux2 - aux1 0; 0 aux1 aux2 0; 0 0 0 1] ;
L=sqr t ( A^2+B^2+C^2) ;
Ry=[ Lp/ L 0 A/ L 0; 0 1 0 0; - A/ L 0 Lp/ L 0; 0 0 0 1] ;
Ry_1=[ Lp/ L 0 - A/ L 0; 0 1 0 0; A/ L 0 Lp/ L 0; 0 0 0 1] ;
Rz=[ cos( - t et ha) si n( - t et ha) 0 0; - si n( - t et ha) cos( - t et ha) 0 0; 0 0 1
0; 0 0 0 1] ;
Rp=T*Rx*Ry*Rz*Ry_1*Rx_1*T_1;
Res=[ X Y Z 1] *Rp;
Xf =Res( 1, 1) ;
Yf =Res( 1, 2) ;
Zf =Res( 1, 3) ;

end


f unct i on [ Xf Yf Zf ] =Rot ar ( xi , yi , zi , xf , yf , zf , angx, angy)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que r ot a un punt o Pf con r espect o a l os ej es x e y
que
% hacemos pasar por ot r o punt o dado Pi :
% [ Xf Yf Zf ] =Rot ar ( 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) si n( angx) 0; 0 - si n( angx) cos( angx) 0; 0 0 0 1] ;
Rx=r edondear ( Rx) ;
Ry=[ cos( angy) 0 si n( angy) 0; 0 1 0 0; - si n( angy) 0 cos( angy) 0; 0 0 0 1] ;
Ry=r edondear ( Ry) ;
Rz=[ cos( angy) si n( angy) 0 0; - si n( angy) cos( angy) 0 0; 0 0 1 0; 0 0 0 1] ;
Rz=r edondear ( Rz) ;

F=[ A B C 1] *Rx*Rz;

Xf =F( 1, 1) +xi ;
Yf =F( 1, 2) +yi ;
Zf =F( 1, 3) +zi ;

f unct i on savebot

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que sal va un r obot al di sco
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal MR n num_si mb_r ot Q1_si mb Q1 Q2_si mb Q2 num_si mb_r ot 2 MRvec
MRper p MR_si mb r ot ac t r as

[ f name, pname] = ui put f i l e( ' *. mat ' , ' Sel ect Fi l e' ) ;

save
( f name, ' MR' , ' n' , ' num_si mb_r ot ' , ' Q1_si mb' , ' Q1' , ' Q2_si mb' , ' Q2' , ' num_si mb
_r ot 2' , ' MRper p' , ' MRvec' , ' MR_si mb' , ' r ot ac' , ' t r as' )
r et ur n

f unct i on saveDH

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que sal va un r obot al di sco, guar dando l a mat r i z
P_D_H,
% que cont i ene l a mat r i z de Denavi t - Har t ember g, y l a mat r i z DH
que es
% i gual que P_D_H per o l e aadi mos el t i po de r ot aci n. DH es
% compat i bl e con el t ool box de r obt i ca
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal P_D_H DH MRej es num_si mb_r ot num_si mb_r ot 2 Q1_si mb Q2_si mb

P_D_Haux=P_D_H;
cont 1=0;
cont 2=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%vemos t i po de ar t i cul aci n
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f MRej es( i , 13) ==1 %Es un ej e de r ot aci on
a( i - 1) =0; %Ti po de ar t i cul aci n
cont 1=cont 1+1; %n de ar t i c de r ot aci n
end
i f MRej es( i , 13) ==2 %Es un ej e de t r asl aci on
a( i - 1) =1; %Ti po de ar t i cul aci n
cont 2=cont 2+1; %n de ar t i c de t r asl aci on
end
end
a( cont 1+cont 2+1) =0; %Consi der amos el ej e f i nal como ot r o
ej e

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Sust i t ui mos var i abl es ar t i cul ar es por cer o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%cr eamos DH
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

DH=P_D_Haux;
f or i =1: ( si ze( DH, 1) )
DH( i , 5) =a( i ) ;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Guar damos
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

[ f name, pname] = ui put f i l e( ' *. mat ' , ' Sel ect Fi l e' ) ;

save ( f name, ' P_D_H' , ' DH' )
r et ur n

f unct i on si con( )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n i ni ci al que di buj a el l ogo y cr ea l os menus
despl egabl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

cl c
cl ear var i abl es
war ni ng of f
i mg = i mr ead( ' Logo. j pg' ) ; %i magen pr i nci pal
scr sz = get ( 0, ' Scr eenSi ze' ) ;
gl obal hf 1 mf i l e_save mf i l e_l oad mf i l e_gr af MR gr aph_DH MRej es
gr aph_cal cbot gr aph_r obot gr aph_ci ndi r gr aph_ci ni nv mf i l e_saveDH

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
%Vent ana pr i nci pal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

hf 1=f i gur e( ' Vi si bl e' , ' on' , . . .
' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' of f ' , . . .
' Name' , ' Si con- Bot ' , . . .
' Number Ti t l e' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' I nt er r upt i bl e' , ' on' , . . .
' Posi t i on' , [ 0 0. 08 1 0. 92] ) ;

subi mage( i mg) %most r ar i magen de f ondo
axi s of f %Si n ej es

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cr eamos l os menus y submenus despl egabl es
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

mf i l e= ui menu( ' Label ' , ' Ar chi vo' ) ;
mf i l e_gr af =ui menu( mf i l e, ' Label ' , ' Nuevo
Gr f i cament e' , ' Cal l back' , ' nuevo' , ' Separ at or ' , ' on' ) ;
mf i l e_save=ui menu( mf i l e, ' Label ' , ' Sal var
Robot ' , ' Cal l back' , ' Savebot ' , ' Separ at or ' , ' on' , ' Enabl e' , ' of f ' , ' Accel er at
or ' , ' S' ) ;
mf i l e_saveDH=ui menu( mf i l e, ' Label ' , ' Sal var D-
H' , ' Cal l back' , ' SaveDH' , ' Separ at or ' , ' of f ' , ' Enabl e' , ' of f ' ) ;
mf i l e_l oad=ui menu( mf i l e, ' Label ' , ' Car gar
Robot ' , ' Cal l back' , ' l oadbot ' , ' Separ at or ' , ' on' , ' Enabl e' , ' of f ' , ' Accel er at
or ' , ' L' ) ;
ui menu( mf i l e, ' Label ' , ' Sal i r ' , ' Cal l back' , ' cl ear
al l ' , ' Cal l back' , ' cl ose al l ' , ' Separ at or ' , ' on' , ' Accel er at or ' , ' Q' ) ;

mopt = ui menu( ' Label ' , ' Conf i gur aci on' ) ;

mopt _par di n=ui menu( mopt , ' Label ' , ' Par met r os
Di nmi cos' , ' Cal l back' , ' par _di n' , ' Accel er at or ' , ' X' , ' Enabl e' , ' of f ' ) ;

gr aph= ui menu( ' Label ' , ' Ej ecuci on' ) ;
gr aph_cal cbot =ui menu( gr aph, ' Label ' , ' Cal cul ar
Robot ' , ' Cal l back' , ' cal cbot ' , ' Enabl e' , ' of f ' ) ;
gr aph_DH=ui menu( gr aph, ' Label ' , ' Par met r os D-
H' , ' Cal l back' , ' Par _DH' , ' Enabl e' , ' of f ' ) ;
gr aph_r obot =ui menu( gr aph, ' Label ' , ' Posi ci onar
Robot ' , ' Cal l back' , ' env_bot ' , ' Enabl e' , ' of f ' ) ;
gr aph_ci ndi r =ui menu( gr aph, ' Label ' , ' Ci nemt i ca
Di r ect a' , ' Cal l back' , ' ci n_di r ' , ' Enabl e' , ' of f ' ) ;
gr aph_ci ni nv=ui menu( gr aph, ' Label ' , ' Ci nemt i ca
I nver sa' , ' Cal l back' , ' ci n_i nv' , ' Enabl e' , ' of f ' ) ;

mopt = ui menu( ' Label ' , ' Ayuda' ) ;
ui menu( mopt , ' Label ' , ' Ayuda
Si con' , ' Cal l back' , ' Ayuda' , ' Accel er at or ' , ' H' , ' Enabl e' , ' on' ) ;
ui menu( mopt , ' Label ' , ' Sobr e. . . ' , ' Cal l back' , ' sobr e' , ' Enabl e' , ' on' ) ;

end

f unct i on sobr e

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nos muest r a una vent ana con dat os sobr e el pr ogr ama
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i mg = i mr ead( ' si con. j pg' ) ; %i magen de f ondo
' Col or ' , [ 1 1 1] , . . .
' Resi ze' , ' of f ' , . . .
' Name' , ' Si con- Bot ' , . . .
' Number Ti t l e' , ' of f ' , . . .
' MenuBar ' , ' none' , . . .
' Posi t i on' , [ 0. 4 0. 4 0. 35 0. 35] ) ;

subi mage( i mg) %nos muest r a l a i magen
axi s of f
end

f unct i on [ xf yf zf ] =esl abon2( xi , yi , zi , xf , yf , zf , ange)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a un esl abn, si t uado ent r e un punt o
i ni ci al y
% ot r o f i nal , se usa en l a f unci n di bubot . La base di buj ada
l l evar
% [ xf yf zf ] =esl abon2( xi , yi , zi , xf , yf , zf , ange)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

gl obal sp1

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%
gi r o
de
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%

l =sqr t ( abs( xf - xi ) ^2+abs( yf - yi ) ^2+abs( zf - zi ) ^2) ; %l ongi t ud del t r amo
i f ( yf - yi ) >0. 0001 %ngul o posi t i vo
el se
angx=( pi / 2- asi n( ( zf - zi ) / l ) ) ; %ngul o negat i vo
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
y
% f i nal .
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f ( xi ==xf ) &( yi ==yf ) %si t r amos es ver t i cal
angz=0; %ngul o y=0
el se
l x=xf - xi ; %l ongi t ud en x
l y=yf - yi ; %l ongi t ud en y
l 1=sqr t ( l x^2+l y^2) ; %l ongi t ud en pl ano x- y
V=[ l x/ l 1 l y/ l 1 0] *l ; %Pr oyecci on en ej e XY
xf 1=V( 1) ;
yf 1=V( 2) ;
zf 1=V( 3) ;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


xf =xi ;
yf =yi ;
zf =zi +l ;

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



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

i f ange
end

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
pat ch( ' Ver t i ces' , ver t i ces, ' Faces' , car as, ' FaceVer t exCDat a' , [ . 4, . 4,
. 125] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

f unct i on [ xf 1 yf 1 zf 1 xf yf zf ] =t r asl ac( xi , yi , zi )

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Funci n que di buj a ej e de t r asl aci on, si t uado en un punt o
i ni ci al y
% que nos devuel ve un punt o f i nal . El esl abn di buj ado
l l evar
% l os ngul os de i ncl i naci n i ndi cados por el usuar i o en l a
vent ana
% pr i nci pal :
% [ xf yf zf ] =t r asl ac( xi , yi , zi )
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

ht ext ot 1 num_si mb_r ot Q1_si mb Q1 t r as

syms a posi t i ve %nombr e de var i abl e de t r asl aci on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Cal cul amos el n de el ement os de t r asl aci on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

i f t r as>1 %n de el ement os de t r asl aci on i gual a t r as- 1
t r as2=t r as- 1;
el se t r as2=t r as;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Obt enemos dat os de var i abl e
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%

num_si mb_r ot =num_si mb_r ot +1; %Aadi mos un el ement o ms de
t r asl aci n
l =st r 2num( get ( ht ext o, ' St r i ng' ) ) ; %l ongi t ud i ni ci al del t r amos
a=get ( ht ext ot 1, ' St r i ng' ) ; %nombr e de var i abl e ( t r asl aci on)
Q1_si mb( num_si mb_r ot ) =a; %mat r i z de nombr e de var i abl es
L=sym( a) ; %Longi t ud

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r adi anes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%
r espect o

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

xf =xi ;
yf =yi ;
zf =zi +L;

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


[ xf 1 yf 1 zf 1] =r ot ar ( xi , yi , zi , xf , yf , zf , angx, angz) ;
xf =subs( xf 1, Q1( t r as2) ) ; %Val or r eal de xf , yf , zf
yf =subs( yf 1, Q1( t r as2) ) ;
zf =subs( zf 1, Q1( t r as2) ) ;

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

i f ange
end

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

x1=subs( x1, Q1( t r as2) ) ;
x2=subs( x2, Q1( t r as2) ) ;
x3=subs( x3, Q1( t r as2) ) ;
x4=subs( x4, Q1( t r as2) ) ;
x5=subs( x5, Q1( t r as2) ) ;
x6=subs( x6, Q1( t r as2) ) ;
x7=subs( x7, Q1( t r as2) ) ;
x8=subs( x8, Q1( t r as2) ) ;
y1=subs( y1, Q1( t r as2) ) ;
y2=subs( y2, Q1( t r as2) ) ;
y3=subs( y3, Q1( t r as2) ) ;

y4=subs( y4, Q1( t r as2) ) ;
y5=subs( y5, Q1( t r as2) ) ;
y6=subs( y6, Q1( t r as2) ) ;
y7=subs( y7, Q1( t r as2) ) ;
y8=subs( y8, Q1( t r as2) ) ;
z1=subs( z1, Q1( t r as2) ) ;
z2=subs( z2, Q1( t r as2) ) ;
z3=subs( z3, Q1( t r as2) ) ;
z4=subs( z4, Q1( t r as2) ) ;
z5=subs( z5, Q1( t r as2) ) ;
z6=subs( z6, Q1( t r as2) ) ;
z7=subs( z7, Q1( t r as2) ) ;
z8=subs( z8, Q1( t r as2) ) ;

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

z7; x8 y8 z8] ;
car as=[ 1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8] ;
subpl ot ( sp1)
pat ch( ' Ver t i ces' , ver t i ces, ' Faces' , car as, ' FaceVer t exCDat a' , [ . 4, . 4,
. 125] , ' FaceCol or ' , ' f l at ' ) ;
r et ur n

Memoria Final

Загружено:

Сведения о документе

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Memoria Final

Загружено:

Авторское право:

Доступные форматы

Diseo de una Plataforma Software Interactiva para la Simulacin Cinemtica de

Robots Manipuladores en entorno MATLAB

Вам также может понравиться