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             nr(0),
00031             nj(chain.getNrOfJoints()),
00032             ns(chain.getNrOfSegments()),
00033             grav(_grav),
00034             jntarraynull(nj),
00035             chainidsolver_coriolis( chain, Vector::Zero()),
00036             chainidsolver_gravity( chain, grav),
00037             wrenchnull(ns,Wrench::Zero()),
00038             X(ns),
00039             S(ns),
00040             Ic(ns)
00041     {
00042         ag=-Twist(grav,Vector::Zero());
00043     }
00044 
00045     //calculate inertia matrix H
00046     int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
00047     {
00048         //Check sizes when in debug mode
00049         if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
00050             return (error = E_SIZE_MISMATCH);
00051         unsigned int k=0;
00052         double q_;
00053         
00054         //Sweep from root to leaf
00055         for(unsigned int i=0;i<ns;i++)
00056         {
00057           //Collect RigidBodyInertia
00058           Ic[i]=chain.getSegment(i).getInertia();
00059           if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00060           {
00061               q_=q(k);
00062               k++;
00063           }
00064           else
00065           {
00066             q_=0.0;
00067           }
00068           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
00069           S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));  
00070         }
00071         //Sweep from leaf to root
00072         int j,l;
00073         k=nj-1; //reset k
00074         for(int i=ns-1;i>=0;i--)
00075         {
00076           
00077           if(i!=0)
00078             {
00079               //assumption that previous segment is parent
00080               Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
00081             } 
00082 
00083           F=Ic[i]*S[i];
00084           if(chain.getSegment(i).getJoint().getType()!=Joint::None)
00085           {
00086               H(k,k)=dot(S[i],F);
00087               j=k; //countervariable for the joints
00088               l=i; //countervariable for the segments
00089               while(l!=0) //go from leaf to root starting at i
00090                 {
00091                   //assumption that previous segment is parent
00092                   F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
00093                   l--; //go down a segment
00094                   
00095                   if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
00096                   {    
00097                     j--;
00098                     H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment 
00099                     H(j,k)=H(k,j);
00100                   }
00101                 } 
00102               k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
00103           }
00104 
00105         }
00106         return (error = E_NOERROR);
00107     }
00108 
00109     //calculate coriolis matrix C
00110     int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
00111     {
00112     //make a null matrix with the size of q_dotdot and a null wrench
00113         SetToZero(jntarraynull);
00114 
00115         
00116         //the calculation of coriolis matrix C
00117         return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
00118         
00119     }
00120 
00121     //calculate gravity matrix G
00122     int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
00123     {
00124 
00125         //make a null matrix with the size of q_dotdot and a null wrench
00126         
00127         SetToZero(jntarraynull);
00128         //the calculation of coriolis matrix C
00129         return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
00130     }
00131 
00132     ChainDynParam::~ChainDynParam()
00133     {
00134     }
00135 
00136 
00137 }


orocos_kdl
Author(s):
autogenerated on Sat Oct 7 2017 03:04:28