Dentro de los brazos robots tipo Scara, los hay con un tercer grado de libertad y aquí lo llamaremos: eje de la muñeca. La muñeca nos puede servir para orientar el terminal. Imagina que el brazo Scara toma un objeto y lo quieres dejar en otro lugar con un ángulo determinado. La muñeca se encargará de ello. En la industria del empaquetaje suele necesitar esta opción. Ahora bien, la mayoría de las veces se usa el ángulo de la muñeca y la longitud de ésta se omite, sólo es para orientar el terminal. No hay problema. Si sólo vamos a usar el eje de la muñeca como ángulo y no tiene longitud has de poner a cero el valor de la longitud de la muñeca, eso es todo. Y si necesitas una longitud para la muñeca, le pones la que corresponda en el código fuente.
(Si no ves la imagen, haz clic en el botón -Actualizar- de tu navegador)
Antes de seguir hay que explicar una cosa sobre el ángulo de cabeceo. Es un ángulo que tu le das al brazo para que esa posición se mantenga constante desde el punto de vista del observador aunque muevas el brazo a otra posición. La cinemática inversa también calcula un ángulo de muñeca, pero son cosas relativamente distintas aunque dependientes la una con la otra. El que calcula la cinemática inversa cambia su valor en la medida en que cambia la posición del brazo para mantener el ángulo de cabeceo (pitch) en la misma posición en todo momento. Si no me explico bien, cuando ejecutes el programa e interactúes con él entenderás a qué me refiero.
Recuerda que no es lo mismo Atan() que Atan2(). Si no sabes cuál es la diferencia ves al apartado "Cinemática Inversa I", donde además encontrarás información sobre las razones trigonométricas.
Más abajo tienes un programa de ejemplo que aplica la cinemática inversa para 3 grados de libertad: brazo, antebrazo y cabeceo de muñeca (pitch).
 Los nombres de las variables que están en la imagen de arriba se corresponde con las variables que usa el código fuente del programa, así podrás ver de dónde viene cada una de las fórmulas trigonométricas. Ten presente que los cálculos del programa se han adaptado para poder situar las tres líneas que forma el brazo Scara de 3GL en el monitor. El eje X se corresponde con el plano que estamos acostumbrados en matemáticas, pero el eje Y no comienza en la esquina inferior izquierda, sino en la parte superior izquierda. Por esta razón en el código fuente interviene la variable BaseY al comienzo de la cinemática inversa.
El código está escrito en FreeBasic, pero es compatible con Qbasic. Si usas otro lenguaje de programación, traducir "Basic" y con las variables declaradas en "tipo", es lo más sencillo del mundo.
The translation could modify the code. Use the code without translating or download the program by clicking the link above.
WindowTitle "Cinemática Inversa (IK) para 3 Grados de Libertad."
Screen 12
Cls
Dim As Double pi, rad, grad
Dim As Double PYa, PYb,PYc, PXa, PXb, PXc, Afx, Afy, LadoA, LadoB, Alfa, Beta, Gamma
Dim As Double AngAntBr,AngBrazo, BrazoPY, BrazoPX, AntBrPY,_
AntBrPX, MunecPY, MunecPX, Hipotenusa, AngMunec
Dim As Integer BaseY, BaseX, LongBrazo, LongAntBr, LongMunec, Xaux, Yaux, Caux, x, y, Cabeceo
Dim As String tecla
pi = Atn(1) * 4
rad = pi / 180
grad = 180 / pi
BaseX = 300 ' Punto X base (hombro) Situamos el brazo en pantalla.
BaseY = 250 ' Punto Y base (hombro)
LongBrazo = 100 ' Longitud brazo.
LongAntBr = 100 ' Longitud antebrazo.
LongMunec = 40 ' Longitud muñeca.
x=(100) ' Posición inicial X. Damos las coordenadas iniciales a la punta del brazo.
y=(150)+BaseY ' Posición inicial Y. Se puede modificar los valores que están dentro del paréntesis.
' Procura que esté dentro del área de trabajo.
Cabeceo=0 ' Ángulo relativo inicial de la muñeca.
tecla=""
While tecla<>Chr(27) And tecla<>Chr(13)
Locate 22,24: Print "Pulsa W S A D Q E para mover el Brazo."
Locate 23,27: Print " Para salir pulsa Esc o Enter."
Xaux=x
Yaux=y
Caux=Cabeceo
tecla=LCase(InKey)
If tecla="a" Then x=x-1
If tecla="d" Then x=x+1
If tecla="s" Then y=y-1
If tecla="w" Then y=y+1
If tecla="z" Then Cabeceo=Cabeceo-1
If tecla="x" Then Cabeceo=Cabeceo+1
'----------------------------------
'***** Cinemática Inversa 3GL.*****
'----------------------------------
rror:'Etiqueta para saltar cuando el brazo esté fuera de rango.
'Si así fuera recalcula todo a la última posición correcta.
Afx=Cos(rad*Cabeceo)*LongMunec
LadoB=x-Afx
Afy=Sin(rad*Cabeceo)*LongMunec
LadoA=y-Afy-BaseY
Hipotenusa=Sqr( (LadoA^2)+(LadoB^2) )
Alfa=ATan2(LadoA, LadoB)
Beta= ACos( ((LongBrazo^2)-(LongAntBr^2)+(Hipotenusa^2) )/(2*LongBrazo*Hipotenusa) )
AngBrazo= Alfa+Beta ' ÁNGULO BRAZO (en radianes).
Gamma=ACos( ((LongBrazo^2)+(LongAntBr^2)-(Hipotenusa^2))/(2*LongBrazo*LongAntBr) )
AngAntBr=-((180*rad)-Gamma) ' ÁNGULO ANTEBRAZO (en radianes).
AngMunec= ((rad*Cabeceo)-AngBrazo-AngAntBr)' ÁNGULO MUÑECA (en radianes).
If Str(Alfa)="-1.#IND" Or Str(Beta)="-1.#IND" Then
x=Xaux ' Si da posición fuera de rango
y=Yaux ' carga la posición anterior correcta
Cabeceo=Caux
GoTo rror ' y recalcula todo de nuevo.
EndIf
'--------------------------------------------
' Cálculos para dibujar 3 líneas en pantalla.
'--------------------------------------------
PXa=LongBrazo* Cos(AngBrazo)
PYa=LongBrazo*-Sin(AngBrazo)
PXb=LongAntBr* Cos(AngAntBr+AngBrazo)
PYb=LongAntBr*-Sin(AngAntBr+AngBrazo)
PXc=LongMunec* Cos(AngMunec+AngAntBr+AngBrazo)
PYc=LongMunec*-Sin(AngMunec+AngAntBr+AngBrazo)
'BRAZO punto(x,y)
BrazoPY=PYa+BaseY ' Punto de coordenada Y del brazo.
BrazoPX=PXa+BaseX ' Punto de coordenada X del brazo.
'ANTEBRAZO punto(x,y)
AntBrPY=PYb+PYa+BaseY ' Punto de coordenada Y del antebrazo.
AntBrPX=PXb+PXa+BaseX ' Punto de coordenada X del antebrazo.
'MUÑECA punto(x, y)
MunecPY=PYc+PYb+PYa+BaseY ' Punto de coordenada Y de la muñeca.
MunecPX=PXc+PXb+PXa+BaseX ' Punto de coordenada X de la muñeca.
'----------------
' Dibuja Líneas.
'----------------
If tecla<>"" Then Cls ' Una manera simple de borrar las líneas anteriores.
PSet (BaseX, BaseY ), 0 ' Punto inicial.
Line -(BrazoPX, BrazoPY ), 4 ' Dibuja línea del brazo.
Line -(AntBrPX, AntBrPY ), 9 ' Dibuja línea del antebrazo.
Line -(MunecPX, MunecPY ), 11 ' Dibuja línea de la muñeca.
'-----------------------------------------------------------------
'Muestra en pantalla los resultados de la Cinemática Inversa (IK).
'-----------------------------------------------------------------
Locate 1,10: Print "Angulos ----> Brazo:"; Int(AngBrazo*grad); Chr(248);_
" "; "AnteBrazo:"; Int((AngAntBr*grad)+180); Chr(248);_
" "; "Muneca:"; Int((AngMunec*grad)+180); Chr(248);_
"("; Str(-Cabeceo); Chr(248); ") "
Locate 2,10: Print "Posicion ---->";x;"X"; " "; y-BaseY; "Y "
'-----------------------------------------------------------------
Wend
End
|