added test for RRT::Tree.getPath()
fixed some issues in Tree.getPath()