function [COG_stance COG_hip] = RobCOG_stance( Pref,q,L,M,wleg )

 %-- Defines the COMi in the local coordinate system ----------------------
  if wleg == +1, % Right leg     
     CMx0 = 0.0091; CMy0 = -0.0027; CMz0 =  0.0317;
     CMx1 = 0.0885; CMy1 =  0.0002; CMz1 = -0.0159;
     CMx2 = 0.1013; CMy2 = -0.0000; CMz2 = -0.0000; % MUST BE ZERO!!
     % Hip section
     %CMx3_rl = 0.0091; CMy3_r = -0.0027; CMz3_r = 0.0317;      
  else  % Left leg
      CMx0 = 0.0091; CMy0 =  0.0027; CMz0 =  0.0317;
      CMx1 = 0.0885; CMy1 = -0.0002; CMz1 = -0.0159;
      CMx2 = 0.1013; CMy2 = -0.0000; CMz2 = -0.0000; % MUST BE ZERO!!      
  end
 %-- Definition of constants & variables ----------------------------------
  l0 = L(1); l1 = L(2); l2 = L(3); l21 = L(4); l22 = L(5); l3 = L(6);
  m0 = M(1); m1 = M(2); m2 = M(3); m3 = M(4); 
  q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); q5 = q(5); q6 = q(6);
  Mt = m0+m1+m2;
 %-- Definition the trignometric functions --------------------------------
  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);
 %-- Foot/ankle (m0) ------------------------------------------------------
  COG_0(1) = Pref(1)+CMx0;
  COG_0(2) = Pref(2)+CMy0;
  COG_0(3) = Pref(3)+CMz0;
 %-- Ankle/knee (m1) ------------------------------------------------------  
  COG_1(1) = Pref(1)+wleg*s2*CMx1+wleg*c2*CMy1; 
  COG_1(2) = Pref(2)-s1*c2*CMx1+s1*s2*CMy1+wleg*c1*CMz1;
  COG_1(3) = Pref(3)+c1*c2*CMx1-c1*s2*CMy1+wleg*s1*CMz1+l0;
 %-- Knee/hip (m2) --------------------------------------------------------
  COG_2(1) = Pref(1)+wleg*s23*CMx2+wleg*c23*CMy2+wleg*l1*s2; 
  COG_2(2) = Pref(2)-s1*c23*CMx2+s1*s23*CMy2+wleg*c1*CMz2-l1*s1*c2;
  COG_2(3) = Pref(3)+c1*c23*CMx2-c1*s23*CMy2+wleg*s1*CMz2+l1*c1*c2+l0;
%-- Hip section (m3) ------------------------------------------------------
  COG_hip(1,1) = Pref(1)+wleg*l1*s2+wleg*l2*s23+l3*(-wleg*c23*s4*c5+s23*s5)/2; 
  COG_hip(2,1) = Pref(2)-l1*s1*c2-l2*s1*c23+l3*((-s1*s23*s4+wleg*c1*c4)*c5-wleg*s1*c23*s5)/2;
  COG_hip(3,1) = Pref(3)+l0+l1*c1*c2+l2*c1*c23+l3*((c1*s23*s4+wleg*s1*c4)*c5+wleg*c1*c23*s5)/2;  
 %-- Centre of Gravity (COG) ----------------------------------------------
  COG_stance = [ COG_0(1) COG_1(1) COG_2(1)
                 COG_0(2) COG_1(2) COG_2(2)
                 COG_0(3) COG_1(3) COG_2(3) ];
 %-- Centre of Gravity (COG) ----------------------------------------------
 % COG(1) = ( m0*COG_0_r(1)+m1*COG_1_r(1)+m2*COG_2_r(1) ) / Mt;
 % COG(2) = ( m0*COG_0_r(2)+m1*COG_1_r(2)+m2*COG_2_r(2) ) / Mt;
 % COG(3) = ( m0*COG_0_r(3)+m1*COG_1_r(3)+m2*COG_2_r(3) ) / Mt;
 % COG = COG';
 %-------------------------------------------------------------------------
end