orocos_kdl
chaindynparam.cpp
Go to the documentation of this file.
1 // Copyright (C) 2009 Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Dominick Vanthienen <dominick dot vanthienen at intermodalics dot eu>
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 
23 #include "chaindynparam.hpp"
24 #include "frames_io.hpp"
25 #include <iostream>
26 
27 namespace KDL {
28 
29  ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
30  chain(_chain),
31  nr(0),
32  nj(chain.getNrOfJoints()),
33  ns(chain.getNrOfSegments()),
34  grav(_grav),
35  jntarraynull(nj),
36  chainidsolver_coriolis( chain, Vector::Zero()),
37  chainidsolver_gravity( chain, grav),
38  wrenchnull(ns,Wrench::Zero()),
39  X(ns),
40  S(ns),
41  Ic(ns)
42  {
44  }
45 
52  wrenchnull.resize(ns,Wrench::Zero());
53  X.resize(ns);
54  S.resize(ns);
55  Ic.resize(ns);
56  }
57 
58 
59  //calculate inertia matrix H
60  int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
61  {
62  if(nj != chain.getNrOfJoints() || ns != chain.getNrOfSegments())
63  return (error = E_NOT_UP_TO_DATE);
64  //Check sizes when in debug mode
65  if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
66  return (error = E_SIZE_MISMATCH);
67  unsigned int k=0;
68  double q_;
69 
70  //Sweep from root to leaf
71  for(unsigned int i=0;i<ns;i++)
72  {
73  //Collect RigidBodyInertia
74  Ic[i]=chain.getSegment(i).getInertia();
76  {
77  q_=q(k);
78  k++;
79  }
80  else
81  {
82  q_=0.0;
83  }
84  X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
85  S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
86  }
87  //Sweep from leaf to root
88  int j,l;
89  k=nj-1; //reset k
90  for(int i=ns-1;i>=0;i--)
91  {
92 
93  if(i!=0)
94  {
95  //assumption that previous segment is parent
96  Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
97  }
98 
99  F=Ic[i]*S[i];
101  {
102  H(k,k)=dot(S[i],F);
103  H(k,k)+=chain.getSegment(i).getJoint().getInertia(); // add joint inertia
104  j=k; //countervariable for the joints
105  l=i; //countervariable for the segments
106  while(l!=0) //go from leaf to root starting at i
107  {
108  //assumption that previous segment is parent
109  F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
110  l--; //go down a segment
111 
112  if(chain.getSegment(l).getJoint().getType()!=Joint::Fixed) //if the joint connected to segment is not a fixed joint
113  {
114  j--;
115  H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
116  H(j,k)=H(k,j);
117  }
118  }
119  k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
120  }
121 
122  }
123  return (error = E_NOERROR);
124  }
125 
126  //calculate coriolis matrix C
127  int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
128  {
129  //make a null matrix with the size of q_dotdot and a null wrench
131 
132 
133  //the calculation of coriolis matrix C
134  return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
135 
136  }
137 
138  //calculate gravity matrix G
140  {
141 
142  //make a null matrix with the size of q_dotdot and a null wrench
143 
145  //the calculation of coriolis matrix C
147  }
148 
150  {
151  }
152 
153 
154 }
KDL::ChainDynParam::wrenchnull
std::vector< Wrench > wrenchnull
Definition: chaindynparam.hpp:70
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainIdSolver_RNE::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
Definition: chainidsolver_recursive_newton_euler.cpp:44
KDL::ChainDynParam::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chaindynparam.cpp:46
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
KDL::ChainDynParam::ag
Twist ag
Definition: chaindynparam.hpp:76
frames_io.hpp
KDL::JntArray::resize
void resize(unsigned int newSize)
Definition: jntarray.cpp:53
KDL::ChainDynParam::Ic
std::vector< ArticulatedBodyInertia, Eigen::aligned_allocator< ArticulatedBodyInertia > > Ic
Definition: chaindynparam.hpp:74
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
iostream
KDL::ChainDynParam::X
std::vector< Frame > X
Definition: chaindynparam.hpp:71
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
chaindynparam.hpp
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
KDL::ChainDynParam::nj
unsigned int nj
Definition: chaindynparam.hpp:63
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
KDL::ChainDynParam::ns
unsigned int ns
Definition: chaindynparam.hpp:64
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::Segment::pose
Frame pose(const double &q) const
Definition: segment.cpp:57
KDL::ChainDynParam::JntToMass
virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix &H)
Definition: chaindynparam.cpp:60
KDL::Joint::getInertia
const double & getInertia() const
Definition: joint.hpp:220
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::ChainDynParam::ChainDynParam
ChainDynParam(const Chain &chain, Vector _grav)
Definition: chaindynparam.cpp:29
KDL::Wrench::Zero
static Wrench Zero()
Definition: frames.inl:185
KDL::ChainDynParam::grav
Vector grav
Definition: chaindynparam.hpp:65
KDL::ChainDynParam::S
std::vector< Twist > S
Definition: chaindynparam.hpp:72
KDL::ChainDynParam::chainidsolver_coriolis
ChainIdSolver_RNE chainidsolver_coriolis
Definition: chaindynparam.hpp:68
KDL::ChainDynParam::chainidsolver_gravity
ChainIdSolver_RNE chainidsolver_gravity
Definition: chaindynparam.hpp:69
KDL::ChainDynParam::JntToGravity
virtual int JntToGravity(const JntArray &q, JntArray &gravity)
Definition: chaindynparam.cpp:139
KDL::ChainDynParam::F
Wrench F
Definition: chaindynparam.hpp:75
KDL::ChainDynParam::~ChainDynParam
virtual ~ChainDynParam()
Definition: chaindynparam.cpp:149
KDL::Joint::getType
const JointType & getType() const
Definition: joint.hpp:159
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainDynParam::chain
const Chain & chain
Definition: chaindynparam.hpp:61
KDL::ChainDynParam::JntToCoriolis
virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
Definition: chaindynparam.cpp:127
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::Segment::getInertia
const RigidBodyInertia & getInertia() const
Definition: segment.hpp:128
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
KDL::ChainDynParam::jntarraynull
JntArray jntarraynull
Definition: chaindynparam.hpp:67
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainIdSolver_RNE::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainidsolver_recursive_newton_euler.cpp:34