Polynomial spline trajectory defined in a StateSpace. More...

#include <aikido/trajectory/Spline.hpp>

Inheritance diagram for aikido::trajectory::Spline:

Classes
struct	PolynomialSegment

Public Member Functions
	Spline (statespace::ConstStateSpacePtr _stateSpace, double _startTime=0.)
	Constructs an empty trajectory. More...

virtual	~Spline ()

void	addSegment (const Eigen::MatrixXd &_coefficients, double _duration, const statespace::StateSpace::State *_startState)
	Add a segment to the end of this trajectory that starts at `_startState`, lasts for `_duration`, and is defined by a polynomial in the tangent space of `_startState`. More...

void	addSegment (const Eigen::MatrixXd &_coefficients, double _duration)
	Adds a segment to the end of the trajectory while preserving C0 continuity. More...

std::size_t	getNumSegments () const
	Gets the number of segments in this spline. More...

statespace::ConstStateSpacePtr	getStateSpace () const override
	Gets the `StateSpace` that this trajectory is defined in. More...

std::size_t	getNumDerivatives () const override
	Gets an upper bound on the number of non-zero derivatives available in this parameterization. More...

double	getStartTime () const override
	Time at which the trajectory starts. More...

double	getEndTime () const override
	Time at which the trajectory ends. More...

double	getDuration () const override
	Duration of the trajectory. More...

void	evaluate (double _t, statespace::StateSpace::State *_state) const override
	Evaluates the state of the trajectory at time `_t` and store the result in a `_state` allocated by `getStateSpace()`. More...

void	evaluateDerivative (double _t, int _derivative, Eigen::VectorXd &_tangentVector) const override
	Evaluates the derivative of the trajectory at time `_t`. More...

std::size_t	getNumWaypoints () const
	Gets the number of waypoints. More...

void	getWaypoint (std::size_t _index, statespace::StateSpace::State *state) const
	Gets a waypoint. More...

double	getWaypointTime (std::size_t _index) const
	Gets the time of a waypoint. More...

void	getWaypointDerivative (std::size_t _index, int _derivative, Eigen::VectorXd &_tangentVector) const
	Gets the derivative of a waypoint. More...

double	getSegmentDuration (std::size_t _index) const
	Gets the duration of a segment. More...

const Eigen::MatrixXd &	getSegmentCoefficients (std::size_t _index) const
	Gets the coefficients of a segment. More...

const aikido::statespace::StateSpace::State *	getSegmentStartState (std::size_t _index) const
	Gets the start state of a segment. More...

Public Member Functions inherited from aikido::trajectory::Trajectory
virtual	~Trajectory ()=default

Private Member Functions
std::pair< std::size_t, double >	getSegmentForTime (double _t) const

Static Private Member Functions
static Eigen::VectorXd	evaluatePolynomial (const Eigen::MatrixXd &_coefficients, double _t, int _derivative)

Private Attributes
statespace::ConstStateSpacePtr	mStateSpace

double	mStartTime

std::vector< PolynomialSegment >	mSegments

Additional Inherited Members
Public Attributes inherited from aikido::trajectory::Trajectory
TrajectoryMetadata	metadata
	Trajectory metadata. More...

Detailed Description

Polynomial spline trajectory defined in a StateSpace.

The trajectory is represented as a sequence of polynomial segments between knot points. Each segment is defined by a start state, a duration, and polynomial coefficients for each dimension of the StateSpace. The polynomial for a segment is parameterized in the tangent space of its start state.

It is possible to avoid singularities in expMap and logMap by starting a new segment whenever passing near a singularity. Such a parameterization always exists because Lie groups are locally Euclidean.

This trajectory does not guarantee any continuity (not even C0). It is the responsibility of the user to pass in continuous spline coefficients if continuity is desired.

Constructor & Destructor Documentation

◆ Spline()

aikido::trajectory::Spline::Spline	(	statespace::ConstStateSpacePtr	_stateSpace,
		double	_startTime = `0.`
	)

Constructs an empty trajectory.

Parameters

_stateSpace	State space this trajectory is defined in
_startTime	Start time of the trajectory

◆ ~Spline()

virtual aikido::trajectory::Spline::~Spline ( )

virtual

Member Function Documentation

◆ addSegment() [1/2]

void aikido::trajectory::Spline::addSegment	(	const Eigen::MatrixXd &	_coefficients,
		double	_duration
	)

Adds a segment to the end of the trajectory while preserving C0 continuity.

This is a helper function that calls the three-argument overload of this function with _startState set to the end of the trajectory. As a result, the trajectory must be non-empty.

Note that repeated calls to this function may accumulate numerical imprecision from repeated polynomial evaluation. We suggest calling the overload of the function that accepts an explicit start state if it is possible to do so.

Parameters

_coefficients	Polynomial coefficients
_duration	Duration of this segment, must be positive

◆ addSegment() [2/2]

void aikido::trajectory::Spline::addSegment	(	const Eigen::MatrixXd &	_coefficients,
		double	_duration,
		const statespace::StateSpace::State *	_startState
	)

Add a segment to the end of this trajectory that starts at _startState, lasts for _duration, and is defined by a polynomial in the tangent space of _startState.

The _coefficients matrix is a (num dimensions) x (num coefficients) matrix where the i-th row defines polynomial coefficients for the i-th dimension of the state space and the j-th column defines the coefficient for the j-th power of time parameter. For example, element (3, 2) defines the polynomial coefficient on t^2 for the third state space dimension.

Polynomial coefficients are defined in the tangent space of the segment's start state, given by _startState. The polynomial encoded by _coefficients will be evaluated at times [ 0, _duration ]. Segments may have heterogeneous polynomial orders, and thus, coefficient matrices with differing numbers of columns.

Note that the value of _startState is not checked for C0 continuity, nor are the derivatives evaluated at the start of the segment checked for higher-order continuity. It is the responsibility of the user to pass in continuous spline coefficients if continuity is desired.

Parameters

_coefficients	Polynomial coefficients
_duration	Duration of this segment, must be positive
_startState	Start state of the segment

◆ evaluate()

void aikido::trajectory::Spline::evaluate	(	double	_t,
		statespace::StateSpace::State *	_state
	)		const

overridevirtual

Evaluates the state of the trajectory at time _t and store the result in a _state allocated by getStateSpace().

The output of this function is implementation-defined if _t is not between getStartTime() and getEndTime().

Parameters

	_t	time parameter
[out]	_state	output state of the trajectory at time `_t`

Implements aikido::trajectory::Trajectory.

◆ evaluateDerivative()

void aikido::trajectory::Spline::evaluateDerivative	(	double	_t,
		int	_derivative,
		Eigen::VectorXd &	_tangentVector
	)		const

overridevirtual

Evaluates the derivative of the trajectory at time _t.

The _tangentVector is defined in the local frame (i.e. "body frame") and is implementation-defined if not between getStartTime() and getEndTime(). Derivatives of order higher than getNumDerivatives are guaranteed to be zero.

Parameters

	_t	time parameter
	_derivative	order of derivative
[out]	_tangentVector	output tangent vector in the local frame

Implements aikido::trajectory::Trajectory.

◆ evaluatePolynomial()

static Eigen::VectorXd aikido::trajectory::Spline::evaluatePolynomial	(	const Eigen::MatrixXd &	_coefficients,
		double	_t,
		int	_derivative
	)

staticprivate

◆ getDuration()

double aikido::trajectory::Spline::getDuration ( ) const

overridevirtual

Duration of the trajectory.

Note that getStartTime() may not be zero.

Returns: duration of the trajectory

Implements aikido::trajectory::Trajectory.

◆ getEndTime()

double aikido::trajectory::Spline::getEndTime ( ) const

overridevirtual

Time at which the trajectory ends.

This may not be getDuration() if getStartTime() is not zero.

Returns: time at which the trajectory ends

Implements aikido::trajectory::Trajectory.

◆ getNumDerivatives()

std::size_t aikido::trajectory::Spline::getNumDerivatives ( ) const

overridevirtual

Gets an upper bound on the number of non-zero derivatives available in this parameterization.

Note that evaluateDerivative may return zero before this value for some trajectories.

Returns: upper bound on the number of non-zero derivatives

Implements aikido::trajectory::Trajectory.

◆ getNumSegments()

std::size_t aikido::trajectory::Spline::getNumSegments ( ) const

Gets the number of segments in this spline.

Returns: Number of segments in this spline

◆ getNumWaypoints()

std::size_t aikido::trajectory::Spline::getNumWaypoints ( ) const

Gets the number of waypoints.

Returns: The number of waypoints

◆ getSegmentCoefficients()

const Eigen::MatrixXd& aikido::trajectory::Spline::getSegmentCoefficients ( std::size_t _index ) const

Gets the coefficients of a segment.

Parameters

_index Segment index

Returns: Coefficients of the segment at index _index

◆ getSegmentDuration()

double aikido::trajectory::Spline::getSegmentDuration ( std::size_t _index ) const

Gets the duration of a segment.

Parameters

_index Segment index

Returns: Duration of the segment at index _index

◆ getSegmentForTime()

std::pair<std::size_t, double> aikido::trajectory::Spline::getSegmentForTime ( double _t ) const

private

◆ getSegmentStartState()

const aikido::statespace::StateSpace::State* aikido::trajectory::Spline::getSegmentStartState ( std::size_t _index ) const

Gets the start state of a segment.

Parameters

_index Segment index

Returns: Start state of the segment at index _index

◆ getStartTime()

double aikido::trajectory::Spline::getStartTime ( ) const

overridevirtual

Time at which the trajectory starts.

This may not be zero.

Returns: time at which the trajectory starts

Implements aikido::trajectory::Trajectory.

◆ getStateSpace()

statespace::ConstStateSpacePtr aikido::trajectory::Spline::getStateSpace ( ) const

overridevirtual

Gets the StateSpace that this trajectory is defined in.

Returns: state space this trajectory is defined in.

Implements aikido::trajectory::Trajectory.

◆ getWaypoint()

void aikido::trajectory::Spline::getWaypoint	(	std::size_t	_index,
		statespace::StateSpace::State *	state
	)		const

Gets a waypoint.

Parameters

	_index	Waypoint index
[out]	state	State of the waypoint at index `_index`

◆ getWaypointDerivative()

void aikido::trajectory::Spline::getWaypointDerivative	(	std::size_t	_index,
		int	_derivative,
		Eigen::VectorXd &	_tangentVector
	)		const

Gets the derivative of a waypoint.

Parameters

	_index	Waypoint index
	_derivative	Order of derivative
[out]	_tangentVector	Output tangent vector in the local frame at index `_index`

◆ getWaypointTime()

double aikido::trajectory::Spline::getWaypointTime ( std::size_t _index ) const

Gets the time of a waypoint.

Parameters

_index Waypoint index

Returns: Time of the waypoint at index _index

Member Data Documentation

◆ mSegments

std::vector<PolynomialSegment> aikido::trajectory::Spline::mSegments

private

◆ mStartTime

double aikido::trajectory::Spline::mStartTime

private

◆ mStateSpace

statespace::ConstStateSpacePtr aikido::trajectory::Spline::mStateSpace

private

Classes

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ Spline()

◆ ~Spline()

Member Function Documentation

◆ addSegment() [1/2]

◆ addSegment() [2/2]

◆ evaluate()

◆ evaluateDerivative()

◆ evaluatePolynomial()

◆ getDuration()

◆ getEndTime()

◆ getNumDerivatives()

◆ getNumSegments()

◆ getNumWaypoints()

◆ getSegmentCoefficients()

◆ getSegmentDuration()

◆ getSegmentForTime()

◆ getSegmentStartState()

◆ getStartTime()

◆ getStateSpace()

◆ getWaypoint()

◆ getWaypointDerivative()

◆ getWaypointTime()

Member Data Documentation

◆ mSegments

◆ mStartTime

◆ mStateSpace