TrapezoidalVelocityTrajectory.h

Go to the documentation of this file.

#ifndef TRAP_VEL_TRAJECTORY_H
#define TRAP_VEL_TRAJECTORY_H

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

class TrapezoidalVelocityUtility
{
public:
    TrapezoidalVelocityUtility();
    ~TrapezoidalVelocityUtility();

    void setConstraints(double posInit_in, double posFinal_in, double velInit_in = 0., double velFinal_in = 0., double accelInit_in = 0., double accelFinal_in = 0.);

    double duration(double vmax_in, double amax_in, bool spline = true);

    void setDuration(double duration_in);

    void setDurationHelper(double duration_in);

    void getTimes(double& t1_out, double& t2_out, double& t3_out) const;

    bool timesValid();

    void calculateTimeHelper();

    bool calculateMaxVel();

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

    void setTimes(double t1_in, double t2_in, double t3_in);

private:
    double posInit, posFinal, velInit, velFinal, accelInit, accelFinal;
    double t1, t2, t3;
    double v1, a1, a2;
    double velMax, accelMax, timeMin;
};

class TrapezoidalVelocityJointTrajectory : public JointTrajectory, protected JointMotionLimiter
{
public:
    TrapezoidalVelocityJointTrajectory(const JointMotionLimiter& jml, const KDL::JntArrayAcc& startPose,
                                       const KDL::JntArrayAcc& goalPose, double duration_in);
    virtual ~TrapezoidalVelocityJointTrajectory();

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

    virtual void setDuration(double duration_in);

private:
    std::vector<TrapezoidalVelocityUtility> utils;

    void setTimes(double t1_in, double t2_in, double t3_in);
};

class TrapezoidalVelocityCartesianTrajectory : public CartesianTrajectory, protected CartesianMotionLimiter
{
public:
    TrapezoidalVelocityCartesianTrajectory(const CartesianMotionLimiter& cml, const KDL::FrameAcc& startPose,
                                           const KDL::FrameAcc& goalPose, double duration_in);
    virtual ~TrapezoidalVelocityCartesianTrajectory();

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

    virtual void setDuration(double duration_in);

private:
    std::vector<TrapezoidalVelocityUtility> utils;
    tf::Quaternion startQ;
    tf::Quaternion goalQ;

    void setTimes(double t1_in, double t2_in, double t3_in);
};

#endif