Blender  V3.3
tree.cpp
Go to the documentation of this file.
1 
4 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 
6 // Version: 1.0
7 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // URL: http://www.orocos.org/kdl
10 
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15 
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 // Lesser General Public License for more details.
20 
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 
25 #include "tree.hpp"
26 #include <sstream>
27 namespace KDL {
28 using namespace std;
29 
31  nrOfJoints(0), nrOfSegments(0) {
32  TreeElement root;
33  std::pair<std::string, TreeElement> val("root", root);
34  segments.insert(val);
35 }
36 
37 Tree::Tree(const Tree& in) {
38  segments.clear();
39  nrOfSegments = 0;
40  nrOfJoints = 0;
41  TreeElement root;
42  std::pair<std::string, TreeElement> val("root", root);
43  segments.insert(val);
44  this->addTree(in, "", "root");
45 
46 }
47 
48 Tree& Tree::operator=(const Tree& in) {
49  segments.clear();
50  nrOfSegments = 0;
51  nrOfJoints = 0;
52  TreeElement root;
53  std::pair<std::string, TreeElement> val("root", root);
54  segments.insert(val);
55  this->addTree(in, "", "root");
56  return *this;
57 }
58 
59 bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
60  const std::string& hook_name) {
61  SegmentMap::iterator parent = segments.find(hook_name);
62  //check if parent exists
63  if (parent == segments.end())
64  return false;
65  pair<SegmentMap::iterator, bool> retval;
66  //insert new element
67  TreeElement elem(segment, *parent, nrOfJoints);
68  std::pair<std::string, TreeElement> val(segment_name, elem);
69 
70  retval = segments.insert(val);
71  //check if insertion succeeded
72  if (!retval.second)
73  return false;
74  //add iterator to new element in parents children list
75  parent->second.children.push_back(retval.first);
76  //increase number of segments
77  nrOfSegments++;
78  //increase number of joints
79  nrOfJoints += segment.getJoint().getNDof();
80  return true;
81 }
82 
83 bool Tree::addChain(const Chain& chain, const std::string& chain_name,
84  const std::string& hook_name) {
85  string parent_name = hook_name;
86  for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
87  ostringstream segment_name;
88  segment_name << chain_name << "Segment" << i;
89  if (this->addSegment(chain.getSegment(i), segment_name.str(),
90  parent_name))
91  parent_name = segment_name.str();
92  else
93  return false;
94  }
95  return true;
96 }
97 
98 bool Tree::addTree(const Tree& tree, const std::string& tree_name,
99  const std::string& hook_name) {
100  return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name);
101 }
102 
103 bool Tree::addTreeRecursive(SegmentMap::const_iterator root,
104  const std::string& tree_name, const std::string& hook_name) {
105  //get iterator for root-segment
106  SegmentMap::const_iterator child;
107  //try to add all of root's children
108  for (unsigned int i = 0; i < root->second.children.size(); i++) {
109  child = root->second.children[i];
110  //Try to add the child
111  if (this->addSegment(child->second.segment, tree_name + child->first,
112  hook_name)) {
113  //if child is added, add all the child's children
114  if (!(this->addTreeRecursive(child, tree_name, tree_name
115  + child->first)))
116  //if it didn't work, return false
117  return false;
118  } else
119  //If the child could not be added, return false
120  return false;
121  }
122  return true;
123 }
124 
125 }
126 
This class encapsulates a serial kinematic interconnection structure. It is build out of segments.
Definition: chain.hpp:36
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
unsigned int getNrOfSegments() const
Definition: chain.hpp:78
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and an inertia) with...
Definition: segment.hpp:46
This class encapsulates a tree kinematic interconnection structure. It is build out of segments.
Definition: tree.hpp:68
Tree & operator=(const Tree &arg)
Definition: tree.cpp:48
bool addTree(const Tree &tree, const std::string &tree_name, const std::string &hook_name)
Definition: tree.cpp:98
bool addSegment(const Segment &segment, const std::string &segment_name, const std::string &hook_name)
Definition: tree.cpp:59
Tree()
Definition: tree.cpp:30
bool addChain(const Chain &chain, const std::string &chain_name, const std::string &hook_name)
Definition: tree.cpp:83
void * tree
Segment< FEdge *, Vec3r > segment
Definition: chain.cpp:27