Programas para Robots.
  Cinematica III
 

Me he trasladado a este "Site":

http://sites.google.com/site/proyectosroboticos/

Esta web dejó de actualizarse el 3 de marzo de 2009 a las 21:51h.








 
Cinemática Inversa de un Brazo Robot de 5 grados de libertad.
 
Programado con Freebasic (implementación de OpenGL)


Casi todos los brazos robots de juguete o caseros que he visto en YouTube suelen ser exibiciones torpes de saber manejar servos con la ayuda de un microcontrolador

Se puede hacer mucho más con la ayuda de unas matemáticas básicas. Aplicando primero la Cinemática Inversa (IK), conseguimos resolver el tema de posicionar el brazo en coordenadas específicas del espacio. Y con la ayuda del Algoritmo de Bresenham, nos desplazaremos linealmente en el espacio de un punto a otro. Estas dos herramientas nos permiten, por ejemplo, que tu brazo robot pueda dibujar en un papel, o bien, realizar movimientos mucho más complejos. Pincha
aquí para ver un ejemplo de un brazo robot con un movimiento armónico, moviéndose todo el brazo a la vez.
Si es la primera vez que das con estos dos temas, sería bueno que comenzases por lo sencillo y poco a poco puedes convertirlo en algo más sofisticado. Puedes coger el programa que está en el apartado: "Cinematica I" y una vez que lo tengas claro, pasar al siguiente: "Cinemática II". Modifica esos programas a tus necesidades, mandando los ángulos calculados a tus servos o microcontroladores.

En este apartado: "
Cinemática III" he tenido que usar una herramienta gráfica llamada "OpenGL" para visualizar el Brazo Robot en 3D. El programa que expongo más abajo es un Brazo Robot en 3D que se puede moverse con el teclado.
En esta página he puesto un enlace para que puedas bajarte dos programas de Brazos Robots en 3D que podrás visualizar en tu monitor. Uno se llama "BrazoSimple.exe" y el otro "BrazoComplejo.exe". BrazoSimple es un ejecutable muy parecido al programa que está más abajo. Con el teclado mueves el brazo en las coordenadas XYZ e independientemente los ángulos de la muñeca: cabeceo (Pitch) y balanceo (Roll).
 
El ejecutable "BrazoComplejo.exe" usa, además de la cinemática inversa, el algoritmo de Bresenham. Se puede mover a través del teclado y además puede leer ficheros de extensión "PLT" (de plotter o CNC), moviéndose sólo para hacer que dibuja. Hace poco que aprendí lo básico de OpenGL y de momento no sé cómo mantener lo dibujado sin que se borre, pero los movimientos para dibujar son totalmente reales. Si pulsas la tecla "F2" verás que te dará la opción de cargar un fichero de extensión PLT y se pondrá en marcha una vez cargado.  
 
 
 



El Programa.
 
Expongo las referencia del material que fui extrayendo hasta conseguir el programa que está más abajo. La aparente complejidad del programa 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 que luego modifiqué hasta formar el brazo robot.

El programa está escrito en FreeBasic, concretamente en FreeBasic IDE (FBIDE),  puedes bajártelo desde esta web:
 
 
 
 
Apenas mide 6 megas y es intuitivo.
 
La cinemática inversa la resolví gracias a una hoja de cálculo que encontré en la siguiente web:
 
http://www.lynxmotion.com/images/files/inversek.xls
 
Implementé los datos de esta hoja de cálculo y lo convertí en un programa con el que podía interactuar libremente con el teclado. La parte gráfica la resolví con OpenGL.
 
Asegúrate de que tienes las librerías
GLU32.DLL y GLUT32.DLL en el directorio WINDOWS, o bien, en SYSTEM, porque sinó no funcionará el interface gráfico. En caso de no tener esas librerías puedes descargarlas cliqueando en las DLLs indicadas en azul.
 
Cuando ejecutes el programa puedes mover el brazo de la siguiente forma:
 
Teclas:

A D —–> Mueve linealmente el brazo en el eje X.
Q E —–> Mueve linealmente el brazo en el eje Y.
W S —-> Mueve linealmente el brazo en el eje Z.
Z X —–> Cabeceo de la muñeca.
C V —–> Balanceo de la muñeca.
N M —-> Abre o cierra la mano.

Pulsar "Esc" para salir.

Este es el programa:


#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, Cabeceo, Balance, AngGiro, AngBrazo, AngAntBr, AngMunecA, AngMunecB
 
Dim Shared As Integer  LongBrazo, LongAntBr, LongMunec, AlturaH
Dim Shared As Single   Xreal, Yreal, Zreal
Dim Shared As Single   LHombro, LBrazo, LAntBr, LMunec, LDedos, DistDedos=.11, Espacio, EscenaY, EscenaX, Distancia
Dim  As Single   EjeX, EjeY, EjeZ
'---------Configurar escenario para el OpenGL---------
Screen 12, 16, , 2             
glClearColor 0.0, 0.0, 0.0, 0.0
glMatrixMode GL_PROJECTION     
glViewport 0, 0, 640, 480      
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 brazo.)
LongBrazo  = 250     ' Longitud Brazo.    
LongAntBr  = 300     ' Longitud AnteBrazo.
LongMunec  = 100     ' Longitud Muñeca.
XReal=(300)          ' Posicion Inicial X.
YReal=(  0)          ' Posicion Inicial Y.
ZReal=(150)+AlturaH  ' Posicion Inicial Z.
Cabeceo=-90          ' Ang Relativo Inicial de la Muñeca.
Balance=0
EscenaX=0
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
LDedos     = 0.49
Espacio    = (LBrazo+LAntBr)
'-------------------------------------------------------------------
'-=-=-=-=--=-Programa Principal-=-=-=-=-=-=-=-=-=-=-=-
While Not MultiKey (SC_ESCAPE)
     InverseK
 
      ' Control del Brazo con el teclado.
 
      If MultiKey(SC_A) Then Xreal=Xreal-1
      If MultiKey(SC_D) Then Xreal=Xreal+1
 
      If MultiKey(SC_S) Then Yreal=Yreal-1
      If MultiKey(SC_W) Then Yreal=Yreal+1
 
      If MultiKey(SC_Q) Then Zreal=Zreal-1   
      If MultiKey(SC_E) Then Zreal=Zreal+1   
 
      If MultiKey(SC_Z) Then Cabeceo=Cabeceo-.5   
      If MultiKey(SC_X) Then Cabeceo=Cabeceo+.5
 
      If MultiKey(SC_C) Then Balance=Balance-.5    
      If MultiKey(SC_V) Then Balance=Balance+.5
 
      If MultiKey(SC_N) Then
           If DistDedos>.11 Then DistDedos=DistDedos-.005
      EndIf        
      If MultiKey(SC_M) Then
          If DistDedos<.4 Then DistDedos=DistDedos+.005
      EndIf
 
      If MultiKey(SC_LEFT)     Then EscenaX=EscenaX-.5
      If MultiKey(SC_RIGHT)    Then EscenaX=EscenaX+.5
      If MultiKey(SC_UP)       Then EscenaY=EscenaY+.5
      If MultiKey(SC_DOWN)     Then EscenaY=EscenaY-.5
 
      If MultiKey(SC_PAGEUP)   Then Distancia=Distancia+.1
      If MultiKey(SC_PAGEDOWN) Then Distancia=Distancia-.1
 
      If MultiKey(SC_F10)   Then  Xreal=0
      If MultiKey(SC_F11)   Then  Yreal=0
      If MultiKey(SC_F12)   Then  Zreal=0
Wend
End
Sub InverseK
 
     Dim    As Double   Afx, Afy, LadoA, LadoB, Alfa, Beta, Gamma, Modulo, 
                                       Hipotenusa, Xprima, Yprima
     Static As Single   Xaux, Yaux, Zaux, Baux, Caux
   '  ----------------------Cinemática Inversa----------------
      Modulo  = Sqr(Abs(Xreal^2)+Abs(Yreal^2))
      AngGiro = (ATan2(Yreal, Xreal))*GRad
 
      Xprima=modulo
      Yprima=Zreal
 
      Afx=Cos(Rad*Cabeceo)*LongMunec
      LadoB=Xprima-Afx
 
      Afy=Sin(Rad*Cabeceo)*LongMunec
      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 Radianes).----
      AngBrazo= (Alfa+Beta)*GRad  
      '--------------------------------------     
 
      Gamma=Acos(((LongBrazo^2)+(LongAntBr^2)-(Hipotenusa^2))/((2*LongBrazo)*LongAntBr))
     
      '----Ang ANTEBRAZO (en Radianes).----
      AngAntBr=(-((180*Rad)-Gamma))*GRad
      '-------------------------------------------
 
      '----Ang MUÑECAs (en Radianes).----
      AngMunecA= Cabeceo-AngBrazo-AngAntBr
      AngMunecB= Balance
      '----------------------------------------
      If (Str(AngBrazo)="-1.#IND") Or (Str(AngAntBr)= "-1.#IND") Then
            Xreal=Xaux 'Si hay ángulos imposibles pasar a posición anterior.
            Yreal=Yaux
            Zreal=Zaux
            Cabeceo=Baux
            Balance=Caux
 
            InverseK
 
      EndIf
      Xaux=Xreal
      Yaux=Yreal
      Zaux=Zreal
      Baux=Cabeceo
      Caux=Balance
  
      DibujaBrazo
  
End Sub
Sub DibujaBrazo
' -------Animación OpenGL--------------------
 
 glClear(GL_COLOR_BUFFER_BIT Or GL_DEPTH_BUFFER_BIT)  
 glLoadIdentity                                    
 
 '----------hombro--------------------
 
 glTranslatef       0.0, 0.0, Distancia-16  
 glRotatef      35, 1.0, 0.0, 0.0     
 glRotatef EscenaX, 0.0, 1.0, 0.0
 glRotatef EscenaY, 1.0, 0.0, 0.0
 glBegin(GL_QUADS)                                                                    
       glColor3f   0.2,  0.2,  0.3                     
       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)
     glColor3f   0.1,  0.2,  0.4                       
     glVertex3f  0.4,  0.0, -0.6           
     glVertex3f -0.4,  0.0, -0.6           
     glVertex3f -0.4,  0.0,  0.6           
     glVertex3f  0.4,  0.0,  0.6           
    
     glColor3f   0.2,  0.2,  0.4           
     glVertex3f  0.4, -LHombro,  0.6       
     glVertex3f -0.4, -LHombro,  0.6       
     glVertex3f -0.4, -LHombro, -0.6                       
     glVertex3f  0.4, -LHombro, -0.6          
    
     glColor3f   0.2,  0.1,  0.4                        
     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.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
 
 glColor3f       0.0,  1.0,  0.0
 glutSolidSphere 0.6,   11,   11
 
 glBegin(GL_QUADS)
   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)
   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)
   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)
   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)
   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()
 
 glLoadIdentity
 
 Flip'<----- Muestra el gráfico por pantalla-------  
  
End Sub
 
  14982 visitantes (29295 clics a subpáginas)  
 
=> ¿Desea una página web gratis? Pues, haz clic aquí! <=