Blender  V3.3
chainfksolverpos_recursive.cpp
Go to the documentation of this file.
1 
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 
27 #include <iostream>
28 
29 namespace KDL {
30 
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 
60  {
61  }
62 
63 
64 }
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
This class encapsulates a serial kinematic interconnection structure. It is build out of segments.
Definition: chain.hpp:36
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
unsigned int getNrOfJoints() const
Definition: chain.hpp:73
unsigned int getNrOfSegments() const
Definition: chain.hpp:78
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
unsigned int getNDof() const
Definition: joint.cpp:149
Frame pose(const double *q) const
Definition: segment.cpp:51
const Joint & getJoint() const
Definition: segment.hpp:120
Definition: chain.cpp:27