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

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

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: Haz clic aquí para más información.

Cliquea aquí para bajarte el código fuente y ejecutable de "BrazoSimple".

(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.
Clic aquí y obtendrás código fuente y ejecutable de un brazo con datos alfanuméricos en pantalla.

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:

(Elige "FBIde + FreeBASIC installer".)

- http://fbide.freebasic.net/download -

Clic aquí para ver un tutorial rápido sobre la instalación.

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.

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.