doxygen: intern/itasc tagged
[blender.git] / intern / itasc / kdl / chainfksolverpos_recursive.cpp
1 /** \file itasc/kdl/chainfksolverpos_recursive.cpp
2  *  \ingroup itasc
3  */
4 // Copyright  (C)  2007  Francois Cauwe <francois at cauwe dot org>
5 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6
7 // Version: 1.0
8 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
10 // URL: http://www.orocos.org/kdl
11
12 // This library is free software; you can redistribute it and/or
13 // modify it under the terms of the GNU Lesser General Public
14 // License as published by the Free Software Foundation; either
15 // version 2.1 of the License, or (at your option) any later version.
16
17 // This library is distributed in the hope that it will be useful,
18 // but WITHOUT ANY WARRANTY; without even the implied warranty of
19 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
20 // Lesser General Public License for more details.
21
22 // You should have received a copy of the GNU Lesser General Public
23 // License along with this library; if not, write to the Free Software
24 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
25
26 #include "chainfksolverpos_recursive.hpp"
27 #include <iostream>
28
29 namespace KDL {
30
31     ChainFkSolverPos_recursive::ChainFkSolverPos_recursive(const Chain& _chain):
32         chain(_chain)
33     {
34     }
35
36     int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr)
37     {
38                 unsigned int segNr = (unsigned int)segmentNr;
39         if(segmentNr<0)
40              segNr=chain.getNrOfSegments();
41
42         p_out = Frame::Identity();
43
44         if(q_in.rows()!=chain.getNrOfJoints())
45             return -1;
46         else if(segNr>chain.getNrOfSegments())
47             return -1;
48         else{
49             int j=0;
50             for(unsigned int i=0;i<segNr;i++){
51                 p_out = p_out*chain.getSegment(i).pose(((JntArray&)q_in)(j));
52                                 j+=chain.getSegment(i).getJoint().getNDof();
53             }
54             return 0;
55         }
56     }
57
58
59     ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive()
60     {
61     }
62
63
64 }