Add hasBodyNode() and hasJoint() to MetaSkeleton

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)

dart/dynamics/MetaSkeleton.hpp

@@ -125,6 +125,9 @@ class MetaSkeleton : public common::Subject
   virtual std::vector<const BodyNode*> 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<const Joint*> 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

dart/dynamics/ReferentialSkeleton.cpp

@@ -167,6 +167,13 @@ std::vector<const BodyNode*> 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<const Joint*> 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
 {

dart/dynamics/ReferentialSkeleton.hpp

@@ -114,6 +114,9 @@ class ReferentialSkeleton : public MetaSkeleton
   std::vector<const BodyNode*> 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<const Joint*> 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;
 

dart/dynamics/Skeleton.cpp

@@ -913,6 +913,14 @@ std::vector<const BodyNode*> Skeleton::getBodyNodes(
     return std::vector<const BodyNode*>();
 }
 
+//==============================================================================
+bool Skeleton::hasBodyNode(const BodyNode* bodyNode) const
+{
+  return std::find(
+      mSkelCache.mBodyNodes.begin(), mSkelCache.mBodyNodes.end(), bodyNode)
+      != mSkelCache.mBodyNodes.end();
+}
+
 //==============================================================================
 template <class ObjectT, std::size_t (ObjectT::*getIndexInSkeleton)() const>
 static std::size_t templatedGetIndexOf(const Skeleton* _skel, const ObjectT* _obj,
@@ -1058,6 +1066,18 @@ std::vector<const Joint*> Skeleton::getJoints(const std::string& name) const
     return std::vector<const Joint*>();
 }
 
+//==============================================================================
+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
 {

dart/dynamics/Skeleton.hpp

@@ -407,6 +407,9 @@ class Skeleton :
   std::vector<const BodyNode*> 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<const Joint*> 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;
 

unittests/comprehensive/test_Skeleton.cpp

@@ -810,6 +810,12 @@ TEST(Skeleton, Referential)
   for(std::size_t i=0; i<skeletons.size(); ++i)
   {
     SkeletonPtr skeleton = skeletons[i];
+
+    const auto& skelJoints = skeleton->getJoints();
+    EXPECT_TRUE(skeleton->getNumJoints() == skelJoints.size());
+    for(auto* joint : skelJoints)
+      EXPECT_TRUE(skeleton->hasJoint(joint));
+
     for(std::size_t j=0; j<skeleton->getNumTrees(); ++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<DegreeOfFreedom*>& skelDofs = skeleton->getTreeDofs(j);