Robotics Library  0.7.0
Spline.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert, Andre Gaschler
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 //
14 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
15 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
18 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
19 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
20 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
21 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
22 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
23 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
24 // POSSIBILITY OF SUCH DAMAGE.
25 //
26 
27 #ifndef RL_MATH_SPLINE_H
28 #define RL_MATH_SPLINE_H
29 
30 #include <limits>
31 #include <vector>
32 
33 #include "Function.h"
34 #include "Polynomial.h"
35 
36 namespace rl
37 {
38  namespace math
39  {
51  template<typename T>
52  class Spline : public Function<T>
53  {
54  public:
55  typedef typename ::std::vector<Polynomial<T>>::const_iterator ConstIterator;
56 
57  typedef typename ::std::vector<Polynomial<T>>::const_reverse_iterator ConstReverseIterator;
58 
59  typedef typename ::std::vector<Polynomial<T>>::iterator Iterator;
60 
61  typedef typename ::std::vector<Polynomial<T>>::reverse_iterator ReverseIterator;
62 
63  Spline() :
64  Function<T>(),
65  polynomials()
66  {
67  this->x0 = 0;
68  this->x1 = 0;
69  }
70 
71  virtual ~Spline()
72  {
73  }
74 
87  template<typename Container1, typename Container2>
88  static Spline LinearParabolic(const Container1& x, const Container2& y, const Real& parabolicInterval)
89  {
90  assert(x.size() > 1);
91  assert(x.size() == y.size());
92 
93  ::std::size_t n = y.size();
94  ::std::size_t dim = TypeTraits<T>::size(y[0]);
95 
96  Spline f;
97 
98  {
99  T yd = (y[1] - y[0]) / (x[1] - x[0]);
100  T yBeforeLinear = y[0] + yd * (parabolicInterval / 2);
101  Real linearInterval = (x[1] - x[0]) - parabolicInterval;
102  assert(linearInterval > 0);
103  T yAfterLinear = y[0] + yd * (parabolicInterval / 2 + linearInterval);
104 
105  Polynomial<T> parabolic = Polynomial<T>::Quadratic(y[0], yBeforeLinear, TypeTraits<T>::Zero(dim), parabolicInterval);
106  f.push_back(parabolic);
107  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
108  f.push_back(linear);
109  }
110 
111  for (::std::size_t i = 1; i < n - 1; ++i)
112  {
113  Real deltaXPrev = x[i] - x[i - 1];
114  Real deltaXNext = x[i + 1] - x[i];
115  Real linearInterval = deltaXNext - parabolicInterval;
116  assert(deltaXPrev > parabolicInterval && deltaXNext > parabolicInterval);
117  T ydPrev = (y[i] - y[i - 1]) / deltaXPrev;
118  T ydNext = (y[i + 1] - y[i]) / deltaXNext;
119  T yBeforeParabolic = y[i] + (-parabolicInterval / 2 * ydPrev);
120  T yBeforeLinear = y[i] + (parabolicInterval / 2 * ydNext);
121  T yAfterLinear = y[i] + ((parabolicInterval / 2 + linearInterval) * ydNext);
122 
123  Polynomial<T> parabolic = Polynomial<T>::Quadratic(yBeforeParabolic, yBeforeLinear, ydPrev, parabolicInterval);
124  f.push_back(parabolic);
125  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
126  f.push_back(linear);
127  }
128 
129  {
130  T yd = (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]);
131  T yBeforeParabolic = y[n - 1] - yd * (parabolicInterval / 2);
132 
133  Polynomial<T> parabolic = Polynomial<T>::Quadratic(yBeforeParabolic, TypeTraits<T>::Zero(dim), yd, parabolicInterval);
134  f.push_back(parabolic);
135  }
136 
137  return f;
138  }
139 
152  template<typename Container1, typename Container2>
153  static Spline LinearQuartic(const Container1& x, const Container2& y, const Real& quarticInterval)
154  {
155  assert(x.size() > 1);
156  assert(x.size() == y.size());
157 
158  ::std::size_t n = y.size();
159  ::std::size_t dim = TypeTraits<T>::size(y[0]);
160 
161  Spline f;
162 
163  {
164  T yd = (y[1] - y[0]) / (x[1] - x[0]);
165  T yBeforeLinear = y[0] + yd * (quarticInterval / 2);
166  Real linearInterval = (x[1] - x[0]) - quarticInterval;
167  assert(linearInterval > 0);
168  T yAfterLinear = y[0] + yd * (quarticInterval / 2 + linearInterval);
169 
170  Polynomial<T> quartic = Polynomial<T>::QuarticFirstSecond(y[0], yBeforeLinear, TypeTraits<T>::Zero(dim), yd, TypeTraits<T>::Zero(dim), quarticInterval);
171  f.push_back(quartic);
172  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
173  f.push_back(linear);
174  }
175 
176  for (::std::size_t i = 1; i < n - 1; ++i)
177  {
178  Real deltaXPrev = x[i] - x[i - 1];
179  Real deltaXNext = x[i + 1] - x[i];
180  Real linearInterval = deltaXNext - quarticInterval;
181  assert(deltaXPrev > quarticInterval && deltaXNext > quarticInterval);
182  T ydPrev = (y[i] - y[i - 1]) / deltaXPrev;
183  T ydNext = (y[i + 1] - y[i]) / deltaXNext;
184  T yBeforeQuartic = y[i] + (-quarticInterval / 2 * ydPrev);
185  T yBeforeLinear = y[i] + (quarticInterval / 2 * ydNext);
186  T yAfterLinear = y[i] + ((quarticInterval / 2 + linearInterval) * ydNext);
187 
188  Polynomial<T> quartic = Polynomial<T>::QuarticFirstSecond(yBeforeQuartic, yBeforeLinear, ydPrev, ydNext, TypeTraits<T>::Zero(dim), quarticInterval);
189  f.push_back(quartic);
190  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
191  f.push_back(linear);
192  }
193 
194  {
195  T yd = (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]);
196  T yBeforeQuartic = y[n - 1] - yd * (quarticInterval / 2);
197 
198  Polynomial<T> quartic = Polynomial<T>::QuarticFirstSecond(yBeforeQuartic, y[n - 1], yd, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), quarticInterval);
199  f.push_back(quartic);
200  }
201 
202  return f;
203  }
204 
217  template<typename Container1, typename Container2>
218  static Spline LinearSextic(const Container1& x, const Container2& y, const Real& sexticInterval)
219  {
220  assert(x.size() > 1);
221  assert(x.size() == y.size());
222 
223  ::std::size_t n = y.size();
224  ::std::size_t dim = TypeTraits<T>::size(y[0]);
225 
226  Spline f;
227 
228  {
229  T yd = (y[1] - y[0]) / (x[1] - x[0]);
230  T yBeforeLinear = y[0] + yd * (sexticInterval / 2);
231  Real linearInterval = (x[1] - x[0]) - sexticInterval;
232  assert(linearInterval > 0);
233  T yAfterLinear = y[0] + yd * (sexticInterval / 2 + linearInterval);
234 
235  Polynomial<T> sextic = Polynomial<T>::SexticFirstSecondThird(y[0], yBeforeLinear, TypeTraits<T>::Zero(dim), yd, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), sexticInterval);
236  f.push_back(sextic);
237  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
238  f.push_back(linear);
239  }
240 
241  for (::std::size_t i = 1; i < n - 1; ++i)
242  {
243  Real deltaXPrev = x[i] - x[i - 1];
244  Real deltaXNext = x[i + 1] - x[i];
245  Real linearInterval = deltaXNext - sexticInterval;
246  assert(deltaXPrev > sexticInterval && deltaXNext > sexticInterval);
247  T ydPrev = (y[i] - y[i - 1]) / deltaXPrev;
248  T ydNext = (y[i + 1] - y[i]) / deltaXNext;
249  T yBeforeSextic = y[i] + (-sexticInterval / 2 * ydPrev);
250  T yBeforeLinear = y[i] + (sexticInterval / 2 * ydNext);
251  T yAfterLinear = y[i] + ((sexticInterval / 2 + linearInterval) * ydNext);
252 
253  Polynomial<T> sextic = Polynomial<T>::SexticFirstSecondThird(yBeforeSextic, yBeforeLinear, ydPrev, ydNext, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), sexticInterval);
254  f.push_back(sextic);
255  Polynomial<T> linear = Polynomial<T>::Linear(yBeforeLinear, yAfterLinear, linearInterval);
256  f.push_back(linear);
257  }
258 
259  {
260  T yd = (y[n - 1] - y[n - 2]) / (x[n - 1] - x[n - 2]);
261  T yBeforeSextic = y[n - 1] - yd * (sexticInterval / 2);
262 
263  Polynomial<T> sextic = Polynomial<T>::SexticFirstSecondThird(yBeforeSextic, y[n - 1], yd, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), sexticInterval);
264  f.push_back(sextic);
265  }
266 
267  return f;
268  }
269 
270 #if !(defined(_MSC_VER) && _MSC_VER < 1800)
271 
280  template<typename U = T>
281  static Spline QuarticLinearQuarticAtRest(
282  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& q0,
283  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& q1,
284  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& vmax,
285  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& amax
286  )
287  {
288  T xta = 3 * ::std::pow(vmax, 2) / (4 * amax); // travel distance during acceleration (or, deceleration)
289  T distance = ::std::abs(q1 - q0);
290  T xl = distance - 2 * xta; // travel distance during linear part
291  T sign = (q1 > q0) ? 1 : -1;
292 
293  Spline f;
294 
295  if (xl > 0)
296  {
297  // vmax is reached, linear segment exists
298  T tl = xl / vmax;
299  T ta = 3 * vmax / (2 * amax);
300  Polynomial<T> acc = Polynomial<T>::QuarticFirstSecond(q0, q0 + sign * xta, 0, sign * vmax, 0, ta);
301  f.push_back(acc);
302  Polynomial<T> lin = Polynomial<T>::Linear(q0 + sign * xta, q1 - sign * xta, tl);
303  f.push_back(lin);
304  Polynomial<T> dec = Polynomial<T>::QuarticFirstSecond(q1 - sign * xta, q1, sign * vmax, 0, 0, ta);
305  f.push_back(dec);
306  }
307  else
308  {
309  // vmax is not reached
310  T vreached = ::std::sqrt(2 * distance * amax / 3);
311  T th = ::std::sqrt(3 * distance / (2 * amax));
312  T xh = (q0 + q1) / 2;
313  Polynomial<T> acc = Polynomial<T>::QuarticFirstSecond(q0, xh, 0, sign * vreached, 0, th);
314  f.push_back(acc);
315  Polynomial<T> dec = Polynomial<T>::QuarticFirstSecond(xh, q1, sign * vreached, 0, 0, th);
316  f.push_back(dec);
317  }
318 
319  return f;
320  }
321 #endif
322 
333 #if !(defined(_MSC_VER) && _MSC_VER < 1800)
334  template<typename U = T>
335  static Spline QuarticLinearQuarticAtRest(
336  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& q0,
337  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& q1,
338  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& vmax,
339  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& amax
340  )
341 #else
342  static Spline QuarticLinearQuarticAtRest(const T& q0, const T& q1, const T& vmax, const T& amax)
343 #endif
344  {
345  assert(q0.size() >= 1 && q0.size() == q1.size() && q0.size() == vmax.size() && q0.size() == amax.size() && "QuarticLinearQuarticAtRest: parameters must have same dimension.");
346 
347  ::std::size_t dim = TypeTraits<T>::size(q0);
348 
349  // Find minimal durations for each dof independently
350  T tAcc(dim);
351  T tLin(dim);
352 
353  for (::std::size_t i = 0; i < dim; ++i)
354  {
355  Real xta = 3 * ::std::pow(vmax[i], 2) / (4 * amax[i]); // travel distance during acceleration (or, deceleration)
356  Real distance = ::std::abs(q1[i] - q0[i]);
357  Real xl = distance - 2 * xta; // travel distance during linear part
358 
359  if (xl > 0)
360  {
361  // vmax is reached, linear segment exists
362  tAcc[i] = 3 * vmax[i] / (2 * amax[i]);
363  tLin[i] = xl / vmax[i];
364  }
365  else
366  {
367  // vmax is not reached
368  //Real vreached = ::std::sqrt(2 * distance * amax(i) / 3);
369  tAcc[i] = ::std::sqrt(3 * distance / (2 * amax[i]));
370  tLin[i] = 0;
371  }
372  }
373 
374  // Use slowest degree-of-freedom to synchronize all
375  Real tAccMax = TypeTraits<T>::max_element(tAcc);
376  Real tLinMax = TypeTraits<T>::max_element(tLin);
377 
378  T sign(dim);
379  T xAfterAcc(dim);
380  T vReached(dim);
381 
382  for (::std::size_t i = 0; i < dim; ++i)
383  {
384  sign[i] = (q1[i] > q0[i]) ? 1 : -1;
385  Real distance = ::std::abs(q1[i] - q0[i]);
386  xAfterAcc[i] = tAccMax * distance / (2 * tLinMax + 2 * tAccMax);
387  vReached[i] = distance / (tLinMax + tAccMax);
388  }
389 
390  Spline f;
391 
392  Polynomial<T> acc = Polynomial<T>::QuarticFirstSecond(q0, q0 + sign * xAfterAcc, TypeTraits<T>::Zero(dim), sign * vReached, TypeTraits<T>::Zero(dim), tAccMax);
393  f.push_back(acc);
394 
395  if (tLinMax > 0)
396  {
397  Polynomial<T> lin = Polynomial<T>::Linear(q0 + sign * xAfterAcc, q1 - sign * xAfterAcc, tLinMax);
398  f.push_back(lin);
399  }
400 
401  Polynomial<T> dec = Polynomial<T>::QuarticFirstSecond(q1 - sign * xAfterAcc, q1, sign * vReached, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), tAccMax);
402  f.push_back(dec);
403 
404  return f;
405  }
406 
407 #if !(defined(_MSC_VER) && _MSC_VER < 1800)
408 
420  template<typename U = T>
421  static Spline SexticLinearSexticAtRest(
422  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& q0,
423  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& q1,
424  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& vmax,
425  const typename ::std::enable_if< ::std::is_floating_point<U>::value, U>::type& amax
426  )
427  {
428  T xta = 15 * ::std::pow(vmax, 2) / (16 * amax); // travel distance during acceleration (or, deceleration)
429  T distance = ::std::abs(q1 - q0);
430  T xl = distance - 2 * xta; // travel distance during linear part
431  T sign = (q1 > q0) ? 1 : -1;
432 
433  Spline f;
434 
435  if (xl > 0)
436  {
437  // vmax is reached, linear segment exists
438  T tl = xl / vmax;
439  T ta = 15 * vmax / (8 * amax);
440  Polynomial<T> acc = Polynomial<T>::SexticFirstSecondThird(q0, q0 + sign * xta, 0, sign * vmax, 0, 0, 0, ta);
441  f.push_back(acc);
442  Polynomial<T> lin = Polynomial<T>::Linear(q0 + sign * xta, q1 - sign * xta, tl);
443  f.push_back(lin);
444  Polynomial<T> dec = Polynomial<T>::SexticFirstSecondThird(q1 - sign * xta, q1, sign * vmax, 0, 0, 0, 0, ta);
445  f.push_back(dec);
446  }
447  else
448  {
449  // vmax is not reached
450  T vreached = ::std::sqrt(8 * distance * amax / 15);
451  T th = ::std::sqrt(15 * distance / (8 * amax));
452  T xh = (q0 + q1) / 2;
453  Polynomial<T> acc = Polynomial<T>::SexticFirstSecondThird(q0, xh, 0, sign * vreached, 0, 0, 0, th);
454  f.push_back(acc);
455  Polynomial<T> dec = Polynomial<T>::SexticFirstSecondThird(xh, q1, sign * vreached, 0, 0, 0, 0, th);
456  f.push_back(dec);
457  }
458 
459  return f;
460  }
461 #endif
462 
473 #if !(defined(_MSC_VER) && _MSC_VER < 1800)
474  template<typename U = T>
475  static Spline SexticLinearSexticAtRest(
476  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& q0,
477  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& q1,
478  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& vmax,
479  const typename ::std::enable_if< ::std::is_class<U>::value, U>::type& amax
480  )
481 #else
482  static Spline SexticLinearSexticAtRest(const T& q0, const T& q1, const T& vmax, const T& amax)
483 #endif
484  {
485  assert(q0.size() >= 1 && q0.size() == q1.size() && q0.size() == vmax.size() && q0.size() == amax.size() && "SexticLinearSexticAtRest: parameters must have same dimension.");
486 
487  ::std::size_t dim = TypeTraits<T>::size(q0);
488 
489  // Find minimal durations for each dof independently
490  T tAcc(dim);
491  T tLin(dim);
492 
493  for (::std::size_t i = 0; i < dim; ++i)
494  {
495  Real xta = 15 * ::std::pow(vmax[i], 2) / (16 * amax[i]); // travel distance during acceleration (or, deceleration)
496  Real distance = ::std::abs(q1[i] - q0[i]);
497  Real xl = distance - 2 * xta; // travel distance during linear part
498 
499  if (xl > 0)
500  {
501  // vmax is reached, linear segment exists
502  tAcc[i] = 15 * vmax[i] / (8 * amax[i]);
503  tLin[i] = xl / vmax[i];
504  }
505  else
506  {
507  // vmax is not reached
508  //Real vreached = ::std::sqrt(8 * distance * amax(i) / 15);
509  tAcc[i] = ::std::sqrt(15 * distance / (8 * amax[i]));
510  tLin[i] = 0;
511  }
512  }
513 
514  // Use slowest degree-of-freedom to synchronize all
515  Real tAccMax = TypeTraits<T>::max_element(tAcc);
516  Real tLinMax = TypeTraits<T>::max_element(tLin);
517 
518  T sign(dim);
519  T xAfterAcc(dim);
520  T vReached(dim);
521 
522  for (::std::size_t i = 0; i < dim; ++i)
523  {
524  sign[i] = (q1[i] > q0[i]) ? 1 : -1;
525  Real distance = ::std::abs(q1[i] - q0[i]);
526  xAfterAcc[i] = tAccMax * distance / (2 * tLinMax + 2 * tAccMax);
527  vReached[i] = distance / (tLinMax + tAccMax);
528  }
529 
530  Spline f;
531 
532  Polynomial<T> acc = Polynomial<T>::SexticFirstSecondThird(q0, q0 + sign * xAfterAcc, TypeTraits<T>::Zero(dim), sign * vReached, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), tAccMax);
533  f.push_back(acc);
534 
535  if (tLinMax > 0)
536  {
537  Polynomial<T> lin = Polynomial<T>::Linear(q0 + sign * xAfterAcc, q1 - sign * xAfterAcc, tLinMax);
538  f.push_back(lin);
539  }
540 
541  Polynomial<T> dec = Polynomial<T>::SexticFirstSecondThird(q1 - sign * xAfterAcc, q1, sign * vReached, TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), TypeTraits<T>::Zero(dim), tAccMax);
542  f.push_back(dec);
543 
544  return f;
545  }
546 
557  static Spline TrapeziodalAccelerationAtRest(const T& q0, const T& q1, const T& vmax, const T& amax, const T& jmax)
558  {
559 // assert((vmax > 0).all() && (amax > 0).all() && (jmax > 0).all() && "TrapeziodalAccelerationAtRest: velocity, acceleration and jerk limits must be positive.");
560 
561  ::std::size_t dim = TypeTraits<T>::size(q0);
562 
563  // Find minimal durations for each dof independently
564  T sign(dim);
565  T tJerk(dim);
566  T tAcc(dim);
567  T tLin(dim);
568  T vReached(dim);
569 
570  for (::std::size_t i = 0; i < dim; ++i)
571  {
572  sign(i) = (q1(i) > q0(i)) ? 1 : -1;
573  Real d = ::std::abs(q1(i) - q0(i));
574  Real tj;
575  Real ta;
576 
577  // assume vmax is reached
578  if (vmax(i) * jmax(i) >= amax(i) * amax(i))
579  {
580  tj = amax(i) / jmax(i); // amax reached
581  ta = tj + vmax(i) / amax(i);
582  }
583  else
584  {
585  tj = ::std::sqrt(vmax(i) / jmax(i)); // amax not reached
586  ta = 2 * tj;
587  }
588 
589  Real tv = d / vmax(i) - ta;
590  vReached(i) = sign(i) * vmax(i);
591 
592  if (tv <= 0)
593  {
594  tv = 0; // vmax not reached, recalculate
595 
596  if (d >= 2 * ::std::pow(amax(i), 3) / ::std::pow(jmax(i), 2))
597  {
598  tj = amax(i) / jmax(i); // amax reached
599  ta = tj / 2 + ::std::sqrt(::std::pow(tj / 2, 2) + d / amax(i));
600  vReached(i) = sign(i) * ((ta - tj) * amax(i));
601  }
602  else
603  {
604  tj = ::std::pow(d / (2 * jmax(i)), 1 / 3.); // amax not reached
605  ta = 2 * tj;
606  vReached(i) = sign(i) * (tj * tj * jmax(i));
607  }
608  }
609 
610  tAcc(i) = ta;
611  tLin(i) = tv;
612  tJerk(i) = tj;
613  }
614 
615  // Use slowest degree-of-freedom to synchronize all with tjFixed, tAccMax, tLinMax
616  Real tjFixed = TypeTraits<T>::max_element(tJerk);
617  Real tAccMax = TypeTraits<T>::max_element(tAcc);
618  Real tLinMax = TypeTraits<T>::max_element(tLin);
619 
620  T vLin(dim);
621  T qBeforeLin(dim);
622  T aAcc(dim);
623  T vAfterJerk(dim);
624  T xAfterJerk(dim);
625 
626  for (::std::size_t i = 0; i < dim; ++i)
627  {
628  // Useful positions and velocities to compute spline
629  vLin(i) = (q1(i) - q0(i)) / (tAccMax + tLinMax);
630  qBeforeLin(i) = (q1(i) + q0(i)) / 2.0f - (tLinMax / 2.0f) * vLin(i);
631  aAcc(i) = vLin(i) / (tAccMax - tjFixed);
632  vAfterJerk(i) = aAcc(i) * tjFixed / 2;
633  xAfterJerk(i) = aAcc(i) * tjFixed * tjFixed / 6;
634  }
635 
636  Spline f;
637 
638  Polynomial<T> jerk1 = Polynomial<T>::CubicFirst(q0, q0 + xAfterJerk, 0 * vmax, vAfterJerk, tjFixed);
639  f.push_back(jerk1);
640 
641  T vAfterConstAcc = vAfterJerk;
642  T xAfterConstAcc = xAfterJerk;
643 
644  if (2 * tjFixed < tAccMax)
645  {
646  Real tConstAcc = tAccMax - 2 * tjFixed;
647  vAfterConstAcc += tConstAcc * aAcc;
648  xAfterConstAcc += tConstAcc * (vAfterJerk + vAfterConstAcc) / 2;
649  Polynomial<T> acc1 = Polynomial<T>::Quadratic(q0 + xAfterJerk, q0 + xAfterConstAcc, vAfterJerk, tConstAcc);
650  f.push_back(acc1);
651  }
652 
653  Polynomial<T> jerk2 = Polynomial<T>::CubicFirst(q0 + xAfterConstAcc, qBeforeLin, vAfterConstAcc, vLin, tjFixed);
654  f.push_back(jerk2);
655 
656  if (tLinMax > 0)
657  {
658  Polynomial<T> lin = Polynomial<T>::Linear(qBeforeLin, qBeforeLin + tLinMax * vLin, tLinMax);
659  f.push_back(lin);
660  }
661 
662  Polynomial<T> jerk3 = Polynomial<T>::CubicFirst(qBeforeLin + tLinMax * vLin, q1 - xAfterConstAcc, vLin, vAfterConstAcc, tjFixed);
663  f.push_back(jerk3);
664 
665  if (2 * tjFixed < tAccMax)
666  {
667  Real tConstAcc = tAccMax - 2 * tjFixed;
668  Polynomial<T> acc2 = Polynomial<T>::Quadratic(q1 - xAfterConstAcc, q1 - xAfterJerk, vAfterConstAcc, tConstAcc);
669  f.push_back(acc2);
670  }
671 
672  Polynomial<T> jerk4 = Polynomial<T>::CubicFirst(q1 - xAfterJerk, q1, vAfterJerk, 0 * vmax, tjFixed);
673  f.push_back(jerk4);
674 
675  return f;
676  }
677 
678  Polynomial<T>& at(const ::std::size_t& i)
679  {
680  return this->polynomials.at(i);
681  }
682 
683  const Polynomial<T>& at(const ::std::size_t& i) const
684  {
685  return this->polynomials.at(i);
686  }
687 
688  Polynomial<T>& back()
689  {
690  return this->polynomials.back();
691  }
692 
693  const Polynomial<T>& back() const
694  {
695  return this->polynomials.back();
696  }
697 
698  Iterator begin()
699  {
700  return this->polynomials.begin();
701  }
702 
703  ConstIterator begin() const
704  {
705  return this->polynomials.begin();
706  }
707 
708  void clear()
709  {
710  this->polynomials.clear();
711  this->x0 = 0;
712  this->x1 = 0;
713  }
714 
715  Spline* clone() const
716  {
717  return new Spline(*this);
718  }
719 
720  Spline derivative() const
721  {
722  Spline spline;
723 
724  for (::std::size_t i = 0; i < this->size(); ++i)
725  {
726  Polynomial<T> polynomial = this->polynomials[i].derivative();
727  spline.push_back(polynomial);
728  }
729 
730  return spline;
731  }
732 
733  bool empty()
734  {
735  return this->polynomials.empty();
736  }
737 
738  Iterator end()
739  {
740  return this->polynomials.end();
741  }
742 
743  ConstIterator end() const
744  {
745  return this->polynomials.end();
746  }
747 
748  Polynomial<T>& front()
749  {
750  return this->polynomials.front();
751  }
752 
753  const Polynomial<T>& front() const
754  {
755  return this->polynomials.front();
756  }
757 
767  T getAbsoluteMaximum() const
768  {
769  T maximum = TypeTraits<T>::abs((*this)(this->x0));
770 
771  for (::std::size_t i = 0; i < this->size(); ++i)
772  {
773  T maximumOfPolynomial = this->polynomials[i].getAbsoluteMaximum();
774 
775  for (::std::ptrdiff_t row = 0; row < maximum.size(); ++row)
776  {
777  Real y = maximumOfPolynomial[row];
778 
779  if (y > maximum[row])
780  {
781  maximum[row] = y;
782  }
783  }
784  }
785 
786  return maximum;
787  }
788 
799  bool isContinuous(const ::std::size_t& upToDerivative = 1) const
800  {
801  for (::std::size_t d = 0; d <= upToDerivative; ++d)
802  {
803  for (::std::size_t i = 1; i < this->polynomials.size(); ++i)
804  {
805  T xBefore = this->polynomials[i - 1](this->polynomials[i - 1].upper(), d);
806  T xAfter = this->polynomials[i](this->polynomials[i].lower(), d);
807  return TypeTraits<T>::equal(xBefore, xAfter);
808  }
809  }
810 
811  return true;
812  }
813 
814  T operator()(const Real& x, const ::std::size_t& derivative = 0) const
815  {
816  assert(x >= this->lower() - FUNCTION_BOUNDARY);
817  assert(x <= this->upper() + FUNCTION_BOUNDARY);
818  assert(this->polynomials.size() > 0);
819 
820  Real x0 = this->lower();
821  ::std::size_t i = 0;
822 
823  for (; x > x0 + this->polynomials[i].duration() && i + 1 < this->polynomials.size(); ++i)
824  {
825  x0 += this->polynomials[i].duration();
826  }
827 
828  return this->polynomials[i](x - x0, derivative);
829  }
830 
831  Polynomial<T>& operator[](const ::std::size_t& i)
832  {
833  return this->polynomials[i];
834  }
835 
836  const Polynomial<T>& operator[](const ::std::size_t& i) const
837  {
838  return this->polynomials[i];
839  }
840 
841  void pop_back()
842  {
843  if (!this->polynomials.empty())
844  {
845  this->x1 -= this->polynomials.back().duration();
846  this->polynomials.pop_back();
847 
848  if (this->polynomials.empty())
849  {
850  this->x0 = 0;
851  }
852  }
853  }
854 
855  void push_back(Polynomial<T>& polynomial)
856  {
857  if (this->polynomials.empty())
858  {
859  this->x0 = polynomial.lower();
860  }
861 
862  this->polynomials.push_back(polynomial);
863  this->x1 += polynomial.duration();
864  }
865 
866  void push_back(Spline& spline)
867  {
868  for (::std::size_t i = 0; i < spline.polynomials.size(); ++i)
869  {
870  this->push_back(spline.polynomials[i]);
871  }
872  }
873 
881  Spline scaledX(const Real& factor) const
882  {
883  Spline spline;
884 
885  for (::std::size_t i = 0; i < this->size(); ++i)
886  {
887  Polynomial<T> polynomial = this->polynomials[i].scaledX(factor);
888  spline.push_back(polynomial);
889  }
890 
891  return spline;
892  }
893 
894  ::std::size_t size() const
895  {
896  return this->polynomials.size();
897  }
898 
899  ReverseIterator rbegin()
900  {
901  return this->polynomials.rbegin();
902  }
903 
904  ConstReverseIterator rbegin() const
905  {
906  return this->polynomials.rbegin();
907  }
908 
909  ReverseIterator rend()
910  {
911  return this->polynomials.rend();
912  }
913 
914  ConstReverseIterator rend() const
915  {
916  return this->polynomials.rend();
917  }
918 
919  protected:
920  ::std::vector<Polynomial<T>> polynomials;
921 
922  private:
923 
924  };
925  }
926 }
927 
928 #endif // RL_MATH_SPLINE_H
rl::math::Function::duration
Real duration() const
Definition: Function.h:59
pow
Quaternion< Scalar > pow(const Scalar &t) const
Definition: QuaternionBaseAddons.h:128
rl::math::sign
T sign(const T &arg)
Definition: Real.h:58
rl::math::Spline::back
const Polynomial< T > & back() const
Definition: Spline.h:693
rl::math::Function
A mathematical mapping from Real -> ArrayX.
Definition: Function.h:45
rl::math::Function::x1
Real x1
Definition: Function.h:104
rl::math::Spline::begin
Iterator begin()
Definition: Spline.h:698
rl::math::TypeTraits::max_element
static T max_element(const T &t)
Definition: TypeTraits.h:65
rl::math::Spline::operator()
T operator()(const Real &x, const ::std::size_t &derivative=0) const
Evaluates the function or a derivative thereof for a given value x.
Definition: Spline.h:814
rl::math::TypeTraits::size
::std::size_t size(const T &t)
Definition: TypeTraits.h:75
rl::math::Polynomial::CubicFirst
static Polynomial CubicFirst(const T &y0, const T &y1, const T &yd0, const T &yd1, const Real &x1=1)
Definition: Polynomial.h:73
Polynomial.h
rl::math::Spline::end
Iterator end()
Definition: Spline.h:738
rl::math::Spline::end
ConstIterator end() const
Definition: Spline.h:743
rl::math::Polynomial::SexticFirstSecondThird
static Polynomial SexticFirstSecondThird(const T &y0, const T &y1, const T &yd0, const T &yd1, const T &ydd0, const T &ydd1, const T &yddd0, const Real &x1=1)
Definition: Polynomial.h:168
rl::math::Spline::ConstIterator
::std::vector< Polynomial< T > >::const_iterator ConstIterator
Definition: Spline.h:55
rl::math::Spline::push_back
void push_back(Polynomial< T > &polynomial)
Definition: Spline.h:855
rl::math::Spline::rend
ConstReverseIterator rend() const
Definition: Spline.h:914
rl::math::Spline::rbegin
ReverseIterator rbegin()
Definition: Spline.h:899
rl::math::Spline::derivative
Spline derivative() const
Definition: Spline.h:720
rl::math::Spline::LinearQuartic
static Spline LinearQuartic(const Container1 &x, const Container2 &y, const Real &quarticInterval)
Generates a piecewise spline with quartic polynomial segments around the given supporting points y an...
Definition: Spline.h:153
rl::math::Spline::clear
void clear()
Definition: Spline.h:708
rl::math::Polynomial::Quadratic
static Polynomial Quadratic(const T &y0, const T &y1, const T &yd0, const Real &x1=1)
Definition: Polynomial.h:110
rl::math::Spline::TrapeziodalAccelerationAtRest
static Spline TrapeziodalAccelerationAtRest(const T &q0, const T &q1, const T &vmax, const T &amax, const T &jmax)
Generates a trapezoidal acceleration trajectory from rest to rest for multiple dimensions that are ph...
Definition: Spline.h:557
rl::math::Spline::front
Polynomial< T > & front()
Definition: Spline.h:748
rl::math::Spline::Iterator
::std::vector< Polynomial< T > >::iterator Iterator
Definition: Spline.h:59
rl::math::Spline::size
::std::size_t size() const
Definition: Spline.h:894
rl::math::Polynomial
A vector-valued polynomial function from Real -> T.
Definition: Polynomial.h:61
rl::math::Spline::push_back
void push_back(Spline &spline)
Definition: Spline.h:866
rl::math::Spline::pop_back
void pop_back()
Definition: Spline.h:841
rl::math::Spline::ConstReverseIterator
::std::vector< Polynomial< T > >::const_reverse_iterator ConstReverseIterator
Definition: Spline.h:57
rl::math::Polynomial::Linear
static Polynomial Linear(const T &y0, const T &y1, const Real &x1=1)
Definition: Polynomial.h:99
rl::math::Spline::getAbsoluteMaximum
T getAbsoluteMaximum() const
Returns the array of the maximum function values of each dimension within the definition range,...
Definition: Spline.h:767
rl::math::Spline::polynomials
::std::vector< Polynomial< T > > polynomials
Definition: Spline.h:920
rl::math::Spline::operator[]
const Polynomial< T > & operator[](const ::std::size_t &i) const
Definition: Spline.h:836
rl::math::Spline::LinearSextic
static Spline LinearSextic(const Container1 &x, const Container2 &y, const Real &sexticInterval)
Generates a piecewise spline with sextic polynomial segments around the given supporting points y and...
Definition: Spline.h:218
rl::math::Function::upper
Real & upper()
Definition: Function.h:74
rl::math::Spline::operator[]
Polynomial< T > & operator[](const ::std::size_t &i)
Definition: Spline.h:831
rl::math::Spline::at
const Polynomial< T > & at(const ::std::size_t &i) const
Definition: Spline.h:683
rl::math::Spline::Spline
Spline()
Definition: Spline.h:63
rl::math::Spline::isContinuous
bool isContinuous(const ::std::size_t &upToDerivative=1) const
Verifies that the spline is smooth and has no jumps at the piecewise function boundaries.
Definition: Spline.h:799
rl::math::Spline::QuarticLinearQuarticAtRest
static Spline QuarticLinearQuarticAtRest(const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &q0, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &q1, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &vmax, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &amax)
Generates a spline of polynomials of degrees 4-1-4 from rest to rest that is phase-synchronized for m...
Definition: Spline.h:335
rl::math::TypeTraits::equal
static bool equal(const T &lhs, const T &rhs, const T &epsilon=::Eigen::NumTraits< T >::dummy_precision())
Definition: TypeTraits.h:60
rl::math::Spline::empty
bool empty()
Definition: Spline.h:733
rl::math::TypeTraits::abs
static T abs(const T &t)
Definition: TypeTraits.h:55
rl::math::Function::x0
Real x0
Definition: Function.h:102
rl::math::Spline::scaledX
Spline scaledX(const Real &factor) const
Stretches the x-axis of a spline by a given factor.
Definition: Spline.h:881
rl::math::Spline::at
Polynomial< T > & at(const ::std::size_t &i)
Definition: Spline.h:678
rl::math::Spline::begin
ConstIterator begin() const
Definition: Spline.h:703
rl::math::TypeTraits
Definition: TypeTraits.h:43
rl::math::Polynomial::QuarticFirstSecond
static Polynomial QuarticFirstSecond(const T &y0, const T &y1, const T &yd0, const T &yd1, const T &ydd0, const Real &x1=1)
Definition: Polynomial.h:122
rl::math::Spline::clone
Spline * clone() const
Definition: Spline.h:715
rl::math::Spline::SexticLinearSexticAtRest
static Spline SexticLinearSexticAtRest(const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &q0, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &q1, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &vmax, const typename ::std::enable_if< ::std::is_class< U >::value, U >::type &amax)
Generates a spline of polynomials of degrees 6-1-6 from rest to rest that is phase-synchronized for m...
Definition: Spline.h:475
rl::math::FUNCTION_BOUNDARY
static const Real FUNCTION_BOUNDARY
Definition: Function.h:110
rl::math::Spline::back
Polynomial< T > & back()
Definition: Spline.h:688
distance
Scalar distance(const Transform< Scalar, Dim, Mode, Options > &other, const Scalar &weight=1) const
Definition: TransformAddons.h:33
rl::math::Spline::SexticLinearSexticAtRest
static Spline SexticLinearSexticAtRest(const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &q0, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &q1, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &vmax, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &amax)
Generates a spline of polynomials of degrees 6-1-6 from rest to rest for one dimension.
Definition: Spline.h:421
rl::math::Function::lower
Real & lower()
Definition: Function.h:64
rl::math::Real
double Real
Definition: Real.h:42
Function.h
rl::math::Spline::rbegin
ConstReverseIterator rbegin() const
Definition: Spline.h:904
rl::math::Spline
A piecewise Function of Polynomial functions.
Definition: Spline.h:53
rl::math::Spline::ReverseIterator
::std::vector< Polynomial< T > >::reverse_iterator ReverseIterator
Definition: Spline.h:61
rl::math::Spline::front
const Polynomial< T > & front() const
Definition: Spline.h:753
rl::math::Spline::LinearParabolic
static Spline LinearParabolic(const Container1 &x, const Container2 &y, const Real &parabolicInterval)
Generates a piecewise spline with parabolic segments around the given supporting points y and linear ...
Definition: Spline.h:88
rl::math::Spline::rend
ReverseIterator rend()
Definition: Spline.h:909
rl::math::Spline::~Spline
virtual ~Spline()
Definition: Spline.h:71
rl::math::Spline::QuarticLinearQuarticAtRest
static Spline QuarticLinearQuarticAtRest(const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &q0, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &q1, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &vmax, const typename ::std::enable_if< ::std::is_floating_point< U >::value, U >::type &amax)
Generates a spline of polynomials of degrees 4-1-4 from rest to rest for one dimension.
Definition: Spline.h:281
rl
Robotics Library.
Definition: AnalogInput.cpp:30
Generated by doxygen 1.8.20