function P = DirKin_6dof(q,L)
%__________________________________________________________________________
% in_args: q, joint vector
%          L, arm's link lengths
% out_args: P, coordinates of the individual joints (with base & tooltip)
%
% Decodes input arguments 
 l1 = L(1); l1_1 = L(2); l1_2 = L(3); l2 = L(4); l3 = L(5);
 q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5); q6 = q(6);
% 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