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