From a102c9699e825843aadca2c79da6843dc48cea0f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 24 Nov 2018 22:55:21 -0800 Subject: [PATCH 01/13] Split skeleton tests and meta-skeleton tests --- unittests/comprehensive/CMakeLists.txt | 3 + unittests/comprehensive/test_MetaSkeleton.cpp | 644 ++++++++++++++++++ unittests/comprehensive/test_Skeleton.cpp | 551 --------------- 3 files changed, 647 insertions(+), 551 deletions(-) create mode 100644 unittests/comprehensive/test_MetaSkeleton.cpp diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index 2e5231bad5b54..da3f4840a5745 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -36,6 +36,9 @@ if(TARGET dart-utils) dart_add_test("comprehensive" test_Skeleton) target_link_libraries(test_Skeleton dart-utils) + dart_add_test("comprehensive" test_MetaSkeleton) + target_link_libraries(test_MetaSkeleton dart-utils) + dart_add_test("comprehensive" test_SoftDynamics) target_link_libraries(test_SoftDynamics dart-utils) diff --git a/unittests/comprehensive/test_MetaSkeleton.cpp b/unittests/comprehensive/test_MetaSkeleton.cpp new file mode 100644 index 0000000000000..e111cfd885e79 --- /dev/null +++ b/unittests/comprehensive/test_MetaSkeleton.cpp @@ -0,0 +1,644 @@ +/* + * Copyright (c) 2011-2018, 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 "TestHelpers.hpp" + +#include "dart/common/sub_ptr.hpp" +#include "dart/math/Geometry.hpp" +#include "dart/utils/SkelParser.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/RevoluteJoint.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/simulation/World.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 skeletons; + for(std::size_t i=0; igetNumSkeletons(); ++j) + skeletons.push_back(world->getSkeleton(j)); + } + + return skeletons; +} + +TEST(Skeleton, Referential) +{ + std::vector skeletons = getSkeletons(); + +#ifndef NDEBUG // Debug mode + std::size_t numIterations = 1; +#else // Release mode + std::size_t numIterations = 20; +#endif + + for(std::size_t i=0; igetJoints(); + EXPECT_TRUE(skeleton->getNumJoints() == skelJoints.size()); + for(auto* joint : skelJoints) + EXPECT_TRUE(skeleton->hasJoint(joint)); + + for(std::size_t j=0; jgetNumTrees(); ++j) + { + BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j)); + + const std::vector& 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); + EXPECT_TRUE(skeleton->hasBodyNode(bn)); + } + + const std::vector& 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(std::size_t k=0; k(-10, 10); + dq[r] = math::Random::uniform(-10, 10); + ddq[r] = math::Random::uniform(-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 std::size_t nDofs = tree->getNumDofs(); + for(std::size_t r1=0; r1getDof(r1)->getIndexInSkeleton(); + for(std::size_t r2=0; r2getDof(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 std::size_t numBodyNodes = tree->getNumBodyNodes(); + for(std::size_t m=0; mgetBodyNode(m); + const Eigen::MatrixXd Jtree = tree->getJacobian(bn); + const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn); + + for(std::size_t r2=0; r2getDof(r2)->getIndexInSkeleton(); + for(std::size_t r1=0; r1<6; ++r1) + { + EXPECT_TRUE( Jtree(r1,r2) == Jskel(r1, sr2) ); + } + } + } + } + } + } +} + +template +BodyNode* addBodyNode(BodyNode* bn, const std::string& name) +{ + BodyNode* result = bn->createChildJointAndBodyNodePair().second; + result->setName(name); + return result; +} + +SkeletonPtr constructLinkageTestSkeleton() +{ + SkeletonPtr skel = Skeleton::create(); + BodyNode* bn = skel->createJointAndBodyNodePair().second; + bn->setName("c1b1"); + bn = addBodyNode(bn, "c1b2"); + + BodyNode* c1b3 = addBodyNode(bn, "c1b3"); + bn = addBodyNode(c1b3, "c2b1"); + bn = addBodyNode(bn, "c2b2"); + addBodyNode(bn, "c2b3"); + + bn = addBodyNode(c1b3, "c3b1"); + bn = addBodyNode(bn, "c3b2"); + BodyNode* c3b3 = addBodyNode(bn, "c3b3"); + bn = addBodyNode(c3b3, "c4b1"); + bn = addBodyNode(bn, "c4b2"); + addBodyNode(bn, "c4b3"); + addBodyNode(c3b3, "c3b4"); + + bn = addBodyNode(c1b3, "c5b1"); + addBodyNode(bn, "c5b2"); + + return skel; +} + +void checkForBodyNodes( + std::size_t& /*count*/, + const ReferentialSkeletonPtr& /*refSkel*/, + const SkeletonPtr& /*skel*/) +{ + // Do nothing +} + +// Variadic function for testing a ReferentialSkeleton for a series of BodyNode +// names +template +void checkForBodyNodes( + std::size_t& count, + const ReferentialSkeletonPtr& refSkel, + const SkeletonPtr& skel, + const std::string& name, + Args ... args) +{ + bool contains = refSkel->getIndexOf(skel->getBodyNode(name)) != INVALID_INDEX; + EXPECT_TRUE(contains); + if(!contains) + { + dtmsg << "The ReferentialSkeleton [" << refSkel->getName() << "] does NOT " + << "contain the BodyNode [" << name << "] of the Skeleton [" + << skel->getName() << "]\n"; + } + + ++count; + checkForBodyNodes(count, refSkel, skel, args...); +} + +template +std::size_t checkForBodyNodes( + const ReferentialSkeletonPtr& refSkel, + const SkeletonPtr& skel, + bool checkCount, + Args ... args) +{ + std::size_t count = 0; + checkForBodyNodes(count, refSkel, skel, args...); + + if(checkCount) + { + bool countValid = (count == refSkel->getNumBodyNodes()); + EXPECT_TRUE(countValid); + if(!countValid) + { + dtmsg << "The number of BodyNodes checked for [" << count << "] " + << "does not equal the number [" << refSkel->getNumBodyNodes() + << "] in the ReferentialSkeleton [" << refSkel->getName() << "]\n"; + } + } + + return count; +} + +void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) +{ + EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints()); + + // Linkages should have the property: + // getJoint(i) == getBodyNode(i)->getParentJoint() + for(std::size_t i=0; i < refSkel->getNumJoints(); ++i) + { + EXPECT_EQ(refSkel->getJoint(i), refSkel->getBodyNode(i)->getParentJoint()); + EXPECT_EQ(refSkel->getIndexOf(refSkel->getJoint(i)), i); + } +} + +TEST(Skeleton, Linkage) +{ + // Test a variety of uses of Linkage::Criteria + SkeletonPtr skel = constructLinkageTestSkeleton(); + + BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree"); + checkForBodyNodes(subtree, skel, true, + "c3b3", "c3b4", "c4b1", "c4b2", "c4b3"); + + ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), + skel->getBodyNode("c3b4"), "midchain"); + checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(midchain); + + Linkage::Criteria criteria; + criteria.mStart = skel->getBodyNode("c5b2"); + criteria.mTargets.push_back( + Linkage::Criteria::Target(skel->getBodyNode("c4b3"))); + LinkagePtr path = Linkage::create(criteria, "path"); + checkForBodyNodes(path, skel, true, "c5b2", "c5b1", "c1b3", "c3b1", "c3b2", + "c3b3", "c4b1", "c4b2", "c4b3"); + checkLinkageJointConsistency(path); + + skel->getBodyNode(0)->copyTo(nullptr); + criteria.mTargets.clear(); + criteria.mStart = skel->getBodyNode("c3b1"); + criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; + criteria.mTargets.push_back( + Linkage::Criteria::Target(skel->getBodyNode("c3b1(1)"), + Linkage::Criteria::UPSTREAM)); + + LinkagePtr combinedTreeBases = Linkage::create(criteria, "combinedTreeBases"); + checkForBodyNodes(combinedTreeBases, skel, true, + "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", + "c3b1(1)", "c1b3(1)", "c2b1(1)", "c2b2(1)", "c2b3(1)", + "c5b1", "c5b2", "c1b2", "c1b1", + "c5b1(1)", "c5b2(1)", "c1b2(1)", "c1b1(1)"); + checkLinkageJointConsistency(combinedTreeBases); + + SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2"); + criteria.mTargets.clear(); + criteria.mTargets.push_back( + Linkage::Criteria::Target(skel2->getBodyNode("c3b1"), + Linkage::Criteria::UPSTREAM)); + LinkagePtr combinedSkelBases = Linkage::create(criteria, "combinedSkelBases"); + std::size_t count = 0; + count += checkForBodyNodes(combinedSkelBases, skel, false, + "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", + "c5b1", "c5b2", "c1b2", "c1b1"); + count += checkForBodyNodes(combinedSkelBases, skel2, false, + "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", + "c5b1", "c5b2", "c1b2", "c1b1"); + EXPECT_TRUE( count == combinedSkelBases->getNumBodyNodes() ); + + ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"), + skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint"); + checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1"); + checkLinkageJointConsistency(downstreamFreeJoint); + + ChainPtr emptyChain = Chain::create(skel->getBodyNode("c1b1"), + skel->getBodyNode("c1b3"), "emptyChain"); + checkForBodyNodes(emptyChain, skel, true); + checkLinkageJointConsistency(emptyChain); + + ChainPtr chainFromNull = Chain::create(nullptr, skel->getBodyNode("c1b2"), + "chainFromNull"); + checkForBodyNodes(chainFromNull, skel, true, "c1b1"); + checkLinkageJointConsistency(chainFromNull); + + ChainPtr upstreamFreeJoint = Chain::create(skel->getBodyNode("c1b3"), + skel->getBodyNode("c1b1"), "upstreamFreeJoint"); + checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upstreamFreeJoint); + + // Using nullptr as the target should bring us towards the root of the tree + ChainPtr upTowardsRoot = + Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot"); + checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upTowardsRoot); + + criteria.mTargets.clear(); + criteria.mTargets.push_back(skel->getBodyNode("c4b3")); + criteria.mStart = skel->getBodyNode("c1b3"); + criteria.mTerminals.push_back(skel->getBodyNode("c3b2")); + LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage"); + checkForBodyNodes(terminatedLinkage, skel, true, + "c1b3", "c3b1", "c3b2"); + checkLinkageJointConsistency(terminatedLinkage); + + criteria.mStart = skel->getBodyNode("c1b1"); + criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM; + criteria.mTargets.clear(); + criteria.mTerminals.clear(); + criteria.mTerminals.push_back( + Linkage::Criteria::Terminal(skel->getBodyNode("c2b1"), false)); + criteria.mTerminals.push_back(skel->getBodyNode("c3b3")); + LinkagePtr terminatedSubtree = Linkage::create(criteria, "terminatedSubtree"); + checkForBodyNodes(terminatedSubtree, skel, true, + "c1b1", "c1b2", "c1b3", "c5b1", + "c5b2", "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(terminatedSubtree); + + criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; + criteria.mStart.mNode = skel->getBodyNode("c3b1"); + LinkagePtr terminatedUpstream = Linkage::create(criteria, "terminatedUpstream"); + checkForBodyNodes(terminatedUpstream, skel, true, + "c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1"); + checkLinkageJointConsistency(terminatedUpstream); +} + +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(std::size_t i=0; i < skel1Group->getNumBodyNodes(); ++i) + EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i)); + + for(std::size_t i=0; i < skel1Group->getNumJoints(); ++i) + EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i)); + + for(std::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 bodyNodes; + std::vector joints; + std::vector dofs; + for(std::size_t i=0; i < 2*skel->getNumBodyNodes(); ++i) + { + std::size_t randomIndex = Random::uniform(0, skel->getNumBodyNodes()-1); + BodyNode* bn = skel->getBodyNode(randomIndex); + if(group->addBodyNode(bn, false)) + bodyNodes.push_back(bn); + + randomIndex = Random::uniform(0, skel->getNumJoints()-1); + Joint* joint = skel->getJoint(randomIndex); + if(group->addJoint(joint, false, false)) + joints.push_back(joint); + + randomIndex = Random::uniform(0, skel->getNumDofs()-1); + 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(std::size_t i=0; i < group->getNumBodyNodes(); ++i) + EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]); + + for(std::size_t i=0; i < group->getNumJoints(); ++i) + EXPECT_EQ(group->getJoint(i), joints[i]); + + for(std::size_t i=0; i < group->getNumDofs(); ++i) + EXPECT_EQ(group->getDof(i), dofs[i]); +} + +TEST(Skeleton, LockSkeletonMutexesWithLockGuard) +{ + // Test a variety of uses of Linkage::Criteria + SkeletonPtr skel = constructLinkageTestSkeleton(); + + BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree"); + checkForBodyNodes(subtree, skel, true, + "c3b3", "c3b4", "c4b1", "c4b2", "c4b3"); + + ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), + skel->getBodyNode("c3b4"), "midchain"); + checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(midchain); + + { + auto mutex = subtree->getLockableReference(); + std::lock_guard lock(*mutex); + } + + { + auto mutex = midchain->getLockableReference(); + std::lock_guard lock(*mutex); + } +} + +//============================================================================== +TEST(Skeleton, GetJointsAndBodyNodes) +{ + auto skelA = Skeleton::create(); + auto skelB = Skeleton::create(); + + BodyNode* bodyNodeA0; + BodyNode* bodyNodeA1; + BodyNode* bodyNodeA2; + + BodyNode* bodyNodeB0; + BodyNode* bodyNodeB1; + BodyNode* bodyNodeB2; + + Joint* jointA0; + Joint* jointA1; + Joint* jointA2; + + Joint* jointB0; + Joint* jointB1; + Joint* jointB2; + + std::tie(jointA0, bodyNodeA0) + = skelA->createJointAndBodyNodePair(); + std::tie(jointA1, bodyNodeA1) + = skelA->createJointAndBodyNodePair(bodyNodeA0); + std::tie(jointA2, bodyNodeA2) + = skelA->createJointAndBodyNodePair(bodyNodeA1); + + std::tie(jointB0, bodyNodeB0) + = skelB->createJointAndBodyNodePair(); + std::tie(jointB1, bodyNodeB1) + = skelB->createJointAndBodyNodePair(bodyNodeB0); + std::tie(jointB2, bodyNodeB2) + = skelB->createJointAndBodyNodePair(bodyNodeB1); + + EXPECT_TRUE(skelA->getNumBodyNodes() == 3u); + EXPECT_TRUE(skelB->getNumBodyNodes() == 3u); + + EXPECT_TRUE(skelA->getJoints().size() == 3u); + EXPECT_TRUE(skelB->getJoints().size() == 3u); + + bodyNodeA0->setName("bodyNode0"); + bodyNodeA1->setName("bodyNode1"); + bodyNodeA2->setName("bodyNode2"); + + bodyNodeB0->setName("bodyNode0"); + bodyNodeB1->setName("bodyNode1"); + bodyNodeB2->setName("bodyNode2"); + + jointA0->setName("joint0"); + jointA1->setName("joint1"); + jointA2->setName("joint2"); + + jointB0->setName("joint0"); + jointB1->setName("joint1"); + jointB2->setName("joint2"); + + EXPECT_TRUE(bodyNodeA0 == skelA->getBodyNode(bodyNodeA0->getName())); + EXPECT_TRUE(bodyNodeA1 == skelA->getBodyNode(bodyNodeA1->getName())); + EXPECT_TRUE(bodyNodeA2 == skelA->getBodyNode(bodyNodeA2->getName())); + + EXPECT_TRUE(bodyNodeB0 == skelB->getBodyNode(bodyNodeB0->getName())); + EXPECT_TRUE(bodyNodeB1 == skelB->getBodyNode(bodyNodeB1->getName())); + EXPECT_TRUE(bodyNodeB2 == skelB->getBodyNode(bodyNodeB2->getName())); + + EXPECT_TRUE(skelA->getBodyNodes("wrong name").empty()); + EXPECT_TRUE(skelB->getBodyNodes("wrong name").empty()); + + EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA0->getName()).size() == 1u); + EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA1->getName()).size() == 1u); + EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA2->getName()).size() == 1u); + + EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB0->getName()).size() == 1u); + EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB1->getName()).size() == 1u); + EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB2->getName()).size() == 1u); + + EXPECT_TRUE(jointA0 == skelA->getJoint(jointA0->getName())); + EXPECT_TRUE(jointA1 == skelA->getJoint(jointA1->getName())); + EXPECT_TRUE(jointA2 == skelA->getJoint(jointA2->getName())); + + EXPECT_TRUE(jointB0 == skelB->getJoint(jointB0->getName())); + EXPECT_TRUE(jointB1 == skelB->getJoint(jointB1->getName())); + EXPECT_TRUE(jointB2 == skelB->getJoint(jointB2->getName())); + + EXPECT_TRUE(skelA->getJoints("wrong name").empty()); + EXPECT_TRUE(skelB->getJoints("wrong name").empty()); + + EXPECT_TRUE(skelA->getJoints(jointA0->getName()).size() == 1u); + EXPECT_TRUE(skelA->getJoints(jointA1->getName()).size() == 1u); + EXPECT_TRUE(skelA->getJoints(jointA2->getName()).size() == 1u); + + EXPECT_TRUE(skelB->getJoints(jointB0->getName()).size() == 1u); + EXPECT_TRUE(skelB->getJoints(jointB1->getName()).size() == 1u); + EXPECT_TRUE(skelB->getJoints(jointB2->getName()).size() == 1u); + + auto group = Group::create(); + group->addBodyNode(bodyNodeA0); + group->addBodyNode(bodyNodeB0); + group->addJoint(jointA0); + group->addJoint(jointB0); + EXPECT_TRUE(group->getJoints("wrong name").empty()); + EXPECT_TRUE(group->getBodyNodes("wrong name").empty()); + EXPECT_TRUE(group->getBodyNode("bodyNode0") == bodyNodeA0 + || group->getBodyNode("bodyNode0") == bodyNodeB0); + EXPECT_TRUE(group->getJoint("joint0") == jointA0 + || group->getJoint("joint0") == jointB0); + EXPECT_EQ(group->getBodyNodes("bodyNode0").size(), 2u); + EXPECT_EQ(group->getBodyNodes("bodyNode1").size(), 0u); + EXPECT_EQ(group->getBodyNodes("bodyNode2").size(), 0u); + EXPECT_EQ(group->getJoints("joint0").size(), 2u); + EXPECT_EQ(group->getJoints("joint1").size(), 0u); + EXPECT_EQ(group->getJoints("joint2").size(), 0u); +} diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index 88db71da8ec5b..026d83ba1603f 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -797,442 +797,6 @@ TEST(Skeleton, ZeroDofJointConstraintForces) EXPECT_EQ(constraintForces.size(), static_cast(numSkelDofs)); } -TEST(Skeleton, Referential) -{ - std::vector skeletons = getSkeletons(); - -#ifndef NDEBUG // Debug mode - std::size_t numIterations = 1; -#else // Release mode - std::size_t numIterations = 20; -#endif - - for(std::size_t i=0; igetJoints(); - EXPECT_TRUE(skeleton->getNumJoints() == skelJoints.size()); - for(auto* joint : skelJoints) - EXPECT_TRUE(skeleton->hasJoint(joint)); - - for(std::size_t j=0; jgetNumTrees(); ++j) - { - BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j)); - - const std::vector& 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); - EXPECT_TRUE(skeleton->hasBodyNode(bn)); - } - - const std::vector& 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(std::size_t k=0; k(-10, 10); - dq[r] = math::Random::uniform(-10, 10); - ddq[r] = math::Random::uniform(-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 std::size_t nDofs = tree->getNumDofs(); - for(std::size_t r1=0; r1getDof(r1)->getIndexInSkeleton(); - for(std::size_t r2=0; r2getDof(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 std::size_t numBodyNodes = tree->getNumBodyNodes(); - for(std::size_t m=0; mgetBodyNode(m); - const Eigen::MatrixXd Jtree = tree->getJacobian(bn); - const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn); - - for(std::size_t r2=0; r2getDof(r2)->getIndexInSkeleton(); - for(std::size_t r1=0; r1<6; ++r1) - { - EXPECT_TRUE( Jtree(r1,r2) == Jskel(r1, sr2) ); - } - } - } - } - } - } -} - -template -BodyNode* addBodyNode(BodyNode* bn, const std::string& name) -{ - BodyNode* result = bn->createChildJointAndBodyNodePair().second; - result->setName(name); - return result; -} - -SkeletonPtr constructLinkageTestSkeleton() -{ - SkeletonPtr skel = Skeleton::create(); - BodyNode* bn = skel->createJointAndBodyNodePair().second; - bn->setName("c1b1"); - bn = addBodyNode(bn, "c1b2"); - - BodyNode* c1b3 = addBodyNode(bn, "c1b3"); - bn = addBodyNode(c1b3, "c2b1"); - bn = addBodyNode(bn, "c2b2"); - addBodyNode(bn, "c2b3"); - - bn = addBodyNode(c1b3, "c3b1"); - bn = addBodyNode(bn, "c3b2"); - BodyNode* c3b3 = addBodyNode(bn, "c3b3"); - bn = addBodyNode(c3b3, "c4b1"); - bn = addBodyNode(bn, "c4b2"); - addBodyNode(bn, "c4b3"); - addBodyNode(c3b3, "c3b4"); - - bn = addBodyNode(c1b3, "c5b1"); - addBodyNode(bn, "c5b2"); - - return skel; -} - -void checkForBodyNodes( - std::size_t& /*count*/, - const ReferentialSkeletonPtr& /*refSkel*/, - const SkeletonPtr& /*skel*/) -{ - // Do nothing -} - -// Variadic function for testing a ReferentialSkeleton for a series of BodyNode -// names -template -void checkForBodyNodes( - std::size_t& count, - const ReferentialSkeletonPtr& refSkel, - const SkeletonPtr& skel, - const std::string& name, - Args ... args) -{ - bool contains = refSkel->getIndexOf(skel->getBodyNode(name)) != INVALID_INDEX; - EXPECT_TRUE(contains); - if(!contains) - { - dtmsg << "The ReferentialSkeleton [" << refSkel->getName() << "] does NOT " - << "contain the BodyNode [" << name << "] of the Skeleton [" - << skel->getName() << "]\n"; - } - - ++count; - checkForBodyNodes(count, refSkel, skel, args...); -} - -template -std::size_t checkForBodyNodes( - const ReferentialSkeletonPtr& refSkel, - const SkeletonPtr& skel, - bool checkCount, - Args ... args) -{ - std::size_t count = 0; - checkForBodyNodes(count, refSkel, skel, args...); - - if(checkCount) - { - bool countValid = (count == refSkel->getNumBodyNodes()); - EXPECT_TRUE(countValid); - if(!countValid) - { - dtmsg << "The number of BodyNodes checked for [" << count << "] " - << "does not equal the number [" << refSkel->getNumBodyNodes() - << "] in the ReferentialSkeleton [" << refSkel->getName() << "]\n"; - } - } - - return count; -} - -void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) -{ - EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints()); - - // Linkages should have the property: - // getJoint(i) == getBodyNode(i)->getParentJoint() - for(std::size_t i=0; i < refSkel->getNumJoints(); ++i) - { - EXPECT_EQ(refSkel->getJoint(i), refSkel->getBodyNode(i)->getParentJoint()); - EXPECT_EQ(refSkel->getIndexOf(refSkel->getJoint(i)), i); - } -} - -TEST(Skeleton, Linkage) -{ - // Test a variety of uses of Linkage::Criteria - SkeletonPtr skel = constructLinkageTestSkeleton(); - - BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree"); - checkForBodyNodes(subtree, skel, true, - "c3b3", "c3b4", "c4b1", "c4b2", "c4b3"); - - ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), - skel->getBodyNode("c3b4"), "midchain"); - checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); - checkLinkageJointConsistency(midchain); - - Linkage::Criteria criteria; - criteria.mStart = skel->getBodyNode("c5b2"); - criteria.mTargets.push_back( - Linkage::Criteria::Target(skel->getBodyNode("c4b3"))); - LinkagePtr path = Linkage::create(criteria, "path"); - checkForBodyNodes(path, skel, true, "c5b2", "c5b1", "c1b3", "c3b1", "c3b2", - "c3b3", "c4b1", "c4b2", "c4b3"); - checkLinkageJointConsistency(path); - - skel->getBodyNode(0)->copyTo(nullptr); - criteria.mTargets.clear(); - criteria.mStart = skel->getBodyNode("c3b1"); - criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; - criteria.mTargets.push_back( - Linkage::Criteria::Target(skel->getBodyNode("c3b1(1)"), - Linkage::Criteria::UPSTREAM)); - - LinkagePtr combinedTreeBases = Linkage::create(criteria, "combinedTreeBases"); - checkForBodyNodes(combinedTreeBases, skel, true, - "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", - "c3b1(1)", "c1b3(1)", "c2b1(1)", "c2b2(1)", "c2b3(1)", - "c5b1", "c5b2", "c1b2", "c1b1", - "c5b1(1)", "c5b2(1)", "c1b2(1)", "c1b1(1)"); - checkLinkageJointConsistency(combinedTreeBases); - - SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2"); - criteria.mTargets.clear(); - criteria.mTargets.push_back( - Linkage::Criteria::Target(skel2->getBodyNode("c3b1"), - Linkage::Criteria::UPSTREAM)); - LinkagePtr combinedSkelBases = Linkage::create(criteria, "combinedSkelBases"); - std::size_t count = 0; - count += checkForBodyNodes(combinedSkelBases, skel, false, - "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", - "c5b1", "c5b2", "c1b2", "c1b1"); - count += checkForBodyNodes(combinedSkelBases, skel2, false, - "c3b1", "c1b3", "c2b1", "c2b2", "c2b3", - "c5b1", "c5b2", "c1b2", "c1b1"); - EXPECT_TRUE( count == combinedSkelBases->getNumBodyNodes() ); - - ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"), - skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint"); - checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1"); - checkLinkageJointConsistency(downstreamFreeJoint); - - ChainPtr emptyChain = Chain::create(skel->getBodyNode("c1b1"), - skel->getBodyNode("c1b3"), "emptyChain"); - checkForBodyNodes(emptyChain, skel, true); - checkLinkageJointConsistency(emptyChain); - - ChainPtr chainFromNull = Chain::create(nullptr, skel->getBodyNode("c1b2"), - "chainFromNull"); - checkForBodyNodes(chainFromNull, skel, true, "c1b1"); - checkLinkageJointConsistency(chainFromNull); - - ChainPtr upstreamFreeJoint = Chain::create(skel->getBodyNode("c1b3"), - skel->getBodyNode("c1b1"), "upstreamFreeJoint"); - checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2"); - checkLinkageJointConsistency(upstreamFreeJoint); - - // Using nullptr as the target should bring us towards the root of the tree - ChainPtr upTowardsRoot = - Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot"); - checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2"); - checkLinkageJointConsistency(upTowardsRoot); - - criteria.mTargets.clear(); - criteria.mTargets.push_back(skel->getBodyNode("c4b3")); - criteria.mStart = skel->getBodyNode("c1b3"); - criteria.mTerminals.push_back(skel->getBodyNode("c3b2")); - LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage"); - checkForBodyNodes(terminatedLinkage, skel, true, - "c1b3", "c3b1", "c3b2"); - checkLinkageJointConsistency(terminatedLinkage); - - criteria.mStart = skel->getBodyNode("c1b1"); - criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM; - criteria.mTargets.clear(); - criteria.mTerminals.clear(); - criteria.mTerminals.push_back( - Linkage::Criteria::Terminal(skel->getBodyNode("c2b1"), false)); - criteria.mTerminals.push_back(skel->getBodyNode("c3b3")); - LinkagePtr terminatedSubtree = Linkage::create(criteria, "terminatedSubtree"); - checkForBodyNodes(terminatedSubtree, skel, true, - "c1b1", "c1b2", "c1b3", "c5b1", - "c5b2", "c3b1", "c3b2", "c3b3"); - checkLinkageJointConsistency(terminatedSubtree); - - criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; - criteria.mStart.mNode = skel->getBodyNode("c3b1"); - LinkagePtr terminatedUpstream = Linkage::create(criteria, "terminatedUpstream"); - checkForBodyNodes(terminatedUpstream, skel, true, - "c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1"); - checkLinkageJointConsistency(terminatedUpstream); -} - -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(std::size_t i=0; i < skel1Group->getNumBodyNodes(); ++i) - EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i)); - - for(std::size_t i=0; i < skel1Group->getNumJoints(); ++i) - EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i)); - - for(std::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 bodyNodes; - std::vector joints; - std::vector dofs; - for(std::size_t i=0; i < 2*skel->getNumBodyNodes(); ++i) - { - std::size_t randomIndex = Random::uniform(0, skel->getNumBodyNodes()-1); - BodyNode* bn = skel->getBodyNode(randomIndex); - if(group->addBodyNode(bn, false)) - bodyNodes.push_back(bn); - - randomIndex = Random::uniform(0, skel->getNumJoints()-1); - Joint* joint = skel->getJoint(randomIndex); - if(group->addJoint(joint, false, false)) - joints.push_back(joint); - - randomIndex = Random::uniform(0, skel->getNumDofs()-1); - 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(std::size_t i=0; i < group->getNumBodyNodes(); ++i) - EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]); - - for(std::size_t i=0; i < group->getNumJoints(); ++i) - EXPECT_EQ(group->getJoint(i), joints[i]); - - for(std::size_t i=0; i < group->getNumDofs(); ++i) - EXPECT_EQ(group->getDof(i), dofs[i]); -} - -TEST(Skeleton, LockSkeletonMutexesWithLockGuard) -{ - // Test a variety of uses of Linkage::Criteria - SkeletonPtr skel = constructLinkageTestSkeleton(); - - BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree"); - checkForBodyNodes(subtree, skel, true, - "c3b3", "c3b4", "c4b1", "c4b2", "c4b3"); - - ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), - skel->getBodyNode("c3b4"), "midchain"); - checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); - checkLinkageJointConsistency(midchain); - - { - auto mutex = subtree->getLockableReference(); - std::lock_guard lock(*mutex); - } - - { - auto mutex = midchain->getLockableReference(); - std::lock_guard lock(*mutex); - } -} - TEST(Skeleton, Configurations) { SkeletonPtr twoLink = createTwoLinkRobot(Vector3d::Ones(), DOF_YAW, @@ -1332,118 +896,3 @@ TEST(Skeleton, Updating) EXPECT_FALSE(originalMass == newMass); EXPECT_TRUE(newMass == originalMass - removedMass); } - -//============================================================================== -TEST(Skeleton, GetJointsAndBodyNodes) -{ - auto skelA = Skeleton::create(); - auto skelB = Skeleton::create(); - - BodyNode* bodyNodeA0; - BodyNode* bodyNodeA1; - BodyNode* bodyNodeA2; - - BodyNode* bodyNodeB0; - BodyNode* bodyNodeB1; - BodyNode* bodyNodeB2; - - Joint* jointA0; - Joint* jointA1; - Joint* jointA2; - - Joint* jointB0; - Joint* jointB1; - Joint* jointB2; - - std::tie(jointA0, bodyNodeA0) - = skelA->createJointAndBodyNodePair(); - std::tie(jointA1, bodyNodeA1) - = skelA->createJointAndBodyNodePair(bodyNodeA0); - std::tie(jointA2, bodyNodeA2) - = skelA->createJointAndBodyNodePair(bodyNodeA1); - - std::tie(jointB0, bodyNodeB0) - = skelB->createJointAndBodyNodePair(); - std::tie(jointB1, bodyNodeB1) - = skelB->createJointAndBodyNodePair(bodyNodeB0); - std::tie(jointB2, bodyNodeB2) - = skelB->createJointAndBodyNodePair(bodyNodeB1); - - EXPECT_TRUE(skelA->getNumBodyNodes() == 3u); - EXPECT_TRUE(skelB->getNumBodyNodes() == 3u); - - EXPECT_TRUE(skelA->getJoints().size() == 3u); - EXPECT_TRUE(skelB->getJoints().size() == 3u); - - bodyNodeA0->setName("bodyNode0"); - bodyNodeA1->setName("bodyNode1"); - bodyNodeA2->setName("bodyNode2"); - - bodyNodeB0->setName("bodyNode0"); - bodyNodeB1->setName("bodyNode1"); - bodyNodeB2->setName("bodyNode2"); - - jointA0->setName("joint0"); - jointA1->setName("joint1"); - jointA2->setName("joint2"); - - jointB0->setName("joint0"); - jointB1->setName("joint1"); - jointB2->setName("joint2"); - - EXPECT_TRUE(bodyNodeA0 == skelA->getBodyNode(bodyNodeA0->getName())); - EXPECT_TRUE(bodyNodeA1 == skelA->getBodyNode(bodyNodeA1->getName())); - EXPECT_TRUE(bodyNodeA2 == skelA->getBodyNode(bodyNodeA2->getName())); - - EXPECT_TRUE(bodyNodeB0 == skelB->getBodyNode(bodyNodeB0->getName())); - EXPECT_TRUE(bodyNodeB1 == skelB->getBodyNode(bodyNodeB1->getName())); - EXPECT_TRUE(bodyNodeB2 == skelB->getBodyNode(bodyNodeB2->getName())); - - EXPECT_TRUE(skelA->getBodyNodes("wrong name").empty()); - EXPECT_TRUE(skelB->getBodyNodes("wrong name").empty()); - - EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA0->getName()).size() == 1u); - EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA1->getName()).size() == 1u); - EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA2->getName()).size() == 1u); - - EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB0->getName()).size() == 1u); - EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB1->getName()).size() == 1u); - EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB2->getName()).size() == 1u); - - EXPECT_TRUE(jointA0 == skelA->getJoint(jointA0->getName())); - EXPECT_TRUE(jointA1 == skelA->getJoint(jointA1->getName())); - EXPECT_TRUE(jointA2 == skelA->getJoint(jointA2->getName())); - - EXPECT_TRUE(jointB0 == skelB->getJoint(jointB0->getName())); - EXPECT_TRUE(jointB1 == skelB->getJoint(jointB1->getName())); - EXPECT_TRUE(jointB2 == skelB->getJoint(jointB2->getName())); - - EXPECT_TRUE(skelA->getJoints("wrong name").empty()); - EXPECT_TRUE(skelB->getJoints("wrong name").empty()); - - EXPECT_TRUE(skelA->getJoints(jointA0->getName()).size() == 1u); - EXPECT_TRUE(skelA->getJoints(jointA1->getName()).size() == 1u); - EXPECT_TRUE(skelA->getJoints(jointA2->getName()).size() == 1u); - - EXPECT_TRUE(skelB->getJoints(jointB0->getName()).size() == 1u); - EXPECT_TRUE(skelB->getJoints(jointB1->getName()).size() == 1u); - EXPECT_TRUE(skelB->getJoints(jointB2->getName()).size() == 1u); - - auto group = Group::create(); - group->addBodyNode(bodyNodeA0); - group->addBodyNode(bodyNodeB0); - group->addJoint(jointA0); - group->addJoint(jointB0); - EXPECT_TRUE(group->getJoints("wrong name").empty()); - EXPECT_TRUE(group->getBodyNodes("wrong name").empty()); - EXPECT_TRUE(group->getBodyNode("bodyNode0") == bodyNodeA0 - || group->getBodyNode("bodyNode0") == bodyNodeB0); - EXPECT_TRUE(group->getJoint("joint0") == jointA0 - || group->getJoint("joint0") == jointB0); - EXPECT_EQ(group->getBodyNodes("bodyNode0").size(), 2u); - EXPECT_EQ(group->getBodyNodes("bodyNode1").size(), 0u); - EXPECT_EQ(group->getBodyNodes("bodyNode2").size(), 0u); - EXPECT_EQ(group->getJoints("joint0").size(), 2u); - EXPECT_EQ(group->getJoints("joint1").size(), 0u); - EXPECT_EQ(group->getJoints("joint2").size(), 0u); -} From 9a4201ad7169904aa23df5398680ae893db2e7a4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 26 Nov 2018 09:40:54 -0800 Subject: [PATCH 02/13] Implementing MetaSkeleton::clone() --- dart/dynamics/Group.cpp | 54 ++++++++++++++++++ dart/dynamics/Group.hpp | 3 + dart/dynamics/Linkage.cpp | 55 +++++++++++++++++++ dart/dynamics/Linkage.hpp | 6 +- dart/dynamics/MetaSkeleton.hpp | 3 + unittests/comprehensive/test_MetaSkeleton.cpp | 51 +++++++++++++++-- 6 files changed, 166 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 0f8d5aefd5007..68b4bd0b0be02 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -68,6 +68,60 @@ GroupPtr Group::create(const std::string& _name, return group; } +//============================================================================== +MetaSkeletonPtr Group::cloneMetaSkeleton() const +{ + // Create an empty Group + GroupPtr newGroup = create(getName(), nullptr); + + if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) + { + dtwarn << "[Group::cloneMetaSkeletonHelper] Attempting to " + << "clone a ReferentialSkeleton that doesn't include any BodyNodes " + << "but including some Joints or DegreeOfFreedoms. This will lead " + << "to dangling Joints or DegreeOfFreedoms in the cloned " + << "ReferentialSkeleton because it only holds the stong reference " + << "to the BodyNodes but not others.\n"; + } + + // Clone skeletons + std::unordered_map map; + for (const Skeleton* skel : mSkeletons) + { + SkeletonPtr skelClone = skel->clone(); + map.insert(std::make_pair(skel, skelClone)); + } + + // Register BodyNode + for (const BodyNodePtr& bodyNode : mBodyNodes) + { + const Skeleton* skel = bodyNode->getSkeleton().get(); + SkeletonPtr& skelClone = map[skel]; + BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName()); + newGroup->registerBodyNode(bodyNodeClone); + } + + // Register Joint + for (const JointPtr& joint : mJoints) + { + const Skeleton* skel = joint->getSkeleton().get(); + SkeletonPtr& skelClone = map[skel]; + Joint* jointClone = skelClone->getJoint(joint->getName()); + newGroup->registerJoint(jointClone); + } + + // Register DegreeOfFreedom + for (const DegreeOfFreedomPtr& dof : mDofs) + { + const Skeleton* skel = dof->getSkeleton().get(); + SkeletonPtr& skelClone = map[skel]; + DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); + newGroup->registerDegreeOfFreedom(dofClone); + } + + return newGroup; +} + //============================================================================== void Group::swapBodyNodeIndices(std::size_t _index1, std::size_t _index2) { diff --git a/dart/dynamics/Group.hpp b/dart/dynamics/Group.hpp index b12e21a0081f0..47f91bf768863 100644 --- a/dart/dynamics/Group.hpp +++ b/dart/dynamics/Group.hpp @@ -70,6 +70,9 @@ class Group : public ReferentialSkeleton /// Destructor virtual ~Group() = default; + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Swap the index of BodyNode _index1 with _index2 void swapBodyNodeIndices(std::size_t _index1, std::size_t _index2); diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 4120544c0890b..48ca08a57510f 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -485,6 +485,61 @@ LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name) return linkage; } +//============================================================================== +Linkage::Criteria::Target getTarget( + const std::unordered_map& map, + const Linkage::Criteria::Target& target) +{ + Linkage::Criteria::Target newTarget; + BodyNodePtr bodyNodePtr = target.mNode.lock(); + if (!bodyNodePtr) + { + newTarget.mNode = WeakBodyNodePtr(); + // NOTE: + } + else + { + const Skeleton* skel = bodyNodePtr->getSkeleton().get(); + SkeletonPtr skelClone = map[skel]; + } + newTarget.mPolicy = target.mPolicy; + newTarget.mChain = target.mChain; + + return newTarget; +} + +//============================================================================== +MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +{ + // Create an empty Linkage + //LinkagePtr newGroup = create(getName(), nullptr); + + if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) + { + dtwarn << "[Linkage::cloneMetaSkeletonHelper] Attempting to " + << "clone a ReferentialSkeleton that doesn't include any BodyNodes " + << "but including some Joints or DegreeOfFreedoms. This will lead " + << "to dangling Joints or DegreeOfFreedoms in the cloned " + << "ReferentialSkeleton because it only holds the stong reference " + << "to the BodyNodes but not others.\n"; + } + + // Clone skeletons + std::unordered_map map; + for (const Skeleton* skel : mSkeletons) + { + SkeletonPtr skelClone = skel->clone(); + map.insert(std::make_pair(skel, skelClone)); + } + + // Create Criteria for creating a new Linkage + mCriteria; + + Criteria newCriteria; + newCriteria.mStart = getTarget(map, mCriteria.mStart); + newCriteria.mTargets; +} + //============================================================================== bool Linkage::isAssembled() const { diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index fab769841008a..aeb7821524726 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -59,7 +59,8 @@ class Linkage : public ReferentialSkeleton { /// The ExpansionPolicy indicates how the collection of BodyNodes should /// expand from the starting BodyNode (mStart) - enum ExpansionPolicy { + enum ExpansionPolicy + { INCLUDE = 0, ///< Do not expand from the target. Include everything up to the target and then stop. EXCLUDE, ///< Do not expand from the target. Include everything up to the target, but NOT the target, and then stop. DOWNSTREAM, ///< Include the target, and then expand downstream, toward the leaves of the tree. @@ -167,6 +168,9 @@ class Linkage : public ReferentialSkeleton static LinkagePtr create(const Criteria& _criteria, const std::string& _name = "Linkage"); + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Returns false if the original assembly of this Linkage has been broken in /// some way bool isAssembled() const; diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index 956d96ff5cb05..3249e21b16a1a 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -71,6 +71,9 @@ class MetaSkeleton : public common::Subject /// Default destructor virtual ~MetaSkeleton() = default; + /// Creates an identical clone of this MetaSkeleton + virtual MetaSkeletonPtr cloneMetaSkeleton() const { return nullptr; } + /// Returns mutex. virtual std::unique_ptr getLockableReference() const = 0; diff --git a/unittests/comprehensive/test_MetaSkeleton.cpp b/unittests/comprehensive/test_MetaSkeleton.cpp index e111cfd885e79..c101fa1abe371 100644 --- a/unittests/comprehensive/test_MetaSkeleton.cpp +++ b/unittests/comprehensive/test_MetaSkeleton.cpp @@ -47,6 +47,7 @@ using namespace math; using namespace dynamics; using namespace simulation; +//============================================================================== std::vector getFileList() { std::vector fileList; @@ -73,6 +74,7 @@ std::vector getFileList() return fileList; } +//============================================================================== std::vector getSkeletons() { const auto fileList = getFileList(); @@ -92,7 +94,8 @@ std::vector getSkeletons() return skeletons; } -TEST(Skeleton, Referential) +//============================================================================== +TEST(MetaSkeleton, Referential) { std::vector skeletons = getSkeletons(); @@ -223,6 +226,7 @@ TEST(Skeleton, Referential) } } +//============================================================================== template BodyNode* addBodyNode(BodyNode* bn, const std::string& name) { @@ -231,6 +235,7 @@ BodyNode* addBodyNode(BodyNode* bn, const std::string& name) return result; } +//============================================================================== SkeletonPtr constructLinkageTestSkeleton() { SkeletonPtr skel = Skeleton::create(); @@ -257,6 +262,7 @@ SkeletonPtr constructLinkageTestSkeleton() return skel; } +//============================================================================== void checkForBodyNodes( std::size_t& /*count*/, const ReferentialSkeletonPtr& /*refSkel*/, @@ -265,6 +271,7 @@ void checkForBodyNodes( // Do nothing } +//============================================================================== // Variadic function for testing a ReferentialSkeleton for a series of BodyNode // names template @@ -288,6 +295,7 @@ void checkForBodyNodes( checkForBodyNodes(count, refSkel, skel, args...); } +//============================================================================== template std::size_t checkForBodyNodes( const ReferentialSkeletonPtr& refSkel, @@ -313,6 +321,7 @@ std::size_t checkForBodyNodes( return count; } +//============================================================================== void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) { EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints()); @@ -326,7 +335,8 @@ void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) } } -TEST(Skeleton, Linkage) +//============================================================================== +TEST(MetaSkeleton, Linkage) { // Test a variety of uses of Linkage::Criteria SkeletonPtr skel = constructLinkageTestSkeleton(); @@ -436,7 +446,8 @@ TEST(Skeleton, Linkage) checkLinkageJointConsistency(terminatedUpstream); } -TEST(Skeleton, Group) +//============================================================================== +TEST(MetaSkeleton, Group) { SkeletonPtr skel = constructLinkageTestSkeleton(); @@ -503,7 +514,8 @@ TEST(Skeleton, Group) EXPECT_EQ(group->getDof(i), dofs[i]); } -TEST(Skeleton, LockSkeletonMutexesWithLockGuard) +//============================================================================== +TEST(MetaSkeleton, LockSkeletonMutexesWithLockGuard) { // Test a variety of uses of Linkage::Criteria SkeletonPtr skel = constructLinkageTestSkeleton(); @@ -529,7 +541,7 @@ TEST(Skeleton, LockSkeletonMutexesWithLockGuard) } //============================================================================== -TEST(Skeleton, GetJointsAndBodyNodes) +TEST(MetaSkeleton, GetJointsAndBodyNodes) { auto skelA = Skeleton::create(); auto skelB = Skeleton::create(); @@ -642,3 +654,32 @@ TEST(Skeleton, GetJointsAndBodyNodes) EXPECT_EQ(group->getJoints("joint1").size(), 0u); EXPECT_EQ(group->getJoints("joint2").size(), 0u); } + +TEST(MetaSkeleton, ReferentialSkeletonHoldsStrongReferencesOfBodyNodes) +{ + // Create skeletons + SkeletonPtr skel1 = constructLinkageTestSkeleton(); + SkeletonPtr skel2 = constructLinkageTestSkeleton(); + EXPECT_TRUE(skel1->getNumBodyNodes() > 1); + EXPECT_TRUE(skel2->getNumBodyNodes() > 2); + + // Create group from the skeletons + GroupPtr group = Group::create(); + group->addBodyNode(skel1->getBodyNode(0)); + group->addBodyNode(skel2->getBodyNode(1)); + EXPECT_EQ(group->getNumBodyNodes(), 2); + + const std::string name1 = group->getBodyNode(0)->getName(); + const std::string name2 = group->getBodyNode(1)->getName(); + EXPECT_EQ(group->getBodyNode(0)->getName(), name1); + EXPECT_EQ(group->getBodyNode(1)->getName(), name2); + + // Release the ownership of skel1 and skel2 + skel1.reset(); + skel2.reset(); + + // However, bodyNodes (and the Skeletons) are still alive because the group is + // alive + EXPECT_EQ(group->getBodyNode(0)->getName(), name1); + EXPECT_EQ(group->getBodyNode(1)->getName(), name2); +} From 0bda3ceeb0264c160d91f368d30392e4d2872c2c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 5 Dec 2018 20:17:17 -0800 Subject: [PATCH 03/13] Add clone for all the concrete MetaSkeletons Next: * Simplify clone functions utilizing Linkage only requires one skeleton clone --- dart/dynamics/Chain.cpp | 42 ++++++++++++++++++++++ dart/dynamics/Chain.hpp | 3 ++ dart/dynamics/Group.cpp | 36 ++++++------------- dart/dynamics/Linkage.cpp | 46 +++++++++--------------- dart/dynamics/Linkage.hpp | 7 ++-- dart/dynamics/ReferentialSkeleton.cpp | 52 +++++++++++++++++++++++++++ dart/dynamics/ReferentialSkeleton.hpp | 6 ++++ 7 files changed, 135 insertions(+), 57 deletions(-) diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index 8a3563d50a845..6079827d8ebfd 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -117,6 +117,48 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, return chain; } +//============================================================================== +MetaSkeletonPtr Chain::cloneMetaSkeleton() const +{ + // Create an empty Group + ChainPtr newGroup = create(nullptr, nullptr, getName()); + + if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) + { + dtwarn << "[Chain::cloneMetaSkeletonHelper] Attempting to " + << "clone a ReferentialSkeleton that doesn't include any BodyNodes " + << "but including some Joints or DegreeOfFreedoms. This will lead " + << "to dangling Joints or DegreeOfFreedoms in the cloned " + << "ReferentialSkeleton because it only holds the stong reference " + << "to the BodyNodes but not others.\n"; + } + + // Array for Skeleton clones that will be collected durig cloning BodyNodes, + // Joints, DegreeOfFreedoms. + // + // The clones will not be destroyed even after the map is destroyed because + // the new Linkage will hold the skeleton by holding the strong referecnes of + // the body nodes. + std::unordered_map mapToSkeletonClones; + mapToSkeletonClones.reserve(mSkeletons.size()); + + newGroup->cloneCriteria(mCriteria, mapToSkeletonClones); + + // Register BodyNode + for (const BodyNodePtr& bodyNode : mBodyNodes) + newGroup->cloneBodyNode(bodyNode.get(), mapToSkeletonClones); + + // Register Joint + for (const JointPtr& joint : mJoints) + newGroup->cloneJoint(joint.get(), mapToSkeletonClones); + + // Register DegreeOfFreedom + for (const DegreeOfFreedomPtr& dof : mDofs) + newGroup->cloneDegreeOfFreedom(dof.get(), mapToSkeletonClones); + + return newGroup; +} + //============================================================================== bool Chain::isStillChain() const { diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index df695f58899e2..f0b2bfb7c98aa 100644 --- a/dart/dynamics/Chain.hpp +++ b/dart/dynamics/Chain.hpp @@ -93,6 +93,9 @@ class Chain : public Linkage static ChainPtr create(BodyNode* _start, BodyNode* _target, IncludeBothTag, const std::string& _name = "Chain"); + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Returns false if this Chain has been broken, or some new Branching has /// been added. bool isStillChain() const; diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 68b4bd0b0be02..6e8a2e41850d3 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -84,40 +84,26 @@ MetaSkeletonPtr Group::cloneMetaSkeleton() const << "to the BodyNodes but not others.\n"; } - // Clone skeletons - std::unordered_map map; - for (const Skeleton* skel : mSkeletons) - { - SkeletonPtr skelClone = skel->clone(); - map.insert(std::make_pair(skel, skelClone)); - } + // Array for Skeleton clones that will be collected durig cloning BodyNodes, + // Joints, DegreeOfFreedoms. + // + // The clones will not be destroyed even after the map is destroyed because + // the new Linkage will hold the skeleton by holding the strong referecnes of + // the body nodes. + std::unordered_map mapToSkeletonClones; + mapToSkeletonClones.reserve(mSkeletons.size()); // Register BodyNode for (const BodyNodePtr& bodyNode : mBodyNodes) - { - const Skeleton* skel = bodyNode->getSkeleton().get(); - SkeletonPtr& skelClone = map[skel]; - BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName()); - newGroup->registerBodyNode(bodyNodeClone); - } + newGroup->cloneBodyNode(bodyNode.get(), mapToSkeletonClones); // Register Joint for (const JointPtr& joint : mJoints) - { - const Skeleton* skel = joint->getSkeleton().get(); - SkeletonPtr& skelClone = map[skel]; - Joint* jointClone = skelClone->getJoint(joint->getName()); - newGroup->registerJoint(jointClone); - } + newGroup->cloneJoint(joint.get(), mapToSkeletonClones); // Register DegreeOfFreedom for (const DegreeOfFreedomPtr& dof : mDofs) - { - const Skeleton* skel = dof->getSkeleton().get(); - SkeletonPtr& skelClone = map[skel]; - DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); - newGroup->registerDegreeOfFreedom(dofClone); - } + newGroup->cloneDegreeOfFreedom(dof.get(), mapToSkeletonClones); return newGroup; } diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 48ca08a57510f..fa92c2b3ac2a6 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -486,8 +486,8 @@ LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name) } //============================================================================== -Linkage::Criteria::Target getTarget( - const std::unordered_map& map, +Linkage::Criteria::Target Linkage::cloneTarget( + std::unordered_map& mapToSkeletonClones, const Linkage::Criteria::Target& target) { Linkage::Criteria::Target newTarget; @@ -500,7 +500,12 @@ Linkage::Criteria::Target getTarget( else { const Skeleton* skel = bodyNodePtr->getSkeleton().get(); - SkeletonPtr skelClone = map[skel]; + auto search = mapToSkeletonClones.find(skel); + assert(search != mapToSkeletonClones.end()); + const SkeletonPtr& skelClone = getOrCloneSkeleton(skel, mapToSkeletonClones); + BodyNode* bodyNodeInClone = skelClone->getBodyNode(bodyNodePtr->getName()); + assert(bodyNodeInClone); + newTarget.mNode = WeakBodyNodePtr(bodyNodeInClone); } newTarget.mPolicy = target.mPolicy; newTarget.mChain = target.mChain; @@ -509,35 +514,16 @@ Linkage::Criteria::Target getTarget( } //============================================================================== -MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +void Linkage::cloneCriteria( + const Criteria& criteria, + std::unordered_map& mapToSkeletonClones) { - // Create an empty Linkage - //LinkagePtr newGroup = create(getName(), nullptr); - - if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) - { - dtwarn << "[Linkage::cloneMetaSkeletonHelper] Attempting to " - << "clone a ReferentialSkeleton that doesn't include any BodyNodes " - << "but including some Joints or DegreeOfFreedoms. This will lead " - << "to dangling Joints or DegreeOfFreedoms in the cloned " - << "ReferentialSkeleton because it only holds the stong reference " - << "to the BodyNodes but not others.\n"; - } - - // Clone skeletons - std::unordered_map map; - for (const Skeleton* skel : mSkeletons) - { - SkeletonPtr skelClone = skel->clone(); - map.insert(std::make_pair(skel, skelClone)); - } - - // Create Criteria for creating a new Linkage - mCriteria; - + // Create Criteria for a Linkage clone Criteria newCriteria; - newCriteria.mStart = getTarget(map, mCriteria.mStart); - newCriteria.mTargets; + newCriteria.mStart = cloneTarget(mapToSkeletonClones, criteria.mStart); + for (const Linkage::Criteria::Target& target : criteria.mTargets) + newCriteria.mTargets.emplace_back(cloneTarget(mapToSkeletonClones, target)); + mCriteria = newCriteria; } //============================================================================== diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index aeb7821524726..3115240a12ee9 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -168,8 +168,7 @@ class Linkage : public ReferentialSkeleton static LinkagePtr create(const Criteria& _criteria, const std::string& _name = "Linkage"); - // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + void cloneCriteria(const Criteria& criteria, std::unordered_map& mapToSkeletonClones); /// Returns false if the original assembly of this Linkage has been broken in /// some way @@ -190,6 +189,10 @@ class Linkage : public ReferentialSkeleton /// Update any metadata needed by the Linkage or its derived classes virtual void update(); + Criteria::Target cloneTarget( + std::unordered_map& mapToSkeletonClones, + const Linkage::Criteria::Target& target); + /// Criteria that defines the structure of this Linkage Criteria mCriteria; diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 456e8bdc3b237..9ca1031b7a219 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -1461,6 +1461,58 @@ void ReferentialSkeleton::updateCaches() mFc = Eigen::VectorXd::Zero(nDofs); } +//============================================================================== +SkeletonPtr ReferentialSkeleton::getOrCloneSkeleton(const Skeleton* skel, std::unordered_map& map) +{ + auto search = map.find(skel); + if (search == map.end()) + { + SkeletonPtr skelClone = skel->clone(); + map.insert(std::make_pair(skel, skelClone)); + return skelClone; + } + else + { + return search->second; + } +} + +//============================================================================== +void ReferentialSkeleton::cloneBodyNode( + const BodyNode* bodyNode, + std::unordered_map& mapToSkeletonClones) +{ + SkeletonPtr skelClone + = getOrCloneSkeleton(bodyNode->getSkeleton().get(), mapToSkeletonClones); + BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName()); + assert(bodyNodeClone); + registerBodyNode(bodyNodeClone); +} + +//============================================================================== +void ReferentialSkeleton::cloneJoint( + const Joint* joint, + std::unordered_map& mapToSkeletonClones) +{ + SkeletonPtr skelClone + = getOrCloneSkeleton(joint->getSkeleton().get(), mapToSkeletonClones); + Joint* jointClone = skelClone->getJoint(joint->getName()); + assert(jointClone); + registerJoint(jointClone); +} + +//============================================================================== +void ReferentialSkeleton::cloneDegreeOfFreedom( + const DegreeOfFreedom* dof, + std::unordered_map& mapToSkeletonClones) +{ + SkeletonPtr skelClone + = getOrCloneSkeleton(dof->getSkeleton().get(), mapToSkeletonClones); + DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); + assert(dofClone); + registerDegreeOfFreedom(dofClone); +} + //============================================================================== void ReferentialSkeleton::registerSkeleton(const Skeleton* skel) { diff --git a/dart/dynamics/ReferentialSkeleton.hpp b/dart/dynamics/ReferentialSkeleton.hpp index bf13b79d398da..6113f31f0e55d 100644 --- a/dart/dynamics/ReferentialSkeleton.hpp +++ b/dart/dynamics/ReferentialSkeleton.hpp @@ -443,6 +443,12 @@ class ReferentialSkeleton : public MetaSkeleton /// ReferentialSkeleton void updateCaches(); + SkeletonPtr getOrCloneSkeleton(const Skeleton* skel, std::unordered_map& map); + + void cloneBodyNode(const BodyNode* bodyNode, std::unordered_map& mapToSkeletonClones); + void cloneJoint(const Joint* joint, std::unordered_map& mapToSkeletonClones); + void cloneDegreeOfFreedom(const DegreeOfFreedom* dof, std::unordered_map& mapToSkeletonClones); + /// Weak pointer to this Skeleton std::weak_ptr mPtr; From 01db55c0c7f47d9597ec327283b70c7058eb727e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 11:03:03 -0800 Subject: [PATCH 04/13] Splify cloning meta-skeletons --- dart/dynamics/Branch.cpp | 37 ++++++++++++ dart/dynamics/Branch.hpp | 5 ++ dart/dynamics/Chain.cpp | 82 ++++++++++++++++----------- dart/dynamics/Chain.hpp | 2 + dart/dynamics/Group.cpp | 29 +++++++++- dart/dynamics/Linkage.cpp | 41 -------------- dart/dynamics/Linkage.hpp | 6 -- dart/dynamics/ReferentialSkeleton.cpp | 52 ----------------- dart/dynamics/ReferentialSkeleton.hpp | 6 -- 9 files changed, 120 insertions(+), 140 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index ca2e0dbdedfbd..77f680ba3b62c 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -65,6 +65,19 @@ Branch::Criteria::operator Linkage::Criteria() const return convert(); } +//============================================================================== +Branch::Criteria Branch::Criteria::convertBack(const Linkage::Criteria& criteria) +{ + BodyNodePtr startBodyNode = criteria.mStart.mNode.lock(); + if (!startBodyNode) + { + // TODO: Warning for invalid criteria + return Branch::Criteria(nullptr); + } + + return Branch::Criteria(startBodyNode.get()); +} + //============================================================================== BranchPtr Branch::create(const Branch::Criteria& _criteria, const std::string& _name) @@ -74,6 +87,30 @@ BranchPtr Branch::create(const Branch::Criteria& _criteria, return branch; } +//============================================================================== +MetaSkeletonPtr Branch::cloneMetaSkeleton() const +{ + // Clone the skeleton (assuming one skeleton is involved) + BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); + if (!bodyNode) + { + // TODO: Warning (failed to clone due to the invalid target) + return nullptr; + } + SkeletonPtr skelClone = bodyNode->getSkeleton(); + + // Create a Criteria + Criteria newCriteria = Criteria::convertBack(mCriteria); + assert(newCriteria.mStart.lock()); + newCriteria.mStart + = skelClone->getBodyNode(newCriteria.mStart.lock()->getName()); + + // Create a Chain clone with the Criteria + BranchPtr newBranch = create(newCriteria, getName()); + + return newBranch; +} + //============================================================================== bool Branch::isStillBranch() const { diff --git a/dart/dynamics/Branch.hpp b/dart/dynamics/Branch.hpp index 9a4aab290e657..429ce289b42cc 100644 --- a/dart/dynamics/Branch.hpp +++ b/dart/dynamics/Branch.hpp @@ -62,12 +62,17 @@ class Branch : public Linkage /// Operator for implicit conversion to a Linkage::Criteria operator Linkage::Criteria() const; + + static Criteria convertBack(const Linkage::Criteria& criteria); }; /// Create a Branch static BranchPtr create(const Branch::Criteria& _criteria, const std::string& _name = "Branch"); + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Returns false if a new BodyNode has been attached to any BodyNode of this /// Branch, or if a BodyNode of this Branch has been detached. bool isStillBranch() const; diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index 6079827d8ebfd..55655e8c9dd28 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -84,6 +84,39 @@ Linkage::Criteria Chain::Criteria::convert() const return criteria; } +//============================================================================== +Chain::Criteria Chain::Criteria::convertBack(const Linkage::Criteria& criteria) +{ + BodyNodePtr startBodyNode = criteria.mStart.mNode.lock(); + if (!startBodyNode) + { + // TODO: Warning for invalid criteria + return Chain::Criteria(nullptr, nullptr); + } + + if (criteria.mTargets.size() != 1u) + { + // TODO: Warning for invalid criteria + return Chain::Criteria(nullptr, nullptr); + } + const Linkage::Criteria::Target& target = criteria.mTargets[0]; + BodyNodePtr targetBodyNode = target.mNode.lock(); + if (!targetBodyNode) + { + // TODO: Warning for invalid criteria + return Chain::Criteria(nullptr, nullptr); + } + + bool includeBoth = true; + if (criteria.mStart.mPolicy != Linkage::Criteria::INCLUDE) + includeBoth = false; + if (target.mPolicy != Linkage::Criteria::INCLUDE) + includeBoth = false; + + return Chain::Criteria( + startBodyNode.get(), targetBodyNode.get(), includeBoth); +} + //============================================================================== Chain::Criteria::operator Linkage::Criteria() const { @@ -120,43 +153,28 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, //============================================================================== MetaSkeletonPtr Chain::cloneMetaSkeleton() const { - // Create an empty Group - ChainPtr newGroup = create(nullptr, nullptr, getName()); - - if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) + // Clone the skeleton (assuming one skeleton is involved) + BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); + if (!bodyNode) { - dtwarn << "[Chain::cloneMetaSkeletonHelper] Attempting to " - << "clone a ReferentialSkeleton that doesn't include any BodyNodes " - << "but including some Joints or DegreeOfFreedoms. This will lead " - << "to dangling Joints or DegreeOfFreedoms in the cloned " - << "ReferentialSkeleton because it only holds the stong reference " - << "to the BodyNodes but not others.\n"; + // TODO: Warning (failed to clone due to the invalid target) + return nullptr; } + SkeletonPtr skelClone = bodyNode->getSkeleton(); - // Array for Skeleton clones that will be collected durig cloning BodyNodes, - // Joints, DegreeOfFreedoms. - // - // The clones will not be destroyed even after the map is destroyed because - // the new Linkage will hold the skeleton by holding the strong referecnes of - // the body nodes. - std::unordered_map mapToSkeletonClones; - mapToSkeletonClones.reserve(mSkeletons.size()); - - newGroup->cloneCriteria(mCriteria, mapToSkeletonClones); - - // Register BodyNode - for (const BodyNodePtr& bodyNode : mBodyNodes) - newGroup->cloneBodyNode(bodyNode.get(), mapToSkeletonClones); - - // Register Joint - for (const JointPtr& joint : mJoints) - newGroup->cloneJoint(joint.get(), mapToSkeletonClones); + // Create a Criteria + Criteria newCriteria = Criteria::convertBack(mCriteria); + assert(newCriteria.mStart.lock()); + assert(newCriteria.mTarget.lock()); + newCriteria.mStart + = skelClone->getBodyNode(newCriteria.mStart.lock()->getName()); + newCriteria.mTarget + = skelClone->getBodyNode(newCriteria.mTarget.lock()->getName()); - // Register DegreeOfFreedom - for (const DegreeOfFreedomPtr& dof : mDofs) - newGroup->cloneDegreeOfFreedom(dof.get(), mapToSkeletonClones); + // Create a Chain clone with the Criteria + ChainPtr newChain = create(newCriteria, getName()); - return newGroup; + return newChain; } //============================================================================== diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index f0b2bfb7c98aa..2fff18bbb4b0c 100644 --- a/dart/dynamics/Chain.hpp +++ b/dart/dynamics/Chain.hpp @@ -72,6 +72,8 @@ class Chain : public Linkage /// Operator for implicit conversion to a Linkage::Criteria operator Linkage::Criteria() const; + + static Criteria convertBack(const Linkage::Criteria& criteria); }; /// This enum is used to specify to the create() function that both the start diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 6e8a2e41850d3..69e87027de309 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -92,18 +92,41 @@ MetaSkeletonPtr Group::cloneMetaSkeleton() const // the body nodes. std::unordered_map mapToSkeletonClones; mapToSkeletonClones.reserve(mSkeletons.size()); + for (const Skeleton* skel : mSkeletons) + { + SkeletonPtr skelClone = skel->clone(); + mapToSkeletonClones.insert(std::make_pair(skel, skelClone)); + } // Register BodyNode for (const BodyNodePtr& bodyNode : mBodyNodes) - newGroup->cloneBodyNode(bodyNode.get(), mapToSkeletonClones); + { + SkeletonPtr skelClone = mapToSkeletonClones[bodyNode->getSkeleton().get()]; + assert(skelClone); + BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName()); + assert(bodyNodeClone); + newGroup->addBodyNode(bodyNodeClone); + } // Register Joint for (const JointPtr& joint : mJoints) - newGroup->cloneJoint(joint.get(), mapToSkeletonClones); + { + SkeletonPtr skelClone = mapToSkeletonClones[joint->getSkeleton().get()]; + assert(skelClone); + Joint* jointClone = skelClone->getJoint(joint->getName()); + assert(jointClone); + newGroup->addJoint(jointClone); + } // Register DegreeOfFreedom for (const DegreeOfFreedomPtr& dof : mDofs) - newGroup->cloneDegreeOfFreedom(dof.get(), mapToSkeletonClones); + { + SkeletonPtr skelClone = mapToSkeletonClones[dof->getSkeleton().get()]; + assert(skelClone); + DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); + assert(dofClone); + newGroup->addDof(dofClone); + } return newGroup; } diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index fa92c2b3ac2a6..4120544c0890b 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -485,47 +485,6 @@ LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name) return linkage; } -//============================================================================== -Linkage::Criteria::Target Linkage::cloneTarget( - std::unordered_map& mapToSkeletonClones, - const Linkage::Criteria::Target& target) -{ - Linkage::Criteria::Target newTarget; - BodyNodePtr bodyNodePtr = target.mNode.lock(); - if (!bodyNodePtr) - { - newTarget.mNode = WeakBodyNodePtr(); - // NOTE: - } - else - { - const Skeleton* skel = bodyNodePtr->getSkeleton().get(); - auto search = mapToSkeletonClones.find(skel); - assert(search != mapToSkeletonClones.end()); - const SkeletonPtr& skelClone = getOrCloneSkeleton(skel, mapToSkeletonClones); - BodyNode* bodyNodeInClone = skelClone->getBodyNode(bodyNodePtr->getName()); - assert(bodyNodeInClone); - newTarget.mNode = WeakBodyNodePtr(bodyNodeInClone); - } - newTarget.mPolicy = target.mPolicy; - newTarget.mChain = target.mChain; - - return newTarget; -} - -//============================================================================== -void Linkage::cloneCriteria( - const Criteria& criteria, - std::unordered_map& mapToSkeletonClones) -{ - // Create Criteria for a Linkage clone - Criteria newCriteria; - newCriteria.mStart = cloneTarget(mapToSkeletonClones, criteria.mStart); - for (const Linkage::Criteria::Target& target : criteria.mTargets) - newCriteria.mTargets.emplace_back(cloneTarget(mapToSkeletonClones, target)); - mCriteria = newCriteria; -} - //============================================================================== bool Linkage::isAssembled() const { diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index 3115240a12ee9..713b0c4686710 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -168,8 +168,6 @@ class Linkage : public ReferentialSkeleton static LinkagePtr create(const Criteria& _criteria, const std::string& _name = "Linkage"); - void cloneCriteria(const Criteria& criteria, std::unordered_map& mapToSkeletonClones); - /// Returns false if the original assembly of this Linkage has been broken in /// some way bool isAssembled() const; @@ -189,10 +187,6 @@ class Linkage : public ReferentialSkeleton /// Update any metadata needed by the Linkage or its derived classes virtual void update(); - Criteria::Target cloneTarget( - std::unordered_map& mapToSkeletonClones, - const Linkage::Criteria::Target& target); - /// Criteria that defines the structure of this Linkage Criteria mCriteria; diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 9ca1031b7a219..456e8bdc3b237 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -1461,58 +1461,6 @@ void ReferentialSkeleton::updateCaches() mFc = Eigen::VectorXd::Zero(nDofs); } -//============================================================================== -SkeletonPtr ReferentialSkeleton::getOrCloneSkeleton(const Skeleton* skel, std::unordered_map& map) -{ - auto search = map.find(skel); - if (search == map.end()) - { - SkeletonPtr skelClone = skel->clone(); - map.insert(std::make_pair(skel, skelClone)); - return skelClone; - } - else - { - return search->second; - } -} - -//============================================================================== -void ReferentialSkeleton::cloneBodyNode( - const BodyNode* bodyNode, - std::unordered_map& mapToSkeletonClones) -{ - SkeletonPtr skelClone - = getOrCloneSkeleton(bodyNode->getSkeleton().get(), mapToSkeletonClones); - BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName()); - assert(bodyNodeClone); - registerBodyNode(bodyNodeClone); -} - -//============================================================================== -void ReferentialSkeleton::cloneJoint( - const Joint* joint, - std::unordered_map& mapToSkeletonClones) -{ - SkeletonPtr skelClone - = getOrCloneSkeleton(joint->getSkeleton().get(), mapToSkeletonClones); - Joint* jointClone = skelClone->getJoint(joint->getName()); - assert(jointClone); - registerJoint(jointClone); -} - -//============================================================================== -void ReferentialSkeleton::cloneDegreeOfFreedom( - const DegreeOfFreedom* dof, - std::unordered_map& mapToSkeletonClones) -{ - SkeletonPtr skelClone - = getOrCloneSkeleton(dof->getSkeleton().get(), mapToSkeletonClones); - DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); - assert(dofClone); - registerDegreeOfFreedom(dofClone); -} - //============================================================================== void ReferentialSkeleton::registerSkeleton(const Skeleton* skel) { diff --git a/dart/dynamics/ReferentialSkeleton.hpp b/dart/dynamics/ReferentialSkeleton.hpp index 6113f31f0e55d..bf13b79d398da 100644 --- a/dart/dynamics/ReferentialSkeleton.hpp +++ b/dart/dynamics/ReferentialSkeleton.hpp @@ -443,12 +443,6 @@ class ReferentialSkeleton : public MetaSkeleton /// ReferentialSkeleton void updateCaches(); - SkeletonPtr getOrCloneSkeleton(const Skeleton* skel, std::unordered_map& map); - - void cloneBodyNode(const BodyNode* bodyNode, std::unordered_map& mapToSkeletonClones); - void cloneJoint(const Joint* joint, std::unordered_map& mapToSkeletonClones); - void cloneDegreeOfFreedom(const DegreeOfFreedom* dof, std::unordered_map& mapToSkeletonClones); - /// Weak pointer to this Skeleton std::weak_ptr mPtr; From 4dce68531d8716388e20d3645c8ae5fb66241cb0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 11:22:49 -0800 Subject: [PATCH 05/13] Implement cloneMetaSkeleton() for the rest of drived classes of MetaSkeleton --- dart/dynamics/Branch.cpp | 4 ++-- dart/dynamics/Branch.hpp | 3 ++- dart/dynamics/Chain.hpp | 3 ++- dart/dynamics/Linkage.cpp | 44 ++++++++++++++++++++++++++++++++++ dart/dynamics/Linkage.hpp | 3 +++ dart/dynamics/MetaSkeleton.hpp | 2 +- dart/dynamics/Skeleton.cpp | 6 +++++ dart/dynamics/Skeleton.hpp | 3 +++ 8 files changed, 63 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index 77f680ba3b62c..9cfd7af786220 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -66,7 +66,7 @@ Branch::Criteria::operator Linkage::Criteria() const } //============================================================================== -Branch::Criteria Branch::Criteria::convertBack(const Linkage::Criteria& criteria) +Branch::Criteria Branch::Criteria::convert(const Linkage::Criteria& criteria) { BodyNodePtr startBodyNode = criteria.mStart.mNode.lock(); if (!startBodyNode) @@ -100,7 +100,7 @@ MetaSkeletonPtr Branch::cloneMetaSkeleton() const SkeletonPtr skelClone = bodyNode->getSkeleton(); // Create a Criteria - Criteria newCriteria = Criteria::convertBack(mCriteria); + Criteria newCriteria = Criteria::convert(mCriteria); assert(newCriteria.mStart.lock()); newCriteria.mStart = skelClone->getBodyNode(newCriteria.mStart.lock()->getName()); diff --git a/dart/dynamics/Branch.hpp b/dart/dynamics/Branch.hpp index 429ce289b42cc..870ddac403634 100644 --- a/dart/dynamics/Branch.hpp +++ b/dart/dynamics/Branch.hpp @@ -63,7 +63,8 @@ class Branch : public Linkage /// Operator for implicit conversion to a Linkage::Criteria operator Linkage::Criteria() const; - static Criteria convertBack(const Linkage::Criteria& criteria); + /// Converts Linkage::Criteria to Branch::Criteria + static Criteria convert(const Linkage::Criteria& criteria); }; /// Create a Branch diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index 2fff18bbb4b0c..1c00b5a6e8631 100644 --- a/dart/dynamics/Chain.hpp +++ b/dart/dynamics/Chain.hpp @@ -73,7 +73,8 @@ class Chain : public Linkage /// Operator for implicit conversion to a Linkage::Criteria operator Linkage::Criteria() const; - static Criteria convertBack(const Linkage::Criteria& criteria); + /// Converts Linkage::Criteria to Chain::Criteria + static Criteria convert(const Linkage::Criteria& criteria); }; /// This enum is used to specify to the create() function that both the start diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 4120544c0890b..54837c8ea0f04 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -485,6 +485,50 @@ LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name) return linkage; } +//============================================================================== +Linkage::Criteria::Target cloneTarget( + Skeleton& skelClone, + const Linkage::Criteria::Target& target) +{ + Linkage::Criteria::Target newTarget; + + BodyNodePtr bodyNodePtr = target.mNode.lock(); + assert(bodyNodePtr); + BodyNode* bodyNodeInClone = skelClone.getBodyNode(bodyNodePtr->getName()); + assert(bodyNodeInClone); + newTarget.mNode = WeakBodyNodePtr(bodyNodeInClone); + newTarget.mPolicy = target.mPolicy; + newTarget.mChain = target.mChain; + + return newTarget; +} + +//============================================================================== +MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +{ + // Clone the skeleton (assuming one skeleton is involved) + BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); + if (!bodyNode) + { + // TODO: Warning (failed to clone due to the invalid target) + return nullptr; + } + SkeletonPtr skelClone = bodyNode->getSkeleton(); + + // Create a Criteria + Criteria newCriteria = mCriteria; + assert(newCriteria.mStart.lock()); + newCriteria.mStart = cloneTarget(*skelClone, mCriteria.mStart); + newCriteria.mTargets.reserve(mCriteria.mTargets.size()); + for (const Criteria::Target& target : mCriteria.mTargets) + newCriteria.mTargets.emplace_back(cloneTarget(*skelClone, target)); + + // Create a Chain clone with the Criteria + LinkagePtr newLinkage = create(newCriteria, getName()); + + return newLinkage; +} + //============================================================================== bool Linkage::isAssembled() const { diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index 713b0c4686710..aeb7821524726 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -168,6 +168,9 @@ class Linkage : public ReferentialSkeleton static LinkagePtr create(const Criteria& _criteria, const std::string& _name = "Linkage"); + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Returns false if the original assembly of this Linkage has been broken in /// some way bool isAssembled() const; diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index 3249e21b16a1a..7ba9203abd6a8 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -72,7 +72,7 @@ class MetaSkeleton : public common::Subject virtual ~MetaSkeleton() = default; /// Creates an identical clone of this MetaSkeleton - virtual MetaSkeletonPtr cloneMetaSkeleton() const { return nullptr; } + virtual MetaSkeletonPtr cloneMetaSkeleton() const = 0; /// Returns mutex. virtual std::unique_ptr getLockableReference() diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index bea3a0419a2ea..87512df9db26f 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -401,6 +401,12 @@ SkeletonPtr Skeleton::clone() const return clone(getName()); } +//============================================================================== +MetaSkeletonPtr Skeleton::cloneMetaSkeleton() const +{ + return clone(getName()); +} + //============================================================================== SkeletonPtr Skeleton::clone(const std::string& cloneName) const { diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index 72397d61fb9db..c5561600c60d5 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -176,6 +176,9 @@ class Skeleton : /// Create an identical clone of this Skeleton. SkeletonPtr clone() const; + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton() const override; + /// Create an identical clone of this Skeleton, except that it has a new name. SkeletonPtr clone(const std::string& cloneName) const; From 6f42f554a9bdd81cfc477110e2a279c774b6266c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 11:36:12 -0800 Subject: [PATCH 06/13] Add warnings --- dart/dynamics/Branch.cpp | 6 ++++-- dart/dynamics/Chain.cpp | 22 ++++++++++++++++------ dart/dynamics/Linkage.cpp | 4 +++- 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index 9cfd7af786220..38223b0d5e2f2 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -94,7 +94,9 @@ MetaSkeletonPtr Branch::cloneMetaSkeleton() const BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); if (!bodyNode) { - // TODO: Warning (failed to clone due to the invalid target) + dtwarn << "[Branch::cloneMetaSkeleton] Failed to clone because the " + << "start node of the criteria in this Branch is not valid anymore. " + << "Returning nullptr.\n"; return nullptr; } SkeletonPtr skelClone = bodyNode->getSkeleton(); @@ -105,7 +107,7 @@ MetaSkeletonPtr Branch::cloneMetaSkeleton() const newCriteria.mStart = skelClone->getBodyNode(newCriteria.mStart.lock()->getName()); - // Create a Chain clone with the Criteria + // Create a Branch clone with the Criteria BranchPtr newBranch = create(newCriteria, getName()); return newBranch; diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index 55655e8c9dd28..46736194eda27 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -85,25 +85,33 @@ Linkage::Criteria Chain::Criteria::convert() const } //============================================================================== -Chain::Criteria Chain::Criteria::convertBack(const Linkage::Criteria& criteria) +Chain::Criteria Chain::Criteria::convert(const Linkage::Criteria& criteria) { BodyNodePtr startBodyNode = criteria.mStart.mNode.lock(); if (!startBodyNode) { - // TODO: Warning for invalid criteria + dtwarn << "[Chain::Criteria::convert] Failed in conversion because the " + << "start node of the input criteria is not valid anymore. Using " + << "the returning Criteria will lead to creating an empty Chain.\n"; return Chain::Criteria(nullptr, nullptr); } if (criteria.mTargets.size() != 1u) { - // TODO: Warning for invalid criteria + dtwarn << "[Chain::Criteria::convert] Failed in conversion because the " + << "input criteria is not for Chain. The number of targets should " + << "be one while the input is " << criteria.mTargets.size() << ". " + << "Using the returning Criteria will lead to creating an empty " + << "Chain.\n"; return Chain::Criteria(nullptr, nullptr); } const Linkage::Criteria::Target& target = criteria.mTargets[0]; BodyNodePtr targetBodyNode = target.mNode.lock(); if (!targetBodyNode) { - // TODO: Warning for invalid criteria + dtwarn << "[Chain::Criteria::convert] Failed in conversion because the " + << "end node of the input criteria is not valid anymore. Using the " + << "returning Criteria will lead to creating an empty Chain.\n"; return Chain::Criteria(nullptr, nullptr); } @@ -157,13 +165,15 @@ MetaSkeletonPtr Chain::cloneMetaSkeleton() const BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); if (!bodyNode) { - // TODO: Warning (failed to clone due to the invalid target) + dtwarn << "[Chain::cloneMetaSkeleton] Failed to clone because the " + << "start node of the criteria in this Chain is not valid anymore. " + << "Returning nullptr.\n"; return nullptr; } SkeletonPtr skelClone = bodyNode->getSkeleton(); // Create a Criteria - Criteria newCriteria = Criteria::convertBack(mCriteria); + Criteria newCriteria = Criteria::convert(mCriteria); assert(newCriteria.mStart.lock()); assert(newCriteria.mTarget.lock()); newCriteria.mStart diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 54837c8ea0f04..d0d52ffd3df4d 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -510,7 +510,9 @@ MetaSkeletonPtr Linkage::cloneMetaSkeleton() const BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); if (!bodyNode) { - // TODO: Warning (failed to clone due to the invalid target) + dtwarn << "[Linkage::cloneMetaSkeleton] Failed to clone because the " + << "start node of the criteria in this Linkage is not valid " + << "anymore. Returning nullptr.\n"; return nullptr; } SkeletonPtr skelClone = bodyNode->getSkeleton(); From 55fde86cc02ce44689d11f2c1aa4ce071588e6d0 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 12:40:46 -0800 Subject: [PATCH 07/13] Fix Linkage::cloneMetaSkeleton() --- dart/dynamics/Branch.cpp | 2 +- dart/dynamics/Chain.cpp | 2 +- dart/dynamics/Group.cpp | 10 +-- dart/dynamics/Linkage.cpp | 48 +++++++---- unittests/comprehensive/test_MetaSkeleton.cpp | 84 +++++++++++++++++++ 5 files changed, 125 insertions(+), 21 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index 38223b0d5e2f2..d154e9d316bbd 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -99,7 +99,7 @@ MetaSkeletonPtr Branch::cloneMetaSkeleton() const << "Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); // Create a Criteria Criteria newCriteria = Criteria::convert(mCriteria); diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index 46736194eda27..e1cf610751a99 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -170,7 +170,7 @@ MetaSkeletonPtr Chain::cloneMetaSkeleton() const << "Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); // Create a Criteria Criteria newCriteria = Criteria::convert(mCriteria); diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 69e87027de309..f2c43516c4be8 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -98,7 +98,7 @@ MetaSkeletonPtr Group::cloneMetaSkeleton() const mapToSkeletonClones.insert(std::make_pair(skel, skelClone)); } - // Register BodyNode + // Add BodyNodes for (const BodyNodePtr& bodyNode : mBodyNodes) { SkeletonPtr skelClone = mapToSkeletonClones[bodyNode->getSkeleton().get()]; @@ -108,24 +108,24 @@ MetaSkeletonPtr Group::cloneMetaSkeleton() const newGroup->addBodyNode(bodyNodeClone); } - // Register Joint + // Add Joints for (const JointPtr& joint : mJoints) { SkeletonPtr skelClone = mapToSkeletonClones[joint->getSkeleton().get()]; assert(skelClone); Joint* jointClone = skelClone->getJoint(joint->getName()); assert(jointClone); - newGroup->addJoint(jointClone); + newGroup->addJoint(jointClone, false); } - // Register DegreeOfFreedom + // Add DegreeOfFreedoms for (const DegreeOfFreedomPtr& dof : mDofs) { SkeletonPtr skelClone = mapToSkeletonClones[dof->getSkeleton().get()]; assert(skelClone); DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName()); assert(dofClone); - newGroup->addDof(dofClone); + newGroup->addDof(dofClone, false); } return newGroup; diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index d0d52ffd3df4d..3d8f278a9b6d6 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -486,21 +486,32 @@ LinkagePtr Linkage::create(const Criteria &_criteria, const std::string& _name) } //============================================================================== -Linkage::Criteria::Target cloneTarget( +Linkage::Criteria::Target createTargetFromClone( Skeleton& skelClone, const Linkage::Criteria::Target& target) { - Linkage::Criteria::Target newTarget; - BodyNodePtr bodyNodePtr = target.mNode.lock(); assert(bodyNodePtr); - BodyNode* bodyNodeInClone = skelClone.getBodyNode(bodyNodePtr->getName()); - assert(bodyNodeInClone); - newTarget.mNode = WeakBodyNodePtr(bodyNodeInClone); - newTarget.mPolicy = target.mPolicy; - newTarget.mChain = target.mChain; + BodyNode* bodyNodeClone = skelClone.getBodyNode(bodyNodePtr->getName()); + assert(bodyNodeClone); + assert(bodyNodeClone != bodyNodePtr.get()); + + return Linkage::Criteria::Target( + bodyNodeClone, target.mPolicy, target.mPolicy); +} + +//============================================================================== +Linkage::Criteria::Terminal createTerminalFromClone( + Skeleton& skelClone, + const Linkage::Criteria::Terminal& terminal) +{ + BodyNodePtr bodyNodePtr = terminal.mTerminal.lock(); + assert(bodyNodePtr); + BodyNode* bodyNodeClone = skelClone.getBodyNode(bodyNodePtr->getName()); + assert(bodyNodeClone); + assert(bodyNodeClone != bodyNodePtr.get()); - return newTarget; + return Linkage::Criteria::Terminal(bodyNodeClone, terminal.mInclusive); } //============================================================================== @@ -515,15 +526,24 @@ MetaSkeletonPtr Linkage::cloneMetaSkeleton() const << "anymore. Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); + assert(skelClone != bodyNode->getSkeleton()); // Create a Criteria - Criteria newCriteria = mCriteria; - assert(newCriteria.mStart.lock()); - newCriteria.mStart = cloneTarget(*skelClone, mCriteria.mStart); + Criteria newCriteria; + newCriteria.mStart = createTargetFromClone(*skelClone, mCriteria.mStart); newCriteria.mTargets.reserve(mCriteria.mTargets.size()); for (const Criteria::Target& target : mCriteria.mTargets) - newCriteria.mTargets.emplace_back(cloneTarget(*skelClone, target)); + { + newCriteria.mTargets.emplace_back( + createTargetFromClone(*skelClone, target)); + } + newCriteria.mTerminals.reserve(mCriteria.mTerminals.size()); + for (const Criteria::Terminal& terminal : newCriteria.mTerminals) + { + newCriteria.mTerminals.emplace_back( + createTerminalFromClone(*skelClone, terminal)); + } // Create a Chain clone with the Criteria LinkagePtr newLinkage = create(newCriteria, getName()); diff --git a/unittests/comprehensive/test_MetaSkeleton.cpp b/unittests/comprehensive/test_MetaSkeleton.cpp index c101fa1abe371..51a1973cb1ddd 100644 --- a/unittests/comprehensive/test_MetaSkeleton.cpp +++ b/unittests/comprehensive/test_MetaSkeleton.cpp @@ -683,3 +683,87 @@ TEST(MetaSkeleton, ReferentialSkeletonHoldsStrongReferencesOfBodyNodes) EXPECT_EQ(group->getBodyNode(0)->getName(), name1); EXPECT_EQ(group->getBodyNode(1)->getName(), name2); } + +//============================================================================== +template +void testReferentialSkeletonClone( + const RefSkelType& skel, const RefSkelType& skelClone) +{ + EXPECT_NE(skel, nullptr); + EXPECT_NE(skelClone, nullptr); + EXPECT_EQ(skel->getName(), skelClone->getName()); + EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes()); + EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints()); + EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs()); + + // The skeleton instance of group clone should be different from the original, + // but the properties should be the same. + for (std::size_t i = 0u; i < skel->getNumBodyNodes(); ++i) + { + const auto& bodyNode = skel->getBodyNode(i); + const auto& skel = bodyNode->getSkeleton(); + + const auto& bodyNodeClone = skelClone->getBodyNode(i); + const auto& skelClone = bodyNodeClone->getSkeleton(); + + EXPECT_NE(skel, skelClone); + EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes()); + EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints()); + EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs()); + } + for (std::size_t i = 0u; i < skel->getNumJoints(); ++i) + { + const auto& joint = skel->getJoint(i); + const auto& skel = joint->getSkeleton(); + + const auto& jointClone = skelClone->getJoint(i); + const auto& skelClone = jointClone->getSkeleton(); + + EXPECT_NE(skel, skelClone); + EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes()); + EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints()); + EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs()); + } +} + +//============================================================================== +TEST(MetaSkeleton, CloneReferentialSkeletons) +{ + SkeletonPtr skel1 = constructLinkageTestSkeleton(); + SkeletonPtr skel2 = constructLinkageTestSkeleton(); + + // Group + GroupPtr group = Group::create(); + group->addBodyNode(skel1->getBodyNode(0)); + group->addBodyNode(skel2->getBodyNode(0)); + group->addJoint(skel1->getJoint(1)); + group->addJoint(skel2->getJoint(1)); + GroupPtr groupClone = std::dynamic_pointer_cast( + group->cloneMetaSkeleton()); + testReferentialSkeletonClone(group, groupClone); + + // Linkage + Linkage::Criteria criteria; + criteria.mStart = skel1->getBodyNode("c5b2"); + criteria.mTargets.push_back( + Linkage::Criteria::Target(skel1->getBodyNode("c4b3"))); + LinkagePtr linkage = Linkage::create(criteria); + LinkagePtr linkageClone = std::dynamic_pointer_cast( + linkage->cloneMetaSkeleton()); + testReferentialSkeletonClone(linkage, linkageClone); + + // Chain + ChainPtr chain = Chain::create( + skel1->getBodyNode("c1b3"), skel1->getBodyNode("c3b4"), "midchain"); + ChainPtr chainClone = std::dynamic_pointer_cast( + chain->cloneMetaSkeleton()); + testReferentialSkeletonClone(chain, chainClone); + + // Branch + BranchPtr branch = Branch::create(skel1->getBodyNode("c3b3"), "subtree"); + checkForBodyNodes( + branch, skel1, true, "c3b3", "c3b4", "c4b1", "c4b2", "c4b3"); + BranchPtr branchClone = std::dynamic_pointer_cast( + branch->cloneMetaSkeleton()); + testReferentialSkeletonClone(branch, branchClone); +} From 9eb80ec1c1ded42a02b978692ac731582e8ae5a7 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 12:50:38 -0800 Subject: [PATCH 08/13] Add type specific clone functions --- dart/dynamics/Branch.cpp | 8 +++++++- dart/dynamics/Branch.hpp | 3 +++ dart/dynamics/Chain.cpp | 8 +++++++- dart/dynamics/Chain.hpp | 3 +++ dart/dynamics/Group.cpp | 8 +++++++- dart/dynamics/Group.hpp | 3 +++ dart/dynamics/Linkage.cpp | 8 +++++++- dart/dynamics/Linkage.hpp | 3 +++ dart/dynamics/MetaSkeleton.hpp | 7 +++++-- 9 files changed, 45 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index d154e9d316bbd..5aa35a92a809c 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -88,7 +88,7 @@ BranchPtr Branch::create(const Branch::Criteria& _criteria, } //============================================================================== -MetaSkeletonPtr Branch::cloneMetaSkeleton() const +BranchPtr Branch::cloneBranch() const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -113,6 +113,12 @@ MetaSkeletonPtr Branch::cloneMetaSkeleton() const return newBranch; } +//============================================================================== +MetaSkeletonPtr Branch::cloneMetaSkeleton() const +{ + return cloneBranch(); +} + //============================================================================== bool Branch::isStillBranch() const { diff --git a/dart/dynamics/Branch.hpp b/dart/dynamics/Branch.hpp index 870ddac403634..9c0de3a72d24f 100644 --- a/dart/dynamics/Branch.hpp +++ b/dart/dynamics/Branch.hpp @@ -71,6 +71,9 @@ class Branch : public Linkage static BranchPtr create(const Branch::Criteria& _criteria, const std::string& _name = "Branch"); + /// Creates and returns a clone of this Branch. + BranchPtr cloneBranch() const; + // Documentation inherited MetaSkeletonPtr cloneMetaSkeleton() const override; diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index e1cf610751a99..c02dd3b478e98 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -159,7 +159,7 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, } //============================================================================== -MetaSkeletonPtr Chain::cloneMetaSkeleton() const +ChainPtr Chain::cloneChain() const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -187,6 +187,12 @@ MetaSkeletonPtr Chain::cloneMetaSkeleton() const return newChain; } +//============================================================================== +MetaSkeletonPtr Chain::cloneMetaSkeleton() const +{ + return cloneChain(); +} + //============================================================================== bool Chain::isStillChain() const { diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index 1c00b5a6e8631..4734e94c9754a 100644 --- a/dart/dynamics/Chain.hpp +++ b/dart/dynamics/Chain.hpp @@ -96,6 +96,9 @@ class Chain : public Linkage static ChainPtr create(BodyNode* _start, BodyNode* _target, IncludeBothTag, const std::string& _name = "Chain"); + /// Creates and returns a clone of this Chain. + ChainPtr cloneChain() const; + // Documentation inherited MetaSkeletonPtr cloneMetaSkeleton() const override; diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index f2c43516c4be8..9e4793572dcc8 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -69,7 +69,7 @@ GroupPtr Group::create(const std::string& _name, } //============================================================================== -MetaSkeletonPtr Group::cloneMetaSkeleton() const +GroupPtr Group::cloneGroup() const { // Create an empty Group GroupPtr newGroup = create(getName(), nullptr); @@ -131,6 +131,12 @@ MetaSkeletonPtr Group::cloneMetaSkeleton() const return newGroup; } +//============================================================================== +MetaSkeletonPtr Group::cloneMetaSkeleton() const +{ + return cloneGroup(); +} + //============================================================================== void Group::swapBodyNodeIndices(std::size_t _index1, std::size_t _index2) { diff --git a/dart/dynamics/Group.hpp b/dart/dynamics/Group.hpp index 47f91bf768863..d8c41b759d087 100644 --- a/dart/dynamics/Group.hpp +++ b/dart/dynamics/Group.hpp @@ -70,6 +70,9 @@ class Group : public ReferentialSkeleton /// Destructor virtual ~Group() = default; + /// Creates and returns a clone of this Group. + GroupPtr cloneGroup() const; + // Documentation inherited MetaSkeletonPtr cloneMetaSkeleton() const override; diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 3d8f278a9b6d6..92cbf2831745f 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -515,7 +515,7 @@ Linkage::Criteria::Terminal createTerminalFromClone( } //============================================================================== -MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +LinkagePtr Linkage::cloneLinkage() const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -551,6 +551,12 @@ MetaSkeletonPtr Linkage::cloneMetaSkeleton() const return newLinkage; } +//============================================================================== +MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +{ + return cloneLinkage(); +} + //============================================================================== bool Linkage::isAssembled() const { diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index aeb7821524726..ab05275be61d4 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -168,6 +168,9 @@ class Linkage : public ReferentialSkeleton static LinkagePtr create(const Criteria& _criteria, const std::string& _name = "Linkage"); + /// Creates and returns a clone of this Linkage. + LinkagePtr cloneLinkage() const; + // Documentation inherited MetaSkeletonPtr cloneMetaSkeleton() const override; diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index 7ba9203abd6a8..0d821919b34b1 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -73,12 +73,15 @@ class MetaSkeleton : public common::Subject /// Creates an identical clone of this MetaSkeleton virtual MetaSkeletonPtr cloneMetaSkeleton() const = 0; + // TODO: In DART7, rename this to clone() and change the current + // Skeleton::clone() to override it. Then consider adding + // Skeleton::cloneSkeleton(). /// Returns mutex. virtual std::unique_ptr getLockableReference() const = 0; - // TODO: Rename this to getMutex once the deprecated Skeleton::getMutex() is - // removed in DART 7. + // TODO: In DART7, rename this to getMutex() and change the current + // Skeleton::getMutex() to override it. //---------------------------------------------------------------------------- /// \{ \name Name From 1b122562f6a3822630d4682d021d7903a929a4ee Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 14:32:46 -0800 Subject: [PATCH 09/13] Add MetaSkeleton::cloneMetaSkeleton() taking new name --- dart/dynamics/Branch.cpp | 14 ++++++++++---- dart/dynamics/Branch.hpp | 8 +++++++- dart/dynamics/Chain.cpp | 14 ++++++++++---- dart/dynamics/Chain.hpp | 8 +++++++- dart/dynamics/Group.cpp | 14 ++++++++++---- dart/dynamics/Group.hpp | 8 +++++++- dart/dynamics/Linkage.cpp | 14 ++++++++++---- dart/dynamics/Linkage.hpp | 8 +++++++- dart/dynamics/MetaSkeleton.cpp | 6 ++++++ dart/dynamics/MetaSkeleton.hpp | 6 +++++- dart/dynamics/Skeleton.cpp | 20 ++++++++++++++++---- dart/dynamics/Skeleton.hpp | 21 ++++++++++++++++++--- dart/simulation/World.cpp | 2 +- 13 files changed, 114 insertions(+), 29 deletions(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index 5aa35a92a809c..01fde2a500b4b 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -89,6 +89,12 @@ BranchPtr Branch::create(const Branch::Criteria& _criteria, //============================================================================== BranchPtr Branch::cloneBranch() const +{ + return cloneBranch(getName()); +} + +//============================================================================== +BranchPtr Branch::cloneBranch(const std::string& cloneName) const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -99,7 +105,7 @@ BranchPtr Branch::cloneBranch() const << "Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->cloneSkeleton(); // Create a Criteria Criteria newCriteria = Criteria::convert(mCriteria); @@ -108,15 +114,15 @@ BranchPtr Branch::cloneBranch() const = skelClone->getBodyNode(newCriteria.mStart.lock()->getName()); // Create a Branch clone with the Criteria - BranchPtr newBranch = create(newCriteria, getName()); + BranchPtr newBranch = create(newCriteria, cloneName); return newBranch; } //============================================================================== -MetaSkeletonPtr Branch::cloneMetaSkeleton() const +MetaSkeletonPtr Branch::cloneMetaSkeleton(const std::string& cloneName) const { - return cloneBranch(); + return cloneBranch(cloneName); } //============================================================================== diff --git a/dart/dynamics/Branch.hpp b/dart/dynamics/Branch.hpp index 9c0de3a72d24f..a936e0ef7dd24 100644 --- a/dart/dynamics/Branch.hpp +++ b/dart/dynamics/Branch.hpp @@ -74,8 +74,14 @@ class Branch : public Linkage /// Creates and returns a clone of this Branch. BranchPtr cloneBranch() const; + /// Creates and returns a clone of this Branch. + BranchPtr cloneBranch(const std::string& cloneName) const; + + // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName. + using MetaSkeleton::cloneMetaSkeleton; + // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override; /// Returns false if a new BodyNode has been attached to any BodyNode of this /// Branch, or if a BodyNode of this Branch has been detached. diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index c02dd3b478e98..b2633f792eb9e 100644 --- a/dart/dynamics/Chain.cpp +++ b/dart/dynamics/Chain.cpp @@ -160,6 +160,12 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target, //============================================================================== ChainPtr Chain::cloneChain() const +{ + return cloneChain(getName()); +} + +//============================================================================== +ChainPtr Chain::cloneChain(const std::string& cloneName) const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -170,7 +176,7 @@ ChainPtr Chain::cloneChain() const << "Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->cloneSkeleton(); // Create a Criteria Criteria newCriteria = Criteria::convert(mCriteria); @@ -182,15 +188,15 @@ ChainPtr Chain::cloneChain() const = skelClone->getBodyNode(newCriteria.mTarget.lock()->getName()); // Create a Chain clone with the Criteria - ChainPtr newChain = create(newCriteria, getName()); + ChainPtr newChain = create(newCriteria, cloneName); return newChain; } //============================================================================== -MetaSkeletonPtr Chain::cloneMetaSkeleton() const +MetaSkeletonPtr Chain::cloneMetaSkeleton(const std::string& cloneName) const { - return cloneChain(); + return cloneChain(cloneName); } //============================================================================== diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index 4734e94c9754a..0ae459b0594b6 100644 --- a/dart/dynamics/Chain.hpp +++ b/dart/dynamics/Chain.hpp @@ -99,8 +99,14 @@ class Chain : public Linkage /// Creates and returns a clone of this Chain. ChainPtr cloneChain() const; + /// Creates and returns a clone of this Chain. + ChainPtr cloneChain(const std::string& cloneName) const; + + // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName. + using MetaSkeleton::cloneMetaSkeleton; + // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override; /// Returns false if this Chain has been broken, or some new Branching has /// been added. diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index 9e4793572dcc8..d632baa6d1658 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -70,9 +70,15 @@ GroupPtr Group::create(const std::string& _name, //============================================================================== GroupPtr Group::cloneGroup() const +{ + return cloneGroup(getName()); +} + +//============================================================================== +GroupPtr Group::cloneGroup(const std::string& cloneName) const { // Create an empty Group - GroupPtr newGroup = create(getName(), nullptr); + GroupPtr newGroup = create(cloneName, nullptr); if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u)) { @@ -94,7 +100,7 @@ GroupPtr Group::cloneGroup() const mapToSkeletonClones.reserve(mSkeletons.size()); for (const Skeleton* skel : mSkeletons) { - SkeletonPtr skelClone = skel->clone(); + SkeletonPtr skelClone = skel->cloneSkeleton(); mapToSkeletonClones.insert(std::make_pair(skel, skelClone)); } @@ -132,9 +138,9 @@ GroupPtr Group::cloneGroup() const } //============================================================================== -MetaSkeletonPtr Group::cloneMetaSkeleton() const +MetaSkeletonPtr Group::cloneMetaSkeleton(const std::string& cloneName) const { - return cloneGroup(); + return cloneGroup(cloneName); } //============================================================================== diff --git a/dart/dynamics/Group.hpp b/dart/dynamics/Group.hpp index d8c41b759d087..af4470f877bcf 100644 --- a/dart/dynamics/Group.hpp +++ b/dart/dynamics/Group.hpp @@ -73,8 +73,14 @@ class Group : public ReferentialSkeleton /// Creates and returns a clone of this Group. GroupPtr cloneGroup() const; + /// Creates and returns a clone of this Group. + GroupPtr cloneGroup(const std::string& cloneName) const; + + // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName. + using MetaSkeleton::cloneMetaSkeleton; + // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override; /// Swap the index of BodyNode _index1 with _index2 void swapBodyNodeIndices(std::size_t _index1, std::size_t _index2); diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 92cbf2831745f..818d3e4f5e925 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -516,6 +516,12 @@ Linkage::Criteria::Terminal createTerminalFromClone( //============================================================================== LinkagePtr Linkage::cloneLinkage() const +{ + return cloneLinkage(getName()); +} + +//============================================================================== +LinkagePtr Linkage::cloneLinkage(const std::string& cloneName) const { // Clone the skeleton (assuming one skeleton is involved) BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock(); @@ -526,7 +532,7 @@ LinkagePtr Linkage::cloneLinkage() const << "anymore. Returning nullptr.\n"; return nullptr; } - SkeletonPtr skelClone = bodyNode->getSkeleton()->clone(); + SkeletonPtr skelClone = bodyNode->getSkeleton()->cloneSkeleton(); assert(skelClone != bodyNode->getSkeleton()); // Create a Criteria @@ -546,15 +552,15 @@ LinkagePtr Linkage::cloneLinkage() const } // Create a Chain clone with the Criteria - LinkagePtr newLinkage = create(newCriteria, getName()); + LinkagePtr newLinkage = create(newCriteria, cloneName); return newLinkage; } //============================================================================== -MetaSkeletonPtr Linkage::cloneMetaSkeleton() const +MetaSkeletonPtr Linkage::cloneMetaSkeleton(const std::string& cloneName) const { - return cloneLinkage(); + return cloneLinkage(cloneName); } //============================================================================== diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index ab05275be61d4..a12bcab23609d 100644 --- a/dart/dynamics/Linkage.hpp +++ b/dart/dynamics/Linkage.hpp @@ -171,8 +171,14 @@ class Linkage : public ReferentialSkeleton /// Creates and returns a clone of this Linkage. LinkagePtr cloneLinkage() const; + /// Creates and returns a clone of this Linkage. + LinkagePtr cloneLinkage(const std::string& cloneName) const; + + // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName. + using MetaSkeleton::cloneMetaSkeleton; + // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override; /// Returns false if the original assembly of this Linkage has been broken in /// some way diff --git a/dart/dynamics/MetaSkeleton.cpp b/dart/dynamics/MetaSkeleton.cpp index 5448a533dc230..c07c8e9ccd56f 100644 --- a/dart/dynamics/MetaSkeleton.cpp +++ b/dart/dynamics/MetaSkeleton.cpp @@ -314,6 +314,12 @@ static double getValueFromIndex(const MetaSkeleton* skel, std::size_t _index, return 0.0; } +//============================================================================== +MetaSkeletonPtr MetaSkeleton::cloneMetaSkeleton() const +{ + return cloneMetaSkeleton(getName()); +} + //============================================================================== void MetaSkeleton::setCommand(std::size_t _index, double _command) { diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index 0d821919b34b1..39ad499e6ae92 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -72,11 +72,15 @@ class MetaSkeleton : public common::Subject virtual ~MetaSkeleton() = default; /// Creates an identical clone of this MetaSkeleton - virtual MetaSkeletonPtr cloneMetaSkeleton() const = 0; + virtual MetaSkeletonPtr cloneMetaSkeleton( + const std::string& cloneName) const = 0; // TODO: In DART7, rename this to clone() and change the current // Skeleton::clone() to override it. Then consider adding // Skeleton::cloneSkeleton(). + /// Creates an identical clone of this MetaSkeleton + MetaSkeletonPtr cloneMetaSkeleton() const; + /// Returns mutex. virtual std::unique_ptr getLockableReference() const = 0; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 87512df9db26f..6c3d66bf0786e 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -398,17 +398,23 @@ Skeleton::~Skeleton() //============================================================================== SkeletonPtr Skeleton::clone() const { - return clone(getName()); + return cloneSkeleton(getName()); } //============================================================================== -MetaSkeletonPtr Skeleton::cloneMetaSkeleton() const +SkeletonPtr Skeleton::clone(const std::string& cloneName) const { - return clone(getName()); + return cloneSkeleton(cloneName); } //============================================================================== -SkeletonPtr Skeleton::clone(const std::string& cloneName) const +SkeletonPtr Skeleton::cloneSkeleton() const +{ + return cloneSkeleton(getName()); +} + +//============================================================================== +SkeletonPtr Skeleton::cloneSkeleton(const std::string& cloneName) const { SkeletonPtr skelClone = Skeleton::create(cloneName); @@ -487,6 +493,12 @@ SkeletonPtr Skeleton::clone(const std::string& cloneName) const return skelClone; } +//============================================================================== +MetaSkeletonPtr Skeleton::cloneMetaSkeleton(const std::string& cloneName) const +{ + return cloneSkeleton(cloneName); +} + //============================================================================== #define SET_CONFIG_VECTOR( V ) \ if(configuration.m ## V . size() > 0) \ diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index c5561600c60d5..6c344d00ff77f 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -174,13 +174,28 @@ class Skeleton : Skeleton& operator=(const Skeleton& _other) = delete; /// Create an identical clone of this Skeleton. + /// \deprecated Deprecated in DART 6.7. Please use cloneSkeleton() instead. + DART_DEPRECATED(6.7) SkeletonPtr clone() const; - - // Documentation inherited - MetaSkeletonPtr cloneMetaSkeleton() const override; + // TODO: In DART 7, change this function to override MetaSkeleton::clone() + // that returns MetaSkeletonPtr /// Create an identical clone of this Skeleton, except that it has a new name. + /// \deprecated Deprecated in DART 6.7. Please use cloneSkeleton() instead. + DART_DEPRECATED(6.7) SkeletonPtr clone(const std::string& cloneName) const; + // TODO: In DART 7, change this function to override MetaSkeleton::clone() + // that returns MetaSkeletonPtr + + /// Creates and returns a clone of this Skeleton. + SkeletonPtr cloneSkeleton() const; + + /// Creates and returns a clone of this Skeleton. + SkeletonPtr cloneSkeleton(const std::string& cloneName) const; + + // Documentation inherited + MetaSkeletonPtr cloneMetaSkeleton( + const std::string& cloneName) const override; /// \} diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 29940e8a70043..9d920c7b69e72 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -101,7 +101,7 @@ WorldPtr World::clone() const // Clone and add each Skeleton for(std::size_t i=0; iaddSkeleton(mSkeletons[i]->clone()); + worldClone->addSkeleton(mSkeletons[i]->cloneSkeleton()); } // Clone and add each SimpleFrame From 78f327d2f7adaa83ad983949031a253496de65b8 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 14:37:37 -0800 Subject: [PATCH 10/13] Add an warning --- dart/dynamics/Branch.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dart/dynamics/Branch.cpp b/dart/dynamics/Branch.cpp index 01fde2a500b4b..88cfafd1681be 100644 --- a/dart/dynamics/Branch.cpp +++ b/dart/dynamics/Branch.cpp @@ -71,7 +71,9 @@ Branch::Criteria Branch::Criteria::convert(const Linkage::Criteria& criteria) BodyNodePtr startBodyNode = criteria.mStart.mNode.lock(); if (!startBodyNode) { - // TODO: Warning for invalid criteria + dtwarn << "[Chain::Criteria::convert] Failed in conversion because the " + << "start node of the input criteria is not valid anymore. Using " + << "the returning Criteria will lead to creating an empty Branch.\n"; return Branch::Criteria(nullptr); } From b169252783677c121271c27782996f6f0599fb93 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 6 Dec 2018 15:42:07 -0800 Subject: [PATCH 11/13] Use cloneSkeleton() instead of clone() --- examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp | 2 +- .../tutorialCollisions-Finished.cpp | 10 +++++----- tutorials/tutorialCollisions/tutorialCollisions.cpp | 10 +++++----- .../tutorialDominoes-Finished.cpp | 2 +- unittests/comprehensive/test_Dynamics.cpp | 4 ++-- unittests/comprehensive/test_ForwardKinematics.cpp | 4 ++-- unittests/comprehensive/test_Skeleton.cpp | 4 ++-- 7 files changed, 18 insertions(+), 18 deletions(-) diff --git a/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp b/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp index 7dff208537a76..90bb2c4703a3f 100644 --- a/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp +++ b/examples/osgExamples/osgHuboPuppet/osgHuboPuppet.cpp @@ -1379,7 +1379,7 @@ int main() Eigen::VectorXd positions = hubo->getPositions(); // We make a clone to test whether the cloned version behaves the exact same // as the original version. - hubo = hubo->clone("hubo_copy"); + hubo = hubo->cloneSkeleton("hubo_copy"); hubo->setPositions(positions); world->addSkeleton(hubo); diff --git a/tutorials/tutorialCollisions-Finished/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished/tutorialCollisions-Finished.cpp index 07ec2a2df325b..ee600f0a4b5b3 100644 --- a/tutorials/tutorialCollisions-Finished/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished/tutorialCollisions-Finished.cpp @@ -131,23 +131,23 @@ class MyWindow : public dart::gui::glut::SimWindow switch(key) { case '1': - addObject(mOriginalBall->clone()); + addObject(mOriginalBall->cloneSkeleton()); break; case '2': - addObject(mOriginalSoftBody->clone()); + addObject(mOriginalSoftBody->cloneSkeleton()); break; case '3': - addObject(mOriginalHybridBody->clone()); + addObject(mOriginalHybridBody->cloneSkeleton()); break; case '4': - addObject(mOriginalRigidChain->clone()); + addObject(mOriginalRigidChain->cloneSkeleton()); break; case '5': - addRing(mOriginalRigidRing->clone()); + addRing(mOriginalRigidRing->cloneSkeleton()); break; case 'd': diff --git a/tutorials/tutorialCollisions/tutorialCollisions.cpp b/tutorials/tutorialCollisions/tutorialCollisions.cpp index 8a7082d7b6eca..506347139b31d 100644 --- a/tutorials/tutorialCollisions/tutorialCollisions.cpp +++ b/tutorials/tutorialCollisions/tutorialCollisions.cpp @@ -114,23 +114,23 @@ class MyWindow : public SimWindow switch(key) { case '1': - addObject(mOriginalBall->clone()); + addObject(mOriginalBall->cloneSkeleton()); break; case '2': - addObject(mOriginalSoftBody->clone()); + addObject(mOriginalSoftBody->cloneSkeleton()); break; case '3': - addObject(mOriginalHybridBody->clone()); + addObject(mOriginalHybridBody->cloneSkeleton()); break; case '4': - addObject(mOriginalRigidChain->clone()); + addObject(mOriginalRigidChain->cloneSkeleton()); break; case '5': - addRing(mOriginalRigidRing->clone()); + addRing(mOriginalRigidRing->cloneSkeleton()); break; case 'd': diff --git a/tutorials/tutorialDominoes-Finished/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished/tutorialDominoes-Finished.cpp index 8b19842268671..2eb54db1e0a50 100644 --- a/tutorials/tutorialDominoes-Finished/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished/tutorialDominoes-Finished.cpp @@ -242,7 +242,7 @@ class MyWindow : public dart::gui::glut::SimWindow void attemptToCreateDomino(double angle) { // Create a new domino - SkeletonPtr newDomino = mFirstDomino->clone(); + SkeletonPtr newDomino = mFirstDomino->cloneSkeleton(); newDomino->setName("domino #" + std::to_string(mDominoes.size() + 1)); const SkeletonPtr& lastDomino = mDominoes.size() > 0 ? diff --git a/unittests/comprehensive/test_Dynamics.cpp b/unittests/comprehensive/test_Dynamics.cpp index 09703e8b58207..a35efdd0a2cac 100644 --- a/unittests/comprehensive/test_Dynamics.cpp +++ b/unittests/comprehensive/test_Dynamics.cpp @@ -781,7 +781,7 @@ void DynamicsTest::testJacobians(const common::Uri& uri) // For the second half of the tests, scramble up the Skeleton if(j > std::ceil(nTestItr/2)) { - SkeletonPtr copy = skeleton->clone(); + SkeletonPtr copy = skeleton->cloneSkeleton(); 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)); @@ -1716,7 +1716,7 @@ void DynamicsTest::testCenterOfMass(const common::Uri& uri) // For the second half of the tests, scramble up the Skeleton if(j > ceil(nRandomItr/2)) { - SkeletonPtr copy = skeleton->clone(); + SkeletonPtr copy = skeleton->cloneSkeleton(); 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)); diff --git a/unittests/comprehensive/test_ForwardKinematics.cpp b/unittests/comprehensive/test_ForwardKinematics.cpp index b38e9ae3b8254..11d31c6e8c332 100644 --- a/unittests/comprehensive/test_ForwardKinematics.cpp +++ b/unittests/comprehensive/test_ForwardKinematics.cpp @@ -181,7 +181,7 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_PARTIAL_CHANGE) SkeletonPtr skeleton1 = loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf"); - SkeletonPtr skeleton2 = skeleton1->clone(); + SkeletonPtr skeleton2 = skeleton1->cloneSkeleton(); std::vector active_indices; for(std::size_t i=0; i < 3; ++i) @@ -225,7 +225,7 @@ TEST(FORWARD_KINEMATICS, JACOBIAN_END_EFFECTOR_CHANGE) BodyNode* last_bn1 = skeleton1->getBodyNode(skeleton1->getNumBodyNodes()-1); EndEffector* ee1 = last_bn1->createEndEffector(); - SkeletonPtr skeleton2 = skeleton1->clone(); + SkeletonPtr skeleton2 = skeleton1->cloneSkeleton(); BodyNode* last_bn2 = skeleton2->getBodyNode(skeleton2->getNumBodyNodes()-1); EndEffector* ee2 = last_bn2->createEndEffector(); diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index 026d83ba1603f..1d337810ebdf9 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -119,7 +119,7 @@ TEST(Skeleton, Restructuring) index = std::min(index, skeletons.size()-1); SkeletonPtr skeleton = skeletons[index]; EXPECT_TRUE(skeleton->checkIndexingConsistency()); - SkeletonPtr original = skeleton->clone(); + SkeletonPtr original = skeleton->cloneSkeleton(); EXPECT_TRUE(original->checkIndexingConsistency()); std::size_t maxNode = skeleton->getNumBodyNodes()-1; @@ -751,7 +751,7 @@ TEST(Skeleton, CloneNodeOrdering) skel->getBodyNode(0)->createEndEffector("another_manip"); skel->getBodyNode(2)->createEndEffector("yet_another_manip"); - SkeletonPtr clone = skel->clone(); + SkeletonPtr clone = skel->cloneSkeleton(); for(std::size_t i=0; i < skel->getNumEndEffectors(); ++i) { From 2f2a947f13c9c72448540922d3011aaf51e05fe9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 16 Dec 2018 01:59:03 -0800 Subject: [PATCH 12/13] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4a65034ed22df..7ead6c67fa7b7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ * Refactor constraint solver: [#1099](https://github.com/dartsim/dart/pull/1099), [#1101](https://github.com/dartsim/dart/pull/1101) * Added mimic joint functionality as a new actuator type: [#1178](https://github.com/dartsim/dart/pull/1178) + * Added clone function to MetaSkeleton: [#1201](https://github.com/dartsim/dart/pull/1178) * Optimization From c69e87513f35e4374376a1ffa5db436b711d2606 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 16 Dec 2018 01:59:48 -0800 Subject: [PATCH 13/13] Remove outdated comment --- dart/dynamics/MetaSkeleton.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index 39ad499e6ae92..7f66281e59490 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -75,8 +75,7 @@ class MetaSkeleton : public common::Subject virtual MetaSkeletonPtr cloneMetaSkeleton( const std::string& cloneName) const = 0; // TODO: In DART7, rename this to clone() and change the current - // Skeleton::clone() to override it. Then consider adding - // Skeleton::cloneSkeleton(). + // Skeleton::clone() to override it. /// Creates an identical clone of this MetaSkeleton MetaSkeletonPtr cloneMetaSkeleton() const;