function [P,Phip] = DirKin6dof_stance( Pref,q,L,wleg )
% Input parameters:  XYZ_target relative to the Sr
%                    L, link lenghts
%                    wleg, which leg -> +1 is right; -1 is left
% Output parameters: Q, joint vector (6x1)
%                    IsVal, valide/possible solution
%--------------------------------------------------------------------------
% Constants & variable atribution -----------------------------------------
 l0 = L(1); l1 = L(2); l2 = L(3); l21 = L(4); l22 = L(5); l3 = L(6);
% Definition of constants & variables -------------------------------------
 xr = Pref(1); yr = Pref(2); zr = Pref(3);
 q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5); q6 = q(6);
% Pre-computing for speed ------------------------------------------------- 
 s1 = sin(q1); c1 = cos(q1);
 s2 = sin(q2); c2 = cos(q2);
 s23 = sin(q2+q3); c23 = cos(q2+q3);
 s4 = sin(q4); c4 = cos(q4);
 s5 = sin(q5); c5 = cos(q5);
% Stance foot -------------------------------------------------------------
 x0=xr;
 y0=yr;
 z0=zr;
% Stance ankle ------------------------------------------------------------
 x1=x0;
 y1=y0;
 z1=z0+l0;
% Stance knee -------------------------------------------------------------
 x2=x1+wleg*l1*s2;
 y2=y1-l1*s1*c2;
 z2=z1+l1*c1*c2;
% Stance vertical axis ----------------------------------------------------
 x3=x2+wleg*l21*s23;
 y3=y2-l21*s1*c23;
 z3=z2+l21*c1*c23;
% Stance hip --------------------------------------------------------------
 x4=x2+wleg*l2*s23;
 y4=y2-l2*s1*c23;
 z4=z2+l2*c1*c23;
% Swing hip ---------------------------------------------------------------
 x5=x4+l3*(-wleg*c23*s4*c5+s23*s5);
 y5=y4+l3*((-s1*s23*s4+wleg*c1*c4)*c5-wleg*s1*c23*s5);
 z5=z4+l3*((c1*s23*s4+wleg*s1*c4)*c5+wleg*c1*c23*s5);
%--------------------------------------------------------------------------
 P = [x0;y0;z0; x1;y1;z1; x2;y2;z2; x3;y3;z3; x4;y4;z4];
 Phip = [x5;y5;z5];
%-----------------------------------------------------------------------end
end