Blender  V3.3
treefksolverpos_recursive.cpp
Go to the documentation of this file.
1 
4 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Copyright (C) 2008 Julia Jesse
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 
27 #include <iostream>
28 
29 namespace KDL {
30 
32  tree(_tree)
33  {
34  }
35 
36  int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
37  {
38  SegmentMap::value_type const* it = tree.getSegmentPtr(segmentName);
39  SegmentMap::value_type const* baseit = tree.getSegmentPtr(baseName);
40 
41  if(q_in.rows() != tree.getNrOfJoints())
42  return -1;
43  else if(!it) //if the segment name is not found
44  return -2;
45  else if(!baseit) //if the base segment name is not found
46  return -3;
47  else{
48  p_out = recursiveFk(q_in, it, baseit);
49  return 0;
50  }
51  }
52 
53  Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, SegmentMap::value_type const* it, SegmentMap::value_type const* baseit)
54  {
55  //gets the frame for the current element (segment)
56  const TreeElement& currentElement = it->second;
57 
58  if(it == baseit){
59  return KDL::Frame::Identity();
60  }
61  else{
62  Frame currentFrame = currentElement.segment.pose(((JntArray&)q_in)(currentElement.q_nr));
63  return recursiveFk(q_in, currentElement.parent, baseit) * currentFrame;
64  }
65  }
66 
68  {
69  }
70 
71 
72 }
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
static Frame Identity()
Definition: frames.inl:719
unsigned int rows() const
Definition: jntarray.cpp:93
Frame pose(const double *q) const
Definition: segment.cpp:51
unsigned int q_nr
Definition: tree.hpp:46
Segment segment
Definition: tree.hpp:43
SegmentMap::value_type const * parent
Definition: tree.hpp:47
virtual int JntToCart(const JntArray &q_in, Frame &p_out, const std::string &segmentName, const std::string &baseName)
This class encapsulates a tree kinematic interconnection structure. It is build out of segments.
Definition: tree.hpp:68
SegmentMap::value_type const * getSegmentPtr(const std::string &segment_name) const
Definition: tree.hpp:154
unsigned int getNrOfJoints() const
Definition: tree.hpp:131
void * tree
Definition: chain.cpp:27