orocos_kdl
chainfksolvervel_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 
26 namespace KDL
27 {
29  chain(_chain)
30  {
31  }
32 
34  {
35  }
36 
38  {
39  unsigned int segmentNr;
40  if(seg_nr<0)
41  segmentNr=chain.getNrOfSegments();
42  else
43  segmentNr = seg_nr;
44 
45  out=FrameVel::Identity();
46 
47  if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
48  return (error = E_SIZE_MISMATCH);
49  else if(segmentNr>chain.getNrOfSegments())
50  return (error = E_OUT_OF_RANGE);
51  else{
52  int j=0;
53  for (unsigned int i=0;i<segmentNr;i++) {
54  //Calculate new Frame_base_ee
56  out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),
57  chain.getSegment(i).twist(in.q(j),in.qdot(j)));
58  j++;//Only increase jointnr if the segment has a joint
59  }else{
60  out=out*FrameVel(chain.getSegment(i).pose(0.0),
61  chain.getSegment(i).twist(0.0,0.0));
62  }
63  }
64  return (error = E_NOERROR);
65  }
66  }
67 
69  {
70  unsigned int segmentNr;
71  if(seg_nr<0)
72  segmentNr=chain.getNrOfSegments();
73  else
74  segmentNr = seg_nr;
75 
76 
77 
78  if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
79  return -1;
80  else if(segmentNr>chain.getNrOfSegments())
81  return -1;
82  else if(out.size()!=segmentNr)
83  return -1;
84  else if(segmentNr == 0)
85  return -1;
86  else{
87  int j=0;
88  // Initialization
90  out[0] = FrameVel(chain.getSegment(0).pose(in.q(0)),
91  chain.getSegment(0).twist(in.q(0),in.qdot(0)));
92  j++;
93  }else
94  out[0] = FrameVel(chain.getSegment(0).pose(0.0),
95  chain.getSegment(0).twist(0.0,0.0));
96 
97  for (unsigned int i=1;i<segmentNr;i++) {
98  //Calculate new Frame_base_ee
100  out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(in.q(j)),
101  chain.getSegment(i).twist(in.q(j),in.qdot(j)));
102  j++;//Only increase jointnr if the segment has a joint
103  }else{
104  out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(0.0),
105  chain.getSegment(i).twist(0.0,0.0));
106  }
107  }
108  return 0;
109  }
110  }
111 }
KDL::ChainFkSolverVel_recursive::JntToCart
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
Definition: chainfksolvervel_recursive.cpp:37
KDL::ChainFkSolverVel_recursive::chain
const Chain & chain
Definition: chainfksolvervel_recursive.hpp:45
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
std::vector
std::vector::size
T size(T... args)
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
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::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::ChainFkSolverVel_recursive::~ChainFkSolverVel_recursive
~ChainFkSolverVel_recursive()
Definition: chainfksolvervel_recursive.cpp:33
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
KDL::FrameVel::Identity
static IMETHOD FrameVel Identity()
Definition: framevel.inl:28
chainfksolvervel_recursive.hpp
KDL::JntArrayVel::qdot
JntArray qdot
Definition: jntarrayvel.hpp:49
KDL::FrameVel
Definition: framevel.hpp:209
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KDL::ChainFkSolverVel_recursive::ChainFkSolverVel_recursive
ChainFkSolverVel_recursive(const Chain &chain)
Definition: chainfksolvervel_recursive.cpp:28
KDL::JntArrayVel::q
JntArray q
Definition: jntarrayvel.hpp:48
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