B-spline trajectory define in a StateSpace. More...

#include <aikido/trajectory/BSpline.hpp>

Inheritance diagram for aikido::trajectory::BSpline:

Public Types
using	SplineType = common::BSpline< double, 1, Eigen::Dynamic >

using	KnotVectorType = typename SplineType::KnotVectorType

using	ControlPointVectorType = typename SplineType::ControlPointVectorType

Public Member Functions
	BSpline (statespace::ConstStateSpacePtr stateSpace, const KnotVectorType &knots, const ControlPointVectorType &controlPoints)
	Constructs BSpline from control points and knots. More...

	BSpline (statespace::ConstStateSpacePtr stateSpace, std::size_t degree, const ControlPointVectorType &controlPoints, double startTime=0.0, double endTime=1.0)
	Constructs BSpline from control points where the knots are uniformly distributed. More...

	BSpline (statespace::ConstStateSpacePtr stateSpace, std::size_t degree, std::size_t numControlPoints, double startTime=0.0, double endTime=1.0)
	Constructs BSpline from the degree and the number of control points, which are all zeros. More...

	BSpline (const BSpline &other)
	Copy constructor. More...

	BSpline (BSpline &&other)
	Move constructor. More...

	~BSpline () override
	Destructor. More...

BSpline &	operator= (const BSpline &other)
	Copy assignment operator. More...

BSpline &	operator= (BSpline &&other)
	Move assignment operator. More...

std::unique_ptr< trajectory::Trajectory >	clone () const
	Returns a clone of this BSpline. More...

std::size_t	getDegree () const
	Returns degree of the spline. Degree is order - 1. More...

std::size_t	getOrder () const
	Returns order of the spline. Order is degree + 1. More...

std::size_t	getNumKnots () const
	Returns the number of knots. More...

std::size_t	getNumControlPoints () const
	Returns the number of control points. More...

void	setStartPoint (std::size_t stateSpaceIndex, double value)
	Sets start point of one sub space of the state space. More...

void	setStartPoint (const Eigen::VectorXd &point)
	Sets start point. More...

void	setStartPoint (const statespace::StateSpace::State *state)
	Sets start point. More...

void	setEndPoint (std::size_t stateSpaceIndex, double value)
	Sets end point of one sub space of the state space. More...

void	setEndPoint (const Eigen::VectorXd &point)
	Sets end point. More...

void	setEndPoint (const statespace::StateSpace::State *state)
	Sets end point. More...

void	setControlPoints (std::size_t stateSpaceIndex, const ControlPointVectorType &controlPoints, bool withStartControlPoint=true, bool withEndControlPoint=true)
	Sets all the control points of one subspace of the state space. More...

void	setControlPoints (std::size_t stateSpaceIndex, double value, bool withStartControlPoint=true, bool withEndControlPoint=true)
	Sets one control point of one subspace of the state space. More...

const ControlPointVectorType &	getControlPoints (std::size_t stateSpaceIndex) const
	Returns control points of one subspace of the state space. More...

ControlPointVectorType	getControlPoints (std::size_t stateSpaceIndex, bool withStartControlPoint, bool withEndControlPoint) const
	Returns control points of one subspace of the state space. 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...

virtual double	computeArcLength (const distance::DistanceMetric &distanceMetric, double resolution=0.1) const
	Computes arc length. More...

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

Static Protected Member Functions
static std::size_t	computeNumKnots (std::size_t degree, std::size_t numControlPoints)
	Computes number of knots given degree and number of control points. More...

static KnotVectorType	computeUniformKnots (std::size_t degree, std::size_t numControlPoints, double startTime=0.0, double endTime=1.0)
	Computes uniform knots. More...

Protected Attributes
statespace::ConstStateSpacePtr	mStateSpace
	State space. More...

std::vector< SplineType >	mSplines
	One-demensional splines. More...

double	mStartTime
	Start time of b-spline. More...

double	mEndTime
	End time of b-spline. More...

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

Detailed Description

B-spline trajectory define in a StateSpace.

Member Typedef Documentation

◆ ControlPointVectorType

using aikido::trajectory::BSpline::ControlPointVectorType = typename SplineType::ControlPointVectorType

◆ KnotVectorType

using aikido::trajectory::BSpline::KnotVectorType = typename SplineType::KnotVectorType

◆ SplineType

using aikido::trajectory::BSpline::SplineType = common::BSpline<double, 1, Eigen::Dynamic>

Constructor & Destructor Documentation

◆ BSpline() [1/5]

aikido::trajectory::BSpline::BSpline	(	statespace::ConstStateSpacePtr	stateSpace,
		const KnotVectorType &	knots,
		const ControlPointVectorType &	controlPoints
	)

Constructs BSpline from control points and knots.

The degree is automatically to be (num_knots - num_control_points - 1).

Parameters

[in]	stateSpace	State space this trajectory is defined in. Throw an exception if null.
[in]	knots	Knots. The number of knots should be the same with the control points. The values of knots should be monotonically increased. Otherwise, it's undefined behavior.
[in]	controlPoints	Control points. The number of control points should be the same with the control points.

Exceptions

If	`stateSpace` is null.
If	the size of `knots` is less than three.
If	the size of `controlPoints` is less than one.
If	the degree (size of `knots` - size of `controlPoints` - 1) is less than zero.

◆ BSpline() [2/5]

aikido::trajectory::BSpline::BSpline	(	statespace::ConstStateSpacePtr	stateSpace,
		std::size_t	degree,
		const ControlPointVectorType &	controlPoints,
		double	startTime = `0.0`,
		double	endTime = `1.0`
	)

Constructs BSpline from control points where the knots are uniformly distributed.

The start and end point are the same with the first and last control points, respectively.

Parameters

[in]	stateSpace	State space this trajectory is defined in.
[in]	degree	Degree of the b-spline.
[in]	controlPoints	Control points. The number of control points should greater than degree. Otherwise, throw an exception.
[in]	startTime	Start value of the time parameter. This will be the first knot value.
[in]	endTime	End value of the time parameter. This will be the last knot value.

Exceptions

If	`stateSpace` is null.
If	the number of control points is equal to or less than the degree.

◆ BSpline() [3/5]

aikido::trajectory::BSpline::BSpline	(	statespace::ConstStateSpacePtr	stateSpace,
		std::size_t	degree,
		std::size_t	numControlPoints,
		double	startTime = `0.0`,
		double	endTime = `1.0`
	)

Constructs BSpline from the degree and the number of control points, which are all zeros.

Parameters

[in]	stateSpace	State space this trajectory is defined in. Throw an exception if null.
[in]	startTime	Start time of the trajectory. This will be the first knot value.
[in]	endTime	End value of the time parameter. This will be the last knot value.

Exceptions

If	`stateSpace` is null.
If	the number of control points is equal to or less than the degree.

◆ BSpline() [4/5]

aikido::trajectory::BSpline::BSpline ( const BSpline & other )

Copy constructor.

Parameters

[in] other The other BSpline being copied.

◆ BSpline() [5/5]

aikido::trajectory::BSpline::BSpline ( BSpline && other )

Move constructor.

Parameters

[in] other The other BSpline being moved.

◆ ~BSpline()

aikido::trajectory::BSpline::~BSpline ( )

override

Destructor.

Member Function Documentation

◆ clone()

std::unique_ptr<trajectory::Trajectory> aikido::trajectory::BSpline::clone ( ) const

Returns a clone of this BSpline.

◆ computeArcLength()

virtual double aikido::trajectory::BSpline::computeArcLength	(	const distance::DistanceMetric &	distanceMetric,
		double	resolution = `0.1`
	)		const

virtual

Computes arc length.

Parameters

[in]	distanceMetric	Distance metric to measure the arc length.
[in]	resolution	Size of the line segments.

◆ computeNumKnots()

static std::size_t aikido::trajectory::BSpline::computeNumKnots	(	std::size_t	degree,
		std::size_t	numControlPoints
	)

staticprotected

Computes number of knots given degree and number of control points.

◆ computeUniformKnots()

static KnotVectorType aikido::trajectory::BSpline::computeUniformKnots	(	std::size_t	degree,
		std::size_t	numControlPoints,
		double	startTime = `0.0`,
		double	endTime = `1.0`
	)

staticprotected

Computes uniform knots.

Parameters

[in]	degree	Degree of the b-spline
[in]	numControlPoints	Number of the control points.
[in]	startTime	Start time of the knots.
[in]	startTime	End time of the knots.

Exceptions

If	the number of control points is equal to or less than the degree.

◆ evaluate()

void aikido::trajectory::BSpline::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::BSpline::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.

◆ getControlPoints() [1/2]

const ControlPointVectorType& aikido::trajectory::BSpline::getControlPoints ( std::size_t stateSpaceIndex ) const

Returns control points of one subspace of the state space.

Parameters

[in] stateSpaceIndex The index to the state space to set the start point.

Returns: Control points.

◆ getControlPoints() [2/2]

ControlPointVectorType aikido::trajectory::BSpline::getControlPoints	(	std::size_t	stateSpaceIndex,
		bool	withStartControlPoint,
		bool	withEndControlPoint
	)		const

Returns control points of one subspace of the state space.

Parameters

[in]	stateSpaceIndex	The index to the state space to set the start point.
[in]	withStartControlPoint	Whether to get the start control point.
[in]	withEndControlPoint	Whether to get the end control point.

Returns: Control points.

◆ getDegree()

std::size_t aikido::trajectory::BSpline::getDegree ( ) const

Returns degree of the spline. Degree is order - 1.

◆ getDuration()

double aikido::trajectory::BSpline::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::BSpline::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.

◆ getNumControlPoints()

std::size_t aikido::trajectory::BSpline::getNumControlPoints ( ) const

Returns the number of control points.

◆ getNumDerivatives()

std::size_t aikido::trajectory::BSpline::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.

◆ getNumKnots()

std::size_t aikido::trajectory::BSpline::getNumKnots ( ) const

Returns the number of knots.

◆ getOrder()

std::size_t aikido::trajectory::BSpline::getOrder ( ) const

Returns order of the spline. Order is degree + 1.

◆ getStartTime()

double aikido::trajectory::BSpline::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::BSpline::getStateSpace ( ) const

overridevirtual

Gets the StateSpace that this trajectory is defined in.

Returns: state space this trajectory is defined in.

Implements aikido::trajectory::Trajectory.

◆ operator=() [1/2]

BSpline& aikido::trajectory::BSpline::operator= ( BSpline && other )

Move assignment operator.

Parameters

[in] other The other BSpline being moved.

◆ operator=() [2/2]

BSpline& aikido::trajectory::BSpline::operator= ( const BSpline & other )

Copy assignment operator.

Parameters

[in] other The other BSpline being copied.

◆ setControlPoints() [1/2]

void aikido::trajectory::BSpline::setControlPoints	(	std::size_t	stateSpaceIndex,
		const ControlPointVectorType &	controlPoints,
		bool	withStartControlPoint = `true`,
		bool	withEndControlPoint = `true`
	)

Sets all the control points of one subspace of the state space.

Parameters

[in]	stateSpaceIndex	The index to the state space to set the start point.
[in]	controlPoints	Control points. The size of the control points can be different depending on `withStartControlPoints` and `withStartControlPoints`. Decrease the number by one per one option is true.
[in]	withStartControlPoint	Whether to set the start control point.
[in]	withEndControlPoint	Whether to set the end control point.

◆ setControlPoints() [2/2]

void aikido::trajectory::BSpline::setControlPoints	(	std::size_t	stateSpaceIndex,
		double	value,
		bool	withStartControlPoint = `true`,
		bool	withEndControlPoint = `true`
	)

Sets one control point of one subspace of the state space.

Parameters

[in]	stateSpaceIndex	The index to the state space to set the start point.
[in]	value	Control point value.
[in]	withStartControlPoint	Whether to set the start control point.
[in]	withEndControlPoint	Whether to set the end control point.

◆ setEndPoint() [1/3]

void aikido::trajectory::BSpline::setEndPoint ( const Eigen::VectorXd & point )

Sets end point.

Identical to setting the last control point. setEndPoint(stateSpace->logMap(state)).

Parameters

[in] point End point on the tangent space.

◆ setEndPoint() [2/3]

void aikido::trajectory::BSpline::setEndPoint ( const statespace::StateSpace::State * state )

Sets end point.

Identical to setting the last control point.

Parameters

[in] state End point.

◆ setEndPoint() [3/3]

void aikido::trajectory::BSpline::setEndPoint	(	std::size_t	stateSpaceIndex,
		double	value
	)

Sets end point of one sub space of the state space.

Identical to setting the last control point.

Parameters

[in] stateSpaceIndex The index to the state space to set the end point.

◆ setStartPoint() [1/3]

void aikido::trajectory::BSpline::setStartPoint ( const Eigen::VectorXd & point )

Sets start point.

Identical to setting the first control point. setStartPoint(stateSpace->logMap(state)).

Parameters

[in] point Start point on the tangent space.

◆ setStartPoint() [2/3]

void aikido::trajectory::BSpline::setStartPoint ( const statespace::StateSpace::State * state )

Sets start point.

Identical to setting the first control point.

Parameters

[in] state Start point.

◆ setStartPoint() [3/3]

void aikido::trajectory::BSpline::setStartPoint	(	std::size_t	stateSpaceIndex,
		double	value
	)

Sets start point of one sub space of the state space.

Identical to setting the first control point.

Parameters

[in] stateSpaceIndex The index to the state space to set the start point.

Member Data Documentation

◆ mEndTime

double aikido::trajectory::BSpline::mEndTime

protected

End time of b-spline.

◆ mSplines

std::vector<SplineType> aikido::trajectory::BSpline::mSplines

protected

One-demensional splines.

◆ mStartTime

double aikido::trajectory::BSpline::mStartTime

protected

Start time of b-spline.

◆ mStateSpace

statespace::ConstStateSpacePtr aikido::trajectory::BSpline::mStateSpace

protected

State space.

Public Types

Public Member Functions

Static Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ ControlPointVectorType

◆ KnotVectorType

◆ SplineType

Constructor & Destructor Documentation

◆ BSpline() [1/5]

◆ BSpline() [2/5]

◆ BSpline() [3/5]

◆ BSpline() [4/5]

◆ BSpline() [5/5]

◆ ~BSpline()

Member Function Documentation

◆ clone()

◆ computeArcLength()

◆ computeNumKnots()

◆ computeUniformKnots()

◆ evaluate()

◆ evaluateDerivative()

◆ getControlPoints() [1/2]

◆ getControlPoints() [2/2]

◆ getDegree()

◆ getDuration()

◆ getEndTime()

◆ getNumControlPoints()

◆ getNumDerivatives()

◆ getNumKnots()

◆ getOrder()

◆ getStartTime()

◆ getStateSpace()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ setControlPoints() [1/2]

◆ setControlPoints() [2/2]

◆ setEndPoint() [1/3]

◆ setEndPoint() [2/3]

◆ setEndPoint() [3/3]

◆ setStartPoint() [1/3]

◆ setStartPoint() [2/3]

◆ setStartPoint() [3/3]

Member Data Documentation

◆ mEndTime

◆ mSplines

◆ mStartTime

◆ mStateSpace