f9dcb336d5d896a7c27cb5952afa5430473c49ea
[blender.git] / intern / itasc / kdl / treefksolverpos_recursive.cpp
1 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 // Copyright  (C)  2008 Julia Jesse
3
4 // Version: 1.0
5 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
7 // URL: http://www.orocos.org/kdl
8
9 // This library is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU Lesser General Public
11 // License as published by the Free Software Foundation; either
12 // version 2.1 of the License, or (at your option) any later version.
13
14 // This library is distributed in the hope that it will be useful,
15 // but WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
17 // Lesser General Public License for more details.
18
19 // You should have received a copy of the GNU Lesser General Public
20 // License along with this library; if not, write to the Free Software
21 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
22
23 #include "treefksolverpos_recursive.hpp"
24 #include <iostream>
25
26 namespace KDL {
27
28     TreeFkSolverPos_recursive::TreeFkSolverPos_recursive(const Tree& _tree):
29         tree(_tree)
30     {
31     }
32
33     int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
34     {      
35                 SegmentMap::const_iterator it = tree.getSegment(segmentName); 
36                 SegmentMap::const_iterator baseit = tree.getSegment(baseName); 
37         
38         if(q_in.rows() != tree.getNrOfJoints())
39                 return -1;
40         else if(it == tree.getSegments().end()) //if the segment name is not found
41                 return -2;
42         else if(baseit == tree.getSegments().end()) //if the base segment name is not found
43                 return -3;
44         else{
45                         p_out = recursiveFk(q_in, it, baseit);  
46                 return 0;               
47         }
48     }
49
50         Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
51         {
52                 //gets the frame for the current element (segment)
53                 const TreeElement& currentElement = it->second;
54                 
55                 if(it == baseit){
56                         return KDL::Frame::Identity();
57                 }
58                 else{
59                         Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
60                         SegmentMap::const_iterator parentIt = currentElement.parent;
61                         return recursiveFk(q_in, parentIt, baseit) * currentFrame;
62                 }
63         }
64
65     TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive()
66     {
67     }
68
69
70 }