Merge pull request #3 from avalenzu/kinematic-planner

Adds to planning infrastructure for LeggedRobot classes
Remove some humanoid specific code from LeggedRobPlanProb
Add smoothDistancePenaltymex

This contains a C++ implementation of the guts of MinDistanceConstraint
Add support for vector v_max in LRPP
Clean up collision constraints in LRPP