00001 00006 #ifndef MOBILE_TREE_IK_H 00007 #define MOBILE_TREE_IK_H 00008 00009 #include "robodyn_controllers/KdlTreeTr.h" 00010 00011 class MobileTreeIk : public KdlTreeTr 00012 { 00013 public: 00014 static const int BASE; 00015 00016 MobileTreeIk(); 00017 ~MobileTreeIk(); 00018 00019 void setBases(const std::vector<std::string>& bases_in); 00020 00021 void resetMobileJoints(); 00022 00031 virtual void getJointPositions(const KDL::JntArray& joints_in, const std::vector<std::string>& nodeNames, 00032 const std::vector<KDL::Frame>& nodeFrames, KDL::JntArray& joints_out, 00033 const std::vector<NodePriority>& nodePriorities); 00034 00035 inline virtual std::string getBaseName() const {return robotModelBase;} 00036 virtual void getJointNames(std::vector<std::string>& jointNames) const; 00037 inline virtual unsigned int getJointCount() const { return tree.getNrOfJoints() - 6; } 00038 // void setJointInertias(const KDL::JntArray &inertia_in); 00039 virtual void getFrames (const KDL::JntArray& joints_in, std::map<std::string, KDL::Frame>& frameMap); 00040 void setPriorityTol(const std::map<int, std::pair<double, double> >& priority_tol) 00041 { 00042 KdlTreeTr::setPriorityTol(priority_tol); 00043 priorityTolMap[MobileTreeIk::BASE] = std::make_pair(eps, eps); 00044 } 00045 00046 protected: 00047 virtual void initialize(); 00048 00049 private: 00050 KDL::JntArray mobileJoints; 00051 std::vector<std::string> bases; 00052 std::string robotModelBase; 00053 00054 }; 00055 00056 #endif