1 35.

5 d1
2 82.2 a2
3 -55 a3
4 70 d5
5 47.5
 0
250
160
179
179
 TABLA DE D-H

Articulacion i di ai
1 35.5 0 0
2 82.2 0 250
3 -55 0 160
4 70 0 0
5 47.5 179 0
i
90
0
0
90
0
 1 d1 a1
35.5 0 0

0.81411552 0.00000000 0.58070296

1 0.58070296 0.00000000 -0.81411552
0.00000000 1.00000000 0.00000000
0.00000000 0.00000000 0.00000000

2 d2 a2
82.2 0 250

0.13571557 -0.99074784 0.00000000

1 0.99074784 0.13571557 0.00000000
0.00000000 0.00000000 1.00000000
0.00000000 0.00000000 0.00000000

3 d3 a3
-55 0 160

0.57357644 0.81915204 0.00000000

-0.81915204 0.57357644 0.00000000
2 0.00000000 0.00000000 1.00000000
0.00000000 0.00000000 0.00000000

4 d4 a4
70 0 0

0.34202014 0.00000000 0.93969262

0.93969262 0.00000000 -0.34202014
0.00000000 1.00000000 0.00000000
0.00000000 0.00000000 0.00000000

5 d5 a5
47.5 179 0

0.67559021 -0.73727734 0.00000000

0.73727734 0.67559021 0.00000000
0.00000000 0.00000000 1.00000000
0.00000000 0.00000000 0.00000000
1 Radianes
90 1 0.6195918845
1 1.5707963268

0.00000000
0.00000000
0.0000000
1.00000000

2 Radianes
0 2 1.4346606451
2 0

33.928893
###
0.00000000
1.00000000

3 Radianes
0 3 -0.959931089
3 0 0.01745

91.772230
###
0.00000000
1.00000000

4 Radianes
90 4 1.2217304764
4 1.5707963268

0.00000000
0.00000000
0.00000000
1.00000000

5 Radianes
0 5 0.8290313947
5 0

0.00000000
0.00000000
179.0000000
1.00000000
PRIMERA PRUEBA DE A01 A A45

0.1104881536 -0.806583192 0.580702956 27.622038400

0.078810434 -0.575330199 -0.814115518 19.702608512
0.9907478405 0.1357155724 0 247.686960118
0 0 0 1

0.7240876718 -0.3721305158 0.5807029557 143.476065885

0.5164867168 -0.2654381173 -0.8141155184 102.340483205
0.4570979271 0.8894163733 0 320.822628447
0 0.00000000 0.00 1.00

-0.1020357304 0.58 0.81 143.48

-0.0727813792 -0.81 0.58 102.34
0.9921147013 0 0.1253332336 320.822628447
0 0 0 1

0.3592047884 0.467545862 0.8076959743 288.05364529

-0.6493993083 -0.4963484107 0.5761239395 205.466668368
0.670262977 -0.7314636848 0.1253332336 343.257277255
0 0 0 1
COMPROBACION DE LA MATRIZ HOMOGENEA DE CINEMATICA DIRECTA
px 288.0536453
py 205.46666837
pz 343.25727726

r11 0.3592047884
r22 -0.496348411
r33 0.1253332336

Esta comprobacion se realiza en funcion a las ecuaciones halladas de la

r11 r12 r13 Px

r21 r22 r23 Py
r31 r32 r33 Pz
0 0 0 1
DE CINEMATICA DIRECTA

179
-35

ecuaciones halladas de la matriz del siguiente modelo:

Introducir puntos.
Px 288.0536
Py 205.46666
Pz 343.25727
 PRUEBA CINEMATICA INVERSA
calculo de teta 1
P1
py/px 0.71329315
atg py/px 0.61959194
teta 1 35.5000032 * *
P2
atg2 px;py 0.61959194
teta 1 35.5000032
P3
atg2 r13;r23 0.61959188
teta 1 35.5

*
 angulo 5

w1 -0.67230818 1.00344797
w2 -0.66999804 0.78711918
-2.35447347 45.098607
-134.901393

angulo 2
al cuadrado
b1 -514.538985 264750.367
b2 164.257277 26980.4531

numerador 203630.82
denominador 80000
division 2.54538525
x1 218781.88
x2 203630.82
0.8212507 47.0541991