Add a new Criteria constructor for creating sequence

CHANGELOG.md

@@ -4,6 +4,10 @@
 
 #### Changes
 
+* Kinematics
+
+  * Added an utility constructor to Linkage::Criteria to create sequence Linkage: [#1273](https://github.com/dartsim/dart/pull/1273)
+
 * Dynamics
 
   * Fixed incorrect transpose check in Inertia::verifySpatialTensor(): [#1258](https://github.com/dartsim/dart/pull/1258)

dart/dynamics/Chain.cpp

@@ -37,11 +37,11 @@ namespace dart {
 namespace dynamics {
 
 //==============================================================================
-Chain::Criteria::Criteria(BodyNode* _start, BodyNode* _target,
-                          bool _includeBoth)
-  : mStart(_start),
-    mTarget(_target),
-    mIncludeBoth(_includeBoth)
+Chain::Criteria::Criteria(BodyNode* start, BodyNode* target,
+                          bool includeUpstreamParentJoint)
+  : mStart(start),
+    mTarget(target),
+    mIncludeUpstreamParentJoint(includeUpstreamParentJoint)
 {
   // Do nothing
 }
@@ -64,7 +64,7 @@ Linkage::Criteria Chain::Criteria::convert() const
   target.mChain = true;
   target.mPolicy = Linkage::Criteria::INCLUDE;
 
-  if(!mIncludeBoth)
+  if(!mIncludeUpstreamParentJoint)
   {
     if(target.mNode.lock() &&
        target.mNode.lock()->descendsFrom(criteria.mStart.mNode.lock()))
@@ -115,14 +115,14 @@ Chain::Criteria Chain::Criteria::convert(const Linkage::Criteria& criteria)
     return Chain::Criteria(nullptr, nullptr);
   }
 
-  bool includeBoth = true;
+  bool includeUpstreamParentJoint = true;
   if (criteria.mStart.mPolicy != Linkage::Criteria::INCLUDE)
-    includeBoth = false;
+    includeUpstreamParentJoint = false;
   if (target.mPolicy != Linkage::Criteria::INCLUDE)
-    includeBoth = false;
+    includeUpstreamParentJoint = false;
 
   return Chain::Criteria(
-      startBodyNode.get(), targetBodyNode.get(), includeBoth);
+      startBodyNode.get(), targetBodyNode.get(), includeUpstreamParentJoint);
 }
 
 //==============================================================================
@@ -151,9 +151,9 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target,
 
 //==============================================================================
 ChainPtr Chain::create(BodyNode* _start, BodyNode* _target,
-                       IncludeBothTag, const std::string& _name)
+                       IncludeUpstreamParentJointTag, const std::string& _name)
 {
-  ChainPtr chain(new Chain(_start, _target, IncludeBoth, _name));
+  ChainPtr chain(new Chain(_start, _target, IncludeUpstreamParentJoint, _name));
   chain->mPtr = chain;
   return chain;
 }
@@ -243,7 +243,7 @@ Chain::Chain(BodyNode* _start, BodyNode* _target, const std::string& _name)
 
 //==============================================================================
 Chain::Chain(BodyNode* _start, BodyNode* _target,
-             IncludeBothTag, const std::string& _name)
+             IncludeUpstreamParentJointTag, const std::string& _name)
   : Linkage(Chain::Criteria(_start, _target, true), _name)
 {
   // Do nothing

dart/dynamics/Chain.hpp

@@ -50,7 +50,10 @@ class Chain : public Linkage
   struct Criteria
   {
     /// Constructor for Chain::Criteria
-    Criteria(BodyNode* _start, BodyNode* _target, bool _includeBoth = false);
+    Criteria(
+      BodyNode* start,
+      BodyNode* target,
+      bool includeUpstreamParentJoint = false);
 
     /// Return a vector of BodyNodes that form a chain
     std::vector<BodyNode*> satisfy() const;
@@ -62,10 +65,9 @@ class Chain : public Linkage
     /// branching or a FreeJoint along the way
     WeakBodyNodePtr mTarget;
 
-    /// Set this to true if both the start and the target BodyNode should be
-    /// included. Otherwise, whichever is upstream of the other will be left out
-    /// of the chain.
-    bool mIncludeBoth;
+    /// Set this to true if the parent joint of whichever is upstream of the
+    /// other should be included.
+    bool mIncludeUpstreamParentJoint;
 
     /// Convert this Criteria into Linkage::Criteria
     Linkage::Criteria convert() const;
@@ -77,10 +79,10 @@ class Chain : public Linkage
     static Criteria convert(const Linkage::Criteria& criteria);
   };
 
-  /// This enum is used to specify to the create() function that both the start
-  /// and the target BodyNodes should be included in the Chain that gets
-  /// generated.
-  enum IncludeBothTag { IncludeBoth };
+  /// This enum is used to specify to the create() function that the parent
+  /// joint of whichever is upstream of the other should be included in the
+  /// Chain that gets generated.
+  enum IncludeUpstreamParentJointTag { IncludeUpstreamParentJoint };
 
   /// Create a Chain given some Chain::Criteria
   static ChainPtr create(const Chain::Criteria& _criteria,
@@ -93,8 +95,11 @@ class Chain : public Linkage
 
   /// Create a Chain given a start and a target BodyNode. In this version, both
   /// BodyNodes will be included in the Chain that gets created.
-  static ChainPtr create(BodyNode* _start, BodyNode* _target,
-                         IncludeBothTag, const std::string& _name = "Chain");
+  static ChainPtr create(
+      BodyNode* _start,
+      BodyNode* _target,
+      IncludeUpstreamParentJointTag,
+      const std::string& _name = "Chain");
 
   /// Creates and returns a clone of this Chain.
   ChainPtr cloneChain() const;
@@ -123,7 +128,7 @@ class Chain : public Linkage
 
   /// Alternative constructor for the Chain class
   Chain(BodyNode* _start, BodyNode* _target,
-        IncludeBothTag, const std::string& _name = "Chain");
+        IncludeUpstreamParentJointTag, const std::string& _name = "Chain");
 
 };
 

dart/dynamics/Linkage.cpp

@@ -124,6 +124,36 @@ Linkage::Criteria::Terminal::Terminal(BodyNode* _terminal, bool _inclusive)
   // Do nothing
 }
 
+//==============================================================================
+Linkage::Criteria::Criteria(
+    BodyNode* start, BodyNode* target, bool includeUpstreamParentJoint)
+{
+  mStart.mNode = start;
+  mStart.mPolicy = Linkage::Criteria::INCLUDE;
+
+  Target endPoint;
+  endPoint.mNode = target;
+  endPoint.mChain = false;
+  endPoint.mPolicy = Linkage::Criteria::INCLUDE;
+
+  if (!includeUpstreamParentJoint)
+  {
+    if(endPoint.mNode.lock() &&
+       endPoint.mNode.lock()->descendsFrom(mStart.mNode.lock()))
+    {
+      mStart.mPolicy = Linkage::Criteria::EXCLUDE;
+    }
+
+    if (mStart.mNode.lock() &&
+       mStart.mNode.lock()->descendsFrom(endPoint.mNode.lock()))
+    {
+      endPoint.mPolicy = Linkage::Criteria::EXCLUDE;
+    }
+  }
+
+  mTargets.push_back(endPoint);
+}
+
 //==============================================================================
 void Linkage::Criteria::refreshTerminalMap() const
 {

dart/dynamics/Linkage.hpp

@@ -127,6 +127,26 @@ class Linkage : public ReferentialSkeleton
     /// entry in mTargets) will be halted if it reaches any entry in mTerminal
     std::vector<Terminal> mTerminals;
 
+    /// Constructs an empty criteria that will lead to creating an empty Linkage
+    Criteria() = default;
+
+    /// Utility constructor to create a contiguous sequence of BodyNodes that
+    /// doesn't include branches in it.
+    ///
+    /// The start and target can be interchangeably set if both are included in
+    /// the sequence.
+    ///
+    /// \param[in] start The first BodyNode in the sequence. If \c nullptr is
+    /// passed the sequence will expand from \c target to the root.
+    /// \param[in] target The second BodyNode in the sequence. If \c nullptr is
+    /// passed the sequence will expand from \c start to the root.
+    /// \param[in] includeUpstreamParentJoint Set this to true if the parent
+    /// joint of whichever is upstream of the other should be included.
+    Criteria(
+        BodyNode* start,
+        BodyNode* target,
+        bool includeUpstreamParentJoint = false);
+
   protected:
 
     /// Refresh the content of mMapOfTerminals

unittests/comprehensive/test_MetaSkeleton.cpp

@@ -238,6 +238,27 @@ BodyNode* addBodyNode(BodyNode* bn, const std::string& name)
 //==============================================================================
 SkeletonPtr constructLinkageTestSkeleton()
 {
+  // Skeleton structure:
+  //
+  //          c1b1 (root)
+  //           |
+  //          c1b2
+  //           |
+  //          c1b3
+  //   +-------+--------+
+  // c2b1     c3b1     c5b1
+  //   |       |        |
+  // c2b2     c3b2     c5b2
+  //   |       |
+  // c2b3     c3b3
+  //       +---+---+
+  //      c4b1    c3b4
+  //       |
+  //      c4b2
+  //       |
+  //      c4b3
+  //
+
   SkeletonPtr skel = Skeleton::create();
   BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
   bn->setName("c1b1");
@@ -391,7 +412,8 @@ TEST(MetaSkeleton, Linkage)
   EXPECT_TRUE( count == combinedSkelBases->getNumBodyNodes() );
 
   ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"),
-      skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint");
+      skel->getBodyNode("c1b3"), Chain::IncludeUpstreamParentJoint,
+      "downstreamFreeJoint");
   checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1");
   checkLinkageJointConsistency(downstreamFreeJoint);
 
@@ -444,6 +466,52 @@ TEST(MetaSkeleton, Linkage)
   checkForBodyNodes(terminatedUpstream, skel, true,
                     "c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1");
   checkLinkageJointConsistency(terminatedUpstream);
+
+  // Sequence linkage 1
+  Linkage::Criteria sequenceCriteria1(
+      skel->getBodyNode("c1b3"), skel->getBodyNode("c4b1"), true);
+  LinkagePtr sequenceLinkage1 = Linkage::create(sequenceCriteria1, "sequence");
+  checkForBodyNodes(
+      sequenceLinkage1, skel, true, "c1b3", "c3b1", "c3b2", "c3b3", "c4b1");
+  checkLinkageJointConsistency(sequenceLinkage1);
+
+  // Sequence linkage 2: nullptr start
+  Linkage::Criteria sequenceCriteria2(nullptr, skel->getBodyNode("c4b1"), true);
+  LinkagePtr sequenceLinkage2 = Linkage::create(sequenceCriteria2, "sequence");
+  checkForBodyNodes(
+      sequenceLinkage2, skel, true,
+      "c1b1", "c1b2", "c1b3", "c3b1", "c3b2", "c3b3", "c4b1");
+  checkLinkageJointConsistency(sequenceLinkage2);
+
+  // Sequence linkage 3: nullptr target
+  Linkage::Criteria sequenceCriteria3(skel->getBodyNode("c1b3"), nullptr, true);
+  LinkagePtr sequenceLinkage3 = Linkage::create(sequenceCriteria3, "sequence");
+  checkForBodyNodes(sequenceLinkage3, skel, true, "c1b1", "c1b2", "c1b3");
+  checkLinkageJointConsistency(sequenceLinkage3);
+
+  // Sequence linkage 4: reversed
+  Linkage::Criteria sequenceCriteria4(
+      skel->getBodyNode("c4b1"), skel->getBodyNode("c1b3"), true);
+  LinkagePtr sequenceLinkage4
+      = Linkage::create(sequenceCriteria4, "sequence");
+  checkForBodyNodes(
+      sequenceLinkage4, skel, true,
+      "c1b3", "c3b1", "c3b2", "c3b3", "c4b1");
+  checkLinkageJointConsistency(sequenceLinkage4);
+
+  // Sequence linkage 5: empty
+  Linkage::Criteria sequenceCriteria5(nullptr, nullptr, true);
+  LinkagePtr sequenceLinkage5 = Linkage::create(sequenceCriteria5, "sequence");
+  checkForBodyNodes(sequenceLinkage5, skel, true);
+  checkLinkageJointConsistency(sequenceLinkage5);
+
+  // Sequence linkage 6: not include both
+  Linkage::Criteria sequenceCriteria6(
+      skel->getBodyNode("c1b3"), skel->getBodyNode("c4b1"), false);
+  LinkagePtr sequenceLinkage6 = Linkage::create(sequenceCriteria6, "sequence");
+  checkForBodyNodes(
+      sequenceLinkage6, skel, true, "c3b1", "c3b2", "c3b3", "c4b1");
+  checkLinkageJointConsistency(sequenceLinkage6);
 }
 
 //==============================================================================