Add dynamic constraint support

[Levi-Armstrong]

This now brings TrajOptIfopt implementation nearly identical to the legacy trajopt. The major difference between the two is how coefficients are handled. After ready more into how trajopt adds constraints with slack variables it was recommend to avoid applying the coefficient to the value and jacobian and only apply it to the slack variable gradient. This simplifies things within so bounds no longer need to be scaled by the coefficient and coefficients are handled directly within the QP problem class. With this change the coefficient for constraints can be lowered which seems to also improve convergence allowing the Trust region loop to increase as needed instead of starting directly with scaled error and jacobians.

Below is the benchmark table of the tesseract_examples with some running of coefficients. As it can be seen trajopt ifopt implementation is mostly faster than the legacy implementation. Next steps will be to profile and consolidate code within the trajopt_qp_problem to allow parallelization. FYI @marrts @rjoomen @marip8

Example	Contact Manager	First Solve (s)	Remaining Solve Avg. (s)	Count
FreespaceOMPLExample	BulletDiscreteBVHManager	0.069706	0.035393	99
FreespaceOMPLExample	BulletDiscreteSimpleManager	0.043283	0.033011	99
BasicCartesianExample	BulletDiscreteBVHManager	0.075885	0.032785	99
BasicCartesianExample	BulletDiscreteSimpleManager	0.030451	0.026948	99
BasicCartesianExample(Ifopt)	BulletDiscreteBVHManager	0.071611	0.068174	99
BasicCartesianExample(Ifopt)	BulletDiscreteSimpleManager	0.054792	0.054215	99
GlassUprightExample	BulletDiscreteBVHManager	0.244184	0.210232	99
GlassUprightExample	BulletDiscreteSimpleManager	0.205867	0.207110	99
GlassUprightExample(Ifopt)	BulletDiscreteBVHManager	0.183184	0.177540	99
GlassUprightExample(Ifopt)	BulletDiscreteSimpleManager	0.215891	0.173534	99
PuzzlePieceExample	BulletDiscreteBVHManager	13.891928	13.151644	9
PuzzlePieceExample	BulletDiscreteSimpleManager	13.119419	13.081440	9
PuzzlePieceExample(Ifopt)	BulletDiscreteBVHManager	8.398480	8.374175	9
PuzzlePieceExample(Ifopt)	BulletDiscreteSimpleManager	6.759843	6.755391	9
PuzzlePieceAuxillaryAxesExample	BulletDiscreteBVHManager	24.295752	24.122716	9
PuzzlePieceAuxillaryAxesExample	BulletDiscreteSimpleManager	24.228213	24.078156	9
PuzzlePieceAuxillaryAxesExample(Ifopt)	BulletDiscreteBVHManager	19.095992	18.356565	9
PuzzlePieceAuxillaryAxesExample(Ifopt)	BulletDiscreteSimpleManager	18.043015	17.972822	9