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