Cinemática Inversa III

Cinemática Inversa para un Brazo Robot de 5 grados de libertad.
 
Programado en FreeBasic (implementación de OpenGL)
 

En los apartados "Cinemática I" y "Cinemática II" nos movíamos siempre sobre los ejes X e Y, y lo aplicábamos al brazo tipo Scara. Ahora es el momento de subir un peldaño y pasar a calcular una cinemática inversa aplicada a las 3 dimensiones para un brazo robot antropomorfo de 4 ó 5 grados de libertad. En realidad es mucho más sencillo de lo que parece porque vamos a seguir usando las mismas fórmulas que anteriormente pero antes de pasar a ellas tenemos que hacer unos ajustes para que el brazo entre en la nueva dimensión. El eje X se mantendrá en la misma posición sobre el plano, el eje Y pasará a convertirse en la profundidad y el eje Z pasa a ser la altura.


Conocemos los ejes XYZla longitud de las articulaciones y el ángulo de muñeca Cabeceo (Pitch).

(Nota sobre el ángulo de cabeceo: Es un ángulo que tu le das al brazo para que esa posición se mantenga constante desde el punto de vista del observador aunque muevas el brazo a otra posición. La cinemática inversa también calcula un ángulo de muñeca, pero son cosas distintas aunque dependientes la una con la otra. El que calcula la cinemática inversa cambia su valor en la medida en que cambia la posición del brazo para mantener el ángulo de cabeceo (pitch) en la misma posición en todo momento.)

Con estos datos hemos de averiguar todos los ángulos del brazo robot.

Lo primero de todo es hallar el ángulo de giro de todo el brazo. Sólo hemos de hacer lo siguiente:


Ya tenemos un resultado. Ahora viene la parte un poco liosa, se trata de entender los conceptos:
Para facilitarnos los cálculos, como ya tenemos resuelto la cinemática inversa para 3 grados de libertad, vamos a hacer una especie de equivalencia, donde sólo tendremos que calcular un eje X ficticio y otro eje Y ficticio; se llamarán Xprima e Yprima. Xprima e Yprima en realidad son los ejes X e Y visto en dos dimensiones, como hacíamos con el brazo Scara; en la imagen de arriba puedes comprobarlo. Veamos cómo hacemos la reconversión.

Primero hemos de calcular el Módulo formado por los catetos X e Y:

Modulo  = Sqr( (X^2) + (Y^2) )

Después hacemos una reconversión de variables, las cuales nos hará de puente para usar el mismo método de resolución de ángulos que en Cinemática II.

Xprima = Modulo
Yprima = Z

Ya podemos resolver el resto de los ángulos de la misma forma que en el apartado anterior. Tenemos Xprima e Yprima que hemos deducido del espacio 3D, y añadimos la variable AlturaH que es la distancia entre la base (suelo) y el hombro del brazo. Todo lo que sigue ahora es prácticamente igual que en el apartado Cinemática II.

(Si no ves la imagen, haz clic en el botón -Actualizar- de tu navegador)

Tenemos las soluciones para calcular los 4 grados de libertad
Que son: El ángulo de giro de todo el brazo, ángulo del brazoángulo del antebrazo y ángulo de la muñeca (pitch). Si añadimos el giro de la muñeca (roll) (éste ángulo no afecta al resto de los ángulos del brazo, por tanto no afecta a la cinemática inversa) pasamos a tener 5 grados de libertad. Las pinzas (o terminal) no se cuenta como grado de libertad pero sí has de tenerlo presente como parte de la longitud de la muñeca a efectos de cálculos; es decir, la longitud de la muñeca total es la suma de la longitud de la muñeca real más la longitud del terminal o pinzas.

Programé un pequeño script para ver los cálculos en modo texto (no gráfico) de los resultados de la cinemática inversa que aquí se expone. Puede servirte de inspiración para realizar tu propio programa. Descarga el código fuente y ejecutable haciendo clic en la imagen de abajo.

(Haz clic en la imagen para descargar el programa y código fuente.)

Está preparado para calcular la cinemática inversa del brazo robot RV-2AJ con pinza incluida, aunque esos datos (las dimensiones del brazo robot) puedes modificarlos en el código fuente y adaptarlos al brazo robot que creas conveniente. Hice unas pequeñas adaptaciones para que los resultados fueran los mismos que da el simulador COSIMIR. Uso todos los decimales en doble precisión, COSIMIR hace un redondeo al alza del último decimal porque no muestra más de dos decimales. Todos los detalles los podrás conocer leyendo un pequeño documento que adjunto (Leeme.txt), además de los códigos fuentes (en FreeBasic y C++) y el ejecutable que podrás descargar haciendo clic aquí.


    Implementando OpenGL.    

Dibujar simples líneas en 3D era mucho más complicado de lo que me imaginaba, así que aprendí a implementar "OpenGL" para visualizar el brazo robot en 3D. Más abajo, en esta página, encontrarás un enlace para que puedas bajarte el código fuente y ejecutable de "BrazoSimple". También puedes hacer clic sobre la siguiente imagen.

 
Nota Importante:
Si al ejecutar el simulador el brazo se mueve con demasiada lentitud
significa que tu tarjeta de vídeo está configurada para tener mucho "Antialiasing".
Esto no tiene nada que ver con la potencia de tu ordenador.

El código fuente de BrazoSimple lo tienes a continuación. Con el teclado mueves el brazo en las coordenadas XYZ. Podrás comprobar que aunque muevas los ángulos de la muñeca, es decir, cabeceo y balanceo de muñeca (en inglés pitch & roll respectivamente), el terminal se mantiene en el mismo punto XYZ. He aquí la gracia de la cinemática inversa.

  • Clic aquí y te llevará a un foro donde publico cómo mostrar los datos de la cinemática inversa con OpenGL.
  • Clic aquí y explico cómo sacar los datos por el puerto RS232.
  • Clic aquí y descargarás código fuente parecido al que se muestra aquí, pero puede leer ficheros de extensión .PLT y así podrás hacer que tu brazo haga los movimientos para dibujar.


El Programa.
(Código fuente de "BrazoSimple")
 
Expongo las referencia del material que fui extrayendo hasta conseguir el código fuente. La aparente complejidad del código es debido al interface gráfico, realizado con OpenGL. Es muy sencillo de manejar desde "FreeBasic". Los encabezamientos son siempre los mismos. Yo comencé con tres cubos y luego modifiqué las dimensiones de los cubos para formar el brazo robot. Con el tiempo añadí la muñeca.
 
El programa está escrito en "FreeBasic", concretamente en FreeBasic IDE (FBIDE),  y puedes descargarlo desde aquí:

(Si haces clic no hagas nada, sólo esperar unos segundos hasta que comience la descarga)
  
 
Apenas mide 9 megas. Si conoces QBasic, pasar a FreeBasic es muy sencillo y las limitaciones que encontrabas con QBasic desaparecen. Además tendrás toda la potencia del C++ con la sencillez del Basic de siempre.

En todas las versiones de FreeBasic viene con una carpeta llamada "NeHe" (Neón Helio).
Está un poco escondida ..\FreeBasic\examples\GL\NeHe. Es todo un curso sobre OpenGL y prácticamente todo lo que sé lo aprendí estudiando esos códigos de ejemplo en forma de lecciones.
 
La cinemática inversa la resolví gracias a una hoja de cálculo que encontré en Internet. Saqué todas las fórmulas, les puse nombres adecuados y estudie a fondo el funcionamiento y que en esta web he explicado (apartados Cinemática I, II y III).
  
http://www.lynxmotion.com/images/files/inversek.xls  Has de tener el Excel instalado y una vez abierto podrás modificar el gráfico de abajo (en la hoja del Excel) cambiando los valores que verás en unas casillas de color amarillas arriba a la derecha.
  
Modifiqué e implementé los datos y fórmulas de esa hoja de cálculo y lo convertí en un programa con el que podía interactuar libremente con el teclado. Como originalmente es un brazo robot bidimensional (X e Y),  más tarde le añadí el eje Z para poder moverlo en las 3 dimensiones del espacio.
 
Hay dos librerías: GLU32.DLL y GLUT32.DLL que son necesarias para el funcionamiento del interface gráfico. Las encontrarás en el ZIP de descarga. Esas dos librerías han de estar en el misma carpeta que se encuentre el ejecutable, o bien, las puedes colocar a mano en el directorio Windows o en System.

Cuando ejecutes el programa podrás mover el brazo de la siguiente forma:
 
Teclas:
 
A D —–> Mueve linealmente el brazo sobre el eje X.
Q E —–> Mueve linealmente el brazo sobre el eje Y.
W S —-> Mueve linealmente el brazo sobre el eje Z.
Z X —–> Cabeceo  de la muñeca. (Pitch.)
C V —–> Balanceo de la muñeca. (Roll.)
N M —-> Abre/cierra las pinzas.
 
Pulsar "Esc" para salir del programa.


Si al compilar te da error o estás bajo plataforma Linux ves al final de esta página.


El programa: Código Fuente. 

 The translation could modify the code. Use the code without translating or download the program by clicking the link above or clic here.
#Include Once "GL/gl.bi"
#Include Once "GL/glu.bi"
#Include Once "GL/glut.bi"
#Include Once "fbgfx.bi"                  
#Include Once "createtex.bi"
 
Declare Sub InverseK
Declare Sub DibujaBrazo
 
'--------------- Declaración de variables ------------------
 
Dim Shared As Double   Pi, Rad, Grad, Balance, Cabeceo, AngGiro, AngBrazo,_
                       AngAntBr, AngMunecA, AngMunecB
 
Dim Shared As Integer  LongBrazo, LongAntBr, LongMunec, LongDedos, AlturaH
Dim Shared As Single   Xreal, Yreal, Zreal
Dim Shared As Single   LHombro, LBrazo, LAntBr, LMunec, LDedos, DistDedos=.11,_
                       EscenaX, EscenaY, Distancia, Espacio
 
Dim        As Single   EjeX, EjeY, EjeZ
 
'---------Configurar escenario para el OpenGL---------
 
Screen 12, 16, , 2                  ' Este encabezamiento de OpenGL siempre se repite.
glClearColor 0.0, 0.0, 0.0, 0.0     ' No hay que asustarse al ver este tipo de código
glMatrixMode GL_PROJECTION          ' porque cada vez que implementas OpenGL se copia
glViewport 0, 0, 640, 480           ' y pega esta parte.
gluPerspective  45.0, 640.0/480.0, 0.1, 100.0
gluLookAt 0,0,1,  0,0,0, 0,1,0 
glMatrixMode GL_MODELVIEW
glLoadIdentity
glClearColor 0.0, 0.0, 0.0, 0.5             
glClearDepth 1.0                            
glEnable GL_DEPTH_TEST           
glDepthFunc GL_LEQUAL                      
glHint GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST 
       
'-----Carga de Variables------
 
Pi   = Atn(1) * 4
Rad  = Pi  / 180
Grad = 180 /  Pi
 
'-=-=- Ajustes del Brazo: Dimensiones, distancias, etc. -=-=-
 
AlturaH   = 200     ' Altura del Hombro. (Distancia del suelo hasta el "hombro".)
LongBrazo = 250     ' Longitud Brazo.        Puedes modificar si quieres las dimensiones
LongAntBr = 300     ' Longitud AnteBrazo. de las articulaciones del brazo robot.
LongMunec = 100     ' Longitud Muñeca.
LongDedos =  50     Longitud Pinzas.

XReal=(300)         ' Posición Inicial X. Situamos el brazo robot en un punto del espacio.
YReal=(  0)         ' Posición Inicial Y. Si modificas estos valores procura que esté dentro
ZReal=(150)         ' Posición Inicial Z. del área de trabajo del brazo robot.
Cabeceo=-90         ' Ang. Pitch inicial de la Muñeca.
Balance=0

EscenaX=0           ' Perspectiva Inicial del Escenario.
EscenaY=0
 
'------ No tocar los valores de estas variables --------------------
 
LHombro    = AlturaH  /100
LBrazo     = LongBrazo/100   ' Equivalente en dimensiones de OpenGL.
LAntBr     = LongAntBr/100   ' No tocar aquí.
LMunec     = LongMunec/100   ' Aunque depende de la configuración de
LDedos     = LongDedos/100   ' OpenGL, las medidas suelen ser unas 100
Espacio    = LBrazo+LAntBr   ' veces más pequeñas que la original y
                             ' suelen, no siempre, incluir decimales.
'-------------------------------------------------------------------
 
'-=-=-=-=-=-   Programa Principal    -=-=-=-=-=-
 
While Not MultiKey(FB.SC_ESCAPE)
    
      InverseK     ' Ir a la Cinemática Inversa para calcular la posición de los ángulos.
  
      ' Control del Brazo mediante el teclado.
  
      If MultiKey(FB.SC_A) Then Xreal=Xreal-1
      If MultiKey(FB.SC_D) Then Xreal=Xreal+1
  
      If MultiKey(FB.SC_S) Then Yreal=Yreal-1
      If MultiKey(FB.SC_W) Then Yreal=Yreal+1
  
      If MultiKey(FB.SC_Q) Then Zreal=Zreal-1   
      If MultiKey(FB.SC_E) Then Zreal=Zreal+1   
  
      If MultiKey(FB.SC_C) Then Balance=Balance-.5   
      If MultiKey(FB.SC_V) Then Balance=Balance+.5
  
      If MultiKey(FB.SC_Z) Then Cabeceo=Cabeceo-.5    
      If MultiKey(FB.SC_X) Then Cabeceo=Cabeceo+.5
  
      If MultiKey(FB.SC_N)  Then
           If DistDedos>.11 Then DistDedos=DistDedos-.005
      EndIf 
        
      If MultiKey(FB.SC_M) Then
           If DistDedos<.4 Then DistDedos=DistDedos+.005
      EndIf
  
      If MultiKey(FB.SC_LEFT)     Then EscenaX=EscenaX-.5
      If MultiKey(FB.SC_RIGHT)    Then EscenaX=EscenaX+.5
      If MultiKey(FB.SC_UP)       Then EscenaY=EscenaY+.5
      If MultiKey(FB.SC_DOWN)     Then EscenaY=EscenaY-.5
  
      If MultiKey(FB.SC_PAGEUP)   Then Distancia=Distancia+.1
      If MultiKey(FB.SC_PAGEDOWN) Then Distancia=Distancia-.1
 
Wend
End
 
 
Sub InverseK   '----------------------Cinemática Inversa----------------
  
     Dim    As Double   Afx, Afy, LadoA, LadoB, Alfa, Beta, Gamma, Modulo, Hipotenusa, Xprima, Yprima
     Static As Single   Xaux, Yaux, Zaux, Baux, Caux
 
     '----Ang Giro (en Grados)-----------  ' En todos los lenguajes de programación, las
    AngGiro = (Atan2(Yreal, Xreal))*Grad   ' funciones trigonométricas las resuelve en
    '-----------------------------------   ' radianes. OpenGL necesita los valores en grados
                                           ' sexagesimales, por eso multiplicamos por 'Grad'.
     Modulo  = Sqr(Abs(Xreal^2)+Abs(Yreal^2))
 
     Xprima=Modulo
     Yprima=Zreal
  
     Afx=Cos(Rad*Cabeceo)*(LongMunec+LongDedos)
     LadoB=Xprima-Afx
  
     Afy=Sin(Rad*Cabeceo)*(LongMunec+LongDedos)
     LadoA=Yprima-Afy-AlturaH
     
     Hipotenusa=Sqr((LadoA^2)+(LadoB^2))
  
     Alfa=Atan2(LadoA,LadoB)
  
     Beta=ACos( ((LongBrazo^2)-(LongAntBr^2)+(Hipotenusa^2))/(2*LongBrazo*Hipotenusa) )
     
     '----Ang BRAZO (en Grados).------------
    AngBrazo= (Alfa+Beta)*Grad
    '--------------------------------------
 
     Gamma=ACos( ((LongBrazo^2)+(LongAntBr^2)-(Hipotenusa^2))/(2*LongBrazo*LongAntBr) )
       
     '----Ang ANTEBRAZO (en Grados).--------
    AngAntBr=(-((180*Rad)-Gamma))*Grad
    '--------------------------------------
  
    '----Ang MUÑECAS (en Grados).----------
    AngMunecA= Cabeceo-AngBrazo-AngAntBr
    AngMunecB= Balance                       Balance no afecta a la IK*, por eso se
    '--------------------------------------  ' carga directamente.
                                             ' *(IK=Inverse Kinematics=Cinemática Inversa)
 
     If (Str(AngBrazo)="-1.#IND") Or (Str(AngAntBr)= "-1.#IND") Then
          
         Xreal=Xaux     ' Si hay ángulos imposibles pasa a posición anterior.
         Yreal=Yaux
         Zreal=Zaux
         Balance=Baux
         Cabeceo=Caux
   
         InverseK       ' En caso de error, se llama a sí misma para repetir el cálculo
                        ' con los valores anteriores correctos.
     EndIf
 
     Xaux=Xreal
     Yaux=Yreal
     Zaux=Zreal
     Baux=Balance
     Caux=Cabeceo
    
     DibujaBrazo         ' Llama al procedimiento que dibuja el brazo robot con OpenGL,
                         ' con los valores de los ángulos calculados.
End Sub
 
 
Sub DibujaBrazo
 
' -------Animación OpenGL--------------------
  
 glClear(GL_COLOR_BUFFER_BIT Or GL_DEPTH_BUFFER_BIT)  ' Es una especie de CLS en OpenGL.
 glLoadIdentity                                    
  
 '---------- Espacio --------------------
  
 glTranslatef       0.0, 0.0, Distancia-10            ' Esta parte nos permite mover todo
 glRotatef      35, 1.0, 0.0, 0.0                     ' el escenario en los ejes X e Y.
 glRotatef EscenaX, 0.0, 1.0, 0.0
 glRotatef EscenaY, 1.0, 0.0, 0.0
 
' ---------- Hombro ---------------------
 
 glBegin(GL_QUADS)                                                                    
   glColor3f   0.2,  0.2,  0.3                        Crea la superficie del suelo.
   glVertex3f  Espacio, -LHombro,  Espacio         
   glVertex3f -Espacio, -LHombro,  Espacio         
   glVertex3f -Espacio, -LHombro, -Espacio         
   glVertex3f  Espacio, -LHombro, -Espacio         
 glEnd  

 glRotatef   AngGiro, 0.0, 1.0, 0.0              
  
 glBegin(GL_QUADS)                                    ' Crea el hombro del brazo.
   glColor3f   0.1,  0.2,  0.4                        
   glVertex3f  0.4,  0.0, -0.6           
   glVertex3f -0.4,  0.0, -0.6                        ' Al igual que un cubo tiene 6 facetas (caras)
   glVertex3f -0.4,  0.0,  0.6                        ' aquí "dibujamos" las 6 facetas de la
   glVertex3f  0.4,  0.0,  0.6                        ' articulación del hombro.
                                                      ' Todo esto se repite en adelante con cada
   glColor3f   0.2,  0.2,  0.4                        ' articulación. Sólo cambia el tamaño y color.           
   glVertex3f  0.4, -LHombro,  0.6       
   glVertex3f -0.4, -LHombro,  0.6       
   glVertex3f -0.4, -LHombro, -0.6                       
   glVertex3f  0.4, -LHombro, -0.6                    ' Tal como lo he configurado, las unidades 
                                                      ' están comprendidas entre -1.0 y +1.0 por 
   glColor3f   0.2,  0.1,  0.4                        eso todo está en decimal. Si multiplicas
   glVertex3f  0.4,  0.0,  0.6                        ' por 100 tienes el tamaño real.
   glVertex3f -0.4,  0.0,  0.6                      
   glVertex3f -0.4, -LHombro,  0.6                     
   glVertex3f  0.4, -LHombro,  0.6                      
     
   glColor3f   0.1,  0.2,  0.5                           
   glVertex3f  0.4, -LHombro, -0.6                     
   glVertex3f -0.4, -LHombro, -0.6                         
   glVertex3f -0.4,  0.0, -0.6                         
   glVertex3f  0.4,  0.0, -0.6                          
      
   glColor3f   0.2,  0.1,  0.5                            
   glVertex3f -0.4,  0.0,  0.6                         
   glVertex3f -0.4,  0.0, -0.6                          
   glVertex3f -0.4, -LHombro, -0.6                        
   glVertex3f -0.4, -LHombro,  0.6                         
    
   glColor3f   0.3,  0.2,  0.5                             
   glVertex3f  0.4,  0.0, -0.6                         
   glVertex3f  0.4,  0.0,  0.6                            
   glVertex3f  0.4, -LHombro,  0.6                 
   glVertex3f  0.4, -LHombro, -0.6
 glEnd()
  
 '------------- Brazo ----------------
 glTranslatef          0.0, 0.0, 0.0
 glRotatef   AngBrazo, 0.0, 0.0, 1.0             ' Cada vez que veas un glRotatef significa que
                                                 ' la figura gira en un plano del espacio; aquí
 glColor3f       0.0,  1.0,  0.0                 ' representa el movimiento de una articulación.
 glutSolidSphere 0.6, 11,   11                        
                                                      
 glBegin(GL_QUADS)                               Crea las 6 facetas (caras) del brazo.
   glColor3f  1.0,  0.0,  0.0
   glVertex3f LBrazo,  0.3, -0.4                          
   glVertex3f 0.0,  0.3, -0.4                        
   glVertex3f 0.0,  0.3,  0.4                           
   glVertex3f LBrazo,  0.3,  0.4          
      
   glColor3f  1.0,  0.5,  0.0                        
   glVertex3f LBrazo, -0.3,  0.4                    
   glVertex3f 0.0, -0.3,  0.4                   
   glVertex3f 0.0, -0.3, -0.4                   
   glVertex3f LBrazo, -0.3, -0.4                    
      
   glColor3f  1.0,  0.0,  0.5                          
   glVertex3f LBrazo,  0.3,  0.4                        
   glVertex3f 0.0,  0.3,  0.4                       
   glVertex3f 0.0, -0.3,  0.4                       
   glVertex3f LBrazo, -0.3,  0.4                    
      
   glColor3f  1.0,  0.2,  0.0                          
   glVertex3f LBrazo, -0.3, -0.4                        
   glVertex3f 0.0, -0.3, -0.4                      
   glVertex3f 0.0,  0.3, -0.4                        
   glVertex3f LBrazo,  0.3, -0.4                       
      
   glColor3f  1.0,  0.7,  0.2                          
   glVertex3f 0.0,  0.3,  0.4                         
   glVertex3f 0.0,  0.3, -0.4                        
   glVertex3f 0.0, -0.3, -0.4                        
   glVertex3f 0.0, -0.3,  0.4                        
      
   glColor3f  1.0,  0.8,  0.3                           
   glVertex3f LBrazo,  0.3, -0.4                          
   glVertex3f LBrazo,  0.3,  0.4     
   glVertex3f LBrazo, -0.3,  0.4
   glVertex3f LBrazo, -0.3, -0.4 
 glEnd()
                                                      
 '----------- Ant.Brazo -----------          
  
 glTranslatef   LBrazo, 0.0, 0.0
 glRotatef   AngAntBr, 0.0, 0.0, 1.0
  
 glColor3f       0.0,  0.0,  1.0
 glutSolidSphere 0.5,   11,   11
  
 glBegin(GL_QUADS)                               'Crea las 6 facetas del antebrazo.
   glColor3f  0.0,  1.0,  0.0
   glVertex3f LAntBr,  0.3, -0.3
   glVertex3f 0.0,  0.3, -0.3
   glVertex3f 0.0,  0.3,  0.3
   glVertex3f LAntBr,  0.3,  0.3
  
   glColor3f  0.1,  1.0,  0.2
   glVertex3f LAntBr, -0.3,  0.3
   glVertex3f 0.0, -0.3,  0.3
   glVertex3f 0.0, -0.3, -0.3
   glVertex3f LAntBr, -0.3, -0.3
  
   glColor3f  0.2,  1.0,  0.5
   glVertex3f LAntBr,  0.3,  0.3
   glVertex3f 0.0,  0.3,  0.3
   glVertex3f 0.0, -0.3,  0.3
   glVertex3f LAntBr, -0.3,  0.3
  
   glColor3f  0.4,  1.0,  0.2
   glVertex3f LAntBr, -0.3, -0.3
   glVertex3f 0.0, -0.3, -0.3
   glVertex3f 0.0,  0.3, -0.3
   glVertex3f LAntBr,  0.3, -0.3
  
   glColor3f  0.0,  1.0,  0.5
   glVertex3f 0.0,  0.3,  0.3
   glVertex3f 0.0,  0.3, -0.3
   glVertex3f 0.0, -0.3, -0.3
   glVertex3f 0.0, -0.3,  0.3
  
   glColor3f  0.3,  1.0,  0.2
   glVertex3f LAntBr,  0.3, -0.3
   glVertex3f LAntBr,  0.3,  0.3
   glVertex3f LAntBr, -0.3,  0.3
   glVertex3f LAntBr, -0.3, -0.3
 glEnd()
  
 '------------- Muñeca -------------
  
 glTranslatef    LAntBr, 0.0, 0.0
 glRotatef    AngMunecA, 0.0, 0.0, 1.0
 glRotatef    AngMunecB, 1.0, 0.0, 0.0
  
 glColor3f       1.0,  0.0,  0.0
 glutSolidSphere 0.4,   13,   13
                                                      
 glBegin(GL_QUADS)                               ' Crea las 6 facetas de la muñeca.
   glColor3f  0.0,  0.0,  1.0
   glVertex3f LMunec,  0.3, -0.2
   glVertex3f 0.0,  0.3, -0.2
   glVertex3f 0.0,  0.3,  0.2
   glVertex3f LMunec,  0.3,  0.2
  
   glColor3f  0.2,  0.0,  1.0
   glVertex3f LMunec, -0.3,  0.2
   glVertex3f 0.0, -0.3,  0.2
   glVertex3f 0.0, -0.3, -0.2
   glVertex3f LMunec, -0.3, -0.2
  
   glColor3f  0.0,  0.3,  1.0
   glVertex3f LMunec,  0.3,  0.2
   glVertex3f 0.0,  0.3,  0.2
   glVertex3f 0.0, -0.3,  0.2
   glVertex3f LMunec, -0.3,  0.2
  
   glColor3f  0.0,  0.4,  1.0
   glVertex3f LMunec, -0.3, -0.2
   glVertex3f 0.0, -0.3, -0.2
   glVertex3f 0.0,  0.3, -0.2
   glVertex3f LMunec,  0.3, -0.2
  
   glColor3f  LMunec,  0.0,  1.0
   glVertex3f 0.0,  0.3,  0.2
   glVertex3f 0.0,  0.3, -0.2
   glVertex3f 0.0, -0.3, -0.2
   glVertex3f 0.0, -0.3,  0.2
  
   glColor3f  0.2,  0.2,  1.0
   glVertex3f LMunec,  0.3, -0.2
   glVertex3f LMunec,  0.3,  0.2
   glVertex3f LMunec, -0.3,  0.2
   glVertex3f LMunec, -0.3, -0.2
 glEnd()   
  
 '------------ Dedos-------------
 glTranslatef    LMunec, 0.0, DistDedos
                                                      
 glBegin(GL_QUADS)                               ' Crea la 6 facetas de la primera pinza.
   glColor3f   0.0,  0.6,  0.5
   glVertex3f  LDedos,  0.1, -0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  LDedos,  0.1,  0.1
  
   glColor3f   0.0,  0.8,  0.4
   glVertex3f  LDedos, -0.1,  0.1
   glVertex3f  0.0, -0.1,  0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  LDedos, -0.1, -0.1
  
   glColor3f   0.0,  0.5,  0.3
   glVertex3f  LDedos,  0.1,  0.1
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  0.0, -0.1,  0.1
   glVertex3f  LDedos, -0.1,  0.1
  
   glColor3f   0.0,  0.4,  0.4
   glVertex3f  LDedos, -0.1, -0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  LDedos,  0.1, -0.1
  
   glColor3f   0.0,  0.3,  0.5
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  0.0, -0.1,  0.1
  
   glColor3f   0.0,  0.2,  0.6
   glVertex3f  LDedos,  0.1, -0.1
   glVertex3f  LDedos,  0.1,  0.1
   glVertex3f  LDedos, -0.1,  0.1
   glVertex3f  LDedos, -0.1, -0.1
 glEnd()
  
 glTranslatef    0.0, 0.0, DistDedos*(-2)   
                                                      
 glBegin(GL_QUADS)                               ' Crea la 6 facetas de la segunda pinza.
   glColor3f   0.0,  0.6,  0.5
   glVertex3f  LDedos,  0.1, -0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  LDedos,  0.1,  0.1
  
   glColor3f   0.0,  0.8,  0.4
   glVertex3f  LDedos, -0.1,  0.1
   glVertex3f  0.0, -0.1,  0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  LDedos, -0.1, -0.1
  
   glColor3f   0.0,  0.5,  0.3
   glVertex3f  LDedos,  0.1,  0.1
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  0.0, -0.1,  0.1
   glVertex3f  LDedos, -0.1,  0.1
  
   glColor3f   0.0,  0.4,  0.4
   glVertex3f  LDedos, -0.1, -0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  LDedos,  0.1, -0.1
  
   glColor3f   0.0,  0.3,  0.5
   glVertex3f  0.0,  0.1,  0.1
   glVertex3f  0.0,  0.1, -0.1
   glVertex3f  0.0, -0.1, -0.1
   glVertex3f  0.0, -0.1,  0.1
  
   glColor3f   0.0,  0.2,  0.6
   glVertex3f  LDedos,  0.1, -0.1
   glVertex3f  LDedos,  0.1,  0.1
   glVertex3f  LDedos, -0.1,  0.1
   glVertex3f  LDedos, -0.1, -0.1
 glEnd()
  
 Flip          '<----- Muestra el gráfico en el monitor. -------  
 Sleep 5       ' Una pausa para que el brazo se mueva a una velocidad adecuada.
    
End Sub

 
Dos últimas cosas:

* 1.) Según la versión del compilador de Freebasic, en las funciones "MultiKey()" puede dar problemas. Si te da fallo en esa función tendrías que cambiar todos los "FB.SC_" por "SC_", o al revés (según el caso), y asunto resuelto.

Ejemplo:
If MultiKey(FB.SC_A) Then Xreal=Xreal-1

por

If MultiKey(SC_A) Then Xreal=Xreal-1

Así con todas las funciones "MultiKey()".

* 2.) Este programa funciona correctamente en plataformas Windows. Si compilas el programa bajo Linux o estás con otro sistema operativo no-Windows aunque te permita emular Windows el brazo robot no tendrá en cuenta los límites y llegado a este punto desaparece del monitor. Cuando pides al brazo que vaya a un lugar donde no puede alcanzar, normalmente el brazo se para en ese límite y no sigue. Pero en Linux y otras plataformas esto no lo podrá tener en cuenta porque en el programa, cuando da un "error de función fuera de rango", espera el mensaje: "-1.#IND". Hay que hacer la sustitución de "-1.#IND" por  "QNaN" (normalmente), ó "Inf" ó bién "NaN" dentro del procedimiento "Inversek". Desconozco la sintaxis concreta o si ha de ser en mayúsculas o minúsculas, o si se alterna mayúsculas con minúsculas. Busca en Internet más referencias usando las palabras claves que he señalado en negrita y color marrón rojizo en este párrafo.


Ir a: Cinemática Inversa IV.                     Ir a: Trayectorias.

Subpáginas (1): Brazo por RS232