Updated to contact reduction; refactoring to fix bug
Updated NQP solver to have framework for anytime solve