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 el nivel 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 en la altura.


Conocemos los ejes XYZ y la longitud de las articulacionesCon 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 calculamos 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

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


Como ves en la imagen, hemos resuelto el ángulo de giro de todo el brazo, el ángulo del brazo, el á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 en modo calculadora (no gráfico) que computa la cinemática inversa según los datos que le entras y muestra los ángulos como resultado. Procediendo de esta manera se aprecia mejor el funcionamiento; para más información 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", o bien 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.

(Si usas antivirus Avast has de añadir una exclusión para poder ejecutarlo. Para analizar este o cualquier otro archivo puedes hacer clic aquí)


 Clic aquí y explico cómo sacar los datos por el puerto RS232 o por un puerto serie virtual.



Sobre el programa.
 
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 este lugar:

  (Si haces clic espera unos segundos hasta que comience la descarga.)

 
En FreeBasic se puede manejar punteros, crear estructuras de datos y está orientado a objetos al igual que C++.

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.

Cuando ejecutes el programa podrás mover el brazo de la siguiente manera:
 
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 la articulación del 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

 
Tres ú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.) La última versión del compilador FreeBasic puede darte un error al compilar si tienes un "windows.bi" de otra versión estando en la misma carpeta que el código fuente. Lo que has de hacer es eliminar el "windows.bi" de dicha carpeta (sólo si el código fuente y "windows.bi" está en la misma carpeta). En los programas de descarga lo dejo para poder ser compatible con versiones anteriores, pero si te sale el Error 58 has de eliminar "windows.bi" de la carpeta que acompaña al código fuente.

windows.bi error 58 FreeBasic

Ejemplos de tipos de errores del compilador en las últimas versiones:

C:\Program Files (x86)\FreeBASIC\inc\win\wincon.bi(412) error 58: Illegal specification, at parameter 3 (lpSecurityAttributes) of CreateConsoleScreenBuffer() in 'declare function CreateConsoleScreenBuffer(byval dwDesiredAccess as DWORD, byval dwShareMode as DWORD, byval lpSecurityAttributes as const SECURITY_ATTRIBUTES ptr, byval dwFlags as DWORD, byval lpScreenBufferData as LPVOID) as HANDLE'
C:\Program Files (x86)\FreeBASIC\inc\win\winbase.bi(2704) error 58: Illegal specification, at parameter 7 (Arguments) of FormatMessageA() in 'declare function FormatMessageA(byval dwFlags as DWORD, byval lpSource as LPCVOID, byval dwMessageId as DWORD, byval dwLanguageId as DWORD, byval lpBuffer as LPSTR, byval nSize as DWORD, byval Arguments as va_list ptr) as DWORD'
C:\Program Files (x86)\FreeBASIC\inc\win\winbase.bi(2705) error 58: Illegal specification, at parameter 7 (Arguments) of FormatMessageW() in 'declare function FormatMessageW(byval dwFlags as DWORD, byval lpSource as LPCVOID, byval dwMessageId as DWORD, byval dwLanguageId as DWORD, byval lpBuffer as LPWSTR, byval nSize as DWORD, byval Arguments as va_list ptr) as DWORD'
C:\Program Files (x86)\FreeBASIC\inc\win\winbase.bi(2710) error 58: Illegal specification, at parameter 7 (Arguments) of FormatMessage() in 'declare function FormatMessage alias "FormatMessageA"(byval dwFlags as DWORD, byval lpSource as LPCVOID, byval dwMessageId as DWORD, byval dwLanguageId as DWORD, byval lpBuffer as LPSTR, byval nSize as DWORD, byval Arguments as va_list ptr) as DWORD'

* 3.) 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 "1.#QNAN" (normalmente), ó "Inf" o bien "NaN" dentro del procedimiento "Inversek". Desconozco la sintaxis concreta.

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

Subpáginas (2): Brazo por RS232 Script