% Matlab example of gravity compensation

syms G real
syms a1 a2
syms m1 m2 real
syms l1 l1cm l2cm real

x1 = l1cm*cos( a1 )
y1 = l1cm*sin( a1 )

xe = l1*cos( a1 )
ye = l1*sin( a1 )

x2 = xe + l2cm*cos( a1 + a2 )
y2 = ye + l2cm*sin( a1 + a2 )

PE = m1*G*y1 + m2*G*y2
% G*m2*(l2cm*sin(a1 + a2) + l1*sin(a1)) + G*l1cm*m1*sin(a1)

diff( PE, a1 )
% G*m2*(l2cm*cos(a1 + a2) + l1*cos(a1)) + G*l1cm*m1*cos(a1)

diff( PE, a2 )
% G*l2cm*m2*cos(a1 + a2)
