orocos_kdl
tree.hpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #ifndef KDL_TREE_HPP
23 #define KDL_TREE_HPP
24 
25 #include "config.h"
26 
27 #include "segment.hpp"
28 #include "chain.hpp"
29 
30 #include <string>
31 #include <map>
32 
33 #ifdef KDL_USE_NEW_TREE_INTERFACE
34 #include <boost/shared_ptr.hpp>
35 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
36 
37 namespace KDL
38 {
39  class TreeElement;
40 
41 #ifdef KDL_USE_NEW_TREE_INTERFACE
42  //We use smart pointers for managing tree nodes for now because
43  //c++11 and unique_ptr support is not ubiquitous
44  typedef boost::shared_ptr<TreeElement> TreeElementPtr;
45  typedef boost::shared_ptr<const TreeElement> TreeElementConstPtr;
47  typedef TreeElementPtr TreeElementType;
48 
49 #define GetTreeElementChildren(tree_element) (tree_element)->children
50 #define GetTreeElementParent(tree_element) (tree_element)->parent
51 #define GetTreeElementQNr(tree_element) (tree_element)->q_nr
52 #define GetTreeElementSegment(tree_element) (tree_element)->segment
53 
54 #else //#ifdef KDL_USE_NEW_TREE_INTERFACE
55  //Forward declaration
58 
59 #define GetTreeElementChildren(tree_element) (tree_element).children
60 #define GetTreeElementParent(tree_element) (tree_element).parent
61 #define GetTreeElementQNr(tree_element) (tree_element).q_nr
62 #define GetTreeElementSegment(tree_element) (tree_element).segment
63 
64 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
65 
67  {
68  public:
69  TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in):
70  segment(segment_in),
71  q_nr(q_nr_in),
72  parent(parent_in)
73  {}
74 
75  static TreeElementType Root(const std::string& root_name)
76  {
77 #ifdef KDL_USE_NEW_TREE_INTERFACE
78  return TreeElementType(new TreeElement(root_name));
79 #else //#define KDL_USE_NEW_TREE_INTERFACE
80  return TreeElementType(root_name);
81 #endif
82  }
83 
85  unsigned int q_nr;
86  SegmentMap::const_iterator parent;
88 
89  private:
90  TreeElement(const std::string& name):segment(name), q_nr(0) {}
91  };
92 
99  class Tree
100  {
101  private:
103  unsigned int nrOfJoints;
104  unsigned int nrOfSegments;
105 
107 
108  bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
109 
110  public:
114  explicit Tree(const std::string& root_name="root");
115  Tree(const Tree& in);
116  Tree& operator= (const Tree& arg);
117 
128  bool addSegment(const Segment& segment, const std::string& hook_name);
129 
138  bool addChain(const Chain& chain, const std::string& hook_name);
139 
149  bool addTree(const Tree& tree, const std::string& hook_name);
150 
159  unsigned int getNrOfJoints()const
160  {
161  return nrOfJoints;
162  };
163 
168  unsigned int getNrOfSegments()const {return nrOfSegments;};
169 
177  SegmentMap::const_iterator getSegment(const std::string& segment_name)const
178  {
179  return segments.find(segment_name);
180  };
186  SegmentMap::const_iterator getRootSegment()const
187  {
188  return segments.find(root_name);
189  };
190 
202  bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const;
203 
204 
205 
216  bool getSubTree(const std::string& segment_name, Tree& tree)const;
217 
218  const SegmentMap& getSegments()const
219  {
220  return segments;
221  }
222 
223  virtual ~Tree(){};
224 
225  };
226 }
227 #endif
chain.hpp
KDL::Tree::nrOfJoints
unsigned int nrOfJoints
Definition: tree.hpp:103
KDL::Tree::Tree
Tree(const std::string &root_name="root")
Definition: tree.cpp:27
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
std::string
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
KDL::Tree::addTreeRecursive
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string &hook_name)
Definition: tree.cpp:97
KDL::TreeElement::parent
SegmentMap::const_iterator parent
Definition: tree.hpp:86
std::vector< SegmentMap::const_iterator >
std::map::find
T find(T... args)
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Tree::root_name
std::string root_name
Definition: tree.hpp:106
KDL::Tree::addChain
bool addChain(const Chain &chain, const std::string &hook_name)
Definition: tree.cpp:82
KDL::Tree::getSegments
const SegmentMap & getSegments() const
Definition: tree.hpp:218
KDL::Tree::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: tree.hpp:168
KDL::TreeElement::children
std::vector< SegmentMap::const_iterator > children
Definition: tree.hpp:87
KDL::Tree::~Tree
virtual ~Tree()
Definition: tree.hpp:223
KDL::TreeElementType
TreeElement TreeElementType
Definition: tree.hpp:57
KDL::Tree::getSubTree
bool getSubTree(const std::string &segment_name, Tree &tree) const
Definition: tree.cpp:165
map
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::TreeElement::TreeElement
TreeElement(const std::string &name)
Definition: tree.hpp:90
KDL::SegmentMap
std::map< std::string, TreeElement > SegmentMap
Definition: tree.hpp:39
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
Definition: segment.hpp:46
KDL::Tree::addSegment
bool addSegment(const Segment &segment, const std::string &hook_name)
Definition: tree.cpp:54
KDL::TreeElement::TreeElement
TreeElement(const Segment &segment_in, const SegmentMap::const_iterator &parent_in, unsigned int q_nr_in)
Definition: tree.hpp:69
KDL::TreeElement::q_nr
unsigned int q_nr
Definition: tree.hpp:85
KDL::Tree::addTree
bool addTree(const Tree &tree, const std::string &hook_name)
Definition: tree.cpp:93
KDL::Tree::getChain
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Definition: tree.cpp:116
KDL::Tree::segments
SegmentMap segments
Definition: tree.hpp:102
KDL::Tree::nrOfSegments
unsigned int nrOfSegments
Definition: tree.hpp:104
KDL::TreeElement::Root
static TreeElementType Root(const std::string &root_name)
Definition: tree.hpp:75
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Tree::getSegment
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Definition: tree.hpp:177
KDL::TreeElement
Definition: tree.hpp:66
segment.hpp
KDL::TreeElement::segment
Segment segment
Definition: tree.hpp:84
KDL::Tree::operator=
Tree & operator=(const Tree &arg)
Definition: tree.cpp:43
string