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 }