initial planning code with and with out obstacle