Planning with Real-Time Collision Avoidance for Cooperating Agents under Rigid Body Constraints