You are on page 1of 1

function w=forward (t1, t2, t3, t4 )

q=[t1 t2 t3 t4 ];
alp = [180 0 0 0];
d = [877 0 q(3) 200];
a = [425 375 0 0];

T = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
for k=1:4
% q(k)= input ( 'enter value of theta');
t = [cosd(q(k)) -cosd(alp(k))*sind(q(k)) sind(alp(k))*sind(q(k))
a(k)*cosd(q(k));
sind(q(k)) cosd(alp(k))*cosd(q(k)) -sind(alp(k))*cosd(q(k))
a(k)*sind(q(k));
0 sind(alp(k)) cosd(alp(k)) d(k);
0 0 0 1];
T = T*t;
end

position_of_tooltip=[T(1,4);T(2,4);T(3,4)];

w6=(-(exp(q(4)/180)));
w=[T(1,4) T(2,4) T(3,4) 0 0 w6];
end

You might also like