Collaborative Motion Planning For Multi-Agent Systems