function hq = h(q)
global Xnew L ell
hq = [L*cos(q(1))+ell*cos(q(1)+q(2));
      L*sin(q(1))+ell*sin(q(1)+q(2))];
end