BangBangInterpolator.h

Go to the documentation of this file.

#pragma once
 
#include <TrajColl/ElementInterpolation.h>
#include <TrajColl/Func.h>
#include <TrajColl/Interpolator.h>
 
namespace TrajColl
{
template<class T, class U = T>
class BangBangInterpolator : public Interpolator<T, U>
{
public:
  BangBangInterpolator(const std::map<double, T> & points = {}, const std::vector<double> & accelDurationList = {})
  : Interpolator<T, U>(points), accelDurationList_(accelDurationList)
  {
    func_ = std::make_shared<PiecewiseFunc<double>>();
 
    if(accelDurationList_.size() > 0 && (this->points_.size() != accelDurationList_.size() + 1))
    {
      throw std::invalid_argument(
          "[BangBangInterpolator] The size of the accelDurationList must be one less than the size of the points: "
          + std::to_string(accelDurationList_.size()) + " != " + std::to_string(this->points_.size()) + " + 1");
    }
 
    if(this->points_.size() >= 2)
    {
      calcCoeff();
    }
  }
 
  BangBangInterpolator(const BangBangInterpolator & inst) : Interpolator<T, U>(inst)
  {
    accelDurationList_ = inst.accelDurationList_;
    func_ = std::make_shared<PiecewiseFunc<double>>(*inst.func_);
  }
 
  virtual std::shared_ptr<Interpolator<T, U>> clone() const override
  {
    return std::shared_ptr<Interpolator<T, U>>(new BangBangInterpolator(*this));
  }
 
  virtual void clearPoints() override
  {
    Interpolator<T, U>::clearPoints();
    accelDurationList_.clear();
  }
 
  virtual void appendPoint(const std::pair<double, T> & point) override
  {
    appendPoint(point, 0.0);
  }
 
  void appendPoint(const std::pair<double, T> & point, double accelDuration)
  {
    if(!this->points_.empty() && point.first <= this->points_.rbegin()->first)
    {
      throw std::invalid_argument("[BangBangInterpolator] Only adding a trailing point is allowed: "
                                  + std::to_string(point.first)
                                  + " <= " + std::to_string(this->points_.rbegin()->first));
    }
 
    this->points_.insert(point);
 
    // Set accelDurationList_
    if(this->points_.size() >= 2)
    {
      double interpDuration = this->points_.rbegin()->first - std::next(this->points_.rbegin())->first;
 
      if(accelDuration <= 0.0)
      {
        constexpr double accelDurationRatio = 0.2;
        accelDuration = accelDurationRatio * interpDuration;
      }
 
      if(accelDuration >= 0.5 * interpDuration)
      {
        std::cerr << "[BangBangInterpolator] Overwrite accelDuration because accelDuration must be less than half of "
                     "interpDuration: "
                  << std::to_string(accelDuration) << " >= " << std::to_string(0.5 * interpDuration) << std::endl;
        constexpr double eps = 1e-10;
        accelDuration = 0.5 * interpDuration - eps;
      }
 
      accelDurationList_.push_back(accelDuration);
    }
  }
 
  virtual void calcCoeff() override
  {
    if(this->points_.size() < 2)
    {
      throw std::out_of_range("[BangBangInterpolator] Number of points should be 2 or more: "
                              + std::to_string(this->points_.size()));
    }
 
    func_->clearFuncs();
 
    auto pointIt = this->points_.begin();
    for(int i = 0; i < static_cast<int>(this->points_.size()) - 1; i++, pointIt++)
    {
      double segStartTime = pointIt->first;
      double segEndTime = std::next(pointIt)->first;
      double segAccelDuration = accelDurationList_[i];
      double segVel = 1.0 / (segEndTime - segStartTime - segAccelDuration);
      double segAccel = segVel / segAccelDuration;
 
      // Set segFunc
      std::shared_ptr<PiecewiseFunc<double>> segFunc = std::make_shared<PiecewiseFunc<double>>();
      segFunc->setDomainLowerLimit(segStartTime);
      segFunc->appendFunc(segStartTime + segAccelDuration,
                          std::make_shared<QuadraticPolynomial<double>>(
                              std::array<double, 3>{static_cast<double>(i), 0.0, 0.5 * segAccel}, segStartTime));
      segFunc->appendFunc(
          segEndTime - segAccelDuration,
          std::make_shared<LinearPolynomial<double>>(std::array<double, 2>{static_cast<double>(i) + 0.5, segVel},
                                                     0.5 * (segStartTime + segEndTime)));
      segFunc->appendFunc(segEndTime,
                          std::make_shared<QuadraticPolynomial<double>>(
                              std::array<double, 3>{static_cast<double>(i + 1), 0.0, -0.5 * segAccel}, segEndTime));
 
      func_->appendFunc(segEndTime, segFunc);
    }
 
    func_->setDomainLowerLimit(this->points_.begin()->first);
  }
 
  virtual T operator()(double t) const override
  {
    size_t idx = func_->index(t);
    double ratio = (*func_)(t) - static_cast<double>(idx);
    return interpolate<T>(std::next(this->points_.begin(), idx)->second,
                          std::next(this->points_.begin(), idx + 1)->second, std::clamp(ratio, 0.0, 1.0));
  }
 
  virtual U derivative(double t, int order = 1) const override
  {
    size_t idx = func_->index(t);
    double ratio = (*func_)(t) - static_cast<double>(idx);
    return func_->derivative(t, order)
           * interpolateDerivative<T, U>(std::next(this->points_.begin(), idx)->second,
                                         std::next(this->points_.begin(), idx + 1)->second, std::clamp(ratio, 0.0, 1.0),
                                         1);
  }
 
protected:
  std::vector<double> accelDurationList_;
 
  std::shared_ptr<PiecewiseFunc<double>> func_;
};
} // namespace TrajColl