Added KDTree class outline
Added definition of Cell class.
Added core functionality for KDTree class.
Added kd_tree.cpp file to CMakeLists.txt
Added ur5_motion_plan::duration field to filewrite and fileread
Merge branch 'dev' of https://github.com/mrkris13/apple-crusher into dev
Added KDTree core functionality.
Seems to be working.
Added KDTree member to TrajectoryLibrary.
Works in parralel with plan_group scheme for storing plans