-Adding visualizePlan method, and create the first couple parameters (current/goal positions)