diff --git a/CHANGELOG.md b/CHANGELOG.md index db837c88d5abb..45ecc4a4cb0fc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,7 @@ * Misc * Added World::create(): [#962](https://github.com/dartsim/dart/pull/962) + * Added MetaSkeleton::hasBodyNode() and MetaSkeleton::hasJoint(): [#1000](https://github.com/dartsim/dart/pull/1000) * Suppressed -Winjected-class-name warnings from Clang 5.0.0: [#964](https://github.com/dartsim/dart/pull/964) * Suppressed -Wdangling-else warnings from GCC 7.2.0: [#937](https://github.com/dartsim/dart/pull/937) * Fixed various build issues with Visual Studio: [#956](https://github.com/dartsim/dart/pull/956) diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index bbc443f8eea77..c91c117282b56 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -125,6 +125,9 @@ class MetaSkeleton : public common::Subject virtual std::vector getBodyNodes( const std::string& name) const = 0; + /// Returns whether this Skeleton contains \c bodyNode. + virtual bool hasBodyNode(const BodyNode* bodyNode) const = 0; + /// Get the index of a specific BodyNode within this ReferentialSkeleton. /// Returns INVALID_INDEX if it is not held in this ReferentialSkeleton. /// When _warning is true, a warning message will be printed if the BodyNode @@ -175,6 +178,9 @@ class MetaSkeleton : public common::Subject virtual std::vector getJoints( const std::string& name) const = 0; + /// Returns whether this Skeleton contains \c join. + virtual bool hasJoint(const Joint* joint) const = 0; + /// Get the index of a specific Joint within this ReferentialSkeleton. Returns /// INVALID_INDEX if it is not held in this ReferentialSkeleton. /// When _warning is true, a warning message will be printed if the Joint is diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index 7e27cbb8b27e0..27f582367886c 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -167,6 +167,13 @@ std::vector ReferentialSkeleton::getBodyNodes( return bodyNodes; } +//============================================================================== +bool ReferentialSkeleton::hasBodyNode(const BodyNode* bodyNode) const +{ + return std::find(mBodyNodes.begin(), mBodyNodes.end(), bodyNode) + != mBodyNodes.end(); +} + //============================================================================== std::size_t ReferentialSkeleton::getIndexOf(const BodyNode* _bn, bool _warning) const { @@ -301,6 +308,12 @@ std::vector ReferentialSkeleton::getJoints( return joints; } +//============================================================================== +bool ReferentialSkeleton::hasJoint(const Joint* joint) const +{ + return std::find(mJoints.begin(), mJoints.end(), joint) != mJoints.end(); +} + //============================================================================== std::size_t ReferentialSkeleton::getIndexOf(const Joint* _joint, bool _warning) const { diff --git a/dart/dynamics/ReferentialSkeleton.hpp b/dart/dynamics/ReferentialSkeleton.hpp index 31f62909f6759..ef12f3143ae04 100644 --- a/dart/dynamics/ReferentialSkeleton.hpp +++ b/dart/dynamics/ReferentialSkeleton.hpp @@ -114,6 +114,9 @@ class ReferentialSkeleton : public MetaSkeleton std::vector getBodyNodes( const std::string& name) const override; + // Documentation inherited + bool hasBodyNode(const BodyNode* bodyNode) const override; + // Documentation inherited std::size_t getIndexOf(const BodyNode* _bn, bool _warning=true) const override; @@ -160,6 +163,9 @@ class ReferentialSkeleton : public MetaSkeleton /// name when ReferentialSkeleton contains Joints from multiple Skeletons. std::vector getJoints(const std::string& name) const override; + // Documentation inherited + bool hasJoint(const Joint* joint) const override; + // Documentation inherited std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const override; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 97ea7bacf8434..2943d6057a35a 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -913,6 +913,14 @@ std::vector Skeleton::getBodyNodes( return std::vector(); } +//============================================================================== +bool Skeleton::hasBodyNode(const BodyNode* bodyNode) const +{ + return std::find( + mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), bodyNode) + != mSkelCache.mBodyNodes.end(); +} + //============================================================================== template static std::size_t templatedGetIndexOf(const Skeleton* _skel, const ObjectT* _obj, @@ -1058,6 +1066,18 @@ std::vector Skeleton::getJoints(const std::string& name) const return std::vector(); } +//============================================================================== +bool Skeleton::hasJoint(const Joint* joint) const +{ + return std::find_if( + mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), + [&joint](const BodyNode* bodyNode) + { + return bodyNode->getParentJoint() == joint; + }) + != mSkelCache.mBodyNodes.end(); +} + //============================================================================== std::size_t Skeleton::getIndexOf(const Joint* _joint, bool _warning) const { diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index a44dccee7b2b9..e18362ee5401e 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -407,6 +407,9 @@ class Skeleton : std::vector getBodyNodes( const std::string& name) const override; + // Documentation inherited + bool hasBodyNode(const BodyNode* bodyNode) const override; + // Documentation inherited std::size_t getIndexOf(const BodyNode* _bn, bool _warning=true) const override; @@ -449,6 +452,9 @@ class Skeleton : /// So this function returns the single Joint of the given name if it exists. std::vector getJoints(const std::string& name) const override; + // Documentation inherited + bool hasJoint(const Joint* joint) const override; + // Documentation inherited std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const override; diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index c6fe10ebe5f41..64c365e9e429d 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -810,6 +810,12 @@ TEST(Skeleton, Referential) 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)); @@ -820,6 +826,7 @@ TEST(Skeleton, Referential) { 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);