DART  6.10.1
Linkage.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_LINKAGE_HPP_
34 #define DART_DYNAMICS_LINKAGE_HPP_
35 
36 #include <unordered_set>
38 
39 namespace dart {
40 namespace dynamics {
41 
54 {
55 public:
57  struct Criteria
58  {
62  {
63  INCLUDE = 0,
65  EXCLUDE,
67  DOWNSTREAM,
69  UPSTREAM
71  };
72 
74  std::vector<BodyNode*> satisfy() const;
75 
78  struct Target
79  {
81  Target(
82  BodyNode* _target = nullptr,
83  ExpansionPolicy _policy = INCLUDE,
84  bool _chain = false);
85 
88 
92 
96  bool mChain;
97  };
98 
102 
110  std::vector<Target> mTargets;
111 
117  struct Terminal
118  {
120  Terminal(BodyNode* _terminal = nullptr, bool _inclusive = true);
121 
124 
128  };
129 
132  std::vector<Terminal> mTerminals;
133 
135  Criteria() = default;
136 
149  Criteria(
150  BodyNode* start,
151  BodyNode* target,
152  bool includeUpstreamParentJoint = false);
153 
154  protected:
156  void refreshTerminalMap() const;
157 
159  void expansionPolicy(
160  BodyNode* _start,
161  ExpansionPolicy _policy,
162  std::vector<BodyNode*>& _bns) const;
163 
165  void expandDownstream(
166  BodyNode* _start,
167  std::vector<BodyNode*>& _bns,
168  bool _includeStart) const;
169 
171  void expandUpstream(
172  BodyNode* _start,
173  std::vector<BodyNode*>& _bns,
174  bool _includeStart) const;
175 
177  void expandToTarget(
178  const Target& _start,
179  const Target& _target,
180  std::vector<BodyNode*>& _bns) const;
181 
183  std::vector<BodyNode*> climbToTarget(
184  BodyNode* _start, BodyNode* _target) const;
185 
187  std::vector<BodyNode*> climbToCommonRoot(
188  const Target& _start, const Target& _target, bool _chain) const;
189 
192  void trimBodyNodes(
193  std::vector<BodyNode*>& _bns, bool _chain, bool _movingUpstream) const;
194 
196  mutable std::unordered_map<BodyNode*, bool> mMapOfTerminals;
197  };
198 
200  static LinkagePtr create(
201  const Criteria& _criteria, const std::string& _name = "Linkage");
202 
204  LinkagePtr cloneLinkage() const;
205 
207  LinkagePtr cloneLinkage(const std::string& cloneName) const;
208 
209  // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName.
211 
212  // Documentation inherited
214  const std::string& cloneName) const override;
215 
218  bool isAssembled() const;
219 
221  void reassemble();
222 
224  void satisfyCriteria();
225 
226 protected:
229  Linkage(const Criteria& _criteria, const std::string& _name = "Linkage");
230 
232  virtual void update();
233 
236 
239  std::vector<WeakBodyNodePtr> mParentBodyNodes;
240 };
241 
242 } // namespace dynamics
243 } // namespace dart
244 
245 #endif // DART_DYNAMICS_LINKAGE_HPP_
BodyNode class represents a single node of the skeleton.
Definition: BodyNode.hpp:79
A Linkage is a ReferentialSkeleton with the special property that all the BodyNodes included in it fo...
Definition: Linkage.hpp:54
LinkagePtr cloneLinkage() const
Creates and returns a clone of this Linkage.
Definition: Linkage.cpp:549
Linkage(const Criteria &_criteria, const std::string &_name="Linkage")
Constructor for the Linkage class.
Definition: Linkage.cpp:636
void reassemble()
Revert the assembly of this Linkage to its original structure.
Definition: Linkage.cpp:611
virtual void update()
Update any metadata needed by the Linkage or its derived classes.
Definition: Linkage.cpp:644
virtual MetaSkeletonPtr cloneMetaSkeleton(const std::string &cloneName) const=0
Creates an identical clone of this MetaSkeleton.
void satisfyCriteria()
Redefine this Linkage so that its Criteria is satisfied.
Definition: Linkage.cpp:621
std::vector< WeakBodyNodePtr > mParentBodyNodes
Recording of the parent BodyNodes that were held by each of the BodyNodes when the Linkage was constr...
Definition: Linkage.hpp:239
bool isAssembled() const
Returns false if the original assembly of this Linkage has been broken in some way.
Definition: Linkage.cpp:598
static LinkagePtr create(const Criteria &_criteria, const std::string &_name="Linkage")
Create a Linkage with the given Criteria.
Definition: Linkage.cpp:514
Criteria mCriteria
Criteria that defines the structure of this Linkage.
Definition: Linkage.hpp:235
MetaSkeletonPtr cloneMetaSkeleton() const
Creates an identical clone of this MetaSkeleton.
Definition: MetaSkeleton.cpp:328
ReferentialSkeleton is a base class used to implement Linkage, Group, and other classes that are used...
Definition: ReferentialSkeleton.hpp:48
std::shared_ptr< Linkage > LinkagePtr
Definition: SmartPointer.hpp:74
std::shared_ptr< MetaSkeleton > MetaSkeletonPtr
Definition: SmartPointer.hpp:68
Definition: BulletCollisionDetector.cpp:65
This structure defines targets for the expansion criteria and the desired behavior for those targets.
Definition: Linkage.hpp:79
bool mChain
If this is set to true, the expansion towards this target will terminate if (1) a fork/split in the k...
Definition: Linkage.hpp:96
WeakBodyNodePtr mNode
The Linkage will expand from the starting BodyNode up to this node.
Definition: Linkage.hpp:87
ExpansionPolicy mPolicy
After the target has been reached (if it is reached), the Linkage will start to follow this expansion...
Definition: Linkage.hpp:91
Target(BodyNode *_target=nullptr, ExpansionPolicy _policy=INCLUDE, bool _chain=false)
Default constructor for Target.
Definition: Linkage.cpp:109
Any expansion performed by the criteria will be halted if mTerminal is reached.
Definition: Linkage.hpp:118
bool mInclusive
Whether or not the BodyNode should be included after expansion has halted.
Definition: Linkage.hpp:127
WeakBodyNodePtr mTerminal
BodyNode that should halt any expansion.
Definition: Linkage.hpp:123
Terminal(BodyNode *_terminal=nullptr, bool _inclusive=true)
Default constructor for Terminal.
Definition: Linkage.cpp:117
The Criteria class is used to specify how a Linkage should be constructed.
Definition: Linkage.hpp:58
Target mStart
This Target will serve as the starting point for the criteria satisfaction.
Definition: Linkage.hpp:101
void expansionPolicy(BodyNode *_start, ExpansionPolicy _policy, std::vector< BodyNode * > &_bns) const
Satisfy the expansion policy of a target.
Definition: Linkage.cpp:164
std::vector< BodyNode * > climbToTarget(BodyNode *_start, BodyNode *_target) const
Expand upwards from the _start BodyNode to the _target BodyNode.
Definition: Linkage.cpp:401
std::vector< Target > mTargets
The Linkage will extend from mStart to each of these targets.
Definition: Linkage.hpp:110
void expandToTarget(const Target &_start, const Target &_target, std::vector< BodyNode * > &_bns) const
Construct a path from start to target.
Definition: Linkage.cpp:353
void refreshTerminalMap() const
Refresh the content of mMapOfTerminals.
Definition: Linkage.cpp:154
std::vector< Terminal > mTerminals
Any expansion (whether from an ExpansionPolicy or an attempt to reach an entry in mTargets) will be h...
Definition: Linkage.hpp:132
ExpansionPolicy
The ExpansionPolicy indicates how the collection of BodyNodes should expand from the starting BodyNod...
Definition: Linkage.hpp:62
@ UPSTREAM
Include the target, and then expand upstream, toward the root of the tree.
Definition: Linkage.hpp:69
@ EXCLUDE
Do not expand from the target.
Definition: Linkage.hpp:65
@ INCLUDE
Do not expand from the target.
Definition: Linkage.hpp:63
@ DOWNSTREAM
Include the target, and then expand downstream, toward the leaves of the tree.
Definition: Linkage.hpp:67
std::vector< BodyNode * > satisfy() const
Return a vector of BodyNodes that satisfy the parameters of the Criteria.
Definition: Linkage.cpp:43
std::unordered_map< BodyNode *, bool > mMapOfTerminals
Hashed set for terminals to allow quick lookup.
Definition: Linkage.hpp:196
std::vector< BodyNode * > climbToCommonRoot(const Target &_start, const Target &_target, bool _chain) const
Expand upwards from both BodyNodes to a common root.
Definition: Linkage.cpp:422
Criteria()=default
Constructs an empty criteria that will lead to creating an empty Linkage.
void trimBodyNodes(std::vector< BodyNode * > &_bns, bool _chain, bool _movingUpstream) const
Crawl through the list and cut it off anywhere that the criteria is violated.
Definition: Linkage.cpp:458
void expandDownstream(BodyNode *_start, std::vector< BodyNode * > &_bns, bool _includeStart) const
Expand downstream.
Definition: Linkage.cpp:253
void expandUpstream(BodyNode *_start, std::vector< BodyNode * > &_bns, bool _includeStart) const
Expand upstream.
Definition: Linkage.cpp:280