function def = RobCOG( A,DM,COG_0, def )

q = DM.q(1,:);
M = DM.M;

 Mt = 2*M(1)+2*M(2)+2*M(3)+2*M(4)+2*M(5)+2*M(6)+M(7)+M(8)+M(9)+M(10)+2*M(11)+2*M(12);
%----------------------------------------------------------------------------------------------------  
% PERNA DIREITA: m1-m6
%----------------------------------------------------------------------------------------------------  
%-- Pe -> m1 ----------------------------------------------------------------------------------------
%----------------------------------------------------------------------------------------------------
 T = Tra_XX(COG_0(1,1))*Tra_YY(COG_0(2,1))*Tra_ZZ(COG_0(3,1));
 COG_m1R = T(1:3,4);
%---------------------------------------------------------------------------------------------------- 
%-- Tornozelo -> m2 ---------------------------------------------------------------------------------
%----------------------------------------------------------------------------------------------------
 T_aux = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
 T_ind = 1; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(1))*Tra_XX(COG_0(1,2))*Tra_YY(COG_0(2,2))*Tra_ZZ(COG_0(3,2));
 COG_m2R = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Perna_R (tornozelo-joelho) -> m3
%----------------------------------------------------------------------------------------------------
 T_ind = 2; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(2))*Tra_XX(COG_0(1,3))*Tra_YY(COG_0(2,3))*Tra_ZZ(COG_0(3,3));
 COG_m3R = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Coxa_R (joelho-anca) -> m4
%----------------------------------------------------------------------------------------------------
 T_ind = 3; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(3))*Tra_XX(COG_0(1,4))*Tra_YY(COG_0(2,4))*Tra_ZZ(COG_0(3,4));
 COG_m4R = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Coxa_R (anca-anca) -> m5
%----------------------------------------------------------------------------------------------------
 T_ind = 4; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(4))*Tra_XX(COG_0(1,5))*Tra_YY(COG_0(2,5))*Tra_ZZ(COG_0(3,5));
 COG_m5R = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Coxa_R (anca-anca) -> m6
%----------------------------------------------------------------------------------------------------
 T_ind = 5; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end, T_aux_cc = T_aux;
 T = T_aux*Rot_ZZ(q(5))*Tra_XX(COG_0(1,6))*Tra_YY(COG_0(2,6))*Tra_ZZ(COG_0(3,6));
 COG_m6R = T(1:3,4);
%----------------------------------------------------------------------------------------------------  
% PERNA ESQUERDA: m1-m6
%----------------------------------------------------------------------------------------------------  
%-- Coxa_L (anca-anca) -> m6
%----------------------------------------------------------------------------------------------------
 T_ind = 6:8; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(7))*Tra_XX(COG_0(1,7))*Tra_YY(COG_0(2,7))*Tra_ZZ(COG_0(3,7));
 COG_m6L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------
%-- Coxa_L (anca-anca) -> m5
%----------------------------------------------------------------------------------------------------
 T_ind = 9; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(8))*Tra_XX(COG_0(1,8))*Tra_YY(COG_0(2,8))*Tra_ZZ(COG_0(3,8));
 COG_m5L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------
%-- Coxa_L (anca-joelho) -> m4
%----------------------------------------------------------------------------------------------------
 T_ind = 10; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(9))*Tra_XX(COG_0(1,9))*Tra_YY(COG_0(2,9))*Tra_ZZ(COG_0(3,9));
 COG_m4L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------
%-- Coxa_L (joelho-tornozelo) -> m3
%----------------------------------------------------------------------------------------------------
 T_ind = 11; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(10))*Tra_XX(COG_0(1,10))*Tra_YY(COG_0(2,10))*Tra_ZZ(COG_0(3,10));
 COG_m3L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------
%-- Coxa_L (tornozelo-tornozelo) -> m2
%----------------------------------------------------------------------------------------------------
 T_ind = 12; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(11))*Tra_XX(COG_0(1,11))*Tra_YY(COG_0(2,11))*Tra_ZZ(COG_0(3,11));
 COG_m2L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------
%-- Coxa_L (tornozelo-pe) -> m1
%----------------------------------------------------------------------------------------------------
 T_ind = 13; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(12))*Tra_XX(COG_0(1,12))*Tra_YY(COG_0(2,12))*Tra_ZZ(COG_0(3,12));
 COG_m1L = T(1:3,4); 
%----------------------------------------------------------------------------------------------------  
% TRONCO-CABEÇA: m7-m10
%----------------------------------------------------------------------------------------------------
%-- Anca (secçao da anca) -> m7
%----------------------------------------------------------------------------------------------------
 T_aux = T_aux_cc;
 T_ind = [6:7 15]; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(0)*Tra_XX(COG_0(1,13))*Tra_YY(COG_0(2,13))*Tra_ZZ(COG_0(3,13));
 COG_m7 = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Cintura -> m8
%----------------------------------------------------------------------------------------------------
 T_ind = 16; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end, T_aux_cc = T_aux;
 T = T_aux*Rot_ZZ(q(13))*Tra_XX(COG_0(1,14))*Tra_YY(COG_0(2,14))*Tra_ZZ(COG_0(3,14));
 COG_m8 = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Tronco -> m9
%----------------------------------------------------------------------------------------------------
 T_ind = 17:18; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(14))*Tra_XX(COG_0(1,15))*Tra_YY(COG_0(2,15))*Tra_ZZ(COG_0(3,15));
 COG_m9 = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Pescoço-cabeça -> m10
%----------------------------------------------------------------------------------------------------
 T_ind = 19; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(0)*Tra_XX(COG_0(1,16))*Tra_YY(COG_0(2,16))*Tra_ZZ(COG_0(3,16));
 COG_m10 = T(1:3,4);
%----------------------------------------------------------------------------------------------------  
% BRAÇO DIREITO: m11-m12
%----------------------------------------------------------------------------------------------------
%-- Ombro_R -> m11
%----------------------------------------------------------------------------------------------------
 T_aux = T_aux_cc;
 T_ind = [17 23]; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(0)*Tra_XX(COG_0(1,17))*Tra_YY(COG_0(2,17))*Tra_ZZ(COG_0(3,17));
 COG_m11R = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Braço_R -> m12
%----------------------------------------------------------------------------------------------------
 T_ind = 24; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(18))*Tra_XX(COG_0(1,18))*Tra_YY(COG_0(2,18))*Tra_ZZ(COG_0(3,18));
 COG_m12R = T(1:3,4);
%----------------------------------------------------------------------------------------------------  
% BRAÇO ESQUERDA: m11-m12
%----------------------------------------------------------------------------------------------------
%-- Ombro_L -> m11
%----------------------------------------------------------------------------------------------------
 T_aux = T_aux_cc;
 T_ind = [17 26]; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(0)*Tra_XX(COG_0(1,19))*Tra_YY(COG_0(2,19))*Tra_ZZ(COG_0(3,19));
 COG_m11L = T(1:3,4);
%----------------------------------------------------------------------------------------------------
%-- Braço_L -> m12
%----------------------------------------------------------------------------------------------------
 T_ind = 27; for i=1:length(T_ind); T_aux = T_aux*A(:,:,T_ind(i)); end
 T = T_aux*Rot_ZZ(q(20))*Tra_XX(COG_0(1,20))*Tra_YY(COG_0(2,20))*Tra_ZZ(COG_0(3,20));
 COG_m12L = T(1:3,4);
 
 COG_mi2R = [ COG_m1R COG_m2R COG_m3R COG_m4R COG_m5R COG_m6R ...   %-- Perna direita
              COG_m6L COG_m5L COG_m4L COG_m3L COG_m2L COG_m1L ...   %-- Perna esquerda
              COG_m7 COG_m8 COG_m9 COG_m10 ...                      %-- Tronco-Cabeça
              COG_m11R COG_m12R COG_m11L COG_m12L ];                %-- Braço direito & esquerdo
%----------------------------------------------------------------------------------------------------
%-- Centro de massa
%----------------------------------------------------------------------------------------------------
 COG(1) = ( M(1)*COG_mi2R(1,1) +M(2)*COG_mi2R(1,2) +M(3)*COG_mi2R(1,3) +M(4)*COG_mi2R(1,4)+M(5)*COG_mi2R(1,5)+M(6)*COG_mi2R(1,6)+...
            M(1)*COG_mi2R(1,12)+M(2)*COG_mi2R(1,11)+M(3)*COG_mi2R(1,10)+M(4)*COG_mi2R(1,9)+M(5)*COG_mi2R(1,8)+M(6)*COG_mi2R(1,7)+...
            M(7)*COG_mi2R(1,13)+M(8)*COG_mi2R(1,14)+M(9)*COG_mi2R(1,15)+M(10)*COG_mi2R(1,16)+...
            M(11)*COG_mi2R(1,17)+M(6)*COG_mi2R(1,18)+M(11)*COG_mi2R(1,19)+M(12)*COG_mi2R(1,20) ) / Mt;
        
 COG(2) = ( M(1)*COG_mi2R(2,1) +M(2)*COG_mi2R(2,2) +M(3)*COG_mi2R(2,3) +M(4)*COG_mi2R(2,4)+M(5)*COG_mi2R(2,5)+M(6)*COG_mi2R(2,6)+...
            M(1)*COG_mi2R(2,12)+M(2)*COG_mi2R(2,11)+M(3)*COG_mi2R(2,10)+M(4)*COG_mi2R(2,9)+M(5)*COG_mi2R(2,8)+M(6)*COG_mi2R(2,7)+...
            M(7)*COG_mi2R(2,13)+M(8)*COG_mi2R(2,14)+M(9)*COG_mi2R(2,15)+M(10)*COG_mi2R(2,16)+...
            M(11)*COG_mi2R(2,17)+M(6)*COG_mi2R(2,18)+M(11)*COG_mi2R(2,19)+M(12)*COG_mi2R(2,20) ) / Mt;
        
 COG(3) = ( M(1)*COG_mi2R(3,1) +M(2)*COG_mi2R(3,2) +M(3)*COG_mi2R(3,3) +M(4)*COG_mi2R(3,4)+M(5)*COG_mi2R(3,5)+M(6)*COG_mi2R(3,6)+...
            M(1)*COG_mi2R(3,12)+M(2)*COG_mi2R(3,11)+M(3)*COG_mi2R(3,10)+M(4)*COG_mi2R(3,9)+M(5)*COG_mi2R(3,8)+M(6)*COG_mi2R(3,7)+...
            M(7)*COG_mi2R(3,13)+M(8)*COG_mi2R(3,14)+M(9)*COG_mi2R(3,15)+M(10)*COG_mi2R(3,16)+...
            M(11)*COG_mi2R(3,17)+M(6)*COG_mi2R(3,18)+M(11)*COG_mi2R(3,19)+M(12)*COG_mi2R(3,20) ) / Mt;
 COG = COG';
 
 
%----------------------------------------------------------------------------------------------------
%-- Saida
%----------------------------------------------------------------------------------------------------
 def.COG = COG;
 def.COG_mi2R = COG_mi2R;