tree.cpp
Go to the documentation of this file.
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         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     //check if parent exists
00058     if (parent == segments.end())
00059         return false;
00060     pair<SegmentMap::iterator, bool> retval;
00061     //insert new element
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     //check if insertion succeeded
00071     if (!retval.second)
00072         return false;
00073     //add iterator to new element in parents children list
00074     GetTreeElementChildren(parent->second).push_back(retval.first);
00075     //increase number of segments
00076     nrOfSegments++;
00077     //increase number of joints
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     //get iterator for root-segment
00100     SegmentMap::const_iterator child;
00101     //try to add all of root's children
00102     for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) {
00103         child = GetTreeElementChildren(root->second)[i];
00104         //Try to add the child
00105         if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) {
00106             //if child is added, add all the child's children
00107             if (!(this->addTreeRecursive(child, child->first)))
00108                 //if it didn't work, return false
00109                 return false;
00110         } else
00111             //If the child could not be added, return false
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         // clear chain
00120         chain = Chain();
00121         
00122         // walk down from chain_root and chain_tip to the root of the tree
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         // remove common part of segment lists
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         // add the segments from the root to the common frame
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         // add the segments from the common frame to the tip frame
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 }//end of namespace
00167 
00168 


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14