Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[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                 const TreeElement& currentElement = it->second;
46                         p_out = recursiveFk(q_in, it, baseit);  
47                 return 0;               
48         }
49     }
50
51         Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
52         {
53                 //gets the frame for the current element (segment)
54                 const TreeElement& currentElement = it->second;
55                 
56                 if(it == baseit){
57                         return KDL::Frame::Identity();
58                 }
59                 else{
60                         Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
61                         SegmentMap::const_iterator parentIt = currentElement.parent;
62                         return recursiveFk(q_in, parentIt, baseit) * currentFrame;
63                 }
64         }
65
66     TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive()
67     {
68     }
69
70
71 }