add init and goal in loop