Visual servo task and motion planning