Added obstacle initialization and subtract gradient function