exact_cubic.h

/**
 * \file exact_cubic.h
 * \brief class allowing to create an Exact cubic spline.
 * \author Steve T.
 * \version 0.1
 * \date 06/17/2013
 *
 * This file contains definitions for the ExactCubic class.
 * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
 * cubic splines fulfulling those 4 restrictions :
 * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
 * - x_i(t_i+1) = x_i+1* ;
 * - its derivative is continous at t_i+1
 * - its acceleration is continous at t_i+1
 * more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
 * By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
 */

#ifndef _CLASS_EXACTCUBIC
#define _CLASS_EXACTCUBIC

#include "curve_abc.h"
#include "cubic_spline.h"
#include "quintic_spline.h"
#include "curve_constraint.h"
#include "piecewise_curve.h"

#include "MathDefs.h"

#include <functional>
#include <vector>

namespace curves {
/// \class ExactCubic.
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization.
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
          typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
          typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> {
  typedef Point point_t;
  typedef T_Point t_point_t;
  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
  typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
  typedef Time time_t;
  typedef Numeric num_t;
  typedef SplineBase spline_t;
  typedef typename std::vector<spline_t> t_spline_t;
  typedef typename t_spline_t::iterator it_spline_t;
  typedef typename t_spline_t::const_iterator cit_spline_t;
  typedef curve_constraints<Point> spline_constraints;

  typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
  typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;

  /* Constructors - destructors */
 public:
  /// \brief Empty constructor. Add at least one curve to call other class functions.
  ///
  exact_cubic() : piecewise_curve_t() {}

  /// \brief Constructor.
  /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
  /// \param wayPointsEns   : an iterator pointing to the last element of a waypoint container.
  ///
  template <typename In>
  exact_cubic(In wayPointsBegin, In wayPointsEnd)
      : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}

  /// \brief Constructor.
  /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
  /// \param wayPointsEns   : an iterator pointing to the last element of a waypoint container.
  /// \param constraints    : constraints on the init and end velocity / accelerations of the spline.
  ///
  template <typename In>
  exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
      : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}

  /// \brief Constructor.
  /// \param subSplines: vector of subSplines.
  exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t(subSplines) {}

  /// \brief Copy Constructor.
  exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}

  /// \brief Destructor.
  virtual ~exact_cubic() {}

  std::size_t getNumberSplines() { return this->getNumberCurves(); }

  spline_t getSplineAt(std::size_t index) { return this->curves_.at(index); }

 private:
  /// \brief Compute polynom of exact cubic spline from waypoints.
  /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
  /// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
  /// with \f$a=x\f$, \f$H_1b=H_2x\f$, \f$c=H_3x+H_4b\f$, \f$d=H_5x+H_6b\f$.<br>
  /// The matrices \f$H\f$ are defined as in the paper in Appendix A.
  ///
  template <typename In>
  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const {
    const std::size_t dim = wayPointsBegin->second.size();
    const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
    if (Safe && size < 1) {
      throw std::length_error("size of waypoints must be superior to 0");  // TODO
    }
    t_spline_t subSplines;
    subSplines.reserve(size);
    // refer to the paper to understand all this.
    MatrixX h1 = MatrixX::Zero(size, size);
    MatrixX h2 = MatrixX::Zero(size, size);
    MatrixX h3 = MatrixX::Zero(size, size);
    MatrixX h4 = MatrixX::Zero(size, size);
    MatrixX h5 = MatrixX::Zero(size, size);
    MatrixX h6 = MatrixX::Zero(size, size);
    MatrixX a = MatrixX::Zero(size, dim);
    MatrixX b = MatrixX::Zero(size, dim);
    MatrixX c = MatrixX::Zero(size, dim);
    MatrixX d = MatrixX::Zero(size, dim);
    MatrixX x = MatrixX::Zero(size, dim);
    In it(wayPointsBegin), next(wayPointsBegin);
    ++next;
    // Fill the matrices H as specified in the paper.
    for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
      num_t const dTi((*next).first - (*it).first);
      num_t const dTi_sqr(dTi * dTi);
      num_t const dTi_cube(dTi_sqr * dTi);
      // filling matrices values
      h3(i, i) = -3 / dTi_sqr;
      h3(i, i + 1) = 3 / dTi_sqr;
      h4(i, i) = -2 / dTi;
      h4(i, i + 1) = -1 / dTi;
      h5(i, i) = 2 / dTi_cube;
      h5(i, i + 1) = -2 / dTi_cube;
      h6(i, i) = 1 / dTi_sqr;
      h6(i, i + 1) = 1 / dTi_sqr;
      if (i + 2 < size) {
        In it2(next);
        ++it2;
        num_t const dTi_1((*it2).first - (*next).first);
        num_t const dTi_1sqr(dTi_1 * dTi_1);
        // this can be optimized but let's focus on clarity as long as not needed
        h1(i + 1, i) = 2 / dTi;
        h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
        h1(i + 1, i + 2) = 2 / dTi_1;
        h2(i + 1, i) = -6 / dTi_sqr;
        h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
        h2(i + 1, i + 2) = 6 / dTi_1sqr;
      }
      x.row(i) = (*it).second.transpose();
    }
    // adding last x
    x.row(size - 1) = (*it).second.transpose();
    // Compute coefficients of polynom.
    a = x;
    PseudoInverse(h1);
    b = h1 * h2 * x;  // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
    c = h3 * x + h4 * b;
    d = h5 * x + h6 * b;
    // create splines along waypoints.
    it = wayPointsBegin, next = wayPointsBegin;
    ++next;
    for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
      subSplines.push_back(create_cubic<Time, Numeric, Safe, Point, T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),
                                                                             (*it).first, (*next).first));
    }
    return subSplines;
  }

  template <typename In>
  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const {
    std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
    if (Safe && size < 1) {
      throw std::length_error("number of waypoints should be superior to one");  // TODO
    }
    t_spline_t subSplines;
    subSplines.reserve(size - 1);
    spline_constraints cons = constraints;
    In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
    ++next;
    for (std::size_t i(0); next != end; ++next, ++it, ++i) {
      compute_one_spline<In>(it, next, cons, subSplines);
    }
    compute_end_spline<In>(it, next, cons, subSplines);
    return subSplines;
  }

  template <typename In>
  void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
                          t_spline_t& subSplines) const {
    const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
    const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
    const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
    const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
    const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
    subSplines.push_back(create_cubic<Time, Numeric, Safe, Point, T_Point>(a0, b0, c0, d0, init_t, end_t));
    constraints.init_vel = subSplines.back().derivate(end_t, 1);
    constraints.init_acc = subSplines.back().derivate(end_t, 2);
  }

  template <typename In>
  void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
                          t_spline_t& subSplines) const {
    const std::size_t dim = wayPointsBegin->second.size();
    const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
    const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc / 2.,
                  c1 = constraints.end_acc;
    const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
    const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
    // solving a system of four linear eq with 4 unknows: d0, e0
    const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
    const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
    const point_t alpha_2 = c1 - 2 * c0;
    const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
    const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
    const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
    point_t d, e, f;
    MatrixX rhs = MatrixX::Zero(3, dim);
    rhs.row(0) = alpha_0;
    rhs.row(1) = alpha_1;
    rhs.row(2) = alpha_2;
    Matrix3 eq = Matrix3::Zero(3, 3);
    eq(0, 0) = x_d_0;
    eq(0, 1) = x_e_0;
    eq(0, 2) = x_f_0;
    eq(1, 0) = x_d_1;
    eq(1, 1) = x_e_1;
    eq(1, 2) = x_f_1;
    eq(2, 0) = x_d_2;
    eq(2, 1) = x_e_2;
    eq(2, 2) = x_f_2;
    rhs = eq.inverse().eval() * rhs;
    d = rhs.row(0);
    e = rhs.row(1);
    f = rhs.row(2);
    subSplines.push_back(create_quintic<Time, Numeric, Safe, Point, T_Point>(a0, b0, c0, d, e, f, init_t, end_t));
  }

 public:
  // Serialization of the class
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    if (version) {
      // Do something depending on version ?
    }
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(piecewise_curve_t);
  }
};
}  // namespace curves
#endif  //_CLASS_EXACTCUBIC