Adding RRT connect algorithm