add infeasibility test to testIKtraj. Also remove TaskSpaceConstraint which we never finished implementing