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`. If
256  * `periodic_end_condition` is `false` (default), then the "Not-a-knot" end
257  * condition is used here, which means the third derivatives are
258  * continuous for the first two and last two segments. If
259  * `periodic_end_condition` is `true`, then the first and second derivatives
260  * between the end of the last segment and the beginning of the first
261  * segment will be continuous. Note that the periodic end condition does
262  * not require the first and last knot to be collocated, nor does it add
263  * an additional knot to connect the first and last segments. Only first
264  * and second derivative continuity is enforced.
265  * See https://en.wikipedia.org/wiki/Spline_interpolation,
266  * https://www.math.uh.edu/~jingqiu/math4364/spline.pdf, and
267  * http://www.maths.lth.se/na/courses/FMN081/FMN081-06/lecture11.pdf
268  * for more about cubic splines and their end conditions.
269  * The MATLAB files "spline.m" and "csape.m" are also good references.
270  *
271  * @p breaks and @p knots must have at least 3 elements. The "not-a-knot"
272  * condition is ill-defined for two knots, and the "periodic" condition
273  * would produce a straight line (use `FirstOrderHold` for this instead).
274  *
275  * @param periodic_end_condition Determines whether the "not-a-knot"
276  * (`false`) or the periodic spline (`true`) end condition is used.
277  *
278  * @throws std::runtime_error if
279  * `breaks` and `knots` have different length,
280  * `breaks` is not strictly increasing,
281  * `knots` has inconsistent dimensions,
282  * `breaks` has length smaller than 3.
283  */
285  const std::vector<double>& breaks,
286  const std::vector<CoefficientMatrix>& knots,
287  bool periodic_end_condition = false);
288 
289  /**
290  * Eigen version of Cubic(breaks, knots) where each column of knots is used
291  * as a knot point and knots.cols() == breaks.size().
292  *
293  * @overloads PiecewisePolynomial<T> Cubic(breaks, knots)
294  */
296  const Eigen::Ref<const Eigen::VectorXd>& breaks,
297  const Eigen::Ref<const MatrixX<T>>& knots,
298  bool periodic_end_condition = false);
299 
300 
301 
302  /// Takes the derivative of this PiecewisePolynomial.
303  /**
304  * Returns a PiecewisePolynomial where each segment is the derivative of the
305  * segment in the input PiecewisePolynomial.
306  * Any rules or limitations of Polynomial::derivative also apply to this
307  * function.
308  *
309  * If `derivative_order` is given, takes the nth derivative of this
310  * PiecewisePolynomial.
311  */
312  PiecewisePolynomial<T> derivative(int derivative_order = 1) const;
313 
314  std::unique_ptr<Trajectory<T>> MakeDerivative(
315  int derivative_order = 1) const override {
316  return derivative(derivative_order).Clone();
317  };
318 
319  /// Takes the integral of this PiecewisePolynomial.
320  /**
321  * Returns a PiecewisePolynomial that is the indefinite integral of this one.
322  * Any rules or limitations of Polynomial::integral also apply to this
323  * function.
324  *
325  * If `value_at_start_time` is given, it does the following only for the
326  * first segment: adds that constant as the constant term
327  * (zeroth-order coefficient) of the resulting Polynomial.
328  */
329  PiecewisePolynomial<T> integral(double value_at_start_time = 0.0) const;
330 
331  /// Takes the integral of this PiecewisePolynomial.
332  /**
333  * Returns a PiecewisePolynomial that is the indefinite integral of this one.
334  * Any rules or limitations of Polynomial::integral also apply to this
335  * function.
336  *
337  * If `value_at_start_time` is given, it does the following only for the
338  * first segment: adds value_at_start_time(row,col) as the constant term
339  * (zeroth-order coefficient) of the resulting Polynomial.
340  */
342  const CoefficientMatrixRef& value_at_start_time) const;
343 
344  bool empty() const { return polynomials_.empty(); }
345 
346  double scalarValue(double t, Eigen::Index row = 0,
347  Eigen::Index col = 0) const;
348 
349  /**
350  * Evaluates the PiecewisePolynomial at the given time \p t.
351  *
352  * @param t The time at which to evaluate the PiecewisePolynomial.
353  * @return The matrix of evaluated values.
354  */
355  MatrixX<T> value(double t) const override;
356 
357  const PolynomialMatrix& getPolynomialMatrix(int segment_index) const;
358 
359  const PolynomialType& getPolynomial(int segment_index, Eigen::Index row = 0,
360  Eigen::Index col = 0) const;
361 
362  int getSegmentPolynomialDegree(int segment_index, Eigen::Index row = 0,
363  Eigen::Index col = 0) const;
364 
365  /// Returns the row count of each and every PolynomialMatrix segment.
366  Eigen::Index rows() const override;
367 
368  /// Returns the column count of each and every PolynomialMatrix segment.
369  Eigen::Index cols() const override;
370 
371  /// @throws std::runtime_error if other.segment_times is not within
372  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
374 
375  /// @throws std::runtime_error if other.segment_times is not within
376  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
378 
379  /// @throws std::runtime_error if other.segment_times is not within
380  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
382 
383  /// @throws std::runtime_error if offset.segment_times is not within
384  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
386 
387  /// @throws std::runtime_error if offset.segment_times is not within
388  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
390 
391  /// @throws std::runtime_error if other.segment_times is not within
392  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
393  const PiecewisePolynomial operator+(const PiecewisePolynomial& other) const;
394 
395  /// @throws std::runtime_error if other.segment_times is not within
396  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
397  const PiecewisePolynomial operator-(const PiecewisePolynomial& other) const;
398 
399  /// @throws std::runtime_error if other.segment_times is not within
400  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
401  const PiecewisePolynomial operator*(const PiecewisePolynomial& other) const;
402 
403  /// @throws std::runtime_error if offset.segment_times is not within
404  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
405  const PiecewisePolynomial operator+(const CoefficientMatrix& offset) const;
406 
407  /// @throws std::runtime_error if offset.segment_times is not within
408  /// PiecewiseFunction::kEpsilonTime from this->segment_times.
409  const PiecewisePolynomial operator-(const CoefficientMatrix& offset) const;
410 
411  /// Checks if a PiecewisePolynomial is approximately equal to this one.
412  /**
413  * Checks that every coefficient of `other` is within `tol` of the
414  * corresponding coefficient of this PiecewisePolynomial. Throws an exception
415  * if any Polynomial in either PiecewisePolynomial is not univariate.
416  */
417  bool isApprox(const PiecewisePolynomial& other, double tol) const;
418 
419  /// Concatenates @p other at the end, yielding a continuous trajectory
420  /// from current start_time() to @p other end_time().
421  ///
422  /// @param other PiecewisePolynomial instance to concatenate.
423  /// @throw std::runtime_error if trajectories' dimensions do not match
424  /// each other (either rows() or cols() does
425  /// not match between this and @p other).
426  /// @throw std::runtime_error if this end_time() and @p other start_time() are
427  /// not within PiecewiseTrajectory<T>::kEpsilonTime
428  /// from each other.
429  void ConcatenateInTime(const PiecewisePolynomial& other);
430 
431  void shiftRight(double offset);
432 
433  void setPolynomialMatrixBlock(const PolynomialMatrix& replacement,
434  int segment_number, Eigen::Index row_start = 0,
435  Eigen::Index col_start = 0);
436 
437  PiecewisePolynomial slice(int start_segment_index, int num_segments) const;
438 
439  private:
440  double segmentValueAtGlobalAbscissa(int segment_index, double t,
441  Eigen::Index row, Eigen::Index col) const;
442 
443  static constexpr T kSlopeEpsilon = 1e-10;
444 
445  // a PolynomialMatrix for each piece (segment)
446  std::vector<PolynomialMatrix> polynomials_;
447 
448  // Computes coeffecients for a cubic spline given the value and first
449  // derivatives at the end points.
450  // Throws std::runtime_error
451  // if `dt` < Eigen::NumTraits<T>::epsilon()
452  static Eigen::Matrix<T, 4, 1> ComputeCubicSplineCoeffs(double dt, T y0, T y1,
453  T yd0, T yd1);
454 
455  // For a cubic spline, there are 4 unknowns for each segment Pi, namely
456  // the coefficients for Pi = a0 + a1 * t + a2 * t^2 + a3 * t^3.
457  // Let N be the size of breaks and knots, there are N-1 segments,
458  // and thus 4*(N-1) unknowns to fully specified a cubic spline for the given
459  // data.
460  //
461  // If we are also given N knot_dot (velocity), each Pi will be fully specified
462  // by (knots[i], knot_dot[i]) and (knots[i+1], knot_dot[i+1]).
463  // When knot_dot are not specified, we make the design choice to enforce
464  // continuity up to the second order (Yddot) for the interior points, i.e.
465  // Pi'(duration_i) = Pi+1'(0), and Pi''(duration_i) = Pi+1''(0), where
466  // ' means time derivative, and duration_i = breaks[i+1] - breaks[i] is the
467  // duration for the ith segment.
468  //
469  // At this point, we have 2 * (N - 1) position constraints:
470  // Pi(0) = knots[i], for i in [0, N - 2]
471  // Pi(duration_i) = knots[i+1], for i in [0, N - 2]
472  // N - 2 velocity constraints for the interior points:
473  // Pi'(duration_i) = Pi+1'(0), for i in [0, N - 3]
474  // N - 2 acceleration constraints for the interior points:
475  // Pi''(duration_i) = Pi+1''(0), for i in [0, N - 3]
476  //
477  // These sum up to 4 * (N - 1) - 2. This function sets up the above
478  // constraints. There are still 2 constraints missing, which can be resolved
479  // by various end point conditions (velocity at the end points /
480  // "not-a-knot" / etc). These will be specified by the callers.
481  static int SetupCubicSplineInteriorCoeffsLinearSystem(
482  const std::vector<double>& breaks,
483  const std::vector<CoefficientMatrix>& knots, int row, int col,
484  MatrixX<T>* A, VectorX<T>* b);
485 
486  // Computes the first derivative at the end point using a non-centered,
487  // shape-preserving three-point formulae.
488  static CoefficientMatrix ComputePchipEndSlope(
489  double dt0, double dt1, const CoefficientMatrix& slope0,
490  const CoefficientMatrix& slope1);
491 
492  // Throws std::runtime_error if
493  // `breaks` and `knots` have different length,
494  // `breaks` is not strictly increasing,
495  // `knots` has inconsistent dimensions,
496  // `breaks` has length smaller than min_length.
497  static void CheckSplineGenerationInputValidityOrThrow(
498  const std::vector<double>& breaks,
499  const std::vector<CoefficientMatrix>& knots, int min_length);
500 };
501 
502 } // namespace trajectories
503 } // namespace drake
504 
PiecewisePolynomial slice(int start_segment_index, int num_segments) const
Definition: piecewise_polynomial.cc:322
PiecewisePolynomial & operator*=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:174
const PiecewisePolynomial operator+(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:204
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:411
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:699
Definition: automotive_demo.cc:105
bool empty() const
Definition: piecewise_polynomial.h:344
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:508
const PiecewisePolynomial operator-(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:213
Eigen::Index rows() const override
Returns the row count of each and every PolynomialMatrix segment.
Definition: piecewise_polynomial.cc:349
MatrixX< PolynomialType > PolynomialMatrix
Definition: piecewise_polynomial.h:53
STL namespace.
void shiftRight(double offset)
Definition: piecewise_polynomial.cc:303
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:314
std::unique_ptr< Trajectory< T > > Clone() const override
Definition: piecewise_polynomial.cc:48
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) const
Definition: piecewise_polynomial.cc:106
#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:144
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:54
PiecewisePolynomial & operator+=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:152
static PiecewisePolynomial< T > FirstOrderHold(const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots)
Constructs a piecewise linear PiecewisePolynomial.
Definition: piecewise_polynomial.cc:437
void ConcatenateInTime(const PiecewisePolynomial &other)
Concatenates other at the end, yielding a continuous trajectory from current start_time() to other en...
Definition: piecewise_polynomial.cc:271
MatrixX< T > value(double t) const override
Evaluates the PiecewisePolynomial at the given time t.
Definition: piecewise_polynomial.cc:115
void setPolynomialMatrixBlock(const PolynomialMatrix &replacement, int segment_number, Eigen::Index row_start=0, Eigen::Index col_start=0)
Definition: piecewise_polynomial.cc:311
Eigen::Ref< CoefficientMatrix > CoefficientMatrixRef
Definition: piecewise_polynomial.h:55
const PolynomialMatrix & getPolynomialMatrix(int segment_index) const
Definition: piecewise_polynomial.cc:131
const PolynomialType & getPolynomial(int segment_index, Eigen::Index row=0, Eigen::Index col=0) const
Definition: piecewise_polynomial.cc:137
PiecewisePolynomial & operator-=(const PiecewisePolynomial &other)
Definition: piecewise_polynomial.cc:163
bool isApprox(const PiecewisePolynomial &other, double tol) const
Checks if a PiecewisePolynomial is approximately equal to this one.
Definition: piecewise_polynomial.cc:250
const PiecewisePolynomial operator*(const PiecewisePolynomial &other) const
Definition: piecewise_polynomial.cc:222
PiecewisePolynomial< T > integral(double value_at_start_time=0.0) const
Takes the integral of this PiecewisePolynomial.
Definition: piecewise_polynomial.cc:73
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
Returns the column count of each and every PolynomialMatrix segment.
Definition: piecewise_polynomial.cc:358