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

 l0 = L(1); l1 = L(2); l2 = L(3); l3 = L(4); l4 = L(5); l5 = L(6); l5h = L(7); l5t = L(8); l6 = L(9);
 r0 = R(1); r1 = R(2); r2 = R(3); r3 = R(4); r4 = R(5); r5 = R(6);
 m0 = M(1); m1 = M(2); m2 = M(3); m3 = M(4); m4 = M(5); m5 = M(6); m6 = M(7);
 ang1 = q(1); ang2 = q(2); ang3 = q(3);  ang4 = q(4);   ang5 = q(5);   ang6 = q(6);
 ang7 = q(7); ang8 = q(8); ang9 = q(9); ang10 = q(10); ang11 = q(11); ang12 = q(12);
 Mt = 2*m0+2*m1+2*m2+2*m3+2*m4+2*m5+m6;
  
 %-- Pe/Tornozelo (m0) --------------------------------------------
  COG_0(1) = 0;
  COG_0(2) = 0;
  COG_0(3) = r0;

 %-- Tornozelo/Tornozelo (m1) ------------------------------------
  COG_1(1) = 0;
  COG_1(2) =   -r1*sin(ang1);
  COG_1(3) = l0+r1*cos(ang1);

 %-- Tornozelo/Joelho (m2) ----------------------------------------  
  COG_2(1) =                 r2*sin(ang2); 
  COG_2(2) =   -l1*sin(ang1)-r2*sin(ang1)*cos(ang2);
  COG_2(3) = l0+l1*cos(ang1)+r2*cos(ang1)*cos(ang2);

 %-- Joelho/Anca (m3) ---------------------------------------------
  COG_3(1) =                 l2*sin(ang2)          +r3*sin(ang2+ang3);
  COG_3(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-r3*sin(ang1)*cos(ang2+ang3);
  COG_3(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+r3*cos(ang1)*cos(ang2+ang3);
  
 %-- Anca/Anca (m4) -----------------------------------------------
  COG_4(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3);
  COG_4(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-r4*sin(ang1);
  COG_4(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+r4*cos(ang1);
  
 %-- Anca/Tronco (m5) ---------------------------------------------
  COG_5(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3);
  COG_5(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1);
  COG_5(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)+r5;
  
 %-- Tronco (m6) ---------------------------------------------
  COG_6(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3);
  COG_6(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6/2;
  COG_6(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)+(l5+l5h+l5t);

  %-- Tronco/Anca (m5) ---------------------------------------------
  COG_7(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3);
  COG_7(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_7(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1);
  
  %-- Anca/Anca (m4) ---------------------------------------------
  COG_8(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3);
  COG_8(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_8(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)-(l4-r4);
 
  %-- Anca/Joelho (m3) ---------------------------------------------
  COG_9(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3)+(l3-r3)*sin(ang9);
  COG_9(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_9(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)-l4-(l3-r3)*cos(ang9);
  
  %-- Joelho/Tornozelo (m2) ---------------------------------------------
  COG_10(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3)+l3*sin(ang9)+(l2-r2)*sin(ang9+ang10);
  COG_10(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_10(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)-l4-l3*cos(ang9)-(l2-r2)*cos(ang9+ang10);

  %-- Tornozelo/Tornozelo (m1) ---------------------------------------------
  COG_11(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3)+l3*sin(ang9)+l2*sin(ang9+ang10);
  COG_11(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_11(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)-l4-l3*cos(ang9)-l2*cos(ang9+ang10)-(l1-r1);

  %-- Tornozelo/Pe (m0) ---------------------------------------------
  COG_12(1) =                 l2*sin(ang2)          +l3*sin(ang2+ang3)+l3*sin(ang9)+l2*sin(ang9+ang10);
  COG_12(2) =   -l1*sin(ang1)-l2*sin(ang1)*cos(ang2)-l3*sin(ang1)*cos(ang2+ang3)-l4*sin(ang1)+l6;
  COG_12(3) = l0+l1*cos(ang1)+l2*cos(ang1)*cos(ang2)+l3*cos(ang1)*cos(ang2+ang3)+l4*cos(ang1)-l4-l3*cos(ang9)-l2*cos(ang9+ang10)-l1-(l0-r0);

%-- Centro de gravidade -------------------------------------------
 COG(1) = ( m0*COG_0(1) +m1*COG_1(1) +m2*COG_2(1) +m3*COG_3(1)+m4*COG_4(1)+m5*COG_5(1)+m6*COG_6(1)+...
            m0*COG_12(1)+m1*COG_11(1)+m2*COG_10(1)+m3*COG_9(1)+m4*COG_8(1)+m5*COG_7(1) ) / Mt;
 COG(2) = ( m0*COG_0(2) +m1*COG_1(2) +m2*COG_2(2) +m3*COG_3(2)+m4*COG_4(2)+m5*COG_5(2)+m6*COG_6(2)+...
            m0*COG_12(2)+m1*COG_11(2)+m2*COG_10(2)+m3*COG_9(2)+m4*COG_8(2)+m5*COG_7(2) ) / Mt;
 COG(3) = ( m0*COG_0(3) +m1*COG_1(3) +m2*COG_2(3) +m3*COG_3(3)+m4*COG_4(3)+m5*COG_5(3)+m6*COG_6(3)+...
            m0*COG_12(3)+m1*COG_11(3)+m2*COG_10(3)+m3*COG_9(3)+m4*COG_8(3)+m5*COG_7(3) ) / Mt;
