Add WholeBodyPlanner and test.
Add WeightedSquaredError and test
Add WeightedSquaredFirstDiffError and test
WholeBodyPlanner uses weighted sq. error clases.
Remove printouts from MATLAB MinDistanceConstraint
Add v as decision variable in WholeBodyPlanner
Add sole frames to atlas_chull.urdf
Special case TSRBM in kinematicPathJoints  ...

in WorldEulerConstraint and WorldPositionConstraint.
Add v and dv to WholeBodyPlanner.

Works for reaching and walking!
Add AngularMomentumConstraint and test.