#include <RoboticsPlugin/Include/ImFusion/Robotics/MotionPlanning/Polynomial.h>

Class representing a piecewise polynomial constructed using Lagrange polynomials at Gauss-Lobatto points. More...

Inheritance diagram for Polynomial:

Detailed Description

Class representing a piecewise polynomial constructed using Lagrange polynomials at Gauss-Lobatto points.

This class represents a piecewise polynomial where each segment is defined using Lagrange interpolation at Gauss-Lobatto points. Internally, it is stored as a vector of size R^{numberOfGLPoints * dimension * numberOfIntervals}.

Each entry in this vector serves a dual purpose:

It is a coefficient of the corresponding Lagrange polynomial.
It directly represents the value of a given coordinate of the polynomial at the Gauss-Lobatto points within a given interval. Given:
- dim: the dimension of the polynomial,
- nglp: the number of Gauss-Lobatto points per interval,
- N: the number of intervals,
The value of the i-th coordinate at the k-th Gauss-Lobatto point in the j-th interval can be accessed via:

pol.data()[((j * nglp + k) * dim) + i]

Conceptually, the layout of the data vector is:
v_0^0

...

v_{nglp-1}^0

----

v_0^1

...

v_{nglp-1}^1

----

...

v_{nglp-1}^{N-1}

Important Note on Boundary Points

Gauss-Lobatto points include the endpoints of the interval. As a result, the last GL point of interval j is the same as the first GL point of interval j+1.

That is:

The shared boundary point between two adjacent intervals will appear twice: once as the last point of interval j, and again as the first point of interval j+1.

This design choice preserves the natural mapping to the internal storage layout and is especially useful when per-interval operations or boundary-specific treatments are needed.

Remark: This implementation does not support dynamically adding points to the curve represented by the polynomial. This design decision stems from our strategy to separate the representation of variables from the problem formulation.

In our context, the polynomial itself represents the variable, while the problem formulation should be handled elsewhere. The formulation of trajectory or path planning problems is significantly more complex than the mere definition of a polynomial, which justifies this separation.

If there is a need to add points or concatenate polynomials, we provide alternative functionalities designed specifically for that purpose. These operations may involve moving memory or allocating a new polynomial when necessary.

Classes
class	citerator
	Constant Iterator for traversing the values of a piecewise polynomial at Gauss-Lobatto points across all intervals. More...

class	iterator
	Iterator for traversing the values of a piecewise polynomial at Gauss-Lobatto points across all intervals. More...

Public Member Functions
	Polynomial (const IntervalPartitionXd &interval, std::size_t nglp, std::size_t codomDim)
	Constructs a zero-valued Polynomial in with domain specified by the interval partition, and a given number of gauss-lobatto points and a given dimension of the codomain.

	Polynomial (std::size_t nIntervals, std::size_t nglp, std::size_t codomDim)
	Constructs a Polynomial in with domain [0, nIntervals], with nIntervals subintervals with unit length, and a given number of gauss-lobatto points and a given dimension of the codomain.

	Polynomial (const Polynomial &other)

	Polynomial (Polynomial &&other) noexcept

Polynomial &	operator= (const Polynomial &other)

	~Polynomial ()=default
	Explicit destructor: handles the pimpl of IntervalPartitionXd and CollocationData.

const Interval &	interval () const
	Returns a reference to the interal of the domain.

IntervalPartitionXd &	intervalPartition ()
	Returns a reference to the interal partition.

const IntervalPartitionXd &	intervalPartition () const
	Returns a const reference to the interal partition.

Eigen::VectorXd	intervalPartitionAndValues () const

bool	value (double input, Eigen::Ref< Eigen::VectorXd > _output) const
	Computes the value for the map.

std::optional< Eigen::VectorXd >	value (double input) const

std::optional< Eigen::VectorXd >	diff (double input) const

bool	valueAndDiff (double input, Eigen::Ref< Eigen::VectorXd > output, Eigen::Ref< Eigen::VectorXd > diff) const
	Computes the value and the deriviatve of the map.

std::size_t	numberOfPoints () const
	Returns the number of points that constitute the Gaus-Lobatto polynomial.

std::size_t	numberOfGLPoints () const

std::size_t	degree () const

std::optional< Eigen::Ref< const Eigen::VectorXd > >	point (std::size_t i) const

std::optional< Eigen::Ref< Eigen::VectorXd > >	point (std::size_t i)
	Returns a reference to the point i (with repeated points)

Eigen::Ref< const Eigen::VectorXd >	finalPoint () const
	Returns a const reference to the point i (with repeated points)

Eigen::Ref< Eigen::VectorXd >	finalPoint ()
	Returns a reference to the point i (with repeated points)

Eigen::Ref< const Eigen::VectorXd >	initialPoint () const
	Returns a const reference to the point i (with repeated points)

Eigen::Ref< Eigen::VectorXd >	initialPoint ()
	Returns a reference to the point i (with repeated points)

std::optional< Eigen::Map< const Eigen::VectorXd, Eigen::Unaligned, Eigen::Stride< 1, Eigen::Dynamic > > >	glPointsAtIntervalOfCoordinate (std::size_t interval, std::size_t coordinate) const

std::optional< Eigen::Map< Eigen::VectorXd, Eigen::Unaligned, Eigen::Stride< 1, Eigen::Dynamic > > >	glPointsAtIntervalOfCoordinate (std::size_t interval, std::size_t coordinate)

std::vector< Eigen::VectorXd >	waypoints () const

std::optional< Eigen::Ref< const Eigen::VectorXd > >	leftJunctionPoint (std::size_t i) const
	Returns return the right end of the segment i.

std::optional< Eigen::Ref< Eigen::VectorXd > >	leftJunctionPoint (std::size_t i)
	Returns return the right end of the segment i.

std::optional< Eigen::Ref< const Eigen::VectorXd > >	rightJunctionPoint (std::size_t i) const
	Returns return the left end of the segment i+1.

std::optional< Eigen::Ref< Eigen::VectorXd > >	rightJunctionPoint (std::size_t i)
	Returns return the left end of the segment i+1.

std::optional< Eigen::Ref< const Eigen::VectorXd > >	pointInInterval (std::size_t i, std::size_t interval) const
	Returns a const reference to the point i of the interval interval (with repeated points)

std::optional< Eigen::Ref< Eigen::VectorXd > >	pointInInterval (std::size_t i, std::size_t interval)
	Returns a reference to the point i of the interval interval (with repeated points)

std::optional< Eigen::Ref< const Eigen::VectorXd > >	intervalSegment (std::size_t interval) const
	Returns a reference to the segment of the polynmoial vector respresentation corresponding to an interval.

std::optional< Eigen::Ref< Eigen::VectorXd > >	intervalSegment (std::size_t interval)
	Returns a reference to the segment of the polynmoial vector respresentation corresponding to an interval.

std::optional< Eigen::Map< const Eigen::VectorXd, Eigen::Unaligned, Eigen::Stride< 1, Eigen::Dynamic > > >	coordinateGLPointsInInterval (std::size_t coordinate, std::size_t interval) const

std::optional< Eigen::Map< Eigen::VectorXd, Eigen::Unaligned, Eigen::Stride< 1, Eigen::Dynamic > > >	coordinateGLPointsInInterval (std::size_t coordinate, std::size_t interval)

Eigen::VectorXd	domainPoints () const

std::shared_ptr< const typename Collocation::GLCollocationData >	collocationData () const
	Returns a pointer to the collocation data instance used by the polynomial.

Eigen::VectorXd	domainBreakPoints () const
	Returns a vector containing the interpolation points (with repeated points)

Polynomial	derivative (std::size_t deg=1) const
	Returns the derivative of the current polynomial as a new instance of Polynomial.

bool	isApproxPol (const Polynomial &other, double tol=1.0e-9) const

std::size_t	codomainDimension () const

citerator	cbegin () const

citerator	cend () const

iterator	begin ()

iterator	end ()

citerator	begin () const

citerator	end () const

Static Public Member Functions
static std::function< bool(const Polynomial &, Polynomial &, Eigen::Ref< Eigen::SparseMatrix< double, Eigen::RowMajor > >)>	lift (std::function< bool(Eigen::Ref< const Eigen::VectorXd >, Eigen::Ref< Eigen::VectorXd >, Eigen::Ref< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > >)> fun)

Protected Attributes
std::vector< double >	m_vector

Eigen::Map< Eigen::VectorXd >	m_eigenMap

Eigen::Map< const Eigen::VectorXd >	m_eigenConstMap

IntervalPartitionXd	m_intervalPartition
	Partition of the interval domain.

std::shared_ptr< const typename Collocation::GLCollocationData >	m_collocationData
	Pointer to the collocation-related procedures.

std::size_t	m_codomDim
	Dimension of the codomain of the polynomial.

Eigen interoperability methods
//{{{
using	Base = Eigen::Map<Eigen::VectorXd>
	Eigen::Ref Inheritance requirement: Base type.

double &	operator() (std::size_t i)
	Get element from the vector.

const double &	operator() (std::size_t i) const
	Get element from the vector.

auto	segment (std::size_t start, std::size_t size)
	Get a segment from the vector of coefficients (which are points because this is a lagrange polynomial)

auto	segment (std::size_t start, std::size_t size) const
	Get a segment from the vector of coefficients (which are points because this is a lagrange polynomial)

auto	tail (std::size_t size) const
	Get the tail from the vector of coefficients (which are points because this is a lagrange polynomial)

auto	tail (std::size_t size)
	Get the tail from the vector of coefficients (which are points because this is a lagrange polynomial)

auto	transpose ()
	Transpose the vector.

auto	transpose () const
	Transpose the vector.

void	setZero ()
	Transpose the vector.

bool	isZero (double toll=1.0e-9) const

void	setRandom ()

bool	hasNaN () const

template<typename T>
auto	operator- (const Eigen::DenseBase< T > &other)
	Eigen::Ref Inheritance requirement: DenseBase.

template<typename T>
auto	operator- (const Eigen::DenseBase< T > &other) const

auto	operator- (const Polynomial &other)

auto	operator- (const Polynomial &other) const

template<typename T>
bool	isApprox (const Eigen::DenseBase< T > &other, double tol=1.0e-9) const

	operator Eigen::Map< Eigen::VectorXd > & ()

	operator Eigen::Ref< Eigen::VectorXd > ()

	operator Eigen::Ref< const Eigen::VectorXd > () const

	operator Eigen::Map< const Eigen::VectorXd > () const

Eigen::Index	size () const

Eigen::Map< Eigen::VectorXd > &	vectorXd ()

const Eigen::Map< const Eigen::VectorXd > &	vectorXd () const

auto	array ()

auto	array () const

double	sum () const

void	setConstant (double in)

Static constructors
//{{{
static Polynomial	random (const IntervalPartitionXd &interval, std::size_t ngl, std::size_t codomDim)

static Polynomial	random (std::size_t numberOfIntervals, std::size_t ngl, std::size_t codomDim)

static Polynomial	constant (const IntervalPartitionXd &interval, std::size_t ngl, std::size_t codomDim, const Eigen::Ref< const Eigen::VectorXd > &vec)
	Construct constant polynomial with the same value in all Gauss-Lobatto points.

static Polynomial	fromFunction (const IntervalPartitionXd &interval, std::size_t ngl, std::size_t codomDim, const std::function< Eigen::VectorXd(double)> &fun)
	Construct an approximation of a function using Lagrange interpolation at Gauss-Lobatto points.

Constructor & Destructor Documentation

◆ Polynomial() [1/2]

Polynomial	(	const IntervalPartitionXd &	interval,
		std::size_t	nglp,
		std::size_t	codomDim )

Constructs a zero-valued Polynomial in with domain specified by the interval partition, and a given number of gauss-lobatto points and a given dimension of the codomain.

Parameters

interval	The interval partition representing the domain.
nglp	The number of Gauss-Lobatto points.
codomDim	The dimension of the codomain.

◆ Polynomial() [2/2]

Polynomial	(	std::size_t	nIntervals,
		std::size_t	nglp,
		std::size_t	codomDim )

Constructs a Polynomial in with domain [0, nIntervals], with nIntervals subintervals with unit length, and a given number of gauss-lobatto points and a given dimension of the codomain.

Parameters

interval	The interval partition representing the domain.
nglp	The number of Gauss-Lobatto points.
codomDim	The dimension of the codomain.
codomDim	The dimension of the codomain.

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

RoboticsPlugin/Include/ImFusion/Robotics/MotionPlanning/Polynomial.h

Detailed Description

Important Note on Boundary Points

Classes

Public Member Functions

Static Public Member Functions

Protected Attributes

Eigen interoperability methods

Static constructors

Constructor & Destructor Documentation

◆ Polynomial() [1/2]

◆ Polynomial() [2/2]