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 mech dot kuleuven dot be>
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  chain(_chain)
30  {
31  }
32 
33  int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) {
34  unsigned int segmentNr;
35  if(seg_nr<0)
36  segmentNr=chain.getNrOfSegments();
37  else
38  segmentNr = seg_nr;
39 
40  p_out = Frame::Identity();
41 
42  if(q_in.rows()!=chain.getNrOfJoints())
43  return (error = E_SIZE_MISMATCH);
44  else if(segmentNr>chain.getNrOfSegments())
45  return (error = E_OUT_OF_RANGE);
46  else{
47  int j=0;
48  for(unsigned int i=0;i<segmentNr;i++){
50  p_out = p_out*chain.getSegment(i).pose(q_in(j));
51  j++;
52  }else{
53  p_out = p_out*chain.getSegment(i).pose(0.0);
54  }
55  }
56  return (error = E_NOERROR);
57  }
58  }
59  int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, std::vector<Frame>& p_out, int seg_nr) {
60  unsigned int segmentNr;
61  if(seg_nr<0)
62  segmentNr=chain.getNrOfSegments();
63  else
64  segmentNr = seg_nr;
65 
66  if(q_in.rows()!=chain.getNrOfJoints())
67  return -1;
68  else if(segmentNr>chain.getNrOfSegments())
69  return -1;
70  else if(p_out.size() != segmentNr)
71  return -1;
72  else if(segmentNr == 0)
73  return -1;
74  else{
75  int j=0;
76  // Initialization
78  p_out[0] = chain.getSegment(0).pose(q_in(j));
79  j++;
80  }else
81  p_out[0] = chain.getSegment(0).pose(0.0);
82 
83  for(unsigned int i=1;i<segmentNr;i++){
85  p_out[i] = p_out[i-1]*chain.getSegment(i).pose(q_in(j));
86  j++;
87  }else{
88  p_out[i] = p_out[i-1]*chain.getSegment(i).pose(0.0);
89  }
90  }
91  return 0;
92  }
93  }
94 
96  {
97  }
98 
99 
100 }
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
unsigned int rows() const
Definition: jntarray.cpp:72
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
Input size does not match internal state.
Definition: solveri.hpp:99
Frame pose(const double &q) const
Definition: segment.cpp:57
Requested index out of range.
Definition: solveri.hpp:103
const Joint & getJoint() const
Definition: segment.hpp:118
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
static Frame Identity()
Definition: frames.hpp:696
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
const JointType & getType() const
Definition: joint.hpp:159


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:15:38