Updated RRT implementation