orocos_kdl
tree.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
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 #include "tree.hpp"
23 #include <sstream>
24 
25 namespace KDL {
26 
27 Tree::Tree(const std::string& _root_name) :
28  nrOfJoints(0), nrOfSegments(0), root_name(_root_name)
29 {
31 }
32 
33 Tree::Tree(const Tree& in) {
34  segments.clear();
35  nrOfSegments = 0;
36  nrOfJoints = 0;
37  root_name = in.root_name;
38 
40  addTree(in, root_name);
41 }
42 
43 Tree& Tree::operator=(const Tree& in) {
44  segments.clear();
45  nrOfSegments = 0;
46  nrOfJoints = 0;
47  root_name = in.root_name;
48 
50  this->addTree(in, root_name);
51  return *this;
52 }
53 
54 bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
55  SegmentMap::iterator parent = segments.find(hook_name);
56  //check if parent exists
57  if (parent == segments.end())
58  return false;
60  //insert new element
61  unsigned int q_nr = segment.getJoint().getType() != Joint::Fixed ? nrOfJoints : 0;
62 
63 #ifdef KDL_USE_NEW_TREE_INTERFACE
64  retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr))));
65 #else //#ifdef KDL_USE_NEW_TREE_INTERFACE
66  retval = segments.insert(make_pair(segment.getName(), TreeElementType(segment, parent, q_nr)));
67 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
68 
69  //check if insertion succeeded
70  if (!retval.second)
71  return false;
72  //add iterator to new element in parents children list
73  GetTreeElementChildren(parent->second).push_back(retval.first);
74  //increase number of segments
75  nrOfSegments++;
76  //increase number of joints
77  if (segment.getJoint().getType() != Joint::Fixed)
78  nrOfJoints++;
79  return true;
80 }
81 
82 bool Tree::addChain(const Chain& chain, const std::string& hook_name) {
83  std::string parent_name = hook_name;
84  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
85  if (this->addSegment(chain.getSegment(i), parent_name))
86  parent_name = chain.getSegment(i).getName();
87  else
88  return false;
89  }
90  return true;
91 }
92 
93 bool Tree::addTree(const Tree& tree, const std::string& hook_name) {
94  return this->addTreeRecursive(tree.getRootSegment(), hook_name);
95 }
96 
97 bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name) {
98  //get iterator for root-segment
99  SegmentMap::const_iterator child;
100  //try to add all of root's children
101  for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
102  child = GetTreeElementChildren(root->second)[i];
103  //Try to add the child
104  if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) {
105  //if child is added, add all the child's children
106  if (!(this->addTreeRecursive(child, child->first)))
107  //if it didn't work, return false
108  return false;
109  } else
110  //If the child could not be added, return false
111  return false;
112  }
113  return true;
114 }
115 
116 bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
117 {
118  // clear chain
119  chain = Chain();
120 
121  // walk down from chain_root and chain_tip to the root of the tree
122  std::vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
123  for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){
124  parents_chain_root.push_back(s->first);
125  if (s->first == root_name) break;
126  }
127  if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
128  for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){
129  parents_chain_tip.push_back(s->first);
130  if (s->first == root_name) break;
131  }
132  if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
133 
134  // remove common part of segment lists
135  SegmentMap::key_type last_segment = root_name;
136  while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
137  parents_chain_root.back() == parents_chain_tip.back()){
138  last_segment = parents_chain_root.back();
139  parents_chain_root.pop_back();
140  parents_chain_tip.pop_back();
141  }
142  parents_chain_root.push_back(last_segment);
143 
144 
145  // add the segments from the root to the common frame
146  for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
147  Segment seg = GetTreeElementSegment(getSegment(parents_chain_root[s])->second);
148  Frame f_tip = seg.pose(0.0).Inverse();
149  Joint jnt = seg.getJoint();
150  if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
151  jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis);
152  else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
153  jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis);
154  chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(),
155  jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia()));
156  }
157 
158  // add the segments from the common frame to the tip frame
159  for (auto rit=parents_chain_tip.rbegin(); rit != parents_chain_tip.rend(); ++rit){
160  chain.addSegment(GetTreeElementSegment(getSegment(*rit)->second));
161  }
162  return true;
163 }
164 
165 bool Tree::getSubTree(const std::string& segment_name, Tree& tree) const
166 {
167  //check if segment_name exists
168  SegmentMap::const_iterator root = segments.find(segment_name);
169  if (root == segments.end())
170  return false;
171  //init the tree, segment_name is the new root.
172  tree = Tree(root->first);
173  return tree.addTreeRecursive(root, segment_name);
174 }
175 
176 }//end of namespace
sstream
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::Joint::RotZ
@ RotZ
Definition: joint.hpp:47
std::string
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
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:97
std::pair
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
std::vector
std::map::find
T find(T... args)
std::vector::size
T size(T... args)
KDL::Joint::TransZ
@ TransZ
Definition: joint.hpp:47
KDL::Joint::JointOrigin
Vector JointOrigin() const
Definition: joint.cpp:144
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
std::vector::back
T back(T... args)
GetTreeElementParent
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
GetTreeElementChildren
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
KDL
Definition: kukaLWR_DHnew.cpp:25
std::map::clear
T clear(T... args)
KDL::Joint::getName
const std::string & getName() const
Definition: joint.hpp:150
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
std::vector::push_back
T push_back(T... args)
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)
Definition: tree.cpp:82
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
KDL::Joint::RotX
@ RotX
Definition: joint.hpp:47
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
tree.hpp
KDL::Joint::TransX
@ TransX
Definition: joint.hpp:47
KDL::Tree::getSubTree
bool getSubTree(const std::string &segment_name, Tree &tree) const
Definition: tree.cpp:165
std::vector::pop_back
T pop_back(T... args)
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
std::vector::rend
T rend(T... args)
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::Joint::RotAxis
@ RotAxis
Definition: joint.hpp:47
KDL::Chain::addSegment
void addSegment(const Segment &segment)
Definition: chain.cpp:53
KDL::Tree::addTree
bool addTree(const Tree &tree, const std::string &hook_name)
Definition: tree.cpp:93
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
std::map::insert
T insert(T... args)
KDL::Segment::getName
const std::string & getName() const
Definition: segment.hpp:108
KDL::Tree::getChain
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Definition: tree.cpp:116
KDL::Joint::TransY
@ TransY
Definition: joint.hpp:47
std::vector::empty
T empty(T... args)
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KDL::Tree::segments
SegmentMap segments
Definition: tree.hpp:102
std::map::end
T end(T... args)
GetTreeElementSegment
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
KDL::Joint
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
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
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
std::vector::rbegin
T rbegin(T... args)
KDL::Tree::operator=
Tree & operator=(const Tree &arg)
Definition: tree.cpp:43
KDL::Joint::JointAxis
Vector JointAxis() const
Definition: joint.cpp:118