Adding BiRRT methods to hw4_navplan.py