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 "curve_constraint.h"
24 #include "piecewise_curve.h"
25 #include "polynomial.h"
26 
27 #include "MathDefs.h"
28 
29 #include <functional>
30 #include <vector>
31 
32 namespace ndcurves {
37 template <typename Time = double, typename Numeric = Time, bool Safe = false,
38  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
39  typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
40  typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
41 struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
42  typedef Point point_t;
43  typedef const Eigen::Ref<const point_t> point_ref_t;
44  typedef T_Point t_point_t;
45  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
46  typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
47  typedef Time time_t;
48  typedef Numeric num_t;
49  typedef SplineBase spline_t;
50  typedef typename std::vector<spline_t> t_spline_t;
51  typedef typename t_spline_t::iterator it_spline_t;
52  typedef typename t_spline_t::const_iterator cit_spline_t;
54 
60 
61  /* Constructors - destructors */
62  public:
65  exact_cubic() : piecewise_curve_t() {}
66 
71  template <typename In>
72  exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t() {
73  t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
74  for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
75  this->add_curve(*it);
76  }
77  }
78 
84  template <typename In>
85  exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) : piecewise_curve_t() {
86  t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints);
87  for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
88  this->add_curve(*it);
89  }
90  }
91 
94  exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t() {
95  for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
96  this->add_curve(*it);
97  }
98  }
99 
100  exact_cubic(const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) {}
101 
103  exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}
104 
106  virtual ~exact_cubic() {}
107 
116  bool isApprox(const exact_cubic_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
117  return piecewise_curve_t::isApprox(other, prec);
118  }
119 
120  std::size_t getNumberSplines() { return this->getNumberCurves(); }
121 
122  spline_t getSplineAt(std::size_t index) {
123  boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->curves_.at(index));
124  if (s_ptr)
125  return *s_ptr;
126  else
127  throw std::runtime_error(
128  "Parent piecewise curve do not contain only curves created from exact_cubic class methods");
129  }
130 
131  private:
132 
133  static polynomial_t create_cubic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
134  const time_t t_min, const time_t t_max){
135  typename polynomial_t::t_point_t coeffs;
136  coeffs.push_back(a);
137  coeffs.push_back(b);
138  coeffs.push_back(c);
139  coeffs.push_back(d);
140  return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
141  }
142  static polynomial_t create_quintic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d,
143  point_ref_t e, point_ref_t f,
144  const time_t t_min, const time_t t_max){
145  typename polynomial_t::t_point_t coeffs;
146  coeffs.push_back(a);
147  coeffs.push_back(b);
148  coeffs.push_back(c);
149  coeffs.push_back(d);
150  coeffs.push_back(e);
151  coeffs.push_back(f);
152  return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
153  }
154 
161  template <typename In>
162  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const {
163  const std::size_t dim = wayPointsBegin->second.size();
164  const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
165  if (Safe && size < 1) {
166  throw std::length_error("size of waypoints must be superior to 0"); // TODO
167  }
168  t_spline_t subSplines;
169  subSplines.reserve(size);
170  // refer to the paper to understand all this.
171  MatrixX h1 = MatrixX::Zero(size, size);
172  MatrixX h2 = MatrixX::Zero(size, size);
173  MatrixX h3 = MatrixX::Zero(size, size);
174  MatrixX h4 = MatrixX::Zero(size, size);
175  MatrixX h5 = MatrixX::Zero(size, size);
176  MatrixX h6 = MatrixX::Zero(size, size);
177  MatrixX a = MatrixX::Zero(size, dim);
178  MatrixX b = MatrixX::Zero(size, dim);
179  MatrixX c = MatrixX::Zero(size, dim);
180  MatrixX d = MatrixX::Zero(size, dim);
181  MatrixX x = MatrixX::Zero(size, dim);
182  In it(wayPointsBegin), next(wayPointsBegin);
183  ++next;
184  // Fill the matrices H as specified in the paper.
185  for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
186  num_t const dTi((*next).first - (*it).first);
187  num_t const dTi_sqr(dTi * dTi);
188  num_t const dTi_cube(dTi_sqr * dTi);
189  // filling matrices values
190  h3(i, i) = -3 / dTi_sqr;
191  h3(i, i + 1) = 3 / dTi_sqr;
192  h4(i, i) = -2 / dTi;
193  h4(i, i + 1) = -1 / dTi;
194  h5(i, i) = 2 / dTi_cube;
195  h5(i, i + 1) = -2 / dTi_cube;
196  h6(i, i) = 1 / dTi_sqr;
197  h6(i, i + 1) = 1 / dTi_sqr;
198  if (i + 2 < size) {
199  In it2(next);
200  ++it2;
201  num_t const dTi_1((*it2).first - (*next).first);
202  num_t const dTi_1sqr(dTi_1 * dTi_1);
203  // this can be optimized but let's focus on clarity as long as not needed
204  h1(i + 1, i) = 2 / dTi;
205  h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
206  h1(i + 1, i + 2) = 2 / dTi_1;
207  h2(i + 1, i) = -6 / dTi_sqr;
208  h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
209  h2(i + 1, i + 2) = 6 / dTi_1sqr;
210  }
211  x.row(i) = (*it).second.transpose();
212  }
213  // adding last x
214  x.row(size - 1) = (*it).second.transpose();
215  // Compute coefficients of polynom.
216  a = x;
217  PseudoInverse(h1);
218  b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
219  c = h3 * x + h4 * b;
220  d = h5 * x + h6 * b;
221  // create splines along waypoints.
222  it = wayPointsBegin, next = wayPointsBegin;
223  ++next;
224  for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
225  subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i),
226  (*it).first, (*next).first));
227  }
228  return subSplines;
229  }
230 
231  template <typename In>
232  t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const {
233  std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
234  if (Safe && size < 1) {
235  throw std::length_error("number of waypoints should be superior to one"); // TODO
236  }
237  t_spline_t subSplines;
238  subSplines.reserve(size - 1);
239  spline_constraints cons = constraints;
240  In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
241  ++next;
242  for (std::size_t i(0); next != end; ++next, ++it, ++i) {
243  compute_one_spline<In>(it, next, cons, subSplines);
244  }
245  compute_end_spline<In>(it, next, cons, subSplines);
246  return subSplines;
247  }
248 
249  template <typename In>
250  void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
251  t_spline_t& subSplines) const {
252  const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
253  const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
254  const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
255  const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
256  const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
257  subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t));
258  constraints.init_vel = subSplines.back().derivate(end_t, 1);
259  constraints.init_acc = subSplines.back().derivate(end_t, 2);
260  }
261 
262  template <typename In>
263  void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints,
264  t_spline_t& subSplines) const {
265  const std::size_t dim = wayPointsBegin->second.size();
266  const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
267  const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc / 2.,
268  c1 = constraints.end_acc;
269  const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
270  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;
271  // solving a system of four linear eq with 4 unknows: d0, e0
272  const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
273  const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
274  const point_t alpha_2 = c1 - 2 * c0;
275  const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
276  const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
277  const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
278  point_t d, e, f;
279  MatrixX rhs = MatrixX::Zero(3, dim);
280  rhs.row(0) = alpha_0;
281  rhs.row(1) = alpha_1;
282  rhs.row(2) = alpha_2;
283  Matrix3 eq = Matrix3::Zero(3, 3);
284  eq(0, 0) = x_d_0;
285  eq(0, 1) = x_e_0;
286  eq(0, 2) = x_f_0;
287  eq(1, 0) = x_d_1;
288  eq(1, 1) = x_e_1;
289  eq(1, 2) = x_f_1;
290  eq(2, 0) = x_d_2;
291  eq(2, 1) = x_e_2;
292  eq(2, 2) = x_f_2;
293  rhs = eq.inverse().eval() * rhs;
294  d = rhs.row(0);
295  e = rhs.row(1);
296  f = rhs.row(2);
297  subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t));
298  }
299 
300  public:
301  // Serialization of the class
302  friend class boost::serialization::access;
303  template <class Archive>
304  void serialize(Archive& ar, const unsigned int version) {
305  if (version) {
306  // Do something depending on version ?
307  }
308  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(piecewise_curve_t);
309  }
310 };
311 } // namespace ndcurves
312 
313 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
314  typename T_Point, typename SplineBase),
316 #endif //_CLASS_EXACTCUBIC
virtual ~exact_cubic()
Destructor.
Definition: exact_cubic.h:106
T_Point t_point_t
Definition: exact_cubic.h:44
Definition: bernstein.h:20
exact_cubic(const exact_cubic &other)
Copy Constructor.
Definition: exact_cubic.h:103
t_spline_t::const_iterator cit_spline_t
Definition: exact_cubic.h:52
bool isApprox(const exact_cubic_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: exact_cubic.h:116
Definition: exact_cubic.h:41
void PseudoInverse(_Matrix_Type_ &pinvmat)
An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference...
Definition: MathDefs.h:26
std::vector< spline_t > t_spline_t
Definition: exact_cubic.h:50
virtual std::size_t dim() const
Get dimension of curve.
Definition: piecewise_curve.h:564
std::size_t getNumberCurves()
Definition: piecewise_curve.h:576
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints)
Constructor.
Definition: exact_cubic.h:85
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: exact_cubic.h:45
const Eigen::Ref< const point_t > point_ref_t
Definition: exact_cubic.h:43
Definition of a cubic spline.
SplineBase spline_t
Definition: exact_cubic.h:49
exact_cubic(const t_spline_t &subSplines)
Constructor.
Definition: exact_cubic.h:94
interface for a Curve of arbitrary dimension.
exact_cubic(const t_curve_ptr_t &subSplines)
Definition: exact_cubic.h:100
Eigen::Matrix< Numeric, 3, 3 > Matrix3
Definition: exact_cubic.h:46
t_curve_ptr_t curves_
Definition: piecewise_curve.h:581
T_Point t_point_t
Definition: polynomial.h:37
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: exact_cubic.h:56
Numeric num_t
Definition: exact_cubic.h:48
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
piecewise_curve< Time, Numeric, Safe, point_t > piecewise_curve_t
Definition: exact_cubic.h:57
curve_constraints< Point > spline_constraints
Definition: exact_cubic.h:53
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:37
piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t
Definition: exact_cubic.h:59
double Time
Definition: effector_spline.h:27
void serialize(Archive &ar, const unsigned int version)
Definition: exact_cubic.h:304
point_t init_acc
Definition: curve_constraint.h:63
exact_cubic(In wayPointsBegin, In wayPointsEnd)
Constructor.
Definition: exact_cubic.h:72
point_t end_acc
Definition: curve_constraint.h:66
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
spline_t getSplineAt(std::size_t index)
Definition: exact_cubic.h:122
std::size_t getNumberSplines()
Definition: exact_cubic.h:120
struct to define constraints on start / end velocities and acceleration on a curve ...
bool isApprox(const piecewise_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: piecewise_curve.h:92
Point point_t
Definition: exact_cubic.h:42
t_spline_t::iterator it_spline_t
Definition: exact_cubic.h:51
double Numeric
Definition: effector_spline.h:26
point_t init_vel
Definition: curve_constraint.h:62
class allowing to create a piecewise curve.
polynomial< Time, Numeric, Safe, point_t > polynomial_t
Definition: exact_cubic.h:58
Definition: fwd.h:34
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
point_t end_vel
Definition: curve_constraint.h:65
Definition: curve_constraint.h:22
std::vector< curve_ptr_t > t_curve_ptr_t
Definition: piecewise_curve.h:41
exact_cubic()
Empty constructor. Add at least one curve to call other class functions.
Definition: exact_cubic.h:65
Time time_t
Definition: exact_cubic.h:47
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
exact_cubic< Time, Numeric, Safe, point_t, T_Point, SplineBase > exact_cubic_t
Definition: exact_cubic.h:55