00001 #ifndef KDL_CHAIN_ID_INERTIA_H 00002 #define KDL_CHAIN_ID_INERTIA_H 00003 00004 #include <kdl/chain.hpp> 00005 #include <kdl/frames.hpp> 00006 #include <kdl/jntarray.hpp> 00007 #include <kdl/chainidsolver.hpp> 00008 #include <kdl/rigidbodyinertia.hpp> 00009 00024 class KdlChainIdInertia { 00025 public: 00031 KdlChainIdInertia(const KDL::Chain& chain,KDL::Vector grav); 00032 ~KdlChainIdInertia(){}; 00033 00045 int CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const KDL::Wrenches& f_ext,KDL::JntArray &torques, KDL::JntArray &Hv); 00046 00047 private: 00048 KDL::Chain chain; 00049 unsigned int nj; 00050 unsigned int ns; 00051 std::vector<KDL::Frame> X; 00052 std::vector<KDL::Twist> S; 00053 std::vector<KDL::Twist> v; 00054 std::vector<KDL::Twist> a; 00055 std::vector<KDL::Wrench> f; 00056 std::vector<KDL::RigidBodyInertia> Ii; 00057 KDL::Wrench Fis; 00058 KDL::Twist ag; 00059 }; 00060 00061 #endif