Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "tree.hpp"
00023 #include <sstream>
00024
00025 namespace KDL {
00026 using namespace std;
00027
00028 Tree::Tree(const std::string& _root_name) :
00029 nrOfJoints(0), nrOfSegments(0), root_name(_root_name)
00030 {
00031 segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
00032 }
00033
00034 Tree::Tree(const Tree& in) {
00035 segments.clear();
00036 nrOfSegments = 0;
00037 nrOfJoints = 0;
00038 root_name = in.root_name;
00039
00040 segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
00041 addTree(in, root_name);
00042 }
00043
00044 Tree& Tree::operator=(const Tree& in) {
00045 segments.clear();
00046 nrOfSegments = 0;
00047 nrOfJoints = 0;
00048 root_name = in.root_name;
00049
00050 segments.insert(make_pair(in.root_name, TreeElement::Root(root_name)));
00051 this->addTree(in, root_name);
00052 return *this;
00053 }
00054
00055 bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
00056 SegmentMap::iterator parent = segments.find(hook_name);
00057
00058 if (parent == segments.end())
00059 return false;
00060 pair<SegmentMap::iterator, bool> retval;
00061
00062 unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0;
00063
00064 #ifdef KDL_USE_NEW_TREE_INTERFACE
00065 retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr))));
00066 #else //#ifdef KDL_USE_NEW_TREE_INTERFACE
00067 retval = segments.insert(make_pair(segment.getName(), TreeElementType(segment, parent, q_nr)));
00068 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
00069
00070
00071 if (!retval.second)
00072 return false;
00073
00074 GetTreeElementChildren(parent->second).push_back(retval.first);
00075
00076 nrOfSegments++;
00077
00078 if (segment.getJoint().getType() != Joint::None)
00079 nrOfJoints++;
00080 return true;
00081 }
00082
00083 bool Tree::addChain(const Chain& chain, const std::string& hook_name) {
00084 string parent_name = hook_name;
00085 for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
00086 if (this->addSegment(chain.getSegment(i), parent_name))
00087 parent_name = chain.getSegment(i).getName();
00088 else
00089 return false;
00090 }
00091 return true;
00092 }
00093
00094 bool Tree::addTree(const Tree& tree, const std::string& hook_name) {
00095 return this->addTreeRecursive(tree.getRootSegment(), hook_name);
00096 }
00097
00098 bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name) {
00099
00100 SegmentMap::const_iterator child;
00101
00102 for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
00103 child = GetTreeElementChildren(root->second)[i];
00104
00105 if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) {
00106
00107 if (!(this->addTreeRecursive(child, child->first)))
00108
00109 return false;
00110 } else
00111
00112 return false;
00113 }
00114 return true;
00115 }
00116
00117 bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
00118 {
00119
00120 chain = Chain();
00121
00122
00123 vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
00124 for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){
00125 parents_chain_root.push_back(s->first);
00126 if (s->first == root_name) break;
00127 }
00128 if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
00129 for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){
00130 parents_chain_tip.push_back(s->first);
00131 if (s->first == root_name) break;
00132 }
00133 if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
00134
00135
00136 SegmentMap::key_type last_segment = root_name;
00137 while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
00138 parents_chain_root.back() == parents_chain_tip.back()){
00139 last_segment = parents_chain_root.back();
00140 parents_chain_root.pop_back();
00141 parents_chain_tip.pop_back();
00142 }
00143 parents_chain_root.push_back(last_segment);
00144
00145
00146
00147 for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
00148 Segment seg = GetTreeElementSegment(getSegment(parents_chain_root[s])->second);
00149 Frame f_tip = seg.pose(0.0).Inverse();
00150 Joint jnt = seg.getJoint();
00151 if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
00152 jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis);
00153 else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
00154 jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis);
00155 chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(),
00156 jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia()));
00157 }
00158
00159
00160 for (int s=parents_chain_tip.size()-1; s>-1; s--){
00161 chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second));
00162 }
00163 return true;
00164 }
00165
00166 }
00167
00168