caching for RRT works
minor rrt changes
fix RRT caching, sped up search by ~2x