Planning with regular optimization kind of works. Update guide path each iteration and do some unintelligent goal re-assignment.