Added support to the sbpl_collision_space for working in a non-kinematics world frame like the map frame.
Added some functions to the occupancy grid used for visualization.
Removed bresenham from collision checker and put in leatherman.
Just added ifndef lines to the BFS_3d
Merge branch 'master' of https://github.com/bcohen/sbpl_manipulation