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
