orocos_kdl
chainfksolverpos_recursive.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Francois Cauwe <francois at cauwe dot org>
2 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
3 
4 // Version: 1.0
5 // Author: Francois Cauwe <francois at cauwe dot org>
6 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
7 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
25 #include <iostream>
26 
27 namespace KDL {
28 
30  chain(_chain)
31  {
32  }
33 
34  int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) {
35  unsigned int segmentNr;
36  if(seg_nr<0)
37  segmentNr=chain.getNrOfSegments();
38  else
39  segmentNr = seg_nr;
40 
41  p_out = Frame::Identity();
42 
43  if(q_in.rows()!=chain.getNrOfJoints())
44  return (error = E_SIZE_MISMATCH);
45  else if(segmentNr>chain.getNrOfSegments())
46  return (error = E_OUT_OF_RANGE);
47  else{
48  int j=0;
49  for(unsigned int i=0;i<segmentNr;i++){
51  p_out = p_out*chain.getSegment(i).pose(q_in(j));
52  j++;
53  }else{
54  p_out = p_out*chain.getSegment(i).pose(0.0);
55  }
56  }
57  return (error = E_NOERROR);
58  }
59  }
60  int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int seg_nr) {
61  unsigned int segmentNr;
62  if(seg_nr<0)
63  segmentNr=chain.getNrOfSegments();
64  else
65  segmentNr = seg_nr;
66 
67  if(q_in.rows()!=chain.getNrOfJoints())
68  return -1;
69  else if(segmentNr>chain.getNrOfSegments())
70  return -1;
71  else if(p_out.size() != segmentNr)
72  return -1;
73  else if(segmentNr == 0)
74  return -1;
75  else{
76  int j=0;
77  // Initialization
79  p_out[0] = chain.getSegment(0).pose(q_in(j));
80  j++;
81  }else
82  p_out[0] = chain.getSegment(0).pose(0.0);
83 
84  for(unsigned int i=1;i<segmentNr;i++){
86  p_out[i] = p_out[i-1]*chain.getSegment(i).pose(q_in(j));
87  j++;
88  }else{
89  p_out[i] = p_out[i-1]*chain.getSegment(i).pose(0.0);
90  }
91  }
92  return 0;
93  }
94  }
95 
97  {
98  }
99 
100 
101 }
KDL::ChainFkSolverPos_recursive::ChainFkSolverPos_recursive
ChainFkSolverPos_recursive(const Chain &chain)
Definition: chainfksolverpos_recursive.cpp:29
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::JntArray
Definition: jntarray.hpp:69
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
std::vector
std::vector::size
T size(T... args)
chainfksolverpos_recursive.hpp
KDL::ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive
~ChainFkSolverPos_recursive()
Definition: chainfksolverpos_recursive.cpp:96
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
KDL::ChainFkSolverPos_recursive::chain
const Chain & chain
Definition: chainfksolverpos_recursive.hpp:45
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
KDL::SolverI::E_OUT_OF_RANGE
@ E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Definition: chainfksolverpos_recursive.cpp:34
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149