hpp-spline  4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
exact_cubic.h
Go to the documentation of this file.
1 
19 #ifndef _CLASS_EXACTCUBIC
20 #define _CLASS_EXACTCUBIC
21 
22 #include "curve_abc.h"
23 #include "cubic_spline.h"
24 #include "quintic_spline.h"
25 
26 #include "MathDefs.h"
27 
28 #include <functional>
29 #include <vector>
30 
31 namespace spline {
36 template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
37  typename Point = Eigen::Matrix<Numeric, Dim, 1>,
38  typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
39  typename SplineBase = polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
40 struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point> {
41  typedef Point point_t;
42  typedef T_Point t_point_t;
43  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
44  typedef Time time_t;
45  typedef Numeric num_t;
46  typedef SplineBase spline_t;
47  typedef typename std::vector<spline_t> t_spline_t;
48  typedef typename t_spline_t::iterator it_spline_t;
49  typedef typename t_spline_t::const_iterator cit_spline_t;
51 
52  /* Constructors - destructors */
53  public:
57  template <typename In>
58  exact_cubic(In wayPointsBegin, In wayPointsEnd)
59  : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
60 
63  exact_cubic(const t_spline_t& subSplines) : curve_abc_t(), subSplines_(subSplines) {}
64 
67 
69  virtual ~exact_cubic() {}
70 
71  private:
72  template <typename In>
73  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const {
74  std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
75  if (Safe && size < 1) {
76  throw; // TODO
77  }
78  t_spline_t subSplines;
79  subSplines.reserve(size);
80 
81  // refer to the paper to understand all this.
82  MatrixX h1 = MatrixX::Zero(size, size);
83  MatrixX h2 = MatrixX::Zero(size, size);
84  MatrixX h3 = MatrixX::Zero(size, size);
85  MatrixX h4 = MatrixX::Zero(size, size);
86  MatrixX h5 = MatrixX::Zero(size, size);
87  MatrixX h6 = MatrixX::Zero(size, size);
88 
89  MatrixX a = MatrixX::Zero(size, Dim);
90  MatrixX b = MatrixX::Zero(size, Dim);
91  MatrixX c = MatrixX::Zero(size, Dim);
92  MatrixX d = MatrixX::Zero(size, Dim);
93  MatrixX x = MatrixX::Zero(size, Dim);
94 
95  In it(wayPointsBegin), next(wayPointsBegin);
96  ++next;
97 
98  for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
99  num_t const dTi((*next).first - (*it).first);
100  num_t const dTi_sqr(dTi * dTi);
101  num_t const dTi_cube(dTi_sqr * dTi);
102  // filling matrices values
103  h3(i, i) = -3 / dTi_sqr;
104  h3(i, i + 1) = 3 / dTi_sqr;
105  h4(i, i) = -2 / dTi;
106  h4(i, i + 1) = -1 / dTi;
107  h5(i, i) = 2 / dTi_cube;
108  h5(i, i + 1) = -2 / dTi_cube;
109  h6(i, i) = 1 / dTi_sqr;
110  h6(i, i + 1) = 1 / dTi_sqr;
111  if (i + 2 < size) {
112  In it2(next);
113  ++it2;
114  num_t const dTi_1((*it2).first - (*next).first);
115  num_t const dTi_1sqr(dTi_1 * dTi_1);
116  // this can be optimized but let's focus on clarity as long as not needed
117  h1(i + 1, i) = 2 / dTi;
118  h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
119  h1(i + 1, i + 2) = 2 / dTi_1;
120  h2(i + 1, i) = -6 / dTi_sqr;
121  h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
122  h2(i + 1, i + 2) = 6 / dTi_1sqr;
123  }
124  x.row(i) = (*it).second.transpose();
125  }
126  // adding last x
127  x.row(size - 1) = (*it).second.transpose();
128  a = x;
129  PseudoInverse(h1);
130  b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
131  c = h3 * x + h4 * b;
132  d = h5 * x + h6 * b;
133  it = wayPointsBegin, next = wayPointsBegin;
134  ++next;
135  for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
136  subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(
137  a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
138  }
139  subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(
140  a.row(size - 1), b.row(size - 1), c.row(size - 1), d.row(size - 1), (*it).first, (*it).first));
141  return subSplines;
142  }
143 
144  private:
145  // exact_cubic& operator=(const exact_cubic&);
146  /* Constructors - destructors */
147 
148  /*Operations*/
149  public:
153  virtual point_t operator()(const time_t t) const {
154  if (Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())) {
155  throw std::out_of_range("TODO");
156  }
157  for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
158  if ((t >= (it->min()) && t <= (it->max())) || it + 1 == subSplines_.end()) return it->operator()(t);
159  }
160  // this should not happen
161  throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
162  }
163 
168  virtual point_t derivate(const time_t t, const std::size_t order) const {
169  if (Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())) {
170  throw std::out_of_range("TODO");
171  }
172  for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
173  if ((t >= (it->min()) && t <= (it->max())) || it + 1 == subSplines_.end()) return it->derivate(t, order);
174  }
175  // this should not happen
176  throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
177  }
178  /*Operations*/
179 
180  /*Helpers*/
181  public:
182  num_t virtual min() const { return subSplines_.front().min(); }
183  num_t virtual max() const { return subSplines_.back().max(); }
184  /*Helpers*/
185 
186  /*Attributes*/
187  public:
189  /*Attributes*/
190 };
191 } // namespace spline
192 #endif //_CLASS_EXACTCUBIC
cubic_spline.h
Definition of a cubic spline.
spline::exact_cubic::curve_abc_t
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: exact_cubic.h:50
spline::exact_cubic::it_spline_t
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:48
spline::curve_abc< Numeric, Numeric, Dim, Safe, point_one_dim_t >::point_t
point_one_dim_t point_t
Definition: curve_abc.h:25
spline::exact_cubic::num_t
Numeric num_t
Definition: exact_cubic.h:45
spline::exact_cubic::cit_spline_t
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:49
spline::exact_cubic::max
virtual num_t max() const
Definition: exact_cubic.h:183
spline::exact_cubic::t_spline_t
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:47
quintic_spline.h
spline::exact_cubic::spline_t
SplineBase spline_t
Definition: exact_cubic.h:46
spline::exact_cubic::MatrixX
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:43
spline::exact_cubic::operator()
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: exact_cubic.h:153
spline::exact_cubic::subSplines_
t_spline_t subSplines_
Definition: exact_cubic.h:188
spline::exact_cubic::exact_cubic
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:66
spline::PseudoInverse
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
spline::exact_cubic::t_point_t
T_Point t_point_t
Definition: exact_cubic.h:42
spline::exact_cubic::min
virtual num_t min() const
Definition: exact_cubic.h:182
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
curve_abc.h
interface for a Curve of arbitrary dimension.
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
spline::exact_cubic::derivate
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: exact_cubic.h:168
spline::exact_cubic::time_t
Time time_t
Definition: exact_cubic.h:44
spline::exact_cubic::exact_cubic
exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:58
MathDefs.h
spline::exact_cubic::point_t
Point point_t
Definition: exact_cubic.h:41
spline::curve_abc
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
spline::exact_cubic::~exact_cubic
virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:69
spline
Definition: bernstein.h:20
spline::helpers::T_Point
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
spline::exact_cubic
Definition: exact_cubic.h:40
spline::curve_abc< Numeric, Numeric, Dim, Safe, point_one_dim_t >::time_t
Numeric time_t
Definition: curve_abc.h:26
spline::exact_cubic::exact_cubic
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:63