Professional Documents
Culture Documents
Facultad de Ingeniera
Escuela de Ingeniera Elctrica
Por:
Por:
David Cuenca Alpzar
_________________________________
Ing. Federico Ruiz Ugalde, Lic.
Profesor Gua
_________________________________ _________________________________
Ing. Francisco Siles Canales, Lic. Ing. Andrs Daz Soto
Profesor lector Profesor lector
ii
DEDICATORIA
A mi familia y amigos.
iii
NDICE GENERAL
iv
CAPTULO 3: La robtica .........................................................................30
3.1 Las bases de los robots................................................................................................30
3.2 El brazo robot..............................................................................................................31
3.2.1 El brazo robot Stubli RX90.............................................................................32
3.2.2 Geometra de brazo robot Stubli RX90...........................................................33
CAPTULO 4: Desarrollo de la aplicacin de simulacin ........................36
4.1 Simulador del brazo robot Stubli RX90 L ................................................................38
4.1.1 El brazo robot Stubli como un modelo jerrquico ..........................................39
4.1.2 El modelado del brazo robot Stubli RX90 L...................................................42
4.1.3 El mtodo de deteccin de colisiones ...............................................................51
CAPTULO 5: La interfaz grfica de usuario de la aplicacin ................64
5.1 Los botones de configuracin del simulador .............................................................68
5.2 Creacin, edicin y ejecucin de rutinas ....................................................................70
5.3 Envo de ordenes al simulador por medio del teclado y el ratn................................72
CAPITULO 6: Pruebas finales y sus resultados........................................74
6.1 Pruebas destructivas....................................................................................................74
6.2 Pruebas de velocidad ..................................................................................................74
6.3 Pruebas de memoria....................................................................................................77
CAPITULO 7: Conclusiones y Recomendaciones .....................................78
7.1 Conclusiones...............................................................................................................78
7.2 Recomendaciones .......................................................................................................80
BIBLIOGRAFA.........................................................................................81
APNDICES ...............................................................................................82
v
NDICE DE FIGURAS
vi
Figura 4.12 Prueba de colisin de orillas (1) ....................................................................59
Figura 4.13 Prueba de colisin de orillas (2) ....................................................................59
Figura 4.14 Prueba de colisin de orillas (3) ....................................................................60
Figura 5.1 La aplicacin grfica de usuario del simulador...............................................64
Figura 5.2 Los botones de configuracin del simulador...................................................68
Figura 5.3 Introduccin de una base externa ....................................................................69
Figura 5.4 Introduccin del margen de error ....................................................................70
Figura 5.5 Ventana 1 de la aplicacin...............................................................................70
vii
NDICE DE CUADROS
viii
GLOSARIO
ix
RGBA: RGBA es el termino utilizado para definir el modelo de color, en
el cual los colores primarios (rojo, verde y azul: <<Red Blue
Green>>) son combinados en forma especfica con el fin de
obtener algn color determinado. La A en RGBA se refiere al
termino alfa, que define la transparencia de los colores.
x
RESUMEN
La creacin de un modelo tridimensional y un simulador virtual del brazo robot
Stubli RX90 L es el objetivo principal de este proyecto. Para el desarrollo del programa de
simulacin se emple OpenGL como herramienta principal de trabajo. El cdigo de la
aplicacin fue desarrollado en su totalidad en el lenguaje de programacin C.
La primera fase del proyecto la comprendi el estudio de la teora de modelado
tridimensional en computadoras. Con ayuda de la informacin obtenida durante esta etapa,
se comenz la bsqueda y anlisis de las herramientas necesarias para el desarrollo de la
aplicacin.
Se eligi la interfaz de programacin de aplicaciones grficas OpenGL, como la
librera principal para la creacin del programa de simulacin virtual. Se estudi y comenz
a experimentar con las funciones brindadas por la librera. A partir de este punto, en el cual
ya se tena un mejor conocimiento de la teora de modelado y de las herramientas al
alcance, se dio inicio a la estructuracin de la aplicacin a desarrollar.
El siguiente paso fue analizar la geometra del brazo robot Stubli RX90 L, con el
fin de determinar cuales caractersticas geomtricas de ste son las ms influyentes en los
procesos de colisin. Despus del estudio del objeto real, se procedi a crear un modelo
virtual tridimensional de ste.
A partir de este punto se establecieron dos objetivos principales: la creacin de un
motor de ejecucin de espacio virtual, en el cual se pudiera manejar el modelo del brazo
robot Stubli y la implementacin de un mtodo de deteccin de colisiones al motor de
ejecucin.
Para alcanzar el segundo objetivo se tuvo que desarrollar un mtodo nuevo de
deteccin de colisiones, el cual result ser, adems de altamente efectivo y flexible,
verdaderamente simple en comparacin con otros mtodos existentes.
Una vez creados el motor de ejecucin y los algoritmos de deteccin de colisiones,
se inici la estructuracin de una interfaz grfica de usuario, que no solo facilitara el uso de
la aplicacin, sino que permitiera una futura unin con otras aplicaciones, en especial con
otros programas desarrollados para el mismo brazo robot. La interfaz se desarroll
utilizando la librera GTK.
Una vez finalizada la aplicacin, se llev a cabo una serie de pruebas mediante las
cuales se descubrieron algunos problemas, solucionados posteriormente.
Se logr cumplir con el objetivo de crear una aplicacin de simulacin para el
sistema operativo GNU/Linux. La librera grfica OpenGL y el lenguaje de programacin
C demostraron poseer las cualidades necesarias para el desarrollo de aplicaciones grficas
complejas. Los mtodos de deteccin de colisiones implementados en el programa,
mostraron un buen funcionamiento durante la etapa de prueba.
xi
CAPTULO 1: Introduccin
1.1 Justificacin
El campo de la robtica se encuentra en constante crecimiento. Los robots se han
convertido en asistentes imprescindibles en muchas reas. Se encargan de tareas que
pueden ser tediosas, pesadas, difciles e inclusive imposibles para un ser humano.
Los avances tecnolgicos de los ltimos aos han permitido que los sistemas
electromecnicos controlables puedan ser aplicados a diferentes reas. Desde la industria
del entretenimiento, hasta la exploracin espacial, desde la industria automotriz, hasta las
ciencias mdicas, los robots afectan nuestras vidas muchas veces sin darnos cuenta.
Una parte imprescindible en el desarrollo de nuevas tecnologas robticas y la
implementacin de stas, es el modelado y la simulacin. Saber como se comportar el
sistema ante diferentes situaciones es indispensable tanto para el diseador como para el
usuario. Mediante el uso de simuladores, los desarrolladores pueden detectar errores y
corregirlos antes del proceso de fabricacin. Por otro lado los usuarios pueden utilizar
modelos virtuales del equipo para verificar previamente si las rdenes que se enviarn
tendrn el resultado deseado. Simular con anterioridad los movimientos que se le ordenarn
a un robot, incrementa no solo la eficiencia del proceso, sino que tambin ayuda a
resguardar la seguridad de los usuarios y del robot mismo, ya que mediante la
representacin virtual se puede determinar si la ejecucin de cierta orden por parte del
equipo, presenta algn riesgo para las personas alrededor o para el equipo mismo.
El uso de sistemas operativos propietarios no solo encarece el proceso de desarrollo
de aplicaciones, sino que tambin resta flexibilidad al producto final, ya que muchas de las
herramientas y aplicaciones desarrolladas sobre sistemas propietarios no pueden ser
implementadas en otros sistemas. El hecho de utilizar una plataforma abierta, para crear
modelos virtuales de equipos electromecnicos, presenta ventajas tanto para el
desarrollador como para el usuario.
Las observaciones anteriores representan algunas de las razones para el desarrollo
de este proyecto. La creacin de un motor de ejecucin y un modelo tridimensional del
brazo robot Stubli RX90 L, adems de la implementacin de un mtodo de deteccin de
colisiones, ayudar a incrementar la eficiencia y seguridad de los movimientos del brazo
robot. Adems, la utilizacin del sistema operativo GNU/Linux y la biblioteca grfica de
cdigo abierto OpenGL, como bases del simulador virtual, incrementa la versatilidad y
estabilidad de la aplicacin y permite que el cdigo desarrollado pueda ser portado, sin
mayor complicacin, a proyectos futuros e implementado en otras aplicaciones.
Aun cuando el objetivo especfico de este proyecto sea la creacin de un simulador
virtual del brazo Robot Stubli RX90 L, el trabajo aqu desarrollado podra utilizarse como
base para crear modelos virtuales de otros sistemas elctricos controlables.
1
2
1.2 Objetivos
1.3 Metodologa
GNU/Linux o Linux son los nombres que suele recibir el sistema operativo de
distribucin libre y cdigo abierto desarrollado en un principio por Linus Torvalds a
principio de los aos noventa (1991) y el proyecto GNU fundado por Richard Stallmann en
1983. El trmino Linux se refiere estrictamente al ncleo o kernel creado en un principio
por Linus Torvalds y GNU/Linux al sistema operativo final, que utiliza bibliotecas y
herramientas desarrolladas por el proyecto GNU y muchos otros proyectos y grupos de
software. La palabra Linux tambin es usada comnmente para referirse a las diferentes
distribuciones Linux, las cuales son colecciones de software que suelen contener grandes
cantidades de paquetes adems del ncleo.
La coleccin de utilidades para la programacin de GNU es por mucho la familia de
compiladores ms utilizada en Linux. Tiene la capacidad de compilar C, C++, Java, Ada
entre muchos otros lenguajes. Existen varios ambientes integrados de desarrollo disponibles
para Linux, entre ellos estn Anjuta, Kdevelop, NetBeans IDE y Eclipse.
Aunque originalmente Linux fue diseado solamente para soportar procesadores
Intel 80386, actualmente soporta una gran variedad de arquitecturas, convirtindose en uno
de los sistemas operativos con mayor portabilidad.
El soporte tcnico para usuarios de Linux normalmente se provee a travs de foros
en lnea, grupos de noticias y listas de correos electrnicos. Los grupos de usuarios Linux
LUGs (por sus siglas en ingls: Linux Users Groups) tradicionalmente prestan soporte
local para los usuarios de Linux y para las personas que desean introducirse en el mundo
del software libre.
Su alta eficiencia, gran portabilidad, variada gama de software libre compatible y
muchas otras caractersticas, hacen que el sistema Linux represente una gran plataforma de
investigacin y desarrollo de aplicaciones.
Al ser un sistema operativo de cdigo abierto, el sistema Linux ha sufrido un
crecimiento acelerado. Programadores provenientes de diferentes lugares del mundo se han
unido al desarrollo y mejoramiento del sistema y las aplicaciones que este soporta. Para el
desarrollo del kernel del sistema Linux se utiliza el lenguaje de programacin C.
4
5
puede ser utilizada sobre diferentes sistemas operativos, como por ejemplo Windows, Mac
OS-X, PlayStation 3, Linux y otros sistemas en base UNIX. Las funciones de OpenGL
pueden ser llamadas desde varios lenguajes, entre ellos C/C++, FORTRAN, Ada y Java.
El API OpenGL, comenz como una iniciativa de la empresa Silicon Graphics para
crear una sola interfaz de programacin grfica, independiente de desarrolladores de
hardware. Antes de la introduccin de OpenGL, muchos vendedores de hardware
utilizaban diferentes libreras grficas, lo cual encareca el proceso de crear programas
compatibles con diferentes plataformas de hardware. Esto llevo a Silicon Graphics a crear
OpenGL, aplicacin cuya base original es la biblioteca grfica IRIS. OpenGL empez
como una especificacin, luego Silicon Graphics cre una implementacin, que poda se
utilizada por los creadores de hardware para desarrollar sus propias implementaciones. Los
desarrolladores de software no necesitan una licencia para utilizar OpenGL en sus
aplicaciones, mientras que los desarrolladores de hardware si deben adquirir una licencia
para poder crear una implementacin de OpenGL. Por ejemplo, Mesa 3D es un API cuyo
cdigo es compatible con OpenGL, sin embargo para evitar el pago de licencias por
creacin de una implementacin de OpenGL, no es llamada como una implementacin
directa de OpenGL, sino como un API muy similar.
Una de la caractersticas ms interesantes de OpenGL es que utiliza un mecanismo
de servidor y cliente para procesar los grficos. El cliente grfico utiliza OpenGL y el
sistema operativo para trasmitir las primitivas grficas, coordenadas de vrtices, colores y
texturas al servidor. En el servidor se utiliza la informacin obtenida para generar los
pxeles, los cuales luego son sometidos a las pruebas de mapeo de texturas, pruebas de
profundidad, pruebas de unin de colores , entre otras.
Aunque normalmente el cliente y el servidor se encuentran en la misma
computadora, la ventaja de la separacin del proceso es que hace posible que una mquina
de bajo costo transmita, a travs de una red, comandos de OpenGL a una mquina ms
costosa y de mayor eficiencia, sobre la cual corre el proceso del servidor, luego las
imgenes finales pueden ser devueltas a la mquina de menor eficiencia para su despliegue.
OpenGL se puede manejar bajo dos tipos de modos, modo inmediato en el cual una
aplicacin enva un comando a OpenGL y sta los ejecuta inmediatamente y el modo de
retencin, en el cual secuencias de comandos grficos son almacenadas en estructuras de
datos conocidas como lista de despliegue o listas de exhibicin. El uso de estas listas
presenta dos ventajas importantes. Si se debe graficar un objeto complicado de manera
frecuente, solamente es necesario referirse a la lista que posee la informacin del objeto,
adems al estar la informacin de objeto en una lista, sta se puede enviar rpidamente a
travs de una red. La desventaja de las listas de exhibicin es que si un objeto est
destinado a sufrir modificaciones frecuentemente, se deben generar nuevas descripciones
del objeto de igual manera.
OpenGL elimina la complejidad que representa para el usuario interactuar con
diferentes tarjetas grficas, presentando al programador una interfaz nica y uniforme para
7
Portabilidad:
Por el momento Direct3D solo se ha implementado en sistemas operativos de la
familia Microsoft, incluyendo las versiones que se han utilizado en las consolas de
videojuegos XBox. Algunas funciones del API Direct3D han sido implementadas en el
proyecto Wine, el cual intenta trasladar APIs comnmente utilizadas en Windows a Linux,
pero el trabajo ha sido difcil debido a la dependencia entre DirectX y otros componentes
del sistema Windows.
1
Fuentes: Artculo: Comparison of Direct3D and OpenGL, http://www.wikipedia.org
Artculo: Direct3D vs. OpenGL, http://www.gamedev.net
10
Facilidad de manejo:
Antes de la versin 8, Direct3D era conocido por ser un tanto difcil de manejar, por
ejemplo para realizar un cambio de estado se requera llevar a cabo un numero de
operaciones complicadas. Por ejemplo para habilitar la combinacin de colores conocida
como <<alpha blending>>, se tenia que crear un <<buffer>> o almacenador intermedio
llamado <<buffer>> de ejecucin, amarrarlo, llenarlo con los cdigos de operacin
correctos, junto con un encabezado estructural sealando cuntos cdigos de operacin
contena el <<buffer>> y un cdigo de operacin especial de salida, liberarlo y finalmente
enviarlo al controlador de la tarjeta de video para su ejecucin. Tal vez la queja mas famosa
fue entablada por el famoso desarrollador de videojuegos John Carmack en el archivo
.plan; l recomendaba a Microsoft abandonar Direct3D y utilizar OpenGL.
Sin embargo se produjeron muchos cambios en la versin Direct3D 8, los cuales
ayudaron a mejorar de manera notoria la imagen de Direct3D. Aun as Direct3D y OpenGL
son guiados por paradigmas distintos.
Direct3D est construido sobre el modelo de objetos COM de Microsoft, lo cual
indica que el uso de codigo C++ es un tanto inusual. Las funciones para adquirir valores no
devuelven el valor en el argumento de retorno, ya que todas las funciones COM devuelven
un HRESULT que dice si la funcin se ejecut correctamente o no. El lado positivo de usar
el modelo COM, es que se puede utilizar la misma interfaz en cualquier lenguaje con
arquitectura COM, como por ejemplo Visual Basic y Visual Basic Script, entre otros.
OpenGL es una especificacin basada en el lenguaje de programacin C. Fue
desarrollada utilizando el concepto de una mquina de estados finitos, aunque las ltimas
versiones de OpenGL lo han transformado ms en un sistema basado en objetos. Aunque la
especificacin esta construida en C, tambin se puede implementar en otros lenguajes.
En general Direct3D esta diseada para ser una interfaz de hardware 3D. Su
desarrollo depende del desarrollo del hardware y lo que el hardware puede proveer. Por
otro lado OpenGL esta diseado para ser un sistema de interpretacin 3D que puede poseer
aceleracin de hardware, como tal, su desarrollo se deriva de todo aquello que es
considerado como til por el usuario. Direct3D espera que la aplicacin maneje los
recursos del hardware, mientras que OpenGL crea la implementacin para hacerlo. Esto
facilita al usuario el hecho de escribir una aplicacin vlida, pero lo deja ms susceptible a
implementar errores. Al mismo tiempo, como OpenGL esconde los detalles del hardware,
11
incluyendo el hecho de si el hardware esta siendo utilizado o no, el usuario debe confiar en
que la implementacin esta utilizando los mejores recursos del hardware.
Desempeo:
Despus de que ambos APIs se establecieron como libreras grficas viables,
Microsoft y SGI comenzaron lo que se ha llamado la guerra de las APIs. La mayor parte de
la disputa giraba en torno a cual interfaz ofreca un mejor desempeo.
En general se ha logrado establecer que ninguna de las dos APIs es superior a la
otra en cuanto a velocidad. El desempeo de una aplicacin depende de la habilidad del
programador, la calidad de los controladores (<<drivers>>) y del hardware de grficos.
Generalmente se desea transformar los vrtices de los objetos que se van a dibujar,
por ejemplo trasladarlos, rotarlos con respecto a un eje e incluso escalarlos para cambiar el
tamao del objeto. Estos procesos de transformacin, conocidos como transformaciones
afines, se llevan a cabo utilizando matrices homogneas. Para poder utilizar estas matrices
de transformacin, los puntos y vectores deben estar descritos utilizando coordenadas
homogneas en lugar de coordenadas cartesianas. Las coordenadas homogneas impiden
que una transformacin acte de igual manera sobre un punto que sobre un vector, lo cual,
si llegara a pasar, sera un error que alterara de forma inesperada el objeto dibujado.
Por tales razones las coordenadas homogneas se utilizan para crear aplicaciones
grficas.
La posicin del punto A en la figura 2.2 est definida por las coordenadas
homogneas A(x,y,z,1), estas coordenadas son con respecto al sistema de coordenadas
globales o sistema Worldspace. Para poder graficar el punto A en la pantalla es necesario
determinar sus coordenadas respecto al sistema de coordenadas de la cmara, ya que este es
el sistema fsico en el cual se dibujar el punto.
Los primeros tres vectores columna de la matriz A definen la base del sistema de
coordenadas, es decir la direccin y magnitud de los vectores x, y, z base. La cuarta
columna define el desplazamiento del sistema de coordenadas, en otras palabras, la
ubicacin del punto de origen del sistema de coordenadas. Al igual que se explic antes
para las coordenadas homogneas, el cuarto valor de cada columna define si se est
describiendo un punto o un vector. Se utiliza un cero para indicar que los valores de la
columna pertenecen a un vector y un uno para indicar que los valores pertenecen a un
punto.
Por ejemplo la base cannica estara representada por la matriz B, la cual a su vez es
la matriz identidad.
1 0 0 0
0 1 0 0
B=
0 0 1 0
0 0 0 1
1 0 0 3 1 0 0 0 1 0 0 3
0 1 0 4 0 1 0 0 0 1 0 4
C = W = O=
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
Para la figura 2.4, si se multiplica el vector de coordenadas (x,y,z,0) del punto P, las
cuales son con respecto al sistema Objeto, por la matriz O, la asociada al sistema Objeto, se
obtienen las coordenadas del punto P respecto al sistema Worldspace. Es importante
recordar que se utilizan coordenadas homogneas y no cartesianas, por lo que se le debe
agregar un uno como cuarto elemento al vector de coordenadas , para indicar que lo que
se est definiendo es un punto y no un vector . El vector P representa las coordenadas del
punto P respecto al sistema Worldspace.
1 0 0 3 2 5
0 1 0 4 4 8
P'= O P = =
0 0 1 0 0 0
0 0 0 1 1 1
16
En la matriz genrica de traslacin, el cuarto vector columna (tx, ty, tz) representa
los valores de traslacin sobre el eje X, el eje Y y el eje Z respectivamente. Ntese que la
matriz T se puede denominar como la matriz asociada a un sistema de coordenadas con
origen en el punto (tx, ty, tz).
1 0 0 0
0 cos( ) sin( ) 0
Matriz genrica de rotacin respecto al eje X R X =
0 sin( ) cos( ) 0
0 1
0 0
cos( ) 0 sin( ) 0
0 1 0 0
Matriz genrica de rotacin respecto al eje Y RY =
sin( ) 0 cos( ) 0
0 1
0 0
cos( ) sin( ) 0 0
sin( ) cos( ) 0 0
Matriz genrica de rotacin respecto al eje Z RZ =
0 0 1 0
0 0 1
0
sX 0 0 0
0 sY 0 0
Matriz genrica de escalamiento S =
0 0 sZ 0
0 1
0 0
18
1
Figura tomada de la gua libre de programacin OpenGL Red Book, http://www.glprogramming.com/red
19
A la entrada est el objeto geomtrico que se desea dibujar, el cual est compuesto
por primitivas geomtricas como puntos y lneas. Estos objetos poseen atributos
previamente establecidos, los cuales pueden ser mviles o fijos, por lo que debe existir la
posibilidad de trasladarlos, rotarlos y escalarlos si es necesario, antes de que sean dibujados
en la pantalla.
Durante el proceso de transformacin del modelo, se rotan, trasladan y escalan los
objetos geomtricos , de manera que se dibujen en la pantalla igual que se dispuso en el
mundo que se desea crear. Para lograr esto se multiplican los vrtices de los objetos por las
matrices de rotacin , traslacin y escalamiento que sean necesarias.
Una vez realizadas las transformaciones de los vrtices, se obtienen las coordenadas
de los objetos con respecto al mundo que se est creando, estas coordenadas son las
coordenadas del mundo o coordenadas globales.
El siguiente paso en el pipeline es el de iluminar los objetos para que sean visibles a
la cmara (pantalla) y determinar las coordenadas de los objetos respecto a la cmara. Este
proceso se llama transformacin del visionado. Luego de haberlo realizado se sabe cuales
son las coordenadas de todos los objetos con respecto a la cmara.
El prximo paso es recortar todo aquello que existe pero no es visible para la
cmara. Este proceso se conoce como <<clipping>>.
Durante el proceso de proyeccin se pasan las coordenadas tridimensionales del
mundo que se desea dibujar a coordenadas bidimensionales del plano de proyeccin.
Una vez proyectado el grfico se tienen las coordenadas de pantalla independientes
del dispositivo o DISC por sus siglas en ingles (<<Device Independent Screen
Coordinates>>). A estas coordenadas se les llama independientes del dispositivo, ya que
aun no se encuentran ligadas a ningn tipo de monitor, es decir todava no se sabe la
resolucin de la pantalla ni el tamao del monitor.
Finalmente se lleva acabo el proceso de rasterizacin el cual consiste en asociar
todos los puntos de la imagen proyectada a pxeles en la pantalla, obteniendo de esta
manera la imagen final en el monitor.
El pipeline grfico puede ser implementado a travs de hardware o software. La
implementacin a travs de software produce que todo el proceso sea ms lento a menos de
que se cuente con tarjetas de aceleracin grfica.
22
#include <gtk/gtk.h>
Todos los programas que utilicen GTK deben incluir el archivo gtk.h, en el cual
se declaran las variables, funciones, estructuras, etc, que sern utilizadas por la aplicacin
GTK. La funcin gtk_init(gint *argc, gchar ***argv) es la encargada de inicializar GTK en
la aplicacin. Las interfaces grficas estn compuestas por objetos llamados <<widgets>>.
Un <<widget>>, tambin conocido como artilugio o control, es un componente grfico con
el cual el usuario interacta, como por ejemplo, una ventana, una barra de tareas, un botn
o una caja de texto. En la aplicaciones GTK los <<widgets>> son definidos como
GtkWidget, por ejemplo GtkWidget *ventana.
En la lnea ventana = gtk_window_new (GTK_WINDOW_TOPLEVEL) del ejemplo
anterior, se define el <<widget>> ventana, como un nueva ventana GTK. El argumento
que fue pasado a la funcin indica que se desea que la ventana acepte las caractersticas de
decoracin y posicin dadas por el controlador de ventanas del sistema operativo. En vez
de crearse una ventana de tamao 0 x 0, se establece una ventana de tamao 200 x 200. Por
supuesto estas dimensiones son argumentos predeterminados que pueden ser variados
posteriormente a gusto del desarrollador.
La funcin gtk_widget_show (GtkWidget *) le indica a Gtk que ya se establecieron
todos los atributos que se deseaban para el <<widget>> y que ste ya puede ser desplegado.
Al igual que las aplicaciones GLUT, todos los programas GTK poseen una estructura de
lazo infinito, en el cual se est a la espera de que suceda algn evento. En GTK el lazo
principal es representado por la funcin gtk_main(). En el cdigo anterior no se estableci
ninguna funcin de respuesta a eventos, por lo que cualquier evento ser ignorado.
En aplicaciones GTK las funciones de respuesta se conectan a los eventos de la
siguiente manera: g_signal_connect (G_OBJECT (GtkObj), "evento", G_CALLBACK
(func_respuesta), argumento), en donde GtkObj es el objeto del tipo GTK, por ejemplo una
ventana o un botn, en el cual se di el evento, func_respuesta es la funcin que debe
llamarse como respuesta al evento y argumento representa posibles argumentos que se le
pueden pasar a la funcin de respuesta.
glBegin() indica el tipo de primitiva que se utilizar para unir los vectores. Algunos tipos
soportados por la librera OpenGL son: GL_LINES, dibuja una lnea cada dos vrtices,
GL_POINTS, dibuja un punto en cada vrtice, GL_POLYGON, dibuja un polgono. Los
polgonos deben ser convexos. La funcin glColor*(), especifica el color que OpenGL
utiliza para dibujar las figuras, el color no cambia hasta que se vuelva a llamar la funcin
glColor*().
Para el cdigo que se presenta a continuacin , dependiendo de si la variable
PRIMITIVA se sustituye por el argumento GL_POLYGON o GL_POINTS se obtendr
uno de los objetos presentados en la figura 2.8.
...
glBegin(PRIMITIVA); /*Determina el inicio de un conjunto de vrtices y el tipo de
primitiva que los une*/
glVertex2f(0.0, 0.0); /*Posicin del vrtice 1*/
glVertex2f(0.0, 3.0); /*Posicin del vrtice 2*/
glVertex2f(4.0, 3.0);
glVertex2f(6.0, 1.5);
glVertex2f(4.0, 0.0);
glEnd(); /*Determina el fin de un conjunto de vrtices*/
...
La librera GLUT posee una serie de rutinas que pueden ser utilizadas para crear
objetos tridimensionales especficos. Por ejemplo glutWireSphere() dibujar una esfera
compuesta por lneas, glutSolidCube(), dibuja un cubo, entre otros. Las dimensiones de
estos objetos se especifican en los argumentos de la funcin.
matrices que se han ido acumulando. Los vrtices que son procesados se multiplican por
esta matriz y por lo tanto son transformados. En OpenGL la CTM est compuesta por la
multiplicacin de dos matrices, la matriz Model-View o matriz de transformacin del
modelo y la matriz de proyeccin. La primera se encarga de las transformaciones que se
llevarn a cabo y la segunda representa la proyeccin sobre el plano bidimensional, es decir
el paso del mundo 3D al mundo 2D.
geometra que se desean transformar y luego recuperar el estado de la pila, sin que el resto
de la geometra se vea alterada por las transformaciones que se llevaron a cabo.
/* Ejemplo que muestra como se pueden usar las funciones de la pila (Model-View) para transformar solo
algunas partes de la geometra */
...
dibujo_parte1(){...} /*se define alguna forma geomtrica*/
dibujo_parte2(){...} /*se define alguna forma geomtrica*/
...
glTranslatef... /*afecta a toda la geometra que se dibuje a partir de ahora*/
glRotatef... /*afecta a toda la geometra que se dibuje a partir de ahora*/
glPushMatrix(); /*salva el estado actual de la matriz, es decir las dos transformaciones anteriores*/
glTranslatef... /*afecta solo la geometra que se dibuje antes del prximo glPopMatrix()*/
glRotatef... /*afecta solo la geometra que se dibuje antes del prximo glPopMatrix()*/
dibujo_parte1(); /*geometra que se ve afectada por 4 transformaciones*/
glPopMatrix(); /*se recupera el estado anterior de la matriz*/
dibujo_parte2(); /*geometra que se ve afectada por solo 4 transformaciones*/
...
void reshape (int w, int h) /*funcin que se llama cuando la ventana se crea o cambia de tamao*/
{
glViewport (0, 0, (GLsizei) w, (GLsizei) h); /*tamao del cuadro de visin*/
glMatrixMode (GL_PROJECTION); /*activa la matriz de proyeccin*/
glLoadIdentity (); /*la reinicia*/
gluPerspective(60.0, (GLfloat) w/(GLfloat) h, 1.0, 20.0); /*determina la perspectiva*/
glMatrixMode(GL_MODELVIEW); /*activa la matriz de transformacin*/
glLoadIdentity(); /*la reinicia*/
gluLookAt (0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0); /*indica la posicin del observador*/
}
1
Figura tomada de la gua libre de programacin OpenGL Red Book
29
1
Figura tomada de la gua libre de programacin OpenGL Red Book
CAPTULO 3: La robtica
Desde un punto de vista muy bsico y tcnico, los seres humanos estn compuestos
por cinco componentes principales:
Una estructura corporal.
Un sistema muscular que mueve la estructura corporal.
Un sistema sensorial que recibe informacin del cuerpo y el ambiente que lo
rodea.
Una fuente de poder para activar los msculos y sensores.
Un cerebro que procesa la informacin sensorial y le indica a los msculos qu
hacer.
Por supuesto que los humanos tambin poseen atributos intangibles tales como la
inteligencia y la moral, entre muchos otros. Pero si se analiza desde un nivel simplemente
fsico y tcnico, los cinco atributos antes mencionados bastan para describir de manera
general el cuerpo humano. Un robot est compuesto por los mismos componentes. Los
robots tpicos poseen una estructura fsica mvil, un motor de algn tipo, un sistema
sensorial, una fuente de poder y una computadora que acta como el cerebro y que
controla todos los componentes anteriores.
30
31
1
Figura tomada de las hojas del fabricante, http://www.staubli.com/web/robot/division.nsf
34
Las articulaciones del brazo robot poseen amplitudes de giro mximas, adems de
valores mximos y nominales para las velocidades angulares, los cuales se muestran en el
cuadro 3.1. La numeracin de las articulaciones en el cuadro 3.1, est basada en la
numeracin que recibieron en la figura 3.2.
1
Figura tomada de las hojas del fabricante, http://www.staubli.com/web/robot/division.nsf
35
Articulacin 1 2 3 4 5 6
Distribucin A B C D E F
de la amplitud 160 137.5 142.5 270 +120 270
de giro () -105
1
Datos y figuras tomados de las hojas del fabricante, http://www.staubli.com/web/robot/division.nsf
CAPTULO 4: Desarrollo de la aplicacin de simulacin
36
37
Para el desarrollo del motor de ejecucin del modelo virtual, se utilizaron las
bibliotecas OpenGL y GLU, luego se creo una interfaz grfica, dndole la posibilidad al
usuario de interactuar con los objetos y la configuracin del espacio virtual. En un principio
se emple la librera GLUT, para el desarrollo de la interfaz, pero dada la necesidad de una
interfaz ms compleja, GLUT tuvo que ser sustituida por la biblioteca GTK-2.0. En general
el programa final se puede dividir en dos secciones principales: la seccin desarrollada
utilizando OpenGL, la cual se encarga de graficar y manejar el espacio virtual y la seccin
desarrollada con GTK, en la cual se encuentra la interfaz grfica de usuario y se llevan a
cabo los procesos de comunicacin con el sistema de ventanas, que sean necesarios. Para
establecer una unin entre esta dos secciones y que fuera posible la comunicacin entre
ellas, se emple la librera GtkGLext, la cul es una extensin de GTK, que permite
desplegar y manejar aplicaciones grficas desarrolladas con OpenGL, desde interfaces
GTK. El simulador fue desarrollado en el lenguaje de programacin C, esta eleccin se
realiz con base en las caractersticas positivas propias del lenguaje y adems con el fin de
simplificar el acople entre la aplicacin aqu implementada y el programa en [4].
En la figura 4.2 se observa un modelo simple de un brazo robot con tres grados de
libertad, el cual est compuesto por tres piezas rgidas llamadas base, brazo1 y antebrazo.
La base puede girar alrededor del eje Y y su ngulo de giro es , brazo1 gira alrededor del
eje Z y su ngulo de giro es y el antebrazo gira alrededor del eje Z con un ngulo de giro
llamado . En la figura 4.2 se puede observar que cada parte del robot posee su propio
sistema de coordenadas en los cuales se encuentran definidos los ngulos de rotacin de
cada una de las partes. Adems es claro que la base est en el nivel de jerarqua ms alto. Si
la base rota, tambin rota el brazo1 y el antebrazo, en otras palabras la base es el padre del
brazo1 y el antebrazo, mientras que brazo1 y el antebrazo por su parte son hijos de la base.
Brazo1 es el padre del antebrazo, ya que si ste rota, el anterazo tambin sufre una rotacin.
40
Todas las transformaciones de traslacin o rotacin que sufren los padres, tambin se dan
en los hijos, pero las transformaciones ocurridas en los hijos no tienen porque ocurrir en los
padres necesariamente. Por ejemplo si brazo1 rota 30 grados, el antebrazo se ve obligado a
rotar 30 grados con respecto al origen de brazo1, pero si el antebrazo por su parte gira 50
grados, brazo1 no se ve obligado a sufrir ninguna rotacin. A esto se le conoce como un
modelo jerrquico. Una de las ventajas de definir un sistema de coordenadas para cada una
de las partes del robot es la siguiente, por ejemplo, si brazo1 de la figura 4.2 es rotado 20,
la posicin de antebrazo debe ser transformada para mantener la jerarqua del brazo robot,
de manera que las partes se encuentren unidas correctamente a la hora de desplegar el
modelo. Ahora bien, sera complicado tener que calcular las nuevas coordenadas que debe
tener el antebrazo en su propio sistema de coordenadas debido a la rotacin de brazo1. En
lugar lo que se hace es que se mantienen las mismas coordenadas que el antebrazo posee
con respecto a su propio sistema de coordenadas y se gira el sistema de coordenadas del
antebrazo 20 con respecto al origen del sistema de brazo1. Teniendo lo anterior en cuenta
y sabiendo que las transformaciones de rotacin y traslacin se llevan a cabo mediante la
multiplicacin de matrices, para graficar el modelo del robot de la figura 4.2 en donde la
base tiene una altura h1 y una rotacin de , brazo1 tiene una altura h2 y una rotacin de
y el antebrazo tiene una altura de h3 y una rotacin de , se llevara acabo el siguiente
proceso:
M_model = RotarY()*Trasladar(0,h1,0)*RotarZ();
M_model = RotarY()*Trasladar(0,h1,0)*RotarZ()*Trasladar(0,h2,0)*RotarZ();
En el proceso anterior se nota que es poco prctico tener que estar calculando la
matriz de transformacin actual cada vez que se produce una transformacin. En lugar de
recalcular la matriz global, sta se puede actualizar concatenando matrices a su derecha de
la siguiente manera:
Como se explico en el captulo dos, OpenGL mantiene una matriz global de estado
llamada matriz Model-View, la cual es actualizada concatenando matrices a su derecha cada
vez que se da una transformacin en el modelo. Utilizando funciones OpenGL el proceso
anterior se puede presentar de la siguiente manera:
42
En la figura 4.3 se muestra el primer modelo de un brazo robot, que se cre en este
proyecto. Con este modelo se lleg a comprender el funcionamiento de las figuras
tridimensionales con diferentes niveles de jerarqua y como desplegaralas y controlarlas
utilizando las funciones del API OpenGL. Este primer modelo de un brazo con tres grados
de libertad, fue la base y punto de partida para el proceso de creacin del modelo final del
brazo Stubli RX90 L.
El brazo robot Stubli RX90 L, posee seis grados de libertad y esta compuesto por
siete partes principales: la base, el hombro, el brazo (o brazo1), el codo, el antebrazo (o
brazo2), la mueca y la mano (ver captulo tres).
43
Para cada uno de los componentes del brazo se estudi y determin cuales de sus
propiedades geomtricas son las ms influyentes durante los procesos de colisin del robot,
ya sea con el piso o consigo mismo. Luego se modelaron los componentes utilizando
primitivas geomtricas, tomando como gua las medidas dadas por el fabricante del brazo
robot. Se utilizaron listas de despliegue, las cuales almacenan la informacin grfica de las
diferentes partes del robot. En total se crearon siete listas de despliegue, una por cada
componente principal del brazo robot.
1
Figura (modificada) tomada de las hojas del fabricante, http://www.staubli.com/web/robot/division.nsf
44
sistema de coordenadas es hijo del sistema que le precede. El orden de jerarqua de los
sistemas de coordenadas propios de los objetos es el siguiente: sistema Base, sistema
Hombro, sistema Brazo1, sistema Codo, sistema Brazo2, sistema Mueca, sistema Mano,
en donde el sistema Base es quien posee el mayor nivel de jerarqua entre todos ellos, en
otras palabras, el sistema Base es padre del resto de los sistemas de coordenadas propios de
los componentes del robot. El punto de origen del sistema Base coincide con el punto de
origen del sistema de coordenadas global de la escena o espacio virtual, de hecho como la
base del brazo no sufre ninguna transformacin, es decir no gira ni se traslada, entonces
ambos sistemas sern realmente el mismo sistema de coordenadas en todo momento.
A continuacin se presenta el cdigo de la funcin dibujar(), en la cul se da el
proceso de posicionamiento y despliegue de los diferentes objetos que componen la escena
virtual. En otras palabras, aqu es donde se giran y trasladan los sistemas de coordenadas
propios de cada componente y donde son llamadas las listas de despliegue, cada vez que
alguna de las partes principales del brazo robot recibe una orden de movimiento por parte
del usuario.
/*Funcin: dibujar().Se encarga de dibujar todo lo que desea desplegar en la escena. Archivo: dibujo.c. */
gboolean dibujar(void){
GdkGLContext *glcontext = gtk_widget_get_gl_context (glarea1);
GdkGLDrawable *gldrawable = gtk_widget_get_gl_drawable (glarea1);
//HOMBRO
glPushMatrix();
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
47
glCallList(HOMBRO);
glTranslatef(-13.0, 0.0, 0.0);
//BRAZO1
glPushMatrix();
glRotatef(angulo2, 1.0, 0.0, 0.0);
glCallList(BRAZO1);
glTranslatef(13.0, 45.0, 0.0);
//CODO
glPushMatrix();
glRotatef(angulo3, 1.0, 0.0, 0.0);
glCallList(CODO);
glTranslatef(0.0, 10.0, 0.0);
//BRAZO2
glPushMatrix();
glRotatef(angulo4, 0.0, 1.0, 0.0);
glCallList(BRAZO2);
glTranslatef(0.0, 55.0, 0.0);
//MUNIECA
glPushMatrix();
glRotatef(angulo5, 1.0, 0.0, 0.0);
glTranslatef(5.5, 0.0, 0.0);
glCallList(MUNIECA);
glTranslatef(-5.5, 0.0, 0.0);
//MANO
glPushMatrix();
glRotatef(angulo6, 0.0, 1.0, 0.0);
glCallList(MANO);
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
...
En ultimo fragmento del cdigo de la funcin dibujar() se llevan a cabo los ltimos
pasos necesarios para desplegar la escena en la ventana. Primero se determina si se desea
dibujar los lmites geomtricos del brazo. Estos lmites fueron creados durante el modelado
del brazo para llevar a cabo pruebas sobre la geometra del modelo con respecto a la del
brazo real, son una herramienta para el programador o modelador ms que para el usuario.
En la siguiente lnea de cdigo se llama a la funcin probar_colisin(), a la cual se le pasa
el argumento marg. Esta funcin es la encargada de realizar las diferentes pruebas de
49
colisin que posee el simulador. El argumento marg determina la magnitud del margen de
precaucin con el cual se llevan a cabo las pruebas. Una vez terminadas las pruebas de
colisin se llama a la funcin glFlush (), la cul exige la ejecucin de todos los comandos
OpenGL que han sido llamados hasta este punto. La funcin
gdk_gl_drawable_swap_buffers (gldrawable) es quien se encarga de intercambiar los
almacenadores intermedios de cuadro o <<frame buffers>>, en los cuales se recolecta todo
lo que se desea dibujar en la pantalla. Esto se debe llevar a cabo en los casos que se utiliza
el mtodo de doble <<buffer>>, el cul consiste en guardar toda la informacin grfica que
se desea desplegar en la pantalla en un <<buffer>> de despliegue secundario, el cal se
intercambia con el <<buffer>> de despliegue primario, es decir se convierte en el primario,
una vez que toda la informacin necesaria haya sido recopilada, de tal manera que nunca se
manipula la informacin del <<buffer>> primario durante el despliegue, ya que esto podra
causar que la animacin se parezca cortada y poco fluida.
La iluminacin es un aspecto sumamente importante en una escena y puede llegar a
ser difcil de dominar. Sin iluminacin, no se podra distinguir entre un objeto
bidimensional y uno tridimensional. Por ejemplo en la figura 4.5 se observa la misma
esfera en dos cuadros diferentes. En el primero hay iluminacin en la escena y en el
segundo no.
gboolean inicio(void){
GLfloat ambient [] = { 0.1, 0.1, 0.1, 0.0 };
GLfloat specular []= { 1.0, 1.0, 1.0, 1.0 };
GLfloat shininess [] = { 100.0 };
GLfloat position0 [] = { 400.0, 400.0, 400.0, 1.0 };
/* Determina la propiedad de reflejo del material*/
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, specular);
glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, shininess);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
En la figura 4.6 se observa el modelo final del brazo robot Stubli RX90 L, que se
desarroll en el proyecto.
El cuadro 4.1 enumera los diferentes puntos de control existentes y las figuras de
control que se encuentran ligadas a cada punto.
Figura(s) de control
Punto de control ligada(s) al punto de
control
CBA (Control Brazo A) cuadBA
CBB cuadBB
CBC cuadBC, circBC
CBD circBD
CWA (Control Wrist A) circWA
CWB circWB
CWC circWC
CHA (Control Hand A) circHA
CHB circHA
Nivel de jerarqua: 3
Pcoord. Global = tras(br1)*rotX(angulo2)*tras(cod)*rotX(angulo3)*Pcoord. Local
Nivel de jerarqua: 4
Pcoord. Global = tras(br1)*rotX(angulo2)*tras(cod)*rotX(angulo3)*tras(br2)
*rotY(angulo4)*Pcoord. Local
Nivel de jerarqua: 5
Pcoord. Global = tras(br1)*rotX(angulo2)*tras(cod)*rotX(angulo3)*tras(br2)
*rotY(angulo4)*tras(man)*Pcoord. Local
55
Para el punto de control CBC el cual se encuentra ligado a dos figuras de control, se
utiliza la figura cuadBC para llevar a cabo estas pruebas de invasin de zona de precaucin,
ya que cuadBC posee un radio mayor que circBC, la cual es la otra figura ligada al punto.
En el simulador se implementaron tres tipos de pruebas de deteccin de colisin: la
prueba de deteccin de colisin contra el piso, la prueba de deteccin de colisin contra la
base, las cuales ya fueron mencionadas anteriormente y la prueba de deteccin de colisin
contra las orillas del hombro.
Anterior a la realizacin de cada prueba se lleva a cabo una inicializacin de sta,
fase en la que se trasladan las coordenadas de cada uno de los puntos que componen la
figura de control que va a ser examinada, de su sistema de coordenadas propio o local al
sistema de coordenadas Global.
La prueba de deteccin de colisin contra el piso consiste en comparar el valor de la
coordenada global Y de cada uno de los vrtices que componen la figura de control en
evaluacin. Si alguno de estos puntos posee un valor de Y menor o igual a la suma de la
altura del piso ms el margen de precaucin, entonces se declara que hubo un choque del
brazo robot contra el suelo.
En la prueba de deteccin de colisin contra la base se monitorea la distancia
ortogonal de cada uno de los puntos de la figura de control examinada hasta el eje Y del
sistema de coordenadas Global, es decir el valor resultante de la ecuacin X 2 + Y 2 para
cada uno de los puntos. Si las coordenadas X y Y de alguno de los puntos conlleva a un
resultado menor o igual al radio de la base del brazo virtual ms el margen de precaucin
dado por el usuario, entonces se determina que hubo un choque del brazo contra la base.
Ntese que las dos pruebas explicadas anteriormente son una derivacin de la
popular prueba de colisin de la caja limtrofe (por su nombre en ingls: <<Bounding-Box
Collision Test>>), la cual es prueba de deteccin de colisin ms utilizada en el desarrollo
de juegos de videos y simuladores virtuales. Esta consiste en encerrar un objeto en una caja
no visible y determinar si algn otro objeto a invadido la caja.
58
Ntese que si se observa el modelo del brazo robot desde una perspectiva lateral, las
cuatro orillas de colisin del hombro, definidas anteriormente, aparecen como puntos en un
plano bidimensional de coordenadas Z, Y, tal y como se observa en la figura 4.13. Esta es
la idea principal detrs de la prueba de colisin de orillas. Las cuatro orillas del hombro se
convierten en puntos de dos dimensiones y la lnea de control entre las figuras de control
circBC y circBD se transforma en una lnea recta en el plano ZY, que puede ser descrita
por la ecuacin y = m z + b , en donde m es la pendiente de la lnea y b el valor de y
cuando la lnea interseca el eje Z.
En la figura 4.13 es posible percibir una segunda lnea recta, entre el punto de
origen inferior de la lnea de control y la orilla superior izquierda del hombro, esta lnea se
denomina lnea al hombro. El nivel Y1 indica la altura a la cual se encuentran las orillas
inferiores del hombro, mientras que el nivel Y2 es la altura de las orillas superiores. Si se
calculan las pendientes de ambas lneas y se comparan entre s es posible determinar si se
dio una colisin entre el antebrazo y la orilla del hombro.
Ntese en la figura 4.14 que si el origen de la lnea de control y de la lnea al
hombro posee un valor de Z negativo y un valor Y menor o igual al valor Y de las orillas
superiores del hombro, entonces se puede afirmar que se da una colisin en la esquina
superior izquierda si y solo si la pendiente de la lnea de control es menor o igual que la
pendiente de la lnea al hombro. Mientras que si el origen de las lnea de control y la lnea
al hombro tiene un valor de Z positivo y un valor de Y menor o igual al valor Y de las
orillas superiores del hombro, entonces se da una colisin en la esquina superior derecha si
y solo si la pendiente de la lnea de control es mayor que la pendiente de la lnea al hombro.
Para acceder al programa de simulacin desarrollado en este proyecto, ste debe ser
ejecutado desde la lnea de comandos, dentro de un entorno grfico que tenga soporte para
OpenGL, GLU, GTK-2.0 y GtkGLext. Una vez que el programa se est corriendo, el
simulador puede ser controlado directamente desde el emulador de terminal (la lnea de
comandos) o mediante la interfaz grfica de usuario GTK. Se recomienda utilizar siempre
la interfaz grfica en lugar de la lnea de comandos, ya que sta facilita el uso de la
aplicacin.
Desde la interfaz de usuario se pueden dar rdenes de movimiento, de creacin y
ejecucin de rutinas y de configuracin al simulador. La interfaz est compuesta por tres
ventanas principales. En la figura 5.1 se observan las tres ventanas de la aplicacin, adems
de la ventana de la terminal desde la cual se encuentra corriendo el programa. Se
recomienda mantener las cuatro ventanas visibles, de manera similar a la figura 5.1. A
pesar de que algunos mensajes son desplegados en el <<textview>> de la aplicacin GTK,
muchos mensajes de importancias tambin son transmitidos a travs de la terminal.
64
65
esfera de color verde indica que el movimiento es seguro y que el brazo no se encuentra
colisionado, la esfera amarilla seala que aunque no se est dando una colisin, al menos
un punto de control se localiza dentro de una de las reas de precaucin, por lo que el
choque puede estar cercano a ocurrir, finalmente la esfera roja indica que se est dando un
choque en al menos uno de los componentes del brazo.
Las ventanas grficas fueron programadas utilizando funciones de la librera gtk-
2.0. Se utilizaron objetos como botones, entradas de texto, etiquetas, etc, para formar la
interfaz de usuario la cual consta de tres ventanas, dos ventanas en donde se ubican los
botones y dems objetos para controlar el simular virtual y una tercer ventana en donde se
despliega la escena virtual, es decir el mundo virtual en el cual se encuentra el modelo de
brazo robot.
Las tres ventanas grficas son creadas en el momento en el que se inicia la
aplicacin. Las funciones encargadas de llamar y desplegar las ventanas se encuentran en el
archivo fuente de cdigo main.c.
gtk_main ();
cada objeto grfico que lo requiera, las funcin de respuesta a diferentes eventos, por
ejemplo, los botones de una ventana grfica deben tener funciones de respuesta que sean
llamadas cada vez que stos son presionados.
A continuacin se muestran algunos segmentos de cdigo que componen la funcin
encargada de crear la ventana grficas superior izquierda de la figura 5.1.
/*Se crea la ventana sobre la cual se colocaran los botones y dems objetos grficos*/
window1 = gtk_window_new (GTK_WINDOW_TOPLEVEL);
gtk_widget_set_name (window1, "window1");
gtk_container_set_border_width (GTK_CONTAINER (window1), 3);
gtk_window_set_title (GTK_WINDOW (window1), _("Controles del Simulador"));
/* Se agregan todos los objetos que configuran la ventana. Por ejemplo, a continuacin
se despliega la funcin que agregar el botn SIMULAR */
Las tres funciones que crean las ventanas de la interfaz se encuentran desarrolladas
en el archivo fuente interface.c. Ntese en el cdigo anterior que la funcin
create_window1 devuelve un puntero a un objeto del tipo GTK. Dentro de la funcin se
inicializan todos los objetos grficos que componen la ventana, en el ejemplo se muestran
solo dos de los objetos de la ventana, la ventana principal window1 y el objeto del tipo
botn bsimular (botn SIMULAR de la aplicacin). En total, esta ventana se encuentra
compuesta por cincuenta y un objetos, incluyendo botones, entradas de texto, marcos,
etiquetas, etc. En el cdigo se puede verificar la existencia de diferentes funciones que
ofrece la librera Gtk, que se pueden utilizar dependiendo del objeto que se desea crear, por
67
ejemplo para crear una ventana vaca se utiliza la funcin gtk_window_new(), mientras que
para crear un botn se utiliza la funcin gtk_button_new(). Algunos objetos grficos
requieren funciones de respuesta para eventos determinados, por ejemplo en el cdigo
anterior se conecta el objeto bsimular, es decir el botn SIMULAR de la aplicacin, con la
funcin on_simular_clicked, sta funcin va a ser llamada cada vez que el botn
SIMULAR sea presionado. La funcin utilizada para ligar el proceso de respuesta al botn
fue la siguiente: g_signal_connect(arg1, arg2, arg3, arg4), a la funcin se le deben pasar
cuatro argumentos, el primero indica el objeto grfico en el que se da el evento, en el
ejemplo anterior es bsimular, el segundo argumento seala el tipo de evento para el cual se
debe llamar la funcin de respuesta, en el ejemplo el evento es del tipo clicked, es decir
cuando se presione el botn utilizando el puntero del ratn, el tercer argumento de la
funcin de conexin indica la funcin de respuesta que se desea ligar al evento y al objeto,
mientras que el cuarto argumento se utiliza para pasar informacin a la funcin de
respuesta.
Todas la funciones de respuesta se encuentran desarrolladas en el archivo fuente
callbacks.c. A continuacin se muestra parte del cdigo de la funcin on_bsimular_clicked,
la cual es llamada cada vez que el botn SIMULAR es presionado.
/*Simula el movimiento del robot utilizando los valores de los botones 'spinner'*/
GtkSpinButton *spin1, *spin2, *spin3;
int velocporciento;
spin1=GTK_SPIN_BUTTON(spinbutton1);
spin2=GTK_SPIN_BUTTON(spinbutton2);
spin3=GTK_SPIN_BUTTON(spinbutton3);
hubochoque=0;
selec_mover_joint();
if(hubochoque==1){
printtv("\n\nCUIDADO!!!-->Se produjo un choque durante la simulacin del movimiento");
}
}
aplicacin, para simular el movimiento del brazo robot. Una vez tomados los valores se
llama a la funcin selec_mover_joint(), la cual se encarga de producir el movimiento en el
modelo virtual del robot. Cada botn de la aplicacin se encuentra ligado a diferentes
funciones de respuesta, las cuales en conjunto permiten el funcionamiento correcto de la
aplicacin.
Tecla(s): Funcin
r Selecciona el Robot (obj = robot)
c Selecciona la Cmara (obj = cmara)
z Modo = Rotar la cmara
x Modo = Trasladar de la cmara
if(obj = camara) if(obj = robot)
if(modo=rotar) if(modo=trasladar)
keyLeft + rotar(eje Y) - + articulacin 1
keyRight - rotar(eje Y) - - articulacin 1
keyUp + rotar(eje X) + trasladar (eje Y) + articulacin 2
keyDown - rotar(eje X) - trasladar (eje Y) - articulacin 2
y - + articulacin 5
n - - articulacin 5
o - + articulacin 6
p - - articulacin 6
74
75
Cada proceso en prueba fue cronometrado treinta veces, luego se calcul el tiempo
promedio de ejecucin de cada uno.
En promedio al programa le tom 443 ms (milisegundos) realizar todas las tareas
que implican la creacin y el despliegue de las tres ventanas de la aplicacin grfica. La
velocidad del proceso se considera buena, ya que es apenas perceptible por el usuario.
Las mediciones de velocidad de las funciones de extraccin y copia de informacin
desde los objetos de la aplicacin grfica, mostraron resultados positivos. Todas las
funciones que realizan este tipo de proceso extrajeron los valores, tanto de las entradas de
texto como de los botones de ajuste Gtk, a velocidades realmente altas. De hecho no se
pudo captar ninguna diferencia entre el tiempo de entrada y el de salida para estas
funciones, en ninguna de las pruebas, es decir que el tiempo de ejecucin siempre fue
menor a 1 milisegundo. Estos resultados muestran un gran desempeo por parte de las
funciones de extraccin de informacin desde objetos Gtk. Se puede afirmar que la
velocidad de stas es lo suficientemente alta como para que no afecten de manera negativa,
el desempeo de la aplicacin.
Al igual que para el proceso de extraccin de informacin desde los objetos Gtk, la
extraccin de informacin desde archivos de texto, durante la funcin de carga de rutinas,
tambin revel tiempos de ejecucin muy bajos. Para este caso tampoco se detectaron
tiempos de ejecucin mayores a un milisegundo, an cuando se cargaron rutinas de hasta
sesenta movimientos.
La funcin dibujar(), es la encargada de graficar la escena virtual en la pantalla.
Realmente esta funcin est compuesta por varias etapas. Lo primero que sta hace es
buscar el rea de dibujo destinada al despliegue de objetos OpenGL, luego si el rea existe
y es encontrada la funcin la selecciona como rea actual de dibujo. Como paso siguiente
se deben limpiar los almacenadores intermedios de color y de profundidad (<<color
buffer>> y <<depth buffer>>), determinar las opciones de dibujo elegidas por el usuario,
llamar a la matriz de transformacin actual, llevar a cabo las multiplicaciones de matrices
de traslacin y rotacin que sean necesarias, as como el apile y desapile de la matriz de
76
transformacin actual en los puntos que lo requieran y llamar las listas de despliegue
especficas en los puntos necesarios. Luego se debe convocar a la funcin encargada de
comparar la amplitud de giro de cada una de las articulaciones del modelo con sus
amplitudes de giro mximas y esperar a que sta termine de ser ejecutada. Posteriormente
se llama a la funcin de prueba de colisiones y se aguarda a que esta funcin emita su
resultado. Como ltimo paso se exige la ejecucin de todas las funciones OpenGL llamadas
hasta este punto y se intercambian los almacenadores intermedios grficos. Se observa que
son varios pasos los que debe realizar la funcin dibujar(), de hecho esta es la funcin cuyo
tiempo de ejecucin posee mayor influencia en el desempeo del programa, en cuanto a
velocidad de procesamiento se refiere, no solo por la cantidad de procesos que debe llevar a
cabo, sino por que es una de las ms utilizadas durante la ejecucin de la aplicacin. A
pesar de todas las tareas que realiza, dibujar() expuso un tiempo promedio de ejecucin de
apenas 29 ms, de hecho el mayor tiempo que se logr cronometrar para esta funcin fue de
32 ms, el cual se considera como aceptable, si se toma en cuenta todos los pasos que
conlleva el proceso. El tiempo es tan bajo que el usuario percibe la graficacin como
inmediata.
Las pruebas de velocidad mostraron resultados muy positivos. En su mayora los
tiempos de ejecucin de los procesos son tan bajos, que no pueden ser percibidos por el
usuario.
A continuacin se muestran algunos de los resultados obtenidos durante las pruebas
de velocidad.
Tiempo de dibujar(): 27 ms
Tiempo de dibujar(): 32 ms
Tiempo de dibujar(): 28 ms
77
No se ha llamado el programa:
total utilizada libre
MemRAM: 256288 247080 9208
Se corre el programa:
total utilizada libre
MemRAM: 256288 252864 3424
... /*Se realizan otros procesos y se mide la memoria antes y despus de cada uno */
Se cierra el programa:
total utilizada libre
MemRAM: 256288 247432 8856
7.1 Conclusiones
El API OpenGL demostr ser una biblioteca de gran robustez y flexibilidad, que
permite el desarrollo de aplicaciones grficas tanto simples como complejas.
78
79
La simulacin virtual de un proceso permite al usuario obtener una idea previa del
resultado a esperar, de manera que se pueden realizar los cambios necesarios para
obtener una mayor eficiencia en el proceso, antes de que ste ocurra realmente.
7.2 Recomendaciones
Sera de gran utilidad para el usuario, que se desarrollara un algoritmo que calcule
la combinacin de ngulos necesaria, para llevar el extremo del brazo robot a una
posicin especfica en el espacio. Tal algoritmo podra ser creado utilizando el
mtodo de cinemtica inversa.
81
BIBLIOGRAFA
[3] Neider, J., Davis, T., Woo, M., The OpenGL Programming Guide The
Red Book, segunda edicin, Addison-Wesley Publishing, E.E.U.U, 1999.
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <glib.h>
#include <gtk/gtk.h>
#include <gtk/gtkgl.h>
#include <gdk/gdk.h>
#include <gdk/gdkgl.h>
#include <gdk/gdkkeysyms.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include "callbacks.h"
#include "interface.h"
#include "dibujo.h"
#include "ventanagl.h"
#include "rutina.h"
82
83
label = gtk_label_new ("CUIDADO->Se dio una colisin. \nDesea aun asi agregar el movimiento a la rutina?");
gtk_container_add (GTK_CONTAINER (GTK_DIALOG(dialog)->vbox), label);
gtk_widget_show_all (dialog);
return result;
}
spin1=GTK_SPIN_BUTTON(spinbutton1);
spin2=GTK_SPIN_BUTTON(spinbutton2);
spin3=GTK_SPIN_BUTTON(spinbutton3);
85
hubochoque=0;
/*Guarda los angulos actuales del robot en una memoria temporal, antes de moverlo*/
temp_angulo[0]=angulo1; temp_angulo[1]=angulo2; temp_angulo[2]=angulo3;
temp_angulo[3]=angulo4; temp_angulo[4]=angulo5; temp_angulo[5]=angulo6;
/*Aqui se debe introducir el codigo para enviar la seal respectiva al brazo robot*/
}
/*Si hubo choque pregunta si se debe guardar el movimiento.*/
if(hubochoque==1){
printtv("\n\n!!CUIDADO!!-->se produjo una colision durante la simulacin del movimiento");
printtv("\nSe recomienda eliminar el ultimo movimiento de la rutina");
guardar=dialogo_mensaje();
if(guardar==1){
/*guardar el movimiento en la matriz de rutinas y enviar seal*/
printtv("\nSe guardo el movimiento en la rutina");
fprintf(archivo1,"M%i, J %d, A %3.1f, V
%d,\n",puntrutina,(int)rutina[puntrutina][0],rutina[puntrutina][1],(int)rutina[puntrutina][2]);
/*Aqui se debe introducir el codigo para enviar la seal respectiva al brazo robot*/
}else {
/*Se devuelve el brazo y el puntero de rutina a la posicin anterior*/
printtv("\nNo se guardo el movimiento en la rutina");
puntrutina-=1;
angulo1=temp_angulo[0]; angulo2=temp_angulo[1]; angulo3=temp_angulo[2];
angulo4=temp_angulo[3]; angulo5=temp_angulo[4]; angulo6=temp_angulo[5];
dibujar();
/*No se le enva ninguna seal al robot*/
86
}
}
/*Si se alcanzan los 60 movimientos-> Se termina la edicin de rutina del simulador*/
if(puntrutina==60){
printtv("Movimiento #60 --> Mximo que puede guardar la rutina del simulador");
fprintf(archivo1,"fin");
fclose(archivo1);
edicion=0;
printtv("\n\nFin de la edicin de la rutina");
printf("\nFin de la edicin de la rutina: %s", nombrerutina);
}
}else if(simon==0){
printtv("\nSimulador Apagado. Enviando la seal directamente al robot");
/*Aqui se debe introducir el codigo para enviar la seal respectiva al brazo robot*/
}
free(temp_angulo);
fflush(stdout);
}
spin1=GTK_SPIN_BUTTON(spinbutton1);
spin2=GTK_SPIN_BUTTON(spinbutton2);
spin3=GTK_SPIN_BUTTON(spinbutton3);
hubochoque=0;
selec_mover_joint();
if(hubochoque==1){
printtv("\n\nCUIDADO!!!-->Se produjo un choque durante la simulacin del movimiento");
}
}
/*lista de comandos */
comandos[0]="e\0"; comandos[1]="edit "; comandos[2]="ex ";
comandos[3]="drive "; comandos[4]="base"; comandos[5]="margen";
}else{
printtv("\nEnviando seal al robot");
/*Aqui se debe introducir el codigo para enviar la seal respectiva al brazo robot*/
}
gtk_entry_set_text(GTK_ENTRY(entrycomando),"");
fflush(stdout);
/* previene que el handler predeterminado corra */
gtk_signal_emit_stop_by_name(GTK_OBJECT(button),"clicked");
}
strcpy(nombrerutina, "./rutinas/\0");
strncat(nombrerutina, gtk_entry_get_text(GTK_ENTRY(entryrutina)),(sizeof(nombrerutina)-sizeof("./rutinas/\0")-3));
nombrerutina[sizeof(nombrerutina)-1]='\0';
printtv("\n\nInicio de la edicin de la rutina");
printf("\nInicio de la edicin de la rutina: %s", nombrerutina);
fflush(stdout);
archivo1=fopen(nombrerutina,"w");
fprintf(archivo1,"inicio\n");
}
gtk_dialog_run(GTK_DIALOG(file_selector));
FILE *archivo2;
char letra_ui;
char articult_ui[3];
char angt_ui[7];
char veloct_ui[7];
char linea_ui[15];
int p1, p2, p3; //punteros
if (!archivo2){
printtv("\n\n-ERROR-->No se pudo abrir el archivo");
printf("\n-ERROR-->No se pudo abrir el archivo %s", archivo_sel);
} else {
89
/*comienza a extraer la informacin del archivo*/
p1=0;
rutina[0][0]=0;
do{
/*se trata de un nuevo movimiento*/
letra_ui=fgetc(archivo2);
if(letra_ui=='M'){
if(p1<60){
p1++;
rutina[0][0]++;
}
}else if(letra_ui=='A'){
/*se trata de un angulo*/
p2=0;
while(letra_ui!=','){
letra_ui=fgetc(archivo2);
angt_ui[p2]=letra_ui;
p2++;
}
rutina[p1][1]=atof(angt_ui);
}else if(letra_ui=='J'){
/*se trata de una articulacin (joint)*/
p2=0;
while(letra_ui!=','){
letra_ui=fgetc(archivo2);
articult_ui[p2]=letra_ui;
p2++;
}
rutina[p1][0]=atof(articult_ui);
}else if(letra_ui=='V'){
/*se trata de la velocidad del movimiento*/
p2=0;
while(letra_ui!=','){
letra_ui=fgetc(archivo2);
veloct_ui[p2]=letra_ui;
p2++;
}
rutina[p1][2]=atof(veloct_ui);
}
}while (letra_ui!='f');
/*se cierra el archivo*/
fclose(archivo2);
}
/***********VENTANA OPENGL************************/
/*Movimiento del Mouse genera movimiento de la camara,
*si se mantiene alguno de los botones presionados*/
gboolean on_glarea_motion_notify_event(GtkWidget *widget, GdkEventMotion *event, gpointer user_data){
int x;
int y;
int difx;
int dify;
GdkModifierType state;
if (event->is_hint) {
gdk_window_get_pointer(event->window, &x, &y, &state);
}
/*calcula la diferencia entre las coordenadas actuales del ratn y las pasadas*/
difx=event->x - lastx;
dify=event->y - lasty;
lastx=event->x;
lasty=event->y;
//Tecla presionada
/*Se llama una funcin por cada tecla presionada dentro de la ventana OpenGL*/
gboolean on_glarea_key_press_event(GtkWidget *widget, GdkEventKey *event, gpointer user_data){
event->keyval;
if(event->keyval==GDK_x){
printtv("\nDesplazar-->CAMARA");
mov_cam=1;
}else if(event->keyval==GDK_z){
printtv("\nRotar-->CAMARA");
mov_cam=2;
}else if(event->keyval==GDK_c){
91
printtv("\nObjeto Seleccionado-->CAMARA");
mover_obj=2;
}else if(event->keyval==GDK_r){
printtv("\nObjeto Seleccionado-->ROBOT");
mover_obj=1;
}else if(event->keyval==GDK_l){
if(lim==0){
lim=1;
}else {
lim=0;
}
}else if(mover_obj == 1){ /*se mueve el modelo del robot*/
if(event->keyval==GDK_y){
angulo5-=aument;
}else if(event->keyval==GDK_n){
angulo5+=aument;
}else if(event->keyval==GDK_j){
angulo4-=aument;
}else if(event->keyval==GDK_g){
angulo4+=aument;
}else if(event->keyval==GDK_o){
angulo6+=aument;
}else if(event->keyval==GDK_p){
angulo6-=aument;
}else if(event->keyval==GDK_a){
printtv("\nAUMENTAR VELOCIDAD ROBOT");
aument+=1;
if (aument > 45){
aument = 45;
}
}else if(event->keyval==GDK_d){
printtv("\nDISMINUIR VELOCIDAD ROBOT");
aument-=1;
if (aument < 1){
aument = 1;
}
}
}else if(mover_obj==2){ //*se mueve la camara en la escena*/
if(event->keyval==GDK_a){
printtv("\nAUMENTAR VELOCIDAD CAMARA");
aument_cam+=1;
if (aument_cam > 45){
aument_cam = 45;
}
}else if(event->keyval==GDK_d){
printtv("\nDISMINUIR VELOCIDAD CAMARA");
aument_cam-=1;
if (aument_cam < 1){
aument_cam = 1;
}
}
}
if(event->keyval==GDK_F1){
if(grid==0){
/*se activa la rejilla del piso*/
grid=1;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bgrid), TRUE);
}else {
/*se desactiva la rejilla*/
grid=0;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bgrid), FALSE);
}
}else if(event->keyval==GDK_F2){
if(checkmax==0){
92
/*se activa la opcin de limite de angulos mximos*/
checkmax=1;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bcheckmax), TRUE);
}else {
checkmax=0;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bcheckmax), FALSE);
}
}else if(event->keyval==GDK_F3){
if(checkcolision==0){
/* se activa la opcin de detencin de la sim. en el primer choque*/
checkcolision=1;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bcolision1), TRUE);
}else {
checkcolision=0;
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON (bcolision1), FALSE);
}
}else if(event->keyval==GDK_F5){
printf("\nCREAR RUTINA");
crear_rutina();
}else if(event->keyval==GDK_F6){
printf("\nABRIR Y CARGAR RUTINA");
abrir_rutina();
}else if(event->keyval==GDK_F7){
printf("\nSALVAR RUTINA");
salvar_rutina();
}else if(event->keyval==GDK_F8){
printtv("\nEJECUTAR RUTINA");
ejecutar_rutina();
}else if(event->keyval==GDK_F9){
printtv("\nAPILAR POSICION");
push_posicion();
dibujar();
}else if(event->keyval==GDK_F10){
printtv("\nDESAPILAR POSICION");
pull_posicion();
dibujar();
}else if(event->keyval==GDK_F11){
printtv("\nREINICIAR PILA");
reiniciar_pila();
}else if(event->keyval==GDK_F12){
printf("\nInformacin:");
printf("\nAngulos: A1 %.0f, A2 %.0f, A3 %.0f, A4 %.0f, A5 %.0f, A6 %.0f\n", angulo1,-1*angulo2,-
1*angulo3,angulo4,-1*angulo5,angulo6);
}
if(mover_obj == 1){/*se mueve el robot*/
if(event->keyval==GDK_Up){
angulo2-=aument;
}else if(event->keyval==GDK_Down){
angulo2+=aument;
}else if(event->keyval==GDK_Right){
angulo1-=aument;
}else if(event->keyval==GDK_Left){
angulo1+=aument;
}else if(event->keyval==GDK_Page_Up){
angulo3-=aument;
}else if(event->keyval==GDK_Page_Down){
angulo3+=aument;
}
}else if(mover_obj== 2){/*se mueve la camara*/
if(event->keyval==GDK_Page_Up){
zoom_z-=1;
}else if(event->keyval==GDK_Page_Down){
zoom_z+=1;
}else if(mov_cam==1){
93
if(event->keyval==GDK_Up){
altura_y+=aument_cam;
}else if(event->keyval==GDK_Down){
altura_y-=aument_cam;
}
}else if(mov_cam==2){
if(event->keyval==GDK_Up){
angulo_xr+=aument_cam;
}else if(event->keyval==GDK_Down){
angulo_xr-=aument_cam;
}else if(event->keyval==GDK_Right){
angulo_yr+=aument_cam;
}else if(event->keyval==GDK_Left){
angulo_yr-=aument_cam;
}
}
reshape(ventanaw, ventanah);
}
if(event->keyval==GDK_Home){
printtv("\nREINICIANDO ROBOT");
mover_obj = 1;
valores_iniciales_robot();//REINICIAR POSICION ROBOT
}else if(event->keyval==GDK_End){
printtv("\nREINICIANDO CAMARA");
valores_iniciales_camara();//REINICIAR POSICION CAMARA
reshape(ventanaw, ventanah);
}
dibujar();
return TRUE;
}
spin1=GTK_SPIN_BUTTON(spinbutton1);
spin2=GTK_SPIN_BUTTON(spinbutton2);
spin3=GTK_SPIN_BUTTON(spinbutton3);
hubochoque=0;
/*simula el movimiento*/
selec_mover_joint();
if(hubochoque==1){
printtv("\n\nCUIDADO!!!-->Se produjo un choque durante la simulacin del movimiento");
}
}
/* Informacin del Programa*/
void on_bacerca_clicked(GtkButton *button, gpointer user_data){
GtkWidget *mensaje;
/*************************************************************************************************************/
/***************************************************************************
* SimQNK
* Simulador virtual del brazo robot Stubli RX90 L
* dibujo.c: funciones de dibujo y pruebas de colision del modelo 3D
* Copyright(C) 2006 David Cuenca
* Email: dcuenc@gmail.com
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*/
#include "dibujo.h"
#define BASE 1
#define BRAZO1 2
#define BRAZO2 4
#define CODO 3
#define MUNIECA 5
95
#define MANO 6
#define ESFERA 7
#define CUBO 8
#define HOMBRO 9
#define EMPO 10
//Figuras de control
float circHB [31][4];
float circHA [31][4];
float circWA [31][4];
float circWB [31][4];
float circWC [31][4];
float circBD [31][4];
float circBC [31][4];
float cuadBA [5][4]={0,0,0,0, 6.6,-15,-5.7,1, -5.7,-15,-5.7,1, -5.7,-15,5.7,1, 6.6,-15,5.7,1};
float cuadBB [5][4]={0,0,0,0, 12,-10,-11.1,1, -10.2,-10,-11.1,1, -10.2,-10,11.1,1, 12,-10,11.1,1};
float cuadBC [5][4]={0,0,0,0, 12,10,-11.1,1, -10.2,10,-11.1,1, -10.2,10,11.1,1, 12,10,11.1,1};
float circHBreal [31][4];
float circHAreal [31][4];
float circWAreal [31][4];
float circWBreal [31][4];
float circWCreal [31][4];
float circBDreal [31][4];
float circBCreal [31][4];
float cuadBAreal [5][4];
float cuadBBreal [5][4];
float cuadBCreal [5][4];
//Puntos de control
float CHB [5]={0.0, 8.5, 0.0, 1, 5};
float CHA [5]={0.0, 5.6, 0.0, 1, 5};
float CWA [5]={-5.5, 55.0, 0.0, 1, 4};
float CWB [5]={0.0, 55.0, 0.0, 1, 4};
96
float CWC [5]={5.5, 55.0, 0.0, 1, 4};
float CBA [5]={0.0, -15, 0.0, 1, 3};
float CBB [5]={0.0, -10, 0.0, 1, 3};
float CBC [5]={0.0, 10, 0.0, 1, 3};
float CBD [5]={0.0, 58, 0.0, 1, 3};
float CHBreal [3];
float CHAreal [3];
float CWAreal [3];
float CWBreal [3];
float CWCreal [3];
float CBAreal [3];
float CBBreal [3];
float CBCreal [3];
float CBDreal [3];
rotx2[1][1]=cos(angulo2*M_PI/180);
rotx2[1][2]=(-1)*sin(angulo2*M_PI/180);
rotx2[2][1]=sin(angulo2*M_PI/180);
rotx2[2][2]=cos(angulo2*M_PI/180);
rotx3[1][1]=cos(angulo3*M_PI/180);
rotx3[1][2]=(-1)*sin(angulo3*M_PI/180);
rotx3[2][1]=sin(angulo3*M_PI/180);
rotx3[2][2]=cos(angulo3*M_PI/180);
rotx5[1][1]=cos(angulo5*M_PI/180);
rotx5[1][2]=(-1)*sin(angulo5*M_PI/180);
rotx5[2][1]=sin(angulo5*M_PI/180);
rotx5[2][2]=cos(angulo5*M_PI/180);
roty4[0][0]=cos(angulo4*M_PI/180);
roty4[0][2]=sin(angulo4*M_PI/180);
roty4[2][0]=(-1)*sin(angulo4*M_PI/180);
roty4[2][2]=cos(angulo4*M_PI/180);
}
/*Llenar los vectores de posicin de la orillas del hombro fijos*/
void init_vortexfijos(float margen){
97
vortexfijo1[1]=28.5-(margen/2);
vortexfijo1[2]=13.1+(margen/2);
vortexfijo2[1]=54.5+(margen/2);
vortexfijo2[2]=13.1+(margen/2);
}
llenar_matrices_rot();
/*Concatenacin de matrices para nivel de jerarquia 5*/
if(s==5){
mult_mat_vect(rotx5, numero);
llenar_vtemp();
mult_mat_vect(tras4, vtemp);
llenar_vtemp();
mult_mat_vect(roty4, vtemp);
llenar_vtemp();
mult_mat_vect(tras3, vtemp);
llenar_vtemp();
mult_mat_vect(rotx3, vtemp);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
numreal[j]=vectr[j];
}
}
/*Concatenacin de matrices para nivel de jerarquia 4*/
if(s==4){
mult_mat_vect(roty4, numero);
llenar_vtemp();
mult_mat_vect(tras3, vtemp);
llenar_vtemp();
98
mult_mat_vect(rotx3, vtemp);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
numreal[j]=vectr[j];
}
}
/*Concatenacin de matrices para nivel de jerarquia 3*/
if(s==3){
mult_mat_vect(rotx3, numero);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
numreal[j]=vectr[j];
}
}
}
numpuntos=array[0][1];
if(fabs(array[i][2])<=fabs(menorz[2])){
menorz[0]=array[i][0];
menorz[1]=array[i][1];
menorz[2]=array[i][2];
menorz[3]=i;
}
}
}
numpuntos=array[0][1];
arrayreal[0][0]=array[0][0];
arrayreal[0][1]=array[0][1];
/*Concatenacin de matrices para nivel de jerarqua 5*/
if(s==5){
for(n=1; n<=numpuntos; n++){
mult_mat_vect(rotx5, array[n]);
llenar_vtemp();
mult_mat_vect(tras4, vtemp);
llenar_vtemp();
mult_mat_vect(roty4, vtemp);
llenar_vtemp();
mult_mat_vect(tras3, vtemp);
llenar_vtemp();
mult_mat_vect(rotx3, vtemp);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
arrayreal[n][j]=vectr[j];
}
}
}
/*Concatenacin de matrices para nivel de jerarqua 4*/
if(s==4){
for(n=1; n<=numpuntos; n++){
101
mult_mat_vect(roty4, array[n]);
llenar_vtemp();
mult_mat_vect(tras3, vtemp);
llenar_vtemp();
mult_mat_vect(rotx3, vtemp);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
arrayreal[n][j]=vectr[j];
}
}
}
/*Concatenacin de matrices para nivel de jerarqua 4*/
if(s==3){
for(n=1; n<=numpuntos; n++){
mult_mat_vect(rotx3, array[n]);
llenar_vtemp();
mult_mat_vect(tras2, vtemp);
llenar_vtemp();
mult_mat_vect(rotx2, vtemp);
llenar_vtemp();
mult_mat_vect(tras1, vtemp);
for(j=0;j<4;j++){
arrayreal[n][j]=vectr[j];
}
}
}
}
if(nivel==1){
pila_menorz(arrayreal1);
mismo_pt_a2(menorz, array2, menorz2);
coord_globales_pta(menorz2, vortexm);
dibujarvortex(menorz,vortexm);
vortex_test(menorz, vortexm, vortexfijo1, 1);
if(choquev==1){
printf("\nCHOQUE Orilla INFERIOR del HOMBRO");
}
}
if(nivel==2){
pila_menory(arrayreal1);
mismo_pt_a2(menory, array2, menory2);
coord_globales_pta(menory2, vortexm);
dibujarvortex(menory,vortexm);
vortex_test(menory, vortexm, vortexfijo2, 2);
102
if(choquev==1){
printf("\nCHOQUE Orilla SUPERIOR del HOMBRO");
}
}
}
//CIRULOS
for(i=1; i<=npuntos; i++){
circHA[i][0]=(3*sin(alpha));//coord. x del punto 1 (local)
circHA[i][1]=5.6;//coord. y del punto 1 (local)
circHA[i][2]=(3*cos(alpha));//coord. z del punto 1 (local)
circHA[i][3]=1;//indica que es un punto
circHB[i][0]=(3*sin(alpha));
circHB[i][1]=8.5;
103
circHB[i][2]=(3*cos(alpha));
circHB[i][3]=1;
circWA[i][0]=-5.5;
circWA[i][1]=(55+5*sin(alpha));
circWA[i][2]=(5*cos(alpha));
circWA[i][3]=1;
circWB[i][0]=0;
circWB[i][1]=(55+5*sin(alpha));
circWB[i][2]=(5*cos(alpha));
circWB[i][3]=1;
circWC[i][0]=5.5;
circWC[i][1]=(55+5*sin(alpha));
circWC[i][2]=(5*cos(alpha));
circWC[i][3]=1;
circBD[i][0]=(8.3*sin(alpha));
circBD[i][1]=48;
circBD[i][2]=(0.7+8.3*cos(alpha));
circBD[i][3]=1;
circBC[i][0]=(9*sin(alpha));
circBC[i][1]=0;
circBC[i][2]=(0.7+9*cos(alpha));
circBC[i][3]=1;
alpha+=dif;
}
}
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glTranslatef(0.0, 55.0, 0.0);
glRotatef(angulo5, 1.0, 0.0, 0.0);
glBegin( GL_LINE_LOOP );
for(i=1; i<=numpuntos; i++){
glVertex3f(circHA[i][0],circHA[i][1],circHA[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_circHB(void){
numpuntos=circHB[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
104
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glTranslatef(0.0, 55.0, 0.0);
glRotatef(angulo5, 1.0, 0.0, 0.0);
glBegin( GL_LINE_LOOP );
for(i=1; i<=numpuntos; i++){
glVertex3f(circHB[i][0],circHB[i][1],circHB[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_circWA(void){
numpuntos=circWA[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(circWA[i][0],circWA[i][1],circWA[i][2]);
}
glEnd();
glPopMatrix();
void dibujar_circWB(void){
numpuntos=circWB[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(circWB[i][0],circWB[i][1],circWB[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_circWC(void){
numpuntos=circWC[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
105
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(circWC[i][0],circWC[i][1],circWC[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_circBC(void){
numpuntos=circBC[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(circBC[i][0],circBC[i][1],circBC[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_circBD(void){
numpuntos=circBD[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glTranslatef(0.0, 10.0, 0.0);
glRotatef(angulo4, 0.0, 1.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(circBD[i][0],circBD[i][1],circBD[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_cuadBA(void){
numpuntos=cuadBA[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
106
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(cuadBA[i][0],cuadBA[i][1],cuadBA[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_cuadBB(void){
numpuntos=cuadBB[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(cuadBB[i][0],cuadBB[i][1],cuadBB[i][2]);
}
glEnd();
glPopMatrix();
}
void dibujar_cuadBC(void){
numpuntos=cuadBC[0][1];
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glRotatef(angulo2, 1.0, 0.0, 0.0);
glTranslatef(0.0, 45.0, 0.0);
glRotatef(angulo3, 1.0, 0.0, 0.0);
glBegin(GL_LINE_LOOP);
for(i=1; i<=numpuntos; i++){
glVertex3f(cuadBC[i][0],cuadBC[i][1],cuadBC[i][2]);
}
glEnd();
glPopMatrix();
}
float radchb;
float acwaz;
float acwbz;
float acwcz;
float acbez;
float acbdz;
float acbcz;
float acbbz;
float acbaz;
float achbz;
float achaz;
choque=choquep=choqueb=choqueh=choquev=0;
cuidado=0;
/*distancia del punto de control CHB al eje Y*/
radchb=sqrt(pow(CHBreal[2],2)+pow(CHBreal[0],2));
/*Valor absoluto de la coordena Z de los putnos de control*/
acwaz=fabs(CWAreal[2]);
acwbz=fabs(CWBreal[2]);
acwcz=fabs(CWCreal[2]);
acbdz=fabs(CBDreal[2]);
acbcz=fabs(CBCreal[2]);
acbbz=fabs(CBBreal[2]);
acbaz=fabs(CBAreal[2]);
achbz=fabs(CHBreal[2]);
achaz=fabs(CHAreal[2]);
//PISO
/*Verificar algun punto de control entro al area de precaucin del piso*/
if(CBAreal[1] <= (6+margen)){
dibujar_cuadBA();
check_piso(cuadBA, cuadBAreal);
cuidado=1;
}
if((CBBreal[1] <= (11.5+margen)) && (CBBreal[1] <= CBCreal[1])){
dibujar_cuadBB();
check_piso(cuadBB, cuadBBreal);
cuidado=1;
}
if((CBCreal[1] <= (11.5+margen)) && (CBCreal[1] < CBBreal[1])){
dibujar_cuadBC();
check_piso(cuadBC, cuadBCreal);
cuidado=1;
}
if((CWAreal[1] <= (5.5+margen)) && (CWAreal[1] <= CWCreal[1])){
dibujar_circWA();
check_piso(circWA, circWAreal);
cuidado=1;
}
if((CWCreal[1] <= (5.5+margen)) && (CWCreal[1] < CWAreal[1])){
dibujar_circWC();
check_piso(circWC, circWCreal);
cuidado=1;
}
if(CHBreal[1] <= (3.5+margen)){
dibujar_circHB();
check_piso(circHB, circHBreal);
108
cuidado=1;
}
//BASE
/*Verificar algun punto de control entro al area de precaucin de la base*/
if((CHBreal[1]<25.5) && (radchb<=15.5+margen)){
dibujar_circHB();
check_base(circHB, circHBreal);
cuidado=1;
}
if((CWAreal[1]<23.5) && (acwaz<=17.5+margen)){
dibujar_circWA();
check_base(circWA, circWAreal);
cuidado=1;
}
if((CWBreal[1]<23.5) && (acwbz<=17.5+margen)){
dibujar_circWB();
check_base(circWB, circWBreal);
cuidado=1;
}
if((CWCreal[1]<23.5) && (acwcz<=17.5+margen)){
dibujar_circWC();
check_base(circWC, circWCreal);
cuidado=1;
}
if((CBDreal[1]<28.5) && (acbdz<=21+margen)){
dibujar_circBD();
check_base(circBD, circBDreal);
cuidado=1;
}
if((CBAreal[1]<28.5) && (acbaz<=25+margen)){
dibujar_cuadBA();
cuidado=1;
choqueh=0;
coord_globales(cuadBA, cuadBAreal);
lim_box_yz(cuadBAreal, marg, altpiso, nively1, radbase);
if(choqueh==1){
printf("CHOQUE BASE\n");
}
}
if((CBBreal[1]<28.5) && (acbbz<=25+margen)){
dibujar_cuadBB();
cuidado=1;
choqueh=0;
coord_globales(cuadBB, cuadBBreal);
lim_box_yz(cuadBBreal, marg, altpiso, nively1, radbase);
if(choqueh==1){
printf("CHOQUE BASE\n");
}
}
//HOMBRO
//ALGUN CHOQUE?
/*Si se dio algun choque se llena la variable de hubochoque y la de choque*/
if((choquep==1) || (choqueb==1) || (choqueh==1) || (choquev==1)){
choque=1;
hubochoque=1;
}
}
if(angulo1<(-160)){
numart=1;
angmax=1;
angulo1=-160;
}else if(angulo1>160){
numart=1;
angmax=1;
angulo1=160;
}
if(angulo2<(-137.5)){
numart=2;
angmax=1;
angulo2=-137.5;
}else if(angulo2>137.5){
numart=2;
angmax=1;
angulo2=137.5;
}
if(angulo3<(-142.5)){
numart=3;
angmax=1;
angulo3=-142.5;
}else if(angulo3>142.5){
numart=3;
angmax=1;
angulo3=142.5;
}
if(angulo4<(-270)){
numart=4;
angmax=1;
angulo4=-270;
}else if(angulo4>270){
numart=4;
angmax=1;
angulo4=270;
}
if(angulo5<(-120)){
numart=5;
angmax=1;
angulo5=-120;
}else if(angulo5>105){
numart=5;
110
angmax=1;
angulo5=105;
}
if(angulo6<(-270)){
numart=6;
angmax=1;
angulo6=-270;
}else if(angulo6>270){
numart=6;
angmax=1;
angulo6=270;
}
if(angmax==1){
printf("\nArticulacin %d alcanzo su mxima amplitud de giro",numart);
}
}
//Limites geomtricos del modelo
/*Herramienta para el desarrollador o modelador*/
void dibujar_limites(void){
//Verticales
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glBegin(GL_LINES);
glVertex3f(-26.8, 0, 0);
glVertex3f(-26.8, 180, 0);
glVertex3f(-12.7, 0, 0);
glVertex3f(-12.7, 180, 0);
glVertex3f(15, 0, 0);
glVertex3f(15, 180, 0);
glVertex3f(11.9, 0, 0);
glVertex3f(11.9, 180, 0);
glVertex3f(0, 0, 0);
glVertex3f(0, 200, 0);
glVertex3f(-5.4, 132, 0);
glVertex3f(-5.4, 150, 0);
glVertex3f(5.4, 132, 0);
glVertex3f(5.4, 150, 0);
glVertex3f(0, 0, 13);
glVertex3f(0, 180, 13);
glVertex3f(0, 0, -13);
glVertex3f(0, 180, -13);
glVertex3f(0, 54, 11);
glVertex3f(0, 180, 11);
glVertex3f(0, 54, -11);
glVertex3f(0, 180, -11);
glEnd();
glPopMatrix();
//HORIZONTALES
glPushMatrix();
glColor3f(1.0, 0.0, 0.0);
glBegin(GL_LINES);
glVertex3f(-20, 42.0, 0);
glVertex3f(20, 42.0, 0);
glVertex3f(-20, 54.5, 0);
glVertex3f(20, 54.5, 0);
glVertex3f(-20, 71.7, 0);
glVertex3f(20, 71.7, 0);
glVertex3f(-20, 87, 0);
glVertex3f(20, 87, 0);
glVertex3f(-20, 96.7, 0);
glVertex3f(20, 96.7, 0);
glVertex3f(-20, 111, 0);
glVertex3f(20, 111, 0);
111
glVertex3f(-20, 152, 0);
glVertex3f(20, 152, 0);
glEnd();
glPopMatrix();
}
//SEAL DE PRECAUCION
/*Esferas de colores que indican el riesgo del movimiento actual*/
void semaforo(void){
GLUquadricObj *quadObj1;
float semaf_x;
float semaf_y;
float color[3];
semaf_x=140*ventanaw/ventanah;
quadObj1 = gluNewQuadric();
if(choque==1){
color[0]=1.0; color[1]=0.0; color[2]=0.0;
semaf_y=150;
}else if(cuidado==1){
color[0]=1.0; color[1]=0.9; color[2]=0.0;
semaf_y=140;
}else{
color[0]=0.0; color[1]=1.0; color[2]=0.5;
semaf_y=130;
}
glPushMatrix();
glColor3fv(color);
glRotatef(-1*angulo_yr, 0.0, 1.0, 0.0);
glTranslatef(-1*ancho_x, -1*altura_y, 0.0);
glRotatef(-1*angulo_xr, 1.0, 0.0, 0.0);
glTranslatef(semaf_x*zoom_z/160, semaf_y*zoom_z/160, 600.0);
gluSphere(quadObj1, 7*zoom_z/160, 30, 30);
glPopMatrix();
}
//----------------------DIBUJAR ESCENA----------------------------------------
gboolean dibujar(void){
if(checkmax==1){
/*limita los angulos de giro a sus amplitudes mximas*/
check_maximo();
}
//HOMBRO
glPushMatrix();
glTranslatef(0.0, 0.0, 0.0);
glTranslatef(0.0, 42.0, 0.0);
glRotatef(angulo1, 0.0, 1.0, 0.0);
glCallList(HOMBRO);
glTranslatef(-13.0, 0.0, 0.0);
//BRAZO1
glPushMatrix();
glRotatef(angulo2, 1.0, 0.0, 0.0);
glCallList(BRAZO1);
glTranslatef(13.0, 45.0, 0.0);
113
//CODO
glPushMatrix();
glRotatef(angulo3, 1.0, 0.0, 0.0);
glCallList(CODO);
glTranslatef(0.0, 10.0, 0.0);
//BRAZO2
glPushMatrix();
glRotatef(angulo4, 0.0, 1.0, 0.0);
glCallList(BRAZO2);
glTranslatef(0.0, 55.0, 0.0);
//MUNIECA
glPushMatrix();
glRotatef(angulo5, 1.0, 0.0, 0.0);
glTranslatef(5.5, 0.0, 0.0);
glCallList(MUNIECA);
glTranslatef(-5.5, 0.0, 0.0);
//MANO
glPushMatrix();
glRotatef(angulo6, 0.0, 1.0, 0.0);
glCallList(MANO);
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
if(lim==1){
dibujar_limites();
}
/*Funciones que crean las listas de despliegue de los componentes principales del robot*/
quadObj1 = gluNewQuadric();
glNewList(BASE, GL_COMPILE);
glPushMatrix();
glRotatef(-90, 1.0, 0.0, 0.0);
gluCylinder(quadObj1, 12, 12, 30, slices, stacks);
glPopMatrix();
glEndList();
}
quadObj1 = gluNewQuadric();
glNewList(HOMBRO, GL_COMPILE);
glPushMatrix();
glRotatef(-90, 1.0, 0.0, 0.0);
glRotatef(45, 0.0, 0.0, 1.0);
glTranslatef(0.0, 0.0, -13.5);
gluCylinder(quadObj1, 18.4, 18.4, 26, 4, 16);
gluDisk(quadObj1, 0, 18.4, 4, 16);
glTranslatef(0.0, 0.0, 26);
gluDisk(quadObj1, 0, 18.4, 4, 16);
glPopMatrix();
glPushMatrix();
glRotatef(90, 0.0, 1.0, 0.0);
glTranslatef(0.0, -0.5, 13.0);
glPushMatrix();
glRotatef(45, 0.0, 0.0, 1.0);
gluCylinder(quadObj1, 18.4, 7.0, 2, 4, 16);
glTranslatef(0.0, 0.0, 2.0);
gluDisk(quadObj1, 0, 7, 4, 16);
glPopMatrix();
glPopMatrix();
glEndList();
}
quadObj1 = gluNewQuadric();
glNewList(BRAZO1, GL_COMPILE);
glPushMatrix();
glRotatef(-90, 0.0, 1.0, 0.0);//ROTAR
gluCylinder(quadObj1, 12, 13, 7, slices, stacks);
glTranslatef(0.0, 0.0, 7.0);
gluCylinder(quadObj1, 13, 13, 7, slices, stacks);
glTranslatef(0.0, 0.0, 7.0);
gluPartialDisk(quadObj1, 0, 13, slices, stacks, 90, 180);
glTranslatef(0.0, 0.0, -3.5);
glBegin(GL_QUADS);
glNormal3f(0.0, 0.0, 1.0); //Izquierda
glVertex3f(-11.0, 45.0, 3.5);
glVertex3f(-13.0, 0.0, 3.5);
glVertex3f(13.0, 0.0, 3.5);
glVertex3f(11.0, 45.0, 3.5);
quadObj1 = gluNewQuadric();
glNewList(CODO, GL_COMPILE);
glPushMatrix();
glPushMatrix();
glRotatef(-90, 0.0, 1.0, 0.0);
glTranslatef(0.0, 0.0, 10.1);
gluCylinder(quadObj1, 9.8, 9.8, 3, 12, 16);
glPopMatrix();
glRotatef(-90, 1.0, 0.0, 0.0);
glRotatef(45, 0.0, 0.0, 1.0);
glTranslatef(0.45, -0.45, -10.0);
gluCylinder(quadObj1, 15.556, 15.556, 20, 4, 16);
glTranslatef(0.0, 0.0, -5.0);
gluCylinder(quadObj1, 4.0, 15.556, 5, 4, 16);
glPushMatrix();
glRotatef(180, 1.0, 0.0, 0.0);
gluDisk(quadObj1, 0, 4.0, 4, 16);
glPopMatrix();
glTranslatef(0.0, 0.0, 25.0);
gluDisk(quadObj1, 0, 15.556, 4, 16);
glPopMatrix();
glEndList();
}
116
//ANTERBRAZO del Robot
void crear_brazo2(int slices, int stacks){
GLUquadricObj *quadObj1;
quadObj1 = gluNewQuadric();
glNewList(BRAZO2, GL_COMPILE);
glPushMatrix();
glPushMatrix();
glRotatef(-90, 1.0, 0.0, 0.0);
gluCylinder(quadObj1, 9.0, 9.0, 14, slices, stacks);
glTranslatef(0.0, 0.0, 14.0);
gluDisk(quadObj1, 0, 9.0, slices, stacks);
glPopMatrix();
glTranslatef(0.0, 0.0, 0.7);
glPushMatrix();
glRotatef(-90, 1.0, 0.0, 0.0);
glRotatef(45, 0.0, 0.0, 1.0);
gluCylinder(quadObj1, 9.0, 8.3, 48, slices, stacks);
glTranslatef(0.0, 0.0, 48);
gluDisk(quadObj1, 0, 8.3, slices, stacks);
gluCylinder(quadObj1, 8.3, 4.0, 7, 4, 16);
glTranslatef(0.0, 0.0, 7.0);
gluDisk(quadObj1, 0, 4.0, 4, stacks);
glPopMatrix();
glPopMatrix();
glEndList();
}
quadObj1 = gluNewQuadric();
glNewList(MUNIECA, GL_COMPILE);
glPushMatrix();
glRotatef(-90, 0.0, 1.0, 0.0);
gluCylinder(quadObj1, 5, 5, 11, slices, stacks);
glPushMatrix();
glRotatef(180, 1.0, 0.0, 0.0);
gluDisk(quadObj1, 0, 5, slices, stacks);
glPopMatrix();
glTranslatef(0, 0, 11);
gluDisk(quadObj1, 0, 5, slices, stacks);
glPopMatrix();
glEndList();
}
quadObj1 = gluNewQuadric();
glNewList(MANO, GL_COMPILE);
glPushMatrix();
glColor3f(0.1, 0.1, 1.0);
glRotatef(-90, 1.0, 0.0, 0.0);
gluCylinder(quadObj1, 3, 3, 8.5, slices, stacks);
glTranslatef(0, 0, 8);
gluDisk(quadObj1, 0, 3, slices, stacks);
glPopMatrix();
glEndList();
117
}
/*************************************************************************************************************/
/***************************************************************************
* SimQNK
* Simulador virtual del brazo robot Stubli RX90 L
* interface.c: interfaz grfica de usuario
* Copyright(C) 2006 David Cuenca
* Email: dcuenc@gmail.com
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*/
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#ifdef ENABLE_NLS
# include <libintl.h>
# undef _
# define _(String) dgettext (PACKAGE, String)
# define Q_(String) g_strip_context ((String), gettext (String))
# ifdef gettext_noop
# define N_(String) gettext_noop (String)
# else
# define N_(String) (String)
# endif
#else
# define textdomain(String) (String)
# define gettext(String) (String)
# define dgettext(Domain,Message) (Message)
# define dcgettext(Domain,Message,Type) (Message)
# define bindtextdomain(Domain,Directory) (Domain)
# define _(String) (String)
# define Q_(String) g_strip_context ((String), (String))
# define N_(String) (String)
#endif
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <string.h>
#include <stdio.h>
#include <gdk/gdkkeysyms.h>
118
#include <gdk/gdk.h>
#include <gdk/gdkgl.h>
#include <gdk/gdkkeysyms.h>
#include <gtk/gtk.h>
#include <gtk/gtkgl.h>
#include "callbacks.h"
#include "interface.h"
#define GLADE_HOOKUP_OBJECT(component,widget,name) \
g_object_set_data_full (G_OBJECT (component), name, \
gtk_widget_ref (widget), (GDestroyNotify) gtk_widget_unref)
#define GLADE_HOOKUP_OBJECT_NO_REF(component,widget,name) \
g_object_set_data (G_OBJECT (component), name, widget)
return window1;
}
glconfig = gdk_gl_config_new_by_mode(GDK_GL_MODE_RGB |
GDK_GL_MODE_DEPTH |
GDK_GL_MODE_DOUBLE);
return window2;
}
return window3;
}
/*************************************************************************************************************/
#include "rutina.h"
int puntpos=0;
133
/*Reiniciar la pila*/
void reiniciar_pila(void){
puntpos=0;
}
/*.............CRONOMETRO DE ESPERA.......................*/
/*Regula la velocidad de dibujo de los cuadro de la escena (frames)*/
void esperar(float segundos)
{
clock_t tiempoespera;
tiempoespera = clock () + segundos * CLK_TCK ;
while (clock() < tiempoespera) {}
}
void selec_mover_joint(void){
/*Seleccionar cual articulacin es la que se debe mover, segun la orden del usuario*/
float* puntang;
if(joint==1){
puntang = &angulo1;
mover_joint(puntang, angulodelta, velocidad);
}else if(joint==2){
puntang = &angulo2;
mover_joint(puntang, -1*angulodelta, velocidad);
}else if(joint==3){
puntang = &angulo3;
mover_joint(puntang, -1*angulodelta, velocidad);
}else if(joint==4){
puntang = &angulo4;
mover_joint(puntang, angulodelta, velocidad);
}else if(joint==5){
puntang = &angulo5;
mover_joint(puntang, -1*angulodelta, velocidad);
}else if(joint==6){
puntang = &angulo6;
mover_joint(puntang, angulodelta, velocidad);
}
}
//.............EJECUTAR RUTINA..............//
/*Ejecutar la rutina cargada en memoria*/
void ejecutar_rutina(void){
int nm;
if(rutina[0][0]!=0){
for(nm=1; nm<=rutina[0][0]; nm++){
joint=rutina[nm][0];
136
angulodelta=rutina[nm][1];
set_velocidad((int)rutina[nm][2], (int)joint);
selec_mover_joint();
}
}
}
fclose(archivo1);
}
archivo1=fopen(nombrefile,"w");
if (!archivo1){
printf("\n-ERROR-->No se pudo abrir el archivo %s", nombrefile);
return 1;
}
p1=0;
fprintf(archivo1,"inicio\n");
for(p1=1;p1<=rutina[0][0];p1++){
/*imprime cada movimiento de la rutina de la memoria, en una linea nueva del archivo de texto*/
fprintf(archivo1,"M%i, J %d, A %3.1f, V %d,\n",p1,(int)rutina[p1][0],rutina[p1][1],(int)rutina[p1][2]);
}
fprintf(archivo1,"fin");
fclose(archivo1);
}
/*************************************************************************************************************/
#include "ventanagl.h"
#define SLICES 30
#define STACKS 30
/*Crear las diferentes partes del robot compilando las listas de despliegue*/
crear_base(SLICES, STACKS);
crear_hombro();
crear_brazo1(SLICES, STACKS);
139
crear_codo();
crear_brazo2(SLICES, STACKS);
crear_munieca(SLICES, STACKS);
crear_mano(SLICES, STACKS);
return TRUE;
}
glMatrixMode(GL_MODELVIEW);
/*************************************************************************************************************/
#include <stdio.h>
#include <gtk/gtk.h>
#include "ventanagl.h"
#include "dibujo.h"
#include "rutina.h"
#include "interface.h"
#include "callbacks.h"
/*Incializar GTK*/
gtk_set_locale ();
gtk_init (&argc, &argv);
/*************************************************************************************************************/
141