BranchListPtr BranchList::removePositionsForLocalRegistration(Eigen::MatrixXd trackingPositions, double maxDistance)
{
BranchListPtr retval = BranchListPtr(new BranchList());
BranchPtr b;
for (int i = 0; i < mBranches.size(); i++)
{
b = BranchPtr(new Branch());
b->setPositions(mBranches[i]->getPositions());
b->setOrientations(mBranches[i]->getOrientations());
retval->addBranch(b);
}
std::vector<BranchPtr> branches = retval->getBranches();
Eigen::MatrixXd positions;
Eigen::MatrixXd orientations;
for (int i = 0; i < branches.size(); i++)
{
positions = branches[i]->getPositions();
orientations = branches[i]->getOrientations();
std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distanceData;
distanceData = dsearchn(positions, trackingPositions);
Eigen::VectorXd distance = distanceData.second;
for (int j = positions.cols() - 1; j >= 0; j--)
{
if (distance(j) > maxDistance)
{
positions = eraseCol(j, positions);
orientations = eraseCol(j, orientations);
}
}
branches[i]->setPositions(positions);
branches[i]->setOrientations(orientations);
}
return retval;
}
unsigned long Branch::querySize() const
{
unsigned long size = 3; // up front overhead: size + move + reply count
for ( unsigned i=0; i < replies.numItems(); i++ )
{
BranchPtr r = replies[i];
size += r->querySize(); // count + branchlist size
}
return size;
}
// Implement Handler::getBranches().
Branches CxUnivarHandler::getBranches(BrCandPtr cand, DoubleVector &x,
RelaxationPtr , SolutionPoolPtr )
{
double minFromBds = 0.1;
BrVarCandPtr vcand = boost::dynamic_pointer_cast <BrVarCand> (cand);
VariablePtr v = vcand->getVar();
double xval = x[v->getIndex()];
double value = xval; // Make sure branch value is not too close to an end point
double len = v->getUb() - v->getLb();
if (value < v->getLb() + minFromBds*len) {
value = v->getLb() + minFromBds*len;
} else if (value > v->getUb() - minFromBds*len) {
value = v->getUb() - minFromBds*len;
}
// can't branch on something that is at its bounds.
if (!(value > v->getLb()+1e-8 && value < v->getUb()-1e-8)) {
std::cerr << "Warning! Branching on variable with bounds/value: [" <<
v->getLb() << " , " << value << " " << v->getUb() << " ]" << std::endl;
//assert(value > v->getLb()+1e-8 && value < v->getUb()-1e-8);
}
Branches branches = (Branches) new BranchPtrVector();
BranchPtr branch = (BranchPtr) new Branch();
VarBoundModPtr mod = (VarBoundModPtr) new VarBoundMod(v, Upper, value);
assert(!"add Mod correctly here.");
branch->addPMod(mod);
branch->setActivity((v->getUb()-value)/len);
branches->push_back(branch);
branch = (BranchPtr) new Branch();
mod = (VarBoundModPtr) new VarBoundMod(v, Lower, value);
assert(!"add Mod correctly here.");
branch->addPMod(mod);
branch->setActivity((value - v->getLb())/len);
branches->push_back(branch);
logger_->msgStream(LogDebug2) << "branching on " << v->getName();
logger_->msgStream(LogDebug2) << " <= " << value << " or "
<< " >= " << value << std::endl;
#if defined(DEBUG_CXUNIVARHANDLER)
std::cout << "branching on " << v->getName();
std::cout << " <= " << value << " or " << " >= " << value << std::endl;
#endif
return branches;
}
TEST(Skeleton, ZeroDofJointInReferential)
{
// This is a regression test which makes sure that the BodyNodes of
// ZeroDofJoints will be correctly included in linkages.
SkeletonPtr skel = Skeleton::create();
BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
BodyNode* zeroDof2 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
BranchPtr branch = Branch::create(skel->getBodyNode(0));
EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes());
EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX);
EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX);
}
void BranchList::selectGenerations(int maxGeneration)
{
std::vector<int> branchNumbersToBeDeleted;
for( int i = 0; i < mBranches.size(); i++ )
{
int generationCounter = 1;
BranchPtr currentBranch = mBranches[i];
while (currentBranch->getParentBranch()){
generationCounter++;
currentBranch = currentBranch->getParentBranch();
if (generationCounter > maxGeneration)
{
branchNumbersToBeDeleted.push_back(i);
break;
}
}
}
for ( int i = branchNumbersToBeDeleted.size() - 1; i >= 0; i-- )
deleteBranch(mBranches[branchNumbersToBeDeleted[i]]);
}
BranchPtr
MultilinearTermsHandler::doBranch_(BranchDirection UpOrDown, ConstVariablePtr v,
double bvalue)
{
BranchPtr branch;
BoundType lu;
VariableType vtype = v->getType();
#if defined(DEBUG_MULTILINEARTERMS_HANDLER)
std::cout << "Branching: " << (UpOrDown == DownBranch ? "Down" : "Up")
<< " at value: " << bvalue << " on: " << std::endl;
v->write(std::cout);
#endif
branch = (BranchPtr) new Branch();
double branching_value = bvalue;
// Change bounds on the x var (called v here)
if (UpOrDown == DownBranch) {
lu = Upper;
if (vtype != Continuous) branching_value = floor(bvalue);
}
else {
lu = Lower;
if (vtype != Continuous) branching_value = ceil(bvalue);
}
VarBoundModPtr vmod = (VarBoundModPtr) new VarBoundMod(v, lu, branching_value);
assert(!"check whether this needs to be addRMod instead");
branch->addPMod(vmod);
branch->setActivity(0.5);// TODO: set this correctly
return branch;
}
BranchPtr CxUnivarHandler::doBranch_(BranchDirection UpOrDown,
ConstVariablePtr v, double bvalue)
{
BranchPtr branch;
LinModsPtr linmods;
#if defined(DEBUG_CXUNIVARHANDLER)
std::cout << "CxUnivarHandler, Branching: " << (UpOrDown == DownBranch ? "Down" : "Up")
<< " at value: " << bvalue << " on: " << std::endl;
v->write(std::cout);
#endif
// Zero out tmpX and grad each time, or else bad things happen
for (UInt j = 0; j < tmpX_.size(); ++j) {
tmpX_[j] = 0.0;
grad_[j] = 0.0;
}
branch = (BranchPtr) new Branch();
linmods = (LinModsPtr) new LinMods();
// Change bounds on the x var (called v here)
if (UpOrDown == DownBranch) {
VarBoundModPtr mod = (VarBoundModPtr) new VarBoundMod(v, Upper, bvalue);
linmods->insert(mod);
// Find *all* cons_data that has v as an input variable.
CxUnivarConstraintIterator dit;
for (dit = cons_data_.begin(); dit != cons_data_.end(); ++dit) {
if ((*dit)->getRInputVar() == v) {
ConstVariablePtr rov = (*dit)->getROutVar();
FunctionPtr fn = (*dit)->getOriginalCon()->getFunction();
int error;
// Change the secant constraint
ConstraintPtr secCon = (*dit)->getSecantCon();
LinearFunctionPtr lf;
FunctionPtr f;
double xlb = v->getLb();
double xub = bvalue;
// TODO: Check the error value!
tmpX_[v->getIndex()] = xlb;
double fxlb = fn->eval(tmpX_, &error);
tmpX_[v->getIndex()] = xub;
double fxub = fn->eval(tmpX_, &error);
tmpX_[v->getIndex()] = 0.0;
// TODO: check/remedy numerical issues in this division
double m = 0.0;
if (xub - xlb > 10e-7) {
m = (fxub - fxlb)/(xub - xlb);
}
double intercept = fxlb - m*xlb;
lf = (LinearFunctionPtr) new LinearFunction();
lf->addTerm(rov, 1.0);
lf->addTerm(v, -m);
LinConModPtr lcmod = (LinConModPtr) new LinConMod(secCon, lf,
-INFINITY,
intercept);
linmods->insert(lcmod);
// Change all linearization constraints
ConstraintVector::iterator lin_it;
for(lin_it = (*dit)->linConsBegin(); lin_it != (*dit)->linConsEnd();
++lin_it) {
ConstraintPtr c = *lin_it;
}
}
}
} else {
VarBoundModPtr mod = (VarBoundModPtr) new VarBoundMod(v, Lower, bvalue);
linmods->insert(mod);
}
assert(!"add Mod correctly here.");
branch->addPMod(linmods);
return branch;
}
void BranchList::findBranchesInCenterline(Eigen::MatrixXd positions)
{
positions = sortMatrix(2,positions);
Eigen::MatrixXd positionsNotUsed = positions;
// int minIndex;
int index;
int splitIndex;
Eigen::MatrixXd::Index startIndex;
BranchPtr branchToSplit;
while (positionsNotUsed.cols() > 0)
{
if (!mBranches.empty())
{
double minDistance = 1000;
for (int i = 0; i < mBranches.size(); i++)
{
std::pair<std::vector<Eigen::MatrixXd::Index>, Eigen::VectorXd> distances;
distances = dsearchn(positionsNotUsed, mBranches[i]->getPositions());
double d = distances.second.minCoeff(&index);
if (d < minDistance)
{
minDistance = d;
branchToSplit = mBranches[i];
startIndex = index;
if (minDistance < 2)
break;
}
}
std::pair<Eigen::MatrixXd::Index, double> dsearchResult = dsearch(positionsNotUsed.col(startIndex) , branchToSplit->getPositions());
splitIndex = dsearchResult.first;
}
else //if this is the first branch. Select the top position (Trachea).
startIndex = positionsNotUsed.cols() - 1;
std::pair<Eigen::MatrixXd,Eigen::MatrixXd > connectedPointsResult = findConnectedPointsInCT(startIndex , positionsNotUsed);
Eigen::MatrixXd branchPositions = connectedPointsResult.first;
positionsNotUsed = connectedPointsResult.second;
if (branchPositions.cols() >= 5) //only include brances of length >= 5 points
{
BranchPtr newBranch = BranchPtr(new Branch());
newBranch->setPositions(branchPositions);
mBranches.push_back(newBranch);
if (mBranches.size() > 1) // do not try to split another branch when the first branch is processed
{
if ((splitIndex + 1 >= 5) && (branchToSplit->getPositions().cols() - splitIndex - 1 >= 5))
//do not split branch if the new branch is close to the edge of the branch
//if the new branch is not close to one of the edges of the
//connected existing branch: Split the existing branch
{
BranchPtr newBranchFromSplit = BranchPtr(new Branch());
Eigen::MatrixXd branchToSplitPositions = branchToSplit->getPositions();
newBranchFromSplit->setPositions(branchToSplitPositions.rightCols(branchToSplitPositions.cols() - splitIndex - 1));
branchToSplit->setPositions(branchToSplitPositions.leftCols(splitIndex + 1));
mBranches.push_back(newBranchFromSplit);
newBranchFromSplit->setParentBranch(branchToSplit);
newBranch->setParentBranch(branchToSplit);
newBranchFromSplit->setChildBranches(branchToSplit->getChildBranches());
branchVector branchToSplitChildren = branchToSplit->getChildBranches();
for (int i = 0; i < branchToSplitChildren.size(); i++)
branchToSplitChildren[i]->setParentBranch(newBranchFromSplit);
branchToSplit->deleteChildBranches();
branchToSplit->addChildBranch(newBranchFromSplit);
branchToSplit->addChildBranch(newBranch);
}
else if (splitIndex + 1 < 5)
// If the new branch is close to the start of the existing
// branch: Connect it to the same position start as the
// existing branch
{
newBranch->setParentBranch(branchToSplit->getParentBranch());
branchToSplit->getParentBranch()->addChildBranch(newBranch);
}
else if (branchToSplit->getPositions().cols() - splitIndex - 1 < 5)
// If the new branch is close to the end of the existing
// branch: Connect it to the end of the existing branch
{
newBranch->setParentBranch(branchToSplit);
branchToSplit->addChildBranch(newBranch);
}
}
}
}
}
TEST(Skeleton, Referential)
{
std::vector<SkeletonPtr> skeletons = getSkeletons();
#ifndef NDEBUG // Debug mode
size_t numIterations = 1;
#else // Release mode
size_t numIterations = 20;
#endif
for(size_t i=0; i<skeletons.size(); ++i)
{
SkeletonPtr skeleton = skeletons[i];
for(size_t j=0; j<skeleton->getNumTrees(); ++j)
{
BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j));
const std::vector<BodyNode*>& skelBns = skeleton->getTreeBodyNodes(j);
EXPECT_TRUE(tree->getNumBodyNodes() == skelBns.size());
for(BodyNode* bn : skelBns)
{
EXPECT_FALSE(tree->getIndexOf(bn) == INVALID_INDEX);
EXPECT_TRUE(tree->getBodyNode(tree->getIndexOf(bn)) == bn);
}
const std::vector<DegreeOfFreedom*>& skelDofs = skeleton->getTreeDofs(j);
EXPECT_TRUE(tree->getNumDofs() == skelDofs.size());
for(DegreeOfFreedom* dof : skelDofs)
{
EXPECT_FALSE(tree->getIndexOf(dof) == INVALID_INDEX);
EXPECT_TRUE(tree->getDof(tree->getIndexOf(dof)) == dof);
}
Eigen::VectorXd q = tree->getPositions();
Eigen::VectorXd dq = tree->getVelocities();
Eigen::VectorXd ddq = tree->getAccelerations();
for(size_t k=0; k<numIterations; ++k)
{
for(int r=0; r<q.size(); ++r)
{
q[r] = math::random(-10, 10);
dq[r] = math::random(-10, 10);
ddq[r] = math::random(-10, 10);
}
tree->setPositions(q);
tree->setVelocities(dq);
tree->setAccelerations(ddq);
EXPECT_TRUE( equals(q, tree->getPositions(), 0.0) );
EXPECT_TRUE( equals(dq, tree->getVelocities(), 0.0) );
EXPECT_TRUE( equals(ddq, tree->getAccelerations(), 0.0) );
const Eigen::MatrixXd& skelMassMatrix = skeleton->getMassMatrix();
const Eigen::MatrixXd& treeMassMatrix = tree->getMassMatrix();
const Eigen::MatrixXd& skelAugM = skeleton->getAugMassMatrix();
const Eigen::MatrixXd& treeAugM = tree->getAugMassMatrix();
const Eigen::MatrixXd& skelInvM = skeleton->getInvMassMatrix();
const Eigen::MatrixXd& treeInvM = tree->getInvMassMatrix();
const Eigen::MatrixXd& skelInvAugM = skeleton->getInvAugMassMatrix();
const Eigen::MatrixXd& treeInvAugM = tree->getInvAugMassMatrix();
const Eigen::VectorXd& skelCvec = skeleton->getCoriolisForces();
const Eigen::VectorXd& treeCvec = tree->getCoriolisForces();
const Eigen::VectorXd& skelFg = skeleton->getGravityForces();
const Eigen::VectorXd& treeFg = tree->getGravityForces();
const Eigen::VectorXd& skelCg = skeleton->getCoriolisAndGravityForces();
const Eigen::VectorXd& treeCg = tree->getCoriolisAndGravityForces();
const Eigen::VectorXd& skelFext = skeleton->getExternalForces();
const Eigen::VectorXd& treeFext = tree->getExternalForces();
const Eigen::VectorXd& skelFc = skeleton->getConstraintForces();
const Eigen::VectorXd& treeFc = tree->getConstraintForces();
const size_t nDofs = tree->getNumDofs();
for(size_t r1=0; r1<nDofs; ++r1)
{
const size_t sr1 = tree->getDof(r1)->getIndexInSkeleton();
for(size_t r2=0; r2<nDofs; ++r2)
{
const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();
EXPECT_TRUE( skelMassMatrix(sr1,sr2) == treeMassMatrix(r1,r2) );
EXPECT_TRUE( skelAugM(sr1,sr2) == treeAugM(r1,r2) );
EXPECT_TRUE( skelInvM(sr1,sr2) == treeInvM(r1,r2) );
EXPECT_TRUE( skelInvAugM(sr1,sr2) == treeInvAugM(r1,r2) );
}
EXPECT_TRUE( skelCvec[sr1] == treeCvec[r1] );
EXPECT_TRUE( skelFg[sr1] == treeFg[r1] );
EXPECT_TRUE( skelCg[sr1] == treeCg[r1] );
EXPECT_TRUE( skelFext[sr1] == treeFext[r1] );
EXPECT_TRUE( skelFext[sr1] == treeFext[r1] );
EXPECT_TRUE( skelFc[sr1] == treeFc[r1] );
}
const size_t numBodyNodes = tree->getNumBodyNodes();
for(size_t m=0; m<numBodyNodes; ++m)
{
const BodyNode* bn = tree->getBodyNode(m);
const Eigen::MatrixXd Jtree = tree->getJacobian(bn);
const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn);
for(size_t r2=0; r2<nDofs; ++r2)
{
const size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();
for(size_t r1=0; r1<6; ++r1)
{
EXPECT_TRUE( Jtree(r1,r2) == Jskel(r1, sr2) );
}
}
}
}
}
}
}
