Drake
piecewise_polynomial.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <limits>
4 #include <memory>
5 #include <vector>
6 
7 #include <Eigen/Core>
8 
13 
14 namespace drake {
15 namespace trajectories {
16 
17 /// A scalar multi-variate piecewise polynomial.
18 /**
19  * PiecewisePolynomial represents a list of contiguous segments in time with a
20  * Matrix of Polynomials defined for each segment. The term segment is used for
21  * piece.
22  *
23  * An example of a piecewise polynomial is a function of x segments in time,
24  * where for each segment a different polynomial is defined. For a more specific
25  * example, consider the absolute value function, which is a piecewise function.
26  * It uses one function for inputs values < 0, and another function for input
27  * values > 0:
28  *
29  * @code
30  * int abs(int x)
31  * {
32  * if (x<0) {
33  * return -x;
34  * }
35  * else return x;
36  * }
37  * @endcode
38  *
39  * PiecewisePolynomials can be added, subtracted, and multiplied.
40  * They cannot be divided because Polynomials are not closed
41  * under division.
42  *
43  * @tparam T is a scalar type. Explicit instantiations are provided for:
44  * - double
45  */
46 template <typename T>
47 class PiecewisePolynomial final : public PiecewiseTrajectory<T> {
48  public:
49  // We are final, so this is okay.
51 
52  typedef Polynomial<T> PolynomialType;
53  typedef MatrixX<PolynomialType> PolynomialMatrix;
55  typedef Eigen::Ref<CoefficientMatrix> CoefficientMatrixRef;
56 
57  // default constructor; just leaves segment_times and polynomials empty
58  PiecewisePolynomial() = default;
59 
60  // single segment and/or constant value constructor
61  template <typename Derived>
62  explicit PiecewisePolynomial(const Eigen::MatrixBase<Derived>& value)
63  : PiecewiseTrajectory<T>(std::vector<double>(
64  {{0.0, std::numeric_limits<double>::infinity()}})) {
65  polynomials_.push_back(value.template cast<PolynomialType>());
66  }
67 
68  // Matrix constructor
69  PiecewisePolynomial(std::vector<PolynomialMatrix> const& polynomials,
70  std::vector<double> const& breaks);
71 
72  // Scalar constructor
73  PiecewisePolynomial(std::vector<PolynomialType> const& polynomials,
74  std::vector<double> const& breaks);
75 
76  ~PiecewisePolynomial() override = default;
77 
78  std::unique_ptr<Trajectory<T>> Clone() const override;
79 
80  /**
81  * Constructs a piecewise constant PiecewisePolynomial.
82  * Note that constructing a PiecewisePolynomial requires at least two knot
83  * points, although in this case, the second knot point's value is ignored,
84  * and only its break time is used.
85  *
86  * @throws std::runtime_error if
87  * `breaks` and `knots` have different length,
88  * `breaks` is not strictly increasing,
89  * `knots` has inconsistent dimensions,
90  * `breaks` has length smaller than 2.
91  */
93  const std::vector<double>& breaks,
94  const std::vector<CoefficientMatrix>& knots);
95 
96  /**
97  * Eigen version of ZeroOrderHold(breaks, knots) where each column of knots
98  * is used as a knot point, and
99  * knots.cols() == breaks.size().
100  *
101  * @overloads PiecewisePolynomial<T> ZeroOrderHold(breaks, knots)
102  */
104  const Eigen::Ref<const Eigen::VectorXd>& breaks,
105  const Eigen::Ref<const MatrixX<T>>& knots);
106 
107  /**
108  * Constructs a piecewise linear PiecewisePolynomial.
109  *
110  * @throws std::runtime_error if
111  * `breaks` and `knots` have different length,
112  * `breaks` is not strictly increasing,
113  * `knots` has inconsistent dimensions,
114  * `breaks` has length smaller than 2.
115  */
117  const std::vector<double>& breaks,
118  const std::vector<CoefficientMatrix>& knots);
119 
120  /**
121  * Eigen version of FirstOrderHold(breaks, knots) where each column of knots
122  * is used as a knot point, and
123  * knots.cols() == breaks.size().
124  *
125  * @overloads PiecewisePolynomial<T> FirstOrderHold(breaks, knots)
126  */
128  const Eigen::Ref<const Eigen::VectorXd>& breaks,
129  const Eigen::Ref<const MatrixX<T>>& knots);
130 
131  /**
132  * Constructs a third order PiecewisePolynomial from `breaks` and `knots`.
133  * First derivatives are chosen to be "shape preserving", i.e. if
134  * `knots` is monotonic within some interval, the interpolated data will
135  * also be monotonic. The second derivative is not guaranteed to be smooth
136  * across the entire spline.
137  *
138  * Pchip stands for "Piecewise Cubic Hermite Interpolating Polynomial".
139  * For more details, refer to the matlab file "pchip.m".
140  * http://home.uchicago.edu/~sctchoi/courses/cs138/interp.pdf is also a good
141  * reference.
142  *
143  * If @p zero_end_point_derivatives is false, the first and last first
144  * derivative is chosen using a non-centered, shape-preserving three-point
145  * formulae. See equation (2.10) in the following reference for more details.
146  * http://www.mi.sanu.ac.rs/~gvm/radovi/mon.pdf
147  * If @p zero_end_point_derivatives is true, they are set to zeros.
148  *
149  * If @p zero_end_point_derivatives is false, @p breaks and @p knots must
150  * have at least 3 elements for the algorithm to determine the first
151  * derivatives.
152  *
153  * If @p zero_end_point_derivatives is true, @p breaks and @p knots may have
154  * 2 or more elements. For the 2 elements case, the result is equivalent to
155  * computing a cubic polynomial whose values are given by @p knots, and
156  * derivatives set to zero.
157  *
158  * @throws std::runtime_error if
159  * `breaks` and `knots` have different length,
160  * `breaks` is not strictly increasing,
161  * `knots` has inconsistent dimensions,
162  * `breaks` has length smaller than 3 and zero_end_point_derivatives is
163  * false,
164  * `breaks` has length smaller than 2 and zero_end_point_derivatives is
165  * true.
166  */
168  const std::vector<double>& breaks,
169  const std::vector<CoefficientMatrix>& knots,
170  bool zero_end_point_derivatives = false);
171 
172  /**
173  * Eigen version of Pchip(breaks, knots, zero_end_point_derivatives)
174  * where each column of knots is used as a knot point, and
175  * knots.cols() == breaks.size().
176  *
177  * @overloads PiecewisePolynomial<T> Pchip(breaks, knots,
178  * zero_end_point_derivatives)
179  */
181  const Eigen::Ref<const Eigen::VectorXd>& breaks,
182  const Eigen::Ref<const MatrixX<T>>& knots,
183  bool zero_end_point_derivatives = false);
184 
185 
186  /**
187  * Constructs a third order PiecewisePolynomial from `breaks` and `knots`.
188  * The PiecewisePolynomial is constructed such that the interior segments
189  * have the same value, first and second derivatives at `breaks`.
190  * `knot_dot_at_start` and `knot_dot_at_end` are used for the first and
191  * last first derivatives.
192  *
193  * @throws std::runtime_error if
194  * `breaks` and `knots` have different length,
195  * `breaks` is not strictly increasing,
196  * `knots` has inconsistent dimensions,
197  * `knots_dot_at_start` or `knot_dot_at_end` and `knots` have
198  * inconsistent dimensions,
199  * `breaks` has length smaller than 2.
200  */
202  const std::vector<double>& breaks,
203  const std::vector<CoefficientMatrix>& knots,
204  const CoefficientMatrix& knot_dot_start,
205  const CoefficientMatrix& knot_dot_end);
206 
207  /**
208  * Eigen version of Cubic(breaks, knots, knots_dot_start, knots_dot_end)
209  * where each column of knots is used as a knot point, and
210  * knots.cols() == breaks.size().
211  *
212  * @overloads PiecewisePolynomial<T> Cubic(breaks, knots, knots_dot_start,
213  * knots_dot_end)
214  */
216  const Eigen::Ref<const Eigen::VectorXd>& breaks,
217  const Eigen::Ref<const MatrixX<T>>& knots,
218  const Eigen::Ref<const VectorX<T>>& knots_dot_start,
219  const Eigen::Ref<const VectorX<T>>& knots_dot_end);
220 
221  /**
222  * Constructs a third order PiecewisePolynomial from `breaks`, `knots` and
223  * `knots`dot.
224  * Each segment is fully specified by @knots and @knot_dot at both ends.
225  * Second derivatives are not continuous.
226  *
227  * @throws std::runtime_error if
228  * `breaks` and `knots` have different length,
229  * `breaks` is not strictly increasing,
230  * `breaks` and `knots`dot have different length,
231  * `knots` has inconsistent dimensions,
232  * `knots_dot` and `knots` have inconsistent dimensions,
233  * `breaks` has length smaller than 2.
234  */
236  const std::vector<double>& breaks,
237  const std::vector<CoefficientMatrix>& knots,
238  const std::vector<CoefficientMatrix>& knots_dot);
239 
240  /**
241  * Eigen version of Cubic(breaks, knots, knots_dot) where each column of knots
242  * and knots_dot are used as the knot point/derivative.
243  * knots.cols() == knots_dot.cols() == breaks.size().
244  *
245  * @overloads PiecewisePolynomial<T> Cubic(breaks, knots, knots_dot)
246  */
248  const Eigen::Ref<const Eigen::VectorXd>& breaks,
249  const Eigen::Ref<const MatrixX<T>>& knots,
250  const Eigen::Ref<const MatrixX<T>>& knots_dot);
251 
252  /**
253  * Constructs a third order PiecewisePolynomial from `breaks` and `knots`.
254  * The PiecewisePolynomial is constructed such that the interior segments
255  * have the same value, first and second derivatives at `breaks`.
256  * "Not-a-knot" end condition is used here, which means the third derivatives
257  * are continuous for the first two and last two segments.
258  * See https://en.wikipedia.org/wiki/Spline_interpolation for more details
259  * about "Not-a-knot" condition.
260  * The matlab file "spline.m" and
261  * http://home.uchicago.edu/~sctchoi/courses/cs138/interp.pdf are also good
262  * references.
263  *
264  * @p breaks and @p knots must have at least 3 elements. Otherwise there is
265  * not enough information to solve for the coefficients.
266  *
267  * @throws std::runtime_error if
268  * `breaks` and `knots` have different length,
269  * `breaks` is not strictly increasing,
270  * `knots` has inconsistent dimensions,
271  * `breaks` has length smaller than 3.
272  */
274  const std::vector<double>& breaks,
275  const std::vector<CoefficientMatrix>& knots);
276 
277  /**
278  * Eigen version of Cubic(breaks, knots) where each column of knots is used
279  * as a knot point and knots.cols() == breaks.size().
280  *
281  * @overloads PiecewisePolynomial<T> Cubic(breaks, knots)
282  */
284  const Eigen::Ref<const Eigen::VectorXd>& breaks,
285  const Eigen::Ref<const MatrixX<T>>& knots);
286 
287 
288  /// Takes the derivative of this PiecewisePolynomial.
289  /**
290  * Returns a PiecewisePolynomial where each segment is the derivative of the
291  * segment in the input PiecewisePolynomial.
292  * Any rules or limitations of Polynomial::derivative also apply to this
293  * function.
294  *
295  * If `derivative_order` is given, takes the nth derivative of this
296  * PiecewisePolynomial.
297  */
298  PiecewisePolynomial<T> derivative(int derivative_order = 1) const;
299 
300  std::unique_ptr<Trajectory<T>> MakeDerivative(
301  int derivative_order = 1) const override {
302  return derivative(derivative_order).Clone();
303  };
304 
305  /// Takes the integral of this PiecewisePolynomial.
306  /**
307  * Returns a PiecewisePolynomial that is the indefinite integral of this one.
308  * Any rules or limitations of Polynomial::integral also apply to this
309  * function.
310  *
311  * If `value_at_start_time` is given, it does the following only for the
312  * first segment: adds that constant as the constant term
313  * (zeroth-order coefficient) of the resulting Polynomial.
314  */
315  PiecewisePolynomial<T> integral(double value_at_start_time = 0.0) const;
316 
317  /// Takes the integral of this PiecewisePolynomial.
318  /**
319  * Returns a PiecewisePolynomial that is the indefinite integral of this one.
320  * Any rules or limitations of Polynomial::integral also apply to this
321  * function.
322  *
323  * If `value_at_start_time` is given, it does the following only for the
324  * first segment: adds value_at_start_time(row,col) as the constant term
325  * (zeroth-order coefficient) of the resulting Polynomial.
326  */
328  const CoefficientMatrixRef& value_at_start_time) const;
329 
330  bool empty() const { return polynomials_.empty(); }
331 
332  double scalarValue(double t, Eigen::Index row = 0, Eigen::Index col = 0);
333 
334  /**
335  * Evaluates the PiecewisePolynomial at the given time \p t.
336  *
337  * @param t The time at which to evaluate the PiecewisePolynomial.
338  * @return The matrix of evaluated values.
339  */
340  MatrixX<T> value(double t) const override;
341 
342  const PolynomialMatrix& getPolynomialMatrix(int segment_index) const;
343 
344  const PolynomialType& getPolynomial(int segment_index, Eigen::Index row = 0,
345  Eigen::Index col = 0) const;
346 
347  int getSegmentPolynomialDegree(int segment_index, Eigen::Index row = 0,
348  Eigen::Index col = 0) const;
349 
350  Eigen::Index rows() const override;
351 
352  Eigen::Index cols() const override;
353 
354  /// @throws std::runtime_error if other.segment_times is not within
355  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
357 
358  /// @throws std::runtime_error if other.segment_times is not within
359  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
361 
362  /// @throws std::runtime_error if other.segment_times is not within
363  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
365 
366  /// @throws std::runtime_error if offset.segment_times is not within
367  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
369 
370  /// @throws std::runtime_error if offset.segment_times is not within
371  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
373 
374  /// @throws std::runtime_error if other.segment_times is not within
375  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
376  const PiecewisePolynomial operator+(const PiecewisePolynomial& other) const;
377 
378  /// @throws std::runtime_error if other.segment_times is not within
379  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
380  const PiecewisePolynomial operator-(const PiecewisePolynomial& other) const;
381 
382  /// @throws std::runtime_error if other.segment_times is not within
383  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
384  const PiecewisePolynomial operator*(const PiecewisePolynomial& other) const;
385 
386  /// @throws std::runtime_error if offset.segment_times is not within
387  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
388  const PiecewisePolynomial operator+(const CoefficientMatrix& offset) const;
389 
390  /// @throws std::runtime_error if offset.segment_times is not within
391  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
392  const PiecewisePolynomial operator-(const CoefficientMatrix& offset) const;
393 
394  /// Checks if a PiecewisePolynomial is approximately equal to this one.
395  /**
396  * Checks that every coefficient of `other` is within `tol` of the
397  * corresponding coefficient of this PiecewisePolynomial. Throws an exception
398  * if any Polynomial in either PiecewisePolynomial is not univariate.
399  */
400  bool isApprox(const PiecewisePolynomial& other, double tol) const;
401 
402  void shiftRight(double offset);
403 
404  void setPolynomialMatrixBlock(const PolynomialMatrix& replacement,
405  int segment_number, Eigen::Index row_start = 0,
406  Eigen::Index col_start = 0);
407 
408  PiecewisePolynomial slice(int start_segment_index, int num_segments) const;
409 
410  private:
411  double segmentValueAtGlobalAbscissa(int segment_index, double t,
412  Eigen::Index row, Eigen::Index col) const;
413 
414  static constexpr T kSlopeEpsilon = 1e-10;
415 
416  // a PolynomialMatrix for each piece (segment)
417  std::vector<PolynomialMatrix> polynomials_;
418 
419  // Computes coeffecients for a cubic spline given the value and first
420  // derivatives at the end points.
421  // Throws std::runtime_error
422  // if `dt` < Eigen::NumTraits<T>::epsilon()
423  static Eigen::Matrix<T, 4, 1> ComputeCubicSplineCoeffs(double dt, T y0, T y1,
424  T yd0, T yd1);
425 
426  // For a cubic spline, there are 4 unknowns for each segment Pi, namely
427  // the coefficients for Pi = a0 + a1 * t + a2 * t^2 + a3 * t^3.
428  // Let N be the size of breaks and knots, there are N-1 segments,
429  // and thus 4*(N-1) unknowns to fully specified a cubic spline for the given
430  // data.
431  //
432  // If we are also given N knot_dot (velocity), each Pi will be fully specified
433  // by (knots[i], knot_dot[i]) and (knots[i+1], knot_dot[i+1]).
434  // When knot_dot are not specified, we make the design choice to enforce
435  // continuity up to the second order (Yddot) for the interior points, i.e.
436  // Pi'(duration_i) = Pi+1'(0), and Pi''(duration_i) = Pi+1''(0), where
437  // ' means time derivative, and duration_i = breaks[i+1] - breaks[i] is the
438  // duration for the ith segment.
439  //
440  // At this point, we have 2 * (N - 1) position constraints:
441  // Pi(0) = knots[i], for i in [0, N - 2]
442  // Pi(duration_i) = knots[i+1], for i in [0, N - 2]
443  // N - 2 velocity constraints for the interior points:
444  // Pi'(duration_i) = Pi+1'(0), for i in [0, N - 3]
445  // N - 2 acceleration constraints for the interior points:
446  // Pi''(duration_i) = Pi+1''(0), for i in [0, N - 3]
447  //
448  // These sum up to 4 * (N - 1) - 2. This function sets up the above
449  // constraints. There are still 2 constraints missing, which can be resolved
450  // by various end point conditions (velocity at the end points /
451  // "not-a-knot" / etc). These will be specified by the callers.
452  static int SetupCubicSplineInteriorCoeffsLinearSystem(
453  const std::vector<double>& breaks,
454  const std::vector<CoefficientMatrix>& knots, int row, int col,
455  MatrixX<T>* A, VectorX<T>* b);
456 
457  // Computes the first derivative at the end point using a non-centered,
458  // shape-preserving three-point formulae.
459  static CoefficientMatrix ComputePchipEndSlope(
460  double dt0, double dt1, const CoefficientMatrix& slope0,
461  const CoefficientMatrix& slope1);
462 
463  // Throws std::runtime_error if
464  // `breaks` and `knots` have different length,
465  // `breaks` is not strictly increasing,
466  // `knots` has inconsistent dimensions,
467  // `breaks` has length smaller than min_length.
468  static void CheckSplineGenerationInputValidityOrThrow(
469  const std::vector<double>& breaks,
470  const std::vector<CoefficientMatrix>& knots, int min_length);
471 };
472 
473 } // namespace trajectories
474 } // namespace drake
PiecewisePolynomial slice(int start_segment_index, int num_segments) const
Definition: piecewise_polynomial.cc:289
PiecewisePolynomial & operator*=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:173
const PiecewisePolynomial operator+(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:203
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
static PiecewisePolynomial< T > ZeroOrderHold(const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots)
Constructs a piecewise constant PiecewisePolynomial.
Definition: piecewise_polynomial.cc:378
static PiecewisePolynomial< T > Cubic(const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots, const CoefficientMatrix &knot_dot_start, const CoefficientMatrix &knot_dot_end)
Constructs a third order PiecewisePolynomial from breaks and knots.
Definition: piecewise_polynomial.cc:666
Definition: automotive_demo.cc:90
bool empty() const
Definition: piecewise_polynomial.h:330
static PiecewisePolynomial< T > Pchip(const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots, bool zero_end_point_derivatives=false)
Constructs a third order PiecewisePolynomial from breaks and knots.
Definition: piecewise_polynomial.cc:475
const PiecewisePolynomial operator-(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:212
Eigen::Index rows() const override
Definition: piecewise_polynomial.cc:316
MatrixX< PolynomialType > PolynomialMatrix
Definition: piecewise_polynomial.h:53
STL namespace.
void shiftRight(double offset)
Definition: piecewise_polynomial.cc:270
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
std::unique_ptr< Trajectory< T > > MakeDerivative(int derivative_order=1) const override
Takes the derivative of this Trajectory.
Definition: piecewise_polynomial.h:300
std::unique_ptr< Trajectory< T > > Clone() const override
Definition: piecewise_polynomial.cc:47
Definition: autodiff_overloads.h:34
std::vector< double > vector
Definition: translator_test.cc:20
std::vector< snopt::doublereal > A
Definition: snopt_solver.cc:93
double scalarValue(double t, Eigen::Index row=0, Eigen::Index col=0)
Definition: piecewise_polynomial.cc:105
#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:57
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > MatrixX
A matrix of dynamic size, templated on scalar type.
Definition: eigen_types.h:108
A scalar multi-variate piecewise polynomial.
Definition: piecewise_polynomial.h:47
int getSegmentPolynomialDegree(int segment_index, Eigen::Index row=0, Eigen::Index col=0) const
Definition: piecewise_polynomial.cc:143
Polynomial< double > PolynomialType
Definition: piecewise_polynomial.h:52
PiecewisePolynomial< T > derivative(int derivative_order=1) const
Takes the derivative of this PiecewisePolynomial.
Definition: piecewise_polynomial.cc:53
PiecewisePolynomial & operator+=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:151
static PiecewisePolynomial< T > FirstOrderHold(const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots)
Constructs a piecewise linear PiecewisePolynomial.
Definition: piecewise_polynomial.cc:404
MatrixX< T > value(double t) const override
Evaluates the PiecewisePolynomial at the given time t.
Definition: piecewise_polynomial.cc:114
void setPolynomialMatrixBlock(const PolynomialMatrix &replacement, int segment_number, Eigen::Index row_start=0, Eigen::Index col_start=0)
Definition: piecewise_polynomial.cc:278
Eigen::Ref< CoefficientMatrix > CoefficientMatrixRef
Definition: piecewise_polynomial.h:55
const PolynomialMatrix & getPolynomialMatrix(int segment_index) const
Definition: piecewise_polynomial.cc:130
const PolynomialType & getPolynomial(int segment_index, Eigen::Index row=0, Eigen::Index col=0) const
Definition: piecewise_polynomial.cc:136
PiecewisePolynomial & operator-=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:162
bool isApprox(const PiecewisePolynomial &other, double tol) const
Checks if a PiecewisePolynomial is approximately equal to this one.
Definition: piecewise_polynomial.cc:249
const PiecewisePolynomial operator*(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:221
PiecewisePolynomial< T > integral(double value_at_start_time=0.0) const
Takes the integral of this PiecewisePolynomial.
Definition: piecewise_polynomial.cc:72
const std::vector< double > & breaks() const
Definition: piecewise_trajectory.h:66
Abstract class that implements the basic logic of maintaining consequent segments of time (delimited ...
Definition: piecewise_trajectory.h:21
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
Eigen::Index cols() const override
Definition: piecewise_polynomial.cc:325