TrajGen.hpp

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
 
#ifndef __TRAJ_GEN_HPP__
#define __TRAJ_GEN_HPP__
 
#include <yarp/os/Log.h>
 
class Traj
{
public:
    virtual ~Traj() {}
    virtual bool configure(const double xi, const double xf, const double _T) = 0;
    virtual bool configure(const double xi, const double xf, const double xdoti, const double xdotf, const double _T) = 0;
    virtual double get(const double ti) const = 0;
    virtual double getdot(const double ti) const = 0;
    virtual double getdotdot(const double ti) const = 0;
    virtual bool maxVelBelow(const double thresVel) const = 0;
    virtual bool maxAccBelow(const double thresAcc) const = 0;
    virtual double getT() const = 0;
    virtual void dump(double samples) const = 0;
};
 
class OrderOneTraj : public Traj
{
public:
    OrderOneTraj() : T(0.0), m(0.0), b(0.0) {}
 
    bool configure(const double xi, const double xf, const double _T)
    {
        T = _T;
        b = xi;
        m = (xf - xi) / T;
        return true;
    }
 
    bool configure(const double xi, const double xf, const double xdoti, const double xdotf, const double _T)
    {
        return false;
    }
 
    double get(const double ti) const
    {
        return b + (m * ti);
    }
 
    double getdot(const double ti) const
    {
        return m;
    }
 
    double getdotdot(const double ti) const
    {
        return 0;
    }
 
    bool maxVelBelow(const double thresVel) const
    {
        return m < thresVel;
    }
 
    bool maxAccBelow(const double thresAcc) const
    {
        return 0 < thresAcc;
    }
 
    double getT() const
    {
        return T;
    }
 
    void dump(double samples) const
    {
        for (double i = 0; i < T; i += (T / samples))
        {
            yInfo("%05.2f %+02.6f %+02.6f %+02.6f", i, get(i), getdot(i), getdotdot(i));
        }
    }
 
private:
    double T, m, b;
};
 
class OrderThreeTraj : public Traj
{
public:
    OrderThreeTraj() : a3(0.0), a2(0.0), a1(0.0), a0(0.0), T(0.0) {}
 
    bool configure(const double xi, const double xf, const double _T)
    {
        T = _T;
        a0 = xi;
        a1 = 0;
        a3 = 2 * (xi - xf) / (T * T * T);
        a2 = (xf - xi) / (T * T) - a3 * T;
        return true;
    }
 
    bool configure(const double xi, const double xf, const double xdoti, const double xdotf, const double _T)
    {
        T = _T;
        a0 = xi;
        a1 = xdoti;
        a3 = 2.0 * (xi - xf) / (T * T * T) + (xdotf + xdoti) / (T * T);
        a2 = (xf - xi - xdoti * T) / (T * T) - a3 * T;
        return true;
    }
 
    double get(const double ti) const
    {
        if (ti > T)
        {
            return a3 * T * T * T + a2 * T * T + a1 * T + a0;  // Security hack
        }
        return a3 * ti * ti * ti + a2 * ti * ti + a1 * ti + a0;
    }
 
    double getdot(const double ti) const
    {
        if (ti > T)
        {
            return 3 * a3 * T * T + 2 * a2 * T + a1;  // Security hack
        }
        return 3 * a3 * ti * ti + 2 * a2 * ti + a1;
    }
 
    double getdotdot(const double ti) const
    {
        if (ti > T)
        {
            return 6 * a3 * T + 2 * a2;  // Security hack
        }
        return 6 * a3 * ti + 2 * a2;
    }
 
    bool maxVelBelow(const double thresVel) const
    {
        return getdot(T / 2) < thresVel;
    }
 
    bool maxAccBelow(const double thresAcc) const
    {
        return getdotdot(T / 2) < thresAcc;
    }
 
    double getT() const
    {
        return T;
    }
 
    void dump(double samples) const
    {
        for (double i = 0; i < T; i += (T / samples))
        {
            yInfo("%05.2f %+02.6f %+02.6f %+02.6f", i, get(i), getdot(i), getdotdot(i));
        }
    }
 
private:
    double a3, a2, a1, a0, T;
};
 
#endif  // __TRAJ_GEN_HPP__