polynomials.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018-2019, CNRS-UM LIRMM
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <Eigen/Dense>

namespace utils
{

template<typename T>
struct PolynomialBase
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  virtual T pos(double t) const = 0;

  virtual T vel(double t) const = 0;

  virtual T accel(double t) const = 0;

  T tangent(double t) const
  {
    T v = this->vel(t);
    return v / v.norm();
  }

  double arcLength(double t_start, double t_end) const
  {
    struct GaussLengendreCoefficient
    {
      double t; 
      double w; 
    };
    static constexpr GaussLengendreCoefficient coeffs[] = {{0.0, 128. / 225.},
                                                           {-0.53846931010568311, 0.47862867049936647},
                                                           {+0.53846931010568311, 0.47862867049936647},
                                                           {-0.90617984593866396, 0.23692688505618908},
                                                           {+0.90617984593866396, 0.23692688505618908}};
    double a = (t_end - t_start) / 2.;
    double b = (t_end + t_start) / 2.;
    double length = 0.;
    for(auto coeff : coeffs)
    {
      length += coeff.w * this->vel(a * coeff.t + b).norm();
    }
    return a * length;
  }

  double arcLengthInverse(double t_start, double length, double t_guess = -1.) const
  {
    constexpr double PRECISION = 1e-3;
    if(t_guess < 0.)
    {
      t_guess = t_start + 1.;
    }
    double l_guess = arcLength(t_start, t_guess);
    if(std::abs(l_guess - length) < PRECISION)
    {
      return t_guess;
    }
    else if(l_guess < length)
    {
      return arcLengthInverse(t_guess, length - l_guess, 2. * t_guess - t_start);
    }
    else // (l_guess > length)
    {
      return arcLengthInverse(t_start, length, (t_guess + t_start) / 2.);
    }
  }
};

template<typename T>
struct CubicPolynomialBase : PolynomialBase<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CubicPolynomialBase(const T & C0, const T & C1, const T & C2, const T & C3) : C0_(C0), C1_(C1), C2_(C2), C3_(C3) {}

  T pos(double t) const
  {
    return C0_ + t * (C1_ + t * (C2_ + t * C3_));
  }

  T vel(double t) const
  {
    return C1_ + t * (2 * C2_ + 3 * t * C3_);
  }

  T accel(double t) const
  {
    return 2 * C2_ + 6 * t * C3_;
  }

protected:
  T C0_;
  T C1_;
  T C2_;
  T C3_;
};

template<typename T>
struct CubicPolynomial : CubicPolynomialBase<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  T zero()
  {
    return T::Zero();
  }

  CubicPolynomial() : CubicPolynomialBase<T>(T::Zero(), T::Zero(), T::Zero(), T::Zero()) {}
};

template<>
struct CubicPolynomial<double> : CubicPolynomialBase<double>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  double zero()
  {
    return 0.;
  }

  CubicPolynomial() : CubicPolynomialBase<double>(0., 0., 0., 0.) {}
};

template<typename T>
struct CubicHermitePolynomial : public CubicPolynomial<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CubicHermitePolynomial()
  {
    CubicHermitePolynomial(this->zero(), this->zero(), this->zero(), this->zero());
  }

  CubicHermitePolynomial(const T & initPos, const T & initVel, const T & targetPos, const T & targetVel)
  {
    reset(initPos, initVel, targetPos, targetVel);
  }

  void reset(const T & initPos, const T & initVel, const T & targetPos, const T & targetVel)
  {
    initPos_ = initPos;
    initVel_ = initVel;
    targetPos_ = targetPos;
    targetVel_ = targetVel;
    reset();
  }

  void reset(const T & initPos, const T & targetPos)
  {
    initPos_ = initPos;
    initVel_ = this->zero();
    targetPos_ = targetPos;
    targetVel_ = this->zero();
    reset();
  }

  virtual void reset()
  {
    this->C0_ = initPos_;
    this->C1_ = initVel_;
    this->C2_ = 3. * (targetPos_ - initPos_) - 2. * initVel_ - targetVel_;
    this->C3_ = -2. * (targetPos_ - initPos_) + initVel_ + targetVel_;
  }

protected:
  T initPos_;
  T initVel_;
  T targetPos_;
  T targetVel_;
};

template<typename T>
struct HoubaPolynomial : CubicHermitePolynomial<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  using CubicHermitePolynomial<T>::reset;

  void reset() override
  {
    T Delta = this->targetPos_ - this->initPos_;
    double Delta_v0 = Delta.dot(this->initVel_);
    double Delta_v1 = Delta.dot(this->targetVel_);
    double v0_v0 = this->initVel_.dot(this->initVel_);
    double v0_v1 = this->initVel_.dot(this->targetVel_);
    double v1_v1 = this->targetVel_.dot(this->targetVel_);
    double houbaInitVelScaling =
        6. * (3. * Delta_v0 * v1_v1 - 2. * Delta_v1 * v0_v1) / (9. * v0_v0 * v1_v1 - 4. * v0_v1 * v0_v1);
    if(houbaInitVelScaling < 0.)
    {
      houbaInitVelScaling *= -1.;
    }
    double houbaTargetVelScaling =
        6. * (-2. * Delta_v0 * v0_v1 + 3. * Delta_v1 * v0_v0) / (9. * v0_v0 * v1_v1 - 4. * v0_v1 * v0_v1);
    if(houbaTargetVelScaling < 0.)
    {
      houbaTargetVelScaling *= -1.;
    }
    this->initVel_ *= houbaInitVelScaling;
    this->initVel_ *= extraInitVelScaling_;
    this->targetVel_ *= houbaTargetVelScaling;
    this->targetVel_ *= extraTargetVelScaling_;
    CubicHermitePolynomial<T>::reset();
  }

  double extraInitVelScaling()
  {
    return extraInitVelScaling_;
  }

  void extraInitVelScaling(double scaling)
  {
    extraInitVelScaling_ = scaling;
  }

  double extraTargetVelScaling()
  {
    return extraTargetVelScaling_;
  }

  void extraTargetVelScaling(double scaling)
  {
    extraTargetVelScaling_ = scaling;
  }

protected:
  double extraInitVelScaling_ = 1.0;
  double extraTargetVelScaling_ = 1.0;
};

template<typename T>
struct QuinticPolynomialBase : PolynomialBase<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  QuinticPolynomialBase(const T & C0, const T & C1, const T & C2, const T & C3, const T & C4, const T & C5)
  : C0_(C0), C1_(C1), C2_(C2), C3_(C3), C4_(C4), C5_(C5)
  {
  }

  T pos(double t) const
  {
    return C0_ + t * (C1_ + t * (C2_ + t * (C3_ + t * (C4_ + t * C5_))));
  }

  T vel(double t) const
  {
    return C1_ + t * (2 * C2_ + t * (3 * C3_ + t * (4 * C4_ + t * 5 * C5_)));
  }

  T accel(double t) const
  {
    return 2 * C2_ + t * (6 * C3_ + t * (12 * C4_ + t * 20 * C5_));
  }

protected:
  T C0_;
  T C1_;
  T C2_;
  T C3_;
  T C4_;
  T C5_;
};

template<typename T>
struct QuinticPolynomial : QuinticPolynomialBase<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  T zero()
  {
    return T::Zero();
  }

  QuinticPolynomial() : QuinticPolynomialBase<T>(T::Zero(), T::Zero(), T::Zero(), T::Zero(), T::Zero(), T::Zero()) {}
};

template<>
struct QuinticPolynomial<double> : QuinticPolynomialBase<double>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  double zero()
  {
    return 0.;
  }

  QuinticPolynomial() : QuinticPolynomialBase<double>(0., 0., 0., 0., 0., 0.) {}
};

template<typename T>
struct QuinticHermitePolynomial : public QuinticPolynomial<T>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  QuinticHermitePolynomial()
  {
    QuinticHermitePolynomial(this->zero(), this->zero(), this->zero(), this->zero());
  }

  QuinticHermitePolynomial(const T & initPos, const T & initVel, const T & targetPos, const T & targetVel)
  {
    reset(initPos, initVel, targetPos, targetVel);
  }

  void reset(const T & initPos,
             const T & initVel,
             const T & initAccel,
             const T & targetPos,
             const T & targetVel,
             const T & targetAccel)
  {
    initPos_ = initPos;
    initVel_ = initVel;
    initAccel_ = initAccel;
    targetPos_ = targetPos;
    targetVel_ = targetVel;
    targetAccel_ = targetAccel;
    reset();
  }

  void reset(const T & initPos, const T & initVel, const T & targetPos, const T & targetVel)
  {
    initPos_ = initPos;
    initVel_ = initVel;
    initAccel_ = this->zero();
    targetPos_ = targetPos;
    targetVel_ = targetVel;
    targetAccel_ = this->zero();
    reset();
  }

  void reset(const T & initPos, const T & targetPos)
  {
    initPos_ = initPos;
    initVel_ = this->zero();
    initAccel_ = this->zero();
    targetPos_ = targetPos;
    targetVel_ = this->zero();
    targetAccel_ = this->zero();
    reset();
  }

  void reset()
  {
    auto Delta = targetPos_ - initPos_;
    auto Sigma = initVel_ + targetVel_;
    auto Gamma = 0.5 * (targetAccel_ - initAccel_);
    auto kappa = initVel_ + 0.5 * initAccel_;
    this->C0_ = initPos_;
    this->C1_ = initVel_;
    this->C2_ = 0.5 * initAccel_;
    this->C3_ = 10 * Delta - 4 * Sigma + Gamma - 2 * kappa;
    this->C4_ = -15 * Delta + 7 * Sigma - 2 * Gamma + kappa;
    this->C5_ = 6 * Delta - 3 * Sigma + Gamma;
  }

protected:
  T initPos_;
  T initVel_;
  T initAccel_;
  T targetPos_;
  T targetVel_;
  T targetAccel_;
};

template<template<class> class Polynomial, typename T>
struct RetimedPolynomial
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  RetimedPolynomial() : duration_(0.) {}

  RetimedPolynomial(Polynomial<T> poly, double duration) : poly_(poly), duration_(duration) {}

  double duration() const
  {
    return duration_;
  }

  void reset(const T & initPos, const T & targetPos, double duration)
  {
    duration_ = duration;
    poly_.reset(initPos, targetPos);
  }

  void reset(const T & initPos, const T & initVel, const T & targetPos, const T & targetVel, double duration)
  {
    duration_ = duration;
    poly_.reset(initPos, duration * initVel, targetPos, duration * targetVel);
  }

  void reset(const T & initPos,
             const T & initVel,
             const T & initAccel,
             const T & targetPos,
             const T & targetVel,
             const T & targetAccel,
             double duration)
  {
    duration_ = duration;
    double duration2 = duration * duration;
    poly_.reset(initPos, duration * initVel, duration2 * initAccel, targetPos, duration * targetVel,
                duration2 * targetAccel);
  }

  double s(double t) const
  {
    return (t < 0.) ? 0. : (t > duration_) ? 1. : t / duration_;
  }

  double sd(double t) const
  {
    return (t < 0.) ? 0. : (t > duration_) ? 0. : 1. / duration_;
  }

  T pos(double t) const
  {
    return poly_.pos(s(t));
  }

  T vel(double t) const
  {
    return sd(t) * poly_.vel(s(t));
  }

  T accel(double t) const
  {
    return std::pow(sd(t), 2) * poly_.accel(s(t));
  }

protected:
  double duration_;
  Polynomial<T> poly_;
};

} // namespace utils

using utils::CubicHermitePolynomial;
using utils::CubicPolynomial;
using utils::HoubaPolynomial;
using utils::QuinticHermitePolynomial;
using utils::QuinticPolynomial;
using utils::RetimedPolynomial;