function P = DirKin_4dof(q,L,warm)
%__________________________________________________________________________
% in_args: q, joint vector
%          L, arm's link lengths
%          warm, which arm: left +1 or right -1
% out_args: P, coordinates of the individual joints (with base & tooltip)
%
% Decodes input arguments 
 lt = L(1); lt_l = L(2); lt_w = L(3); lt_h = L(4); 
 lr = L(5); l0 = L(6); 
 l1 = L(7); l1_1 = L(8); l1_2 = L(9); l2 = L(10);
 q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); 
% Decodes the desired arm
 if warm == +1, FLAG_sign =  1;     %Left arm
 else           FLAG_sign = -1; end % Right arm    
% ... Joint#1,2&3 coordinate system: shoulder
 x1 = 0;
 y1 = FLAG_sign*(lt_l/2+l0);
 z1 = lt+lt_h-lr;     % 0.07m is the cylinder radius 
% ... Joint#4 coordinate system: elbow
 x2 = FLAG_sign*l1*sin(q1)*cos(q2);
 y2 = l1*sin(q2)+y1;
 z2 = -l1*cos(q1)*cos(q2)+z1;
% ... End-effector coordinate system
 xt = l2*sin(q4)*(sin(q1)*sin(q2)*sin(q3)+FLAG_sign*cos(q1)*cos(q3))+...
                                          FLAG_sign*sin(q1)*cos(q2)*(l2*cos(q4)+l1);
 yt =-FLAG_sign*l2*cos(q2)*sin(q3)*sin(q4)+sin(q2)*(l2*cos(q4)+l1)+y1;
 zt =-FLAG_sign*l2*sin(q4)*(cos(q1)*sin(q2)*sin(q3)-FLAG_sign*sin(q1)*cos(q3))-...
                                        cos(q1)*cos(q2)*(l2*cos(q4)+l1)+z1;
% Encode the final matrix P ()
 P = [x1 x2 xt; y1 y2 yt; z1 z2 zt];
%__________________________________________________________________________
end