bool BodyVerifier::fixRobotBody(RobotRepresentation &robot) { bool changed = false; while (true) { // check velidity of body int errorCode; std::vector<std::pair<std::string, std::string> > offenders; if (!BodyVerifier::verify(robot, errorCode, offenders)) { // TODO treat other cases: Arduino constraints, missing core if (errorCode == BodyVerifier::SELF_INTERSECTION) { std::cerr << "Robot body has following intersection pairs:" << std::endl; for (unsigned int i = 0; i < offenders.size(); ++i) { // get robots IdPartMap: Volatile, so needs update const RobotRepresentation::IdPartMap idPartMap = robot.getBody(); std::cerr << offenders[i].first << " with " << offenders[i].second << std::endl; // check if offending body part hasn't been removed yet if (idPartMap.find(offenders[i].first) == idPartMap.end() || idPartMap.find(offenders[i].second) == idPartMap.end()) { continue; } // will remove body part with less descendants (i.e. this // covers the case where one part descends from the other) unsigned int numDesc[] = { idPartMap.find(offenders[i].first)->second.lock()->numDescendants(), idPartMap.find(offenders[i].second)->second.lock()->numDescendants() }; std::cout << offenders[i].first << " has " << numDesc[0] << " descendants" << std::endl; std::cout << offenders[i].second << " has " << numDesc[1] << " descendants" << std::endl; if (numDesc[0] > numDesc[1]) { robot.trimBodyAt(offenders[i].second); std::cout << "Removing latter" << std::endl; } else { robot.trimBodyAt(offenders[i].first); std::cout << "Removing former" << std::endl; } changed = true; } } if (errorCode == BodyVerifier::INTERNAL_ERROR) { std::cout << "Body verification failed due to internal error!" << std::endl; return false; } } else { break; } } return changed; }