add the pseudo code to the HeuristicRRTPlanner.py