Closed loop motion planning of cooperating mobile robots using graph connectivity