orocos_kdl
treefksolverpos_recursive.cpp
Go to the documentation of this file.
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 
24 #include <iostream>
25 
26 namespace KDL {
27 
29  tree(_tree)
30  {
31  }
32 
33  int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)
34  {
35  SegmentMap::const_iterator it = tree.getSegment(segmentName);
36 
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{
43  p_out = recursiveFk(q_in, it);
44  return 0;
45  }
46  }
47 
48  Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
49  {
50  //gets the frame for the current element (segment)
51  const TreeElementType& currentElement = it->second;
52  Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement)));
53 
54  SegmentMap::const_iterator rootIterator = tree.getRootSegment();
55  if(it == rootIterator){
56  return currentFrame;
57  }
58  else{
59  SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement);
60  return recursiveFk(q_in, parentIt) * currentFrame;
61  }
62  }
63 
65  {
66  }
67 
68 
69 }
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
std::string
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Tree::getRootSegment
SegmentMap::const_iterator getRootSegment() const
Definition: tree.hpp:186
GetTreeElementParent
#define GetTreeElementParent(tree_element)
Definition: tree.hpp:60
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
treefksolverpos_recursive.hpp
KDL::Tree::getSegments
const SegmentMap & getSegments() const
Definition: tree.hpp:218
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::TreeFkSolverPos_recursive::~TreeFkSolverPos_recursive
~TreeFkSolverPos_recursive()
Definition: treefksolverpos_recursive.cpp:64
KDL::TreeFkSolverPos_recursive::tree
const Tree tree
Definition: treefksolverpos_recursive.hpp:46
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::TreeFkSolverPos_recursive::recursiveFk
Frame recursiveFk(const JntArray &q_in, const SegmentMap::const_iterator &it)
Definition: treefksolverpos_recursive.cpp:48
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
std::map::end
T end(T... args)
GetTreeElementSegment
#define GetTreeElementSegment(tree_element)
Definition: tree.hpp:62
KDL::TreeFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, std::string segmentName)
Definition: treefksolverpos_recursive.cpp:33
KDL::Tree::getSegment
SegmentMap::const_iterator getSegment(const std::string &segment_name) const
Definition: tree.hpp:177
KDL::TreeElement
Definition: tree.hpp:66
KDL::TreeFkSolverPos_recursive::TreeFkSolverPos_recursive
TreeFkSolverPos_recursive(const Tree &tree)
Definition: treefksolverpos_recursive.cpp:28
GetTreeElementQNr
#define GetTreeElementQNr(tree_element)
Definition: tree.hpp:61