Updated main script to use goal directed RRTs