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

%-- Distiller --------------------------------------
 q0 = Q(1); q1 = Q(2); q2 = Q(3);
 l1 = L(1); l2 = L(2);
 r1 = R(1); r2 = R(2);
 m1 = M(1); m2 = M(2); Mt = m1+m2;
 
%-- COG1 -------------------------------------------
 COG_1(1,:) = r1*cos( q0+q1 );
 COG_1(2,:) = r1*sin( q0+q1 );
 
 COG_2(1,:) = l1*cos( q0+q1 )+r2*cos( q0+q1+q2 );
 COG_2(2,:) = l1*sin( q0+q1 )+r2*sin( q0+q1+q2 );

 COG(1,:) = ( m1*COG_1(1,:)+m2*COG_2(1,:) ) / Mt;
 COG(2,:) = ( m1*COG_1(2,:)+m2*COG_2(2,:) ) / Mt;
%---------------------------------------------------
