Go to the documentation of this file.00001
00006 #ifndef KDL_TREE_FK_H
00007 #define KDL_TREE_FK_H
00008
00009 #include <kdl/treefksolver.hpp>
00010 #include <memory>
00011 #include <kdl/jntarrayvel.hpp>
00012 #include <kdl/framevel.hpp>
00013
00014 #include "robodyn_controllers/KdlTreeUtilities.h"
00015 #include <kdl/treefksolverpos_recursive.hpp>
00016 #include "nasa_common_logging/Logger.h"
00017
00018 class KdlTreeFk : public KdlTreeUtilities
00019 {
00020 public:
00021 KdlTreeFk();
00022 ~KdlTreeFk();
00023
00024 void getPose(const KDL::JntArray& joints, const std::string& name, KDL::Frame& frame);
00032 void getPoses(const KDL::JntArray& joints, std::map<std::string, KDL::Frame>& frames);
00040 void getVelocities(const KDL::JntArrayVel& joints, std::map<std::string, KDL::FrameVel>& frames);
00041
00042 protected:
00043 virtual void initialize();
00044
00045 void recursiveGetPoses(bool getAll, const KDL::Frame& baseFrame, const KDL::SegmentMap::const_iterator& it,
00046 const KDL::JntArray& joints, std::map<std::string, KDL::Frame>& frames);
00047
00048 void recursiveGetVels(bool getAll, const KDL::FrameVel& baseFrame, const KDL::SegmentMap::const_iterator& it,
00049 const KDL::JntArrayVel& joints, std::map<std::string, KDL::FrameVel>& frames);
00050
00051 private:
00052 std::auto_ptr<KDL::TreeFkSolverPos> fkSolver;
00053 };
00054
00055 #endif