From e338f1b2158cbaf4beec8c28ee14f2c79c9aebf6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Feb 2018 04:14:43 -0800 Subject: [PATCH 1/4] Add hasBodyNode() and hasJoint() to MetaSkeleton --- dart/dynamics/MetaSkeleton.hpp | 6 ++++++ dart/dynamics/ReferentialSkeleton.cpp | 13 +++++++++++++ dart/dynamics/ReferentialSkeleton.hpp | 6 ++++++ dart/dynamics/Skeleton.cpp | 22 +++++++++++++++++++++- dart/dynamics/Skeleton.hpp | 6 ++++++ unittests/comprehensive/test_Skeleton.cpp | 8 ++++++++ 6 files changed, 60 insertions(+), 1 deletion(-) 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..ce1204902a0a9 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -910,7 +910,15 @@ std::vector Skeleton::getBodyNodes( if (bodyNode) return {bodyNode}; else - return std::vector(); + return std::vector(); +} + +//============================================================================== +bool Skeleton::hasBodyNode(const BodyNode* bodyNode) const +{ + return std::find( + mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), bodyNode) + != mSkelCache.mBodyNodes.end(); } //============================================================================== @@ -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..7c0780fbba339 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; + // 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..abc59c6a57032 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,8 +826,10 @@ 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); EXPECT_TRUE(tree->getNumDofs() == skelDofs.size()); for(DegreeOfFreedom* dof : skelDofs) From 6ee7da9f4fe7fc56d617f78b6579d55adb74ed98 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Feb 2018 04:16:40 -0800 Subject: [PATCH 2/4] Code format --- dart/dynamics/Skeleton.cpp | 2 +- unittests/comprehensive/test_Skeleton.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index ce1204902a0a9..2943d6057a35a 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -910,7 +910,7 @@ std::vector Skeleton::getBodyNodes( if (bodyNode) return {bodyNode}; else - return std::vector(); + return std::vector(); } //============================================================================== diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index abc59c6a57032..64c365e9e429d 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -829,7 +829,6 @@ TEST(Skeleton, Referential) EXPECT_TRUE(skeleton->hasBodyNode(bn)); } - const std::vector& skelDofs = skeleton->getTreeDofs(j); EXPECT_TRUE(tree->getNumDofs() == skelDofs.size()); for(DegreeOfFreedom* dof : skelDofs) From 5c16c72ae49d019cab9a9fd19058e39d55f36ce1 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 20 Feb 2018 09:39:41 -0800 Subject: [PATCH 3/4] Add missing override specifier --- dart/dynamics/Skeleton.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index 7c0780fbba339..e18362ee5401e 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -453,7 +453,7 @@ class Skeleton : std::vector getJoints(const std::string& name) const override; // Documentation inherited - bool hasJoint(const Joint* joint) const; + bool hasJoint(const Joint* joint) const override; // Documentation inherited std::size_t getIndexOf(const Joint* _joint, bool _warning=true) const override; From 640e37a48c3bb3ea5549d6949a26b2932b70260e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 21 Feb 2018 07:59:28 -0800 Subject: [PATCH 4/4] Update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) 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)