CINEMÁTICA INVERSA EJEMPLO 2

Obtención de las variables articulares mediante el método de desacoplo cinemático:

Robot con seis grados de libertad y cuyos parámetros de Denavit Hartenberg están dados en
la tabla 1:

Parámetros de
Denavit Hartenberg
qi θi di ai αi
1 q1 0 0  π/ 2

2 q2 L1 0 π/ 2
3 q 3  π 2 0  L 2 π/ 2
4 q4 L3 0  π/ 2
5 q5 0 0 π/ 2
6 q6 L4 0 0
Figura 1. Imagen tomada de Fundamentos De Robótica [2] Tabla 1

Nota: por comodidad dadas expresiones extensas se toman cosq i   Ci y sin q i   Si

Con base en los parámetros de la tabla se generan las diferentes matrices de transformación
i 1
Ai :
C1 0  S1 0 C 2 0 S 2 0
S 0 C1 0 1   S2 0  C 2 0 
0
A1   1
A2  
 0  1 0 0 0 1 0 L1 
   
0 0 0 1 0 0 0 1
C(q 3  π 2 ) 0 S(q 3  π 2 )  L 2 C(q 3  π 2 )   S3 0  C3  L 2S3 
 S(q  π 2 ) 0  C(q  π 2 )  L S(q  π 2 )   
2
A3   3 3 2 3  Simplifica ndo 2 A 3   C3 0  S3 L 2 C3 
 0 1 0 0   0 1 0 0 
   
 0 0 0 1   0 0 0 1 

1
 C 4 0  S4 0 C 5 0 S5 0 C 6  S6 0 0
S 0 C4 
0 4 S 0  C5 
0 5 S C6 0 0 
3
A4   4 A5   5 A6   6
0 1 0 L3  0 1 0 0 0 0 1 L4 
     
0 0 0 1 0 0 0 1 0 0 0 1

Se obtiene la matriz simbólica de configuración de la herramienta S 6 respecto a la base S 0 :

ThSimb  0 A11 A 2 2 A 3 3 A 4 4 A 5 5 A 6

De igual manera la matriz objetivo ThObj para la configuración deseada para la herramienta:

n hx o hx a hx Phx 
n o hy a hy Phy  QhObj Ph 
  hy 
Phz   0 1 
ThObj
 n hz o hz a hz
 
 0 0 0 1

Recordando la equivalencia entre ambas matrices:

ThObj  ThSimb

ThObj  0 A 1 1 A 2 2 A 3 3 A 4 4 A 5 5 A 6

DESACOPLO CINEMÁTICO

Punto de la muñeca del robot:

En este caso el punto Pm donde se cortan las tres últimas articulaciones, está justo sobre el
origen O 5 del sistema S 5 y cuyo vector asociado al sistema S 0 de la base del robot es:

Pm  O 0O 5  Pmx  Pmy Pmz 

T

Las coordenadas de Ph , vector conocido que indica la posición que se desea para el extremo
del robot:
Ph  O0O6  Phx Phy Phz
T
 
En la figura 2 se observa que la magnitud del vector Pd  O 5O 6 que va desde la muñeca hasta
el extremo del robot es L 4 , su dirección es la misma que la del conocido vector unitario
 
Z 6  ah  a hx a hy a hz ; por lo tanto:
T

Pd  O5O6  L 4 a hx  a hy a hz 
T

2
 Figura 2

A partir de los vectores Ph y Pd , se puede obtener Pm mediante una suma de lazo vectorial:

Pm  Pd  Ph  0

Pm  Ph  Pd

Pmx  Phx  a hx  Phx  L 4 a hx 

       
Pm  Pmy   Phy   L 4 a hy   Phy  L 4 a hy 
P  P  a  P  L a 
 mz   hz   hz   hz 4 hz 

3
 PARTE 1: OBTENCIÓN DE q1 , q 2 y q 3

En esta parte, para el análisis sólo se consideran las tres primeras articulaciones, tomando
únicamente la parte de la estructura del robot que va desde su base hasta el punto que
representa la muñeca (ver figura 3):

Parámetros de
Denavit Hartenberg
qi θi di ai αi
π
1 q1 0 0 
2
π
2 q2 L1 0
2
π L 
3 q3   atan 3  0 L 2  L3
2 2
0
2  L2 
Figura 3 Tabla 2

Al realizar la nueva asignación de los sistemas S i para el robot de la figura 3, se observa que
los sistemas S 0 , S1 y S 2 son los mismos que los del robot de la figura 1, no siendo así el sistema
S 3aux que en este caso indica el extremo del robot de la figura 3. Se puede observar que la
dirección del eje x 3 aux del sistema final es algo particular y, esto se debe a que al aplicar las
reglas para obtener los parámetros de D.H. el eje x i es la línea normal que corta los ejes z i y
z i1 . Por otra parte, la articulación cuatro y que hace girar el eslabón cuatro, puesto que no se
considera en esta parte, se toman los eslabones tres y cuatro como un solo eslabón rígido.
Por tanto los parámetros de Denavit Hartenberg para este robot serán las dos primeras filas
de la tabla 1. La última es necesario hallarla. La matriz de transformación homogénea
4
simbólica TmSimb que representa el extremo del robot de la figura 3, en este caso la muñeca se
obtiene:

C1 0  S1 0 C 2 0 S2 0
S 0 C1 0 1  S 0  C2 0 
0
A1   1 A2   2
0  1 0 0 0 1 0 L1 
   
0 0 0 1 0 0 0 1

C(q  π 2  atan( L L ))  S(q  π 2  atan( L L )) 0 C(q 3  π 2  atan( L3 L 2 )) L 2  L3 

2 2

 3 3 2 3 3 2

  S(q 3  π 2  atan( L3 L 2 )) C(q 3  π 2  atan( L3 L 2 )) L 2  L3 
2 2
2
A 3aux 0 S(q 3  π 2  atan( L3 L 2 ))

 0 0 1 0 
 0 0 0 1 

Simplificando la matriz 2 A 3 aux usando el comando simplify de matlab:

 S(q  atan( L L ))  C(q  atan( L L )) 0  S(q 3  atan( L3 L 2 )) L 2  L3 

2 2

 3 3 2 3 3 2

  C(q 3  atan( L3 L 2 ))  S(q 3  atan( L3 L 2 )) L 2  L3 
2 2
2
A 3aux 0 C(q 3  atan( L3 L 2 ))

 0 0 1 0 
 0 0 0 1 

n m x om x amx Pm x 
0Q n Pm y 
PmSimb  om y amy
TmSimb  0 A11 A 2 2 A 3 aux   3aux  TmObj   my
 0 1  n mz om z a mz Pm z 
 
 0 0 0 1 

De la matriz TmObj sólo se conoce su última columna PmObj , la cual representa el vector posición
de la muñeca Pm . Ambas matrices son equivalentes ( TmSimb  TmObj ), sólo que una es simbólica
y la otra es la matriz objetivo para la muñeca. Recordando que para la primera parte del
desacoplo cinemático sólo se consideran las posiciones y no las orientaciones, se extraen los
vectores de posición (las tres primeras filas de la cuarta columna) de ambas matrices y se
igualan:
PmSimb  Pm

 L S  S C(q  atan( L L )) L 2  L 2  C C S(q  atan( L L )) L 2  L 2  P 

 1 1 1 3 3 2 2 3 1 2 3 3 2 2 3
  mx 
 L1C1  C1C(q 3  atan( L3 L2 )) L 2  L 3  S1C 2S(q 3  atan( L3 L2 )) L 2  L 3   Pm y 
2 2 2 2

  
   Pm z 
2 2
 S 2 S (q 3  atan( L L
3 2 )) L 2 L 3

5
De manera que se tienen tres ecuaciones con tres incógnitas. Por comodidad, dado el tamaño
de las ecuaciones anteriores, se simplificarán haciendo α  q 3  atan L 3 L 2  y b  L 2  L 3 :
2 2

 L1S1  S1C α b  C1C 2Sα b  Pm x 

 L C  C C b S C S b   P 
 1 1 1 α 1 2 α   my 
 S 2Sα b   Pm z 

Reacomodando el sistema de tres ecuaciones (esto se hace después de haber pensado en

una estrategia para abordar el problema) con tres incógnitas, las cuales tienen factores en
común en la parte izquierda:

 S1 L1  C α b   C1C 2Sα b  Pm x (1.1)

C1 L1  C α b   S1C 2 Sα b  Pm y (1.2)

S2Sα b  Pm z (1.3)

• Obtención de q 3

Elevando al cuadrado las ecuaciones 1.1, 1.2 y 1.3 se obtienen las ecuaciones 1.4, 1.5 y 1.6
respectivamente:

- S1 L1  Cα b   C1C 2Sα b 2  Pm x 2

 2

S1 L1  Cα b 2  2 Cα L1b  2 S1C1C 2Sα L1  Cα b  b C1 C 2 Sα b 2  Pm x
2 2 2 2 2 2
(1.4)

C1 L1  Cα b   S1C2Sα b 2  Pm y 2

 2

C1 L1  Cα b 2  2 Cα L1b  2 S1C1C 2Sα L1  Cα b  b S1 C 2 Sα b 2  Pm y
2 2 2 2 2 2
(1.5)

S2Sα b 2  Pm z 2

S2 Sα b 2  Pm z
2 2 2
(1.6)

Sumando las ecuaciones 1.4 y 1.5:

S 1
2
 2
 
 C1 L1  C α b 2  2 C α L1b  S1  C1 C 2 Sα b 2  Pm x  Pm y
2 2 2 2
 2 2 2 2

C α b 2  C 2 Sα b 2  2 C α L1b  Pm x  Pm y  L1
2 2 2 2 2 2

Sumando la ecuación anterior con la ecuación 1.6:

6
  2

Cα b 2  S2  C 2 Sα b 2  2 Cα L1b  Pm x  Pm y  Pm z  L1
2 2 2 2 2 2 2

C α
2

 Sα b 2  2 Cα L1b  Pm x  Pm y  Pm z  L1
2 2 2 2 2

b 2  2 Cα L1b  Pm x  Pm y  Pm z  L1
2 2 2 2

Despejando Cα :

Pm x  Pm y  Pm z  L1  b 2
2 2 2 2

Cα 
2 L1b

Como b  L 2  L 3 y Pmx  Phx  L 4 a hx , Pmy  Phy  L 4 a hy , Pmz  Phz  L 4 a hz

2 2

Phx  L 4a hx 2  Phy  L 4a hy 2  Phz  L 4a hz 2  L12  L 2 2  L32

Cα 
2 L1 L 2  L 3
2 2

Con Cα y aplicando la identidad Sα  C α  1  Sα   1  C α :

2 2 2

α  atan 2Sα , Cα   atan 2  1  Cα , Cα  2


Reemplazando α  q 3  atan L 3 L 2  en la ecuación anterior y despejando q 3 se llega a la
solución:

Phx  L 4a hx 2  Phy  L 4a hy 2  Phz  L 4a hz 2  L12  L 2 2  L32

Cα 
2 L1 L 2  L 3
2 2


q 3 I  atan 2  1  Cα 2 , Cα  atan   L3 
 2
L
 (1.7)


q 3 II  atan 2  1  Cα 2 , Cα  atan   L3 

 L2 
El término  1 Cα
2
impone dos soluciones para q 3 al considerar ambos signos. Por otra
parte, ésta no depende de otras articulaciones.

• Obtención de q 2

Una vez q 3 es conocida, se despeja S 2 de la ecuación 1.3 y se reemplazan los términos

conocidos α  q 3  atan L3 L 2 , b  L 2  L3 , Pmz  Phz  L 4a hz :
2 2

7
 Pm z Phz  L 4a hz
S2  

 L2   
Sα b S q 3  atan L3  L 2 2  L3 2

Por último, empleando la identidad C 2   1  S2 se puede llegar a una expresión para q 2

2

q 2  atan 2S2 , C 2   atan 2 S2 ,  1  S2 2  

Phz  L 4 a hz
S2 

S q 3  atan


 
L3 

L2  L 2  L3
2 2


q 2 I  atan 2 S2 ,  1  S2 2  (1.8)

q 2 II  atan 2S 2 ,  1  S2
2

Arrojando dos soluciones para q 2 . Es función f q 3  .

• Obtención de q1

Una vez conocidas q 2 y q 3 , se reorganizan las ecuaciones 1.1 y 1.2 de la forma:

Pm x   L1  Cα b S1   C 2Sα b  C1
Pm y   C 2Sα b S1  L1  Cα b  C1

Aplicando la regla de Cramer para el sistema de ecuaciones anterior se obtienen las

expresiones para S1 y C1 :

Pm x   L1  C α b  C 2Sα b   S1 
P     
 m y    C 2Sα b L1  Cα b C1 

Pm x L1  Cα b   Pm y  C 2Sα b   Pm x L1  C α b   C 2Sα Pm y b

S1  
 L1  Cα b L1  Cα b    C 2Sα b  C 2Sα b  L1  Cα b 2  C 2Sα b 2

 L1  Cα b Pm y   C 2Sα b Pm x  Pm L1  C α b   C 2Sα Pm x b

C1   y
 L1  Cα b L1  Cα b    C 2Sα b  C 2Sα b  L1  Cα b 2  C 2Sα b 2

Con las dos expresiones anteriores, una para S1 y otra para C1 ; se puede encontrar una
solución de tipo q1  atan 2S1 , C1  :

  Pm x L1  C α b   C 2Sα Pm y b Pm y L1  C α b   C 2Sα Pm x b 

q1  atan 2 , 
 L1  C α b 2
 C 2Sα b 
2
L1  C α b2  C 2Sα b2 

Donde: Pmx  Phx  L 4 a hx , Pmy  Phy  L 4 a hy , b  L 2  L3 , α  q 3  atan L3 L 2 

2 2

8
 Si se desea, la solución para q1 se puede simplificar aún más. Observe que la expresión
L1  Cα b 2  C 2Sα b 2
es el denominador tanto de la parte real como imaginaria (como se vio en
el ejemplo 1, no influye en la magnitud de q1 ). Además su resultado siempre es positivo, lo
cual indica que tampoco influye sobre su cuadrante. De manera que se puede prescindir (ante
una situación como esta, si no se está seguro sobre si influye o no en la magnitud y
cuadrante, es mejor abstenerse de hacerlo) de dicha expresión:

α  q 3  atan L 3 L 2 , b  L 2  L 3
2 2

q1  atan 2 Phx  L 4 a hx L1  C α b   C 2Sα Phy  L 4 a hy b , Phy  L 4 a hy L1  C α b   C 2Sα Phx  L 4 a hx  b 
(1.9)

Existe una solución única para q1 . Es función f q 2 , q 3  .

PARTE 2: OBTENCIÓN DE q 4 , q 5 y q 6

Una vez se conocen los valores para las tres primeras articulaciones, se obtienen las
posteriores que definen la orientación del robot. Retomando la matriz ThSimb generada con los
parámetros de la tabla 1:
ThSimb  0 A11 A 2 2 A 3 3 A 4 4 A 5 5 A 6

i 1
Tomando únicamente las submatrices de orientación de las matrices Ai :

QhSimb 0 Q 6 0 Q11 Q 2 2 Q3 3 Q 4 4 Q5 5 Q 6
Donde:
C1 0  S1  C 2 0 S 2   S3 0  C3 
0
Q1   S1 0 C1  1
Q 2   S2 0  C 2  2
Q 3   C3 0  S3 
 0  1 0   0 1 0   0 1 0 
C 4 0  S 4  C5 0 S5   C 6  S6 0 
3
Q 4   S4 0 C 4  4
Q 5   S5 0  C5  5
Q 6   S6 C 6 0
 0  1 0   0 1 0   0 0 1

De igual manera se extrae la submatriz de orientación de la matriz objetivo ThObj :

n hx o hx a hx 
QhObj  n hy o hy a hy 
 n hz o hz a hz 

Igualando las matrices Q hSim y Q hObj : 0

Q1 1 Q 2 2 Q 3 3 Q 4 4 Q 5 5 Q 6  Q hObj

Dado que 0 Q1 ,1 Q 2 y 2 Q3 son conocidas (dependen de q1 , q 2 y q 3 ), se despeja a la derecha

los términos conocidos y a la izquierda los que aún no se conocen:

3
Q6  Q
0
3
1
QhObj Donde 0 Q3 0 Q11 Q 2 2 Q3 y 3
Q 6 3 Q 4 4 Q5 5 Q 6 (1.10)
9
Resolviendo la parte izquierda de la ecuación 1.10:
 S4S6  C 4 C5C6  S4C6  C 4 C5S6 C 4S5 
3
Q 6   C 4S6 + S4 C5C6 C 4C6  S4C5S6 S4S5  (1.11)
  S5C6 S5S6 C5 
Ahora nombrando como t el resultado de la parte derecha de la ecuación 1.10:
 t11 t12 t13 
t   Q3  QhObj   t 21 t 22 t 23 
0 1
(1.12)
 t 31 t 32 t 33 
donde:
t11  n hx S1C3  C1C 2S3  + n hy S1C 2S3  C1C3   n hz S2S3 
t12  o hx S1C3  C1C 2S3  + o hy S1C 2S3  C1C3   o hz S2S3 
t13  a hx S1C3  C1C 2S3  + a hy S1C 2S3  C1C3   a hz S2S3 
t 21  n hx C1S2  n hyS1S2  n hz C 2
t 22  o hx C1S2  o hyS1S2  o hz C 2
t 23  a hx C1S2  a hyS1S2  a hz C 2
t 31  n hx S1S3  C1C 2 C3   n hy S1C 2 C3  C1S3   n hz S2 C3 
t 32  o hx S1S3  C1C 2 C3   o hy S1C 2C3  C1S3   o hz S2 C3 
t 33  a hx S1S3  C1C 2 C3   a hy S1C 2 C3  C1S3   a hz S2 C3 

Igualando de nuevo las ecuaciones 1.11 y 1.12:

 S4S6  C 4 C5C6  S4 C 6  C 4 C5S6 C 4S5   t11 t12 t13 
 C S +S C C C 4 C 6  S4 C5S6 S4S5    t 21 t 22 t 23 
 4 6 4 5 6 (1.13)
  S5C6 S5S6 C5   t 31 t 32 t 33 
En la igualdad matricial 1.13, se aprecia que se puede disponer de nueve ecuaciones (contrario
a la primera parte del desacoplo donde sólo se puede disponer de tres), para buscar el conjunto
de soluciones para q 4 , q 5 y q 6 .

• Obtención de q 5

Tomando las posiciones C4S5  t13 y S4S5  t 23 de la ecuación 1.15, elevándolas al cuadrado y
luego sumando ambos resultados:
C 4 S5  t13
2 2 2

S4 S5  t 23
2 2 2

S 4
2 2

 C 4 S5  t13  t 23
2 2 2

S5  t13  t 23
2 2 2

S5   t13  t 23
2 2

10
Usando S5 y tomando la posición C5  t 33 , se llega a la solución: q 5  atan 2S5 , C5 

t  0 Q 3  QhObj
1


q 5 I  atan 2  t132  t 23 2 , t 33  (1.14)
q 5 II  atan 2 t132  t 232 , t 
33

Dos posibles soluciones para q 5 . Es función f q1 , q 2 , q 3 

• Obtención de q 4

Una vez q 5 es conocida, q 4 se puede hallar tomando las posiciones C4S5  t13 y S4S5  t 23 para
despejar C 4 y S 4 respectivamente:
t13
C4S5  t13  C4 
S5
t 23
S4S5  t 23  S4 
S5
q 4  atan 2S4 , C4 

t  0 Q 3  QhObj
1

 t 23 t13  (1.15)
q 4  atan 2 , 
 S5 S5 
Solución única para q 4 . Es función f q1 , q 2 , q 3 , q 5 

• Obtención de q 6

Para q 6 se toman las posiciones  S5C6  t 31 y S5S 6  t 32 para despejar C6 y S6

respectivamente:
t 31
 S5C6  t 31  C6  
S5
t 32
S5S6  t 32  S6 
S5

Aplicando q 6  atan 2S6 , C6 

t  0 Q3  QhObj
1

 t 32 t  (1.16)
q 6  atan 2 ,  31 
 S5 S5 
Solución única para q 6 . Es función f q1 , q 2 , q 3 , q 5 

11
 RECOPILACIÓN DE LOS RESULTADOS
qi TABLA 3: SOLUCIONES PARA LAS VARIABLES ARTICULARES f q ,.., q 
i n


q 3 I  atan 2  1  C α 2 , C α  atan   L3 

 L2  Phx  L 4a hx 2  Phy  L 4a hy 2  Phz  L 4a hz 2  L12  L 2 2  L3 2
q3 Cα 
2 L1 L 2  L 3
 
2 2
L 
q 3 II  atan 2  1  Cα , Cα  atan 3 
2

 L2 


q 2 I  atan 2 S2 ,  1  S2 2  Phz  L 4a hz
q2 S2 
  q3
 atan 2S 
 L3 
S q 3  atan L 2  L3
2 2
 
 L2 
,  1  S2
2
q 2 II 2

q1  atan 2 Phx  L 4a hx L1  Cα b   C 2Sα Phy  L 4a hy b , Phy  L 4a hy L1  Cα b   C 2Sα Phx  L 4a hx  b 
q1 L  q2 ,q3
α  q 3  atan 3  b  L 2  L3
2 2

 L2 


q 5 I  atan 2  t132  t 232 , t 33 
q5 t  0 Q3  QhObj
1
q1 , q 2 , q 3

q 5 II  atan 2  t13  t 23 , t 33
2 2

 t 23 t13 
q4 t  0 Q3  QhObj
1
q 4  atan 2 ,  q1 , q 2 , q 3 , q 5
 S5 S5 

 t 32 t 
t  0 Q3  QhObj
1
q6 q 6  atan 2 ,  31  q1 , q 2 , q 3 , q 5
 S5 S5 

12
La tabla 3 muestra que la articulación q 3 es independiente, mientras que las demás variables
son funciones de otras. En la figura 4 se puede ver un árbol de 8 soluciones que pueden
satisfacer la configuración deseada para el robot:

I
q4 q 6 COMBINACIÓN 1

q1 q 5

I II q4 q 6 COMBINACIÓN 2

q 2

I II I
q4 q 6 COMBINACIÓN 3

q1 q 5

II q4 q 6 COMBINACIÓN 4

q 3

I
q4 q 6 COMBINACIÓN 5

q1 q 5

II I II q4 q 6 COMBINACIÓN 6

q 2

II I
q4 q 6 COMBINACIÓN 7

q1 q 5

II q4 q 6 COMBINACIÓN 8

Figura 4

COMPROBACIÓN

Con el fin de comprobar si las ocho combinaciones posibles satisfacen los parámetros de la
matriz Tr dada, sea entonces:

 0.05 - 0.79 - 0.62 - 34.99  L1  40cm

 0.45 - 0.54 0.72 76.79 
Tr    L 2  20cm
- 0.89 - 0.31 0.33 10.33 con
L 3  30cm
 
 0 0 0 1  L 4  10cm

Resultados:
13
======================= COMBINACIÓN 1 =======================

q1 q2 q3 q4 q5 q6
0.3569 1.2088 -0.7728 -2.6310 0.3994 -0.3479

ThCalculada=
0.0487 -0.7859 -0.6164 -34.9540
0.4505 -0.5335 0.7158 76.7482
-0.8914 -0.3126 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000

======================= COMBINACIÓN 2 =======================

q1 q2 q3 q4 q5 q6
0.3569 1.2088 -0.7728 0.5106 -0.3994 2.7937

ThCalculada=
0.0487 -0.7859 -0.6164 -34.9540
0.4505 -0.5335 0.7158 76.7482
-0.8914 -0.3126 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000

======================= COMBINACIÓN 3 =======================

q1 q2 q3 q4 q5 q6
0.4276 1.9328 -0.7728 -2.5139 0.6666 -1.1285

ThCalculada=
0.0508 -0.7858 -0.6164 -34.9540
0.4520 -0.5323 0.7158 76.7482
-0.8906 -0.3150 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
======================= COMBINACIÓN 4 =======================

q1 q2 q3 q4 q5 q6
0.4276 1.9328 -0.7728 0.6277 -0.6666 2.0131

ThCalculada=
0.0508 -0.7858 -0.6164 -34.9540
0.4520 -0.5323 0.7158 76.7482
-0.8906 -0.3150 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
14
======================= COMBINACIÓN 5 =======================

q1 q2 q3 q4 q5 q6
0.4276 -1.2088 -1.1928 2.5435 0.7009 3.0199

ThCalculada=
0.0441 -0.7862 -0.6164 -34.9540
0.4474 -0.5361 0.7158 76.7482
-0.8932 -0.3074 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000

======================= COMBINACIÓN 6 =======================

q1 q2 q3 q4 q5 q6
0.4276 -1.2088 -1.1928 -0.5981 -0.7009 -0.1217

ThCalculada=
0.0441 -0.7862 -0.6164 -34.9540
0.4474 -0.5361 0.7158 76.7482
-0.8932 -0.3074 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000

======================= COMBINACIÓN 7 =======================

q1 q2 q3 q4 q5 q6
0.3569 -1.9328 -1.1928 2.8835 0.8399 -2.8305

ThCalculada=
0.0423 -0.7863 -0.6164 -34.9540
0.4461 -0.5372 0.7158 76.7482
-0.8940 -0.3053 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
======================= COMBINACIÓN 8 =======================

q1 q2 q3 q4 q5 q6
0.3569 -1.9328 -1.1928 -0.2581 -0.8399 0.3111

ThCalculada=
0.0423 -0.7863 -0.6164 -34.9540
0.4461 -0.5372 0.7158 76.7482
-0.8940 -0.3053 0.3281 10.3109
0 0 0 1.0000

ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
15
BIBLIOGRAFÍA:

[1] JOHN J. CRAIG. Robótica. Tercera Edición. Prentice Hall, 2006. ISBN: 9789702607724

[2] ANTONIO BARRIENTOS. Fundamentos De Robótica. Segunda Edición. McGraw-Hill,

2014. ISBN: 9788448156367

[3] SUBIR KUMAR SAHA. Introducción A La Robótica. Primera Edición. McGraw-Hill, 2008.
ISBN: 9786071503138

16