function P = DirKin_6dof(q)
%__________________________________________________________________________
% in_args: q, joint vector
%          L, arm's link lengths
% out_args: P, coordinates of the individual joints (with base & tooltip)

% Link lengths ------------------------------------------------------------
 l1= 146; l1_1 = 81; l1_2 = 65; l2= 140; l3 = 47; % Link lenghts
% Global variables (only animation purposes!) -----------------------------
 global Hh P_trace
 Hh = (l1+l2+l3)*0.923; 
% Joint angular&speed limits ----------------------------------------------
 q_lim = [  -pi  -pi  -pi  -pi;...     % Joint physical limits
             pi   pi   pi  pi];        % Joint physical limits
 dq_max = [3 3 3 3 3 3]';              % Maximum joint angular velocities 
% Gains
Gain1 = (30/22);
Gain2 = (32/8);
Gain3 = 1;
Gain4 = (30/22);
Gain5 = (32/8);
Gain6 = (32/8);
 
q1 = -(q(1)+0);
q2 = -(q(2)+10);
q3 = (q(3)+0);
q4 = (q(4)-80);
q5 = -(q(5)+10);
q6 = (q(6)+0);

q1 = q1/Gain1;
q2 = q2/Gain2;
q3 = q3/Gain3;
q4 = q4/Gain4;
q5 = q5/Gain5;
q6 = q6/Gain6;

q1 = q1*pi/180;
q2 = q2*pi/180;
q3 = q3*pi/180;
q4 = q4*pi/180;
q5 = q5*pi/180;
q6 = q6*pi/180;
 
% Speed up calculations ...
 s1  = sin(q1);  c1  = cos(q1);
 s2  = sin(q2);  c2  = cos(q2);
 s3  = sin(q3);  c3  = cos(q3);
 s4  = sin(q4);  c4  = cos(q4);
 s45 = sin(q4+q5); c45 = cos(q4+q5);
 s6  = sin(q6);  c6  = cos(q6);
% ... Joint#1,2&3 coordinate system: HIP
 global Hh
 
 x1 = 0;
 y1 = 0;
 z1 = Hh;     % input parameter 
% ... Joint#4 coordinate system: KNEE
 x2 = l1*s1*c2;
 y2 = l1*s2;
 z2 = -l1*c1*c2+z1;
% ... Joint#5,6 coordinate system: ANKLE
 x3 = l2*s4*(s1*s2*s3+c1*c3)+l2*s1*c2*c4+x2;
 y3 = -l2*c2*s3*s4+l2*s2*c4+y2;
 z3 = -l2*s4*(c1*s2*s3-s1*c3)-l2*c1*c2*c4+z2;
% ... End-effector coordinate system
 xt = (s1*s2*s3+c1*c3)*l3*s45*c6+l3*s1*c2*c45*c6-...
      (s1*s2*c3-c1*s3)*l3*s6+x3;
 yt = -l3*c2*s3*s45*c6+l3*s2*c45*c6+l3*c2*c3*s6+y3;
 zt = -(c1*s2*s3-s1*c3)*l3*s45*c6-l3*c1*c2*c45*c6+...
      (c1*s2*c3+s1*s3)*l3*s6+z3;
% Encode the final matrix P ()
 P = [x1 x2 x3 xt; y1 y2 y3 yt; z1 z2 z3 zt];
%__________________________________________________________________________
end