RRT Planner improved a bit