chainfksolvervel_recursive.cpp
Go to the documentation of this file.
1 // Copyright (C) 2020 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Francois Cauwe <francois at cauwe dot org>
5 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
6 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
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 
25 namespace KDL
26 {
28  chain(_chain)
29  {
30  }
31 
33  {
34  }
35 
37  {
38  unsigned int segmentNr;
39  if(seg_nr<0)
40  segmentNr=chain.getNrOfSegments();
41  else
42  segmentNr = seg_nr;
43 
44  out=FrameVel::Identity();
45 
46  if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
47  return (error = E_SIZE_MISMATCH);
48  else if(segmentNr>chain.getNrOfSegments())
49  return (error = E_OUT_OF_RANGE);
50  else{
51  int j=0;
52  for (unsigned int i=0;i<segmentNr;i++) {
53  //Calculate new Frame_base_ee
55  out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),
56  chain.getSegment(i).twist(in.q(j),in.qdot(j)));
57  j++;//Only increase jointnr if the segment has a joint
58  }else{
59  out=out*FrameVel(chain.getSegment(i).pose(0.0),
60  chain.getSegment(i).twist(0.0,0.0));
61  }
62  }
63  return (error = E_NOERROR);
64  }
65  }
66 
67  int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,std::vector<FrameVel>& out,int seg_nr)
68  {
69  unsigned int segmentNr;
70  if(seg_nr<0)
71  segmentNr=chain.getNrOfSegments();
72  else
73  segmentNr = seg_nr;
74 
75 
76 
77  if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
78  return -1;
79  else if(segmentNr>chain.getNrOfSegments())
80  return -1;
81  else if(out.size()!=segmentNr)
82  return -1;
83  else if(segmentNr == 0)
84  return -1;
85  else{
86  int j=0;
87  // Initialization
89  out[0] = FrameVel(chain.getSegment(0).pose(in.q(0)),
90  chain.getSegment(0).twist(in.q(0),in.qdot(0)));
91  j++;
92  }else
93  out[0] = FrameVel(chain.getSegment(0).pose(0.0),
94  chain.getSegment(0).twist(0.0,0.0));
95 
96  for (unsigned int i=1;i<segmentNr;i++) {
97  //Calculate new Frame_base_ee
99  out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(in.q(j)),
100  chain.getSegment(i).twist(in.q(j),in.qdot(j)));
101  j++;//Only increase jointnr if the segment has a joint
102  }else{
103  out[i]=out[i-1]*FrameVel(chain.getSegment(i).pose(0.0),
104  chain.getSegment(i).twist(0.0,0.0));
105  }
106  }
107  return 0;
108  }
109  }
110 }
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
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
Input size does not match internal state.
Definition: solveri.hpp:99
unsigned int rows() const
Definition: jntarray.cpp:72
const Joint & getJoint() const
Definition: segment.hpp:118
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Frame pose(const double &q) const
Definition: segment.cpp:57
static IMETHOD FrameVel Identity()
Definition: framevel.hpp:29
Requested index out of range.
Definition: solveri.hpp:103
const JointType & getType() const
Definition: joint.hpp:159
virtual int JntToCart(const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149


orocos_kdl
Author(s):
autogenerated on Tue Sep 1 2020 03:18:51