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.
bezier_curve.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
11 
12 #include "curve_abc.h"
13 #include "bernstein.h"
14 #include "curve_constraint.h"
15 
16 #include "MathDefs.h"
17 
18 #include <vector>
19 #include <stdexcept>
20 
21 #include <iostream>
22 
23 namespace spline {
29 template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
30  typename Point = Eigen::Matrix<Numeric, Dim, 1> >
31 struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
32  typedef Point point_t;
33  typedef Time time_t;
34  typedef Numeric num_t;
36  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
37  typedef typename t_point_t::const_iterator cit_point_t;
39 
40  /* Constructors - destructors */
41  public:
45  template <typename In>
46  bezier_curve(In PointsBegin, In PointsEnd)
47  : T_(1.),
48  mult_T_(1.),
49  size_(std::distance(PointsBegin, PointsEnd)),
50  degree_(size_ - 1),
51  bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) {
52  assert(bernstein_.size() == size_);
53  In it(PointsBegin);
54  if (Safe && (size_ < 1 || T_ <= 0.))
55  throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
56  for (; it != PointsEnd; ++it) pts_.push_back(*it);
57  }
58 
62  template <typename In>
63  bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
64  : T_(T),
65  mult_T_(1.),
66  size_(std::distance(PointsBegin, PointsEnd)),
67  degree_(size_ - 1),
68  bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) {
69  assert(bernstein_.size() == size_);
70  In it(PointsBegin);
71  if (Safe && (size_ < 1 || T_ <= 0.))
72  throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
73  for (; it != PointsEnd; ++it) pts_.push_back(*it);
74  }
75 
79  template <typename In>
80  bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
81  : T_(T),
82  mult_T_(mult_T),
83  size_(std::distance(PointsBegin, PointsEnd)),
84  degree_(size_ - 1),
85  bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) {
86  assert(bernstein_.size() == size_);
87  In it(PointsBegin);
88  if (Safe && (size_ < 1 || T_ <= 0.))
89  throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
90  for (; it != PointsEnd; ++it) pts_.push_back(*it);
91  }
92 
99  template <typename In>
100  bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T = 1.)
101  : T_(T),
102  mult_T_(1.),
103  size_(std::distance(PointsBegin, PointsEnd) + 4),
104  degree_(size_ - 1),
105  bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) {
106  if (Safe && (size_ < 1 || T_ <= 0.))
107  throw std::out_of_range("can't create bezier min bound is higher than max bound");
108  t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
109  for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) pts_.push_back(*cit);
110  }
111 
114  // NOTHING
115  }
116 
117  private:
118  // bezier_curve(const bezier_curve&);
119  // bezier_curve& operator=(const bezier_curve&);
120  /* Constructors - destructors */
121 
122  /*Operations*/
123  public:
127  virtual point_t operator()(const time_t t) const {
128  if (Safe & !(0 <= t && t <= T_)) throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
129  if (size_ == 1) {
130  return mult_T_ * pts_[0];
131  } else {
132  return evalHorner(t);
133  }
134  }
135 
139  bezier_curve_t compute_derivate(const std::size_t order) const {
140  if (order == 0) return *this;
141  t_point_t derived_wp;
142  for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end() - 1; ++pit)
143  derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
144  if (derived_wp.empty()) derived_wp.push_back(point_t::Zero(Dim));
145  bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_, mult_T_ * (1. / T_));
146  return deriv.compute_derivate(order - 1);
147  }
148 
152  bezier_curve_t compute_primitive(const std::size_t order) const {
153  if (order == 0) return *this;
154  num_t new_degree = (num_t)(degree_ + 1);
155  t_point_t n_wp;
156  point_t current_sum = point_t::Zero(Dim);
157  // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
158  // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
159  n_wp.push_back(current_sum);
160  for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) {
161  current_sum += *pit;
162  n_wp.push_back(current_sum / new_degree);
163  }
164  bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_, mult_T_ * T_);
165  return integ.compute_primitive(order - 1);
166  }
167 
174  virtual point_t derivate(const time_t t, const std::size_t order) const {
175  bezier_curve_t deriv = compute_derivate(order);
176  return deriv(t);
177  }
178 
184  point_t evalBernstein(const Numeric t) const {
185  const Numeric u = t / T_;
186  point_t res = point_t::Zero(Dim);
187  typename t_point_t::const_iterator pts_it = pts_.begin();
188  for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
189  ++cit, ++pts_it)
190  res += cit->operator()(u) * (*pts_it);
191  return res * mult_T_;
192  }
193 
197  point_t evalHorner(const Numeric t) const {
198  const Numeric u = t / T_;
199  typename t_point_t::const_iterator pts_it = pts_.begin();
200  Numeric u_op, bc, tn;
201  u_op = 1.0 - u;
202  bc = 1;
203  tn = 1;
204  point_t tmp = (*pts_it) * u_op;
205  ++pts_it;
206  for (unsigned int i = 1; i < degree_; i++, ++pts_it) {
207  tn = tn * u;
208  bc = bc * ((num_t)(degree_ - i + 1)) / i;
209  tmp = (tmp + tn * bc * (*pts_it)) * u_op;
210  }
211  return (tmp + tn * u * (*pts_it)) * mult_T_;
212  }
213 
214  const t_point_t& waypoints() const { return pts_; }
215 
221  point_t evalDeCasteljau(const Numeric t) const {
222  // normalize time :
223  const Numeric u = t / T_;
225  while (pts.size() > 1) {
226  pts = deCasteljauReduction(pts, u);
227  }
228  return pts[0] * mult_T_;
229  }
230 
232 
239  t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
240  if (u < 0 || u > 1) throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
241  if (pts.size() == 1) return pts;
242 
243  t_point_t new_pts;
244  for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
245  new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
246  }
247  return new_pts;
248  }
249 
255  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) {
256  if (t == T_) throw std::runtime_error("can't split curve, interval range is equal to original curve");
257  t_point_t wps_first(size_), wps_second(size_);
258  const double u = t / T_;
259  wps_first[0] = pts_.front();
260  wps_second[degree_] = pts_.back();
261  t_point_t casteljau_pts = waypoints();
262  size_t id = 1;
263  while (casteljau_pts.size() > 1) {
264  casteljau_pts = deCasteljauReduction(casteljau_pts, u);
265  wps_first[id] = casteljau_pts.front();
266  wps_second[degree_ - id] = casteljau_pts.back();
267  ++id;
268  }
269 
270  bezier_curve_t c_first(wps_first.begin(), wps_first.end(), t, mult_T_);
271  bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_ - t, mult_T_);
272  return std::make_pair(c_first, c_second);
273  }
274 
275  bezier_curve_t extract(const Numeric t1, const Numeric t2) {
276  if (t1 < 0. || t1 > T_ || t2 < 0. || t2 > T_) throw std::out_of_range("In Extract curve : times out of bounds");
277  if (t1 == 0. && t2 == T_) return bezier_curve_t(waypoints().begin(), waypoints().end(), T_, mult_T_);
278  if (t1 == 0.) return split(t2).first;
279  if (t2 == T_) return split(t1).second;
280 
281  std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
282  return c_split.second.split(t2 - t1).first;
283  }
284 
285  private:
286  template <typename In>
287  t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
288  t_point_t res;
289  point_t P0, P1, P2, P_n_2, P_n_1, PN;
290  P0 = *PointsBegin;
291  PN = *(PointsEnd - 1);
292  P1 = P0 + constraints.init_vel / (num_t)degree_;
293  P_n_1 = PN - constraints.end_vel / (num_t)degree_;
294  P2 = constraints.init_acc / (num_t)(degree_ * (degree_ - 1)) + 2 * P1 - P0;
295  P_n_2 = constraints.end_acc / (num_t)(degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
296 
297  res.push_back(P0);
298  res.push_back(P1);
299  res.push_back(P2);
300 
301  for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it);
302 
303  res.push_back(P_n_2);
304  res.push_back(P_n_1);
305  res.push_back(PN);
306  return res;
307  }
308 
309  /*Operations*/
310 
311  /*Helpers*/
312  public:
313  virtual time_t min() const { return 0.; }
314  virtual time_t max() const { return T_; }
315  /*Helpers*/
316 
317  public:
318  /*const*/ time_t T_;
319  /*const*/ time_t mult_T_;
320  /*const*/ std::size_t size_;
321  /*const*/ std::size_t degree_;
322  /*const*/ std::vector<Bern<Numeric> > bernstein_;
323 
324  private:
325  t_point_t pts_;
326 
327  public:
328  static bezier_curve_t zero(const time_t T = 1.) {
329  std::vector<point_t> ts;
330  ts.push_back(point_t::Zero(Dim));
331  return bezier_curve_t(ts.begin(), ts.end(), T);
332  }
333 };
334 } // namespace spline
335 #endif //_CLASS_BEZIERCURVE
spline::bezier_curve::bezier_curve
bezier_curve(In PointsBegin, In PointsEnd)
Constructor.
Definition: bezier_curve.h:46
spline::bezier_curve::num_t
Numeric num_t
Definition: bezier_curve.h:34
spline::bezier_curve::max
virtual time_t max() const
Definition: bezier_curve.h:314
spline::bezier_curve::time_t
Time time_t
Definition: bezier_curve.h:33
spline::bezier_curve::zero
static bezier_curve_t zero(const time_t T=1.)
Definition: bezier_curve.h:328
spline::bezier_curve::bezier_curve
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T=1.)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:100
spline::bezier_curve::bezier_curve_t
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
spline::bezier_curve::size_
std::size_t size_
Definition: bezier_curve.h:320
spline::bezier_curve::derivate
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluates the derivative at order N of the curve. If the derivative is to be evaluated several times,...
Definition: bezier_curve.h:174
spline::bezier_curve::t_point_t
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
spline::bezier_curve::extract
bezier_curve_t extract(const Numeric t1, const Numeric t2)
Definition: bezier_curve.h:275
spline::bezier_curve::compute_primitive
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:152
spline::bezier_curve::deCasteljauReduction
t_point_t deCasteljauReduction(const t_point_t &pts, const Numeric u) const
deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
Definition: bezier_curve.h:239
spline::bezier_curve::point_t
Point point_t
Definition: bezier_curve.h:32
spline::bezier_curve::evalHorner
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:197
spline::bezier_curve::mult_T_
time_t mult_T_
Definition: bezier_curve.h:319
spline::bezier_curve::cit_point_t
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
spline::makeBernstein
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:58
spline::bezier_curve::~bezier_curve
~bezier_curve()
Destructor.
Definition: bezier_curve.h:113
spline::bezier_curve::split
std::pair< bezier_curve_t, bezier_curve_t > split(const Numeric t)
split split the curve in 2 at time t
Definition: bezier_curve.h:255
spline::bezier_curve::evalDeCasteljau
point_t evalDeCasteljau(const Numeric t) const
evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
Definition: bezier_curve.h:221
spline::bezier_curve::degree_
std::size_t degree_
Definition: bezier_curve.h:321
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
spline::bezier_curve::bezier_curve
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
Constructor.
Definition: bezier_curve.h:80
spline::Bern
Definition: bernstein.h:39
curve_abc.h
interface for a Curve of arbitrary dimension.
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
bernstein.h
spline::bezier_curve::evalBernstein
point_t evalBernstein(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree Warning: the horner scheme is about 100 times ...
Definition: bezier_curve.h:184
spline::bezier_curve
Definition: bezier_curve.h:31
MathDefs.h
spline::bezier_curve::curve_constraints_t
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
spline::bezier_curve::bezier_curve
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
Constructor.
Definition: bezier_curve.h:63
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::bezier_curve::min
virtual time_t min() const
Definition: bezier_curve.h:313
curve_constraint.h
struct to define constraints on start / end velocities and acceleration on a curve
spline::bezier_curve::bernstein_
std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:322
spline::bezier_curve::T_
time_t T_
Definition: bezier_curve.h:318
spline
Definition: bernstein.h:20
spline::bezier_curve::waypoints
const t_point_t & waypoints() const
Definition: bezier_curve.h:214
spline::bezier_curve::deCasteljauReduction
t_point_t deCasteljauReduction(const Numeric t) const
Definition: bezier_curve.h:231
spline::bezier_curve::compute_derivate
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:139
spline::bezier_curve::operator()
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:127
spline::curve_constraints
Definition: curve_constraint.h:21