Cinemática Inversa II

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 porque no tiene, sólo es para orientar el terminal. Si este es tu caso has de poner a cero el valor de la longitud de la muñeca en el código fuente.

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.

Cuando observes el programa recuerda que no es lo mismo Atan() que Atan2(). Si no sabes cuál es la diferencia ves al apartado "Cinemática Inversa I".

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.

Nota importante: El programa está preparado para representar el brazo en la pantalla, eso significa que si sacas los datos de los ángulos al mundo real, por ejemplo a unos servos, no serán coincidentes. En la pantalla estoy situando el brazo en un lugar de ésta (BaseX y BaseY) y además el eje Y está invertido.

El código está escrito en FreeBasic. Si usas otro lenguaje de programación, traducir "Basic" y con las variables declaradas en "tipo", es lo más sencillo del mundo.

Descargar código fuente y ejecutable.

(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í)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."
Declare Sub InverseK
Declare Sub DibujaBrazo
Screen 19         ' Ventana de 800x600.
Dim Shared As Double   AngAntBr, AngBrazo, AngMunec, pi, rad, grad, x, y, Xaux, Yaux, Cabeceo, Caux, BaseY, BaseX, LongBrazo, LongAntBr, LongMunec

Dim Shared As String   Tecla

pi   = Atn(1)*4
rad  = pi/180
grad = 180/pi
BaseX  = 400      ' Punto X Base (hombro).  Situamos el brazo en pantalla.
BaseY  = 300      ' Punto Y Base (hombro).
LongBrazo  = 150  ' Longitud Brazo.     
LongAntBr  = 150  ' Longitud AnteBrazo.
LongMunec  =  50  ' Longitud Muñeca.
x=( 50)           ' Posición Inicial X.    Damos las coordenadas iniciales de 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. Cero grados es horizontal en este caso.
Tecla=" "
While Tecla<>Chr(27) And Tecla<>Chr(13) And Tecla<>Chr(255)+"k"

   Locate 36,30: Print "Pulsa W S A D Z X para mover el Brazo.";
   Locate 37,32: Print "   Para salir pulsa Esc o Enter.";

   InverseK

   If Tecla<>"" Then

      DibujaBrazo  ' Sólo cuando se pulsa una tecla se dibuja el brazo.

   EndIf

   Tecla=LCase(InKey)
   If Tecla="a" Then x-=1
   If Tecla="d" Then x+=1
   If Tecla="s" Then y-=1
   If Tecla="w" Then y+=1
   If Tecla="x" Then Cabeceo-=1
   If Tecla="z" Then Cabeceo+=1

Wend
End
Sub InverseK

   '----------------------------------
   '***** Cinemática Inversa 3GL.*****
   '----------------------------------
  
   Dim As Double  Hipotenusa, Afx, Afy, LadoA, LadoB, Alfa, Beta, Gamma
   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            ' En caso de error (fuera de rango) se vuelve a llamar a sí misma
      y=Yaux            ' y carga con los valores anteriores correctos.
      Cabeceo=Caux

      InverseK

   EndIf
      
   Xaux=x
   Yaux=y
   Caux=Cabeceo 
      
End Sub
Sub DibujaBrazo

   '---------------------
   ' PUNTOS PARA DIBUJAR.
   '---------------------

   Dim As Double   PYa, PYb, PYc, PXa, PXb, PXc, BrazoPY, BrazoPX, AntBrPY, AntBrPX, MunecPY, MunecPX

   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 (x,y)
   BrazoPY=PYa+BaseY           ' Punto de coordenada Y del Brazo.
   BrazoPX=PXa+BaseX           ' Punto de coordenada X del Brazo.

   'ANTEBRAZO (x,y)
   AntBrPY=PYb+PYa+BaseY       ' Punto de coordenada Y del AnteBrazo.
   AntBrPX=PXb+PXa+BaseX       ' Punto de coordenada X del AnteBrazo.

   'MUÑECA (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.
   
   Cls                         ' Borra 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,20:  Print "Angulos  ----> Brazo:"; CInt(AngBrazo*grad); Chr(248);_
                      "  "; "AnteBrazo:"; CInt((AngAntBr*grad)+180); Chr(248);_
                      "  "; "Muneca:"; CInt((AngMunec*grad)+180); Chr(248);_
                      "  "; "("; Str(Cabeceo+180); Chr(248); ")  ";
   Locate 2,20:  Print "Posicion ---->";x;"X"; "  "; y-BaseY;   "Y   "
End Sub

A continuación te dejo un enlace de una web externa con lo mismo que hemos tratado aquí pero en formato de hoja de cálculo:

http://www.lynxmotion.com/images/files/inversek.xls

Has de tener el Excel instalado y una vez abierto podrás modificar el gráfico de abajo (en la hoja del Excel) cambiando los valores que verás en unas casillas de color amarillas arriba a la derecha.

Ir a: Cinemática Inversa III.