Update RRTPlanner.py

add code to calculate path length