function [COG_1, COG_2, COG_3, COG] = RobCOG( q,l1,l2,l3,r1,r2,r3,m1,m2,m3 )

 Mt = m1+m2+m3;
 
%-- COG1 -------------------------------------------------------------------------
% Coordenadas do centro de gravidade do elo 1: 
COG_1(1,:) = r1*cos( q(1,:) );
COG_1(2,:) = r1*sin( q(1,:) ); 
 
% Coordenadas do centro de gravidade do elo 2:
 COG_2(1,:) = l1*cos( q(1,:) )+r2*cos( q(1,:)+q(2,:) );
 COG_2(2,:) = l1*sin( q(1,:) )+r2*sin( q(1,:)+q(2,:) );

 % Coordenadas do centro de gravidade do tronco:
 COG_3(1,:) = l1*cos( q(1,:) )+l2*cos( q(1,:)+q(2,:) )+r3*cos( q(1,:)+q(2,:)+q(3,:) );
 COG_3(2,:) = l1*sin( q(1,:) )+l2*sin( q(1,:)+q(2,:) )+r3*sin( q(1,:)+q(2,:)+q(3,:) );
 
 % Calculo das coordenadas do centro de gravidade total:
 COG(1,:) = ( m1*COG_1(1,:)+m2*COG_2(1,:)+m3*COG_3(1,:) ) / Mt;
 COG(2,:) = ( m1*COG_1(2,:)+m2*COG_2(2,:)+m3*COG_3(2,:) ) / Mt;
 
