Added first working RRT implementation