void check_self_consistency(SkeletonPtr skeleton) { for(size_t i=0; i<skeleton->getNumBodyNodes(); ++i) { BodyNode* bn = skeleton->getBodyNode(i); EXPECT_TRUE(bn->getIndexInSkeleton() == i); EXPECT_TRUE(skeleton->getBodyNode(bn->getName()) == bn); Joint* joint = bn->getParentJoint(); EXPECT_TRUE(skeleton->getJoint(joint->getName()) == joint); for(size_t j=0; j<joint->getNumDofs(); ++j) { DegreeOfFreedom* dof = joint->getDof(j); EXPECT_TRUE(dof->getIndexInJoint() == j); EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof); } } for(size_t i=0; i<skeleton->getNumDofs(); ++i) { DegreeOfFreedom* dof = skeleton->getDof(i); EXPECT_TRUE(dof->getIndexInSkeleton() == i); EXPECT_TRUE(skeleton->getDof(dof->getName()) == dof); } }
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) { // Set the color of all the shapes in the object for(size_t i=0; i < object->getNumBodyNodes(); ++i) { BodyNode* bn = object->getBodyNode(i); for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j) bn->getVisualizationShape(j)->setColor(color); } }
SkeletonPtr createHubo() { dart::utils::DartLoader loader; loader.addPackageDirectory("drchubo", DART_DATA_PATH"/urdf/drchubo"); SkeletonPtr hubo = loader.parseSkeleton(DART_DATA_PATH"/urdf/drchubo/drchubo.urdf"); for(size_t i = 0; i < hubo->getNumBodyNodes(); ++i) { BodyNode* bn = hubo->getBodyNode(i); if(bn->getName().substr(0, 7) == "Body_LF" || bn->getName().substr(0, 7) == "Body_RF" || bn->getName().substr(0, 7) == "Body_NK") { bn->remove(); --i; } } hubo->setPosition(5, 0.97); for(size_t i=1; i < hubo->getNumJoints(); ++i) { hubo->getJoint(i)->setActuatorType(Joint::VELOCITY); } for(size_t i=0; i < hubo->getNumBodyNodes(); ++i) { BodyNode* bn = hubo->getBodyNode(i); for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j) { const ShapePtr& shape = bn->getVisualizationShape(j); shape->setColor(Eigen::Vector3d(0.2, 0.2, 0.2)); if(MeshShapePtr mesh = std::dynamic_pointer_cast<MeshShape>(shape)) mesh->setColorMode(MeshShape::SHAPE_COLOR); } } hubo->setName("drchubo"); return hubo; }
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color) { // Set the color of all the shapes in the object for(size_t i=0; i < object->getNumBodyNodes(); ++i) { BodyNode* bn = object->getBodyNode(i); auto visualShapeNodes = bn->getShapeNodesWith<VisualAddon>(); for(auto visualShapeNode : visualShapeNodes) visualShapeNode->getVisualAddon()->setColor(color); } }
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 enableDragAndDrops(osgDart::Viewer& viewer, const SkeletonPtr& atlas) { // Turn on drag-and-drop for the whole Skeleton for(size_t i=0; i < atlas->getNumBodyNodes(); ++i) viewer.enableDragAndDrop(atlas->getBodyNode(i), false, false); for(size_t i=0; i < atlas->getNumEndEffectors(); ++i) { EndEffector* ee = atlas->getEndEffector(i); if(!ee->getIK()) continue; // Check whether the target is an interactive frame, and add it if it is if(const auto& frame = std::dynamic_pointer_cast<osgDart::InteractiveFrame>( ee->getIK()->getTarget())) viewer.enableDragAndDrop(frame.get()); } }
//============================================================================== TEST(SdfParser, SDFSingleBodyWithoutJoint) { // Regression test for #444 WorldPtr world = SdfParser::readSdfFile( DART_DATA_PATH"/sdf/test/single_bodynode_skeleton.world"); EXPECT_TRUE(world != nullptr); SkeletonPtr skel = world->getSkeleton(0); EXPECT_TRUE(skel != nullptr); EXPECT_EQ(skel->getNumBodyNodes(), 1u); EXPECT_EQ(skel->getNumJoints(), 1u); BodyNodePtr bodyNode = skel->getBodyNode(0); EXPECT_TRUE(bodyNode != nullptr); EXPECT_EQ(bodyNode->getNumVisualizationShapes(), 1u); EXPECT_EQ(bodyNode->getNumCollisionShapes(), 1u); JointPtr joint = skel->getJoint(0); EXPECT_TRUE(joint != nullptr); EXPECT_EQ(joint->getType(), FreeJoint::getStaticType()); }
TEST(Skeleton, Group) { SkeletonPtr skel = constructLinkageTestSkeleton(); // Make twice as many BodyNodes in the Skeleton SkeletonPtr skel2 = constructLinkageTestSkeleton(); skel2->getRootBodyNode()->moveTo(skel, nullptr); // Test nullptr construction GroupPtr nullGroup = Group::create("null_group", nullptr); EXPECT_EQ(nullGroup->getNumBodyNodes(), 0u); EXPECT_EQ(nullGroup->getNumJoints(), 0u); EXPECT_EQ(nullGroup->getNumDofs(), 0u); // Test conversion from Skeleton GroupPtr skel1Group = Group::create("skel1_group", skel); EXPECT_EQ(skel1Group->getNumBodyNodes(), skel->getNumBodyNodes()); EXPECT_EQ(skel1Group->getNumJoints(), skel->getNumJoints()); EXPECT_EQ(skel1Group->getNumDofs(), skel->getNumDofs()); for(size_t i=0; i < skel1Group->getNumBodyNodes(); ++i) EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i)); for(size_t i=0; i < skel1Group->getNumJoints(); ++i) EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i)); for(size_t i=0; i < skel1Group->getNumDofs(); ++i) EXPECT_EQ(skel1Group->getDof(i), skel->getDof(i)); // Test arbitrary Groups by plucking random BodyNodes, Joints, and // DegreesOfFreedom from a Skeleton. GroupPtr group = Group::create(); std::vector<BodyNode*> bodyNodes; std::vector<Joint*> joints; std::vector<DegreeOfFreedom*> dofs; for(size_t i=0; i < 2*skel->getNumBodyNodes(); ++i) { size_t randomIndex = floor(random(0, skel->getNumBodyNodes())); BodyNode* bn = skel->getBodyNode(randomIndex); if(group->addBodyNode(bn, false)) bodyNodes.push_back(bn); randomIndex = floor(random(0, skel->getNumJoints())); Joint* joint = skel->getJoint(randomIndex); if(group->addJoint(joint, false, false)) joints.push_back(joint); randomIndex = floor(random(0, skel->getNumDofs())); DegreeOfFreedom* dof = skel->getDof(randomIndex); if(group->addDof(dof, false, false)) dofs.push_back(dof); } EXPECT_EQ(group->getNumBodyNodes(), bodyNodes.size()); EXPECT_EQ(group->getNumJoints(), joints.size()); EXPECT_EQ(group->getNumDofs(), dofs.size()); for(size_t i=0; i < group->getNumBodyNodes(); ++i) EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]); for(size_t i=0; i < group->getNumJoints(); ++i) EXPECT_EQ(group->getJoint(i), joints[i]); for(size_t i=0; i < group->getNumDofs(); ++i) EXPECT_EQ(group->getDof(i), dofs[i]); }
TEST(Skeleton, Persistence) { WeakBodyNodePtr weakBnPtr; SoftBodyNodePtr softBnPtr; WeakSoftBodyNodePtr weakSoftBnPtr; WeakSkeletonPtr weakSkelPtr; { BodyNodePtr strongPtr; { { SkeletonPtr skeleton = createThreeLinkRobot( Eigen::Vector3d(1.0, 1.0, 1.0), DOF_X, Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Y, Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Z); weakSkelPtr = skeleton; // Test usability of the BodyNodePtr strongPtr = skeleton->getBodyNode(0); weakBnPtr = strongPtr; ConstBodyNodePtr constPtr = strongPtr; EXPECT_FALSE( strongPtr == nullptr ); EXPECT_FALSE( nullptr == strongPtr ); EXPECT_TRUE( strongPtr == skeleton->getBodyNode(0) ); EXPECT_TRUE( skeleton->getBodyNode(0) == strongPtr ); EXPECT_TRUE( constPtr == strongPtr ); EXPECT_TRUE( weakBnPtr.lock() == strongPtr ); EXPECT_FALSE( strongPtr < constPtr ); EXPECT_FALSE( strongPtr < skeleton->getBodyNode(0) ); EXPECT_FALSE( strongPtr < weakBnPtr.lock() ); EXPECT_FALSE( skeleton->getBodyNode(0) < strongPtr ); EXPECT_FALSE( weakBnPtr.lock() < strongPtr); EXPECT_FALSE( strongPtr > constPtr ); EXPECT_FALSE( strongPtr > skeleton->getBodyNode(0) ); EXPECT_FALSE( strongPtr > weakBnPtr.lock() ); EXPECT_FALSE( skeleton->getBodyNode(0) > strongPtr ); EXPECT_FALSE( weakBnPtr.lock() > strongPtr ); BodyNodePtr tail = skeleton->getBodyNode(skeleton->getNumBodyNodes()-1); std::pair<Joint*, SoftBodyNode*> pair = skeleton->createJointAndBodyNodePair<RevoluteJoint, SoftBodyNode>( tail); softBnPtr = pair.second; weakSoftBnPtr = softBnPtr; WeakBodyNodePtr otherWeakPtr = weakSoftBnPtr; // Test convertability // Test usability of the DegreeOfFreedomPtr DegreeOfFreedomPtr dof = skeleton->getDof(1); WeakDegreeOfFreedomPtr weakdof = dof; ConstDegreeOfFreedomPtr const_dof = dof; WeakConstDegreeOfFreedomPtr const_weakdof = weakdof; const_weakdof = const_dof; EXPECT_TRUE( dof == skeleton->getDof(1) ); EXPECT_TRUE( dof == const_dof ); EXPECT_TRUE( weakdof.lock() == const_weakdof.lock() ); EXPECT_TRUE( const_weakdof.lock() == skeleton->getDof(1) ); EXPECT_TRUE( skeleton->getDof(1) == const_weakdof.lock() ); EXPECT_FALSE( dof < const_dof ); EXPECT_FALSE( dof < skeleton->getDof(1) ); EXPECT_FALSE( dof < weakdof.lock() ); EXPECT_FALSE( skeleton->getDof(1) < dof ); EXPECT_FALSE( weakdof.lock() < dof ); EXPECT_FALSE( dof > const_dof ); EXPECT_FALSE( dof > skeleton->getDof(1) ); EXPECT_FALSE( dof > weakdof.lock() ); EXPECT_FALSE( skeleton->getDof(1) > dof ); EXPECT_FALSE( weakdof.lock() > dof ); dof = nullptr; weakdof = nullptr; const_dof = nullptr; const_weakdof = nullptr; EXPECT_TRUE( dof == nullptr ); EXPECT_TRUE( nullptr == dof ); EXPECT_TRUE( weakdof.lock() == nullptr ); EXPECT_TRUE( nullptr == weakdof.lock() ); EXPECT_TRUE( const_dof == nullptr ); EXPECT_TRUE( const_weakdof.lock() == nullptr ); EXPECT_FALSE( dof < const_dof ); // Test usability of the JointPtr JointPtr joint = skeleton->getJoint(1); WeakJointPtr weakjoint = joint; ConstJointPtr const_joint = joint; WeakConstJointPtr const_weakjoint = const_joint; EXPECT_TRUE( joint == skeleton->getJoint(1) ); EXPECT_TRUE( joint == const_joint ); EXPECT_TRUE( weakjoint.lock() == const_weakjoint.lock() ); EXPECT_TRUE( const_weakjoint.lock() == skeleton->getJoint(1) ); EXPECT_FALSE( joint < const_joint ); EXPECT_FALSE( joint < skeleton->getJoint(1) ); EXPECT_FALSE( joint < weakjoint.lock() ); EXPECT_FALSE( skeleton->getJoint(1) < joint ); EXPECT_FALSE( weakjoint.lock() < joint ); EXPECT_FALSE( joint > const_joint ); EXPECT_FALSE( joint > skeleton->getJoint(1) ); EXPECT_FALSE( joint > weakjoint.lock() ); EXPECT_FALSE( skeleton->getJoint(1) > joint ); EXPECT_FALSE( weakjoint.lock() > joint ); joint = nullptr; weakjoint = nullptr; const_joint = nullptr; const_weakjoint = nullptr; EXPECT_TRUE( joint == nullptr ); EXPECT_TRUE( weakjoint.lock() == nullptr ); EXPECT_TRUE( const_joint == nullptr ); EXPECT_TRUE( const_weakjoint.lock() == nullptr ); } // The BodyNode should still be alive, because a BodyNodePtr still // references it EXPECT_FALSE(weakBnPtr.expired()); // The Skeleton should still be alive, because a BodyNodePtr still // references one of its BodyNodes EXPECT_FALSE(weakSkelPtr.lock() == nullptr); // Take the BodyNode out of its Skeleton and put it into a temporary one strongPtr->remove(); // The BodyNode should still be alive, because a BodyNodePtr still // references it EXPECT_FALSE(weakBnPtr.expired()); // The Skeleton should be destroyed, because it lost the only BodyNode // that still had a reference EXPECT_TRUE(weakSkelPtr.lock() == nullptr); // Update the weakSkelPtr so it references the Skeleton that still exists weakSkelPtr = strongPtr->getSkeleton(); EXPECT_FALSE(weakSkelPtr.lock() == nullptr); // Change the BodyNode that this BodyNodePtr is referencing strongPtr = strongPtr->getChildBodyNode(0); // Make sure the Skeleton is still alive. If the SkeletonPtr being used // by the BodyNodePtr is not swapped atomically, then this will fail, // which means we cannot rely on BodyNodePtr to keep BodyNodes alive. EXPECT_FALSE(weakSkelPtr.lock() == nullptr); } SkeletonPtr other_skeleton = createThreeLinkRobot( Eigen::Vector3d(1.0, 1.0, 1.0), DOF_X, Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Y, Eigen::Vector3d(1.0, 1.0, 1.0), DOF_Z); BodyNode* tail = other_skeleton->getBodyNode( other_skeleton->getNumBodyNodes()-1); WeakConstBodyNodePtr weakParentPtr; { ConstBodyNodePtr parent = strongPtr; parent = parent->getParentBodyNode(); weakParentPtr = parent; strongPtr->moveTo(tail); // The Skeleton should still be alive because 'parent' exists EXPECT_FALSE(weakSkelPtr.lock() == nullptr); } // Now that 'parent' is out of scope, the Skeleton should be gone EXPECT_TRUE(weakSkelPtr.lock() == nullptr); EXPECT_TRUE(weakParentPtr.lock() == nullptr); weakBnPtr = strongPtr; weakSkelPtr = strongPtr->getSkeleton(); EXPECT_FALSE(weakBnPtr.expired()); EXPECT_FALSE(weakSkelPtr.expired()); } // softBnPtr still exists, so it should be keeping the Skeleton active EXPECT_FALSE(weakBnPtr.expired()); std::weak_ptr<Skeleton> weakSkel = softBnPtr->remove(); // Now that the SoftBodyNode which is holding the reference has been moved to // another Skeleton, the weakBnPtr and weakSkelPtr should disappear EXPECT_TRUE(weakBnPtr.expired()); EXPECT_TRUE(weakSkelPtr.expired()); // The WeakSoftBodyNodePtr should not have expired yet, because a strong // reference to its SoftBodyNode still exists EXPECT_FALSE(weakSoftBnPtr.expired()); // Test the user-defined copy constructor SoftBodyNodePtr otherSoftBnPtr = softBnPtr; softBnPtr = nullptr; EXPECT_FALSE(weakSkel.lock() == nullptr); EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr); BodyNodePtr strongPtr = otherSoftBnPtr; otherSoftBnPtr = nullptr; BodyNodePtr otherStrongPtr = strongPtr; strongPtr = nullptr; EXPECT_FALSE(weakSkel.lock() == nullptr); EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr); otherStrongPtr = nullptr; // Now that all the strong BodyNodePtrs have been cleared, the // WeakSoftBodyNodePtr should also be cleared EXPECT_TRUE(weakSoftBnPtr.lock() == nullptr); EXPECT_TRUE(weakSkel.lock() == nullptr); }
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); } }