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

 Mt = m0+m1+m2+m3; %Massa total da estrutura
 
%-- COG1  - Center of Gravity
 COG_0(1,:) = r0*cos( q(1,:) )
 COG_0(2,:) = r0*sin( q(1,:) )
 
 COG_1(1,:) = r1*cos( q(2,:) );
 COG_1(2,:) = r1*sin( q(2,:) );
 
 COG_2(1,:) = l1*cos( q(2,:) )+r2*cos( q(2,:)+q(3,:) );
 COG_2(2,:) = l1*sin( q(2,:) )+r2*sin( q(2,:)+q(3,:) );

 COG_3(1,:) = l1*cos( q(2,:) )+l2*cos( q(2,:)+q(3,:) )+r3*cos( q(2,:)+q(3,:)+q(4,:) );
 COG_3(2,:) = l1*sin( q(2,:) )+l2*sin( q(2,:)+q(3,:) )+r3*sin( q(2,:)+q(3,:)+q(4,:) );
 
 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;
 
