$search
00001 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 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 nrOfSegments(0), nrOfJoints(0),root_name(_root_name) { 00030 segments.insert(make_pair(root_name, TreeElement::Root(root_name))); 00031 } 00032 00033 Tree::Tree(const Tree& in) { 00034 segments.clear(); 00035 nrOfSegments = 0; 00036 nrOfJoints = 0; 00037 root_name = in.root_name; 00038 00039 segments.insert(make_pair(root_name, TreeElement::Root(root_name))); 00040 this->addTree(in, root_name); 00041 } 00042 00043 Tree& Tree::operator=(const Tree& in) { 00044 segments.clear(); 00045 nrOfSegments = 0; 00046 nrOfJoints = 0; 00047 root_name = in.root_name; 00048 00049 segments.insert(make_pair(in.root_name, TreeElement::Root(root_name))); 00050 this->addTree(in, root_name); 00051 return *this; 00052 } 00053 00054 bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { 00055 SegmentMap::iterator parent = segments.find(hook_name); 00056 //check if parent exists 00057 if (parent == segments.end()) 00058 return false; 00059 pair<SegmentMap::iterator, bool> retval; 00060 //insert new element 00061 unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0; 00062 retval = segments.insert(make_pair(segment.getName(), TreeElement(segment, parent, q_nr))); 00063 //check if insertion succeeded 00064 if (!retval.second) 00065 return false; 00066 //add iterator to new element in parents children list 00067 parent->second.children.push_back(retval.first); 00068 //increase number of segments 00069 nrOfSegments++; 00070 //increase number of joints 00071 if (segment.getJoint().getType() != Joint::None) 00072 nrOfJoints++; 00073 return true; 00074 } 00075 00076 bool Tree::addChain(const Chain& chain, const std::string& hook_name) { 00077 string parent_name = hook_name; 00078 for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) { 00079 if (this->addSegment(chain.getSegment(i), parent_name)) 00080 parent_name = chain.getSegment(i).getName(); 00081 else 00082 return false; 00083 } 00084 return true; 00085 } 00086 00087 bool Tree::addTree(const Tree& tree, const std::string& hook_name) { 00088 return this->addTreeRecursive(tree.getRootSegment(), hook_name); 00089 } 00090 00091 bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name) { 00092 //get iterator for root-segment 00093 SegmentMap::const_iterator child; 00094 //try to add all of root's children 00095 for (unsigned int i = 0; i < root->second.children.size(); i++) { 00096 child = root->second.children[i]; 00097 //Try to add the child 00098 if (this->addSegment(child->second.segment, hook_name)) { 00099 //if child is added, add all the child's children 00100 if (!(this->addTreeRecursive(child, child->first))) 00101 //if it didn't work, return false 00102 return false; 00103 } else 00104 //If the child could not be added, return false 00105 return false; 00106 } 00107 return true; 00108 } 00109 00110 bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const 00111 { 00112 // clear chain 00113 chain = Chain(); 00114 00115 // walk down from chain_root and chain_tip to the root of the tree 00116 vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip; 00117 for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){ 00118 parents_chain_root.push_back(s->first); 00119 if (s->first == root_name) break; 00120 } 00121 if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false; 00122 for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){ 00123 parents_chain_tip.push_back(s->first); 00124 if (s->first == root_name) break; 00125 } 00126 if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false; 00127 00128 // remove common part of segment lists 00129 SegmentMap::key_type last_segment = root_name; 00130 while (!parents_chain_root.empty() && !parents_chain_tip.empty() && 00131 parents_chain_root.back() == parents_chain_tip.back()){ 00132 last_segment = parents_chain_root.back(); 00133 parents_chain_root.pop_back(); 00134 parents_chain_tip.pop_back(); 00135 } 00136 parents_chain_root.push_back(last_segment); 00137 00138 00139 // add the segments from the root to the common frame 00140 for (unsigned int s=0; s<parents_chain_root.size()-1; s++){ 00141 Segment seg = getSegment(parents_chain_root[s])->second.segment; 00142 Frame f_tip = seg.pose(0.0).Inverse(); 00143 Joint jnt = seg.getJoint(); 00144 if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis) 00145 jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis); 00146 else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis) 00147 jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis); 00148 chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(), 00149 jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia())); 00150 } 00151 00152 // add the segments from the common frame to the tip frame 00153 for (int s=parents_chain_tip.size()-1; s>-1; s--){ 00154 chain.addSegment(getSegment(parents_chain_tip[s])->second.segment); 00155 } 00156 return true; 00157 } 00158 00159 }//end of namespace 00160 00161