constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/test/mytreefksolverpos_recursive.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 // Copyright  (C)  2008 Julia Jesse
00003 
00004 // Version: 1.0
00005 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00007 // URL: http://www.orocos.org/kdl
00008 
00009 // This library is free software; you can redistribute it and/or
00010 // modify it under the terms of the GNU Lesser General Public
00011 // License as published by the Free Software Foundation; either
00012 // version 2.1 of the License, or (at your option) any later version.
00013 
00014 // This library is distributed in the hope that it will be useful,
00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017 // Lesser General Public License for more details.
00018 
00019 // You should have received a copy of the GNU Lesser General Public
00020 // License along with this library; if not, write to the Free Software
00021 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00022 
00023 #include "mytreefksolverpos_recursive.hpp"
00024 #include <iostream>
00025 #include <kdl/frames_io.hpp>
00026 
00027 using namespace KDL;
00028 
00029 MyTreeFkSolverPos_recursive::MyTreeFkSolverPos_recursive(const Tree& _tree):
00030   tree(_tree)
00031 {
00032 }
00033 
00034 int MyTreeFkSolverPos_recursive::JntToCart(const KDL::JntArray& q_in, SegmentToPoseMap& result)
00035 {
00036   if(q_in.rows() != tree.getNrOfJoints())
00037     return -1;
00038 
00039   result.clear();
00040   addFrameToMap(q_in, tree.getRootSegment(), KDL::Frame::Identity(), result);
00041   
00042   return 0;
00043 }
00044 
00045 void MyTreeFkSolverPos_recursive::addFrameToMap(const JntArray& q_in,
00046                                                 const SegmentMap::const_iterator thisSegment,
00047                                                 const KDL::Frame& previousFrame,
00048                                                 SegmentToPoseMap& result )
00049 {
00050   //gets the frame for the current element (segment)
00051   const TreeElement& currentElement = thisSegment->second;
00052   Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr));
00053 
00054   KDL::Frame thisFrame = previousFrame * currentFrame;
00055   
00056   if (thisSegment->first != tree.getRootSegment()->first)
00057     result.insert(std::make_pair(thisSegment->first, thisFrame));
00058 
00059   //std::cout << "addFrameToMap: frame=" << thisSegment->first << ", frame=" << std::endl
00060   //        << thisFrame << std::endl;
00061 
00062   // get poses of child segments
00063   for (std::vector<SegmentMap::const_iterator>::const_iterator child = thisSegment->second.children.begin(); 
00064        child !=thisSegment->second.children.end(); 
00065        ++child ) {
00066     addFrameToMap(q_in, *child, thisFrame, result);
00067   }      
00068 }
00069 /*
00070 Frame MyTreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
00071 {
00072   //gets the frame for the current element (segment)
00073   const TreeElement& currentElement = it->second;
00074   Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr));
00075   
00076   SegmentMap::const_iterator rootIterator = tree.getRootSegment();
00077   if(it == rootIterator){
00078     return currentFrame;        
00079   }
00080   else{
00081     SegmentMap::const_iterator parentIt = currentElement.parent;
00082     return recursiveFk(q_in, parentIt) * currentFrame;
00083   }
00084 }
00085 */
00086 MyTreeFkSolverPos_recursive::~MyTreeFkSolverPos_recursive()
00087 {
00088 }
00089 
 All Classes Files Functions Variables Typedefs