exact_cubic.h 5.27 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
24
#include "curve_abc.h"
#include "cubic_function.h"
25

26
27
#include "MathDefs.h"

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

namespace spline
{
33
34
35
36
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function 
/// crossing each of the waypoint given in its initialization
///
37
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
38
39
40
41
42
43
44
45
46
47
48
49
50
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
	typedef Point 	point_t;
	typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
	typedef Time 	time_t;
	typedef Numeric	num_t;
	typedef cubic_function<time_t, Numeric, Dim, Safe, Point> cubic_function_t;
	typedef typename std::vector<cubic_function_t*> T_cubic;
	typedef typename T_cubic::iterator IT_cubic;
	typedef typename T_cubic::const_iterator CIT_cubic;

	/* Constructors - destructors */
51
	public:
52
53
54
55
	///\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
56
	exact_cubic(In wayPointsBegin, In wayPointsEnd)
57
58
59
60
61
62
63
64
65
66
67
68
69
70
	{
		std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
		if(Safe && size < 1)
		{
			throw; // TODO
		}

		// 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);
71

72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
		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;
		Numeric t_previous((*it).first);

		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;
100
				num_t const dTi_1((*it2).first - (*next).first);
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
				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)
		{
			subSplines_.push_back(new cubic_function_t(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
		}
		subSplines_.push_back(new cubic_function_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
	}

	///\brief Destructor
stonneau's avatar
stonneau committed
128
	~exact_cubic()
129
130
131
132
133
134
	{
		for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
		{
			delete(*it);
		}
	}
135
136

	private:
137
138
139
	exact_cubic(const exact_cubic&);
	exact_cubic& operator=(const exact_cubic&);
	/* Constructors - destructors */
140

141
	/*Operations*/
142
143
144
	public:
	///  \brief Evaluation of the cubic spline at time t.
	///  \param t : the time when to evaluate the spine
145
	///  \param return : the value x(t)
stonneau's avatar
stonneau committed
146
	virtual point_t operator()(time_t t) const
147
148
	{

149
    	if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
150
151
152
153
154
155
156
157
158
		for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
		{
			if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
			{
				return (*it)->operator()(t);
			}
		}
	}
	/*Operations*/
159

160
	/*Helpers*/
161
	public:
stonneau's avatar
stonneau committed
162
163
	num_t virtual min() const{return subSplines_.front()->t_min_;}
	num_t virtual max() const{return subSplines_.back()->t_max_;}
164
	/*Helpers*/
165

166
	/*Attributes*/
167
	private:
168
169
170
	T_cubic subSplines_;
	/*Attributes*/
};
171
}
172
173
#endif //_CLASS_EXACTCUBIC