exact_cubic.h 11.2 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
#include "curve_constraint.h"
27

28
29
#include "piecewise_curve.h"

30
31
#include "MathDefs.h"

32
#include <functional>
33
#include <vector>
34

35
36
37
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"

38
namespace curves
39
{
40
41
42
43
44
45
46
47
  /// \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, std::size_t Dim=3, 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, Dim, Safe, Point, T_Point> >
48
  struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
49
  {
JasonChmn's avatar
JasonChmn committed
50
    typedef Point   point_t;
51
    typedef T_Point t_point_t;
Steve Tonneau's avatar
Steve Tonneau committed
52
    typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
53
    typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
JasonChmn's avatar
JasonChmn committed
54
55
    typedef Time    time_t;
    typedef Numeric num_t;
56
    typedef SplineBase spline_t;
57
58
59
    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;
60
    typedef curve_constraints<Point, Dim> spline_constraints;
61

62
    typedef exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> exact_cubic_t;
63
64
    typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> piecewise_curve_t;

JasonChmn's avatar
JasonChmn committed
65
66
    /* Constructors - destructors */
    public:
67
68
      /// \brief Empty constructor. Add at least one curve to call other class functions.
      ///
69
70
71
      exact_cubic()
        : piecewise_curve_t()
      {}
72

73
74
75
76
77
78
      /// \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)
79
        : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
80
81
82
83
84
85
86
87
88
      {}

      /// \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)
89
        : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints))
90
      {}
91

92
93
94
      /// \brief Constructor.
      /// \param subSplines: vector of subSplines.
      exact_cubic(const t_spline_t& subSplines)
95
        : piecewise_curve_t(subSplines)
96
      {}
97

98
99
      /// \brief Copy Constructor.
      exact_cubic(const exact_cubic& other)
100
        : piecewise_curve_t(other)
101
      {}
102

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

106
107
      std::size_t getNumberSplines()
      {
108
        return this->getNumberCurves();
109
      }
110

111
112
      spline_t getSplineAt(std::size_t index)
      {
113
        return this->curves_.at(index);
114
      }
115

116
    private:
117
118
119
120
121
122
123
124
125
      /// \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
      {
126
127
        std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
        if(Safe && size < 1)
Steve Tonneau's avatar
Steve Tonneau committed
128
        {
129
          throw std::length_error("size of waypoints must be superior to 0") ; // TODO
Steve Tonneau's avatar
Steve Tonneau committed
130
        }
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
        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;
JasonChmn's avatar
JasonChmn committed
146
        // Fill the matrices H as specified in the paper.
147
148
        for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
        {
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
          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();
175
        }
176
177
        // adding last x
        x.row(size-1)= (*it).second.transpose();
JasonChmn's avatar
JasonChmn committed
178
        // Compute coefficients of polynom.
179
        a = x;
180
181
182
183
        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;
JasonChmn's avatar
JasonChmn committed
184
        // create splines along waypoints.
185
186
187
        it= wayPointsBegin, next=wayPointsBegin; ++ next;
        for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
        {
188
189
190
191
          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));
192
193
        }
        return subSplines;
194
      }
195

196
197
198
      template<typename In>
      t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
      {
199
200
201
        std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
        if(Safe && size < 1) 
        {
202
          throw std::length_error("number of waypoints should be superior to one"); // TODO
203
204
205
206
207
208
209
210
        }
        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)
        {
211
          compute_one_spline<In>(it, next, cons, subSplines);
212
213
214
        }
        compute_end_spline<In>(it, next,cons, subSplines);
        return subSplines;
215
      }
216

217
218
219
      template<typename In>
      void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
      {
220
221
222
223
224
        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;
225
        subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a0,b0,c0,d0,init_t, end_t));
226
227
        constraints.init_vel = subSplines.back().derivate(end_t, 1);
        constraints.init_acc = subSplines.back().derivate(end_t, 2);
228
      }
229

230
231
232
      template<typename In>
      void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
      {
233
234
        const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
        const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
235
        c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
        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);
254
255
        subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>(a0,b0,c0,d,e,f, init_t, end_t));
      }
256
257

    public:
258
259
260
261
      // Serialization of the class
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
262
        if (version) {
263
          // Do something depending on version ?
264
        }
265
        ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(piecewise_curve_t);
266
267
      }
  };
268
} // namespace curves
269
270
#endif //_CLASS_EXACTCUBIC