Drake
Drake C++ Documentation
BsplineTrajectory< T > Class Template Referencefinal

Detailed Description

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

Represents a B-spline curve using a given basis with ordered control_points such that each control point is a matrix in ℝʳᵒʷˢ ˣ ᶜᵒˡˢ.

See also
math::BsplineBasis
Template Parameters
TThe scalar type, which must be one of the default scalars.

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

Public Member Functions

 BsplineTrajectory ()
 
 BsplineTrajectory (math::BsplineBasis< T > basis, std::vector< MatrixX< T >> control_points)
 Constructs a B-spline trajectory with the given basis and control_points. More...
 
 BsplineTrajectory (math::BsplineBasis< double > basis, std::vector< MatrixX< T >> control_points)
 Constructs a T-valued B-spline trajectory from a double-valued basis and T-valued control_points. More...
 
 ~BsplineTrajectory () final
 
MatrixX< T > value (const T &t) const final
 Evaluates the BsplineTrajectory at the given time t. More...
 
Eigen::SparseMatrix< T > AsLinearInControlPoints (int derivative_order=1) const
 Supports writing optimizations using the control points as decision variables. More...
 
VectorX< T > EvaluateLinearInControlPoints (const T &t, int derivative_order=0) const
 Returns the vector, M, such that. More...
 
int num_control_points () const
 Returns the number of control points in this curve. More...
 
const std::vector< MatrixX< T > > & control_points () const
 Returns the control points of this curve. More...
 
MatrixX< T > InitialValue () const
 Returns this->value(this->start_time()) More...
 
MatrixX< T > FinalValue () const
 Returns this->value(this->end_time()) More...
 
const math::BsplineBasis< T > & basis () const
 Returns the basis of this curve. More...
 
void InsertKnots (const std::vector< T > &additional_knots)
 Adds new knots at the specified additional_knots without changing the behavior of the trajectory. More...
 
BsplineTrajectory< T > CopyWithSelector (const std::function< MatrixX< T >(const MatrixX< T > &)> &select) const
 Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling select(point) on each point in this->control_points(). More...
 
BsplineTrajectory< T > CopyBlock (int start_row, int start_col, int block_rows, int block_cols) const
 Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.block(start_row, start_col, block_rows, block_cols) on each point in this->control_points(). More...
 
BsplineTrajectory< T > CopyHead (int n) const
 Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.head(n) on each point in this->control_points(). More...
 
boolean< T > operator== (const BsplineTrajectory< T > &other) const
 
template<typename Archive >
void Serialize (Archive *a)
 Passes this object to an Archive. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 BsplineTrajectory (const BsplineTrajectory &)=default
 
BsplineTrajectoryoperator= (const BsplineTrajectory &)=default
 
 BsplineTrajectory (BsplineTrajectory &&)=default
 
BsplineTrajectoryoperator= (BsplineTrajectory &&)=default
 
- Public Member Functions inherited from Trajectory< T >
virtual ~Trajectory ()
 
virtual std::unique_ptr< Trajectory< T > > Clone () const
 
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...
 
virtual Eigen::Index rows () const
 
virtual Eigen::Index cols () const
 
virtual T start_time () const
 
virtual T end_time () const
 

Additional Inherited Members

- Protected Member Functions inherited from Trajectory< T >
 Trajectory ()=default
 
 Trajectory (const Trajectory &)=default
 
Trajectoryoperator= (const Trajectory &)=default
 
 Trajectory (Trajectory &&)=default
 
Trajectoryoperator= (Trajectory &&)=default
 

Constructor & Destructor Documentation

◆ BsplineTrajectory() [1/5]

BsplineTrajectory ( const BsplineTrajectory< T > &  )
default

◆ BsplineTrajectory() [2/5]

BsplineTrajectory ( BsplineTrajectory< T > &&  )
default

◆ BsplineTrajectory() [3/5]

◆ BsplineTrajectory() [4/5]

BsplineTrajectory ( math::BsplineBasis< T >  basis,
std::vector< MatrixX< T >>  control_points 
)

Constructs a B-spline trajectory with the given basis and control_points.

Precondition
control_points.size() == basis.num_basis_functions()

◆ BsplineTrajectory() [5/5]

BsplineTrajectory ( math::BsplineBasis< double basis,
std::vector< MatrixX< T >>  control_points 
)

Constructs a T-valued B-spline trajectory from a double-valued basis and T-valued control_points.

Precondition
control_points.size() == basis.num_basis_functions()

◆ ~BsplineTrajectory()

~BsplineTrajectory ( )
final

Member Function Documentation

◆ AsLinearInControlPoints()

Eigen::SparseMatrix<T> AsLinearInControlPoints ( int  derivative_order = 1) const

Supports writing optimizations using the control points as decision variables.

This method returns the matrix, M, defining the control points of the order derivative in the form:

derivative.control_points() = this.control_points() * M

See BezierCurve::AsLinearInControlPoints() for more details.

Precondition
derivative_order >= 0.

◆ basis()

const math::BsplineBasis<T>& basis ( ) const

Returns the basis of this curve.

◆ control_points()

const std::vector<MatrixX<T> >& control_points ( ) const

Returns the control points of this curve.

◆ CopyBlock()

BsplineTrajectory<T> CopyBlock ( int  start_row,
int  start_col,
int  block_rows,
int  block_cols 
) const

Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.block(start_row, start_col, block_rows, block_cols) on each point in this->control_points().

◆ CopyHead()

BsplineTrajectory<T> CopyHead ( int  n) const

Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling point.head(n) on each point in this->control_points().

Precondition
this->cols() == 1
control_points()[0].head(n) must be a valid operation.

◆ CopyWithSelector()

BsplineTrajectory<T> CopyWithSelector ( const std::function< MatrixX< T >(const MatrixX< T > &)> &  select) const

Returns a new BsplineTrajectory that uses the same basis as this, and whose control points are the result of calling select(point) on each point in this->control_points().

◆ EvaluateLinearInControlPoints()

VectorX<T> EvaluateLinearInControlPoints ( const T &  t,
int  derivative_order = 0 
) const

Returns the vector, M, such that.

EvalDerivative(t, derivative_order) = control_points() * M

where cols()==1 (so control_points() is a matrix). This is useful for writing linear constraints on the control points. Note that if the derivative order is greater than or equal to the order of the basis, then the result is a zero vector.

Precondition
t ≥ start_time()
t ≤ end_time()

◆ FinalValue()

MatrixX<T> FinalValue ( ) const

Returns this->value(this->end_time())

◆ InitialValue()

MatrixX<T> InitialValue ( ) const

Returns this->value(this->start_time())

◆ InsertKnots()

void InsertKnots ( const std::vector< T > &  additional_knots)

Adds new knots at the specified additional_knots without changing the behavior of the trajectory.

The basis and control points of the trajectory are adjusted such that it produces the same value for any valid time before and after this method is called. The resulting trajectory is guaranteed to have the same level of continuity as the original, even if knot values are duplicated. Note that additional_knots need not be sorted.

Precondition
start_time() <= t <= end_time() for all t in additional_knots

◆ num_control_points()

int num_control_points ( ) const

Returns the number of control points in this curve.

◆ operator=() [1/2]

BsplineTrajectory& operator= ( BsplineTrajectory< T > &&  )
default

◆ operator=() [2/2]

BsplineTrajectory& operator= ( const BsplineTrajectory< T > &  )
default

◆ operator==()

boolean<T> operator== ( const BsplineTrajectory< T > &  other) const

◆ 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.

◆ value()

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

Evaluates the BsplineTrajectory at the given time t.

Parameters
tThe time at which to evaluate the BsplineTrajectory.
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 [start_time(), end_time()], the trajectory will silently be evaluated at the closest valid value of time to t. For example, value(-1) will return value(0) for a trajectory defined over [0, 1].

Reimplemented from Trajectory< T >.


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