//==============================================================================
std::vector<BodyNode*> Linkage::Criteria::climbToCommonRoot(
const Target& _start, const Target& _target,
bool _chain) const
{
BodyNode* start_bn = _start.mNode.lock();
BodyNode* target_bn = _target.mNode.lock();
BodyNode* root = start_bn->getParentBodyNode();
while(root != nullptr)
{
if(target_bn->descendsFrom(root))
break;
root = root->getParentBodyNode();
}
std::vector<BodyNode*> bnStart = climbToTarget(start_bn, root);
trimBodyNodes(bnStart, _chain, true);
if(root != nullptr && bnStart.back() != root)
{
// We did not reach the common root, so we should stop here
return bnStart;
}
std::vector<BodyNode*> bnTarget = climbToTarget(target_bn, root);
std::reverse(bnTarget.begin(), bnTarget.end());
trimBodyNodes(bnTarget, _chain, false);
std::vector<BodyNode*> bnAll;
bnAll.reserve(bnStart.size() + bnTarget.size());
bnAll.insert(bnAll.end(), bnStart.begin(), bnStart.end());
bnAll.insert(bnAll.end(), bnTarget.begin(), bnTarget.end());
return bnAll;
}
//==============================================================================
std::vector<BodyNode*> Linkage::Criteria::climbToTarget(
BodyNode* _start, BodyNode* _target) const
{
std::vector<BodyNode*> newBns;
newBns.reserve(_start->getSkeleton()->getNumBodyNodes());
BodyNode* bn = _start;
BodyNode* finalBn = nullptr==_target? nullptr : _target->getParentBodyNode();
while( bn != finalBn && bn != nullptr )
{
newBns.push_back(bn);
bn = bn->getParentBodyNode();
}
return newBns;
}
TEST(Skeleton, Restructuring)
{
std::vector<SkeletonPtr> skeletons = getSkeletons();
#ifndef NDEBUG
size_t numIterations = 10;
#else
size_t numIterations = 2*skeletons.size();
#endif
// Test moves within the current Skeleton
for(size_t i=0; i<numIterations; ++i)
{
size_t index = floor(math::random(0, skeletons.size()));
index = std::min(index, skeletons.size()-1);
SkeletonPtr skeleton = skeletons[index];
SkeletonPtr original = skeleton->clone();
size_t maxNode = skeleton->getNumBodyNodes()-1;
BodyNode* bn1 = skeleton->getBodyNode(floor(math::random(0, maxNode)));
BodyNode* bn2 = skeleton->getBodyNode(ceil(math::random(0, maxNode)));
if(bn1 == bn2)
{
--i;
continue;
}
BodyNode* child = bn1->descendsFrom(bn2)? bn1 : bn2;
BodyNode* parent = child == bn1? bn2 : bn1;
child->moveTo(parent);
EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
if(skeleton->getNumBodyNodes() == original->getNumBodyNodes())
{
for(size_t j=0; j<skeleton->getNumBodyNodes(); ++j)
{
// Make sure no BodyNodes have been lost or gained in translation
std::string name = original->getBodyNode(j)->getName();
BodyNode* bn = skeleton->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
name = skeleton->getBodyNode(j)->getName();
bn = original->getBodyNode(name);
EXPECT_FALSE(bn == nullptr);
if(bn)
EXPECT_TRUE(bn->getName() == name);
// Make sure no Joints have been lost or gained in translation
name = original->getJoint(j)->getName();
Joint* joint = skeleton->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
name = skeleton->getJoint(j)->getName();
joint = original->getJoint(name);
EXPECT_FALSE(joint == nullptr);
if(joint)
EXPECT_TRUE(joint->getName() == name);
}
}
EXPECT_TRUE(skeleton->getNumDofs() == original->getNumDofs());
for(size_t j=0; j<skeleton->getNumDofs(); ++j)
{
std::string name = original->getDof(j)->getName();
DegreeOfFreedom* dof = skeleton->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
name = skeleton->getDof(j)->getName();
dof = original->getDof(name);
EXPECT_FALSE(dof == nullptr);
if(dof)
EXPECT_TRUE(dof->getName() == name);
}
}
// Test moves between Skeletons
for(size_t i=0; i<numIterations; ++i)
{
size_t fromIndex = floor(math::random(0, skeletons.size()));
fromIndex = std::min(fromIndex, skeletons.size()-1);
SkeletonPtr fromSkel = skeletons[fromIndex];
if(fromSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
size_t toIndex = floor(math::random(0, skeletons.size()));
toIndex = std::min(toIndex, skeletons.size()-1);
SkeletonPtr toSkel = skeletons[toIndex];
if(toSkel->getNumBodyNodes() == 0)
{
--i;
continue;
}
BodyNode* childBn = fromSkel->getBodyNode(
floor(math::random(0, fromSkel->getNumBodyNodes()-1)));
BodyNode* parentBn = toSkel->getBodyNode(
floor(math::random(0, toSkel->getNumBodyNodes()-1)));
if(fromSkel == toSkel)
{
if(childBn == parentBn)
{
--i;
continue;
}
if(parentBn->descendsFrom(childBn))
{
BodyNode* tempBn = childBn;
childBn = parentBn;
parentBn = tempBn;
SkeletonPtr tempSkel = fromSkel;
fromSkel = toSkel;
toSkel = tempSkel;
}
}
BodyNode* originalParent = childBn->getParentBodyNode();
std::vector<BodyNode*> subtree;
constructSubtree(subtree, childBn);
// Move to a new Skeleton
childBn->moveTo(parentBn);
// Make sure all the objects have moved
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Move to the Skeleton's root while producing a new Joint type
sub_ptr<Joint> originalJoint = childBn->getParentJoint();
childBn->moveTo<FreeJoint>(nullptr);
// The original parent joint should be deleted now
EXPECT_TRUE(originalJoint == nullptr);
// The BodyNode should now be a root node
EXPECT_TRUE(childBn->getParentBodyNode() == nullptr);
// The subtree should still be in the same Skeleton
for(size_t j=0; j<subtree.size(); ++j)
{
BodyNode* bn = subtree[j];
EXPECT_TRUE(bn->getSkeleton() == toSkel);
}
// Create some new Skeletons and mangle them all up
childBn->copyTo<RevoluteJoint>(fromSkel, originalParent);
SkeletonPtr temporary = childBn->split("temporary");
SkeletonPtr other_temporary =
childBn->split<PrismaticJoint>("other temporary");
SkeletonPtr another_temporary = childBn->copyAs("another temporary");
SkeletonPtr last_temporary = childBn->copyAs<ScrewJoint>("last temporary");
childBn->copyTo(another_temporary->getBodyNode(
another_temporary->getNumBodyNodes()-1));
childBn->copyTo<PlanarJoint>(another_temporary->getBodyNode(0));
childBn->copyTo<TranslationalJoint>(temporary, nullptr);
childBn->moveTo(last_temporary,
last_temporary->getBodyNode(last_temporary->getNumBodyNodes()-1));
childBn->moveTo<BallJoint>(last_temporary, nullptr);
childBn->moveTo<EulerJoint>(last_temporary,
last_temporary->getBodyNode(0));
childBn->changeParentJointType<FreeJoint>();
// Test the non-recursive copying
if(toSkel->getNumBodyNodes() > 1)
{
SkeletonPtr singleBodyNode =
toSkel->getBodyNode(0)->copyAs("single", false);
EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1);
std::pair<Joint*, BodyNode*> singlePair =
toSkel->getBodyNode(0)->copyTo(nullptr, false);
EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0);
}
// Check that the mangled Skeletons are all self-consistent
check_self_consistency(fromSkel);
check_self_consistency(toSkel);
check_self_consistency(temporary);
check_self_consistency(other_temporary);
check_self_consistency(another_temporary);
check_self_consistency(last_temporary);
}
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
// compute c(q)
mC = mSkel->getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
// compute J(q)
Vector4d offset;
offset << mSkel->getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
// w.r.t ankle dofs
BodyNode *node = mSkel->getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent = node->getParentBodyNode()->getTransform().matrix();
Matrix4d parentToJoint = joint->getTransformFromParentBodyNode().matrix();
Matrix4d dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R = joint->getTransform(1).matrix();
Matrix4d jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
Vector4d jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
int colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Update offset so it stores the chain below the parent joint
// w.r.t knee dof
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
// w.r.t hip dofs
node = node->getParentBodyNode();
joint = node->getParentJoint();
worldToParent = node->getParentBodyNode()->getTransform().matrix();
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R1 = joint->getTransform(1).matrix();
Matrix4d R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
// Current code only works for the left leg with only one constraint
VectorXd MyWorld::updateGradients() {
mJ.setZero();
mC.setZero();
// compute c(q)
//std::cout << "HAMYDEBUG: mConstrainedMarker = " << getMarker(mConstrainedMarker) << std::endl;
mC = getMarker(mConstrainedMarker)->getWorldPosition() - mTarget;
std::cout << "C(q) = " << mC << std::endl;
// compute J(q)
Vector4d offset;
offset << getMarker(mConstrainedMarker)->getLocalPosition(), 1; // Create a vector in homogeneous coordinates
//Setup vars
BodyNode *node = getMarker(mConstrainedMarker)->getBodyNode();
Joint *joint = node->getParentJoint();
Matrix4d worldToParent;
Matrix4d parentToJoint;
//Declare Vars
Matrix4d dR; // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
Matrix4d R;
Matrix4d R1;
Matrix4d R2;
Matrix4d jointToChild;
Vector4d jCol;
int colIndex;
//TODO: Might want to change this to check if root using given root fcn
//Iterate until we get to the root node
while(true) {
//std::cout << "HAMY DEBUG: Beginning new looop" << std::endl;
if(node->getParentBodyNode() == NULL) {
worldToParent = worldToParent.setIdentity();
} else {
worldToParent = node->getParentBodyNode()->getTransform().matrix();
}
parentToJoint = joint->getTransformFromParentBodyNode().matrix();
// Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
//TODO: R1, R2, ... Rn code depending on DOFs
int nDofs = joint->getNumDofs();
//std::cout << "HAMY: nDofs=" << nDofs << std::endl;
//Can only have up to 3 DOFs on any one piece
switch(nDofs){
case 1: //std::cout << "HAMY: 1 nDOF" << std::endl;
dR = joint->getTransformDerivative(0);
jCol = worldToParent * parentToJoint * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
offset = parentToJoint * joint->getTransform(0).matrix() * jointToChild * offset;
break;
case 2: //std::cout << "HAMY: 2 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R = joint->getTransform(1).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of jCol
dR = joint->getTransformDerivative(1);
R = joint->getTransform(0).matrix();
jCol = worldToParent * parentToJoint * R * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * jointToChild * offset; // Upd
break;
case 3: //std::cout << "HAMY: 3 nDOF" << std::endl;
dR = joint->getTransformDerivative(0); // Doesn't need .matrix() because it returns a Matrix4d instead of Isometry3d
R1 = joint->getTransform(1).matrix();
R2 = joint->getTransform(2).matrix();
jointToChild = joint->getTransformFromChildBodyNode().inverse().matrix();
jCol = worldToParent * parentToJoint * dR * R1 * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(0);
mJ.col(colIndex) = jCol.head(3); // Take the first 3 elelemtns of J
R1 = joint->getTransform(0).matrix();
dR = joint->getTransformDerivative(1);
R2 = joint->getTransform(2).matrix();
jCol = worldToParent * parentToJoint * R1 * dR * R2 * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(1);
mJ.col(colIndex) = jCol.head(3);
R1 = joint->getTransform(0).matrix();
R2 = joint->getTransform(1).matrix();
dR = joint->getTransformDerivative(2);
jCol = worldToParent * parentToJoint * R1 * R2 * dR * jointToChild * offset;
colIndex = joint->getIndexInSkeleton(2);
mJ.col(colIndex) = jCol.head(3);
offset = parentToJoint * joint->getTransform(0).matrix() * joint->getTransform(1).matrix() * joint->getTransform(2).matrix() * jointToChild * offset;
break;
default: //std::cout << "HAMY: Unexpected nDOF = " << nDofs << std::endl;
break;
}
if(node != mSkel->getRootBodyNode()) {
//std::cout << "HAMY DEBUG: Not root, continue loop" << std::endl;
node = node->getParentBodyNode(); // return NULL if node is the root node
joint = node->getParentJoint();
} else {
break;
}
}
// compute gradients
VectorXd gradients = 2 * mJ.transpose() * mC;
return gradients;
}
//==============================================================================
TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
{
// -- set up the root BodyNode
BodyNode* root = new BodyNode("root");
WeldJoint* rootjoint = new WeldJoint("base");
root->setParentJoint(rootjoint);
// -- set up the FreeJoint
BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
FreeJoint* freejoint = new FreeJoint("freejoint");
freejoint_bn->setParentJoint(freejoint);
root->addChildBodyNode(freejoint_bn);
freejoint->setTransformFromParentBodyNode(random_transform());
freejoint->setTransformFromChildBodyNode(random_transform());
// -- set up the EulerJoint
BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
EulerJoint* eulerjoint = new EulerJoint("eulerjoint");
eulerjoint_bn->setParentJoint(eulerjoint);
root->addChildBodyNode(eulerjoint_bn);
eulerjoint->setTransformFromParentBodyNode(random_transform());
eulerjoint->setTransformFromChildBodyNode(random_transform());
// -- set up the BallJoint
BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
BallJoint* balljoint = new BallJoint("balljoint");
balljoint_bn->setParentJoint(balljoint);
root->addChildBodyNode(balljoint_bn);
balljoint->setTransformFromParentBodyNode(random_transform());
balljoint->setTransformFromChildBodyNode(random_transform());
// -- set up Skeleton and compute forward kinematics
Skeleton* skel = new Skeleton;
skel->addBodyNode(root);
skel->addBodyNode(freejoint_bn);
skel->addBodyNode(eulerjoint_bn);
skel->addBodyNode(balljoint_bn);
skel->init();
// Test a hundred times
for(size_t n=0; n<100; ++n)
{
// -- convert transforms to positions and then positions back to transforms
Eigen::Isometry3d desired_freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
freejoint->getPositions());
Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
eulerjoint->getPositions());
Eigen::Isometry3d desired_balljoint_tf = random_transform();
desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(desired_balljoint_tf.linear()));
Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
balljoint->getPositions());
skel->computeForwardKinematics(true, false, false);
// -- collect everything so we can cycle through the tests
std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> desired_tfs;
std::vector<Eigen::Isometry3d> actual_tfs;
joints.push_back(freejoint);
bns.push_back(freejoint_bn);
desired_tfs.push_back(desired_freejoint_tf);
actual_tfs.push_back(actual_freejoint_tf);
joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
desired_tfs.push_back(desired_eulerjoint_tf);
actual_tfs.push_back(actual_eulerjoint_tf);
joints.push_back(balljoint);
bns.push_back(balljoint_bn);
desired_tfs.push_back(desired_balljoint_tf);
actual_tfs.push_back(actual_balljoint_tf);
for(size_t i=0; i<joints.size(); ++i)
{
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = desired_tfs[i];
bool check_transform_conversion =
equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix());
EXPECT_TRUE(check_transform_conversion);
if(!check_transform_conversion)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
}
bool check_full_cycle = equals(desired_tfs[i].matrix(),
actual_tfs[i].matrix());
EXPECT_TRUE(check_full_cycle);
if(!check_full_cycle)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Desired:\n" << desired_tfs[i].matrix()
<< "\n\nActual:\n" << actual_tfs[i].matrix()
<< "\n\n";
}
}
}
}
