TrapezoidalVelocityProfile.h

#pragma once
 
#include "../navtypes.h"
#include "../utils/time.h"
#include "../world_interface/data.h"
 
#include <array>
#include <cmath>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
 
namespace control {

template <int dim>
class TrapezoidalVelocityProfile {
public:
    TrapezoidalVelocityProfile(double maxVel, double maxAccel)
        : maxVel(maxVel), maxAccel(maxAccel) {}

    void reset() {
        profile.reset();
    }

    bool hasTarget() const {
        return profile.has_value();
    }

    void setTarget(robot::types::datatime_t currTime, const std::array<double, dim>& startPos,
                   const std::array<double, dim>& endPos) {
        navtypes::Vectord<dim> startVec, endVec;
        for (int i = 0; i < dim; i++) {
            startVec(i) = startPos[i];
            endVec(i) = endPos[i];
        }
        setTarget(currTime, startVec, endVec);
    }

    void setTarget(robot::types::datatime_t currTime, const navtypes::Vectord<dim>& startPos,
                   const navtypes::Vectord<dim>& endPos) {
        profile_t profile{.startTime = currTime, .startPos = startPos, .endPos = endPos};
        // min time required to accelerate to top speed
        double rampUpTime = maxVel / maxAccel;
        // min distance required to accelerate to top speed
        double rampUpDist = std::pow(maxVel, 2) / (2 * maxAccel);
 
        double dist = (endPos - startPos).norm();
        if (2 * rampUpDist < dist) {
            // the profile distance is too short to allow us to get to top speed
            // so the profile looks like a triangle instead of a trapezoid
            profile.stopAccelTime = rampUpTime;
            double coastTime = (dist - 2 * rampUpDist) / maxVel;
            profile.startDecelTime = rampUpTime + coastTime;
            profile.totalTime = coastTime + 2 * rampUpTime;
        } else {
            // this is the normal case, where we accelerate to top speed, coast for a bit, then
            // decelerate.
            double totalTime = 2 * std::sqrt(dist / maxAccel);
            profile.stopAccelTime = profile.startDecelTime = totalTime / 2;
            profile.totalTime = totalTime;
        }
        this->profile = profile;
    }

    navtypes::Vectord<dim> getCommand(robot::types::datatime_t currTime) const {
        if (!profile) {
            return navtypes::Vectord<dim>::Zero();
        }
 
        profile_t profile = this->profile.value();
        navtypes::Vectord<dim> dir = (profile.endPos - profile.startPos).normalized();
        double elapsedTime = util::durationToSec(currTime - profile.startTime);
        if (elapsedTime < 0) {
            return profile.startPos;
        } else if (elapsedTime < profile.stopAccelTime) {
            double dist = 0.5 * maxAccel * std::pow(elapsedTime, 2);
            return profile.startPos + dir * dist;
        } else if (elapsedTime < profile.startDecelTime) {
            // area of trapezoid is average of bases times height
            double dist = (2 * elapsedTime - profile.stopAccelTime) / 2.0 * maxVel;
            return profile.startPos + dir * dist;
        } else if (elapsedTime < profile.totalTime) {
            double distFromEnd = 0.5 * maxAccel * std::pow(profile.totalTime - elapsedTime, 2);
            return profile.endPos - dir * distFromEnd;
        } else {
            return profile.endPos;
        }
    }

    std::optional<util::dseconds> getTotalTime() const {
        if (profile.has_value()) {
            return util::dseconds(profile->totalTime);
        } else {
            return {};
        }
    }
 
private:
    struct profile_t {
        robot::types::datatime_t startTime;
        double stopAccelTime;
        double startDecelTime;
        double totalTime;
        navtypes::Vectord<dim> startPos;
        navtypes::Vectord<dim> endPos;
    };
    double maxVel, maxAccel;
    std::optional<profile_t> profile;
};

class SingleDimTrapezoidalVelocityProfile {
public:
    SingleDimTrapezoidalVelocityProfile(double maxVel, double maxAccel);

    void setTarget(robot::types::datatime_t currTime, double startPos, double endPos);

    bool hasTarget() const;

    double getCommand(robot::types::datatime_t currTime) const;

    void reset();

    std::optional<util::dseconds> getTotalTime() const;
 
private:
    TrapezoidalVelocityProfile<1> profile;
};
 
} // namespace control