function c = cost(angles)
%COST: function to do two joint inverse kinematics using optimization.
% Link lengths and desired tip position are set by caller.
  global L1 L2 XD YD;
  c1 = cos(angles(1));
  s1 = sin(angles(1));
  c12 = cos(angles(1) + angles(2));
  s12 = sin(angles(1) + angles(2));
  x = L1*c1 + L2*c12;
  y = L1*s1 + L2*s12;
  c = (x-XD)*(x-XD) + (y-YD)*(y-YD);