Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / chainfksolverpos_recursive.cpp
1 // Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
2 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
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 "chainfksolverpos_recursive.hpp"
24 #include <iostream>
25
26 namespace KDL {
27
28     ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain):
29         chain(_chain)
30     {
31     }
32
33     int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
34     {
35                 unsigned int segNr = (unsigned int)segmentNr;
36         if(segmentNr<0)
37              segNr=chain.getNrOfSegments();
38
39         p_out = Frame::Identity();
40
41         if(q_in.rows()!=chain.getNrOfJoints())
42             return -1;
43         else if(segNr>chain.getNrOfSegments())
44             return -1;
45         else{
46             int j=0;
47             for(unsigned int i=0;i<segNr;i++){
48                 p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j));
49                                 j+=chain.getSegment(i).getJoint().getNDof();
50             }
51             return 0;
52         }
53     }
54
55
56     ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
57     {
58     }
59
60
61 }