KdlTreeUtilities.h

Go to the documentation of this file.

#ifndef KDL_TREE_UTILITIES_H
#define KDL_TREE_UTILITIES_H

#include <kdl/jntarray.hpp>
#include <robodyn_controllers/KdlTreeParser.h>
#include <iostream>

class KdlTreeUtilities : public KdlTreeParser
{
public:
    KdlTreeUtilities();
    ~KdlTreeUtilities();

    inline virtual std::string getBaseName() const
    {
        return tree.getRootSegment()->first;
    }

    virtual void getJointNames(std::vector<std::string>& jointNames) const;
    inline virtual unsigned int getJointCount() const
    {
        return tree.getNrOfJoints();
    }

    void getChainJointNames(const KDL::Chain& chain,
                            std::vector<std::string>& jointNames) const;

    virtual void getChainJointNames(const std::string& toolFrame,
                                    std::vector<std::string>& jointNames) const;

    virtual void getChainJointNames(const std::string& baseFrame, const std::string& toolFrame,
                                    std::vector<std::string>& jointNames) const;


    bool hasSegment(const std::string& segName) const
    {
        return getTree().getSegment(segName) != getTree().getSegments().end();
    }

    virtual bool getChain(const std::string& baseFrame, const std::string& toolFrame, KDL::Chain& chain) const;

    virtual void addSegment(const KDL::Segment& segment, const std::string& hookName);

    virtual bool removeSegment(const std::string& segName);

    bool removeSegRecursive(KDL::Tree& tree, KDL::SegmentMap::const_iterator root, const std::string& hook_name, const std::string& seg_remove_name);

protected:

private:

};

#endif