function COG = RobCOG( q,L,R,M )

global CM0x CM1y 

 %-- Definition of constants & variables ----------------------------------
  l0 = L(1); l1 = L(2); l2 = L(3); 
  r0 = R(1); r1 = R(2); r2 = R(3); 
  m0 = M(1); m1 = M(2); m2 = M(3); 
  q1 = q(1); q2 = q(2); q3 = q(3);
  Mt = m0+m1+m2;
 %-- Foot/ankle (m0) ------------------------------------------------------
  COG_0(1) = CM0x;
  COG_0(2) = 0;
  COG_0(3) = r0;
 %-- Ankle/knee (m1) ------------------------------------------------------  
  COG_1(1) =    r1*sin(q2); 
  COG_1(2) =   -r1*sin(q1)*cos(q2)+CM1y*cos(q1);
  COG_1(3) = l0+r1*cos(q1)*cos(q2)+CM1y*sin(q1);
 %-- Knee/hip (m2) --------------------------------------------------------
  COG_2(1) =    l1*sin(q2)+r2*sin(q2+q3);
  COG_2(2) =   -l1*sin(q1)*cos(q2)-r2*sin(q1)*cos(q2+q3);
  COG_2(3) = l0+l1*cos(q1)*cos(q2)+r2*cos(q1)*cos(q2+q3);
 %-- Centre of Gravity (COG) ----------------------------------------------
  COG(1) = ( m0*COG_0(1)+m1*COG_1(1)+m2*COG_2(1) ) / Mt;
  COG(2) = ( m0*COG_0(2)+m1*COG_1(2)+m2*COG_2(2) ) / Mt;
  COG(3) = ( m0*COG_0(3)+m1*COG_1(3)+m2*COG_2(3) ) / Mt;
  COG = COG';
 %----------------------------------------------------------------------end