/*
* s_j + r CT + 2*|R| CT (3-c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_i + d_i (16)
* s_i + 2*|R| CT (2+c_{i,j}^{k1,k2,r}-m_i^k1-m_j^k2) \geq s_j + d_j + r CT (17)
*/
void ConstraintsGenerator::addCollisions(const map1to1& s, const map1to1& d, const map2to1& x, const map2to1& y, const map4toN& c) {
assert(mRobots.size() > 1 && mLine != nullptr && "More than one robot expected!");
for (const pair<ActivityMode*, ActivityMode*>& col : mLine->collisions()) {
int32_t numberOfRobots = mRobots.size();
for (int32_t r = -numberOfRobots; r <= numberOfRobots; ++r) {
Operator op = GREATER_EQUAL;
SparseMatrix<double>::Row row;
double constVal = 2.0*numberOfRobots*mCycleTime;
double b1 = -3.0*constVal-r*mCycleTime, b2 = -2.0*constVal+r*mCycleTime;
uint32_t a[2], m[2], i = 0;
for (ActivityMode* mode : {col.first, col.second}) {
Movement *mv = dynamic_cast<Movement*>(mode);
if (mv != nullptr) {
DynamicActivity *da = mv->parent();
a[i] = da->aid(); m[i] = mv->mid();
row.emplace_back(getValue(y, {a[i], m[i]}, caller()), -constVal);
}
Location *loc = dynamic_cast<Location*>(mode);
if (loc != nullptr) {
StaticActivity *sa = loc->parent();
a[i] = sa->aid(); m[i] = loc->lid();
row.emplace_back(getValue(x, {a[i], loc->point()}, caller()), -constVal);
}
++i;
}
SparseMatrix<double>::Row row1 = row, row2 = row;
const vector<uint32_t>& v = getValue(c, {pack(a[0], m[0]), pack(a[1], m[1])}, caller());
row1.insert(row1.end(), {{getValue(s, a[1], caller()), 1.0}, {getValue(s, a[0], caller()), -1.0}, {getValue(d, a[0], caller()), -1.0}, {v[r+numberOfRobots], -constVal}});
row2.insert(row2.end(), {{getValue(s, a[0], caller()), 1.0}, {getValue(s, a[1], caller()), -1.0}, {getValue(d, a[1], caller()), -1.0}, {v[r+numberOfRobots], constVal}});
addConstraint(row1, op, b1, "(16$"+to_string(mConstraintsCounter++)+")");
addConstraint(row2, op, b2, "(17$"+to_string(mConstraintsCounter++)+")");
}
}
}
DynamicActivity* XmlReader::processDynamicActivityNode(const Node *node) {
const Element *activityElement = castToElement(node);
DynamicActivity *dynamicActivity = new DynamicActivity(stoul(getAttribute(activityElement, "aid")));
dynamicActivity->name(getTextFromElement(activityElement, "name", false));
dynamicActivity->description(getTextFromElement(activityElement, "desc", false));
double minDuration = F64_MAX, maxDuration = F64_MIN;
const Element *movements = castToElement(activityElement->get_first_child("movements"));
string a1 = getAttribute(movements, "from_aid", false), a2 = getAttribute(movements, "to_aid", false);
int64_t fromActivity = (a1.empty() ? -1 : stol(a1)), toActivity = (a2.empty() ? -1 : stol(a2));
const NodeSet movementSet = movements->find("movement");
for (const Node *movementNode : movementSet) {
const Element *movementElement = castToElement(movementNode);
Movement *mv = new Movement(stoul(getAttribute(movementElement, "mid")));
mv->minDuration(stod(getTextFromElement(movementElement, "min-duration")));
mv->maxDuration(stod(getTextFromElement(movementElement, "max-duration")));
if (mv->minDuration() < TIME_ERR) {
string mid = to_string(mv->mid());
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Dynamic activity's movement "+mid+" must have positive duration!", movementNode->get_line());
}
// Workaround the incapability of ILP solvers to solve the problem with the fixed variables.
if (abs(mv->maxDuration()-mv->minDuration()) < TIME_ERR)
mv->maxDuration(mv->minDuration()+TIME_ERR);
const NodeSet monomials = movementElement->find("energy-function/monomial");
for (const Node *monomialNode : monomials) {
const Element *monomialElement = castToElement(monomialNode);
int32_t degree = stoi(getAttribute(monomialElement, "degree"));
double coeff = stod(getAttribute(monomialElement, "coeff"));
mv->addMonomial({degree, coeff});
}
uint32_t fromPoint = stoul(getTextFromElement(movementElement, "from-point"));
uint32_t toPoint = stoul(getTextFromElement(movementElement, "to-point"));
Location *locFrom = nullptr, *locTo = nullptr;
StaticActivity *actFrom = nullptr, *actTo = nullptr;
auto fSit = mPointToLocation.find(fromPoint), tSit = mPointToLocation.find(toPoint);
if (fSit == mPointToLocation.cend() || tSit == mPointToLocation.cend()) {
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Dynamic activity's point is not linked with any static activity!", movementNode->get_line());
} else {
locFrom = fSit->second;
locTo = tSit->second;
assert(locFrom != nullptr && locTo != nullptr && "Unexpected null pointers!");
actFrom = locFrom->parent();
actTo = locTo->parent();
assert(actFrom != nullptr && actTo != nullptr && "Should not be nullptr as it has been set in processRobotNode method!");
}
if ((actFrom->aid() != fromActivity && fromActivity != -1) || (actTo->aid() != toActivity && toActivity != -1)) {
delete dynamicActivity;
delete mv;
throw InvalidDatasetFile(caller(), "Optional attributes 'from_aid' and/or 'to_aid' are invalid!", movementNode->get_line());
}
minDuration = min(minDuration, mv->minDuration());
maxDuration = max(maxDuration, mv->maxDuration());
setValue(mMovementToPoints, mv, {fromPoint, toPoint}, caller());
dynamicActivity->addMovement(mv);
}
dynamicActivity->minAbsDuration(minDuration);
dynamicActivity->maxAbsDuration(maxDuration);
return dynamicActivity;
}