Add init and goal config to robot interface