Tagged: Safe guided insertion and extraction of teams into difficult terrain