La cinemtica inversa se refiere a la utilizacin de las ecuaciones cinemticas de

un robot para determinar los parmetros comunes que proporcionan una posicin
deseada del efector final. Especificacin del movimiento de un robot de manera
que su extremo efector logra una tarea deseada es conocido como planificacin
de movimientos.La cinemtica inversa transforma el plan de movimiento en
trayectorias del actuador en conjuntos para el robot. El movimiento de una cadena
cinemtica si se trata de un robot o un personaje animado es modelada por las
ecuaciones cinemticas de la cadena. Estas ecuaciones definen la configuracin
de la cadena en trminos de sus parmetros conjuntos. Delantero cinemtica
utiliza los parmetros comunes para calcular la configuracin de la cadena, y la
cinemtica inversa invierte este clculo para determinar los parmetros de
conjuntos que logra una configuracin deseada.
Una figura articulada consta de un conjunto de segmentos rgidos conectados
mediante articulaciones. Los mltiples ngulos que pueden adoptar estas
articulaciones permiten un nmero indefinido de configuraciones o posiciones de
la figura. La solucin al problema cinemtico inverso consiste en encontrar los
valores que deben adoptar las coordenadas articulares del robot q = [q1, q2, . . . ,
qn] para que su extremo se posicione y oriente segn una determinada
configuracin deseada.
Para nuestro objetivo de simular un robot de 4 grados de libertad primero con los
datos de la longitud de las articulaciones y el angulo de cabeceo del efector,
debemos encontrar todos los angulos del brazo robot

Lo primero de todo es hallar el ngulo de giro de todo el brazo

Primero hemos de calcular el Mdulo formado por los catetos X e Y:

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

Despus hacemos una reconversin de variables, las cuales nos har de puente
para usar el mismo mtodo de resolucin de ngulos
Xprima = Modulo
Yprima = Z
Tenemos Xprima e Yprima que hemos deducido del espacio 3D, y aadimos la
variable AlturaH que es la distancia entre la base (suelo) y el hombro del brazo.
Tenemos las soluciones para calcular los 4 grados de libertad:
-El ngulo de giro de todo el brazo
-ngulo del brazo
-ngulo del antebrazo
-ngulo de la mueca
 UNIVERSIDAD MAYOR DE SAN SIMN
FACULTAD DE CIENCIAS Y TECNOLOGA
CARRERA DE INGENIERA ELECTROMECANICA

PROYECTO ROBOTICA

Estudiante: CHRISTIAN JESS MRQUEZ HURTADO

Cochabamba 7 de Julio del 2014

#Include Once "GL/gl.bi" Declare Sub InverseK

#Include Once "GL/glu.bi" Declare Sub DibujaBrazo

#Include Once "GL/glut.bi" Declare Sub BuildFont

#Include Once "fbgfx.bi" Declare Sub glPrint(ByVal As Integer, ByVal

As Integer, ByVal As String, ByVal As
#Include Once "createtex.bi" Integer)

#Include Once "windows.bi"

'----------------------------------- Declaracin de LongMunec = 100
variables ------------------------------------------------
-- LongDedos = 50

Dim Shared As Double Pi, Rad, Grad, Terminal = LongMunec+LongDedos

AngGiro, AngBrazo, AngAntBr, AngMunecA,
AngMunecB

Dim Shared As Double LHombro, LBrazo, EjeX=( 200)

LAntBr, LMunec, LDedos, DistDedos=.11,
Espacio, EscenaY, EscenaX, Distancia EjeY=(-100)

Dim Shared As Integer LongBrazo, EjeZ=( 250)

LongAntBr, LongMunec, LongDedos,
EjeV=( 0) ' Ang del cabeceo
AlturaH, Terminal
EjeW=( 0) ' Ang Relativo

EjeD=( 0)
Dim Shared As Integer EjeX, EjeY, EjeZ,
EjeV, EjeW, EjeD

EscenaX=-0
Dim Shared As String Tecla EscenaY=15

Dim Shared As UInteger gbase Distancia=-10

Dim Shared As UInteger texture

Dim Shared As Integer gloop LHombro = AlturaH /100

LBrazo = LongBrazo/100
'-----------------------------------------Carga de LAntBr = LongAntBr/100
Variables------------------------------------------------
----- LMunec = LongMunec/100

LDedos = LongDedos/100
Pi = Atn(1) * 4 Espacio = LBrazo+LAntBr
Rad = Pi / 180

Grad = 180 / Pi Screen 19, 1, , 2

'-=-=-=-=-=-=-=-=-=-=- Ajustes -=-=-=-=-=-=-=- '----------------------------------

=-=-=-=-=-=-
ReDim buffer(256*256*4+4) As UByte

BLoad "Font.bmp", @buffer(0)

AlturaH = 200
texture = CreateTexture(@buffer(0))
LongBrazo = 200
BuildFont
LongAntBr = 200
'---------------------------------- If LCase(Tecla)="d" Then
EjeX=EjeX+1

If LCase(Tecla)="s" Then
glViewport 0, 0, 800, 600 EjeY=EjeY-1

glMatrixMode GL_PROJECTION If LCase(Tecla)="w" Then

EjeY=EjeY+1
glLoadIdentity
If LCase(Tecla)="e" Then
gluPerspective 45.0, 800.0/600.0, 0.2, 255.0 EjeZ=EjeZ-1
glMatrixMode GL_MODELVIEW If LCase(Tecla)="q" Then
EjeZ=EjeZ+1
glLoadIdentity

Else
glShadeModel GL_FLAT

glClearColor 0.0, 0.0, 0.0, 0.5

If MultiKey(FB.SC_A) Then
glClearDepth 1.0 EjeX=EjeX-1
glEnable GL_DEPTH_TEST If MultiKey(FB.SC_D) Then
EjeX=EjeX+1
glDepthFunc GL_LEQUAL
If MultiKey(FB.SC_S) Then
glHint
EjeY=EjeY-1
GL_PERSPECTIVE_CORRECTION_HINT,
GL_NICEST If MultiKey(FB.SC_W) Then
EjeY=EjeY+1

If MultiKey(FB.SC_E) Then
glBlendFunc GL_SRC_ALPHA, GL_ONE
EjeZ=EjeZ-1
glEnable GL_TEXTURE_2D
If MultiKey(FB.SC_Q) Then
EjeZ=EjeZ+1

'-=-=-=-=-=-=-=-=-=-Programa Principal-=--=-
=-=-=-=-=-=-=-=-=-=-=-=-

If LCase(Tecla)="x" Then
While Tecla<>Chr(27) EjeV=EjeV-1

If LCase(Tecla)="z" Then
EjeV=EjeV+1
Tecla=InKey
If LCase(Tecla)="c" Then
EjeW=EjeW-1

If MultiKey(FB.SC_LSHIFT) Then If LCase(Tecla)="v" Then

EjeW=EjeW+1

If LCase(Tecla)="a" Then
EjeX=EjeX-1
 If LCase(Tecla)="n" Then

If EjeD>0 Then Wend

EjeD=EjeD-1

EndIf End

EndIf

If LCase(Tecla)="m" Then Sub InverseK

If EjeD<40 Then

EjeD=EjeD+1 Dim As Double Afx, Afy, LadoA, LadoB,

Alfa, Beta, Gamma, Modulo, Hipotenusa,
EndIf Xprima, Yprima

EndIf Static As Integer Xaux, Yaux, Zaux, Vaux

If MultiKey(FB.SC_LEFT) ' Cinemtica Inversa---------------------------------

Then EscenaX=EscenaX-.5 ------------------------------------------

If MultiKey(FB.SC_RIGHT) Then
EscenaX=EscenaX+.5
Modulo =
If MultiKey(FB.SC_UP) Then Sqr((EjeX*EjeX)+(EjeY*EjeY))
EscenaY=EscenaY+.5
AngGiro = Atan2(EjeY, EjeX)*Grad
If MultiKey(FB.SC_DOWN) Then
EscenaY=EscenaY-.5

If MultiKey(FB.SC_PAGEUP) Xprima=Modulo
Then Distancia=Distancia+.1
Yprima=EjeZ
If MultiKey(FB.SC_PAGEDOWN)
Then Distancia=Distancia-.1

Afx=Cos(Rad*EjeV)*Terminal

EndIf LadoB=Xprima-Afx

If Tecla=Chr(255)+"k" Then Afy=Sin(Rad*EjeV)*Terminal

Close LadoA=Yprima-Afy-AlturaH

End

EndIf Hipotenusa=Sqr((LadoA^2)+(LadoB^
2))

InverseK
Alfa=Atan2(LadoA,LadoB)
DibujaBrazo
 Beta=Acos(((LongBrazo^2)- Vaux=EjeV
(LongAntBr^2)+(Hipotenusa^2))/(2*LongBraz
o*Hipotenusa))

End Sub

AngBrazo= (Alfa+Beta)*Grad
' Ang BRAZO (en Radianes).
Sub DibujaBrazo

Gamma=Acos(((LongBrazo^2)+(Long
AntBr^2)- Dim As Double ContX, ContY
(Hipotenusa^2))/(2*LongBrazo*LongAntBr))

AngAntBr=(-((180*Rad)-
Gamma))*Grad ' Ang ANTEBRAZO ' ----------------------Animacin OpenGL--
(en Radianes). ------------------

AngMunecA= EjeV-AngBrazo- glClear(GL_COLOR_BUFFER_BIT

AngAntBr ' Ang MUECA (en Or GL_DEPTH_BUFFER_BIT)
Radianes).
glBindTexture GL_TEXTURE_2D, 0
AngMunecB= EjeW '+AngGiro
glLoadIdentity

glDisable GL_BLEND
If (Str(AngBrazo)="-1.#IND") Or
(Str(AngAntBr)= "-1.#IND") Or
(Str(AngMunecA)= "-1.#IND") Then '--------------------------Plano Base--------
------------

EjeX=Xaux 'Si hay

ngulos imposibles, pasar a posicin anterior. glTranslatef 0, 0, Distancia
EjeY=Yaux glRotatef EscenaX, 0.0, 1.0, 0.0
EjeZ=Zaux glRotatef EscenaY, 1.0, 0.0, 0.0
EjeV=Vaux

InverseK For contY=0 To Espacio Step .5

For contX=0 To Espacio Step

.5
EndIf

glBegin(GL_LINE_LOOP)
Xaux=EjeX
glColor3f 1.0, 1.0, 1.0
Yaux=EjeY
glVertex3f contx, -LHombro-
Zaux=EjeZ .01, conty
 glVertex3f -contx, -LHombro- glVertex3f 0.4, -LHombro, 0.6
.01, conty

glVertex3f -contx, -LHombro-

.01, -conty
glColor3f 0.2, 0.1, 0.4
glVertex3f contx, -LHombro-
.01, -conty glVertex3f 0.4, -LHombro, -0.6

glEnd glVertex3f -0.4, -LHombro, -0.6

Next glVertex3f -0.4, 0.0, -0.6

Next glVertex3f 0.4, 0.0, -0.6

'------ Cubo del Hombro ------------ glColor3f 0.2, 0.2, 0.5

glVertex3f -0.4, 0.0, 0.6

glRotatef AngGiro, 0.0, 1.0, 0.0 glVertex3f -0.4, 0.0, -0.6

glVertex3f -0.4, -LHombro, -0.6

glBegin(GL_QUADS) glVertex3f -0.4, -LHombro, 0.6

'glColor3f 0.1, 0.2, 0.4

'glVertex3f 0.4, 0.0, -0.6 glColor3f 0.2, 0.2, 0.5

'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

'glVertex3f 0.4, 0.0, 0.6 glVertex3f 0.4, -LHombro, 0.6

glVertex3f 0.4, -LHombro, -0.6

'glColor3f 0.2, 0.2, 0.4 glEnd()

'glVertex3f 0.4, -LHombro, 0.6

'glVertex3f -0.4, -LHombro, 0.6 '-------------------------brazo-----------------

-----
'glVertex3f -0.4, -LHombro, -0.6

'glVertex3f 0.4, -LHombro, -0.6

glTranslatef 0.0, 0.0, 0.0

glRotatef AngBrazo, 0.0, 0.0, 1.0

glColor3f 0.2, 0.1, 0.4

glVertex3f 0.4, 0.0, 0.6

glColor3f 0.8, 0.0, 0.0
glVertex3f -0.4, 0.0, 0.6
glutSolidSphere 0.6, 11, 11
glVertex3f -0.4, -LHombro, 0.6
glBegin(GL_QUADS) '

glColor3f 0.5, 0.1, 0.2 'glColor3f 0.6, 0.1, 0.3

glVertex3f LBrazo, 0.3, -0.4 'glVertex3f LBrazo, 0.3, -0.4

glVertex3f 0.0, 0.3, -0.4 'glVertex3f LBrazo, 0.3, 0.4

glVertex3f 0.0, 0.3, 0.4 'glVertex3f LBrazo, -0.3, 0.4

glVertex3f LBrazo, 0.3, 0.4 'glVertex3f LBrazo, -0.3, -0.4

glEnd()

glColor3f 0.5, 0.1, 0.2

glVertex3f LBrazo, -0.3, 0.4 '------------------------Ant.Brazo------------

-------
glVertex3f 0.0, -0.3, 0.4

glVertex3f 0.0, -0.3, -0.4

glTranslatef LBrazo, 0.0, 0.0
glVertex3f LBrazo, -0.3, -0.4
glRotatef AngAntBr, 0.0, 0.0, 1.0

glColor3f 0.5, 0.2, 0.1

glColor3f 0.0, 0.3, 0.0
glVertex3f LBrazo, 0.3, 0.4
glutSolidSphere 0.5, 11, 11
glVertex3f 0.0, 0.3, 0.4

glVertex3f 0.0, -0.3, 0.4

glBegin(GL_QUADS)
glVertex3f LBrazo, -0.3, 0.4
glColor3f 0.2, 0.4, 0.1

glVertex3f LAntBr, 0.3, -0.3

glColor3f 0.5, 0.2, 0.1
glVertex3f 0.0, 0.3, -0.3
glVertex3f LBrazo, -0.3, -0.4
glVertex3f 0.0, 0.3, 0.3
glVertex3f 0.0, -0.3, -0.4
glVertex3f LAntBr, 0.3, 0.3
glVertex3f 0.0, 0.3, -0.4

glVertex3f LBrazo, 0.3, -0.4

glColor3f 0.2, 0.4, 0.1

glVertex3f LAntBr, -0.3, 0.3

'glColor3f 0.6, 0.2, 0.2
glVertex3f 0.0, -0.3, 0.3
'glVertex3f 0.0, 0.3, 0.4
glVertex3f 0.0, -0.3, -0.3
'glVertex3f 0.0, 0.3, -0.4
glVertex3f LAntBr, -0.3, -0.3
'glVertex3f 0.0, -0.3, -0.4

'glVertex3f 0.0, -0.3, 0.4

glColor3f 0.1, 0.4, 0.1
 glVertex3f LAntBr, 0.3, 0.3 glColor3f 0.0, 0.0, 0.3

glVertex3f 0.0, 0.3, 0.3 glutSolidSphere 0.4, 13, 13

glVertex3f 0.0, -0.3, 0.3

glVertex3f LAntBr, -0.3, 0.3 glBegin(GL_QUADS)

glColor3f 0.0, 0.0, 0.7

glColor3f 0.1, 0.4, 0.1 glVertex3f LMunec, 0.3, -0.21

glVertex3f LAntBr, -0.3, -0.3 glVertex3f 0.0, 0.3, -0.21

glVertex3f 0.0, -0.3, -0.3 glVertex3f 0.0, 0.3, 0.21

glVertex3f 0.0, 0.3, -0.3 glVertex3f LMunec, 0.3, 0.21

glVertex3f LAntBr, 0.3, -0.3

glColor3f 0.0, 0.0, 0.7

'glColor3f 0.0, 0.0, 0.0 glVertex3f LMunec, -0.3, 0.21

'glVertex3f 0.0, 0.3, 0.3 glVertex3f 0.0, -0.3, 0.21

'glVertex3f 0.0, 0.3, -0.3 glVertex3f 0.0, -0.3, -0.21

'glVertex3f 0.0, -0.3, -0.3 glVertex3f LMunec, -0.3, -0.21

'glVertex3f 0.0, -0.3, 0.3

glColor3f 0.0, 0.0, 0.5

'glColor3f 0.1, 0.5, 0.2 glVertex3f LMunec, 0.3, 0.21

'glVertex3f LAntBr, 0.3, -0.3 glVertex3f 0.0, 0.3, 0.21

'glVertex3f LAntBr, 0.3, 0.3 glVertex3f 0.0, -0.3, 0.21

'glVertex3f LAntBr, -0.3, 0.3 glVertex3f LMunec, -0.3, 0.21

'glVertex3f LAntBr, -0.3, -0.3

glEnd() glColor3f 0.0, 0.0, 0.5

glVertex3f LMunec, -0.3, -0.21

'------------------------ Mueca ------------- glVertex3f 0.0, -0.3, -0.21

--------
glVertex3f 0.0, 0.3, -0.21

glVertex3f LMunec, 0.3, -0.21

glTranslatef LAntBr, 0.0, 0.0

glRotatef AngMunecA, 0.0, 0.0, 1.0

'glColor3f LMunec, 0.0, 1.0
glRotatef AngMunecB, 1.0, 0.0, 0.0
'glVertex3f 0.0, 0.3, 0.21
 'glVertex3f 0.0, 0.3, -0.21 glVertex3f 0.0, 0.1, 0.1

'glVertex3f 0.0, -0.3, -0.21 glVertex3f 0.0, -0.1, 0.1

'glVertex3f 0.0, -0.3, 0.21 glVertex3f LDedos, -0.1, 0.1

'glColor3f 0.2, 0.2, 1.0 glColor3f 0.0, 0.4, 0.4

'glVertex3f LMunec, 0.3, -0.21 glVertex3f LDedos, -0.1, -0.1

'glVertex3f LMunec, 0.3, 0.21 glVertex3f 0.0, -0.1, -0.1

'glVertex3f LMunec, -0.3, 0.21 glVertex3f 0.0, 0.1, -0.1

'glVertex3f LMunec, -0.3, -0.21 glVertex3f LDedos, 0.1, -0.1

glEnd()

'glColor3f 0.0, 0.3, 0.5

'--------------Dedos------------------ 'glVertex3f 0.0, 0.1, 0.1

DistDedos=(EjeD/100)+.11 'glVertex3f 0.0, 0.1, -0.1

glTranslatef LMunec, 0.0, 'glVertex3f 0.0, -0.1, -0.1

DistDedos
'glVertex3f 0.0, -0.1, 0.1

glBegin(GL_QUADS)
glColor3f 0.0, 0.2, 0.6
glColor3f 0.0, 0.6, 0.5
glVertex3f LDedos, 0.1, -0.1
glVertex3f LDedos, 0.1, -0.1
glVertex3f LDedos, 0.1, 0.1
glVertex3f 0.0, 0.1, -0.1
glVertex3f LDedos, -0.1, 0.1
glVertex3f 0.0, 0.1, 0.1
glVertex3f LDedos, -0.1, -0.1
glVertex3f LDedos, 0.1, 0.1
glEnd()

glColor3f 0.0, 0.8, 0.4

glTranslatef 0.0, 0.0, DistDedos*(-2)
glVertex3f LDedos, -0.1, 0.1 ' En esta posicin, 'DistDedos*(-2)' invierte

glVertex3f 0.0, -0.1, 0.1

glVertex3f 0.0, -0.1, -0.1 ' la

posicin 'DistDedos' de arriba.
glVertex3f LDedos, -0.1, -0.1
glBegin(GL_QUADS)

glColor3f 0.0, 0.6, 0.5

glColor3f 0.0, 0.5, 0.3
glVertex3f LDedos, 0.1, -0.1
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
glEnable GL_BLEND
glColor3f 0.0,1.0,0.0 ' Color de las
letras y nmeros.
glPrint 200, 460, "X = " +
Left(Str(EjeX) ,6) , 0
glPrint 200, 440, "Y = " +
Left(Str(EjeY) ,6) , 0
glPrint 200, 420, "Z = " +
Left(Str(EjeZ) ,6) , 0
glPrint 1, 460, "Ang. Giro Br.: " + Left
(Str( AngGiro) ,6) , 0
glPrint 1, 440, "Ang. Brazo : " + Left
(Str(-AngBrazo+90) ,6) , 0
glPrint 1, 420, "Ang. Codo : " + Left
(Str(-AngAntBr) ,6) , 0
glPrint 1, 400, "Ang. Cabeceo : " +
Left (Str(-AngMunecA) ,6) + " (" + Left
(Str(EjeV),6) + ")", 0
glPrint 1, 380, "Ang. Balanceo: " +
Left (Str( AngMunecB) ,6) , 0
glPrint 1, 360, "Mano : " + Left
(Str( EjeD) ,6) , 0
Flip '<----- Muestra el grfico por
pantalla ------
 Sleep 3 'Demoramos un tiempo para glTranslated 6, 0, 0
que no vaya demasiado rpido.
glEndList

Next
End Sub
End Sub

Sub BuildFont
Sub glPrint(ByVal x As Integer, ByVal y As
Integer, ByVal glstring As String, ByVal gset
As Integer)
Static cx As Single
If gset>1 Then gset=1
Static cy As Single
glBindTexture GL_TEXTURE_2D,
texture

gbase = glGenLists(256) glDisable GL_DEPTH_TEST

glBindTexture GL_TEXTURE_2D, glMatrixMode GL_PROJECTION
texture
glPushMatrix
For gloop = 0 To 255
glLoadIdentity

glOrtho 0, 640, 0, 480,-1, 1

cx = (gloop Mod 16)/16.0
glMatrixMode
cy = (gloop\16)/16.0 GL_MODELVIEW

glPushMatrix
glNewList gbase+gloop, glLoadIdentity
GL_COMPILE
glTranslated x, y, 0
glBegin GL_QUADS
glListBase gbase-
glTexCoord2f cx, 1- 32+(128*gset)
cy-0.0625
glCallLists
glVertex2i 0, 0 Len(glstring),GL_BYTE, StrPtr(glstring)
glTexCoord2f glMatrixMode
cx+0.0625, 1-cy-0.0625 GL_PROJECTION

glVertex2i 12,0 glPopMatrix

glTexCoord2f glMatrixMode
cx+0.0625, 1-cy GL_MODELVIEW

glVertex2i 12, 12 glPopMatrix

glTexCoord2f cx,1-cy glEnable GL_DEPTH_TEST
glVertex2i 0, 12 End Sub
glEnd