I add the code from hw2 so as to it can run the path shortening and rrt in the code, the content of HeuristicRRTPlanner is indeed forward rrt right now, will try to change to hrrt tomorrow morning