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 {
30  segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
31 }
32 
33 Tree::Tree(const Tree& in) {
34  segments.clear();
35  nrOfSegments = 0;
36  nrOfJoints = 0;
37  root_name = in.root_name;
38 
39  segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
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 
49  segments.insert(make_pair(in.root_name, TreeElement::Root(root_name)));
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;
59  std::pair<SegmentMap::iterator, bool> retval;
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
Vector JointOrigin() const
Definition: joint.cpp:144
unsigned int nrOfSegments
Definition: tree.hpp:104
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
Tree & operator=(const Tree &arg)
Definition: tree.cpp:43
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
bool addTreeRecursive(SegmentMap::const_iterator root, const std::string &hook_name)
Definition: tree.cpp:97
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
Definition: segment.hpp:46
TreeElement TreeElementType
Definition: tree.hpp:57
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
void addSegment(const Segment &segment)
Definition: chain.cpp:53
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
std::string root_name
Definition: tree.hpp:106
#define GetTreeElementChildren(tree_element)
Definition: tree.hpp:59
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
const Joint & getJoint() const
Definition: segment.hpp:118
bool addChain(const Chain &chain, const std::string &hook_name)
Definition: tree.cpp:82
Vector JointAxis() const
Definition: joint.cpp:118
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
Frame pose(const double &q) const
Definition: segment.cpp:57
bool addTree(const Tree &tree, const std::string &hook_name)
Definition: tree.cpp:93
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Definition: tree.cpp:116
const JointType & getType() const
Definition: joint.hpp:159
SegmentMap segments
Definition: tree.hpp:102
static TreeElementType Root(const std::string &root_name)
Definition: tree.hpp:75
bool getSubTree(const std::string &segment_name, Tree &tree) const
Definition: tree.cpp:165
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
const std::string & getName() const
Definition: segment.hpp:108
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:45
const std::string & getName() const
Definition: joint.hpp:150
bool addSegment(const Segment &segment, const std::string &hook_name)
Definition: tree.cpp:54
unsigned int nrOfJoints
Definition: tree.hpp:103
Tree(const std::string &root_name="root")
Definition: tree.cpp:27
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
Definition: tree.hpp:99
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Definition: tree.hpp:177


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14