move 'infeasible_constraint' to NonlinearProgram from InverseKinematics