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); }
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) ); } } } } } } }