added cover tree caching for the RRT planner (still working out bugs), fixed many bugs in the cover tree implementation