Set a goal configuration in pr2 collaboration scenario