1. Fix the previous SNOPT 43 problem. It was caused by NaN initial values.
2. Now InverseKin and InverseKinTraj override the addNonlinearConstraint, addCost , replaceCost and addDecisionVariable
3. SNOPT support print file