Add extra parameter to provide initial agent pos&dest