Drake

A scalar multivariate piecewise polynomial. More...
#include <drake/common/trajectories/piecewise_polynomial.h>
Public Types  
typedef Polynomial< CoefficientType >  PolynomialType 
typedef drake::MatrixX< PolynomialType >  PolynomialMatrix 
typedef drake::MatrixX< CoefficientType >  CoefficientMatrix 
typedef Eigen::Ref< CoefficientMatrix >  CoefficientMatrixRef 
Public Member Functions  
virtual  ~PiecewisePolynomial () 
PiecewisePolynomial ()  
template<typename Derived >  
PiecewisePolynomial (const Eigen::MatrixBase< Derived > &value)  
PiecewisePolynomial (std::vector< PolynomialMatrix > const &polynomials, std::vector< double > const &breaks)  
PiecewisePolynomial (std::vector< PolynomialType > const &polynomials, std::vector< double > const &breaks)  
PiecewisePolynomial  derivative (int derivative_order=1) const 
Takes the derivative of this PiecewisePolynomial. More...  
PiecewisePolynomial  integral (double value_at_start_time=0.0) const 
Takes the integral of this PiecewisePolynomial. More...  
PiecewisePolynomial  integral (const CoefficientMatrixRef &value_at_start_time) const 
Takes the integral of this PiecewisePolynomial. More...  
bool  empty () const 
double  scalarValue (double t, Eigen::Index row=0, Eigen::Index col=0) 
drake::MatrixX< double >  value (double t) const 
Evaluates the PiecewisePolynomial at the given time t . More...  
const PolynomialMatrix &  getPolynomialMatrix (int segment_index) const 
const PolynomialType &  getPolynomial (int segment_index, Eigen::Index row=0, Eigen::Index col=0) const 
int  getSegmentPolynomialDegree (int segment_index, Eigen::Index row=0, Eigen::Index col=0) const override 
Eigen::Index  rows () const override 
Eigen::Index  cols () const override 
PiecewisePolynomial &  operator+= (const PiecewisePolynomial &other) 
PiecewisePolynomial &  operator= (const PiecewisePolynomial &other) 
PiecewisePolynomial &  operator*= (const PiecewisePolynomial &other) 
PiecewisePolynomial &  operator+= (const CoefficientMatrix &offset) 
PiecewisePolynomial &  operator= (const CoefficientMatrix &offset) 
const PiecewisePolynomial  operator+ (const PiecewisePolynomial &other) const 
const PiecewisePolynomial  operator (const PiecewisePolynomial &other) const 
const PiecewisePolynomial  operator* (const PiecewisePolynomial &other) const 
const PiecewisePolynomial  operator+ (const CoefficientMatrix &offset) const 
const PiecewisePolynomial  operator (const CoefficientMatrix &offset) const 
bool  isApprox (const PiecewisePolynomial &other, double tol) const 
Checks if a PiecewisePolynomial is approximately equal to this one. More...  
void  shiftRight (double offset) 
void  setPolynomialMatrixBlock (const PolynomialMatrix &replacement, int segment_number, Eigen::Index row_start=0, Eigen::Index col_start=0) 
PiecewisePolynomial  slice (int start_segment_index, int num_segments) const 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable  
PiecewisePolynomial (const PiecewisePolynomial &)=default  
PiecewisePolynomial &  operator= (const PiecewisePolynomial &)=default 
PiecewisePolynomial (PiecewisePolynomial &&)=default  
PiecewisePolynomial &  operator= (PiecewisePolynomial &&)=default 
Public Member Functions inherited from PiecewisePolynomialBase  
PiecewisePolynomialBase (std::vector< double > const &breaks)  
virtual  ~PiecewisePolynomialBase () 
int  getNumberOfCoefficients (int segment_number, Eigen::Index row=0, Eigen::Index col=0) const 
int  getTotalNumberOfCoefficients (Eigen::Index row=0, Eigen::Index col=0) const 
Public Member Functions inherited from PiecewiseFunction  
PiecewiseFunction (std::vector< double > const &breaks)  
virtual  ~PiecewiseFunction () 
int  getNumberOfSegments () const 
double  getStartTime (int segment_number) const 
double  getEndTime (int segment_number) const 
double  getDuration (int segment_number) const 
double  getStartTime () const 
double  getEndTime () const 
bool  isTimeInRange (double t) const 
Returns true iff t >= getStartTime() && t <= getEndTime() . More...  
int  getSegmentIndex (double t) const 
const std::vector< double > &  getSegmentTimes () const 
void  segmentNumberRangeCheck (int segment_number) const 
Static Public Member Functions  
static PiecewisePolynomial< CoefficientType >  ZeroOrderHold (const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots) 
Constructs a piecewise constant PiecewisePolynomial. More...  
static PiecewisePolynomial< CoefficientType >  FirstOrderHold (const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots) 
Constructs a piecewise linear PiecewisePolynomial. More...  
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 . More...  
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 . More...  
static PiecewisePolynomial< CoefficientType >  Cubic (const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots, const std::vector< CoefficientMatrix > &knots_dot) 
Constructs a third order PiecewisePolynomial from breaks , knots and knots dot. More...  
static PiecewisePolynomial< CoefficientType >  Cubic (const std::vector< double > &breaks, const std::vector< CoefficientMatrix > &knots) 
Constructs a third order PiecewisePolynomial from breaks and knots . More...  
Static Public Member Functions inherited from PiecewiseFunction  
static std::vector< double >  randomSegmentTimes (int num_segments, std::default_random_engine &generator) 
Additional Inherited Members  
Static Public Attributes inherited from PiecewiseFunction  
static constexpr double  kEpsilonTime = 1e10 
Minimum delta quantity used for comparing time. More...  
Protected Member Functions inherited from PiecewisePolynomialBase  
PiecewisePolynomialBase ()  
Protected Member Functions inherited from PiecewiseFunction  
bool  segmentTimesEqual (const PiecewiseFunction &b, double tol=kEpsilonTime) const 
void  checkScalarValued () const 
PiecewiseFunction ()  
Protected Attributes inherited from PiecewiseFunction  
std::vector< double >  breaks 
A scalar multivariate piecewise polynomial.
PiecewisePolynomial represents a list of contiguous segments in time with a Matrix of Polynomials defined for each segment. The term segment is used for piece.
An example of a piecewise polynomial is a function of x segments in time, where for each segment a different polynomial is defined. For a more specific example, consider the absolute value function, which is a piecewise function. It uses one function for inputs values < 0, and another function for input values > 0:
PiecewisePolynomials can be added, subtracted, and multiplied. They cannot be divided because Polynomials are not closed under division.
typedef drake::MatrixX<CoefficientType> CoefficientMatrix 
typedef Eigen::Ref<CoefficientMatrix> CoefficientMatrixRef 
typedef drake::MatrixX<PolynomialType> PolynomialMatrix 
typedef Polynomial<CoefficientType> PolynomialType 

default 

default 

inlinevirtual 

inlineexplicit 
PiecewisePolynomial  (  std::vector< PolynomialMatrix > const &  polynomials, 
std::vector< double > const &  breaks  
) 
PiecewisePolynomial  (  std::vector< PolynomialType > const &  polynomials, 
std::vector< double > const &  breaks  
) 

overridevirtual 
Implements PiecewiseFunction.

static 
Constructs a third order PiecewisePolynomial from breaks
and knots
.
The PiecewisePolynomial is constructed such that the interior segments have the same value, first and second derivatives at breaks
. knot_dot_at_start
and knot_dot_at_end
are used for the first and last first derivatives.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, knots has inconsistent dimensions, knots_dot_at_start or knot_dot_at_end and knots have inconsistent dimensions, breaks has length smaller than 2. 

static 
Constructs a third order PiecewisePolynomial from breaks
, knots
and knots
dot.
Each segment is fully specified by and at both ends. Second derivatives are not continuous.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, breaks and knots dot have different length, knots has inconsistent dimensions, knots_dot and knots have inconsistent dimensions, breaks has length smaller than 2. 

static 
Constructs a third order PiecewisePolynomial from breaks
and knots
.
The PiecewisePolynomial is constructed such that the interior segments have the same value, first and second derivatives at breaks
. "Notaknot" end condition is used here, which means the third derivatives are continuous for the first two and last two segments. See https://en.wikipedia.org/wiki/Spline_interpolation for more details about "Notaknot" condition. The matlab file "spline.m" and http://home.uchicago.edu/~sctchoi/courses/cs138/interp.pdf are also good references.
breaks
and knots
must have at least 3 elements. Otherwise there is not enough information to solve for the coefficients.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, knots has inconsistent dimensions, breaks has length smaller than 3. 
PiecewisePolynomial< CoefficientType > derivative  (  int  derivative_order = 1  )  const 
Takes the derivative of this PiecewisePolynomial.
Returns a PiecewisePolynomial where each segment is the derivative of the segment in the input PiecewisePolynomial. Any rules or limitations of Polynomial::derivative also apply to this function.
If derivative_order
is given, takes the nth derivative of this PiecewisePolynomial.

inline 

static 
Constructs a piecewise linear PiecewisePolynomial.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, knots has inconsistent dimensions, breaks has length smaller than 2. 
const Polynomial< CoefficientType > & getPolynomial  (  int  segment_index, 
Eigen::Index  row = 0 , 

Eigen::Index  col = 0 

)  const 
const PiecewisePolynomial< CoefficientType >::PolynomialMatrix & getPolynomialMatrix  (  int  segment_index  )  const 

overridevirtual 
Implements PiecewisePolynomialBase.
PiecewisePolynomial< CoefficientType > integral  (  double  value_at_start_time = 0.0  )  const 
Takes the integral of this PiecewisePolynomial.
Returns a PiecewisePolynomial that is the indefinite integral of this one. Any rules or limitations of Polynomial::integral also apply to this function.
If value_at_start_time
is given, it does the following only for the first segment: adds that constant as the constant term (zerothorder coefficient) of the resulting Polynomial.
PiecewisePolynomial integral  (  const CoefficientMatrixRef &  value_at_start_time  )  const 
Takes the integral of this PiecewisePolynomial.
Returns a PiecewisePolynomial that is the indefinite integral of this one. Any rules or limitations of Polynomial::integral also apply to this function.
If value_at_start_time
is given, it does the following only for the first segment: adds value_at_start_time(row,col) as the constant term (zerothorder coefficient) of the resulting Polynomial.
bool isApprox  (  const PiecewisePolynomial< CoefficientType > &  other, 
double  tol  
)  const 
Checks if a PiecewisePolynomial is approximately equal to this one.
Checks that every coefficient of other
is within tol
of the corresponding coefficient of this PiecewisePolynomial. Throws an exception if any Polynomial in either PiecewisePolynomial is not univariate.
const PiecewisePolynomial< CoefficientType > operator*  (  const PiecewisePolynomial< CoefficientType > &  other  )  const 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
PiecewisePolynomial< CoefficientType > & operator*=  (  const PiecewisePolynomial< CoefficientType > &  other  ) 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
const PiecewisePolynomial< CoefficientType > operator+  (  const PiecewisePolynomial< CoefficientType > &  other  )  const 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
const PiecewisePolynomial operator+  (  const CoefficientMatrix &  offset  )  const 
std::runtime_error  if offset.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
PiecewisePolynomial< CoefficientType > & operator+=  (  const PiecewisePolynomial< CoefficientType > &  other  ) 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
PiecewisePolynomial& operator+=  (  const CoefficientMatrix &  offset  ) 
std::runtime_error  if offset.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
const PiecewisePolynomial< CoefficientType > operator  (  const PiecewisePolynomial< CoefficientType > &  other  )  const 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
const PiecewisePolynomial operator  (  const CoefficientMatrix &  offset  )  const 
std::runtime_error  if offset.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
PiecewisePolynomial< CoefficientType > & operator=  (  const PiecewisePolynomial< CoefficientType > &  other  ) 
std::runtime_error  if other.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 
PiecewisePolynomial& operator=  (  const CoefficientMatrix &  offset  ) 
std::runtime_error  if offset.segment_times is not within PiecewiseFunction::kEpsilonTime from this>segment_times. 

default 

default 

static 
Constructs a third order PiecewisePolynomial from breaks
and knots
.
First derivatives are chosen to be "shape preserving", i.e. if knots
is monotonic within some interval, the interpolated data will also be monotonic. The second derivative is not guaranteed to be smooth across the entire spline. Pchip stands for "Piecewise Cubic Hermite Interpolating Polynomial". For more details, refer to the matlab file "pchip.m". http://home.uchicago.edu/~sctchoi/courses/cs138/interp.pdf is also a good reference.
If zero_end_point_derivatives
is false, the first and last first derivative is chosen using a noncentered, shapepreserving threepoint formulae. See equation (2.10) in the following reference for more details. http://www.mi.sanu.ac.rs/~gvm/radovi/mon.pdf If zero_end_point_derivatives
is true, they are set to zeros.
If zero_end_point_derivatives
is false, breaks
and knots
must have at least 3 elements for the algorithm to determine the first derivatives.
If zero_end_point_derivatives
is true, breaks
and knots
may have 2 or more elements. For the 2 elements case, the result is equivalent to computing a cubic polynomial whose values are given by knots
, and derivatives set to zero.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, knots has inconsistent dimensions, breaks has length smaller than 3 and zero_end_point_derivatives is false, breaks has length smaller than 2 and zero_end_point_derivatives is true. 

overridevirtual 
void setPolynomialMatrixBlock  (  const PolynomialMatrix &  replacement, 
int  segment_number,  
Eigen::Index  row_start = 0 , 

Eigen::Index  col_start = 0 

) 
void shiftRight  (  double  offset  ) 
PiecewisePolynomial< CoefficientType > slice  (  int  start_segment_index, 
int  num_segments  
)  const 
drake::MatrixX< double > value  (  double  t  )  const 
Evaluates the PiecewisePolynomial at the given time t
.
t  The time at which to evaluate the PiecewisePolynomial. 

static 
Constructs a piecewise constant PiecewisePolynomial.
Note that constructing a PiecewisePolynomial requires at least two knot points, although in this case, the second knot point's value is ignored, and only its break time is used.
std::runtime_error  if breaks and knots have different length, breaks is not strictly increasing, knots has inconsistent dimensions, breaks has length smaller than 2. 