function score = fk_score(p)
%
global l1;
global l2;
global x_d;
global y_d;
a1 = p(1);
a2 = p(2);
x = cos(a1)*l1+cos(a1+a2)*l2;
y = sin(a1)*l1+sin(a1+a2)*l2;
score = ( x - x_d )*( x - x_d ) + ( y - y_d )*( y - y_d );