Added path length calculations for the RRT Planners