MinJerkTrajectory.h

Go to the documentation of this file.

#ifndef MIN_JERK_TRAJECTORY_H
#define MIN_JERK_TRAJECTORY_H

#include <robodyn_controllers/Trajectory.h>
#include <robodyn_controllers/MotionLimiter.h>
#include <tf/LinearMath/Quaternion.h>

class MinJerkUtility
{
public:
    class TimeUtility
    {
    public:
        friend class MinJerkUtility;
        void setTime(double t_in);

    private:
        double t, t2, t3, t4, t5;
    };

    void setConstraints(double pinit, double pfinal, double vinit = 0., double vfinal = 0., double ainit = 0., double afinal = 0.);

    double duration(double vmax) const;

    double p(const TimeUtility& t) const;
    double v(const TimeUtility& t) const;
    double a(const TimeUtility& t) const;

private:
    double dist;
    double a0, a1, a2, a3, a4, a5;
};

class MinJerkJointTrajectory : public JointTrajectory, protected JointVelocityLimiter
{
public:
    MinJerkJointTrajectory(const JointVelocityLimiter& jvl, const KDL::JntArrayAcc& startPose,
                           const KDL::JntArrayAcc& goalPose, double duration_in);
    virtual ~MinJerkJointTrajectory();

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

private:
    std::vector<MinJerkUtility> utils;
};

class MinJerkCartesianTrajectory : public CartesianTrajectory, protected CartesianVelocityLimiter
{
public:
    MinJerkCartesianTrajectory(const CartesianVelocityLimiter& cvl, const KDL::FrameAcc& startPose,
                               const KDL::FrameAcc& goalPose, double duration_in);
    virtual ~MinJerkCartesianTrajectory();

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

private:
    std::vector<MinJerkUtility> utils;
    KDL::Vector moveVec;
    KDL::Vector startVec;
    tf::Quaternion startQ;
    tf::Quaternion goalQ;
};

#endif