/* * Copyright (c) 2011-2022, The DART development contributors * All rights reserved. * * The list of contributors can be found at: * https://github.com/dartsim/dart/blob/master/LICENSE * * This file is provided under the following "BSD-style" License: * Redistribution and use in source and binary forms, with or * without modification, are permitted provided that the following * conditions are met: * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include "dart/common/sub_ptr.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/RevoluteJoint.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/math/Geometry.hpp" #include "dart/simulation/World.hpp" #include "dart/utils/SkelParser.hpp" #include "TestHelpers.hpp" using namespace dart; using namespace math; using namespace dynamics; using namespace simulation; std::vector getFileList() { std::vector fileList; fileList.push_back("dart://sample/skel/test/chainwhipa.skel"); fileList.push_back("dart://sample/skel/test/single_pendulum.skel"); fileList.push_back( "dart://sample/skel/test/single_pendulum_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/double_pendulum.skel"); fileList.push_back( "dart://sample/skel/test/double_pendulum_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel"); fileList.push_back( "dart://sample/skel/test/serial_chain_revolute_joint.skel"); fileList.push_back( "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel"); fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel"); fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel"); fileList.push_back( "dart://sample/skel/test/simple_tree_structure_euler_joint.skel"); fileList.push_back( "dart://sample/skel/test/simple_tree_structure_ball_joint.skel"); fileList.push_back("dart://sample/skel/test/tree_structure.skel"); fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel"); fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel"); fileList.push_back("dart://sample/skel/fullbody1.skel"); return fileList; } std::vector getSkeletons() { const auto fileList = getFileList(); std::vector worlds; for (std::size_t i = 0; i < fileList.size(); ++i) worlds.push_back(utils::SkelParser::readWorld(fileList[i])); std::vector skeletons; for (std::size_t i = 0; i < worlds.size(); ++i) { WorldPtr world = worlds[i]; for (std::size_t j = 0; j < world->getNumSkeletons(); ++j) skeletons.push_back(world->getSkeleton(j)); } return skeletons; } void constructSubtree(std::vector& _tree, BodyNode* bn) { _tree.push_back(bn); for (std::size_t i = 0; i < bn->getNumChildBodyNodes(); ++i) constructSubtree(_tree, bn->getChildBodyNode(i)); } TEST(Skeleton, Restructuring) { std::vector skeletons = getSkeletons(); #ifndef NDEBUG std::size_t numIterations = 10; #else std::size_t numIterations = 2 * skeletons.size(); #endif for (const auto& skeleton : skeletons) EXPECT_TRUE(skeleton->checkIndexingConsistency()); // Test moves within the current Skeleton for (std::size_t i = 0; i < numIterations; ++i) { std::size_t index = math::Random::uniform(0, skeletons.size() - 1); index = std::min(index, skeletons.size() - 1); SkeletonPtr skeleton = skeletons[index]; EXPECT_TRUE(skeleton->checkIndexingConsistency()); SkeletonPtr original = skeleton->cloneSkeleton(); EXPECT_TRUE(original->checkIndexingConsistency()); std::size_t maxNode = skeleton->getNumBodyNodes() - 1; BodyNode* bn1 = skeleton->getBodyNode(math::Random::uniform(0, maxNode)); BodyNode* bn2 = skeleton->getBodyNode(math::Random::uniform(0, maxNode)); if (bn1 == bn2) { --i; continue; } BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2; BodyNode* parent = child == bn1 ? bn2 : bn1; auto skelVer = skeleton->getVersion(); auto childVer = child->getVersion(); child->moveTo(parent); EXPECT_NE(skeleton->getVersion(), skelVer); EXPECT_NE(child->getVersion(), childVer); EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes()); if (skeleton->getNumBodyNodes() == original->getNumBodyNodes()) { for (std::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 (std::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 (std::size_t i = 0; i < numIterations; ++i) { std::size_t fromIndex = math::Random::uniform(0, skeletons.size() - 1); fromIndex = std::min(fromIndex, skeletons.size() - 1); SkeletonPtr fromSkel = skeletons[fromIndex]; if (fromSkel->getNumBodyNodes() == 0) { --i; continue; } std::size_t toIndex = math::Random::uniform(0, skeletons.size() - 1); toIndex = std::min(toIndex, skeletons.size() - 1); SkeletonPtr toSkel = skeletons[toIndex]; if (toSkel->getNumBodyNodes() == 0) { --i; continue; } BodyNode* childBn = fromSkel->getBodyNode( math::Random::uniform(0, fromSkel->getNumBodyNodes() - 1)); BodyNode* parentBn = toSkel->getBodyNode( math::Random::uniform(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 subtree; constructSubtree(subtree, childBn); // Move to a new Skeleton auto fromSkelVer = fromSkel->getVersion(); auto toSkelVer = toSkel->getVersion(); auto child_ver = childBn->getVersion(); childBn->moveTo(parentBn); EXPECT_NE(fromSkel->getVersion(), fromSkelVer); EXPECT_NE(toSkel->getVersion(), toSkelVer); EXPECT_NE(childBn->getVersion(), child_ver); EXPECT_TRUE(childBn->getSkeleton()->checkIndexingConsistency()); EXPECT_TRUE(parentBn->getSkeleton()->checkIndexingConsistency()); // Make sure all the objects have moved for (std::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 originalJoint = childBn->getParentJoint(); childBn->moveTo(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 (std::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(fromSkel, originalParent); SkeletonPtr temporary = childBn->split("temporary"); SkeletonPtr other_temporary = childBn->split("other temporary"); SkeletonPtr another_temporary = childBn->copyAs("another temporary"); SkeletonPtr last_temporary = childBn->copyAs("last temporary"); childBn->copyTo(another_temporary->getBodyNode( another_temporary->getNumBodyNodes() - 1)); childBn->copyTo(another_temporary->getBodyNode(0)); childBn->copyTo(temporary, nullptr); childBn->moveTo( last_temporary, last_temporary->getBodyNode(last_temporary->getNumBodyNodes() - 1)); childBn->moveTo(last_temporary, nullptr); childBn->moveTo(last_temporary, last_temporary->getBodyNode(0)); childBn->changeParentJointType(); // Test the non-recursive copying if (toSkel->getNumBodyNodes() > 1) { SkeletonPtr singleBodyNode = toSkel->getBodyNode(0)->copyAs("single", false); EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1); std::pair singlePair = toSkel->getBodyNode(0)->copyTo(nullptr, false); EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0); } // Check that the mangled Skeletons are all self-consistent EXPECT_TRUE(fromSkel->checkIndexingConsistency()); EXPECT_TRUE(toSkel->checkIndexingConsistency()); EXPECT_TRUE(temporary->checkIndexingConsistency()); EXPECT_TRUE(other_temporary->checkIndexingConsistency()); EXPECT_TRUE(another_temporary->checkIndexingConsistency()); EXPECT_TRUE(last_temporary->checkIndexingConsistency()); } } 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 pair = skeleton->createJointAndBodyNodePair( 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 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); } class GenericNode final : public dart::dynamics::Node, public AccessoryNode { public: GenericNode(BodyNode* bn, const std::string& name) : Node(bn), mName(name) {} const std::string& setName(const std::string& newName) override { mName = registerNameChange(newName); return mName; } const std::string& getName() const override { return mName; } protected: Node* cloneNode(BodyNode* bn) const override { return new GenericNode(bn, mName); } std::string mName; }; TEST(Skeleton, NodePersistence) { SkeletonPtr skel = Skeleton::create(); skel->createJointAndBodyNodePair(nullptr); //-------------------------------------------------------------------------- // Testing EndEffector, which is a specialized Node type //-------------------------------------------------------------------------- { EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip"); // Test both methods of adding a Support to an EndEffector manip->createAspect(); manip->createSupport(); EXPECT_EQ(skel->getEndEffector("manip"), manip); EXPECT_EQ(skel->getEndEffector(0), manip); // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip); WeakEndEffectorPtr weakManip = manip; EXPECT_NE(weakManip.lock(), nullptr); manip->remove(); // The Node has been removed, and no strong reference to it exists, so it // should be gone from the Skeleton EXPECT_EQ(skel->getEndEffector("manip"), nullptr); EXPECT_EQ(skel->getNumEndEffectors(), 0u); // EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u); EXPECT_EQ(weakManip.lock(), nullptr); } { EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip"); EXPECT_EQ(skel->getEndEffector("manip"), manip); EXPECT_EQ(skel->getEndEffector(0), manip); // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip); EndEffectorPtr strongManip = manip; WeakEndEffectorPtr weakManip = strongManip; EXPECT_FALSE(weakManip.lock() == nullptr); manip->remove(); // The Node has been removed, so no reference to it will exist in the // Skeleton #ifdef NDEBUG // Release Mode EXPECT_NE(skel->getEndEffector("manip"), manip); EXPECT_EQ(skel->getEndEffector("manip"), nullptr); EXPECT_NE(skel->getEndEffector(0), manip); EXPECT_EQ(skel->getEndEffector(0), nullptr); #endif // Release Mode #ifdef NDEBUG // Release Mode // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. // EXPECT_NE(skel->getBodyNode(0)->getEndEffector(0), manip); // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), nullptr); #endif // Release Mode EXPECT_NE(weakManip.lock(), nullptr); strongManip = nullptr; // The Node has been removed, and no strong reference to it exists any // longer, so it should be gone from the Skeleton EXPECT_EQ(skel->getEndEffector("manip"), nullptr); EXPECT_EQ(skel->getNumEndEffectors(), 0u); // EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u); EXPECT_EQ(weakManip.lock(), nullptr); } using GenericNodePtr = TemplateNodePtr; using WeakGenericNodePtr = TemplateWeakNodePtr; //-------------------------------------------------------------------------- // Testing GenericNode, which is NOT a specialized Node type //-------------------------------------------------------------------------- { GenericNode* node = skel->getBodyNode(0)->createNode("node"); EXPECT_EQ(skel->getNode("node"), node); EXPECT_EQ(skel->getNode(0), node); EXPECT_EQ(skel->getBodyNode(0)->getNode(0), node); WeakGenericNodePtr weakNode = node; EXPECT_NE(weakNode.lock(), nullptr); node->remove(); // The Node has been removed, and no strong reference to it exists, so it // should be gone from the Skeleton EXPECT_EQ(skel->getNode("node"), nullptr); EXPECT_EQ(skel->getNumNodes(), 0u); EXPECT_EQ(skel->getBodyNode(0)->getNumNodes(), 0u); EXPECT_EQ(weakNode.lock(), nullptr); } { GenericNode* node = skel->getBodyNode(0)->createNode("node"); EXPECT_EQ(skel->getNode("node"), node); EXPECT_EQ(skel->getNode(0), node); EXPECT_EQ(skel->getBodyNode(0)->getNode(0), node); GenericNodePtr strongNode = node; WeakGenericNodePtr weakNode = strongNode; EXPECT_FALSE(weakNode.lock() == nullptr); node->remove(); // The Node has been removed, so no reference to it will exist in the // Skeleton #ifdef NDEBUG // Release Mode EXPECT_NE(skel->getNode("node"), node); EXPECT_EQ(skel->getNode("node"), nullptr); EXPECT_NE(skel->getNode(0), node); EXPECT_EQ(skel->getNode(0), nullptr); #endif // Release Mode #ifdef NDEBUG // Release Mode // But it will not remain in the BodyNode's indexing. // Note: We should only run this test in release mode, because otherwise it // will trigger an assertion. EXPECT_NE(skel->getBodyNode(0)->getNode(0), node); EXPECT_EQ(skel->getBodyNode(0)->getNode(0), nullptr); #endif // Release Mode EXPECT_NE(weakNode.lock(), nullptr); strongNode = nullptr; // The Node has been removed, and no strong reference to it exists any // longer, so it should be gone from the Skeleton EXPECT_EQ(skel->getNode("node"), nullptr); EXPECT_EQ(skel->getNumNodes(), 0u); EXPECT_EQ(skel->getBodyNode(0)->getNumNodes(), 0u); EXPECT_EQ(weakNode.lock(), nullptr); } } TEST(Skeleton, CloneNodeOrdering) { // This test checks that the ordering of Nodes in a cloned Skeleton will match // the ordering of Nodes in the original that was copied. SkeletonPtr skel = Skeleton::create(); skel->createJointAndBodyNodePair(nullptr); skel->createJointAndBodyNodePair(nullptr); skel->createJointAndBodyNodePair(nullptr); // Add Nodes in the reverse order, so that their indexing is different from // the BodyNodes they are attached to for (int i = skel->getNumBodyNodes() - 1; i > 0; --i) { skel->getBodyNode(i)->createEndEffector("manip_" + std::to_string(i)); } skel->getBodyNode(1)->createEndEffector("other_manip"); skel->getBodyNode(0)->createEndEffector("another_manip"); skel->getBodyNode(2)->createEndEffector("yet_another_manip"); SkeletonPtr clone = skel->cloneSkeleton(); for (std::size_t i = 0; i < skel->getNumEndEffectors(); ++i) { EXPECT_EQ( skel->getEndEffector(i)->getName(), clone->getEndEffector(i)->getName()); } } 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().second; BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; bn = skel->createJointAndBodyNodePair(zeroDof1).second; BodyNode* zeroDof2 = skel->createJointAndBodyNodePair(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, ZeroDofJointConstraintForces) { // This is a regression test which makes sure that the BodyNodes of // ZeroDofJoints will be correctly aggregate constraint forces. SkeletonPtr skel = Skeleton::create(); BodyNode* bn = skel->createJointAndBodyNodePair().second; BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; bn = skel->createJointAndBodyNodePair(zeroDof1).second; skel->createJointAndBodyNodePair(bn); const auto numSkelDofs = skel->getNumDofs(); for (auto& bodyNode : skel->getBodyNodes()) bodyNode->setConstraintImpulse(Eigen::Vector6d::Random()); // Make sure this does not cause seg-fault Eigen::VectorXd constraintForces = skel->getConstraintForces(); EXPECT_EQ(constraintForces.size(), static_cast(numSkelDofs)); } TEST(Skeleton, Configurations) { SkeletonPtr twoLink = createTwoLinkRobot( Vector3d::Ones(), DOF_YAW, Vector3d::Ones(), DOF_ROLL); SkeletonPtr threeLink = createThreeLinkRobot( Vector3d::Ones(), DOF_PITCH, Vector3d::Ones(), DOF_ROLL, Vector3d::Ones(), DOF_YAW); Skeleton::Configuration c2 = twoLink->getConfiguration(); Skeleton::Configuration c3 = threeLink->getConfiguration(); EXPECT_FALSE(c2 == c3); EXPECT_TRUE(c2 == c2); EXPECT_TRUE(c3 == c3); EXPECT_TRUE(c2 != c3); twoLink->setPosition(0, 1.0); EXPECT_FALSE(c2 == twoLink->getConfiguration()); threeLink->setPosition(1, 2.0); EXPECT_FALSE(c3 == twoLink->getConfiguration()); c2 = twoLink->getConfiguration(Skeleton::CONFIG_VELOCITIES); EXPECT_TRUE(c2.mPositions.size() == 0); EXPECT_TRUE(c2.mVelocities.size() == static_cast(twoLink->getNumDofs())); EXPECT_TRUE(c2.mAccelerations.size() == 0); } TEST(Skeleton, LinearJacobianDerivOverload) { // Regression test for #626: Make sure that getLinearJacobianDeriv's overload // is working appropriately. SkeletonPtr skeleton = createThreeLinkRobot( Vector3d::Ones(), DOF_PITCH, Vector3d::Ones(), DOF_ROLL, Vector3d::Ones(), DOF_YAW); skeleton->getLinearJacobianDeriv(skeleton->getBodyNode(0)); LinkagePtr linkage = Branch::create(skeleton->getBodyNode(0)); linkage->getLinearJacobianDeriv(linkage->getBodyNode(0)); } TEST(Skeleton, Updating) { // Make sure that structural properties get automatically updated correctly // RevoluteJoint SkeletonPtr skeleton = createTwoLinkRobot( Vector3d::Ones(), DOF_PITCH, Vector3d::Ones(), DOF_ROLL); Joint* joint0 = skeleton->getJoint(0); Joint* joint1 = skeleton->getJoint(1); math::Jacobian J0i = joint0->getRelativeJacobian(); joint0->get()->setProperties( joint1->get()->getProperties()); math::Jacobian J0f = joint0->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); // PrismaticJoint skeleton = createTwoLinkRobot(Vector3d::Ones(), DOF_X, Vector3d::Ones(), DOF_Y); joint0 = skeleton->getJoint(0); joint1 = skeleton->getJoint(1); J0i = joint0->getRelativeJacobian(); joint0->get()->setProperties( joint1->get()->getProperties()); J0f = joint0->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); skeleton = Skeleton::create(); ScrewJoint* screw = skeleton->createJointAndBodyNodePair().first; screw->setAxis(Eigen::Vector3d::UnitX()); screw->setPitch(2); J0i = screw->getRelativeJacobian(); screw->setAxis(Eigen::Vector3d::UnitY()); J0f = screw->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); J0i = J0f; screw->setPitch(3); J0f = screw->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); // Regression test for Pull Request #731 const double originalMass = skeleton->getMass(); BodyNode* lastBn = skeleton->getBodyNode(skeleton->getNumBodyNodes() - 1); const double removedMass = lastBn->getMass(); EXPECT_FALSE(removedMass == 0.0); lastBn->remove(); const double newMass = skeleton->getMass(); EXPECT_FALSE(originalMass == newMass); EXPECT_TRUE(newMass == originalMass - removedMass); }