% forward kinematics of SCARA 4 DOF manipulator using the following

% DH parameters:

%

%    link    alpha    a      theta     d

%      1       0      3       var      0

%      2      180     2       var      0

%      3       0      0       var      0

%      4       0      0        0      var

%

% Note that joint 3 rotates opposite to joint 2 and

% positive extension of the prismatic joint is downward.

%

% The world origin is taken at joint 1.



function T = scarafk(q)



a1 = 3;

a2 = 2;



th1 = q(1);

th2 = q(2);

th3 = q(3);

d4  = q(4);



C123 = cos(th1+th2-th3);

S123 = sin(th1+th2-th3);

C12 = cos(th1+th2);

S12 = sin(th1+th2);

C1  = cos(th1);

S1  = sin(th1);





T = [C123  S123  0  a2*C12+a1*C1

     S123 -C123  0  a2*S12+a1*S1

       0     0  -1   -d4

       0     0   0    1];





