Go to the documentation of this file.00001
00006 #ifndef KDL_TREE_IK_H
00007 #define KDL_TREE_IK_H
00008
00009 #include "robodyn_controllers/MotionLimiter.h"
00010 #include "robodyn_controllers/KdlTreeUtilities.h"
00011 #include "robodyn_controllers/KdlTreeFk.h"
00012
00013 #include <kdl/treejnttojacsolver.hpp>
00014 #include <kdl/frameacc.hpp>
00015 #include <kdl/jntarrayacc.hpp>
00016
00017 #include <r2_msgs/PriorityArray.h>
00018
00019 #include <kdl/frames.hpp>
00020 #include <kdl/utilities/svd_eigen_HH.hpp>
00021 #include <iomanip>
00022
00023 class KdlTreeIk : public KdlTreeUtilities
00024 {
00025 public:
00026 typedef KDL::Twist NodePriority;
00027 static const int IGNORE;
00028 static const int LOW;
00029 static const int MEDIUM;
00030 static const int HIGH;
00031 static const int CRITICAL;
00032
00033 KdlTreeIk();
00034 ~KdlTreeIk();
00035
00036 void resetSolver();
00037
00038 inline double getTimeStep() const {return timeStep;}
00039
00046 void setTimeStep(double timeStep_in = 0.01);
00047
00048
00049
00050 void setMaxCriticalIterations(unsigned int maxIter_in = 50)
00051 {
00052 critMaxIter = maxIter_in;
00053 }
00054
00055
00056 void setMaxNonCriticalIterations(unsigned int maxIter_in = 5)
00057 {
00058 nonCritMaxIter = maxIter_in;
00059 }
00060
00061 void setEpsilon(double eps_in = 1e-6)
00062 {
00063 eps = eps_in;
00064 }
00065
00066 inline void setPositionLimiter(boost::shared_ptr<JointNamePositionLimiter> posLimiter) {positionLimiter = posLimiter;}
00067
00068 void setLambda(double lambda = 0.05)
00069 {
00070 lambda_squared = lambda * lambda;
00071 }
00072
00073 void setJointInertias(const std::map<std::string, double>& inertia_in);
00074
00075 void setPriorityTol(const std::map<int, std::pair<double, double> >& priority_tol)
00076 {
00077 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.KdlTreeIk") << log4cpp::Priority::INFO << "updated priority tol map: ";
00078
00079 for (std::map<int, std::pair<double, double> >::const_iterator itr = priority_tol.begin(); itr != priority_tol.end(); ++itr)
00080 {
00081 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.KdlTreeIk") << log4cpp::Priority::INFO << itr->first << ": (" << itr->second.first << "," << itr->second.second << ")";
00082 }
00083
00084 priorityTolMap = priority_tol;
00085 }
00086
00095 virtual void getFrames (const KDL::JntArray& joints_in, std::map<std::string, KDL::Frame>& frameMap);
00096 virtual void getJointPositions(const KDL::JntArray& joints_in, const std::vector<std::string>& nodeNames,
00097 const std::vector<KDL::Frame>& nodeFrames, KDL::JntArray& joints_out,
00098 const std::vector<NodePriority>& nodePriorities);
00099
00100 std::auto_ptr<KdlTreeFk> fkPosSolverPtr;
00101
00102 protected:
00103 virtual void initialize();
00104
00105 std::vector<std::string> jointNames;
00106
00107
00108 double timeStep;
00109
00110 unsigned int critMaxIter;
00111 unsigned int nonCritMaxIter;
00112 double eps;
00113 double lambda_squared;
00114
00115 std::auto_ptr<KDL::TreeJntToJacSolver> jacSolverPtr;
00116
00119 KDL::JntArray inertias;
00120
00124 std::map<std::string, double> inertiaMap;
00125 double minInertia;
00126 double maxInertia;
00127
00132 std::map<int, std::pair<double, double> > priorityTolMap;
00133
00134 boost::shared_ptr<JointNamePositionLimiter> positionLimiter;
00135
00136 private:
00137
00141 void getJointVelocities(const KDL::JntArray& joints_in, const std::vector<std::pair<std::string, KDL::Twist> >& twist_deltas,
00142 const std::vector<NodePriority>& nodePriorities, KDL::JntArray& joint_deltas) const;
00143
00144 bool useDampedLeastSquares;
00145 };
00146
00147 #endif