Planning and control for multi-robot manipulation and assembly in unstructured environments