initial checkin, which shows rough modificiations to
http://sertac.scripts.mit.edu/rrtstar/software/ to perform incremental updates
and move the vehicle