Collaborative multi-robot autonomy via distributed optimization