added RRT and RRTConnect, modified Extend