exact_cubic.h 7.49 KB
Newer Older
1
/**
2
* \file exact_cubic.h
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
* \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

stonneau's avatar
stonneau committed
23
#include "curve_abc.h"
Steve Tonneau's avatar
Steve Tonneau committed
24
#include "cubic_spline.h"
25
#include "quintic_spline.h"
26

27 28
#include "MathDefs.h"

29
#include <functional>
30
#include <vector>
31 32 33

namespace spline
{
34 35 36 37
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function 
/// crossing each of the waypoint given in its initialization
///
38
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
39
, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >
40
, typename SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
41 42 43
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
	typedef Point 	point_t;
44
    typedef T_Point t_point_t;
Steve Tonneau's avatar
Steve Tonneau committed
45
    typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
46 47
	typedef Time 	time_t;
	typedef Numeric	num_t;
48
    typedef SplineBase spline_t;
49 50 51
    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;
52
    typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
53 54

	/* Constructors - destructors */
55
	public:
56 57 58 59
	///\brief Constructor
	///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
	///\param wayPointsEns   : an iterator pointing to the end           of a waypoint container
	template<typename In>
stonneau's avatar
stonneau committed
60
	exact_cubic(In wayPointsBegin, In wayPointsEnd)
Steve Tonneau's avatar
Steve Tonneau committed
61
        : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
62

63 64 65 66 67 68

    ///\brief Constructor
    ///\param subSplines: vector of subsplines
    exact_cubic(const t_spline_t& subSplines)
        : curve_abc_t(), subSplines_(subSplines) {}

69 70 71 72
    ///\brief Copy Constructor
    exact_cubic(const exact_cubic& other)
        : curve_abc_t(), subSplines_(other.subSplines_) {}

73
	///\brief Destructor
stevet's avatar
stevet committed
74
    virtual ~exact_cubic(){}
75

76
    private:
77 78
    template<typename In>
    t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
Steve Tonneau's avatar
Steve Tonneau committed
79
    {
80 81
        std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
        if(Safe && size < 1)
Steve Tonneau's avatar
Steve Tonneau committed
82
        {
83
            throw; // TODO
Steve Tonneau's avatar
Steve Tonneau committed
84
        }
85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100
        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);

101

102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
        In it(wayPointsBegin), next(wayPointsBegin);
        ++next;

        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();
        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;
        it= wayPointsBegin, next=wayPointsBegin; ++ next;
        for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
        {
144 145
            subSplines.push_back(
                create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
146
        }
147 148
        subSplines.push_back(
                create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
149 150 151
        return subSplines;
    }

152
    private:
153
    //exact_cubic& operator=(const exact_cubic&);
154
	/* Constructors - destructors */
155

156
	/*Operations*/
157 158
	public:
	///  \brief Evaluation of the cubic spline at time t.
159
    ///  \param t : the time when to evaluate the spline
160
	///  \param return : the value x(t)
161
    virtual point_t operator()(const time_t t) const
162
    {
163
        if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
164
        for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
stevet's avatar
stevet committed
165 166
        {
            if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
167
                return it->operator()(t);
168
        }
stevet's avatar
stevet committed
169 170
        // this should not happen
        throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
171 172 173
    }

    ///  \brief Evaluation of the derivative spline at time t.
174
    ///  \param t : the time when to evaluate the spline
175
    ///  \param order : order of the derivative
176
    ///  \param return : the value x(t)
177
    virtual point_t derivate(const time_t t, const std::size_t order) const
178 179 180 181
    {
        if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
        for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
        {
stevet's avatar
stevet committed
182
            if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
183 184
                return it->derivate(t, order);
        }
stevet's avatar
stevet committed
185 186
        // this should not happen
        throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
187
    }
188
	/*Operations*/
189

190
	/*Helpers*/
191
	public:
192 193
    num_t virtual min() const{return subSplines_.front().min();}
    num_t virtual max() const{return subSplines_.back().max();}
194
	/*Helpers*/
195

196
	/*Attributes*/
197
    public:
198
    t_spline_t subSplines_; // const
199 200
	/*Attributes*/
};
201
}
202 203
#endif //_CLASS_EXACTCUBIC