function [C,F,H,Thip] = JointCoord_stance( XY_ref,q,L,wleg )

 %-- Constants & variable atribution --------------------------------------
  Xref = XY_ref(1); Yref = XY_ref(2); 
  l0 = L(1); l1 = L(2); l2 = L(3); l21 = L(4); l22 = L(5); l3 = L(6);
 %-- Definition of constants & variables ----------------------------------
  ang1 = q(1); ang2 = q(2); ang3 = q(3); 
  ang4 = q(4); ang5 = q(5); ang6 = q(6);
 %-- Original cylinder ----------------------------------------------------
  [X,Y,Z] = cylinder(1,100);
  X = 1.0*0.01*X; Y = 1.0*0.01*Y; Z = 6.5*0.01*(Z-0.5);
  X1 = X(1,:); Y1 = Y(1,:); Z1 = Z(1,:);
  X2 = X(2,:); Y2 = Y(2,:); Z2 = Z(2,:);
  Pcyl = [X1;X2;Y1;Y2;Z1;Z2;ones(1,length(X1))];
%-- Original foot --------------------------------------------------------
  T0 = [ 0  0  1  Xref
         0  1  0  Yref
        -1  0  0  0
         0  0  0  1 ];
  Pfoot1 = [  0      0     0      0       0     ;             
             -0.030 -0.030 0.030  0.030  -0.030 ;
             -0.070  0.070 0.070 -0.070  -0.070 ];
  Pfoot2 = [ -0.005 -0.005 -0.005 -0.005  -0.005 ; 
             -0.030 -0.030 0.030  0.030  -0.030 ;
             -0.070  0.070 0.070 -0.070  -0.070 ]; 
  F(:,:,1) = T0*[Pfoot1; ones(1,length(Pfoot1(1,:)))];
  F(:,:,2) = T0*[Pfoot2; ones(1,length(Pfoot2(1,:)))];
 %-- Joint#1: homogeneous transformation ----------------------------------
  T1 = [ 0  0 1 Xref
         0 -1 0 Yref
         1  0 0 l0
         0  0 0 1 ];
  C1([1 3 5 7],:) = T1*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C1([2 4 6 7],:) = T1*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];
 %-- Joint#2: homogeneous transformation ----------------------------------
  T2 = T1*[ cos(ang1)  0      wleg*sin(ang1)  0
            sin(ang1)  0     -wleg*cos(ang1)  0
            0          wleg   0               0
            0          0      0               1 ];
  C2([1 3 5 7],:) = T2*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C2([2 4 6 7],:) = T2*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];
 %-- Joint#3: homogeneous transformation ----------------------------------
  T3 = T2*[ cos(ang2) -sin(ang2) 0 l1*cos(ang2)
            sin(ang2)  cos(ang2) 0 l1*sin(ang2)
            0          0         1 0
            0          0         0 1           ];
  C3([1 3 5 7],:) = T3*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C3([2 4 6 7],:) = T3*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];
 %-- Joint#4: homogeneous transformation ----------------------------------
  T4 = T3*[ wleg*sin(ang3)  0      cos(ang3)  l21*cos(ang3)
           -wleg*cos(ang3)  0      sin(ang3)  l21*sin(ang3)
            0              -wleg   0          0
            0               0      0          1 ];
  C4([1 3 5 7],:) = T4*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C4([2 4 6 7],:) = T4*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];
 %-- Joint#5: homogeneous transformation ----------------------------------
  T5 = T4*[ wleg*sin(ang4)  0     -cos(ang4)  0
           -wleg*cos(ang4)  0     -sin(ang4)  0
            0               wleg   0          l22
            0               0      0          1 ];
  C5([1 3 5 7],:) = T5*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C5([2 4 6 7],:) = T5*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];
 %-- Joint#6: homogeneous transformation ----------------------------------
  T6 = T5*[ wleg*sin(ang5)  0      cos(ang5)  0
           -wleg*cos(ang5)  0      sin(ang5)  0
            0              -wleg   0          0
            0               0      0          1 ];
  C6([1 3 5 7],:) = T6*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                                ones(1,length(Pcyl(1,:)))];
  C6([2 4 6 7],:) = T6*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                                ones(1,length(Pcyl(1,:)))];                                          
 %-- Output matrix --------------------------------------------------------
  C(:,:,1) = C1;
  C(:,:,2) = C2;
  C(:,:,3) = C3;
  C(:,:,4) = C4;
  C(:,:,5) = C5;
  C(:,:,6) = C6;
 %-- Joint#6: homogeneous transformation ----------------------------------
  Thip = T6*[ cos(ang6)  sin(ang6)  0  0
              sin(ang6) -cos(ang6)  0  0
              0          0         -1  l3
              0          0          0  1 ];
          
%T1,T2,T3,T4,
%T5, Thip, pause          
%-- Hip section -----------------------------------------------------------
  Hip1 = [  0.03250  0.03250 -0.03250 -0.03250  0.03250;
            0.09000  0.09000  0.09000  0.09000  0.09000;
            0.03250 -0.03250 -0.03250  0.03250  0.03250];
  Hip2 = [  0.03250  0.03250 -0.03250 -0.03250  0.03250;
           -0.09000 -0.09000 -0.09000 -0.09000 -0.09000;
            0.03250 -0.03250 -0.03250  0.03250  0.03250];     
  Hip3 = [  0.03250  0.03250 -0.03250 -0.03250  0.03250;
           -0.09000  0.09000  0.09000 -0.09000 -0.09000;
            0.03250  0.03250  0.03250  0.03250  0.03250];     
        
  T7 = T6*[ sin(ang6)  0  -cos(ang6)  0
           -cos(ang6)  0  -sin(ang6)  0
            0          1   0          l3/2
            0          0   0          1   ];
  H(:,:,1) = T7*[Hip1; ones(1,length(Hip1(1,:)))];
  H(:,:,2) = T7*[Hip2; ones(1,length(Hip2(1,:)))];
  H(:,:,3) = T7*[Hip3; ones(1,length(Hip3(1,:)))];
 %----------------------------------------------------------------------end
end