diff --git a/CHANGELOG.md b/CHANGELOG.md index 51f720185a9c4..e79f747c7a2f3 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/dart/dynamics/Chain.cpp b/dart/dynamics/Chain.cpp index e62048eb5b8e1..6df2ac7e98cf4 100644 --- a/dart/dynamics/Chain.cpp +++ b/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 diff --git a/dart/dynamics/Chain.hpp b/dart/dynamics/Chain.hpp index 890dcedb855ee..d0c0f0636873c 100644 --- a/dart/dynamics/Chain.hpp +++ b/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 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"); }; diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 808c9b79cf011..ae6b0bafd9f05 100644 --- a/dart/dynamics/Linkage.cpp +++ b/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 { diff --git a/dart/dynamics/Linkage.hpp b/dart/dynamics/Linkage.hpp index 411509288853a..2992e7d56cbde 100644 --- a/dart/dynamics/Linkage.hpp +++ b/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 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 diff --git a/unittests/comprehensive/test_MetaSkeleton.cpp b/unittests/comprehensive/test_MetaSkeleton.cpp index 681651c0261b6..171c70d648867 100644 --- a/unittests/comprehensive/test_MetaSkeleton.cpp +++ b/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().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); } //==============================================================================