Changed include format
Added DaVinci Kinematics
Davinci follow mode Testing/Debugging
Needs refactoring, but stable
Minor refactoring
Adding plane data and vf
Few changes in plane constraint; Now user can also pass a frame representing a plane
Added some comments and reformatted to fit optimizer changes
Merging fork to jhu-saw
Merge branch 'pwilken3-master'