KDL  1.4.0
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;
46  typedef std::map<std::string, TreeElementPtr> SegmentMap;
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
56  typedef std::map<std::string,TreeElement> SegmentMap;
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;
87  std::vector<SegmentMap::const_iterator > children;
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 
106  std::string root_name;
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  const SegmentMap& getSegments()const
206  {
207  return segments;
208  }
209 
210  virtual ~Tree(){};
211 
212  };
213 }
214 #endif
215 
216 
217 
218 
219 
chain.hpp
KDL::Tree::nrOfJoints
unsigned int nrOfJoints
Definition: tree.hpp:103
KDL::Tree::Tree
Tree(const std::string &root_name="root")
The constructor of a tree, a new tree is always empty.
Definition: tree.cpp:28
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the tree.
Definition: tree.hpp:159
KDL::Joint::RotZ
@ RotZ
Definition: joint.hpp:47
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Request the root segment of the tree.
Definition: tree.hpp:186
KDL::Joint::RotY
@ RotY
Definition: joint.hpp:47
KDL::Tree::addTreeRecursive
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string &hook_name)
Definition: tree.cpp:98
KDL::TreeElement::parent
SegmentMap::const_iterator parent
Definition: tree.hpp:86
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
KDL::Joint::TransZ
@ TransZ
Definition: joint.hpp:47
KDL::Joint::JointOrigin
Vector JointOrigin() const
Request the Vector corresponding to the origin of a revolute joint.
Definition: joint.cpp:157
GetTreeElementParent
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
GetTreeElementChildren
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Joint::getName
const std::string & getName() const
Request the name of the joint.
Definition: joint.hpp:150
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::Tree::root_name
std::string root_name
Definition: tree.hpp:106
KDL::Joint::TransAxis
@ TransAxis
Definition: joint.hpp:47
KDL::Tree::addChain
bool addChain(const Chain &chain, const std::string &hook_name)
Adds a complete chain to the end of the segment with hook_name as segment_name.
Definition: tree.cpp:83
KDL::Tree::getSegments
const SegmentMap & getSegments() const
Definition: tree.hpp:205
KDL::Tree::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the tree.
Definition: tree.hpp:168
KDL::TreeElement::children
std::vector< SegmentMap::const_iterator > children
Definition: tree.hpp:87
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::Joint::RotX
@ RotX
Definition: joint.hpp:47
KDL::Segment::pose
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
KDL::Tree::~Tree
virtual ~Tree()
Definition: tree.hpp:210
KDL::Frame
Definition: frames.hpp:570
tree.hpp
KDL::TreeElementType
TreeElement TreeElementType
Definition: tree.hpp:57
KDL::Joint::TransX
@ TransX
Definition: joint.hpp:47
KDL::Tree
This class encapsulates a tree kinematic interconnection structure.
Definition: tree.hpp:99
KDL::Joint::None
@ None
Definition: joint.hpp:47
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
Definition: segment.hpp:46
KDL::Tree::addSegment
bool addSegment(const Segment &segment, const std::string &hook_name)
Adds a new segment to the end of the segment with hook_name as segment_name.
Definition: tree.cpp:55
KDL::Joint::RotAxis
@ RotAxis
Definition: joint.hpp:47
KDL::TreeElement::TreeElement
TreeElement(const Segment &segment_in, const SegmentMap::const_iterator &parent_in, unsigned int q_nr_in)
Definition: tree.hpp:69
KDL::Chain::addSegment
void addSegment(const Segment &segment)
Adds a new segment to the end of the chain.
Definition: chain.cpp:54
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)
Adds a complete tree to the end of the segment with hookname as segment_name.
Definition: tree.cpp:94
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::Segment::getName
const std::string & getName() const
Request the name of the segment.
Definition: segment.hpp:108
KDL::Tree::getChain
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Request the chain of the tree between chain_root and chain_tip.
Definition: tree.cpp:117
KDL::Joint::TransY
@ TransY
Definition: joint.hpp:47
config.h
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
KDL::Tree::segments
SegmentMap segments
Definition: tree.hpp:102
GetTreeElementSegment
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
KDL::Joint
Definition: joint.hpp:45
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
Definition: chain.hpp:35
KDL::Tree::getSegment
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Request the segment of the tree with name segment_name.
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:44
KDL::Joint::JointAxis
Vector JointAxis() const
Request the Vector corresponding to the axis of a revolute joint.
Definition: joint.cpp:131