chaindynparam.cpp

Go to the documentation of this file.
00001 // Copyright  (C)  2009  Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "chaindynparam.hpp"
00023 #include "frames_io.hpp"
00024 #include <iostream>
00025 
00026 namespace KDL {
00027 
00028     ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
00029         chain(_chain), 
00030         grav(_grav),
00031         chainidsolver_coriolis( chain, Vector::Zero()),
00032         chainidsolver_gravity( chain, grav),
00033         nj(chain.getNrOfJoints()),
00034         ns(chain.getNrOfSegments()),
00035         jntarraynull(nj),
00036         wrenchnull(ns,Wrench::Zero()),
00037         Ic(ns),
00038         X(ns),
00039         S(ns)
00040     {
00041         ag=-Twist(grav,Vector::Zero());
00042     }
00043 
00044     //calculate inertia matrix H
00045     int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
00046     {
00047         //Check sizes when in debug mode
00048         if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
00049             return -1;
00050         unsigned int k=0;
00051         double q_;
00052         
00053         //Sweep from root to leaf
00054         for(unsigned int i=0;i<ns;i++)
00055         {
00056           //Collect RigidBodyInertia
00057           Ic[i]=chain.getSegment(i).getInertia();
00058           if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00059           {
00060               q_=q(k);
00061               k++;
00062           }
00063           else
00064           {
00065             q_=0.0;
00066           }
00067           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
00068           S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));  
00069         }
00070         //Sweep from leaf to root
00071         int j,l;
00072         k=nj-1; //reset k
00073         for(int i=ns-1;i>=0;i--)
00074         {
00075           
00076           if(i!=0)
00077             {
00078               //assumption that previous segment is parent
00079               Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
00080             } 
00081 
00082           F=Ic[i]*S[i];
00083           if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00084           {
00085               H(k,k)=dot(S[i],F);
00086               j=k; //countervariable for the joints
00087               l=i; //countervariable for the segments
00088               while(l!=0) //go from leaf to root starting at i
00089                 {
00090                   //assumption that previous segment is parent
00091                   F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
00092                   l--; //go down a segment
00093                   
00094                   if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
00095                   {    
00096                     j--;
00097                     H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment 
00098                     H(j,k)=H(k,j);
00099                   }
00100                 } 
00101               k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
00102           }
00103 
00104         }
00105     }
00106 
00107     //calculate coriolis matrix C
00108     int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
00109     {
00110         //make a null matrix with the size of q_dotdot and a null wrench
00111         SetToZero(jntarraynull);
00112 
00113         
00114         //the calculation of coriolis matrix C
00115         return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
00116         
00117 
00118     }
00119 
00120     //calculate gravity matrix G
00121     int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
00122     {
00123 
00124         //make a null matrix with the size of q_dotdot and a null wrench
00125         
00126         SetToZero(jntarraynull);
00127         //the calculation of coriolis matrix C
00128         return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
00129     }
00130 
00131     ChainDynParam::~ChainDynParam()
00132     {
00133     }
00134 
00135 
00136 }

Generated on Wed Nov 23 19:00:00 2011 for FreeCAD by  doxygen 1.6.1