Go to the documentation of this file.00001 #ifndef KDL_TREE_ID_H
00002 #define KDL_TREE_ID_H
00003
00004 #include "robodyn_controllers/KdlChainIdRne.h"
00005 #include "robodyn_controllers/JointDynamicsData.h"
00006 #include "robodyn_controllers/KdlTreeUtilities.h"
00007 #include <kdl/chainfksolverpos_recursive.hpp>
00008 #include <memory>
00009
00010
00011 class KdlTreeId : public KdlTreeUtilities
00012 {
00013 public:
00014 KdlTreeId();
00015 ~KdlTreeId();
00016
00017 bool isBaseFrameInTree(const std::string& baseFrame);
00018
00019 void getAccelInBaseFrame(KDL::Vector gravity, const std::string& gravityFrame, JointDynamicsData& jd, const std::string& base, KDL::Twist& a_in);
00020
00021 bool treeRecursiveNewtonEuler(JointDynamicsData& jd, const std::string& baseFrame, const std::string& ignoreFrame, const KDL::Twist& v_in, const KDL::Twist& a_in, KDL::Wrench& f_out, KDL::RigidBodyInertia& I_out);
00022
00023 void findChainFromNode(const std::string& baseFrame, const std::string& direction, std::string& toolFrame);
00024
00025 int findBranchNodes(const std::string& baseFrame, const std::string& ignoreFrame, std::vector<std::string>& nodeList, std::vector<std::string>& direction);
00026
00027 KDL::Wrench sumForces(const KDL::Wrenches& f_in);
00028
00029 protected:
00030 virtual void initialize();
00031 std::vector<std::string> jointNames;
00032
00033 void setFrames(const std::string& gravityFrame_in, const std::string& baseFrame_in);
00034
00035 std::string gravityFrame;
00036 std::string baseFrame;
00037 KDL::Chain gravBaseChain;
00038 std::auto_ptr<KDL::ChainFkSolverPos_recursive> chainFkSolver;
00039 KDL::JntArrayAcc joints;
00040 };
00041
00042 #endif