KdlTreeIk.h

Go to the documentation of this file.

#ifndef KDL_TREE_IK_H
#define KDL_TREE_IK_H

#include "robodyn_controllers/MotionLimiter.h"
#include "robodyn_controllers/KdlTreeUtilities.h"
#include "robodyn_controllers/KdlTreeFk.h"

#include <kdl/treejnttojacsolver.hpp>
#include <kdl/frameacc.hpp>
#include <kdl/jntarrayacc.hpp>

#include <r2_msgs/PriorityArray.h>

#include <kdl/frames.hpp>
#include <kdl/utilities/svd_eigen_HH.hpp>
#include <iomanip>

class KdlTreeIk : public KdlTreeUtilities
{
public:
    typedef KDL::Twist NodePriority;
    static const int IGNORE;// = 0;
    static const int LOW;// = 1;
    static const int MEDIUM;// = 128;
    static const int HIGH;// = 254;
    static const int CRITICAL;// = 255;

    KdlTreeIk();
    ~KdlTreeIk();

    void resetSolver();

    inline double getTimeStep() const {return timeStep;}

    void setTimeStep(double timeStep_in = 0.01);


    // maximum number of iterations allowed to achieve critical convergience
    void setMaxCriticalIterations(unsigned int maxIter_in = 50)
    {
        critMaxIter = maxIter_in;
    }

    // maximum number of iterations allowed to achieve noncritical convergience
    void setMaxNonCriticalIterations(unsigned int maxIter_in = 5)
    {
        nonCritMaxIter = maxIter_in;
    }

    void setEpsilon(double eps_in = 1e-6)
    {
        eps = eps_in;
    }

    inline void setPositionLimiter(boost::shared_ptr<JointNamePositionLimiter> posLimiter) {positionLimiter = posLimiter;}

    void setLambda(double lambda = 0.05)
    {
        lambda_squared = lambda * lambda;
    }

    void setJointInertias(const std::map<std::string, double>& inertia_in);

    void setPriorityTol(const std::map<int, std::pair<double, double> >& priority_tol)
    {
        NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.KdlTreeIk") << log4cpp::Priority::INFO << "updated priority tol map: ";

        for (std::map<int, std::pair<double, double> >::const_iterator itr = priority_tol.begin(); itr != priority_tol.end(); ++itr)
        {
            NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.KdlTreeIk") << log4cpp::Priority::INFO << itr->first << ": (" << itr->second.first << "," << itr->second.second << ")";
        }

        priorityTolMap = priority_tol;
    }

    virtual void getFrames (const KDL::JntArray& joints_in, std::map<std::string, KDL::Frame>& frameMap);
    virtual void getJointPositions(const KDL::JntArray& joints_in, const std::vector<std::string>& nodeNames,
                                   const std::vector<KDL::Frame>& nodeFrames, KDL::JntArray& joints_out,
                                   const std::vector<NodePriority>& nodePriorities);

    std::auto_ptr<KdlTreeFk> fkPosSolverPtr;

protected:
    virtual void initialize();

    std::vector<std::string> jointNames;
    //std::auto_ptr<KdlTreeFk> fkPosSolverPtr;

    double timeStep;

    unsigned int critMaxIter;
    unsigned int nonCritMaxIter;
    double eps;
    double lambda_squared;

    std::auto_ptr<KDL::TreeJntToJacSolver> jacSolverPtr;

    KDL::JntArray inertias;

    std::map<std::string, double> inertiaMap;
    double minInertia;
    double maxInertia;

    std::map<int, std::pair<double, double> > priorityTolMap;

    boost::shared_ptr<JointNamePositionLimiter> positionLimiter;

private:

    void getJointVelocities(const KDL::JntArray& joints_in, const std::vector<std::pair<std::string, KDL::Twist> >& twist_deltas,
                            const std::vector<NodePriority>& nodePriorities, KDL::JntArray& joint_deltas) const;

    bool useDampedLeastSquares;
};

#endif