Puma 761

LUIS BARRON VARGAS

syms t1t2t3t4t5t6;
syms a2d2d4d6;
T10 = [cos(t1) 0 -sin(t1) 0
sin(t1) 0 cos(t1) 0
0
-1 0
0
0
00
1];
T21 = [cos(t2) -sin(t2) 0 a2*cos(t2)
sin(t2) cos(t2) 0 a2*sin(t2)
0
0
1 d2
0
0
0 1];
T32 = [cos(t3) 0 sin(t3) 0
sin(t3) 0 -cos(t3) 0
0
10
0
0
00
1];
T43 = [cos(t4) 0 -sin(t4) 0
sin(t4) 0 cos(t4) 0
0
-1 0
d4
0
00
1];
T54 = [cos(t5) 0sin(t5) 0
sin(t5) 0 -cos(t5) 0
0
10
0
0
00
1];
T65 = [cos(t6) -sin(t6) 0 0
sin(t6) cos(t6) 0 0
0
0
1 d6
0
0
0 1];
T30 = T10*T21*T32
T63 = T43*T54*T65

T60 = T30*T63

T30 =
[ cos(t1)*cos(t2)*cos(t3) - cos(t1)*sin(t2)*sin(t3), -sin(t1), cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2), a2*cos(t1)*cos(t2) - d2*sin(t1)]
[ cos(t2)*cos(t3)*sin(t1) - sin(t1)*sin(t2)*sin(t3), cos(t1),cos(t2)*sin(t1)*sin(t3) + cos(t3)*sin(t1)*sin(t2), d2*cos(t1) + a2*cos(t2)*sin(t1)]
[
- cos(t2)*sin(t3) - cos(t3)*sin(t2),
0,
cos(t2)*cos(t3) - sin(t2)*sin(t3),
-a2*sin(t2)]
[
0,
0,
0,
1]

T63 =
[ cos(t4)*cos(t5)*cos(t6) - sin(t4)*sin(t6), - cos(t6)*sin(t4) - cos(t4)*cos(t5)*sin(t6), cos(t4)*sin(t5), d6*cos(t4)*sin(t5)]
[ cos(t4)*sin(t6) + cos(t5)*cos(t6)*sin(t4), cos(t4)*cos(t6) -cos(t5)*sin(t4)*sin(t6), sin(t4)*sin(t5), d6*sin(t4)*sin(t5)]
[
-cos(t6)*sin(t5),
sin(t5)*sin(t6),
cos(t5),
d4 + d6*cos(t5)]
[
0,
0,
0,
1]

T60 =
R11 = (cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3))*(sin(t4)*sin(t6) - cos(t4)*cos(t5)*cos(t6)) - sin(t1)*(cos(t4)*sin(t6) +
cos(t5)*cos(t6)*sin(t4)) - cos(t6)*sin(t5)*(cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2))
R12 =(cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3))*(cos(t6)*sin(t4) + cos(t4)*cos(t5)*sin(t6)) - sin(t1)*(cos(t4)*cos(t6) cos(t5)*sin(t4)*sin(t6)) + sin(t5)*sin(t6)*(cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2))
R13 = cos(t5)*(cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2)) - sin(t1)*sin(t4)*sin(t5) cos(t4)*sin(t5)*(cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3))
R14 =(cos(t1)*cos(t2)*sin(t3) + cos(t1)*cos(t3)*sin(t2))*(d4 + d6*cos(t5)) - d2*sin(t1) + a2*cos(t1)*cos(t2) d6*sin(t1)*sin(t4)*sin(t5) - d6*cos(t4)*sin(t5)*(cos(t1)*sin(t2)*sin(t3) - cos(t1)*cos(t2)*cos(t3))

R21 = (sin(t1)*sin(t2)*sin(t3) - cos(t2)*cos(t3)*sin(t1))*(sin(t4)*sin(t6) - cos(t4)*cos(t5)*cos(t6)) + cos(t1)*(cos(t4)*sin(t6) +
cos(t5)*cos(t6)*sin(t4)) - cos(t6)*sin(t5)*(cos(t2)*sin(t1)*sin(t3) +cos(t3)*sin(t1)*sin(t2))
R22 = (sin(t1)*sin(t2)*sin(t3) - cos(t2)*cos(t3)*sin(t1))*(cos(t6)*sin(t4) + cos(t4)*cos(t5)*sin(t6)) + cos(t1)*(cos(t4)*cos(t6)
- cos(t5)*sin(t4)*sin(t6)) + sin(t5)*sin(t6)*(cos(t2)*sin(t1)*sin(t3) + cos(t3)*sin(t1)*sin(t2))
R23 = cos(t5)*(cos(t2)*sin(t1)*sin(t3) + cos(t3)*sin(t1)*sin(t2)) + cos(t1)*sin(t4)*sin(t5) cos(t4)*sin(t5)*(sin(t1)*sin(t2)*sin(t3) -cos(t2)*cos(t3)*sin(t1))
R24 = (d4 + d6*cos(t5))*(cos(t2)*sin(t1)*sin(t3) + cos(t3)*sin(t1)*sin(t2)) + d2*cos(t1) + a2*cos(t2)*sin(t1) +
d6*cos(t1)*sin(t4)*sin(t5) - d6*cos(t4)*sin(t5)*(sin(t1)*sin(t2)*sin(t3) - cos(t2)*cos(t3)*sin(t1))
R31 = (sin(t4)*sin(t6) - cos(t4)*cos(t5)*cos(t6))*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) - cos(t6)*sin(t5)*(cos(t2)*cos(t3) sin(t2)*sin(t3))
R32 = (cos(t6)*sin(t4) +cos(t4)*cos(t5)*sin(t6))*(cos(t2)*sin(t3) + cos(t3)*sin(t2)) + sin(t5)*sin(t6)*(cos(t2)*cos(t3) sin(t2)*sin(t3))
R33 = cos(t5)*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - cos(t4)*sin(t5)*(cos(t2)*sin(t3) + cos(t3)*sin(t2))
R34 = (d4 + d6*cos(t5))*(cos(t2)*cos(t3) - sin(t2)*sin(t3)) - a2*sin(t2) - d6*cos(t4)*sin(t5)*(cos(t2)*sin(t3) + cos(t3)*sin(t2))
R41 = 0
R42 = 0
R43 = 0
R44 = 1

//Autor: Luis BarrónVargas.
//Fecha: 20/04/2012
//Calcula la matriz de Denavith-Hartenberg de cada eslabón
//del manipulador cilindrico, de la muñeca y la matriz total del Robot PUMA761
#include
#include
#include
#define A2 650
#define D2 191
#define D4 850
#define D6 125
#define N 4
#define C 3.141592/180
#define CERO 0
#define AL1 270
#define AL3 90
#define AL4 270
#define AL5 90
void...