/
tree.cpp
168 lines (142 loc) · 6.82 KB
/
tree.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
// Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "tree.hpp"
#include <sstream>
namespace KDL {
using namespace std;
Tree::Tree(const std::string& _root_name) :
nrOfJoints(0), nrOfSegments(0), root_name(_root_name)
{
segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
}
Tree::Tree(const Tree& in) {
segments.clear();
nrOfJoints = 0;
nrOfSegments = 0;
root_name = in.root_name;
segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
addTree(in, root_name);
}
Tree& Tree::operator=(const Tree& in) {
segments.clear();
nrOfSegments = 0;
nrOfJoints = 0;
root_name = in.root_name;
segments.insert(make_pair(in.root_name, TreeElement::Root(root_name)));
this->addTree(in, root_name);
return *this;
}
bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
SegmentMap::iterator parent = segments.find(hook_name);
//check if parent exists
if (parent == segments.end())
return false;
pair<SegmentMap::iterator, bool> retval;
//insert new element
unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0;
#ifdef KDL_USE_NEW_TREE_INTERFACE
retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr))));
#else //#ifdef KDL_USE_NEW_TREE_INTERFACE
retval = segments.insert(make_pair(segment.getName(), TreeElementType(segment, parent, q_nr)));
#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
//check if insertion succeeded
if (!retval.second)
return false;
//add iterator to new element in parents children list
GetTreeElementChildren(parent->second).push_back(retval.first);
//increase number of segments
nrOfSegments++;
//increase number of joints
if (segment.getJoint().getType() != Joint::None)
nrOfJoints++;
return true;
}
bool Tree::addChain(const Chain& chain, const std::string& hook_name) {
string parent_name = hook_name;
for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
if (this->addSegment(chain.getSegment(i), parent_name))
parent_name = chain.getSegment(i).getName();
else
return false;
}
return true;
}
bool Tree::addTree(const Tree& tree, const std::string& hook_name) {
return this->addTreeRecursive(tree.getRootSegment(), hook_name);
}
bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name) {
//get iterator for root-segment
SegmentMap::const_iterator child;
//try to add all of root's children
for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
child = GetTreeElementChildren(root->second)[i];
//Try to add the child
if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) {
//if child is added, add all the child's children
if (!(this->addTreeRecursive(child, child->first)))
//if it didn't work, return false
return false;
} else
//If the child could not be added, return false
return false;
}
return true;
}
bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
// clear chain
chain = Chain();
// walk down from chain_root and chain_tip to the root of the tree
vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){
parents_chain_root.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){
parents_chain_tip.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
// remove common part of segment lists
SegmentMap::key_type last_segment = root_name;
while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
parents_chain_root.back() == parents_chain_tip.back()){
last_segment = parents_chain_root.back();
parents_chain_root.pop_back();
parents_chain_tip.pop_back();
}
parents_chain_root.push_back(last_segment);
// add the segments from the root to the common frame
for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
Segment seg = GetTreeElementSegment(getSegment(parents_chain_root[s])->second);
Frame f_tip = seg.pose(0.0).Inverse();
Joint jnt = seg.getJoint();
if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis);
else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis);
chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(),
jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia()));
}
// add the segments from the common frame to the tip frame
for (int s=parents_chain_tip.size()-1; s>-1; s--){
chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second));
}
return true;
}
}//end of namespace