TreeIkTrajectory.h

Go to the documentation of this file.

#ifndef TREE_IK_TRAJECTORY_H
#define TREE_IK_TRAJECTORY_H

#include <robodyn_controllers/Trajectory.h>
#include <robodyn_controllers/KdlTreeIk.h>

class TreeIkTrajectory : public JointTrajectory
{
public:
    TreeIkTrajectory();
    virtual ~TreeIkTrajectory() {}

    void setCartesianTrajectory(boost::shared_ptr<SynchedCartesianTrajectory> traj_in)
    {
        setDuration(traj_in->getDuration());
        trajectory = traj_in;
    }

    void setTreeIk(boost::shared_ptr<KdlTreeIk> treeIk_in)
    {
        treeIk = treeIk_in;
    }

    void setInitialJoints(const KDL::JntArrayAcc& joints_in)
    {
        jointsLast = jointsInit = joints_in;
    }

    void setNodeNames(const std::vector<std::string>& nodeNames_in)
    {
        nodeNames = nodeNames_in;
    }

    void setNodePriorities(const std::vector<KdlTreeIk::NodePriority>& nodePriorities_in)
    {
        nodePriorities = nodePriorities_in;
    }

    virtual void getPose(double time, KDL::JntArrayAcc& pose);

protected:
    boost::shared_ptr<SynchedCartesianTrajectory> trajectory;
    boost::shared_ptr<KdlTreeIk> treeIk;
    KDL::JntArrayAcc jointsInit, jointsLast;
    double lastTime;
    std::vector<std::string> nodeNames;
    std::vector<KDL::FrameAcc> nodeFramesAcc;
    std::vector<KDL::Frame> nodeFrames;
    std::vector<KdlTreeIk::NodePriority> nodePriorities;

};

#endif