Working animation except for a discontinuity issue for the link positioning equations