orocos_kdl: orocos_kdl: chaindynparam.hpp Source 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 #ifndef KDLCHAINDYNPARAM_HPP
00023 #define KDLCHAINDYNPARAM_HPP
00024 
00025 #include "chainidsolver_recursive_newton_euler.hpp"
00026 #include "articulatedbodyinertia.hpp"
00027 #include "jntspaceinertiamatrix.hpp"
00028 
00029 namespace KDL {
00030 
00046     class ChainDynParam
00047     {
00048     public:
00049         ChainDynParam(const Chain& chain, Vector _grav);
00050         ~ChainDynParam();
00051 
00052         virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis);
00053         virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H);
00054         virtual int JntToGravity(const JntArray &q,JntArray &gravity);
00055 
00056     private:
00057         const Chain chain;
00058         int nr;
00059         unsigned int nj;
00060         unsigned int ns;        
00061         Vector grav;
00062         Vector vectornull;
00063         JntArray jntarraynull;
00064         ChainIdSolver_RNE chainidsolver_coriolis;
00065         ChainIdSolver_RNE chainidsolver_gravity;
00066         std::vector<Wrench> wrenchnull;
00067         std::vector<Frame> X;
00068         std::vector<Twist> S;
00069         //std::vector<RigidBodyInertia> I;
00070         std::vector<ArticulatedBodyInertia> Ic;
00071         Wrench F;
00072         Twist ag;
00073         
00074     };
00075 
00076 }
00077 
00078 #endif