11/29/14 4:37 PM

C:\Users\win\Desktop\477 project\project.m

clc
syms t1 t2 t3 t4 t5 t6 t7
a1=0;a2=350;a3=1150;a4=0;a5=0;a6=0;a7=0;
d1=0;d2=0;d3=0;d4=1200;d5=0;d6=0;d7=0;
alf1=0;alf2=-90*pi/180;alf3=0;alf4=90*pi/180;alf5=-90*pi/180;alf6=90*pi/180;
alf7=0;
syms alf1 alf2 alf3 alf4 alf5 alf6 alf7
t2=t2+90;
T1=[cos(t1),-sin(t1),0,a1;...
sin(t1)*cos(alf1),cos(t1)*cos(alf1),-sin(alf1),-sin(alf1)*d1; ...
sin(t1)*sin(alf1),cos(t1)*sin(alf1),cos(alf1),cos(alf1)*d1; ...
0,0,0,1];
T2=[cos(t2),-sin(t2),0,a2;
sin(t2)*cos(alf2),cos(t2)*cos(alf2),-sin(alf2),sin(alf2)*d2;
sin(t2)*sin(alf2),cos(t2)*cos(alf2),-sin(alf2),-sin(alf2)*d2;
0,0,0,1];
T3=[cos(t3),-sin(t3),0,a3;
sin(t3)*cos(alf3),cos(t3)*cos(alf3),-sin(alf3),sin(alf3)*d3;
sin(t3)*sin(alf3),cos(t3)*cos(alf3),-sin(alf3),-sin(alf3)*d3;
0,0,0,1];
T4=[cos(t4),-sin(t4),0,a4;
sin(t4)*cos(alf4),cos(t4)*cos(alf4),-sin(alf4),sin(alf4)*d4;
sin(t4)*sin(alf4),cos(t4)*cos(alf4),-sin(alf4),-sin(alf4)*d4;
0,0,0,4];
T5=[cos(t5),-sin(t5),0,a5;
sin(t5)*cos(alf5),cos(t5)*cos(alf5),-sin(alf5),sin(alf5)*d5;
sin(t5)*sin(alf5),cos(t5)*cos(alf5),-sin(alf5),-sin(alf5)*d5;
0,0,0,1];
T6=[cos(t6),-sin(t6),0,a6;
sin(t6)*cos(alf6),cos(t6)*cos(alf6),-sin(alf6),sin(alf6)*d6;
sin(t6)*sin(alf6),cos(t6)*cos(alf6),-sin(alf6),-sin(alf6)*d6;
0,0,0,1];
T7=[cos(t7),-sin(t7),0,a7;
sin(t7)*cos(alf7),cos(t7)*cos(alf7),-sin(alf7),sin(alf7)*d7;
sin(t7)*sin(alf7),cos(t7)*cos(alf7),-sin(alf7),-sin(alf7)*d7;
0,0,0,1];
T_global=T1*T2*T3*T4*T5*T6*T7;
%%Velocity Propagation
R1_0=inv([T1(1:3,1:3)]);R2_1=inv([T2(1:3,1:3)]);R3_2=inv([T3(1:3,1:3)]);
R4_3=inv([T4(1:3,1:3)]);R5_4=inv([T5(1:3,1:3)]);R6_5=inv([T6(1:3,1:3)]);
R7_6=inv([T7(1:3,1:3)]);P1=[T1(1:3,3)];P2=[T2(1:3,3)];P3=[T3(1:3,3)];
P4=[T4(1:3,3)];P5=[T5(1:3,3)];P6=[T6(1:3,3)];P7=[T7(1:3,3)];
w0=[0;0;0];v0=[0;0;0];w1=R1_0*w0+[0;0;t1];v1=R1_0*(v0+cross(w0,P1));
w2=R2_1*w1+[0;0;t2];v2=R2_1*(v1+cross(w1,P2));
w3=R3_2*w2+[0;0;t3];v3=R3_2*(v2+cross(w2,P3));
w4=R4_3*w3+[0;0;t4];v4=R4_3*(v3+cross(w3,P4));
w5=R5_4*w4+[0;0;t5];v5=R5_4*(v4+cross(w4,P5));
w6=R6_5*w5+[0;0;t6];v6=R6_5*(v5+cross(w5,P6));
w7=R7_6*w6+[0;0;t7];v7=R7_6*(v6+cross(w6,P7));
V1=[w1;v1];V2=[w2;v2];V3=[w3;v3];V4=[w4;v4];
V5=[w5;v5];V6=[w6;v6];V_endeffector=[w7;v7];

1 of 2

11/29/14 4:37 PM

C:\Users\win\Desktop\477 project\project.m

2 of 2