‘MODELADO GEOMÉTRICO DE LA INTERFAZ HÁPTICA GEOMAGIC® TOUCH ™

Presentado por:
Lusvin Javier Amado Forero

Trabajo de la Asignatura:
Sistemas Robóticos en Medicina

Docente:
Mg. SERGIO ALEXANDER SALINAS
Ingeniero Electrónico

UNIVERSIDAD PONTIFICIA BOLIVARIANA

SECCIONAL BUCARAMANGA
2017
 CONTENIDO

1. Descripción del robot

2. Estructura geométrica

2.1. Esquema geométrico

2.2. Tabla de parámetros geométricos

3. Modelo geométrico

3.1. Modelo geométrico Directo

3.2. Modelo geométrico Inverso

4. Prueba de los modelos geométricos en Simulink

4.1. Pruebas en Simulink de los Modelos Geométricos

5. Tabla de parámetros inerciales

5.1. Calculo de los parámetros inerciales de las 3 primeras articulaciones
5.2. Tabla de parámetros inerciales completa

6. Piezas en SolidWorks del Robot

6.1. Diseño de piezas en Solidworks del robot
6.2. Tablas con los valores de las masas y parámetros de cada pieza del robot

7. Modelos Dinámicos
7.1. Archivos en Matlab del Modelo dinámico Inverso
7.1. Archivos en Matlab del Modelos dinámico Directo

8. Trayectoria de Simulación
8.1. Concepto de la colecistectomía laparoscópica
8.2. Trayectoria para la simulación

9. Bibliografía
1. Descripción del robot

La interfaz háptica geomagic® touch™ (ver figura 1) es un dispositivo háptico profesional

de gama media (entre los menos costoso de la 3D Systems). Este dispositivo otorga al
usuario una retroalimentación de fuerza, lo cual permite sentir objetos virtuales y
producir sensaciones táctiles reales a medida que el usuario manipula objetos 3D en
pantalla o en una interfaz gráfica[1] .
Es importante mencionar que este dispositivo otorgar una posición espacial en 3D (a lo
largo de los ejes X, Y y Z) y una orientación de giro, inclinación y dirección del lápiz de
mano (Ver figura 2). Esto es posible en función de su modelo, el cual le proporciona 6
grados de libertad (DOF). Por lo cual es utilizado en diversas aplicaciones tales como: la
simulación, el entrenamiento, la evaluación de habilidades, ensayos, ensamblaje virtual,
control robótico, detección de colisiones, diseño de interfaz de máquina, rehabilitación
etc.[1].
 Figura 1. Interfaz háptica Geomagic® Touch™

La Geomagic® Touch™ requiere ciertos componentes mínimos de hardware y software

para poder funcionar correctamente como un dispositivo de transmisión en tiempo real.
Los cuales se describen a continuación:

• Procesador Intel® Core™ 2 Dúo.

• Puerto RJ45, Ethernet y USB
• Sistema operativo Windows XP, Windows 7 o Windows 8.
• Memoria de 2 GB
 Figura 2. Límites físicos del dispositivo

Las principales características de la interfaz háptica geomagic® touch™ se presentan a

continuación en la tabla N°1:

Área de trabajo ~ 6,4 An x 4,8 Al x 2,8 P pulg.

Rango de movimiento
Resolución de la posición nominal
Fuerza de esfuerzo máxima y valor
par en la posición nominal
Rigidez
Retroalimentación de fuerza (6
grados de libertad)
Entrada/sensor de posición (6
grados de libertad)
Interfaz
~ 160 An x 120 Al x 70 P mm
Movimiento de la mano con giro
de la muñeca
> 450 dpi
~ 0,055 mm
0,75 lbf/3,3 N
Eje X > 7,3 lb/pulg. (1,26 N/mm)
Eje Y > 13,4 lb/pulg. (2,31 N/mm)
Eje Z > 5,9 lb/pulg. (1,02 N/mm)
x, y, z
x, y, z (codificadores digitales)
[Giro, inclinación, dirección (± 5% de
potenciómetros de linealidad)]
Puerto IEEE 1394
FireWire®: 6 pines a 6 pines

Tabla N°1. Características técnicas

2. Estructura geométrica

La interfaz Geomagic® Touch™ presenta una estructura establecida en la patente

denominada “FORCE REFLECTING HAPTIC INTERFACE” con número US007411576B2
del 12 de agosto del 2008 (Ver figura 3).[3]
 Figura 3. Diagrama de la patente US007411576B2

2.1. Esquema geométrico

2.2. Tabla de parámetros geométricos.

J σ α d θ r
1 0 0 0 q1 0
2 0 -90 0 q2 0
3 0 0 D3 q3 0
4 0 90 0 q4 R4
5 0 -90 0 q5 0
6 0 90 0 q6 0
3. Modelo geométrico

3.1. Tabla de parámetros en Matlab

clc;
clear all;
close all;
syms q1 q2 q3 q4 q5 q6 D3 R4;

% sigma alpha d theta r

tabla = [ 0 0 0 q1 0;
0 -pi/2 0 q2 0;
0 0 D3 q3 0;
0 pi/2 0 q4 R4;
0 -pi/2 0 q5 0;
0 pi/2 0 q6 0];

3.2. Hallando cada una de las matrices

Para realizar el cálculo de matrices con el fin de hallar el MGD se utilizó la función transf (Ver
anexo 1) suministrada por el Profesor Sergio Alexander Salinas.

T01=transf(tabla,0,1);
T12=transf(tabla,1,2);
T23=transf(tabla,2,3);
T34=transf(tabla,3,4);
T45=transf(tabla,4,5);
T56=transf(tabla,5,6);

3.3. Hallando el modelo geométrico Directo:

0
T1=
cos(q1) -sin(q1) 0 0
sin(q1) cos(q1) 0 0
0 0 1 0
0 0 0 1
1
T2=
cos(q2) -sin(q2) 0 0
0 0 1 0
-sin(q2) -cos(q2) 1 0
0 0 0 1

2
T3=
cos(q3) -sin(q3) 0 D3
sin(q3) cos(q3) 0 0
0 0 1 0
0 0 0 1

3
T4=
cos(q4) -sin(q4) 0 0
0 0 -1 R4
sin(q4) cos(q4) 0 0
0 0 0 1

4
T5=
cos(q5) -sin(q5) 0 0
0 0 1 0
-sin(q5) -cos(q5) 0 0
0 0 0 1

5
T6=
cos(q6) -sin(q6) 0 0
0 0 -1 0
sin(q6) cos(q6) 0 0
0 0 0 1

MGD=transf(tabla,0,6);

%Cálculo de matrices
% U4=simplify(T45*T56);
% U3=simplify(T34*U4);
% U2=simplify(T23*U3);
% U1=simplify(T12*U2);
% U0=simplify(T01*U1);
Modelo Geométrico Directo obtenido en Matlab

MGD =

[ - sin(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)) - cos(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)),

sin(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)) - cos(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)),
sin(q2 + q3)*cos(q1)*cos(q5) - sin(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)), cos(q1)*(R4*sin(q2 + q3) + D3*cos(q2))]

[ sin(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) + cos(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),

cos(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),
sin(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) + sin(q2 + q3)*cos(q5)*sin(q1), sin(q1)*(R4*sin(q2 + q3) + D3*cos(q2))]

[ sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)), sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)) +
sin(q2 + q3)*cos(q6)*sin(q4), cos(q2 + q3)*cos(q5) - sin(q2 + q3)*cos(q4)*sin(q5), R4*cos(q2 + q3) - D3*sin(q2)]

[ 0, 0, 0, 1]
 3.4. Hallando el Modelo Geométrico Inverso

syms sx sy sz nx ny nz ax ay az px py pz

Uo = [ sx nx ax px;
sy ny ay py;
sz nz az pz;
0 0 0 1];

3.4.1. Usando método de Paul

%Cálculo de matriz T10

T10=simplify(inv(T01));
U1 = T10*Uo;
T16=transf(tabla,1,6);

%Cálculo de matriz T21

T21=simplify(inv(T12));
U2 = T21*U1;
T26=transf(tabla,2,6);

%Cálculo de matriz T32

T32=simplify(inv(T23));
U3=T32*U2;
T36=transf(tabla,3,6);

%Cálculo de matriz T34

T34=simplify(inv(T34));
U4=T34*U3;
T46=transf(tabla,4,6);

%Cálculo de matriz T45

T45=simplify(inv(T45));
U5=T45*U4;
T56=transf(tabla,5,6);

Teniendo como resultado las siguientes matrices en Matlab

T10 =

[ cos(q1), sin(q1), 0, 0]
[ -sin(q1), cos(q1), 0, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
U1 =
[ sx*cos(q1) + sy*sin(q1), nx*cos(q1) + ny*sin(q1), ax*cos(q1) + ay*sin(q1), px*cos(q1) + py*sin(q1)]
[ sy*cos(q1) - sx*sin(q1), ny*cos(q1) - nx*sin(q1), ay*cos(q1) - ax*sin(q1), py*cos(q1) - px*sin(q1)]
[ sz, nz, az, pz]
[ 0, 0, 0, 1]

T16 =
[ - cos(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5)) - cos(q2 + q3)*sin(q4)*sin(q6), sin(q6)*(sin(q2 + q3)*sin(q5) - cos(q2 + q3)*cos(q4)*cos(q5))
- cos(q2 + q3)*cos(q6)*sin(q4), sin(q2 + q3)*cos(q5) + cos(q2 + q3)*cos(q4)*sin(q5), R4*sin(q2 + q3) + D3*cos(q2)]
[ cos(q4)*sin(q6) + cos(q5)*cos(q6)*sin(q4), cos(q4)*cos(q6) - cos(q5)*sin(q4)*sin(q6), sin(q4)*sin(q5), 0]
[ sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5)), sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 + q3)*cos(q4)*cos(q5))
+sin(q2+q3)*cos(q6)*sin(q4), cos(q2+q3)*cos(q5)-sin(q2+q3)*cos(q4)*sin(q5), R4*cos(q2+q3)-D3*sin(q2)]
[ 0, 0, 0, 1]

T21 =
[cos(q2), 0, -sin(q2), 0]
[-sin(q2), 0, -cos(q2), 0]
[0, 1, 0, 0]
[0, 0, 0, 1]

U2 =
[cos(q2)*(sx*cos(q1)+sy*sin(q1))-sz*sin(q2), cos(q2)*(nx*cos(q1)+ny*sin(q1))-nz*sin(q2), cos(q2)*(ax*cos(q1)+ay*sin(q1))-az*sin(q2),
cos(q2)*(px*cos(q1)+py*sin(q1))-pz*sin(q2) ]
[ - sin(q2)*(sx*cos(q1) + sy*sin(q1)) - sz*cos(q2), - nz*cos(q2) - sin(q2)*(nx*cos(q1) + ny*sin(q1)), - az*cos(q2) - sin(q2)*(ax*cos(q1) + ay*sin(q1)), -
pz*cos(q2)-sin(q2)*(px*cos(q1)+py*sin(q1)) ]
[sy*cos(q1) - sx*sin(q1), ny*cos(q1) - nx*sin(q1), ay*cos(q1) - ax*sin(q1), py*cos(q1) - px*sin(q1)]
[ 0, 0, 0, 1]
T26 =
[- cos(q6)*(sin(q3)*sin(q5) - cos(q3)*cos(q4)*cos(q5)) - cos(q3)*sin(q4)*sin(q6), sin(q6)*(sin(q3)*sin(q5) - cos(q3)*cos(q4)*cos(q5))- cos(q3)*cos(q6)*sin(q4),
cos(q5)*sin(q3)+cos(q3)*cos(q4)*sin(q5), D3+R4*sin(q3)]
[ cos(q6)*(cos(q3)*sin(q5)+ cos(q4)*cos(q5)*sin(q3)) - sin(q3)*sin(q4)*sin(q6), - sin(q6)*(cos(q3)*sin(q5) + cos(q4)*cos(q5)*sin(q3)) - cos(q6)*sin(q3)*sin(q4),
cos(q4)*sin(q3)*sin(q5)-cos(q3)*cos(q5), -R4*cos(q3)]
[cos(q4)*sin(q6) + cos(q5)*cos(q6)*sin(q4), cos(q4)*cos(q6) - cos(q5)*sin(q4)*sin(q6), sin(q4)*sin(q5), 0]
[ 0, 0, 0, 1]

T32 =
[cos(q3), sin(q3), 0, -D3*cos(q3)]
[-sin(q3), cos(q3), 0, D3*sin(q3)]
[0, 0, 1, 0]
[0, 0, 0, 1]

U3 =
[-cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) - sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)), -cos(q3)*(nz*sin(q2) -
cos(q2)*(nx*cos(q1) + ny*sin(q1))) - sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1))), -cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) -
sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))), -cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) - sin(q3)*(pz*cos(q2) +
sin(q2)*(px*cos(q1)+py*sin(q1)))-D3*cos(q3) ]
[sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) - cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)), sin(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1)
+ny*sin(q1))) - cos(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1))), sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) - cos(q3)*(az*cos(q2) +
sin(q2)*(ax*cos(q1) + ay*sin(q1))), sin(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) - cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) +
D3*sin(q3) ]
[sy*cos(q1)-sx*sin(q1), ny*cos(q1)-nx*sin(q1), ay*cos(q1)-ax*sin(q1), py*cos(q1)-px*sin(q1)]
[ 0, 0, 0, 1]
T36 =
[cos(q4)*cos(q5)*cos(q6)-sin(q4)*sin(q6), -cos(q6)*sin(q4)-cos(q4)*cos(q5)*sin(q6), cos(q4)*sin(q5), 0]
[cos(q6)*sin(q5), -sin(q5)*sin(q6), -cos(q5), -R4]
[cos(q4)*sin(q6)+cos(q5)*cos(q6)*sin(q4), cos(q4)*cos(q6)-cos(q5)*sin(q4)*sin(q6), sin(q4)*sin(q5), 0]
[0, 0, 0, 1]

T34 =
[cos(q4), 0, sin(q4), 0]
[-sin(q4), 0, cos(q4), 0]
[0, -1, 0, -R4]
[0, 0, 0, 1]

U4 =
[sin(q4)*(sy*cos(q1) - sx*sin(q1)) - cos(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))),
sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) +
ny*sin(q1)))), sin(q4)*(ay*cos(q1) - ax*sin(q1)) - cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) +
sin(q2)*(ax*cos(q1) + ay*sin(q1)))), sin(q4)*(py*cos(q1) - px*sin(q1)) - cos(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) +
sin(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+D3*cos(q3)) ]
[sin(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))) + cos(q4)*(sy*cos(q1) - sx*sin(q1)),
sin(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1)))) + cos(q4)*(ny*cos(q1) -
nx*sin(q1)), sin(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) +
cos(q4)*(ay*cos(q1) - ax*sin(q1)), sin(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1)))
+D3*cos(q3))+cos(q4)*(py*cos(q1)-px*sin(q1)) ]
[cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)) - sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))), cos(q3)*(nz*cos(q2)
+ sin(q2)*(nx*cos(q1) + ny*sin(q1))) - sin(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))), cos(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1)
+ ay*sin(q1))) - sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))), cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) - R4 -
sin(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))-D3*sin(q3) ]
[0, 0, 0, 1]
T46 =
[cos(q5)*cos(q6), -cos(q5)*sin(q6), sin(q5), 0]
[sin(q6), cos(q6), 0, 0]
[-cos(q6)*sin(q5), sin(q5)*sin(q6), cos(q5), 0]
[0, 0, 0, 1]

T45 =
[cos(q5), 0, -sin(q5), 0]
[-sin(q5), 0, -cos(q5), 0]
[0, 1, 0, 0]
[0, 0, 0, 1]

U5 =
[cos(q5)*(sin(q4)*(sy*cos(q1) - sx*sin(q1)) - cos(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) +
sz*cos(q2))))-sin(q5)*(cos(q3)*(sin(q2)*(sx*cos(q1)+sy*sin(q1))+sz*cos(q2))-sin(q3)*(sz*sin(q2)-cos(q2)*(sx*cos(q1)+sy*sin(q1)))),
cos(q5)*(sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) +
ny*sin(q1)))))-sin(q5)*(cos(q3)*(nz*cos(q2)+sin(q2)*(nx*cos(q1)+ny*sin(q1)))-sin(q3)*(nz*sin(q2)-cos(q2)*(nx*cos(q1)+ny*sin(q1)))),
-cos(q5)*(cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) - sin(q4)*(ay*cos(q1) -
ax*sin(q1))) - sin(q5)*(cos(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1))) - sin(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1)))), sin(q5)*(R4 -
cos(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+sin(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))+D3*sin(q3))-
cos(q5)*(cos(q4)*(cos(q3)*(pz*sin(q2)-cos(q2)*(px*cos(q1)+py*sin(q1)))+sin(q3)*(pz*cos(q2)+sin(q2)*(px*cos(q1)+py*sin(q1)))+D3*cos(q3))-
sin(q4)*(py*cos(q1)-px*sin(q1))) ]
[-cos(q5)*(cos(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2)) - sin(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1)))) - sin(q5)*(sin(q4)*(sy*cos(q1) -
sx*sin(q1))-cos(q4)*(cos(q3)*(sz*sin(q2)-cos(q2)*(sx*cos(q1)+sy*sin(q1)))+sin(q3)*(sin(q2)*(sx*cos(q1)+sy*sin(q1))+sz*cos(q2)))),
- sin(q5)*(sin(q4)*(ny*cos(q1) - nx*sin(q1)) - cos(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) +
ny*sin(q1)))))-cos(q5)*(cos(q3)*(nz*cos(q2)+sin(q2)*(nx*cos(q1)+ny*sin(q1)))-sin(q3)*(nz*sin(q2)-cos(q2)*(nx*cos(q1)+ny*sin(q1)))),
sin(q5)*(cos(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) - sin(q4)*(ay*cos(q1) -
ax*sin(q1)))-cos(q5)*(cos(q3)*(az*cos(q2)+sin(q2)*(ax*cos(q1)+ay*sin(q1)))-sin(q3)*(az*sin(q2)-cos(q2)*(ax*cos(q1)+ay*sin(q1)))),
sin(q5)*(cos(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + D3*cos(q3)) -
sin(q4)*(py*cos(q1) - px*sin(q1))) + cos(q5)*(R4 - cos(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) +
py*sin(q1)))+D3*sin(q3)) ]
[sin(q4)*(cos(q3)*(sz*sin(q2) - cos(q2)*(sx*cos(q1) + sy*sin(q1))) + sin(q3)*(sin(q2)*(sx*cos(q1) + sy*sin(q1)) + sz*cos(q2))) + cos(q4)*(sy*cos(q1) - sx*sin(q1)),
sin(q4)*(cos(q3)*(nz*sin(q2) - cos(q2)*(nx*cos(q1) + ny*sin(q1))) + sin(q3)*(nz*cos(q2) + sin(q2)*(nx*cos(q1) + ny*sin(q1)))) + cos(q4)*(ny*cos(q1) -
nx*sin(q1)),
sin(q4)*(cos(q3)*(az*sin(q2) - cos(q2)*(ax*cos(q1) + ay*sin(q1))) + sin(q3)*(az*cos(q2) + sin(q2)*(ax*cos(q1) + ay*sin(q1)))) + cos(q4)*(ay*cos(q1) -
ax*sin(q1)),
sin(q4)*(cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) + sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) + D3*cos(q3)) +
cos(q4)*(py*cos(q1)-px*sin(q1)) ]
[0, 0, 0, 1]

T56 =
[cos(q6), -sin(q6), 0, 0]
[0, 0, -1, 0]
[sin(q6), cos(q6), 0, 0]
[0, 0, 0, 1]
 3.4.2. Ecuaciones para cada q
%q1
ec1=py*cos(q1)-px*sin(q1);
q1s=atan2(py,px);
pretty(q1s);

%q2
ec2=simplify((cos(q2)*(px*cos(q1)+py*sin(q1))-pz*sin(q2)-D3)^2+(-pz*cos(q2)-
sin(q2)*(px*cos(q1)+py*sin(q1)))^2-R4^2);
q2s=solve(ec2,q2,'Real',true);
q2s=simplify(q2s);
pretty(q2s);

%q3
ec3=simplify(- cos(q3)*(pz*sin(q2) - cos(q2)*(px*cos(q1) + py*sin(q1))) -
sin(q3)*(pz*cos(q2) + sin(q2)*(px*cos(q1) + py*sin(q1))) - D3*cos(q3));
q3s=solve(ec3,q3,'Real',true);
q3s = simplify(q3s)
pretty(q3s)
%
% %q4
ec4=simplify(T36(3,3)/T36(1,3));
ec4=ec4-simplify(U3(3,3)/U3(1,3));
q4s=solve(ec4,q4,'Real',true);
q4s=simplify(q4s);
pretty(q4s);

%q5
ec5=simplify(T46(1,3)/T46(3,3));
ec5=ec5-simplify(U4(1,3)/U4(3,3));
q5s=solve(ec5,q5,'Real',true);
q5s=simplify(q5s);
pretty(q5s);

% q6
ec6=simplify(-T36(2,2)/T36(2,1));
ec6=ec6-simplify(-U3(2,2)/U3(2,1));
q6s=solve(ec6,q6,'Real',true);
q6s=simplify(q6s)
pretty(q6s)

Al realizar las operaciones en Matlab se obtuvieron los siguientes resultados para cada
q:

q1s = atan2(py, px)

q2s =

-2*atan((2*D3*pz - (-(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 - 2*D3*R4

+ 2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 +
2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2))/(px^2*cos(q1)^2 + py^2*sin(q1)^2 + D3^2
- R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1) + 2*px*py*cos(q1)*sin(q1)))

-2*atan((2*D3*pz + (-(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 - 2*D3*R4

+ 2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2 + py^2*sin(q1)^2 - D3^2 - R4^2 + pz^2 +
2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2))/(px^2*cos(q1)^2 + py^2*sin(q1)^2 + D3^2
- R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1) + 2*px*py*cos(q1)*sin(q1)))

/ / 2 D3 pz - #1 \ \
| -atan| ------------ | 2 |
| \ #2 / |
| |
| / 2 D3 pz + #1 \ |
| -atan| ------------ | 2 |
\ \ #2 / /

En donde:

2 2 2 2 2 2
#1 == sqrt(-(#5 + #4 - D3 - R4 + pz - 2 D3 R4 + #3) (#5 + #4 - D3 - R4 + pz + 2 D3 R4 + #3))

2 2 2
#2 == #5 + #4 + D3 - R4 + pz + 2 D3 px cos(q1) + 2 D3 py sin(q1) + #3

#3 == 2 px py cos(q1) sin(q1)

2 2
#4 == py sin(q1)

2 2
#5 == px cos(q1)

q3s =

2*atan((pz*cos(q2) + (px^2*cos(q1)^2 - py^2*cos(q1)^2 + D3^2 + py^2 + pz^2 + px*py*sin(2*q1)

+ 2*D3*pz*sin(q2) - 2*D3*px*cos(q1)*cos(q2) - 2*D3*py*cos(q2)*sin(q1))^(1/2) +
px*cos(q1)*sin(q2) + py*sin(q1)*sin(q2))/(D3 + pz*sin(q2) - px*cos(q1)*cos(q2) -
py*cos(q2)*sin(q1)))

2*atan((pz*cos(q2) - (px^2*cos(q1)^2 - py^2*cos(q1)^2 + D3^2 + py^2 + pz^2 +

px*py*sin(2*q1) + 2*D3*pz*sin(q2) - 2*D3*px*cos(q1)*cos(q2) -
2*D3*py*cos(q2)*sin(q1))^(1/2) + px*cos(q1)*sin(q2) + py*sin(q1)*sin(q2))/(D3 + pz*sin(q2) -
px*cos(q1)*cos(q2) - py*cos(q2)*sin(q1)))
 / / pz cos(q2) + #1 + px cos(q1) sin(q2) + py sin(q1) sin(q2) \ \
| atan| --------------------------------------------------------- | 2 |
| \ #2 / |
| |
| / pz cos(q2) - #1 + px cos(q1) sin(q2) + py sin(q1) sin(q2) \ |
| atan| --------------------------------------------------------- | 2 |
\ \ #2 / /

En donde:

2 2 2 2 2 2 2
#1 == sqrt(px cos(q1) - py cos(q1) + D3 + py + pz + px py sin(2 q1) + 2 D3 pz sin(q2) - 2 D3
px cos(q1) cos(q2) - 2 D3 py

cos(q2) sin(q1))

#2 == D3 + pz sin(q2) - px cos(q1) cos(q2) - py cos(q2) sin(q1)

q4s =

-2*atan((ax*cos(q2 - q1 + q3) - ay*sin(q2 - q1 + q3) - 2*az*sin(q2 + q3) + ax*cos(q1 + q2 + q3) +

ay*sin(q1 + q2 + q3) + 2^(1/2)*((ay^2*cos(2*q1))/2 - (ax^2*cos(2*q1))/2 + (ax^2*cos(2*q2 -
2*q1 + 2*q3))/4 + (ax^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (ay^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(ay^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ax^2*cos(2*q2 + 2*q3))/2 + (ay^2*cos(2*q2 + 2*q3))/2 -
az^2*cos(2*q2 + 2*q3) + (3*ax^2)/2 + (3*ay^2)/2 + az^2 - ax*az*sin(q1 + 2*q2 + 2*q3) -
ay*az*cos(2*q2 - q1 + 2*q3) - ax*ay*sin(2*q1) - (ax*ay*sin(2*q2 - 2*q1 + 2*q3))/2 +
(ax*ay*sin(2*q1 + 2*q2 + 2*q3))/2 - ax*az*sin(2*q2 - q1 + 2*q3) + ay*az*cos(q1 + 2*q2 +
2*q3))^(1/2))/(2*ay*cos(q1) - 2*ax*sin(q1)))

-2*atan((ax*cos(q2 - q1 + q3) - ay*sin(q2 - q1 + q3) - 2*az*sin(q2 + q3) + ax*cos(q1 + q2 + q3) +

ay*sin(q1 + q2 + q3) - 2^(1/2)*((ay^2*cos(2*q1))/2 - (ax^2*cos(2*q1))/2 + (ax^2*cos(2*q2 -
2*q1 + 2*q3))/4 + (ax^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (ay^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(ay^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ax^2*cos(2*q2 + 2*q3))/2 + (ay^2*cos(2*q2 + 2*q3))/2 -
az^2*cos(2*q2 + 2*q3) + (3*ax^2)/2 + (3*ay^2)/2 + az^2 - ax*az*sin(q1 + 2*q2 + 2*q3) -
ay*az*cos(2*q2 - q1 + 2*q3) - ax*ay*sin(2*q1) - (ax*ay*sin(2*q2 - 2*q1 + 2*q3))/2 +
(ax*ay*sin(2*q1 + 2*q2 + 2*q3))/2 - ax*az*sin(2*q2 - q1 + 2*q3) + ay*az*cos(q1 + 2*q2 +
2*q3))^(1/2))/(2*ay*cos(q1) - 2*ax*sin(q1)))

/ / #4 - #2 - #6 + #5 + #3 + #7 \ \
| -atan| --------------------------- | 2 |
| \ #1 / |
| |
| / #4 - #2 - #6 + #5 + #3 - #7 \ |
| -atan| --------------------------- | 2 |
\ \ #1 / /

En donde:

#1 == 2 ay cos(q1) - 2 ax sin(q1)
 #2 == ay sin(q2 - q1 + q3)

#3 == ay sin(q1 + q2 + q3)

#4 == ax cos(q2 - q1 + q3)

#5 == ax cos(q1 + q2 + q3)

#6 == 2 az sin(q2 + q3)

/ 2 2 2 2 2 2 2 2
| ay cos(2 q1) ax cos(2 q1) ax cos(#9) ax cos(#10) ay cos(#9) ay cos(#10) ax
#8 ay #8
#7 == sqrt(2) sqrt| ------------- - ------------- + ----------- + ------------ - ----------- - ------------ + ------ + -
-----
\ 2 2 4 4 4 4 2 2

2 2
2 3 ax 3 ay 2 ax ay sin(#9) ax ay sin(#10)
- az #8 + ----- + ----- + az - ax az sin(#12) - ay az cos(#11) - ax ay sin(2 q1) - ------------- + --------
------
2 2 2 2

\
|
- ax az sin(#11) + ay az cos(#12) |
/

#8 == cos(2 q2 + 2 q3)

#9 == 2 q2 - 2 q1 + 2 q3

#10 == 2 q1 + 2 q2 + 2 q3

#11 == 2 q2 - q1 + 2 q3

#12 == q1 + 2 q2 + 2 q3

q5s =

2*atan(((az^2*cos(q2)^2*cos(q3)^2 + ay^2*cos(q1)^2*sin(q4)^2 + ax^2*sin(q1)^2*sin(q4)^2 +

az^2*sin(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2 +
ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2 +
az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2 + ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2 +
ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2 + ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2 -
2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2 + ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2 -
2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2 +
ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2 - 2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2 -
2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3) + 2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2 +
2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2 +
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) +
2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) +
2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3) +
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2) + 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3) -
2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2 - 2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3) +
2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2) + 2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3) -
2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3) -
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2) -
2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3) +
2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2 +
2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3) -
2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2) -
2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3) +
2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2 +
2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3) -
2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) +
2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) +
2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) -
2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) -
2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4) -
2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4) +
2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1) +
2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4) +
2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4) +
2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2 +
2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4) -
2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4) -
2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4) +
2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4) -
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) +
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3) -
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/2) +
az*cos(q2)*cos(q3) - az*sin(q2)*sin(q3) + ax*cos(q1)*cos(q2)*sin(q3) +
ax*cos(q1)*cos(q3)*sin(q2) + ay*cos(q2)*sin(q1)*sin(q3) +
ay*cos(q3)*sin(q1)*sin(q2))/(ax*sin(q1)*sin(q4) - ay*cos(q1)*sin(q4) +
az*cos(q2)*cos(q4)*sin(q3) + az*cos(q3)*cos(q4)*sin(q2) - ax*cos(q1)*cos(q2)*cos(q3)*cos(q4)
- ay*cos(q2)*cos(q3)*cos(q4)*sin(q1) + ax*cos(q1)*cos(q4)*sin(q2)*sin(q3) +
ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)))

2*atan((az*cos(q2)*cos(q3) - (az^2*cos(q2)^2*cos(q3)^2 + ay^2*cos(q1)^2*sin(q4)^2 +

ax^2*sin(q1)^2*sin(q4)^2 + az^2*sin(q2)^2*sin(q3)^2 + ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2 +
ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2 +
az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2 + ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2 +
ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2 + ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2 -
2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2 + ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2 -
2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) + ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2 +
ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2 - 2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2 -
2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3) + 2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2 +
2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2 +
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3) +
2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) +
2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3) +
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2) + 2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3) -
2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2 - 2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3) +
2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2) + 2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3) -
2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3) -
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2) -
2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3) +
2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2 +
2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3) -
2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2) -
2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3) +
2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2 +
2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3) -
2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) +
2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4) +
2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) -
2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4) -
2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4) -
2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4) +
2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1) +
2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4) +
2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4) +
2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2 +
2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4) -
2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4) -
2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4) +
2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4) -
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3) +
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3) -
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/2) -
az*sin(q2)*sin(q3) + ax*cos(q1)*cos(q2)*sin(q3) + ax*cos(q1)*cos(q3)*sin(q2) +
ay*cos(q2)*sin(q1)*sin(q3) + ay*cos(q3)*sin(q1)*sin(q2))/(ax*sin(q1)*sin(q4) -
ay*cos(q1)*sin(q4) + az*cos(q2)*cos(q4)*sin(q3) + az*cos(q3)*cos(q4)*sin(q2) -
ax*cos(q1)*cos(q2)*cos(q3)*cos(q4) - ay*cos(q2)*cos(q3)*cos(q4)*sin(q1) +
ax*cos(q1)*cos(q4)*sin(q2)*sin(q3) + ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)))

/ / #1 + #8 - #7 + #6 + #5 + #4 + #3 \ \
| atan| -------------------------------- | 2 |
| \ #2 / |
| |
| / #8 - #1 - #7 + #6 + #5 + #4 + #3 \ |
| atan| -------------------------------- | 2 |
\ \ #2 / /

En donde:
 2 2 2 2 2 2 2 2 2 2 2 2
#1 == sqrt(az cos(q2) cos(q3) + ay cos(q1) sin(q4) + ax sin(q1) sin(q4) + az sin(q2) sin(q3)

2 2 2 2 2 2 2 2 2 2 2 2
+ ax cos(q1) cos(q2) sin(q3) + ax cos(q1) cos(q3) sin(q2) + az cos(q2) cos(q4) sin(q3)

2 2 2 2 2 2 2 2 2 2 2 2
+ az cos(q3) cos(q4) sin(q2) + ay cos(q2) sin(q1) sin(q3) + ay cos(q3) sin(q1) sin(q2)

2 2 2 2 2 2 2 2 2 2 2
+ ax cos(q1) cos(q4) sin(q2) sin(q3) - 2 ax ay cos(q1) sin(q1) sin(q4) + ay cos(q4) sin(q1)
sin(q2) sin(q3)

2 2 2 2 2 2 2 2 2 2 2
- 2 az cos(q2) cos(q3) sin(q2) sin(q3) + ax cos(q1) cos(q2) cos(q3) cos(q4) + ay cos(q2)
cos(q3) cos(q4) sin(q1)

2 2 2
- 2 ay az cos(q2) sin(q1) sin(q2) sin(q3) - 2 ay az cos(q3) sin(q1) sin(q2) sin(q3) + 2 ax ay
cos(q1) cos(q2)

2 2 2 2 2 2
sin(q1) sin(q3) + 2 ax ay cos(q1) cos(q3) sin(q1) sin(q2) + 2 ax cos(q1) cos(q2) cos(q3)
sin(q2) sin(q3) + 2 az

2 2 2 2
cos(q2) cos(q3) cos(q4) sin(q2) sin(q3) + 2 ay cos(q2) cos(q3) sin(q1) sin(q2) sin(q3) + 2 ax
az cos(q1) cos(q2) cos(q3)

2 2
sin(q2) + 2 ax az cos(q1) cos(q2) cos(q3) sin(q3) - 2 ax az cos(q1) cos(q2) sin(q2) sin(q3)

2 2 2
- 2 ax az cos(q1) cos(q3) sin(q2) sin(q3) + 2 ay az cos(q2) cos(q3) sin(q1) sin(q2) + 2 ay az
cos(q2)

2 2 2 2 2
cos(q3) sin(q1) sin(q3) - 2 ay cos(q2) cos(q3) cos(q4) sin(q1) sin(q2) sin(q3) - 2 ax az cos(q1)
cos(q2) cos(q3) cos(q4)

2 2 2 2
sin(q2) - 2 ax az cos(q1) cos(q2) cos(q3) cos(q4) sin(q3) + 2 ax az cos(q1) cos(q2) cos(q4)
sin(q2) sin(q3)

2 2 2 2 2
+ 2 ax az cos(q1) cos(q3) cos(q4) sin(q2) sin(q3) - 2 ay az cos(q2) cos(q3) cos(q4) sin(q1)
sin(q2) - 2 ay az cos(q2)

2 2 2 2
cos(q3) cos(q4) sin(q1) sin(q3) + 2 ay az cos(q2) cos(q4) sin(q1) sin(q2) sin(q3) + 2 ay az
cos(q3) cos(q4)
 2 2 2
sin(q1) sin(q2) sin(q3) - 2 ax cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q4) + 2 ay

2 2
cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q4) + 2 ax cos(q1) cos(q4) sin(q1) sin(q2) sin(q3)
sin(q4) - 2 ay

cos(q1) cos(q4) sin(q1) sin(q2) sin(q3) sin(q4) - 2 ay az cos(q1) cos(q2) cos(q4) sin(q3) sin(q4)
- 2 ay az

2 2 2
cos(q1) cos(q3) cos(q4) sin(q2) sin(q4) + 2 ax ay cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) + 2
ax az

2
cos(q2) cos(q4) sin(q1) sin(q3) sin(q4) + 2 ax az cos(q3) cos(q4) sin(q1) sin(q2) sin(q4) + 2 ax
ay cos(q1) cos(q4)

2 2 2 2
sin(q1) sin(q2) sin(q3) + 2 ax ay cos(q1) cos(q2) cos(q3) cos(q4) sin(q4) - 2 ax ay cos(q2)
cos(q3) cos(q4) sin(q1)

2 2 2 2
sin(q4) - 2 ax ay cos(q1) cos(q4) sin(q2) sin(q3) sin(q4) + 2 ax ay cos(q4) sin(q1) sin(q2) sin(q3)
sin(q4) - 2 ax cos(q1)

2
cos(q2) cos(q3) cos(q4) sin(q2) sin(q3) + 4 ax ay cos(q1) cos(q2) cos(q3) sin(q1) sin(q2) sin(q3)
- 4 ax ay

2
cos(q1) cos(q2) cos(q3) cos(q4) sin(q1) sin(q2) sin(q3))

#2 == ax sin(q1) sin(q4) - ay cos(q1) sin(q4) + az cos(q2) cos(q4) sin(q3) + az cos(q3) cos(q4)

sin(q2) - ax

cos(q1) cos(q2) cos(q3) cos(q4) - ay cos(q2) cos(q3) cos(q4) sin(q1) + ax cos(q1) cos(q4)
sin(q2) sin(q3) + ay

cos(q4) sin(q1) sin(q2) sin(q3)

#3 == ay cos(q3) sin(q1) sin(q2)

#4 == ay cos(q2) sin(q1) sin(q3)

#5 == ax cos(q1) cos(q3) sin(q2)

#6 == ax cos(q1) cos(q2) sin(q3)

#7 == az sin(q2) sin(q3)

#8 == az cos(q2) cos(q3)
q6s =

2*atan((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2 + q3) +

2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 - (nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2
+ 2*q3))/4 + (sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (sy^2*cos(2*q1 + 2*q2
+ 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2 - (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) -
(sx^2*cos(2*q2 + 2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 +
ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) - sy*sz*cos(q1 + 2*q2 +
2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1 + 2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 +
2*q3) + sy*sz*cos(2*q2 - q1 + 2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2 - 2*q1 + 2*q3))/2 -
(nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) + sx*sy*sin(2*q1) +
(sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 + 2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 +
2*q3))^(1/2) - sy*cos(q1 + q2 + q3) + sx*sin(q1 + q2 + q3))/(ny*cos(q2 - q1 + q3) + nx*sin(q2 -
q1 + q3) + 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)))

2*atan((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2 + q3) -

2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 - (nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2
+ 2*q3))/4 + (sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 + 2*q3))/4 -
(sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (sy^2*cos(2*q1 + 2*q2
+ 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2 - (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) -
(sx^2*cos(2*q2 + 2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 +
ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) - sy*sz*cos(q1 + 2*q2 +
2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1 + 2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 +
2*q3) + sy*sz*cos(2*q2 - q1 + 2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2 - 2*q1 + 2*q3))/2 -
(nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) + sx*sy*sin(2*q1) +
(sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 + 2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 +
2*q3))^(1/2) - sy*cos(q1 + q2 + q3) + sx*sin(q1 + q2 + q3))/(ny*cos(q2 - q1 + q3) + nx*sin(q2 -
q1 + q3) + 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)))

/ / sy #11 + sx #9 + #1 + #2 - sy #12 + sx #10 \ \

| atan| ------------------------------------------ | 2 |
| \ #3 / |
| |
| / sy #11 + sx #9 + #1 - #2 - sy #12 + sx #10 \ |
| atan| ------------------------------------------ | 2 |
\ \ #3 / /

En donde:

#1 == 2 sz cos(q2 + q3)
 / 2 2 2 2 2 2 2
| nx cos(2 q1) ny cos(2 q1) nx cos(#5) nx cos(#6) ny cos(#5) ny cos(#6) sx
cos(2 q1)
#2 == sqrt(2) sqrt| ------------- - ------------- - ----------- - ----------- + ----------- + ----------- + ------------
-
\ 2 2 4 4 4 4 2

2 2 2 2 2 2 2 2 2
sy cos(2 q1) sx cos(#5) sx cos(#6) sy cos(#5) sy cos(#6) nx #4 ny #4 2 sx #4
sy #4
- ------------- - ----------- - ----------- + ----------- + ----------- - ------ - ------ + nz #4 - ------ - ------
2 4 4 4 4 2 2 2 2

2 2 2 2
2 nx ny 2 sx sy 2
+ sz #4 + --- + --- + nz + --- + --- + sz - ny nz cos(#8) - sy sz cos(#8) + nx nz sin(#8) + sx sz sin(#8)
+ ny nz
2 2 2 2

nx ny sin(#5) nx ny sin(#6) sx sy sin(#5)

cos(#7) + sy sz cos(#7) + nx ny sin(2 q1) + ------------- - ------------- + nx nz sin(#7) + sx sy sin(2
q1) + -------------
2 2 2

\
sx sy sin(#6) |
- ------------- + sx sz sin(#7) |
2 /

#3 == ny #11 + nx #9 + 2 nz cos(q2 + q3) - ny #12 + nx #10

#4 == cos(2 q2 + 2 q3)

#5 == 2 q2 - 2 q1 + 2 q3

#6 == 2 q1 + 2 q2 + 2 q3

#7 == 2 q2 - q1 + 2 q3

#8 == q1 + 2 q2 + 2 q3

#9 == sin(q2 - q1 + q3)

#10 == sin(q1 + q2 + q3)

#11 == cos(q2 - q1 + q3)

#12 == cos(q1 + q2 + q3)

Se crearon dos funciones en Matlab una llamada MGD la cual nos solicita los valores de
q1, q2, q3, q4, q5, q6 y nos entrega los valores de sx, sy, sz, nx, ny, nz, ax, ay, az, px,
py, pz. Tal como se muestra a continuación:

Teniendo en cuenta que los limites que se probaron para q1, q2 y q3 según la estructura
de la interfaz:

• q1= -1.05 a 1.05

• q2= 0 a -1.57
• q3= 1.6 a 4

Los valores se establecen en radiales.

function salida=MGD (q1, q2, q3, q4, q5, q6)

%Modelo Geométrico Directo

R4=13.5; %cm
D3=14; %cm

U0=[ - sin(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)) -

cos(q6)*(cos(q5)*(sin(q1)*sin(q4) - cos(q2 + q3)*cos(q1)*cos(q4)) +
sin(q2 + q3)*cos(q1)*sin(q5)), sin(q6)*(cos(q5)*(sin(q1)*sin(q4) -
cos(q2 + q3)*cos(q1)*cos(q4)) + sin(q2 + q3)*cos(q1)*sin(q5)) -
cos(q6)*(cos(q4)*sin(q1) + cos(q2 + q3)*cos(q1)*sin(q4)), sin(q2 +
q3)*cos(q1)*cos(q5) - sin(q5)*(sin(q1)*sin(q4) - cos(q2 +
q3)*cos(q1)*cos(q4)), cos(q1)*(R4*sin(q2 + q3) + D3*cos(q2));

sin(q6)*(cos(q1)*cos(q4) - cos(q2 + q3)*sin(q1)*sin(q4)) +

cos(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) -
sin(q2 + q3)*sin(q1)*sin(q5)), cos(q6)*(cos(q1)*cos(q4) - cos(q2 +
q3)*sin(q1)*sin(q4)) - sin(q6)*(cos(q5)*(cos(q1)*sin(q4) + cos(q2 +
q3)*cos(q4)*sin(q1)) - sin(q2 + q3)*sin(q1)*sin(q5)),
sin(q5)*(cos(q1)*sin(q4) + cos(q2 + q3)*cos(q4)*sin(q1)) + sin(q2 +
q3)*cos(q5)*sin(q1), sin(q1)*(R4*sin(q2 + q3) + D3*cos(q2));

sin(q2 + q3)*sin(q4)*sin(q6) - cos(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 +

q3)*cos(q4)*cos(q5)),sin(q6)*(cos(q2 + q3)*sin(q5) + sin(q2 +
q3)*cos(q4)*cos(q5)) + sin(q2 + q3)*cos(q6)*sin(q4),
cos(q2 + q3)*cos(q5) - sin(q2 + q3)*cos(q4)*sin(q5), R4*cos(q2
+ q3) - D3*sin(q2);

0, 0, 0, 1];

sx=U0(1,1);
sy=U0(2,1);
sz=U0(3,1);
nx=U0(1,2);
ny=U0(2,2);
nz=U0(3,2);
ax=U0(1,3);
ay=U0(2,3);
az=U0(3,3);
px=U0(1,4);
py=U0(2,4);
pz=U0(3,4);

salida= [sx sy sz nx ny nz ax ay az px py pz];

Teniendo los valores de sx, sy, sz, nx, ny, nz, ax, ay, az, px, py, pz se pues calcular el MGI
para cual se creó la siguiente función con el fin que sean calculados los valore de q1, q2,
q3, q4, q5, q6, Tal como se muestra a continuación:

function salida=MGI(sx,sy,sz,nx,ny,nz,ax,ay,az,px,py,pz)

R4=13.5;
D3=14; %cm

%Ecuación para q1

q1=atan2(py,px);

%Ecuación para q2

q2=-2*atan2((2*D3*pz+(-(px^2*cos(q1)^2+py^2*sin(q1)^2-D3^2-R4^2+pz^2-
2*D3*R4+2*px*py*cos(q1)*sin(q1))*(px^2*cos(q1)^2+py^2*sin(q1)^2-D3^2-
R4^2 + pz^2 + 2*D3*R4 + 2*px*py*cos(q1)*sin(q1)))^(1/2)),(px^2*cos(q1)^2
+ py^2*sin(q1)^2 + D3^2 - R4^2 + pz^2 + 2*D3*px*cos(q1) + 2*D3*py*sin(q1)
+ 2*px*py*cos(q1)*sin(q1)));

%Ecuación para q3

q3=2*atan2(-(pz*cos(q2)-(px^2*cos(q1)^2-py^2*cos(q1)^2+D3^2+py^2+pz^2
+px*py*sin(2*q1)+2*D3*pz*sin(q2)-2*D3*px*cos(q1)*cos(q2)-2*D3*py
*cos(q2)*sin(q1))^(1/2)+px*cos(q1)*sin(q2)+py*sin(q1)*sin(q2)),-
(D3+pz*sin(q2)- px*cos(q1)*cos(q2)-py*cos(q2)*sin(q1)));

%Ecuación para q4

q4=atan2(-(ay*cos(q1)-ax*sin(q1)), (cos(q3)*(az*sin(q2)-cos(q2)*(ax*cos
(q1)+ay*sin(q1)))+sin(q3)*(az*cos(q2)+sin(q2)*(ax*cos(q1)+ay*sin(q1))
)));

%Ecuación para q5

q5=2*atan2(-(az*cos(q2)*cos(q3)-(az^2*cos(q2)^2*cos(q3)^2+
ay^2*cos(q1)^2*sin(q4)^2+ax^2*sin(q1)^2*sin(q4)^2+
az^2*sin(q2)^2*sin(q3)^2+ax^2*cos(q1)^2*cos(q2)^2*sin(q3)^2+
ax^2*cos(q1)^2*cos(q3)^2*sin(q2)^2 + az^2*cos(q2)^2*cos(q4)^2*sin(q3)^2
+az^2*cos(q3)^2*cos(q4)^2*sin(q2)^2+ ay^2*cos(q2)^2*sin(q1)^2*sin(q3)^2
+ay^2*cos(q3)^2*sin(q1)^2*sin(q2)^2+
ax^2*cos(q1)^2*cos(q4)^2*sin(q2)^2*sin(q3)^2-
2*ax*ay*cos(q1)*sin(q1)*sin(q4)^2+
ay^2*cos(q4)^2*sin(q1)^2*sin(q2)^2*sin(q3)^2-
2*az^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)+
ax^2*cos(q1)^2*cos(q2)^2*cos(q3)^2*cos(q4)^2+
ay^2*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)^2-
2*ay*az*cos(q2)*sin(q1)*sin(q2)*sin(q3)^2-
2*ay*az*cos(q3)*sin(q1)*sin(q2)^2*sin(q3)+
2*ax*ay*cos(q1)*cos(q2)^2*sin(q1)*sin(q3)^2+
2*ax*ay*cos(q1)*cos(q3)^2*sin(q1)*sin(q2)^2+
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*sin(q2)*sin(q3)+
2*az^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3)+
2*ay^2*cos(q2)*cos(q3)*sin(q1)^2*sin(q2)*sin(q3)+
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*sin(q2)+
2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*sin(q3)-
2*ax*az*cos(q1)*cos(q2)*sin(q2)*sin(q3)^2-
2*ax*az*cos(q1)*cos(q3)*sin(q2)^2*sin(q3)+
2*ay*az*cos(q2)*cos(q3)^2*sin(q1)*sin(q2)+
2*ay*az*cos(q2)^2*cos(q3)*sin(q1)*sin(q3)-
2*ay^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)^2*sin(q2)*sin(q3)-
2*ax*az*cos(q1)*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q2)-
2*ax*az*cos(q1)*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q3)+
2*ax*az*cos(q1)*cos(q2)*cos(q4)^2*sin(q2)*sin(q3)^2+
2*ax*az*cos(q1)*cos(q3)*cos(q4)^2*sin(q2)^2*sin(q3)-
2*ay*az*cos(q2)*cos(q3)^2*cos(q4)^2*sin(q1)*sin(q2)-
2*ay*az*cos(q2)^2*cos(q3)*cos(q4)^2*sin(q1)*sin(q3)+
2*ay*az*cos(q2)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3)^2+
2*ay*az*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)-
2*ax^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4)+
2*ay^2*cos(q1)*cos(q2)*cos(q3)*cos(q4)*sin(q1)*sin(q4)+
2*ax^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4)-
2*ay^2*cos(q1)*cos(q4)*sin(q1)*sin(q2)*sin(q3)*sin(q4)-
2*ay*az*cos(q1)*cos(q2)*cos(q4)*sin(q3)*sin(q4)-
2*ay*az*cos(q1)*cos(q3)*cos(q4)*sin(q2)*sin(q4)+
2*ax*ay*cos(q1)*cos(q2)^2*cos(q3)^2*cos(q4)^2*sin(q1)+
2*ax*az*cos(q2)*cos(q4)*sin(q1)*sin(q3)*sin(q4)+
2*ax*az*cos(q3)*cos(q4)*sin(q1)*sin(q2)*sin(q4)+
2*ax*ay*cos(q1)*cos(q4)^2*sin(q1)*sin(q2)^2*sin(q3)^2+
2*ax*ay*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)*sin(q4)-
2*ax*ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)^2*sin(q4)-
2*ax*ay*cos(q1)^2*cos(q4)*sin(q2)*sin(q3)*sin(q4)+
2*ax*ay*cos(q4)*sin(q1)^2*sin(q2)*sin(q3)*sin(q4)-
2*ax^2*cos(q1)^2*cos(q2)*cos(q3)*cos(q4)^2*sin(q2)*sin(q3)+
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*sin(q1)*sin(q2)*sin(q3)-
4*ax*ay*cos(q1)*cos(q2)*cos(q3)*cos(q4)^2*sin(q1)*sin(q2)*sin(q3))^(1/
2)-az*sin(q2)*sin(q3)+ax*cos(q1)*cos(q2)*sin(q3)+
ax*cos(q1)*cos(q3)*sin(q2)+ay*cos(q2)*sin(q1)*sin(q3)+
ay*cos(q3)*sin(q1)*sin(q2)),-(ax*sin(q1)*sin(q4) - ay*cos(q1)*sin(q4)+
az*cos(q2)*cos(q4)*sin(q3)+az*cos(q3)*cos(q4)*sin(q2)-
ax*cos(q1)*cos(q2)*cos(q3)*cos(q4) - ay*cos(q2)*cos(q3)*cos(q4)*sin(q1)
+ax*cos(q1)*cos(q4)*sin(q2)*sin(q3)+ay*cos(q4)*sin(q1)*sin(q2)*sin(q3)
));

%Ecuación para q6

q6= 2*atan2((sy*cos(q2 - q1 + q3) + sx*sin(q2 - q1 + q3) + 2*sz*cos(q2

+ q3) - 2^(1/2)*((nx^2*cos(2*q1))/2 - (ny^2*cos(2*q1))/2 -
(nx^2*cos(2*q2 - 2*q1 + 2*q3))/4 - (nx^2*cos(2*q1 + 2*q2 + 2*q3))/4 +
(ny^2*cos(2*q2 - 2*q1 + 2*q3))/4 + (ny^2*cos(2*q1 + 2*q2 + 2*q3))/4 +
(sx^2*cos(2*q1))/2 - (sy^2*cos(2*q1))/2 - (sx^2*cos(2*q2 - 2*q1 +
2*q3))/4 - (sx^2*cos(2*q1 + 2*q2 + 2*q3))/4 + (sy^2*cos(2*q2 - 2*q1 +
2*q3))/4 + (sy^2*cos(2*q1 + 2*q2 + 2*q3))/4 - (nx^2*cos(2*q2 + 2*q3))/2
- (ny^2*cos(2*q2 + 2*q3))/2 + nz^2*cos(2*q2 + 2*q3) - (sx^2*cos(2*q2 +
2*q3))/2 - (sy^2*cos(2*q2 + 2*q3))/2 + sz^2*cos(2*q2 + 2*q3) + nx^2/2 +
ny^2/2 + nz^2 + sx^2/2 + sy^2/2 + sz^2 - ny*nz*cos(q1 + 2*q2 + 2*q3) -
sy*sz*cos(q1 + 2*q2 + 2*q3) + nx*nz*sin(q1 + 2*q2 + 2*q3) + sx*sz*sin(q1
+ 2*q2 + 2*q3) + ny*nz*cos(2*q2 - q1 + 2*q3) + sy*sz*cos(2*q2 - q1 +
2*q3) + nx*ny*sin(2*q1) + (nx*ny*sin(2*q2 - 2*q1 + 2*q3))/2 -
(nx*ny*sin(2*q1 + 2*q2 + 2*q3))/2 + nx*nz*sin(2*q2 - q1 + 2*q3) +
sx*sy*sin(2*q1) + (sx*sy*sin(2*q2 - 2*q1 + 2*q3))/2 - (sx*sy*sin(2*q1 +
2*q2 + 2*q3))/2 + sx*sz*sin(2*q2 - q1 + 2*q3))^(1/2) - sy*cos(q1 + q2 +
q3) + sx*sin(q1 + q2 + q3)),(ny*cos(q2 - q1 + q3) + nx*sin(q2 - q1 + q3)
+ 2*nz*cos(q2 + q3) - ny*cos(q1 + q2 + q3) + nx*sin(q1 + q2 + q3)));

salida= [q1 q2 q3 q4 q5 q6];

Published with MATLAB® R2016a

4. Prueba de los modelos geométricos en Simulink

4.1. Modelo de la interfaz en 3D

Mediante el uso de Simulink y más específicamente de la toolbox de 3D Animation

se creó un modelo virtual mediante la implementación del bloque VR Sink (figura 4).

Figura 4. Bloque VR Sink

El primer paso fue el de crear el modelo en 3D por medio del 3D World Editor en
donde se tuvo como referencia los tres primeros ángulos del robot (q1, q2 y q3),
obteniendo como resultado la siguiente estructura:

Figura 5. Imagen del modelo 3D de la interfaz en 3D World Editor

 Figura 6. Imágenes del modelo 3D de la interfaz en 3D World Editor

Posteriormente de crear el modelo en el mundo virtual se creó en Simulink una

simulación de los modelos geométricos MGD y MGI en donde con los parámetros de X,
Y y Z dados por la función Circular creada por el Ing. Sergio Salinas se probarán los
modelos geométricos. Al crear y conectar los diferentes bloques en Simulink se obtuvo
el siguiente diagrama:

Figura 7. Diagrama de bloques en Simulink de las pruebas de los Modelos Geométricos.

Para realizar la prueba de los modelos geométricos se siguieron los siguientes pasos:

a) Se ejecutó el archivo circular.m con el fin de obtener los parámetros X, Y y Z.

b) Se ejecuta la simulación Inter1.slx
c) Se abre el VR sink con el fin de ver la simulación de la interfaz 3D.

Se adjuntan video generado por simulimk (video.avi) y los archivos necesarios para la
correcta simulación.

Figura 8. Vista de 3D World Editor

5. Tabla de parámetros inerciales

5.1. Cálculo de los parámetros inerciales de las 3 primeras articulaciones

El cálculo de los parámetros inerciales se realizó utilizando el el método de Lagrange

explicado por el Ing. Sergio Salinas y descrito en el capítulo 4 del libro de Diseño y
control de robots industriales[4]. El desarrollo de los cálculos se realizó de forma
manual por lo cual se adjuntan imágenes (figuras 9 a 14) en donde se evidencia el
proceso desarrollado.
figura 9.

figura 10.
figura 11.

figura 12.
 figura 13.

figura 14.

Teniendo como resultado las siguientes ecuaciones para el cálculo de las tres primeras
articulaciones.

• ZZ1R=IA1 + d3**2*M3 + YY2 + YY3 + ZZ1

 • XX2R=-(d3**2*M3) + XX2 - YY2
• XZ2R=-(d3*MZ3) + XZ2
• ZZ2R=IA2 + d3**2*M3 + ZZ2
• MX2R=d3*M3 + MX2
• XX3R=XX3 - YY3

5.2. Tabla de parámetros inerciales completa

El proceso para calcular los parámetros inerciales se efectuó mediante el software

SYMORO (Symbolic Modelingof Robots) en donde se ingresó cada uno de los valores de
la tabla de parámetros geométricos, para lograr obtener un archivo con las ecuaciones
y la tabla de parámetros inerciales los cuales se muestran a continuación:

(********************************************)
(** SYMORO+ : SYmbolic MOdelling of RObots **)
(**========================================**)
(** IRCCyN-ECN - 1, rue de la Noe **)
(** B.P.92101 **)
(** 44321 Nantes cedex 3, FRANCE **)
(** www.irccyn.ec-nantes.fr **)
(********************************************)

Name of file : D:\Maestria\Sistemas Robo'ticos en Me'dicina\Archivo Ejemplo

Symoro\RobotCompleto\RobotCompletobaseI.parbase.par

Geometric parameters

j ant mu sigma gamma b alpha d theta r

1 0 1 0 0 0 0 0 q1 0

-Pi
2 1 1 0 0 0 --- 0 q2 0
2

3 2 1 0 0 0 0 d3 q3 0

Pi
4 3 0 0 0 0 -- 0 q4 R4
2
-Pi
5 4 0 0 0 0 --- 0 q5 0
2
Pi
6 5 0 0 0 0 -- 0 q6 0
2
Inertial parameters

j XX XY XZ YY YZ ZZ MX MY MZ M Ia

1 XX1 XY1 XZ1 YY1 YZ1 ZZ1 MX1 MY1 MZ1 M1 IA1

2 XX2 XY2 XZ2 YY2 YZ2 ZZ2 MX2 MY2 MZ2 M2 IA2

3 XX3 XY3 XZ3 YY3 YZ3 ZZ3 MX3 MY3 MZ3 M3 IA3

4 XX4 XY4 XZ4 YY4 YZ4 ZZ4 MX4 MY4 MZ4 M4 0

5 XX5 XY5 XZ5 YY5 YZ5 ZZ5 MX5 MY5 MZ5 M5 0

6 XX6 XY6 XZ6 YY6 YZ6 ZZ6 MX6 MY6 MZ6 M6 0

The regrouping relations are

• ZZ1R=IA1 + d3**2*(M3 + M4 + M5 + M6) + YY2 + YY3 + ZZ1

• XX2R=-(d3**2*(M3 + M4 + M5 + M6)) + XX2 - YY2
• XZ2R=-(d3*MZ3) + XZ2
• ZZ2R=IA2 + d3**2*(M3 + M4 + M5 + M6) + ZZ2
• MX2R=d3*(M3 + M4 + M5 + M6) + MX2
• XX3R=2*MZ4*R4 + (M4 + M5 + M6)*R4**2 + XX3 - YY3 + YY4
• ZZ3R=2*MZ4*R4 + (M4 + M5 + M6)*R4**2 + YY4 + ZZ3
• MY3R=MY3 - MZ4 - (M4 + M5 + M6)*R4
• XX4R=XX4 - YY4 + YY5
• ZZ4R=YY5 + ZZ4
• MY4R=MY4 + MZ5
• XX5R=XX5 - YY5 + YY6
• ZZ5R=YY6 + ZZ5
• MY5R=MY5 - MZ6
• XX6R=XX6 - YY6

The base inertial parameters are

Inertial parameters

j XX XY XZ YY YZ ZZ MX MY MZ M Ia
1 0 0 0 0 0 ZZ1R 0 0 0 0 0
2 XX2R XY2 XZ2R 0 YZ2 ZZ2R MX2R MY2 0 0 0
3 XX3R XY3 XZ3 0 YZ3 ZZ3R MX3 MY3R 0 0 IA3
4 XX4R XY4 XZ4 0 YZ4 ZZ4R MX4 MY4R 0 0 0
5 XX5R XY5 XZ5 0 YZ5 ZZ5R MX5 MY5R 0 0 0
6 XX6R XY6 XZ6 0 YZ6 ZZ6 MX6 MY6 0 0 0

6. Piezas en SolidWorks del Robot

6.1. Diseño de piezas en Solidworks del robot

El diseño de las piezas se realizó teniendo como principal referente el diagrama de

la patente US007411576B2, el resultado del trabajo realizado se evidencia en las
figuras 15 a 22.

Figura 15. Pieza A

Figura 16. Pieza B

Figura 17. Pieza C

Figura 18. Pieza D

 Figura 19. Pieza E

Figura 20. Pieza F

Se realizó el ensamble de las piezas diseñadas en Solidworks, tal como se muestra a

continuación en la figura 21.
 Figura 21. Ensamble con las diferentes piezas diseñadas.

6.2. Tablas con los valores de las masas y parámetros de cada pieza del robot

Se utilizó la propiedad del software solidworks mediante el uso de cálculo de

propiedades físicas. Obtenido los valores que se utilizaran en los modelos
cinemáticos.

Figura 22. Parámetros para la pieza A.

Figura 23. Parámetros para la pieza B.

Figura 24. Parámetros para la pieza C.

Figura 25. Parámetros para la pieza D.

Figura 26. Parámetros para la pieza E.

 Figura 27. Parámetros para la pieza F.

7. Modelos Dinámicos

El proceso para obtener los modelos dinámicos se efectuó mediante el software

SYMORO (Symbolic Modelingof Robots) el cual se describe a continuación:

A. Cargar el archivo con los parámetros base o tabla de parámetros geométricos

B. Se ubica Dynamic en la parte superior del software SYMORO
C. Se ubica Inverse Dynamic model para generar un archivo. DYN
D. Ubicar Direct Dynamic model para generar un archivo.DDM
E. Se ubica la pestaña de optimizer en donde se ubicarán los archivos anteriores
F. Generar la optimización sobre los parámetros deseados como salidas de cada
modelo dinamo.
G. Generar archivo .m para ser utilizados en Matlab
H. Modificar los archivos como funciones de Matlab

Al lograr realizar los anteriores pasos se obtiene los siguientes archivos en Matlab:

7.1. Modelo dinámico inverso

% (********************************************)
% (** SYMORO+ : SYmbolic MOdelling of RObots **)
% (**========================================**)
% (** IRCCyN-ECN - 1, rue de la Noe **)
% (** B.P.92101 **)
% (** 44321 Nantes cedex 3, FRANCE **)
% (** www.irccyn.ec-nantes.fr **)
% (********************************************)
% Name of file : D:\Maestria\Sistemas Robo'ticos en Me'dicina\Archivo Ejemplo
Symoro\RobotCompleto\RobotCompletobaseI.parbase.dyn

% Geometric parameters

%j ant mu sigma gamma b alpha d theta r

%1 0 1 0 0 0 0 0 q1 0

% -Pi
%2 1 1 0 0 0 --- 0 q2 0
% 2

%3 2 1 0 0 0 0 d3 q3 0

% Pi
%4 3 0 0 0 0 -- 0 q4 R4
% 2
% -Pi
%5 4 0 0 0 0 --- 0 q5 0
% 2
% Pi
%6 5 0 0 0 0 -- 0 q6 0
% 2

% Inertial parameters

%j XX XY XZ YY YZ ZZ MX MY MZ M Ia

%1 0 0 0 0 0 ZZ1R 0 0 0 0 0

%2 XX2R XY2 XZ2R 0 YZ2 ZZ2R MX2R MY2 0 0 0

%3 XX3R XY3 XZ3 0 YZ3 ZZ3R MX3 MY3R 0 0 IA3

%4 XX4R XY4 XZ4 0 YZ4 ZZ4R MX4 MY4R 0 0 0

%5 XX5R XY5 XZ5 0 YZ5 ZZ5R MX5 MY5R 0 0 0

%6 XX6R XY6 XZ6 0 YZ6 ZZ6 MX6 MY6 0 0 0

% External forces,friction parameters, joint velocities and accelerations

%j FX FY FZ CX CY CZ FS FV QP QDP

%1 FX1 FY1 FZ1 CX1 CY1 CZ1 FS1 FV1 QP1 QDP1
%2 FX2 FY2 FZ2 CX2 CY2 CZ2 FS2 FV2 QP2 QDP2

%3 FX3 FY3 FZ3 CX3 CY3 CZ3 FS3 FV3 QP3 QDP3

%4 FX4 FY4 FZ4 CX4 CY4 CZ4 FS4 FV4 QP4 QDP4

%5 FX5 FY5 FZ5 CX5 CY5 CZ5 FS5 FV5 QP5 QDP5

%6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6

% Base velocity, base accelerations, and gravity

%j W0 WP0 V0 VP0 G

%1 0 0 0 0 0

%2 0 0 0 0 0

%3 0 0 0 0 G3

% Dynamic model: Newton Euler method

% Equations:

% Declaration of the function

functionGAM=MDI(q1,q2,q3,q4,q5,q6,QP1,QP2,QP3,QP4,QP5,QP6,QDP1,QDP2,QDP3,QDP4,QDP5,
QDP6)
% Declaration of global input variables
global ZZ1R G3 XX2R ZZ2R XY2 XZ2R YZ2 d3 MX3
global MY3R XX3R ZZ3R XY3 XZ3 YZ3 R4 MX4
global MY4R XX4R ZZ4R XY4 XZ4 YZ4 MX5 MY5R
global XX5R ZZ5R XY5 XZ5 YZ5 MX6 MY6 XX6R
global ZZ6 XY6 XZ6 YZ6 FX6 FY6 FZ6 CX6 CY6 CZ6
global FX5 FY5 FZ5 CX5 CY5 CZ5 FX4 FY4 FZ4 CX4
global CY4 CZ4 FX3 FY3 FZ3 CX3 CY3 CZ3 CX2 CY2
global CZ2 MY2 MX2R CZ1 FV1 FS1 FV2 FS2 IA3 FV3
global FS3 FV4 FS4 FV5 FS5 FV6 FS6

% Declaration of global output variables

global GAM1 GAM2 GAM3 GAM4 GAM5 GAM6

% Function description:

S2=sin(q2);
C2=cos(q2);
S3=sin(q3);
C3=cos(q3);
S4=sin(q4);
C4=cos(q4);
S5=sin(q5);
C5=cos(q5);
S6=sin(q6);
C6=cos(q6);
No31=QDP1.*ZZ1R;
WI12=-(QP1.*S2);
WI22=-(C2.*QP1);
WP12=-(QDP1.*S2) + QP2.*WI22;
WP22=-(C2.*QDP1) - QP2.*WI12;
DV112=-WI12.^2;
DV222=-WI22.^2;
DV332=-QP2.^2;
DV122=WI12.*WI22;
DV132=QP2.*WI12;
DV232=QP2.*WI22;
U112=DV222 + DV332;
U122=DV122 - QDP2;
U132=DV132 + WP22;
U212=DV122 + QDP2;
U232=DV232 - WP12;
U312=DV132 - WP22;
U322=DV232 + WP12;
VP12=G3.*S2;
VP22=C2.*G3;
PIS22=XX2R - ZZ2R;
No12=WP12.*XX2R - U312.*XY2 + U212.*XZ2R + (-DV222 + DV332).*YZ2 + DV232.*ZZ2R;
No22=DV132.*PIS22 + U322.*XY2 + (DV112 - DV332).*XZ2R - U122.*YZ2;
No32=-(DV122.*XX2R) + (-DV112 + DV222).*XY2 - U232.*XZ2R + U132.*YZ2 + QDP2.*ZZ2R;
WI13=C3.*WI12 + S3.*WI22;
WI23=-(S3.*WI12) + C3.*WI22;
W33=QP2 + QP3;
WP13=QP3.*WI23 + C3.*WP12 + S3.*WP22;
WP23=-(QP3.*WI13) - S3.*WP12 + C3.*WP22;
WP33=QDP2 + QDP3;
DV113=-WI13.^2;
DV223=-WI23.^2;
DV333=-W33.^2;
DV123=WI13.*WI23;
DV133=W33.*WI13;
DV233=W33.*WI23;
U113=DV223 + DV333;
U123=DV123 - WP33;
U133=DV133 + WP23;
U213=DV123 + WP33;
U223=DV113 + DV333;
U233=DV233 - WP13;
U313=DV133 - WP23;
U323=DV233 + WP13;
VSP13=d3.*U112 + VP12;
VSP23=d3.*U212 + VP22;
VSP33=d3.*U312;
VP13=C3.*VSP13 + S3.*VSP23;
VP23=-(S3.*VSP13) + C3.*VSP23;
F13=MX3.*U113 + MY3R.*U123;
F23=MX3.*U213 + MY3R.*U223;
F33=MX3.*U313 + MY3R.*U323;
PIS23=XX3R - ZZ3R;
No13=WP13.*XX3R - U313.*XY3 + U213.*XZ3 + (-DV223 + DV333).*YZ3 + DV233.*ZZ3R;
No23=DV133.*PIS23 + U323.*XY3 + (DV113 - DV333).*XZ3 - U123.*YZ3;
No33=-(DV123.*XX3R) + (-DV113 + DV223).*XY3 - U233.*XZ3 + U133.*YZ3 + WP33.*ZZ3R;
WI14=S4.*W33 + C4.*WI13;
WI24=C4.*W33 - S4.*WI13;
W34=QP4 - WI23;
WP14=QP4.*WI24 + C4.*WP13 + S4.*WP33;
WP24=-(QP4.*WI14) - S4.*WP13 + C4.*WP33;
WP34=QDP4 - WP23;
DV114=-WI14.^2;
DV224=-WI24.^2;
DV334=-W34.^2;
DV124=WI14.*WI24;
DV134=W34.*WI14;
DV234=W34.*WI24;
U114=DV224 + DV334;
U124=DV124 - WP34;
U134=DV134 + WP24;
U214=DV124 + WP34;
U224=DV114 + DV334;
U234=DV234 - WP14;
U314=DV134 - WP24;
U324=DV234 + WP14;
VSP14=-(R4.*U123) + VP13;
VSP24=-(R4.*U223) + VP23;
VSP34=-(R4.*U323) + VSP33;
VP14=C4.*VSP14 + S4.*VSP34;
VP24=-(S4.*VSP14) + C4.*VSP34;
F14=MX4.*U114 + MY4R.*U124;
F24=MX4.*U214 + MY4R.*U224;
F34=MX4.*U314 + MY4R.*U324;
PIS24=XX4R - ZZ4R;
No14=WP14.*XX4R - U314.*XY4 + U214.*XZ4 + (-DV224 + DV334).*YZ4 + DV234.*ZZ4R;
No24=DV134.*PIS24 + U324.*XY4 + (DV114 - DV334).*XZ4 - U124.*YZ4;
No34=-(DV124.*XX4R) + (-DV114 + DV224).*XY4 - U234.*XZ4 + U134.*YZ4 + WP34.*ZZ4R;
WI15=-(S5.*W34) + C5.*WI14;
WI25=-(C5.*W34) - S5.*WI14;
W35=QP5 + WI24;
WP15=QP5.*WI25 + C5.*WP14 - S5.*WP34;
WP25=-(QP5.*WI15) - S5.*WP14 - C5.*WP34;
WP35=QDP5 + WP24;
DV115=-WI15.^2;
DV225=-WI25.^2;
DV335=-W35.^2;
DV125=WI15.*WI25;
DV135=W35.*WI15;
DV235=W35.*WI25;
U115=DV225 + DV335;
U125=DV125 - WP35;
U135=DV135 + WP25;
U215=DV125 + WP35;
U225=DV115 + DV335;
U235=DV235 - WP15;
U315=DV135 - WP25;
U325=DV235 + WP15;
VP15=C5.*VP14 + S5.*VSP24;
VP25=-(S5.*VP14) + C5.*VSP24;
F15=MX5.*U115 + MY5R.*U125;
F25=MX5.*U215 + MY5R.*U225;
F35=MX5.*U315 + MY5R.*U325;
PIS25=XX5R - ZZ5R;
No15=WP15.*XX5R - U315.*XY5 + U215.*XZ5 + (-DV225 + DV335).*YZ5 + DV235.*ZZ5R;
No25=DV135.*PIS25 + U325.*XY5 + (DV115 - DV335).*XZ5 - U125.*YZ5;
No35=-(DV125.*XX5R) + (-DV115 + DV225).*XY5 - U235.*XZ5 + U135.*YZ5 + WP35.*ZZ5R;
WI16=S6.*W35 + C6.*WI15;
WI26=C6.*W35 - S6.*WI15;
W36=QP6 - WI25;
WP16=QP6.*WI26 + C6.*WP15 + S6.*WP35;
WP26=-(QP6.*WI16) - S6.*WP15 + C6.*WP35;
WP36=QDP6 - WP25;
DV116=-WI16.^2;
DV226=-WI26.^2;
DV336=-W36.^2;
DV126=WI16.*WI26;
DV136=W36.*WI16;
DV236=W36.*WI26;
U116=DV226 + DV336;
U126=DV126 - WP36;
U136=DV136 + WP26;
U216=DV126 + WP36;
U226=DV116 + DV336;
U236=DV236 - WP16;
U316=DV136 - WP26;
U326=DV236 + WP16;
VP16=C6.*VP15 + S6.*VP24;
VP26=-(S6.*VP15) + C6.*VP24;
F16=MX6.*U116 + MY6.*U126;
F26=MX6.*U216 + MY6.*U226;
F36=MX6.*U316 + MY6.*U326;
PIS26=XX6R - ZZ6;
No16=WP16.*XX6R - U316.*XY6 + U216.*XZ6 + (-DV226 + DV336).*YZ6 + DV236.*ZZ6;
No26=DV136.*PIS26 + U326.*XY6 + (DV116 - DV336).*XZ6 - U126.*YZ6;
No36=-(DV126.*XX6R) + (-DV116 + DV226).*XY6 - U236.*XZ6 + U136.*YZ6 + WP36.*ZZ6;
E16=F16 + FX6;
E26=F26 + FY6;
E36=F36 + FZ6;
N16=CX6 + No16 - MY6.*VP25;
N26=CY6 + No26 + MX6.*VP25;
N36=CZ6 + No36 - MY6.*VP16 + MX6.*VP26;
FDI16=C6.*E16 - E26.*S6;
FDI36=C6.*E26 + E16.*S6;
E15=F15 + FDI16 + FX5;
E25=-E36 + F25 + FY5;
E35=F35 + FDI36 + FZ5;
N15=CX5 + C6.*N16 + No15 - N26.*S6 + MY5R.*VP24;
N25=CY5 - N36 + No25 - MX5.*VP24;
N35=CZ5 + C6.*N26 + No35 + N16.*S6 - MY5R.*VP15 + MX5.*VP25;
FDI15=C5.*E15 - E25.*S5;
FDI35=-(C5.*E25) - E15.*S5;
E14=F14 + FDI15 + FX4;
E24=E35 + F24 + FY4;
E34=F34 + FDI35 + FZ4;
N14=CX4 + C5.*N15 + No14 - N25.*S5 - MY4R.*VSP24;
N24=CY4 + N35 + No24 + MX4.*VSP24;
N34=CZ4 - C5.*N25 + No34 - N15.*S5 - MY4R.*VP14 + MX4.*VP24;
 FDI14=C4.*E14 - E24.*S4;
FDI34=C4.*E24 + E14.*S4;
E13=F13 + FDI14 + FX3;
E23=-E34 + F23 + FY3;
E33=F33 + FDI34 + FZ3;
N13=CX3 + C4.*N14 + No13 - FDI34.*R4 - N24.*S4 + MY3R.*VSP33;
N23=CY3 - N34 + No23 - MX3.*VSP33;
N33=CZ3 + C4.*N24 + No33 + FDI14.*R4 + N14.*S4 - MY3R.*VP13 + MX3.*VP23;
FDI23=C3.*E23 + E13.*S3;
N12=CX2 + C3.*N13 + No12 - N23.*S3;
N22=CY2 - d3.*E33 + C3.*N23 + No22 + N13.*S3;
N32=CZ2 + d3.*FDI23 + N33 + No32 - MY2.*VP12 + MX2R.*VP22;
N31=CZ1 - C2.*N22 + No31 - N12.*S2;
GAM1=N31 + FV1.*QP1 + FS1.*sign(QP1);
GAM2=N32 + FV2.*QP2 + FS2.*sign(QP2);
GAM3=N33 + IA3.*QDP3 + FV3.*QP3 + FS3.*sign(QP3);
GAM4=N34 + FV4.*QP4 + FS4.*sign(QP4);
GAM5=N35 + FV5.*QP5 + FS5.*sign(QP5);
GAM6=N36 + FV6.*QP6 + FS6.*sign(QP6);

GAM=[GAM1; GAM2; GAM3; GAM4; GAM5; GAM6;];

7.2. Modelos dinámico directo

% (********************************************)
% (** SYMORO+ : SYmbolic MOdelling of RObots **)
% (**========================================**)
% (** IRCCyN-ECN - 1, rue de la Noe **)
% (** B.P.92101 **)
% (** 44321 Nantes cedex 3, FRANCE **)
% (** www.irccyn.ec-nantes.fr **)
% (********************************************)

% Name of file : D:\Maestria\Sistemas Robo'ticos en Me'dicina\Archivo Ejemplo

Symoro\RobotCompleto\RobotCompletobaseI.parbase.ddm
% Geometric parameters
% j ant mu sigma gamma b alpha d theta r
% 1 0 1 0 0 0 0 0 q1 0
% -Pi
% 2 1 1 0 0 0 --- 0 q2 0
% 2
% 3 2 1 0 0 0 0 d3 q3 0
% Pi
% 4 3 0 0 0 0 -- 0 q4 R4
% 2
% -Pi
% 5 4 0 0 0 0 --- 0 q5 0
% 2
% Pi
% 6 5 0 0 0 0 -- 0 q6 0
% 2
 % Inertial parameters
% j XX XY XZ YY YZ ZZ MX MY MZ M Ia
% 1 0 0 0 0 0 ZZ1R 0 0 0 0 0
% 2 XX2R XY2 XZ2R 0 YZ2 ZZ2R MX2R MY2 0 0 0
% 3 XX3R XY3 XZ3 0 YZ3 ZZ3R MX3 MY3R 0 0 IA3
% 4 XX4R XY4 XZ4 0 YZ4 ZZ4R MX4 MY4R 0 0 0
% 5 XX5R XY5 XZ5 0 YZ5 ZZ5R MX5 MY5R 0 0 0
% 6 XX6R XY6 XZ6 0 YZ6 ZZ6 MX6 MY6 0 0 0
% External forces,friction parameters, joint velocities and accelerations

%j FX FY FZ CX CY CZ FS FV QP QDP

%1 FX1 FY1 FZ1 CX1 CY1 CZ1 FS1 FV1 QP1 QDP1

%2 FX2 FY2 FZ2 CX2 CY2 CZ2 FS2 FV2 QP2 QDP2

%3 FX3 FY3 FZ3 CX3 CY3 CZ3 FS3 FV3 QP3 QDP3

%4 FX4 FY4 FZ4 CX4 CY4 CZ4 FS4 FV4 QP4 QDP4

%5 FX5 FY5 FZ5 CX5 CY5 CZ5 FS5 FV5 QP5 QDP5

%6 FX6 FY6 FZ6 CX6 CY6 CZ6 FS6 FV6 QP6 QDP6

% Base velocity, base accelerations, and gravity

%j W0 WP0 V0 VP0 G

%1 0 0 0 0 0

%2 0 0 0 0 0

%3 0 0 0 0 G3

% Direct Calculation of the Joints Accelerations

% Equations:

% Declaration of the function

function
QDP=MDD(GAM1,GAM2,GAM3,GAM4,GAM5,GAM6,q1,q2,q3,q4,q5,q6,QP1,QP2,QP3,QP4,QP5,QP6)
% Declaration of global input variables
global d3 R4 XX2R XY2 XZ2R YZ2 ZZ2R XX3R XY3
global XZ3 YZ3 ZZ3R MX3 MY3R XX4R XY4 XZ4 YZ4 ZZ4R
global MX4 MY4R XX5R XY5 XZ5 YZ5 ZZ5R MX5 MY5R XX6R
global XY6 XZ6 YZ6 ZZ6 MX6 MY6 CZ6 FV6 FS6
global CX6 CY6 FX6 FY6 FZ6 CX5 CY5 CZ5 FX5 FY5
global FZ5 FV5 FS5 CX4 CY4 CZ4 FX4 FY4 FZ4
global FV4 FS4 CX3 CY3 CZ3 FX3 FY3 FZ3 IA3
global FV3 FS3 MY2 MX2R CX2 CY2 CZ2 FV2
global FS2 ZZ1R CZ1 FV1 FS1 G3
% Function description:

S2=sin(q2);
C2=cos(q2);
S3=sin(q3);
C3=cos(q3);
S4=sin(q4);
C4=cos(q4);
S5=sin(q5);
C5=cos(q5);
S6=sin(q6);
C6=cos(q6);
WI12=-(QP1.*S2);
WI22=-(C2.*QP1);
WI13=C3.*WI12 + S3.*WI22;
WI23=-(S3.*WI12) + C3.*WI22;
W33=QP2 + QP3;
JPR133=d3.*S3;
JPR233=C3.*d3;
WI14=S4.*W33 + C4.*WI13;
WI24=C4.*W33 - S4.*WI13;
W34=QP4 - WI23;
JPR114=-(R4.*S4);
JPR134=C4.*R4;
JPR214=-(C4.*R4);
JPR234=-(R4.*S4);
WI15=-(S5.*W34) + C5.*WI14;
WI25=-(C5.*W34) - S5.*WI14;
W35=QP5 + WI24;
WI16=S6.*W35 + C6.*WI15;
WI26=C6.*W35 - S6.*WI15;
W36=QP6 - WI25;
JW12=WI12.*XX2R + WI22.*XY2 + QP2.*XZ2R;
JW22=WI12.*XY2 + QP2.*YZ2;
JW32=WI12.*XZ2R + WI22.*YZ2 + QP2.*ZZ2R;
KW12=-(JW22.*QP2) + JW32.*WI22;
KW22=JW12.*QP2 - JW32.*WI12;
KW32=JW22.*WI12 - JW12.*WI22;
WQ12=QP2.*WI22;
WQ22=-(QP2.*WI12);
JW13=WI13.*XX3R + WI23.*XY3 + W33.*XZ3;
JW23=WI13.*XY3 + W33.*YZ3;
JW33=WI13.*XZ3 + WI23.*YZ3 + W33.*ZZ3R;
KW13=-(JW23.*W33) + JW33.*WI23;
KW23=JW13.*W33 - JW33.*WI13;
KW33=JW23.*WI13 - JW13.*WI23;
SW13=-(MX3.*W33.^2) + WI23.*(MY3R.*WI13 - MX3.*WI23);
SW23=-(MY3R.*W33.^2) - WI13.*(MY3R.*WI13 - MX3.*WI23);
SW33=MX3.*W33.*WI13 + MY3R.*W33.*WI23;
WQ13=QP3.*WI23;
WQ23=-(QP3.*WI13);
LW13=d3.*S3.*WI12.*WI22 + C3.*(-(d3.*QP2.^2) - d3.*WI22.^2);
LW23=C3.*d3.*WI12.*WI22 - S3.*(-(d3.*QP2.^2) - d3.*WI22.^2);
LW33=d3.*QP2.*WI12;
JW14=WI14.*XX4R + WI24.*XY4 + W34.*XZ4;
JW24=WI14.*XY4 + W34.*YZ4;
JW34=WI14.*XZ4 + WI24.*YZ4 + W34.*ZZ4R;
KW14=-(JW24.*W34) + JW34.*WI24;
KW24=JW14.*W34 - JW34.*WI14;
KW34=JW24.*WI14 - JW14.*WI24;
SW14=-(MX4.*W34.^2) + WI24.*(MY4R.*WI14 - MX4.*WI24);
SW24=-(MY4R.*W34.^2) - WI14.*(MY4R.*WI14 - MX4.*WI24);
SW34=MX4.*W34.*WI14 + MY4R.*W34.*WI24;
WQ14=QP4.*WI24;
WQ24=-(QP4.*WI14);
LW14=-(R4.*S4.*W33.*WI23) - C4.*R4.*WI13.*WI23;
LW24=-(C4.*R4.*W33.*WI23) + R4.*S4.*WI13.*WI23;
LW34=-(R4.*W33.^2) - R4.*WI13.^2;
JW15=WI15.*XX5R + WI25.*XY5 + W35.*XZ5;
JW25=WI15.*XY5 + W35.*YZ5;
JW35=WI15.*XZ5 + WI25.*YZ5 + W35.*ZZ5R;
KW15=-(JW25.*W35) + JW35.*WI25;
KW25=JW15.*W35 - JW35.*WI15;
KW35=JW25.*WI15 - JW15.*WI25;
SW15=-(MX5.*W35.^2) + WI25.*(MY5R.*WI15 - MX5.*WI25);
SW25=-(MY5R.*W35.^2) - WI15.*(MY5R.*WI15 - MX5.*WI25);
SW35=MX5.*W35.*WI15 + MY5R.*W35.*WI25;
WQ15=QP5.*WI25;
WQ25=-(QP5.*WI15);
JW16=WI16.*XX6R + WI26.*XY6 + W36.*XZ6;
JW26=WI16.*XY6 + W36.*YZ6;
JW36=WI16.*XZ6 + WI26.*YZ6 + W36.*ZZ6;
KW16=-(JW26.*W36) + JW36.*WI26;
KW26=JW16.*W36 - JW36.*WI16;
KW36=JW26.*WI16 - JW16.*WI26;
SW16=-(MX6.*W36.^2) + WI26.*(MY6.*WI16 - MX6.*WI26);
SW26=-(MY6.*W36.^2) - WI16.*(MY6.*WI16 - MX6.*WI26);
SW36=MX6.*W36.*WI16 + MY6.*W36.*WI26;
WQ16=QP6.*WI26;
WQ26=-(QP6.*WI16);
JD6=1./ZZ6;
JU16=JD6.*XZ6;
JU26=JD6.*YZ6;
JU36=JD6.*ZZ6;
JU46=-(JD6.*MY6);
JU56=JD6.*MX6;
GW6=-CZ6 + GAM6 - KW36 - FV6.*QP6 - FS6.*sign(QP6);
GK116=XX6R - JU16.*XZ6;
GK126=XY6 - JU16.*YZ6;
GK136=XZ6 - JU16.*ZZ6;
GK216=XY6 - JU26.*XZ6;
GK226=-(JU26.*YZ6);
GK236=YZ6 - JU26.*ZZ6;
GK316=XZ6 - JU36.*XZ6;
GK326=YZ6 - JU36.*YZ6;
GK336=ZZ6 - JU36.*ZZ6;
GK416=-(JU46.*XZ6);
GK426=-(JU46.*YZ6);
GK436=-MY6 - JU46.*ZZ6;
GK446=JU46.*MY6;
GK456=-(JU46.*MX6);
GK516=-(JU56.*XZ6);
GK526=-(JU56.*YZ6);
GK536=MX6 - JU56.*ZZ6;
GK546=JU56.*MY6;
GK556=-(JU56.*MX6);
NG16=GK116.*WQ16 + GK126.*WQ26;
NG26=GK216.*WQ16 + GK226.*WQ26;
NG36=GK316.*WQ16 + GK326.*WQ26;
NG46=GK416.*WQ16 + GK426.*WQ26;
NG56=GK516.*WQ16 + GK526.*WQ26;
NG66=MY6.*WQ16 - MX6.*WQ26;
VS16=GW6.*JU16 + NG16;
VS26=GW6.*JU26 + NG26;
VS36=GW6.*JU36 + NG36;
VS46=GW6.*JU46 + NG46;
VS56=GW6.*JU56 + NG56;
AP16=CX6 + KW16 + VS16;
AP26=CY6 + KW26 + VS26;
AP36=CZ6 + KW36 + VS36;
AP46=FX6 + SW16 + VS46;
AP56=FY6 + SW26 + VS56;
AP66=FZ6 + NG66 + SW36;
GX116=C6.*GK116 - GK216.*S6;
GX126=C6.*GK126 - GK226.*S6;
GX316=C6.*GK216 + GK116.*S6;
GX326=C6.*GK226 + GK126.*S6;
GX336=C6.*GK236 + GK136.*S6;
GX416=C6.*GK416 - GK516.*S6;
GX426=C6.*GK426 - GK526.*S6;
GX436=C6.*GK436 - GK536.*S6;
GX446=C6.*GK446 - GK546.*S6;
GX456=C6.*GK456 - GK556.*S6;
GX616=C6.*GK516 + GK416.*S6;
GX626=C6.*GK526 + GK426.*S6;
GX636=C6.*GK536 + GK436.*S6;
GX646=C6.*GK546 + GK446.*S6;
GX656=C6.*GK556 + GK456.*S6;
TKT116=C6.*GX116 - GX126.*S6;
TKT216=-(C6.*GK316) + GK326.*S6;
TKT316=C6.*GX316 - GX326.*S6;
TKT416=C6.*GX416 - GX426.*S6;
TKT516=-(C6.*MY6) - MX6.*S6;
TKT616=C6.*GX616 - GX626.*S6;
TKT336=C6.*GX326 + GX316.*S6;
TKT436=C6.*GX426 + GX416.*S6;
TKT536=C6.*MX6 - MY6.*S6;
TKT636=C6.*GX626 + GX616.*S6;
TKT446=C6.*GX446 - GX456.*S6;
TKT646=C6.*GX646 - GX656.*S6;
TKT666=C6.*GX656 + GX646.*S6;
MJE115=TKT116 + XX5R;
MJE215=TKT216 + XY5;
MJE315=TKT316 + XZ5;
MJE615=MY5R + TKT616;
MJE325=-GX336 + YZ5;
MJE625=-GX636 - MX5;
MJE335=TKT336 + ZZ5R;
MJE435=-MY5R + TKT436;
MJE535=MX5 + TKT536;
VBE15=-(AP16.*C6) - CX5 - KW15 + AP26.*S6;
VBE25=AP36 - CY5 - KW25;
VBE35=-(AP26.*C6) - CZ5 - KW35 - AP16.*S6;
VBE45=-(AP46.*C6) - FX5 + AP56.*S6 - SW15;
VBE55=AP66 - FY5 - SW25;
VBE65=-(AP56.*C6) - FZ5 - AP46.*S6 - SW35;
JD5=1./MJE335;
JU15=JD5.*MJE315;
JU25=JD5.*MJE325;
JU35=JD5.*MJE335;
JU45=JD5.*MJE435;
JU55=JD5.*MJE535;
JU65=JD5.*TKT636;
GW5=GAM5 - FV5.*QP5 + VBE35 - FS5.*sign(QP5);
GK115=MJE115 - JU15.*MJE315;
GK125=MJE215 - JU15.*MJE325;
GK135=MJE315 - JU15.*MJE335;
GK215=MJE215 - JU25.*MJE315;
GK225=GK336 - JU25.*MJE325;
GK235=MJE325 - JU25.*MJE335;
GK315=MJE315 - JU35.*MJE315;
GK325=MJE325 - JU35.*MJE325;
GK335=MJE335 - JU35.*MJE335;
GK415=-(JU45.*MJE315) + TKT416;
GK425=-GX436 - JU45.*MJE325;
GK435=-(JU45.*MJE335) + MJE435;
GK445=-(JU45.*MJE435) + TKT446;
GK455=-(JU45.*MJE535);
GK465=-(JU45.*TKT636) + TKT646;
GK515=-(JU55.*MJE315) + TKT516;
GK525=-(JU55.*MJE325);
GK535=-(JU55.*MJE335) + MJE535;
GK545=-(JU55.*MJE435);
GK555=-(JU55.*MJE535);
GK565=-(JU55.*TKT636);
GK615=-(JU65.*MJE315) + MJE615;
GK625=-(JU65.*MJE325) + MJE625;
GK635=-(JU65.*MJE335) + TKT636;
GK645=-(JU65.*MJE435) + TKT646;
GK655=-(JU65.*MJE535);
GK665=-(JU65.*TKT636) + TKT666;
NG15=GK115.*WQ15 + GK125.*WQ25;
NG25=GK215.*WQ15 + GK225.*WQ25;
NG35=GK315.*WQ15 + GK325.*WQ25;
NG45=GK415.*WQ15 + GK425.*WQ25;
NG55=GK515.*WQ15 + GK525.*WQ25;
NG65=GK615.*WQ15 + GK625.*WQ25;
VS15=GW5.*JU15 + NG15;
VS25=GW5.*JU25 + NG25;
VS35=GW5.*JU35 + NG35;
VS45=GW5.*JU45 + NG45;
VS55=GW5.*JU55 + NG55;
VS65=GW5.*JU65 + NG65;
AP15=-VBE15 + VS15;
AP25=-VBE25 + VS25;
AP35=-VBE35 + VS35;
AP45=-VBE45 + VS45;
AP55=-VBE55 + VS55;
AP65=-VBE65 + VS65;
GX115=C5.*GK115 - GK215.*S5;
GX125=C5.*GK125 - GK225.*S5;
GX315=-(C5.*GK215) - GK115.*S5;
GX325=-(C5.*GK225) - GK125.*S5;
GX335=-(C5.*GK235) - GK135.*S5;
GX415=C5.*GK415 - GK515.*S5;
GX425=C5.*GK425 - GK525.*S5;
GX435=C5.*GK435 - GK535.*S5;
GX445=C5.*GK445 - GK545.*S5;
GX455=C5.*GK455 - GK555.*S5;
GX615=-(C5.*GK515) - GK415.*S5;
GX625=-(C5.*GK525) - GK425.*S5;
GX635=-(C5.*GK535) - GK435.*S5;
GX645=-(C5.*GK545) - GK445.*S5;
GX655=-(C5.*GK555) - GK455.*S5;
GX665=-(C5.*GK565) - GK465.*S5;
TKT115=C5.*GX115 - GX125.*S5;
TKT215=C5.*GK315 - GK325.*S5;
TKT315=C5.*GX315 - GX325.*S5;
TKT415=C5.*GX415 - GX425.*S5;
TKT515=C5.*GK615 - GK625.*S5;
TKT615=C5.*GX615 - GX625.*S5;
TKT335=-(C5.*GX325) - GX315.*S5;
TKT435=-(C5.*GX425) - GX415.*S5;
TKT535=-(C5.*GK625) - GK615.*S5;
TKT635=-(C5.*GX625) - GX615.*S5;
TKT445=C5.*GX445 - GX455.*S5;
TKT545=C5.*GK645 - GK655.*S5;
TKT645=C5.*GX645 - GX655.*S5;
TKT665=-(C5.*GX655) - GX645.*S5;
MJE114=TKT115 + XX4R;
MJE214=TKT215 + XY4;
MJE314=TKT315 + XZ4;
MJE614=MY4R + TKT615;
MJE324=GX335 + YZ4;
MJE624=GX635 - MX4;
MJE334=TKT335 + ZZ4R;
MJE434=-MY4R + TKT435;
MJE534=MX4 + TKT535;
VBE14=-(AP15.*C5) - CX4 - KW14 + AP25.*S5;
VBE24=-AP35 - CY4 - KW24;
VBE34=AP25.*C5 - CZ4 - KW34 + AP15.*S5;
VBE44=-(AP45.*C5) - FX4 + AP55.*S5 - SW14;
VBE54=-AP65 - FY4 - SW24;
VBE64=AP55.*C5 - FZ4 + AP45.*S5 - SW34;
JD4=1./MJE334;
JU14=JD4.*MJE314;
JU24=JD4.*MJE324;
JU34=JD4.*MJE334;
JU44=JD4.*MJE434;
JU54=JD4.*MJE534;
JU64=JD4.*TKT635;
GW4=GAM4 - FV4.*QP4 + VBE34 - FS4.*sign(QP4);
GK114=MJE114 - JU14.*MJE314;
GK124=MJE214 - JU14.*MJE324;
GK134=MJE314 - JU14.*MJE334;
GK144=-(JU14.*MJE434) + TKT415;
GK154=-(JU14.*MJE534) + TKT515;
GK164=MJE614 - JU14.*TKT635;
GK214=MJE214 - JU24.*MJE314;
GK224=GK335 - JU24.*MJE324;
GK234=MJE324 - JU24.*MJE334;
GK244=GX435 - JU24.*MJE434;
GK254=GK635 - JU24.*MJE534;
GK264=MJE624 - JU24.*TKT635;
GK314=MJE314 - JU34.*MJE314;
GK324=MJE324 - JU34.*MJE324;
GK334=MJE334 - JU34.*MJE334;
GK344=MJE434 - JU34.*MJE434;
GK354=MJE534 - JU34.*MJE534;
GK364=TKT635 - JU34.*TKT635;
GK414=-(JU44.*MJE314) + TKT415;
GK424=GX435 - JU44.*MJE324;
GK434=-(JU44.*MJE334) + MJE434;
GK444=-(JU44.*MJE434) + TKT445;
GK454=-(JU44.*MJE534) + TKT545;
GK464=-(JU44.*TKT635) + TKT645;
GK514=-(JU54.*MJE314) + TKT515;
GK524=GK635 - JU54.*MJE324;
GK534=-(JU54.*MJE334) + MJE534;
GK544=-(JU54.*MJE434) + TKT545;
GK554=GK665 - JU54.*MJE534;
GK564=GX665 - JU54.*TKT635;
GK614=-(JU64.*MJE314) + MJE614;
GK624=-(JU64.*MJE324) + MJE624;
GK634=-(JU64.*MJE334) + TKT635;
GK644=-(JU64.*MJE434) + TKT645;
GK654=GX665 - JU64.*MJE534;
GK664=-(JU64.*TKT635) + TKT665;
NG14=GK144.*LW14 + GK154.*LW24 + GK164.*LW34 + GK114.*WQ14 + GK124.*WQ24;
NG24=GK244.*LW14 + GK254.*LW24 + GK264.*LW34 + GK214.*WQ14 + GK224.*WQ24;
NG34=GK344.*LW14 + GK354.*LW24 + GK364.*LW34 + GK314.*WQ14 + GK324.*WQ24;
NG44=GK444.*LW14 + GK454.*LW24 + GK464.*LW34 + GK414.*WQ14 + GK424.*WQ24;
NG54=GK544.*LW14 + GK554.*LW24 + GK564.*LW34 + GK514.*WQ14 + GK524.*WQ24;
NG64=GK644.*LW14 + GK654.*LW24 + GK664.*LW34 + GK614.*WQ14 + GK624.*WQ24;
VS14=GW4.*JU14 + NG14;
VS24=GW4.*JU24 + NG24;
VS34=GW4.*JU34 + NG34;
VS44=GW4.*JU44 + NG44;
VS54=GW4.*JU54 + NG54;
VS64=GW4.*JU64 + NG64;
AP14=-VBE14 + VS14;
AP24=-VBE24 + VS24;
AP34=-VBE34 + VS34;
AP44=-VBE44 + VS44;
AP54=-VBE54 + VS54;
AP64=-VBE64 + VS64;
GX114=C4.*GK114 + GK414.*JPR114 + GK514.*JPR214 - GK214.*S4;
GX124=C4.*GK124 + GK424.*JPR114 + GK524.*JPR214 - GK224.*S4;
GX144=C4.*GK144 + GK444.*JPR114 + GK544.*JPR214 - GK244.*S4;
GX154=C4.*GK154 + GK454.*JPR114 + GK554.*JPR214 - GK254.*S4;
GX314=C4.*GK214 + GK414.*JPR134 + GK514.*JPR234 + GK114.*S4;
GX324=C4.*GK224 + GK424.*JPR134 + GK524.*JPR234 + GK124.*S4;
GX334=C4.*GK234 + GK434.*JPR134 + GK534.*JPR234 + GK134.*S4;
GX344=C4.*GK244 + GK444.*JPR134 + GK544.*JPR234 + GK144.*S4;
GX354=C4.*GK254 + GK454.*JPR134 + GK554.*JPR234 + GK154.*S4;
GX414=C4.*GK414 - GK514.*S4;
GX424=C4.*GK424 - GK524.*S4;
GX434=C4.*GK434 - GK534.*S4;
GX444=C4.*GK444 - GK544.*S4;
GX454=C4.*GK454 - GK554.*S4;
GX614=C4.*GK514 + GK414.*S4;
GX624=C4.*GK524 + GK424.*S4;
GX634=C4.*GK534 + GK434.*S4;
GX644=C4.*GK544 + GK444.*S4;
GX654=C4.*GK554 + GK454.*S4;
GX664=C4.*GK564 + GK464.*S4;
TKT114=C4.*GX114 + GX144.*JPR114 + GX154.*JPR214 - GX124.*S4;
TKT214=-(C4.*GK314) - GK344.*JPR114 - GK354.*JPR214 + GK324.*S4;
TKT314=C4.*GX314 + GX344.*JPR114 + GX354.*JPR214 - GX324.*S4;
TKT414=C4.*GX414 + GX444.*JPR114 + GX454.*JPR214 - GX424.*S4;
TKT514=-(C4.*GK614) - GK644.*JPR114 - GK654.*JPR214 + GK624.*S4;
TKT614=C4.*GX614 + GX644.*JPR114 + GX654.*JPR214 - GX624.*S4;
TKT334=C4.*GX324 + GX344.*JPR134 + GX354.*JPR234 + GX314.*S4;
TKT434=C4.*GX424 + GX444.*JPR134 + GX454.*JPR234 + GX414.*S4;
TKT534=-(C4.*GK624) - GK644.*JPR134 - GK654.*JPR234 - GK614.*S4;
TKT634=C4.*GX624 + GX644.*JPR134 + GX654.*JPR234 + GX614.*S4;
TKT444=C4.*GX444 - GX454.*S4;
TKT544=-(C4.*GK644) + GK654.*S4;
TKT644=C4.*GX644 - GX654.*S4;
TKT664=C4.*GX654 + GX644.*S4;
MJE113=TKT114 + XX3R;
MJE213=TKT214 + XY3;
MJE313=TKT314 + XZ3;
MJE613=MY3R + TKT614;
MJE323=-GX334 + YZ3;
MJE623=-GX634 - MX3;
MJE333=TKT334 + ZZ3R;
MJE433=-MY3R + TKT434;
MJE533=MX3 + TKT534;
VBE13=-(AP14.*C4) - CX3 - AP44.*JPR114 - AP54.*JPR214 - KW13 + AP24.*S4;
VBE23=AP34 - CY3 - KW23;
VBE33=-(AP24.*C4) - CZ3 - AP44.*JPR134 - AP54.*JPR234 - KW33 - AP14.*S4;
VBE43=-(AP44.*C4) - FX3 + AP54.*S4 - SW13;
VBE53=AP64 - FY3 - SW23;
VBE63=-(AP54.*C4) - FZ3 - AP44.*S4 - SW33;
JD3=1./(IA3 + MJE333);
JU13=JD3.*MJE313;
JU23=JD3.*MJE323;
JU33=JD3.*MJE333;
JU43=JD3.*MJE433;
JU53=JD3.*MJE533;
JU63=JD3.*TKT634;
GW3=GAM3 - FV3.*QP3 + VBE33 - FS3.*sign(QP3);
GK113=MJE113 - JU13.*MJE313;
GK123=MJE213 - JU13.*MJE323;
GK143=-(JU13.*MJE433) + TKT414;
GK153=-(JU13.*MJE533) + TKT514;
GK163=MJE613 - JU13.*TKT634;
GK213=MJE213 - JU23.*MJE313;
GK223=GK334 - JU23.*MJE323;
GK243=-GX434 - JU23.*MJE433;
GK253=GK634 - JU23.*MJE533;
GK263=MJE623 - JU23.*TKT634;
GK313=MJE313 - JU33.*MJE313;
GK323=MJE323 - JU33.*MJE323;
GK333=MJE333 - JU33.*MJE333;
GK343=MJE433 - JU33.*MJE433;
GK353=MJE533 - JU33.*MJE533;
GK363=TKT634 - JU33.*TKT634;
GK413=-(JU43.*MJE313) + TKT414;
GK423=-GX434 - JU43.*MJE323;
GK433=-(JU43.*MJE333) + MJE433;
GK443=-(JU43.*MJE433) + TKT444;
GK453=-(JU43.*MJE533) + TKT544;
GK463=-(JU43.*TKT634) + TKT644;
GK513=-(JU53.*MJE313) + TKT514;
GK523=GK634 - JU53.*MJE323;
GK533=-(JU53.*MJE333) + MJE533;
GK543=-(JU53.*MJE433) + TKT544;
GK553=GK664 - JU53.*MJE533;
GK563=-GX664 - JU53.*TKT634;
GK613=-(JU63.*MJE313) + MJE613;
GK623=-(JU63.*MJE323) + MJE623;
GK643=-(JU63.*MJE433) + TKT644;
GK653=-GX664 - JU63.*MJE533;
GK663=-(JU63.*TKT634) + TKT664;
NG13=GK143.*LW13 + GK153.*LW23 + GK163.*LW33 + GK113.*WQ13 + GK123.*WQ23;
NG23=GK243.*LW13 + GK253.*LW23 + GK263.*LW33 + GK213.*WQ13 + GK223.*WQ23;
NG33=GK343.*LW13 + GK353.*LW23 + GK363.*LW33 + GK313.*WQ13 + GK323.*WQ23;
NG43=GK443.*LW13 + GK453.*LW23 + GK463.*LW33 + GK413.*WQ13 + GK423.*WQ23;
NG53=GK543.*LW13 + GK553.*LW23 + GK563.*LW33 + GK513.*WQ13 + GK523.*WQ23;
NG63=GK643.*LW13 + GK653.*LW23 + GK663.*LW33 + GK613.*WQ13 + GK623.*WQ23;
VS13=GW3.*JU13 + NG13;
VS23=GW3.*JU23 + NG23;
VS33=GW3.*JU33 + NG33;
VS43=GW3.*JU43 + NG43;
VS53=GW3.*JU53 + NG53;
VS63=GW3.*JU63 + NG63;
AP13=-VBE13 + VS13;
AP23=-VBE23 + VS23;
AP33=-VBE33 + VS33;
AP43=-VBE43 + VS43;
AP53=-VBE53 + VS53;
AP63=-VBE63 + VS63;
GX113=C3.*GK113 - GK213.*S3;
GX123=C3.*GK123 - GK223.*S3;
GX213=C3.*GK213 - d3.*GK613 + GK113.*S3;
GX223=C3.*GK223 - d3.*GK623 + GK123.*S3;
GX263=C3.*GK263 - d3.*GK663 + GK163.*S3;
GX313=GK313 + GK413.*JPR133 + GK513.*JPR233;
GX323=GK323 + GK423.*JPR133 + GK523.*JPR233;
GX333=GK333 + GK433.*JPR133 + GK533.*JPR233;
GX343=GK343 + GK443.*JPR133 + GK543.*JPR233;
GX353=GK353 + GK453.*JPR133 + GK553.*JPR233;
GX363=GK363 + GK463.*JPR133 + GK563.*JPR233;
GX413=C3.*GK413 - GK513.*S3;
GX423=C3.*GK423 - GK523.*S3;
GX433=C3.*GK433 - GK533.*S3;
GX443=C3.*GK443 - GK543.*S3;
GX453=C3.*GK453 - GK553.*S3;
GX463=C3.*GK463 - GK563.*S3;
GX513=C3.*GK513 + GK413.*S3;
GX523=C3.*GK523 + GK423.*S3;
GX533=C3.*GK533 + GK433.*S3;
GX543=C3.*GK543 + GK443.*S3;
GX553=C3.*GK553 + GK453.*S3;
GX563=C3.*GK563 + GK463.*S3;
TKT113=C3.*GX113 - GX123.*S3;
TKT213=C3.*GX213 - GX223.*S3;
TKT313=C3.*GX313 - GX323.*S3;
TKT413=C3.*GX413 - GX423.*S3;
TKT513=C3.*GX513 - GX523.*S3;
TKT223=C3.*GX223 - d3.*GX263 + GX213.*S3;
TKT323=C3.*GX323 - d3.*GX363 + GX313.*S3;
TKT423=C3.*GX423 - d3.*GX463 + GX413.*S3;
TKT523=C3.*GX523 - d3.*GX563 + GX513.*S3;
TKT333=GX333 + GX343.*JPR133 + GX353.*JPR233;
TKT433=GX433 + GX443.*JPR133 + GX453.*JPR233;
TKT533=GX533 + GX543.*JPR133 + GX553.*JPR233;
MJE112=TKT113 + XX2R;
MJE212=TKT213 + XY2;
MJE312=TKT313 + XZ2R;
MJE322=TKT323 + YZ2;
MJE332=TKT333 + ZZ2R;
MJE432=-MY2 + TKT433;
MJE532=MX2R + TKT533;
VBE12=-(AP13.*C3) - CX2 - KW12 + AP23.*S3;
VBE22=-(AP23.*C3) - CY2 + AP63.*d3 - KW22 - AP13.*S3;
VBE32=-AP33 - CZ2 - AP43.*JPR133 - AP53.*JPR233 - KW32;
JD2=1./MJE332;
JU12=JD2.*MJE312;
JU22=JD2.*MJE322;
JU42=JD2.*MJE432;
JU52=JD2.*MJE532;
GW2=GAM2 - FV2.*QP2 + VBE32 - FS2.*sign(QP2);
GK112=MJE112 - JU12.*MJE312;
GK122=MJE212 - JU12.*MJE322;
GK212=MJE212 - JU22.*MJE312;
GK222=-(JU22.*MJE322) + TKT223;
GK412=-(JU42.*MJE312) + TKT413;
GK422=-(JU42.*MJE322) + TKT423;
GK512=-(JU52.*MJE312) + TKT513;
GK522=-(JU52.*MJE322) + TKT523;
NG12=GK112.*WQ12 + GK122.*WQ22;
NG22=GK212.*WQ12 + GK222.*WQ22;
VS12=GW2.*JU12 + NG12;
VS22=GW2.*JU22 + NG22;
AP12=-VBE12 + VS12;
AP22=-VBE22 + VS22;
GX312=-(C2.*GK212) - GK112.*S2;
GX322=-(C2.*GK222) - GK122.*S2;
GX612=-(C2.*GK512) - GK412.*S2;
GX622=-(C2.*GK522) - GK422.*S2;
TKT332=-(C2.*GX322) - GX312.*S2;
TKT632=-(C2.*GX622) - GX612.*S2;
MJE331=TKT332 + ZZ1R;
VBE31=AP22.*C2 - CZ1 + AP12.*S2;
JD1=1./MJE331;
JU61=JD1.*TKT632;
GW1=GAM1 - FV1.*QP1 + VBE31 - FS1.*sign(QP1);
GU1=-(G3.*JU61);
QDP1=-GU1 + GW1.*JD1;
VR12=-(QDP1.*S2) + WQ12;
VR22=-(C2.*QDP1) + WQ22;
VR42=G3.*S2;
VR52=C2.*G3;
GU2=JU12.*VR12 + JU22.*VR22 + JU42.*VR42 + JU52.*VR52;
QDP2=-GU2 + GW2.*JD2;
VR13=C3.*VR12 + S3.*VR22 + WQ13;
VR23=-(S3.*VR12) + C3.*VR22 + WQ23;
VR43=LW13 + JPR133.*QDP2 + C3.*VR42 + S3.*VR52;
VR53=LW23 + JPR233.*QDP2 - S3.*VR42 + C3.*VR52;
VR63=LW33 - d3.*VR22;
GU3=JU33.*QDP2 + JU13.*VR13 + JU23.*VR23 + JU43.*VR43 + JU53.*VR53 + JU63.*VR63;
QDP3=-GU3 + GW3.*JD3;
WP33=QDP2 + QDP3;
VR14=C4.*VR13 + S4.*WP33 + WQ14;
VR24=-(S4.*VR13) + C4.*WP33 + WQ24;
VR44=LW14 + JPR114.*VR13 + C4.*VR43 + S4.*VR63 + JPR134.*WP33;
VR54=LW24 + JPR214.*VR13 - S4.*VR43 + C4.*VR63 + JPR234.*WP33;
VR64=LW34 - VR53;
GU4=JU14.*VR14 - JU34.*VR23 + JU24.*VR24 + JU44.*VR44 + JU54.*VR54 + JU64.*VR64;
QDP4=-GU4 + GW4.*JD4;
WP34=QDP4 - VR23;
VR15=C5.*VR14 - S5.*WP34 + WQ15;
VR25=-(S5.*VR14) - C5.*WP34 + WQ25;
VR45=C5.*VR44 - S5.*VR64;
VR55=-(S5.*VR44) - C5.*VR64;
GU5=JU15.*VR15 + JU35.*VR24 + JU25.*VR25 + JU45.*VR45 + JU65.*VR54 + JU55.*VR55;
QDP5=-GU5 + GW5.*JD5;
 WP35=QDP5 + VR24;
VR16=C6.*VR15 + S6.*WP35 + WQ16;
VR26=-(S6.*VR15) + C6.*WP35 + WQ26;
VR46=C6.*VR45 + S6.*VR54;
VR56=-(S6.*VR45) + C6.*VR54;
GU6=JU16.*VR16 - JU36.*VR25 + JU26.*VR26 + JU46.*VR46 + JU56.*VR56;
QDP6=-GU6 + GW6.*JD6;

QDP=[QDP1; QDP2; QDP3; QDP4; QDP5; QDP6];

8. Trayectoria de Simulación
8.1. Concepto de la colecistectomía laparoscópica

La cirugía laparoscópica ha generado una verdadera revolución en el manejo de la

enfermedad biliar. Erich Mühe practicó la primera colecistectomía con técnica
laparoscópica directa en 1985 y Philippe Mouret, en 1987, la primera
colecistectomía con técnica de videolaparoscopia. Actualmente, esta se acepta
como la intervención laparoscópica más practicada en el mundo y es el
procedimiento estándar en los pacientes que requieren colecistectomía[5].

Teniendo como principal referente el artículo titulado “Simulación 3D de

Movimientos Quirúrgicos de una Colecistectomía Asistida por el Robot ‘LapBot’”[6].
De donde se tomó la “Trayectoria2 disección triángulo de Calot” ver figura 28.

Figura 28. Trayectoria2 disección triángulo de Calot [6].

8.2. Trayectoria para la simulación

Para recrear la “Trayectoria2 disección triángulo de Calot” se utilizó el archivo

Calot2.mat para lograr crear un archivo de Matlab que permita recrear la trayectoria.
 Al momento de crear el archivo .m se obtiene una trayectoria (ver figura 29), la cual
será utilizada en la parte de control PID y CTC.

Figura 29. Trayectoria generada con Matlab.

9. Bibliografía

[1] “Dispositivos Hápticos Geomagic Touch.” [Online]. Available:

http://www.geomagic.com/es/products/phantom-omni/overview/.
[2] “Dispositivos Hápticos Geomagic Touch.” [Online]. Available:
http://www.geomagic.com/es/products/phantom-omni/overview. [Accessed:
13-Feb-2017].
[3] “Force reflecting haptic interface,” 2003.
[4] Oscar Andrés Vivas Albán, Diseño y control de robots industriales: teoría y práctica
de Oscar Andrés Vivas Albán | elaleph.com | ebooks | ePub y PDF. .
[5] A. Asociación Colombiana de Cirugi ́a, H. Serrano, M. Guerrero, and J. C. Uribe,
Revista colombiana de cirugía., vol. 31, no. 4. 2016.
[6] S. A. Salinas, O. Andrés, and V. Albán, “Simulación 3D de Movimientos Quirúrgicos
de una Colecistectomía Asistida por el Robot ‘LapBot.’”

Anexos

1. Función de Transf
 Función suministrada por el Ing. Sergio Alexander Salinas para el cálculo de matrices.

function T = transf(tabla,a,b)
% Obtencion de las matrices de transformacion

T=eye(4); %Matriz identidad de 4x4

for i=(a+1):b,
Tgen= [cos(tabla(i,4)) -sin(tabla(i,4)) 0 tabla(i,3);
cos(tabla(i,2))*sin(tabla(i,4)) cos(tabla(i,2))*cos(tabla(i,4)) -sin(tabla(i,2)) -tabla(i,5)*sin(tabla(i,2));
sin(tabla(i,2))*sin(tabla(i,4)) sin(tabla(i,2))*cos(tabla(i,4)) cos(tabla(i,2)) tabla(i,5)*cos(tabla(i,2));
0 0 0 1];
T=(T*Tgen);
end
T=simplify(T);

Function para la Trajectory

load('D:\Maestria\SistemasRobóticosenMédicina\Trabajos\Control_PID\Calot2.mat'
)
cons1=10*cons1+10*ones(length(cons1),1);
cons2=10*cons2+10*ones(length(cons1),1);
cons3=10*cons3;

xd=cons1;
yd=cons2;
zd=cons3;