treefksolverpos_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 "treefksolverpos_recursive.hpp"
00024 #include <iostream>
00025 
00026 namespace KDL {
00027 
00028     TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree):
00029         tree(_tree)
00030     {
00031     }
00032 
00033     int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)
00034     {      
00035                 SegmentMap::const_iterator it = tree.getSegment(segmentName); 
00036        
00037         
00038         if(q_in.rows() != tree.getNrOfJoints())
00039                 return -1;
00040         else if(it == tree.getSegments().end()) //if the segment name is not found
00041                 return -2;
00042         else{
00043                         p_out = recursiveFk(q_in, it);
00044                 return 0;               
00045         }
00046     }
00047 
00048         Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
00049         {
00050                 //gets the frame for the current element (segment)
00051         const TreeElementType& currentElement = it->second;
00052         Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));
00053 
00054                 SegmentMap::const_iterator rootIterator = tree.getRootSegment();
00055                 if(it == rootIterator){
00056                         return currentFrame;    
00057                 }
00058                 else{
00059             SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement);
00060                         return recursiveFk(q_in, parentIt) * currentFrame;
00061                 }
00062         }
00063 
00064     TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive()
00065     {
00066     }
00067 
00068 
00069 }


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:23