Detailed Description

template<typename T>
class drake::trajectories::PiecewisePolynomial< T >

A scalar multi-variate piecewise polynomial.

PiecewisePolynomial represents a list of contiguous segments in a scalar independent variable (typically corresponding to time) with Polynomials defined at each segment. We call the output from evaluating the PiecewisePolynomial at the scalar independent variable "the output", and that output can be either a Eigen MatrixX<T> (if evaluated using value()) or a scalar (if evaluated using scalar_value()).

An example of a piecewise polynomial is a function of m segments in time, where a different polynomial is defined for each segment. For a specific example, consider the absolute value function over the interval [-1, 1]. We can define a PiecewisePolynomial over this interval using breaks at t = { -1.0, 0.0, 1.0 }, and "samples" of abs(t).

// Construct the PiecewisePolynomial.
const std::vector<double> breaks = { -1.0, 0.0, 1.0 };
std::vector<Eigen::MatrixXd> samples(3);
for (int i = 0; i < static_cast<int>(breaks.size()); ++i) {
  samples[i].resize(1, 1);
  samples[i](0, 0) = std::abs(breaks[i]);
}
const auto pp =
     PiecewisePolynomial<double>::FirstOrderHold(breaks, samples);
const int row = 0, col = 0;
// Evaluate the PiecewisePolynomial at some values.
std::cout << pp.value(-.5)(row, col) << std::endl;    // Outputs 0.5.
std::cout << pp.value(0.0)(row, col) << std::endl;    // Outputs 0.0;
// Show how we can evaluate the first derivative (outputs -1.0).
std::cout << pp.derivative(1).value(-.5)(row, col) << std::endl;

A note on terminology. For piecewise-polynomial interpolation, we use breaks to indicate the scalar (e.g. times) which form the boundary of each segment. We use samples to indicate the function value at the breaks, e.g. p(breaks[i]) = samples[i]. The term knot should be reserved for the "(x,y)" coordinate, here knot[i] = (breaks[i], samples[i]), though it is used inconsistently in the interpolation literature (sometimes for breaks, sometimes for samples), so we try to mostly avoid it here.

PiecewisePolynomial objects can be added, subtracted, and multiplied. They cannot be divided because Polynomials are not closed under division.

Warning: PiecewisePolynomial silently clips input evaluations outside of the range defined by the breaks. So pp.value(-2.0, row, col) in the example above would evaluate to -1.0. See value().

Template Parameters

T	The scalar type, which must be one of the default scalars.

#include <drake/common/trajectories/piecewise_polynomial.h>

Public Types
typedef MatrixX< Polynomial< T > >	PolynomialMatrix

Public Member Functions
	PiecewisePolynomial ()=default
	Constructs an empty piecewise polynomial. More...

template<typename Derived >
	PiecewisePolynomial (const Eigen::MatrixBase< Derived > &constant_value)
	Single segment, constant value constructor over the interval [-∞, ∞]. More...

	~PiecewisePolynomial () override

std::unique_ptr< Trajectory< T > >	Clone () const override

PiecewisePolynomial< T >	derivative (int derivative_order=1) const
	Returns a PiecewisePolynomial where each segment is the specified derivative of the corresponding segment in `this`. More...

PiecewisePolynomial< T >	integral (const T &value_at_start_time=0.0) const
	Returns a PiecewisePolynomial that is the indefinite integral of this one. More...

PiecewisePolynomial< T >	integral (const Eigen::Ref< MatrixX< T >> &value_at_start_time) const
	Returns a PiecewisePolynomial that is the indefinite integral of this one. More...

bool	empty () const
	Returns `true` if this trajectory has no breaks/samples/polynomials. More...

T	scalarValue (const T &t, Eigen::Index row=0, Eigen::Index col=0) const
	Evaluates the trajectory at the given time without returning the entire matrix. More...

MatrixX< T >	value (const T &t) const override
	Evaluates the PiecewisePolynomial at the given time t. More...

const PolynomialMatrix &	getPolynomialMatrix (int segment_index) const
	Gets the matrix of Polynomials corresponding to the given segment index. More...

const Polynomial< T > &	getPolynomial (int segment_index, Eigen::Index row=0, Eigen::Index col=0) const
	Gets the Polynomial with the given matrix row and column index that corresponds to the given segment index. More...

int	getSegmentPolynomialDegree (int segment_index, Eigen::Index row=0, Eigen::Index col=0) const
	Gets the degree of the Polynomial with the given matrix row and column index that corresponds to the given segment index. More...

Eigen::Index	rows () const override
	Returns the row count of the output matrices. More...

Eigen::Index	cols () const override
	Returns the column count of the output matrices. More...

void	Reshape (int rows, int cols)
	Reshapes the dimensions of the Eigen::MatrixX<T> returned by value(), EvalDerivative(), etc. More...

PiecewisePolynomial	Transpose () const
	Constructs a new PiecewisePolynomial for which value(t) == this.value(t).transpose(). More...

PiecewisePolynomial	Block (int start_row, int start_col, int block_rows, int block_cols) const
	Extracts a trajectory representing a block of size (block_rows, block_cols) starting at (start_row, start_col) from the PiecewisePolynomial. More...

PiecewisePolynomial &	operator+= (const PiecewisePolynomial &other)
	Adds each Polynomial in the PolynomialMatrix of `other` to the corresponding Polynomial in the PolynomialMatrix of `this`, storing the result in `this`. More...

PiecewisePolynomial &	operator-= (const PiecewisePolynomial &other)
	Subtracts each Polynomial in the PolynomialMatrix of `other` from the corresponding Polynomial in the PolynomialMatrix of `this`, storing the result in `this`. More...

PiecewisePolynomial &	operator *= (const PiecewisePolynomial &other)
	Multiplies each Polynomial in the PolynomialMatrix of `other` by the corresponding Polynomial in the PolynomialMatrix of `this` (i.e., a coefficient-wise multiplication), storing the result in `this`. More...

PiecewisePolynomial &	operator+= (const MatrixX< T > &coeff)

PiecewisePolynomial &	operator-= (const MatrixX< T > &coeff)

const PiecewisePolynomial	operator+ (const PiecewisePolynomial &other) const
	Adds each Polynomial in the PolynomialMatrix of `other` to the corresponding Polynomial in the PolynomialMatrix of `this`. More...

const PiecewisePolynomial	operator- (const PiecewisePolynomial &other) const
	Subtracts each Polynomial in the PolynomialMatrix of `other` from the corresponding Polynomial in the PolynomialMatrix of `this`. More...

const PiecewisePolynomial	operator- () const
	Implements unary minus operator. More...

const PiecewisePolynomial	operator * (const PiecewisePolynomial &other) const
	Multiplies each Polynomial in the PolynomialMatrix of `other` by the corresponding Polynomial in the PolynomialMatrix of `this` (i.e., a coefficient-wise multiplication). More...

const PiecewisePolynomial	operator+ (const MatrixX< T > &coeff) const

const PiecewisePolynomial	operator- (const MatrixX< T > &coeff) const

bool	isApprox (const PiecewisePolynomial &other, double tol, const ToleranceType &tol_type=ToleranceType::kRelative) const
	Checks whether a PiecewisePolynomial is approximately equal to this one by calling Polynomial<T>::CoefficientsAlmostEqual() on every element of every segment. More...

void	ConcatenateInTime (const PiecewisePolynomial &other)
	Concatenates `other` to the end of `this`. More...

void	AppendCubicHermiteSegment (const T &time, const Eigen::Ref< const MatrixX< T >> &sample, const Eigen::Ref< const MatrixX< T >> &sample_dot)
	The CubicHermite spline construction has a nice property of being incremental (each segment can be solved independently). More...

void	AppendFirstOrderSegment (const T &time, const Eigen::Ref< const MatrixX< T >> &sample)
	Given a new sample, this method adds one segment to the end of `this` using a first-order hold, where the start sample is taken as the value at the final break of `this`. More...

void	RemoveFinalSegment ()
	Removes the final segment from the trajectory, reducing the number of segments by 1. More...

void	ReverseTime ()
	Modifies the trajectory so that pp_after(t) = pp_before(-t). More...

void	ScaleTime (const T &scale)
	Scales the time of the trajectory by non-negative `scale` (use ReverseTime() if you want to also negate time). More...

void	shiftRight (const T &offset)
	Adds `offset` to all of the breaks. More...

int	AddBreak (const T &new_break)
	Adds a break at the specified time. More...

PiecewisePolynomial	SliceByTime (const T &start_time, const T &end_time) const
	Slices the trajectory within a specified time range. More...

void	setPolynomialMatrixBlock (const PolynomialMatrix &replacement, int segment_index, Eigen::Index row_start=0, Eigen::Index col_start=0)
	Replaces the specified block of the PolynomialMatrix at the given segment index. More...

PiecewisePolynomial	slice (int start_segment_index, int num_segments) const
	Returns the PiecewisePolynomial comprising the `num_segments` segments starting at the specified `start_segment_index`. More...

template<typename Archive >
void	Serialize (Archive *a)
	Passes this object to an Archive. More...

Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
	PiecewisePolynomial (const PiecewisePolynomial &)=default

PiecewisePolynomial &	operator= (const PiecewisePolynomial &)=default

	PiecewisePolynomial (PiecewisePolynomial &&)=default

PiecewisePolynomial &	operator= (PiecewisePolynomial &&)=default

Polynomial-based construction methods.
Various methods for constructing a PiecewisePolynomial using vectors of matrices of polynomials, one for each output dimension. Unlike the coefficient-based methods, the number of polynomials must equal the number of segments, which will be one fewer than the number of breaks. The following shows how such a PiecewisePolynomial might be constructed and used: // Construct the PiecewisePolynomial. const std::vector<double> breaks = { -1.0, 0.0, 1.0 }; Polynomiald t("t"); std::vector<Polynomiald> polynomials = { -(tt), (tt) }; const PiecewisePolynomial<double> pp(polynomials, breaks); // Evaluate the PiecewisePolynomial at some values. std::cout << pp.scalar_value(-1.0) << std::endl; // Outputs -1.0 std::cout << pp.scalar_value(1.0) << std::endl; // Outputs 1.0 WARNING: For robust floating point arithmetic, the polynomial for a segment will be evaluated (using value()) by first subtracting the break time from the evaluation time. In other words, when t lies in the half-open interval `[breaks[i], breaks[i+1])` then: value(t) == polynomials[i].eval(t - breaks[i]) meaning that constructing the polynomial like: const std::vector<double> breaks = { 0.0, 1.0, 2.0 }; Polynomiald t("t"); std::vector<Polynomiald> polynomials = { (tt), (tt) }; const PiecewisePolynomial<double> pp(polynomials, breaks); would give the following result: // Evaluate the PiecewisePolynomial on both sides of a break. const int row = 0, col = 0; const double eps = 0.5 * std::numeric_limits<double>::epsilon(); std::cout << pp.value(1.0-eps)(row, col) << std::endl; // Outputs 1.0 std::cout << pp.value(1.0+eps)(row, col) << std::endl; // Outputs 1e-32 because the second polynomial will be evaluated at 1.0+eps minus the break time for that polynomial (1.0), i.e., t=eps. The intended result for the above example can be obtained by shifting the piecewise polynomial like so: const std::vector<double> breaks = { 0.0, 1.0, 2.0 }; Polynomiald t("t"); std::vector<Polynomiald> polynomials = { (tt), ((t+breaks[1])(t+breaks[1])) }; const PiecewisePolynomial<double> pp(polynomials, breaks); // Evaluate the PiecewisePolynomial on both sides of a break. const double eps = 0.5 * std::numeric_limits<double>::epsilon(); std::cout << pp.value(1.0-eps)(row, col) << std::endl; // Outputs 1.0 std::cout << pp.value(1.0+eps)(row, col) << std::endl; // Outputs 1.0
	PiecewisePolynomial (const std::vector< PolynomialMatrix > &polynomials_matrix, const std::vector< T > &breaks)
	Constructs a PiecewisePolynomial using matrix-output Polynomials defined over each segment. More...

	PiecewisePolynomial (const std::vector< Polynomial< T >> &polynomials, const std::vector< T > &breaks)
	Constructs a PiecewisePolynomial using scalar-output Polynomials defined over each segment. More...

Public Member Functions inherited from PiecewiseTrajectory< T >
	~PiecewiseTrajectory () override

int	get_number_of_segments () const

T	start_time (int segment_number) const

T	end_time (int segment_number) const

T	duration (int segment_number) const

T	start_time () const override

T	end_time () const override

boolean< T >	is_time_in_range (const T &t) const
	Returns true iff `t >= getStartTime() && t <= getEndTime()`. More...

int	get_segment_index (const T &t) const

const std::vector< T > &	get_segment_times () const

void	segment_number_range_check (int segment_number) const

Public Member Functions inherited from Trajectory< T >
virtual	~Trajectory ()

MatrixX< T >	vector_values (const std::vector< T > &t) const
	If cols()==1, then evaluates the trajectory at each time `t`, and returns the results as a Matrix with the ith column corresponding to the ith time. More...

MatrixX< T >	vector_values (const Eigen::Ref< const VectorX< T >> &t) const
	If cols()==1, then evaluates the trajectory at each time `t`, and returns the results as a Matrix with the ith column corresponding to the ith time. More...

bool	has_derivative () const
	Returns true iff the Trajectory provides and implementation for EvalDerivative() and MakeDerivative(). More...

MatrixX< T >	EvalDerivative (const T &t, int derivative_order=1) const
	Evaluates the derivative of `this` at the given time `t`. More...

std::unique_ptr< Trajectory< T > >	MakeDerivative (int derivative_order=1) const
	Takes the derivative of this Trajectory. More...

Static Public Member Functions
Coefficient-based construction methods.
Various methods for constructing a PiecewisePolynomial using samples of coefficient matrices. Under the hood, PiecewisePolynomial constructs interpolating Polynomial objects that pass through the sample points. These methods differ by the continuity constraints that they enforce at break points and whether each sample represents a full matrix (versions taking `const std::vector<MatrixX<T>>&`) or a column vector (versions taking `const Eigen::Ref<const MatrixX<T>>&`). These methods will throw `std::exception` if: the breaks and samples have different length, the breaks are not strictly increasing, the samples have inconsistent dimensions (i.e., the matrices do not all have identical dimensions), the breaks vector has length smaller than 2.
static PiecewisePolynomial< T >	ZeroOrderHold (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples)
	Constructs a piecewise constant PiecewisePolynomial using matrix samples. More...

static PiecewisePolynomial< T >	ZeroOrderHold (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples)
	Version of ZeroOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd/MatrixX<T> arguments. More...

static PiecewisePolynomial< T >	FirstOrderHold (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples)
	Constructs a piecewise linear PiecewisePolynomial using matrix samples. More...

static PiecewisePolynomial< T >	FirstOrderHold (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples)
	Version of FirstOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. More...

static PiecewisePolynomial< T >	CubicShapePreserving (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples, bool zero_end_point_derivatives=false)
	Constructs a third order PiecewisePolynomial using vector samples, where each column of `samples` represents a sample point. More...

static PiecewisePolynomial< T >	CubicShapePreserving (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples, bool zero_end_point_derivatives=false)
	Version of CubicShapePreserving(breaks, samples, zero_end_point_derivatives) that uses vector samples and Eigen VectorXd and MatrixX<T> arguments. More...

static PiecewisePolynomial< T >	CubicWithContinuousSecondDerivatives (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples, const MatrixX< T > &sample_dot_at_start, const MatrixX< T > &sample_dot_at_end)
	Constructs a third order PiecewisePolynomial using matrix samples. More...

static PiecewisePolynomial< T >	CubicWithContinuousSecondDerivatives (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples, const Eigen::Ref< const VectorX< T >> &sample_dot_at_start, const Eigen::Ref< const VectorX< T >> &sample_dot_at_end)
	Version of CubicWithContinuousSecondDerivatives() that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. More...

static PiecewisePolynomial< T >	CubicHermite (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples, const std::vector< MatrixX< T >> &samples_dot)
	Constructs a third order PiecewisePolynomial using matrix samples and derivatives of samples (`samples_dot`); each matrix element of `samples_dot` represents the derivative with respect to the independent variable (e.g., the time derivative) of the corresponding entry in `samples`. More...

static PiecewisePolynomial< T >	CubicHermite (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples, const Eigen::Ref< const MatrixX< T >> &samples_dot)
	Version of CubicHermite(breaks, samples, samples_dot) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. More...

static PiecewisePolynomial< T >	CubicWithContinuousSecondDerivatives (const std::vector< T > &breaks, const std::vector< MatrixX< T >> &samples, bool periodic_end_condition=false)
	Constructs a third order PiecewisePolynomial using matrix samples. More...

static PiecewisePolynomial< T >	CubicWithContinuousSecondDerivatives (const Eigen::Ref< const VectorX< T >> &breaks, const Eigen::Ref< const MatrixX< T >> &samples, bool periodic_end_condition=false)
	Version of CubicWithContinuousSecondDerivatives(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. More...

static PiecewisePolynomial	LagrangeInterpolatingPolynomial (const std::vector< T > &times, const std::vector< MatrixX< T >> &samples)
	Constructs a polynomial with a single segment of the lowest possible degree that passes through all of the sample points. More...

static PiecewisePolynomial< T >	LagrangeInterpolatingPolynomial (const Eigen::Ref< const VectorX< T >> &times, const Eigen::Ref< const MatrixX< T >> &samples)
	Version of LagrangeInterpolatingPolynomial(times, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments. More...

Static Public Member Functions inherited from PiecewiseTrajectory< T >
static std::vector< T >	RandomSegmentTimes (int num_segments, std::default_random_engine &generator)

Additional Inherited Members
Static Public Attributes inherited from PiecewiseTrajectory< T >
static constexpr double	kEpsilonTime = std::numeric_limits<double>::epsilon()
	Minimum delta quantity used for comparing time. More...

Protected Member Functions inherited from PiecewiseTrajectory< T >
	PiecewiseTrajectory ()=default

	PiecewiseTrajectory (const std::vector< T > &breaks)
	`breaks` increments must be greater or equal to kEpsilonTime. More...

bool	SegmentTimesEqual (const PiecewiseTrajectory &b, double tol=kEpsilonTime) const

const std::vector< T > &	breaks () const

std::vector< T > &	get_mutable_breaks ()

	PiecewiseTrajectory (const PiecewiseTrajectory &)=default

PiecewiseTrajectory &	operator= (const PiecewiseTrajectory &)=default

	PiecewiseTrajectory (PiecewiseTrajectory &&)=default

PiecewiseTrajectory &	operator= (PiecewiseTrajectory &&)=default

Protected Member Functions inherited from Trajectory< T >
	Trajectory ()=default

	Trajectory (const Trajectory &)=default

Trajectory &	operator= (const Trajectory &)=default

	Trajectory (Trajectory &&)=default

Trajectory &	operator= (Trajectory &&)=default

Member Typedef Documentation

◆ PolynomialMatrix

typedef MatrixX<Polynomial<T> > PolynomialMatrix

Constructor & Destructor Documentation

◆ PiecewisePolynomial() [1/6]

PiecewisePolynomial ( )

default

Constructs an empty piecewise polynomial.

◆ PiecewisePolynomial() [2/6]

PiecewisePolynomial ( const PiecewisePolynomial< T > & )

default

◆ PiecewisePolynomial() [3/6]

PiecewisePolynomial ( PiecewisePolynomial< T > && )

default

◆ PiecewisePolynomial() [4/6]

PiecewisePolynomial ( const Eigen::MatrixBase< Derived > & constant_value )

explicit

Single segment, constant value constructor over the interval [-∞, ∞].

The constructed PiecewisePolynomial will return constant_value at every evaluated point (i.e., value(t) = constant_value ∀t ∈ [-∞, ∞]).

◆ PiecewisePolynomial() [5/6]

PiecewisePolynomial	(	const std::vector< PolynomialMatrix > &	polynomials_matrix,
		const std::vector< T > &	breaks
	)

Constructs a PiecewisePolynomial using matrix-output Polynomials defined over each segment.

Precondition: polynomials.size() == breaks.size() - 1

◆ PiecewisePolynomial() [6/6]

PiecewisePolynomial	(	const std::vector< Polynomial< T >> &	polynomials,
		const std::vector< T > &	breaks
	)

Constructs a PiecewisePolynomial using scalar-output Polynomials defined over each segment.

Precondition: polynomials.size() == breaks.size() - 1

◆ ~PiecewisePolynomial()

~PiecewisePolynomial ( )

override

Member Function Documentation

◆ AddBreak()

int AddBreak ( const T & new_break )

Adds a break at the specified time.

It does not change the value of the trajectory at any point but the number of segments increases by 1.

Returns: the index of the new break.

Exceptions

std::exception if new_break is not within the trajectory's time range.

Warning: If new_break is within PiecewiseTrajectory::kEpsilonTime from an existing break, the new break will be silently ignored. Returns the index of the existing break.

◆ AppendCubicHermiteSegment()

void AppendCubicHermiteSegment	(	const T &	time,
		const Eigen::Ref< const MatrixX< T >> &	sample,
		const Eigen::Ref< const MatrixX< T >> &	sample_dot
	)

The CubicHermite spline construction has a nice property of being incremental (each segment can be solved independently).

Given a new sample and it's derivative, this method adds one segment to the end of this where the start sample and derivative are taken as the value and derivative at the final break of this.

Precondition: this is not empty(); time > end_time(); sample and sample_dot must have size rows() x cols().

◆ AppendFirstOrderSegment()

void AppendFirstOrderSegment	(	const T &	time,
		const Eigen::Ref< const MatrixX< T >> &	sample
	)

Given a new sample, this method adds one segment to the end of this using a first-order hold, where the start sample is taken as the value at the final break of this.

◆ Block()

PiecewisePolynomial Block	(	int	start_row,
		int	start_col,
		int	block_rows,
		int	block_cols
	)		const

Extracts a trajectory representing a block of size (block_rows, block_cols) starting at (start_row, start_col) from the PiecewisePolynomial.

Returns: a PiecewisePolynomial such that ret.value(t) = this.value(t).block(i,j,p,q);

◆ Clone()

std::unique_ptr<Trajectory<T> > Clone ( ) const

overridevirtual

Returns: A deep copy of this Trajectory.

Implements Trajectory< T >.

◆ cols()

Eigen::Index cols ( ) const

overridevirtual

Returns the column count of the output matrices.

Exceptions

std::exception if empty().

Implements Trajectory< T >.

◆ ConcatenateInTime()

void ConcatenateInTime ( const PiecewisePolynomial< T > & other )

Concatenates other to the end of this.

Warning: The resulting PiecewisePolynomial will only be continuous to the degree that the first Polynomial of other is continuous with the last Polynomial of this. See warning about evaluating discontinuous derivatives at breaks in derivative().

Parameters

other PiecewisePolynomial instance to concatenate.

Exceptions

std::exception	if trajectories' dimensions do not match each other (either rows() or cols() does not match between this and `other`).
std::exception	if `this->end_time()` and `other->start_time()` are not within PiecewiseTrajectory<T>::kEpsilonTime from each other.

◆ CubicHermite() [1/2]

static PiecewisePolynomial<T> CubicHermite	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples,
		const std::vector< MatrixX< T >> &	samples_dot
	)

static

Constructs a third order PiecewisePolynomial using matrix samples and derivatives of samples (samples_dot); each matrix element of samples_dot represents the derivative with respect to the independent variable (e.g., the time derivative) of the corresponding entry in samples.

Each segment is fully specified by samples and sample_dot at both ends. Second derivatives are not continuous.

◆ CubicHermite() [2/2]

static PiecewisePolynomial<T> CubicHermite	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples,
		const Eigen::Ref< const MatrixX< T >> &	samples_dot
	)

static

Version of CubicHermite(breaks, samples, samples_dot) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments.

Corresponding columns of samples and samples_dot are used as the sample point and independent variable derivative, respectively.

Precondition: samples.cols() == samples_dot.cols() == breaks.size().

◆ CubicShapePreserving() [1/2]

static PiecewisePolynomial<T> CubicShapePreserving	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples,
		bool	zero_end_point_derivatives = `false`
	)

static

Constructs a third order PiecewisePolynomial using vector samples, where each column of samples represents a sample point.

First derivatives are chosen to be "shape preserving", i.e. if samples 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.

MATLAB calls this method "pchip" (short for "Piecewise Cubic Hermite Interpolating Polynomial"), and provides a nice description in their documentation. 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 non-centered, shape-preserving three-point 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 samples must have at least 3 elements for the algorithm to determine the first derivatives.

If zero_end_point_derivatives is true, breaks and samples 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 samples, and derivatives set to zero.

Exceptions

std::exception	if: `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.
std::exception	under the conditions specified under coefficient_construction_methods.

◆ CubicShapePreserving() [2/2]

static PiecewisePolynomial<T> CubicShapePreserving	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples,
		bool	zero_end_point_derivatives = `false`
	)

static

Version of CubicShapePreserving(breaks, samples, zero_end_point_derivatives) that uses vector samples and Eigen VectorXd and MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == breaks.size().

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

◆ CubicWithContinuousSecondDerivatives() [1/4]

static PiecewisePolynomial<T> CubicWithContinuousSecondDerivatives	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples,
		const MatrixX< T > &	sample_dot_at_start,
		const MatrixX< T > &	sample_dot_at_end
	)

static

Constructs a third order PiecewisePolynomial using matrix samples.

The PiecewisePolynomial is constructed such that the interior segments have the same value, first and second derivatives at breaks. sample_dot_at_start and sample_dot_at_end are used for the first and last first derivatives.

Exceptions

std::exception	if `sample_dot_at_start` or `sample_dot_at_end` and `samples` have inconsistent dimensions.
std::exception	under the conditions specified under coefficient_construction_methods.

◆ CubicWithContinuousSecondDerivatives() [2/4]

static PiecewisePolynomial<T> CubicWithContinuousSecondDerivatives	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples,
		const Eigen::Ref< const VectorX< T >> &	sample_dot_at_start,
		const Eigen::Ref< const VectorX< T >> &	sample_dot_at_end
	)

static

Version of CubicWithContinuousSecondDerivatives() that uses vector samples and Eigen VectorXd / MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == breaks.size().

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

◆ CubicWithContinuousSecondDerivatives() [3/4]

static PiecewisePolynomial<T> CubicWithContinuousSecondDerivatives	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples,
		bool	periodic_end_condition = `false`
	)

static

Constructs a third order PiecewisePolynomial using matrix samples.

The PiecewisePolynomial is constructed such that the interior segments have the same value, first and second derivatives at breaks. If periodic_end_condition is false (default), then the "Not-a-sample" end condition is used here, which means the third derivatives are continuous for the first two and last two segments. If periodic_end_condition is true, then the first and second derivatives between the end of the last segment and the beginning of the first segment will be continuous. Note that the periodic end condition does not require the first and last sample to be collocated, nor does it add an additional sample to connect the first and last segments. Only first and second derivative continuity is enforced. See https://en.wikipedia.org/wiki/Spline_interpolation and https://web.archive.org/web/20140125011904/https://www.math.uh.edu/~jingqiu/math4364/spline.pdf for more about cubic splines and their end conditions. The MATLAB docs for methods "spline" and "csape" are also good references.

Precondition: breaks and samples must have at least 3 elements. If periodic_end_condition is true, then for two samples, it would produce a straight line (use FirstOrderHold for this instead), and if periodic_end_condition is false the problem is ill-defined.

◆ CubicWithContinuousSecondDerivatives() [4/4]

static PiecewisePolynomial<T> CubicWithContinuousSecondDerivatives	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples,
		bool	periodic_end_condition = `false`
	)

static

Version of CubicWithContinuousSecondDerivatives(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == breaks.size().

◆ derivative()

PiecewisePolynomial<T> derivative ( int derivative_order = 1 ) const

Returns a PiecewisePolynomial where each segment is the specified derivative of the corresponding segment in this.

Any rules or limitations of Polynomial::derivative() also apply to this function.

Derivatives evaluated at non-differentiable points return the value at the left hand side of the interval.

Parameters

derivative_order The order of the derivative, namely, if derivative_order = n, the n'th derivative of the polynomial will be returned.

Warning: In the event of discontinuous derivatives evaluated at breaks, it is not defined which polynomial (i.e., to the left or right of the break) will be the one that is evaluated at the break.

◆ empty()

bool empty ( ) const

Returns true if this trajectory has no breaks/samples/polynomials.

◆ FirstOrderHold() [1/2]

static PiecewisePolynomial<T> FirstOrderHold	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples
	)

static

Constructs a piecewise linear PiecewisePolynomial using matrix samples.

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

◆ FirstOrderHold() [2/2]

static PiecewisePolynomial<T> FirstOrderHold	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples
	)

static

Version of FirstOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == breaks.size()

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

◆ getPolynomial()

const Polynomial<T>& getPolynomial	(	int	segment_index,
		Eigen::Index	row = `0`,
		Eigen::Index	col = `0`
	)		const

Gets the Polynomial with the given matrix row and column index that corresponds to the given segment index.

Equivalent to getPolynomialMatrix(segment_index)(row, col).

Note: Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

◆ getPolynomialMatrix()

const PolynomialMatrix& getPolynomialMatrix ( int segment_index ) const

Gets the matrix of Polynomials corresponding to the given segment index.

Warning: segment_index is not checked for validity.

◆ getSegmentPolynomialDegree()

int getSegmentPolynomialDegree	(	int	segment_index,
		Eigen::Index	row = `0`,
		Eigen::Index	col = `0`
	)		const

Gets the degree of the Polynomial with the given matrix row and column index that corresponds to the given segment index.

Equivalent to getPolynomial(segment_index, row, col).GetDegree().

◆ integral() [1/2]

PiecewisePolynomial<T> integral ( const T & value_at_start_time = 0.0 ) const

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 (zeroth-order coefficient) of the resulting Polynomial.

◆ integral() [2/2]

PiecewisePolynomial<T> integral ( const Eigen::Ref< MatrixX< T >> & value_at_start_time ) const

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 (zeroth-order coefficient) of the resulting Polynomial.

◆ isApprox()

bool isApprox	(	const PiecewisePolynomial< T > &	other,
		double	tol,
		const ToleranceType &	tol_type = `ToleranceType::kRelative`
	)		const

Checks whether a PiecewisePolynomial is approximately equal to this one by calling Polynomial<T>::CoefficientsAlmostEqual() on every element of every segment.

See also: Polynomial<T>::CoefficientsAlmostEqual().

◆ LagrangeInterpolatingPolynomial() [1/2]

static PiecewisePolynomial LagrangeInterpolatingPolynomial	(	const std::vector< T > &	times,
		const std::vector< MatrixX< T >> &	samples
	)

static

Constructs a polynomial with a single segment of the lowest possible degree that passes through all of the sample points.

See "polynomial interpolation" and/or "Lagrange polynomial" on Wikipedia for more information.

Precondition: times must be monotonically increasing.; samples.size() == times.size().

◆ LagrangeInterpolatingPolynomial() [2/2]

static PiecewisePolynomial<T> LagrangeInterpolatingPolynomial	(	const Eigen::Ref< const VectorX< T >> &	times,
		const Eigen::Ref< const MatrixX< T >> &	samples
	)

static

Version of LagrangeInterpolatingPolynomial(times, samples) that uses vector samples and Eigen VectorXd / MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == times.size().

◆ operator *()

const PiecewisePolynomial operator * ( const PiecewisePolynomial< T > & other ) const

Multiplies each Polynomial in the PolynomialMatrix of other by the corresponding Polynomial in the PolynomialMatrix of this (i.e., a coefficient-wise multiplication).

If this corresponds to t² and other corresponds to t³, this *= other will correspond to t⁵.

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times()1.

◆ operator *=()

PiecewisePolynomial& operator *= ( const PiecewisePolynomial< T > & other )

Multiplies each Polynomial in the PolynomialMatrix of other by the corresponding Polynomial in the PolynomialMatrix of this (i.e., a coefficient-wise multiplication), storing the result in this.

If this corresponds to t² and other corresponds to t³, this *= other will correspond to t⁵.

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times().

◆ operator+() [1/2]

const PiecewisePolynomial operator+ ( const PiecewisePolynomial< T > & other ) const

Adds each Polynomial in the PolynomialMatrix of other to the corresponding Polynomial in the PolynomialMatrix of this.

If this corresponds to t² and other corresponds to t³, this + other will correspond to t³ + t².

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times().

◆ operator+() [2/2]

const PiecewisePolynomial operator+ ( const MatrixX< T > & coeff ) const

◆ operator+=() [1/2]

PiecewisePolynomial& operator+= ( const PiecewisePolynomial< T > & other )

Adds each Polynomial in the PolynomialMatrix of other to the corresponding Polynomial in the PolynomialMatrix of this, storing the result in this.

If this corresponds to t² and other corresponds to t³, this += other will correspond to t³ + t².

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times().

◆ operator+=() [2/2]

PiecewisePolynomial& operator+= ( const MatrixX< T > & coeff )

◆ operator-() [1/3]

const PiecewisePolynomial operator- ( const PiecewisePolynomial< T > & other ) const

Subtracts each Polynomial in the PolynomialMatrix of other from the corresponding Polynomial in the PolynomialMatrix of this.

If this corresponds to t² and other corresponds to t³, this - other will correspond to t² - t³.

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times().

◆ operator-() [2/3]

const PiecewisePolynomial operator- ( ) const

Implements unary minus operator.

Multiplies each Polynomial in this by -1.

◆ operator-() [3/3]

const PiecewisePolynomial operator- ( const MatrixX< T > & coeff ) const

◆ operator-=() [1/2]

PiecewisePolynomial& operator-= ( const PiecewisePolynomial< T > & other )

Subtracts each Polynomial in the PolynomialMatrix of other from the corresponding Polynomial in the PolynomialMatrix of this, storing the result in this.

If this corresponds to t² and other corresponds to t³, this -= other will correspond to t² - t³.

Exceptions

std::exception if every element of other.get_segment_times() is not within PiecewiseTrajectory::kEpsilonTime from `this->get_segment_times().

◆ operator-=() [2/2]

PiecewisePolynomial& operator-= ( const MatrixX< T > & coeff )

◆ operator=() [1/2]

PiecewisePolynomial& operator= ( PiecewisePolynomial< T > && )

default

◆ operator=() [2/2]

PiecewisePolynomial& operator= ( const PiecewisePolynomial< T > & )

default

◆ RemoveFinalSegment()

void RemoveFinalSegment ( )

Removes the final segment from the trajectory, reducing the number of segments by 1.

Precondition: this is not empty()

◆ Reshape()

void Reshape	(	int	rows,
		int	cols
	)

Reshapes the dimensions of the Eigen::MatrixX<T> returned by value(), EvalDerivative(), etc.

Precondition: rows x cols must equal this.rows() * this.cols().

See also: Eigen::PlainObjectBase::resize().

◆ ReverseTime()

void ReverseTime ( )

Modifies the trajectory so that pp_after(t) = pp_before(-t).

Note: The new trajectory will evaluate differently at precisely the break points if the original trajectory was discontinuous at the break points. This is because the segments are defined on the half-open intervals [breaks(i), breaks(i+1)), and the order of the breaks have been reversed.

◆ rows()

Eigen::Index rows ( ) const

overridevirtual

Returns the row count of the output matrices.

Exceptions

std::exception if empty().

Implements Trajectory< T >.

◆ scalarValue()

T scalarValue	(	const T &	t,
		Eigen::Index	row = `0`,
		Eigen::Index	col = `0`
	)		const

Evaluates the trajectory at the given time without returning the entire matrix.

Equivalent to value(t)(row, col).

Warning: See warnings in value().

◆ ScaleTime()

void ScaleTime ( const T & scale )

Scales the time of the trajectory by non-negative scale (use ReverseTime() if you want to also negate time).

The resulting polynomial evaluates to pp_after(t) = pp_before(t/scale).

As an example, scale=2 will result in a trajectory that is twice as long (start_time() and end_time() have both doubled).

◆ Serialize()

void Serialize ( Archive * a )

Passes this object to an Archive.

Refer to YAML Serialization for background. This method is only available when T = double.

◆ setPolynomialMatrixBlock()

void setPolynomialMatrixBlock	(	const PolynomialMatrix &	replacement,
		int	segment_index,
		Eigen::Index	row_start = `0`,
		Eigen::Index	col_start = `0`
	)

Replaces the specified block of the PolynomialMatrix at the given segment index.

Note: Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

Warning: This code relies upon Eigen to verify that the replacement block is not too large.

◆ shiftRight()

void shiftRight ( const T & offset )

Adds offset to all of the breaks.

offset need not be a non-negative number. The resulting polynomial will evaluate to pp_after(t) = pp_before(t-offset).

As an example, offset=2 will result in the start_time() and end_time() being 2 seconds later.

◆ slice()

PiecewisePolynomial slice	(	int	start_segment_index,
		int	num_segments
	)		const

Returns the PiecewisePolynomial comprising the num_segments segments starting at the specified start_segment_index.

Note: Calls PiecewiseTrajectory<T>::segment_number_range_check() to validate segment_index.

◆ SliceByTime()

PiecewisePolynomial SliceByTime	(	const T &	start_time,
		const T &	end_time
	)		const

Slices the trajectory within a specified time range.

q = p.SliceByTime(t1, t2) returns a PiecewisePolynomial q such that q.start_time() = t1, q.end_time() = t2, and q(t) = p(t) for t1 <= t <= t2.

Exceptions

std::exception if start_time or end_time is not within the trajectory's time range.

◆ Transpose()

PiecewisePolynomial Transpose ( ) const

Constructs a new PiecewisePolynomial for which value(t) == this.value(t).transpose().

◆ value()

MatrixX<T> value ( const T & t ) const

overridevirtual

Evaluates the PiecewisePolynomial at the given time t.

Parameters

t	The time at which to evaluate the PiecewisePolynomial.

Returns: The matrix of evaluated values.

Precondition: If T == symbolic::Expression, t.is_constant() must be true.

Warning: If t does not lie in the range that the polynomial is defined over, the polynomial will silently be evaluated at the closest point to t. For example, value(-1) will return value(0) for a polynomial defined over [0, 1].; See warning in the constructor overview above.

Implements Trajectory< T >.

◆ ZeroOrderHold() [1/2]

static PiecewisePolynomial<T> ZeroOrderHold	(	const std::vector< T > &	breaks,
		const std::vector< MatrixX< T >> &	samples
	)

static

Constructs a piecewise constant PiecewisePolynomial using matrix samples.

Note that constructing a PiecewisePolynomial requires at least two sample points, although in this case, the second sample point's value is ignored, and only its break time is used.

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

◆ ZeroOrderHold() [2/2]

static PiecewisePolynomial<T> ZeroOrderHold	(	const Eigen::Ref< const VectorX< T >> &	breaks,
		const Eigen::Ref< const MatrixX< T >> &	samples
	)

static

Version of ZeroOrderHold(breaks, samples) that uses vector samples and Eigen VectorXd/MatrixX<T> arguments.

Each column of samples represents a sample point.

Precondition: samples.cols() == breaks.size()

Exceptions

std::exception under the conditions specified under coefficient_construction_methods.

The documentation for this class was generated from the following file:

drake/common/trajectories/piecewise_polynomial.h

Detailed Description

template<typename T> class drake::trajectories::PiecewisePolynomial< T >

Public Types

Public Member Functions

Static Public Member Functions

Additional Inherited Members

Member Typedef Documentation

◆ PolynomialMatrix

Constructor & Destructor Documentation

◆ PiecewisePolynomial() [1/6]

◆ PiecewisePolynomial() [2/6]

◆ PiecewisePolynomial() [3/6]

◆ PiecewisePolynomial() [4/6]

◆ PiecewisePolynomial() [5/6]

◆ PiecewisePolynomial() [6/6]

◆ ~PiecewisePolynomial()

Member Function Documentation

◆ AddBreak()

◆ AppendCubicHermiteSegment()

◆ AppendFirstOrderSegment()

◆ Block()

◆ Clone()

◆ cols()

◆ ConcatenateInTime()

◆ CubicHermite() [1/2]

◆ CubicHermite() [2/2]

◆ CubicShapePreserving() [1/2]

◆ CubicShapePreserving() [2/2]

◆ CubicWithContinuousSecondDerivatives() [1/4]

◆ CubicWithContinuousSecondDerivatives() [2/4]

◆ CubicWithContinuousSecondDerivatives() [3/4]

◆ CubicWithContinuousSecondDerivatives() [4/4]

◆ derivative()

◆ empty()

◆ FirstOrderHold() [1/2]

◆ FirstOrderHold() [2/2]

◆ getPolynomial()

◆ getPolynomialMatrix()

◆ getSegmentPolynomialDegree()

◆ integral() [1/2]

◆ integral() [2/2]

◆ isApprox()

◆ LagrangeInterpolatingPolynomial() [1/2]

◆ LagrangeInterpolatingPolynomial() [2/2]

◆ operator *()

◆ operator *=()

◆ operator+() [1/2]

◆ operator+() [2/2]

◆ operator+=() [1/2]

◆ operator+=() [2/2]

◆ operator-() [1/3]

◆ operator-() [2/3]

◆ operator-() [3/3]

◆ operator-=() [1/2]

◆ operator-=() [2/2]

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ RemoveFinalSegment()

◆ Reshape()

◆ ReverseTime()

◆ rows()

◆ scalarValue()

◆ ScaleTime()

◆ Serialize()

◆ setPolynomialMatrixBlock()

◆ shiftRight()

◆ slice()

◆ SliceByTime()

◆ Transpose()

◆ value()

◆ ZeroOrderHold() [1/2]

◆ ZeroOrderHold() [2/2]

template<typename T>
class drake::trajectories::PiecewisePolynomial< T >