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 
14 
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 
84  const std::vector<double>& breaks,
85  const std::vector<CoefficientMatrix>& knots);
86 
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 
140  const std::vector<double>& breaks,
141  const std::vector<CoefficientMatrix>& knots,
142  bool zero_end_point_derivatives = false);
143 
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 
180  const std::vector<double>& breaks,
181  const std::vector<CoefficientMatrix>& knots,
182  const std::vector<CoefficientMatrix>& knots_dot);
183 
206  const std::vector<double>& breaks,
207  const std::vector<CoefficientMatrix>& knots);
208 
210 
219  PiecewisePolynomial derivative(int derivative_order = 1) const;
220 
222 
231  PiecewisePolynomial integral(double value_at_start_time = 0.0) const;
232 
234 
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 
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 
273 
277 
281 
284  PiecewisePolynomial& operator+=(const CoefficientMatrix& offset);
285 
288  PiecewisePolynomial& operator-=(const CoefficientMatrix& offset);
289 
292  const PiecewisePolynomial operator+(const PiecewisePolynomial& other) const;
293 
296  const PiecewisePolynomial operator-(const PiecewisePolynomial& other) const;
297 
300  const PiecewisePolynomial operator*(const PiecewisePolynomial& other) const;
301 
304  const PiecewisePolynomial operator+(const CoefficientMatrix& offset) const;
305 
308  const PiecewisePolynomial operator-(const CoefficientMatrix& offset) const;
309 
311 
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
int b
Definition: rgbd_camera.cc:91
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:59
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