Go to the documentation of this file.00001 #ifndef KDL_TREE_TR_H
00002 #define KDL_TREE_TR_H
00003
00004 #include "robodyn_controllers/KdlTreeIk.h"
00005 #include "kdl/articulatedbodyinertia.hpp"
00006 #include <Eigen/Core>
00007 #include <kdl/frames.hpp>
00008 #include <kdl/utilities/svd_eigen_HH.hpp>
00009 #include <iomanip>
00010
00011 class KdlTreeTr : public KdlTreeIk
00012 {
00013 public:
00014 KdlTreeTr();
00015 ~KdlTreeTr();
00016
00017 virtual void getJointPositions(const KDL::JntArray& joints_in, const std::vector<std::string>& nodeNames,
00018 const std::vector<KDL::Frame>& nodeFrames, KDL::JntArray& joints_out,
00019 const std::vector<NodePriority>& nodePriorities);
00020
00021 inline void setMBar(double mbar = 1e-7) { this->mbar = mbar; }
00022 inline void setKr(double Kr = 0.00032) {this->Kr = Kr;}
00023 inline void setMaxTwist(double maxTwist = 0.1) {this->maxTwist = maxTwist;}
00024 inline void setMaxJointDecel(double maxDecel = 1.0) {this->jointMaxDecel = std::abs(maxDecel);}
00025 virtual void initialize();
00026 void setJointInertias(const std::map<std::string, double>& inertia_in);
00027 bool limitJoints(const KDL::JntArray& joints_in, KDL::JntArray& joints_out, KDL::JntArray& delta_joints);
00028
00029 protected:
00030
00031 typedef std::map<std::string, KDL::Frame>::iterator FrameMapItr;
00032 typedef std::pair<KDL::Twist, NodePriority> TwistPriority;
00033 typedef std::map<std::string, TwistPriority>::iterator TwistMapItr;
00034 typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Task;
00035 typedef std::multimap<int, std::pair<std::string, int> >::iterator PriorityTaskAxisItr;
00036
00037 double mbar;
00038 double Kr;
00039 double maxTwist;
00040 double jointMaxDecel;
00041
00042 bool limitTaskTwist(Eigen::VectorXd& twist);
00043 bool limitTaskTwist(KDL::Twist& twist);
00044 private:
00045
00046 int numJoints;
00047 int numConstraints;
00048 std::vector<std::string> nodeNames;
00049 std::vector<NodePriority> nodePriorities;
00050 std::map<std::string, KDL::Frame> currentFrameMap;
00051 std::map<std::string, KDL::Frame> desiredFrameMap;
00052 std::map<std::string, TwistPriority > twistMap;
00053 std::vector<Task> taskList;
00054 std::multimap<int, std::pair<std::string, int> > priorityTaskAxisMap;
00055
00056
00057 void createFrameTwistMaps(const std::vector<KDL::Frame>& nodeFrames, const std::vector<NodePriority>& NodePriorities);
00058 void updateNodeFrameMap(const std::vector<KDL::Frame>& nodeFrames);
00059 bool findFrameTwist(const KDL::JntArray& joints_in);
00060 bool isTaskAchieved(const KDL::JntArray& joints_in, std::stringstream& errMsg);
00061 void findJointInertia(const KDL::JntArray& joints_in, KDL::RotationalInertia& inertia_out);
00062
00063 void getTaskReconstruction(const KDL::JntArray& joints_in, KDL::JntArray& joint_deltas);
00064 void getOrderedTasksByPriority(const KDL::JntArray& joints_in);
00065 void getOrderedTasksByFrame(const KDL::JntArray& joints_in);
00066 Task getCenterJointsTask(const KDL::JntArray& joints_in);
00067
00068 void modifyToAvoidSingularities(const Eigen::VectorXd& nm, const double mom, const Eigen::VectorXd& dr, Eigen::VectorXd& drp );
00069
00070 inline double sign(double num) {return ( num < 0. ? -1. : 1.);}
00071 double cubic(const double& q, const double& pLeft, const double& pRight, const double& ct1, const double& ct2);
00072 double boundedCubic(const double& q, const double& pLeft, const double& pRight, const double& ct1, const double& ct2);
00073 int pInv(const Eigen::MatrixXd& jac_in, Eigen::MatrixXd& ijac_out, double& mom);
00074
00075
00076 Eigen::VectorXd getBiasVector(const Eigen::VectorXd& joints_in, const Eigen::VectorXd& joint_deltas, const Eigen::MatrixXd& ijac);
00077 Eigen::MatrixXd jacJntWeights;
00078 Eigen::MatrixXd ijacJntWeights;
00079
00080 bool velocityLim;
00081 bool jointLim;
00082 bool momLim;
00083 };
00084
00085 #endif // KDL_TREE_TR_H